数值法求六轴机械臂逆运动学解

问题描述：已知工具坐标系相对于固定坐标系的期望位置和姿态，如何求解满足期望位姿的关节角？

本篇将通过基于雅克比矩阵的牛顿-拉夫森迭代法求运动学逆解。

牛顿 - 拉夫森法（Newton - Raphson）

对于机械臂逆运动学求解方法，如果逆运动学方程无解析解，这时可以采用数值迭代方法求解。即使存在解析解，数值算法也经常用于改善求解精度。

算法详解：

数值求解方程,假设为初值，利用泰勒级数在处展开，并截取第一项，得:

若只保留第一阶，令，求解，得到

在将上式求得的值作为初值，代入上述方程中

上述过程不断迭代，直到的精度满足要求。

同样的，上述求解公式可以扩展到n维：

只需将改成向量即可。

基于雅克比矩阵的牛顿 - 拉夫森迭代法求逆解

根据机器人的MDH参数和连杆坐标系，6DOF机器人的末端位姿可写为：

由于矩阵中的元素是关节变量的函数，则有：

(1)

式中：为连杆长度，为连杆扭角，为连杆偏置，为关节变量，为机器人末端位姿矩阵。

因此，机器人的逆解问题就是根据已知的连杆参数以及给定的末端位姿矩阵,求解关节变量.

使用最原始的方法，取12个元素全部参与计算得到12个方程（由于末端位姿矩阵的自由度为6，所以取3个位置元素和任意三个姿态元素，建立6个方程即可。但是，在某些特殊情况下，通过迭代的得到的解是一样的，而实际结果应该是不一样的。所以，作者采用最原始的建立12个方程进行求解）

根据上述12个方程，求解jacobian矩阵：

由上式得到的jacobian是12×6的矩阵，不能按照方阵的形式对其求逆，所以使用广义逆求其最小二乘解。

因此，得到迭代公式

总结

牛顿 - 拉夫森迭代法求机器人你运动学问题，具体步骤为：

1、估计一个临近的关节变量,通过正运动学计算初始迭代位姿.

2、根据初始迭代位姿计算jacobian矩阵

3、根据初始迭代位姿和jacobian矩阵计算微分运动量

4、根据迭代公式计算目标位姿的逆运动学解

附部分matlab求解代码

%求雅可比矩阵
j = jacobian(F, Theta);
t = [Theta1 Theta2 Theta3 Theta4 Theta5 Theta6];
j_k = subs(j, t, theta_init);
Jacobian = eval(j_k); % %求微分运动量
Tcur = double(robot.fkine(theta_init));
ff1 = Tcur(1,4) - Tend(1,4);
ff2 = Tcur(2,4) - Tend(2,4);
ff3 = Tcur(3,4) - Tend(3,4);
ff4 = Tcur(2,3) - Tend(2,3);
ff5 = Tcur(3,3) - Tend(3,3);
ff6 = Tcur(3,2) - Tend(3,2);
ff7 = Tcur(1,1) - Tend(1,1);
ff8 = Tcur(1,2) - Tend(1,2);
ff9 = Tcur(1,3) - Tend(1,3);
ff10 = Tcur(2,1) - Tend(2,1);
ff11 = Tcur(2,2) - Tend(2,2);
ff12 = Tcur(3,1) - Tend(3,1);
delta_f = [ff1;ff2;ff3;ff4;ff5;ff6;ff7;ff8;ff9;ff10;ff11;ff12];%求微分增加量
d_theta = inv(Jacobian' * Jacobian) * Jacobian' * delta_f;
error = 1e-6;
while norm(d_theta) > errortheta_cur = theta_init - d_theta';%更新雅可比矩阵Theta = [Theta1; Theta2; Theta3; Theta4; Theta5; Theta6];j = jacobian(F, Theta);t = [Theta1 Theta2 Theta3 Theta4 Theta5 Theta6];j_k = subs(j, t, theta_cur);Jacobian = eval(j_k); Theta = theta_cur;Tcur = double(robot.fkine(theta_cur));ff1 = Tcur(1,4) - Tend(1,4);ff2 = Tcur(2,4) - Tend(2,4);ff3 = Tcur(3,4) - Tend(3,4);ff4 = Tcur(2,3) - Tend(2,3);ff5 = Tcur(3,3) - Tend(3,3);ff6 = Tcur(3,2) - Tend(3,2);ff7 = Tcur(1,1) - Tend(1,1);ff8 = Tcur(1,2) - Tend(1,2);ff9 = Tcur(1,3) - Tend(1,3);ff10 = Tcur(2,1) - Tend(2,1);ff11 = Tcur(2,2) - Tend(2,2);ff12 = Tcur(3,1) - Tend(3,1);delta_f = [ff1;ff2;ff3;ff4;ff5;ff6;ff7;ff8;ff9;ff10;ff11;ff12];theta_init = theta_cur;%求微分增加量d_theta = inv(Jacobian' * Jacobian) * Jacobian' * delta_f;norm(d_theta)
end