拉格朗日法建立六轴机器人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 Izz6

g = 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]; %质心3在{3}的坐标
P03 = T03([1 2 3],[1 2 3])*P3; %质心3在{0}系下坐标

P4 =[x4;y4;z4]; %质心4在{4}的坐标
P04 = T04([1 2 3],[1 2 3])*P4; %质心4在{0}系下坐标

P5 =[x5;y5;z5]; %质心5在{5}的坐标
P05 = T05([1 2 3],[1 2 3])*P5; %质心5在{0}系下坐标

P6 =[x1;y1;z1]; %质心6在{6}的坐标
P06 = T06([1 2 3],[1 2 3])*P6; %质心6在{0}系下坐标

%势能表达式
P = m1*g*P01(3,1)+m2*g*P02(3,1)+m3*g*P03(3,1)+m4*g*P04(3,1)+m5*g*P05(3,1)+m6*g*P06(3,1);

%势能对theta1求偏导
G01 = diff( P,theta1);
%势能对theta2求偏导
G02 = diff( P,theta2);
%势能对theta3求偏导
G03 = diff( P,theta3);
%势能对theta4求偏导
G04 = diff( P,theta4);
%势能对theta5求偏导
G05 = diff( P,theta5);
%势能对theta6求偏导
G06 = diff( P,theta6);

%重力矩阵
G = [G01; G02; G03; G04; G05; G06];

%%%离心哥式矩阵求解
%轴1相关
syms c111 c121 c131 c141 c151 c161 c211 c221 c231 c241 c251 c261 c311 c321 c331 c341 c351 c361
syms c411 c421 c431 c441 c451 c461 c511 c521 c531 c541 c551 c561 c611 c621 c631 c641 c651 c661
%轴2相关
syms c112 c122 c132 c142 c152 c162 c212 c222 c232 c242 c252 c262 c312 c322 c332 c342 c352 c362
syms c412 c422 c432 c442 c452 c462 c512 c522 c532 c542 c552 c562 c612 c622 c632 c642 c652 c662
%轴3相关
syms c113 c123 c133 c143 c153 c163 c213 c223 c233 c243 c