六自由度机械臂标准DH参数法的逆运动学求解

1 前沿

最近因为项目需求，需要对六自由度机械臂进行正逆运动学分析与仿真。其中，正运动学分析与仿真只需在完成DH参数建模的基础上，通过Robotics工具箱，结合B站刘海涛老师的教学视频即可轻易完成。但是，在深入学习中发现，Robotics工具箱的机械臂求逆解函数ifine6s存在问题，无法计算我的机械臂的逆运动学的封闭解。(工具箱的数值解的逆解函数ifine是能够正常使用的，但是只能获取一个解)。因此，结合刘海涛老师的教学视频以及陈赛旋博士的论文，编写了对六自由度机械臂标准DH参数法的逆运动学求解的Matlab代码，希望能够对大家有所帮助。如果后续有问题，也欢迎一起讨论学习。

2 相关说明

这里，我采用标准DH参数法建立机械臂模型，相关机械臂参数是和我的机械臂一致的，因此，连杆偏移、连杆长度需要按照自己的实际模型进行修改。另外，关注连杆扭角alpha，如果其值是和我一致的，那就不需要改变计算代码，只需要更改前面的d与a的参数值即可；如果不一致，则可能需要对代码做一定调整，可以结合上述提及的论文进行修改。

function [J] = Pieper_STD(a)
%定义连杆的标准D-H参数
%连杆偏移
d1 = 0.096;
d2 = 0;
d3 = 0;
d4 = 0.122;
d5 = 0.098;
d6 = 0.089;
%连杆长度
a1 = 0;
a2 = 0.418;
a3 = 0.398;
a4 = 0;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = -pi/2;
alpha2 = 0;
alpha3 = 0;
alpha4 = -pi/2;
alpha5 = -pi/2;
alpha6 = 0;

%目标位置姿态矩阵
nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4);
ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4);
nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4);

% 求解关节角1
	theta1_1 = atan2(-d4,sqrt((d6*ay-py)^2+(px-d6*ax)^2-d4^2))-atan2(d6*ay-py,px-d6*ax);
	theta1_2 = atan2(-d4,-sqrt((d6*ay-py)^2+(px-d6*ax)^2-d4^2))-atan2(d6*ay-py,px-d6*ax);
    
% 求解关节角5, theta5不应该为0或者pi
    theta5_1 = atan2(sqrt((nx*sin(theta1_1)-ny*cos(theta1_1))^2+(ox*sin(theta1_1)-oy*cos(theta1_1))^2), ax*sin(theta1_1)-ay*cos(theta1_1));
    theta5_2 = atan2(-sqrt((nx*sin(theta1_1)-ny*cos(theta1_1))^2+(ox*sin(theta1_1)-oy*cos(theta1_1))^2), ax*sin(theta1_1)-ay*cos(theta1_1));
    theta5_3 = atan2(sqrt((nx*sin(theta1_2)-ny*cos(theta1_2))^2+(ox*sin(theta1_2)-oy*cos(theta1_2))^2), ax*sin(theta1_2)-ay*cos(theta1_2));
    theta5_4 = atan2(-sqrt((nx*sin(theta1_2)-ny*cos(theta1_2))^2+(ox*sin(theta1_2)-oy*cos(theta1_2))^2), ax*sin(theta1_2)-ay*cos(theta1_2));
    
% 求解关节角6
    theta6_1 = atan2(-(ox*sin(theta1_1)-oy*cos(theta1_1))/sin(theta5_1), (nx*sin(theta1_1)-ny*cos(theta1_1))/sin(theta5_1));
    theta6_2 = atan2(-(ox*sin(theta1_1)-oy*cos(theta1_1))/sin(theta5_2), (nx*sin(theta1_1)-ny*cos(theta1_1))/sin(theta5_2));
    theta6_3 = atan2(-(ox*sin(theta1_2)-oy*cos(theta1_2))/sin(theta5_3), (nx*sin(theta1_2)-ny*cos(theta1_2))/sin(theta5_3));
    theta6_4 = atan2(-(ox*sin(theta1_2)-oy*cos(theta1_2))/sin(theta5_4), (nx*sin(theta1_2)-ny*cos(theta1_2))/sin(theta5_4));
    
