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);% 求解关节角1theta1_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是能够求解三个相邻关节轴交于一点的情况，因此上述代码针对的问题是机械臂存在三轴平行的情况的求解。