拉格朗日法建立六轴机器人TX90动力学模型

参考书籍，感谢这本书的作者让我对雅克比矩阵有了更加深刻的认识：

以下是我建模的相关草稿：

草稿有点凌乱，没有整理，详细建模过程可参考以下代码，笔者最后反对称性没有验证出来，可能有地方出问题了，相关代码如下：

function [ T ] = Trans( alpha, a, theta, d )                            % 变换矩阵
T =[ cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);0,          sin(alpha),              cos(alpha),            d;0,              0,                        0,                  1];
end

syms theta1 a1 d1 theta2 a2 d2 theta3 a3 d3 theta4 a4 d4 theta5 a5 d5 theta6 a6 d6
syms alpha1 alpha2 alpha3 alpha4 alpha5  alpha6
syms x1 y1 z1 x2 y2 z2 x3 y3 z3 x4 y4 z4 x5 y5 z5 x6 y6 z6  %质心位置
syms m1 m2 m3 m4 m5 m6 g
syms Ixx1 Ixy1 Ixz1 Iyy1 Iyz1 Izz1 Ixx2 Ixy2 Ixz2 Iyy2 Iyz2 Izz2 Ixx3 Ixy3 Ixz3 Iyy3 Iyz3 Izz3 
syms Ixx4 Ixy4 Ixz4 Iyy4 Iyz4 Izz4 Ixx5 Ixy5 Ixz5 Iyy5 Iyz5 Izz5 Ixx6 Ixy6 Ixz6 Iyy6 Iyz6 Izz6g = sym(9.8);%重力加速度
%各连杆在其质心坐标系下的惯性张量
Ic1=[Ixx1, Ixy1, Ixz1; Ixy1, Iyy1, Iyz1;Ixz1, Iyz1, Izz1 ];
Ic2=[Ixx2, Ixy2, Ixz2; Ixy2, Iyy2, Iyz2;Ixz2, Iyz2, Izz2 ];
Ic3=[Ixx3, Ixy3, Ixz3; Ixy3, Iyy3, Iyz3;Ixz3, Iyz3, Izz3 ];
Ic4=[Ixx4, Ixy4, Ixz4; Ixy4, Iyy4, Iyz4;Ixz4, Iyz4, Izz4 ];
Ic5=[Ixx5, Ixy5, Ixz5; Ixy5, Iyy5, Iyz5;Ixz5, Iyz5, Izz5 ];
Ic6=[Ixx6, Ixy6, Ixz6; Ixy6, Iyy6, Iyz6;Ixz6, Iyz6, Izz6 ];
%这样赋值好化简
%  theta1 = sym(0);theta2 = sym(0);theta3 = sym(pi/2);theta4 = sym(0);theta5 = sym(pi/4);theta6 = sym(0);
d1 = sym(0);d2 = sym(0);d3 = sym(50);d4 = sym(425);d5 = sym(0);d6 = sym(100);
a1 = sym(50); a2 =sym(425); a3 =sym(0); a4 = sym(0);a5 = sym(0); a6 = sym(0);
alpha1 = sym(-pi/2); alpha2 = sym(0); alpha3 = sym(pi/2); alpha4 = sym(-pi/2); alpha5 = sym(pi/2); alpha6 = sym(0);
A1=Trans(alpha1, a1, theta1,d1 );
A2=Trans(alpha2 , a2, theta2,d2 );
A3=Trans(alpha3, a3, theta3,d3 );
A4=Trans(alpha4, a4, theta4,d4 );
A5=Trans(alpha5, a5, theta5,d5 );
A6=Trans(alpha6, a6, theta6,d6 );T00 = [1 0 0 0; 0 1 0 0 ; 0 0 1 0; 0 0 0 1];
T01 = A1;
T02 = A1*A2;
T03 = A1*A2*A3;
T04 = A1*A2*A3*A4;
T05 = A1*A2*A3*A4*A5;
T06 = A1*A2*A3*A4*A5*A6;C1 = [1, 0, 0, x1; 0, 1, 0, y1; 0, 0, 1, z1;0 0 0 1]; %连杆1质心在{1}系下，因为矩阵是右乘的，先转的T01,再由T1到TC1
T0c1 = T01* C1;C2 = [1, 0, 0, x2; 0, 1, 0, y2; 0, 0, 1, z2;0 0 0 1];  %连杆2质心在{2}系下
T0c2 = T02* C2;C3 = [1, 0, 0, x3; 0, 1, 0, y3; 0, 0, 1, z3;0 0 0 1];  %连杆3质心在{3}系下
T0c3 = T03* C3;C4 = [1, 0, 0, x4; 0, 1, 0, y4; 0, 0, 1, z4;0 0 0 1];  %连杆4质心在{4}系下
T0c4 = T04* C4;C5 = [1, 0, 0, x5; 0, 1, 0, y5; 0, 0, 1, z5;0 0 0 1];  %连杆5质心在{5}系下
T0c5 = T05* C5;C6 = [1, 0, 0, x6; 0, 1, 0, y6; 0, 0, 1, z6;0 0 0 1];  %连杆6质心在{6}系下
T0c6 = T06* C6;%雅克比准备工作
B = [0;0;0]; %零矩阵
Z0 = T00([1 2 3],3);
Z1 = T01([1 2 3],3);
Z2 = T02([1 2 3],3);
Z3 = T03([1 2 3],3);
Z4 = T04([1 2 3],3);
Z5 = T05([1 2 3],3);O0 = T00([1 2 3],4);
O1 = T01([1 2 3],4);
O2 = T02([1 2 3],4);
O3 = T03([1 2 3],4);
O4 = T04([1 2 3],4);
O5 = T05([1 2 3],4);
O6 = T06([1 2 3],4); Oc1 = T0c1([1 2 3],4);
Oc2 = T0c2([1 2 3],4);
Oc3 = T0c3([1 2 3],4);
Oc4 = T0c4([1 2 3],4);
Oc5 = T0c5([1 2 3],4);
Oc6 = T0c6([1 2 3],4);%质心1的雅克比
JV1_1 = cross(Z0, Oc1-O0);
J1 = [JV1_1, B , B, B, B, B;Z0,B,B,B,B,B];
Jv1 = J1([1 2 3], :);
Jw1 = J1([4 5 6], :);
%质心2的雅克比
JV2_1 = cross(Z0, Oc2-O0);
JV2_2 = cross(Z1, Oc2-O1);
J2 = [JV2_1, JV2_2 , B, B, B, B;Z0,Z1,B,B,B,B];
Jv2 = J2([1 2 3],  :);
Jw2 = J2([4 5 6], :);
%质心3的雅克比
JV3_1 = cross(Z0, Oc3-O0);
JV3_2 = cross(Z1, Oc3-O1);
JV3_3 = cross(Z2, Oc3-O2);
J3 = [JV3_1, JV3_2 , JV3_3, B, B, B;Z0,Z1,Z2,B,B,B];
Jv3 = J3([1 2 3],  :);
Jw3 = J3([4 5 6], :);
%质心4的雅克比
JV4_1 = cross(Z0, Oc4-O0);
JV4_2 = cross(Z1, Oc4-O1);
JV4_3 = cross(Z2, Oc4-O2);
JV4_4 = cross(Z3, Oc4-O3);
J4 = [JV4_1, JV4_2 , JV4_3, JV4_4, B, B;Z0,Z1,Z2,Z3,B,B];
Jv4 = J4([1 2 3],  :);
Jw4 = J4([4 5 6], :);
%质心5的雅克比
JV5_1 = cross(Z0, Oc5-O0);
JV5_2 = cross(Z1, Oc5-O1);
JV5_3 = cross(Z2, Oc5-O2);
JV5_4 = cross(Z3, Oc5-O3);
JV5_5 = cross(Z4, Oc5-O4);
J5 = [JV5_1, JV5_2 , JV5_3, JV5_4, JV5_5, B;Z0,Z1,Z2,Z3,Z4,B];
Jv5 = J5([1 2 3],  :);
Jw5 = J5([4 5 6], :);
%质心6的雅克比
JV6_1 = cross(Z0, Oc6-O0);
JV6_2 = cross(Z1, Oc6-O1);
JV6_3 = cross(Z2, Oc6-O2);
JV6_4 = cross(Z3, Oc6-O3);
JV6_5 = cross(Z4, Oc6-O4);
JV6_6 = cross(Z5, Oc6-O5);
J6 = [JV6_1, JV6_2 , JV6_3, JV6_4, JV6_5, JV6_6;Z0,Z1,Z2,Z3,Z4,Z5];
Jv6 = J6([1 2 3],  :);
Jw6 = J6([4 5 6], :);%动能定理准备：
R0c1 = T0c1([1 2 3],[1 2 3]);
R0c2 = T0c2([1 2 3],[1 2 3]);
R0c3 = T0c3([1 2 3],[1 2 3]);
R0c4 = T0c4([1 2 3],[1 2 3]);
R0c5 = T0c5([1 2 3],[1 2 3]);
R0c6 = T0c6([1 2 3],[1 2 3]);%动能定理：
D1 = m1.* (Jv1.' * Jv1)+Jw1.' *(R0c1*Ic1* R0c1.') *Jw1;
D2 = m2.* (Jv2.' * Jv2)+Jw2.' *(R0c2*Ic2* R0c2.') *Jw2;
D3 = m3.* (Jv3.' * Jv3)+Jw3.' *(R0c3*Ic3* R0c3.') *Jw3;
D4 = m4.* (Jv4.' * Jv4)+Jw4.' *(R0c4*Ic4* R0c4.' )*Jw4;
D5 = m5.* (Jv5.' * Jv5)+Jw5.' *(R0c5*Ic5* R0c5.') *Jw5;
D6 = m6.* (Jv6.' * Jv6)+Jw6.' *(R0c6*Ic6* R0c6.' )*Jw6;
%惯性矩阵
D= D1+D2+D3+D4+D5+D6;
%验证是否对称%求重力矩阵
P1 =[x1;y1;z1]; %质心1在{1}的坐标
P01 = T01([1 2 3],[1 2 3])*P1; %质心1在{0}系下坐标P2 =[x2;y2;z2]; %质心2在{2}的坐标
P02 = T02([1 2 3],[1 2 3])*P2; %质心2在{0}系下坐标P3 =[x3;y3;z3];