% 求解关节角2，3，4
    q234_1 = atan2(az/sin(theta5_1), -(ax*cos(theta1_1)+ay*sin(theta1_1))/sin(theta5_1));
    q234_2 = atan2(az/sin(theta5_2), -(ax*cos(theta1_1)+ay*sin(theta1_1))/sin(theta5_2));
    q234_3 = atan2(az/sin(theta5_3), -(ax*cos(theta1_2)+ay*sin(theta1_2))/sin(theta5_3));
    q234_4 = atan2(az/sin(theta5_4), -(ax*cos(theta1_2)+ay*sin(theta1_2))/sin(theta5_4));
    
    A_1 = px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_1)+d6*sin(theta5_1)*cos(q234_1);
    B_1 = d1-pz-d5*cos(q234_1)+d6*sin(theta5_1)*sin(q234_1);
    A_2 = px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2);
    B_2 = d1-pz-d5*cos(q234_2)+d6*sin(theta5_2)*sin(q234_2);
    A_3 = px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3);
    B_3 = d1-pz-d5*cos(q234_3)+d6*sin(theta5_3)*sin(q234_3);
    A_4 = px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4);
    B_4 = d1-pz-d5*cos(q234_4)+d6*sin(theta5_4)*sin(q234_4);
    
    theta2_1 = atan2(A_1^2+B_1^2+a2^2-a3^2, sqrt(abs(4*a2^2*(A_1^2+B_1^2)-(A_1^2+B_1^2+a2^2-a3^2)^2)))-atan2(A_1, B_1);
    theta2_2 = atan2(A_1^2+B_1^2+a2^2-a3^2, -sqrt(abs(4*a2^2*(A_1^2+B_1^2)-(A_1^2+B_1^2+a2^2-a3^2)^2)))-atan2(A_1, B_1);
    theta2_3 = atan2(A_2^2+B_2^2+a2^2-a3^2, sqrt(abs(4*a2^2*(A_2^2+B_2^2)-(A_2^2+B_2^2+a2^2-a3^2)^2)))-atan2(A_2, B_2);
    theta2_4 = atan2(A_2^2+B_2^2+a2^2-a3^2, -sqrt(abs(4*a2^2*(A_2^2+B_2^2)-(A_2^2+B_2^2+a2^2-a3^2)^2)))-atan2(A_2, B_2);
    theta2_5 = atan2(A_3^2+B_3^2+a2^2-a3^2, sqrt(abs(4*a2^2*(A_3^2+B_3^2)-(A_3^2+B_3^2+a2^2-a3^2)^2)))-atan2(A_3, B_3);
    theta2_6 = atan2(A_3^2+B_3^2+a2^2-a3^2, -sqrt(abs(4*a2^2*(A_3^2+B_3^2)-(A_3^2+B_3^2+a2^2-a3^2)^2)))-atan2(A_3, B_3);
    theta2_7 = atan2(A_4^2+B_4^2+a2^2-a3^2, sqrt(abs(4*a2^2*(A_4^2+B_4^2)-(A_4^2+B_4^2+a2^2-a3^2)^2)))-atan2(A_4, B_4);
    theta2_8 = atan2(A_4^2+B_4^2+a2^2-a3^2, -sqrt(abs(4*a2^2*(A_4^2+B_4^2)-(A_4^2+B_4^2+a2^2-a3^2)^2)))-atan2(A_4, B_4);
    
    q23_1 = atan2(d1-pz-d5*cos(q234_1)+d6*sin(theta5_1)*sin(q234_1)-a2*sin(theta2_1), px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_1)+d6*sin(theta5_1)*cos(q234_1)-a2*cos(theta2_1));
    q23_2 = atan2(d1-pz-d5*cos(q234_1)+d6*sin(theta5_1)*sin(q234_1)-a2*sin(theta2_2), px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_1)+d6*sin(theta5_1)*cos(q234_1)-a2*cos(theta2_2));
    q23_3 = atan2(d1-pz-d5*cos(q234_2)+d6*sin(theta5_2)*sin(q234_2)-a2*sin(theta2_3), px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2)-a2*cos(theta2_3));
    q23_4 = atan2(d1-pz-d5*cos(q234_2)+d6*sin(theta5_2)*sin(q234_2)-a2*sin(theta2_4), px*cos(theta1_1)+py*sin(theta1_1)+d5*sin(q234_2)+d6*sin(theta5_2)*cos(q234_2)-a2*cos(theta2_4));
    q23_5 = atan2(d1-pz-d5*cos(q234_3)+d6*sin(theta5_3)*sin(q234_3)-a2*sin(theta2_5), px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3)-a2*cos(theta2_5));
    q23_6 = atan2(d1-pz-d5*cos(q234_3)+d6*sin(theta5_3)*sin(q234_3)-a2*sin(theta2_6), px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_3)+d6*sin(theta5_3)*cos(q234_3)-a2*cos(theta2_6));
    q23_7 = atan2(d1-pz-d5*cos(q234_4)+d6*sin(theta5_4)*sin(q234_4)-a2*sin(theta2_7), px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4)-a2*cos(theta2_7));
    q23_8 = atan2(d1-pz-d5*cos(q234_4)+d6*sin(theta5_4)*sin(q234_4)-a2*sin(theta2_8), px*cos(theta1_2)+py*sin(theta1_2)+d5*sin(q234_4)+d6*sin(theta5_4)*cos(q234_4)-a2*cos(theta2_8));
    
    theta3_1 = q23_1 - theta2_1;
    theta3_2 = q23_2 - theta2_2;
    theta3_3 = q23_3 - theta2_3;
    theta3_4 = q23_4 - theta2_4;
    theta3_5 = q23_5 - theta2_5;
    theta3_6 = q23_6 - theta2_6;
    theta3_7 = q23_7 - theta2_7;
    theta3_8 = q23_8 - theta2_8;
    
    theta4_1 = q234_1 - q23_1;
    theta4_2 = q234_1 - q23_2;
    theta4_3 = q234_2 - q23_3;
    theta4_4 = q234_2 - q23_4;
    theta4_5 = q234_3 - q23_5;
    theta4_6 = q234_3 - q23_6;
    theta4_7 = q234_4 - q23_7;
    theta4_8 = q234_4 - q23_8;
   
%输出最终的八个解
theta_STD = [ 
              theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;
 			  theta1_1,theta2_2,theta3_2,theta4_2,theta5_1,theta6_1;
 			  theta1_1,theta2_3,theta3_3,theta4_3,theta5_2,theta6_2;
 			  theta1_1,theta2_4,theta3_4,theta4_4,theta5_2,theta6_2;
              theta1_2,theta2_5,theta3_5,theta4_5,theta5_3,theta6_3;
 			  theta1_2,theta2_6,theta3_6,theta4_6,theta5_3,theta6_3;
 			  theta1_2,theta2_7,theta3_7,theta4_7,theta5_4,theta6_4;
 			  theta1_2,theta2_8,theta3_8,theta4_8,theta5_4,theta6_4;
             ]

最后，友情提示：一般的六自由度机器人没有封闭解！只有在机械臂满足Pieper准则时，机械臂才能具有封闭解。即Pieper准则是六自由度机械臂具有封闭解的充分条件。Pieper准则：机器人的三个相邻关节轴交于一点或三轴线平行。事实上，Robotics工具箱的逆解的封闭解函数ifine6s是能够求解三个相邻关节轴交于一点的情况，因此上述代码针对的问题是机械臂存在三轴平行的情况的求解。