机器人控制算法四之迭代法求解四轴机器人逆解

迭代法求解四轴机器人逆解

前提：只知道末端点坐标，分别求出各轴角度，C++实现
思路：
四轴对应四个转角j0,j1,j2,j3，并且已知各个Link的长度 L1,L2,L3
通过已知条件可以列出3个方程，但问题有4个未知数，就需要先给应一个角度假设值，即在一定范围内进行迭代，然后代入方程求出其他夹角，不可否认的是会得到多重解，然后再通过施加约束条件选出合适的最优解
如果对相关原理有疑问，可参考：四轴机械臂正、逆运动学详解
对matlab机器人仿真有疑问，可参考：matlab工具箱机械臂运动学正逆矩阵解具体分析

1. 正运动学

首先，正运动学用于迭代过程误差验证：

// 根据舵机角度正向解出目标坐标
x = (L1 * <
1