5 机器人正动力学代码及验证
得到了机器人的逆动力学方程后，为了得到最终动力学普遍方程的矩阵形式，需要提取求解惯性矩阵、刚度矩阵、科氏力等速度相关矩阵。
采用基于关节空间惯量矩阵的方法进行正动力学求解，基于系统动力学普遍方程式的特点，根据相关条件求出各系数矩阵，进而得出关节加速度。机器人动力学模型矩阵形式表示的一般方程如下：
代码1：逆动力学计算（不考虑重力情况）
%% 平面3dof简化模型（逆动力学）
function tol=equation3(dq1,dq2,dq3,ddq1,ddq2,ddq3)
% syms q1 q2 q3 l1 l2 l3 m1 m2 m3;
syms q1 q2 q3 ;
%% 参数初始化
R01=[cos(q1) -sin(q1) 0;
sin(q1) cos(q1) 0;
0 0 1];
R12=[cos(q2) -sin(q2) 0;
sin(q2) cos(q2) 0;
0 0 1];
R23=[cos(q3) -sin(q3) 0;
sin(q3) cos(q3) 0;
0 0 1];
R34=[1 0 0;
0 1 0;
0 0 1];
R10 =R01.';
R21 =R12.';
R32 =R23.';
R43=R34.';
l1=0.5;l2=0.5;l3=0.5;
m1=1.8751381;m2=1.8751381;m3=1.8751381;
%连杆坐标系位置
p10=[0; 0; 0];p21=[l1; 0; 0];p32=[l2;0;0];p43=[l3;0;0];
% 每个连杆质心的位置矢量
pc11 = 0.5 * p21; pc22 = 0.5 * p32; pc33=0.5*p43;
% 惯性张量
I1=[ 0.00058598 0 0;
0 0.03935837 0;
0 0 0.03935837];
I2=[ 0.00058598 0 0;
0 0.03935837 0;
0 0 0.03935837];
I3=[ 0.00058598 0 0;
0 0.03935837 0;
0 0 0.03935837];
% 连杆间角速度和角加速度
w00 = [0; 0; 0]; v00 = [0; 0; 0]; w00d = [0; 0; 0]; v00d = [0; 0; 0];%base_link的各项初始值
z = [0; 0; 1];
% 末端执行器没有力
f44 = [0; 0; 0]; n44= [0; 0; 0];
%% 建立动力学方程
%i=0
w11 = R10*w00 + dq1*z;
w11d = R10*w00d + cross(R10*w00, z*dq1) + ddq1*z;
v11d = R10*(cross(w00d, p10) + cross(w00, cross(w00, p10)) + v00d);
vc11d = cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d;
F11 = m1*vc11d;
N11 = I1*w11d + cross(w11, I1*w11);
% i = 1
w22 = R21*w11 + dq2*z;
w22d = R21*w11d + cross(R21*w11, z*dq2) + ddq2*z;
v22d = R21*(cross(w11d, p21) + cross(w11, cross(w11, p21)) + v11d);
vc22d = cross(w22d, pc22) + cross(w22, cross(w22, pc22)) + v22d;
F22 = m2*vc22d;
N22 = I2*w22d + cross(w22, I2*w22);
% i = 2
w33 = R32*w22 + dq3*z;
w33d = R32*w22d + cross(R32*w22, z*dq3) + ddq3*z;
v33d = R32*(cross(w22d, p32) + cross(w22, cross(w22, p32)) + v22d);
vc33d = cross(w33d, pc33) + cross(w33, cross(w33, pc33)) + v33d;
F33 = m3*vc33d;
N33 = I3*w33d + cross(w33, I3*w33);
%% Inward iterations: i: 6->1
% i = 3
f33 = R34*f44 + F33;
n33 = N33 + R34*n44 + cross(pc33, F33) + cross(p43, R34*f44);
tau(3) = n33(3);
% i=2
f22 = R23*f33 + F22;
n22 = N22 + R23*n33 + cross(pc22, F22) + cross(p32, R23*f33);
tau(2) = n22(3);
% i =1
f11 = R12*f22 + F11;
n11 = N11 + R12*n22 + cross(pc11, F11) + cross(p21, R12*f22);
tau(1) = n11(3);
tol(1)=simplify(tau(1));
tol(2)=simplify(tau(2));
tol(3)=simplify(tau(3));
代码2：系数提取
%% 计算M阵和C阵系数（逆动力学）
function [M,C]=call_dynamic()
% syms l1 l2 l3 m1 m2 m3 ;
syms dq1 dq2 dq3 ;
m11=equation3(0,0,0,1,0,0);
M11=vpa(m11(1),5);
M21=vpa(m11(2),5);
M31=vpa(m11(3),5);
m12=equation3(0,0,0,0,1,0);
M12=vpa(m12(1),5);
M22=vpa(m12(2),5);
M32=vpa(m12(3),5);
m13=equation3(0,0,0,0,0,1);
M13=vpa(m13(1),5);
M23=vpa(m13(2),5);
M33=vpa(m13(3),5);
c1=equation3(dq1,dq2,dq3,0,0,0);
C1=vpa(c1(1),5);
C2=vpa(c1(2),5);
C3=vpa(c1(3),5);
M=[M11,M12,M13;
M21,M22,M23;
M31,M32,M33];
C=[C1;C2;C3];
代码3：系数整理
[M,C]=call_dynamic();
M11=vpa(simplify(M(1,1)),6);
M12=vpa(simplify(M(1,2)),6);
M13=vpa(simplify(M(1,3)),6);
M21=vpa(simplify(M(2,1)),6);
M22=vpa(simplify(M(2,2)),6);
M23=vpa(simplify(M(2,3)),6);
M31=vpa(simplify(M(3,1)),6);
M32=vpa(simplify(M(3,2)),6);
M33=vpa(simplify(M(3,3)),6);
C1=vpa(simplify(C(1)),6);
C2=vpa(simplify(C(2)),6);
C3=vpa(simplify(C(3)),6);
代码4：写入S函数
%% 平面3dof动力学模型（正动力学）
function [sys,x0,str,ts,simStateCompliance] = plant(t,x,u,flag)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
end
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 6;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 6;
sizes.NumInputs = 3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
% x0 = [0.1,0.1,0.1,0.1,0.1,0.1];
x0 = [0,0,0,0,0,0];
str = [];
ts = [0 0];
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
end
function sys=mdlDerivatives(t,x,u)
tol=[u(1);u(2);u(3)];
q1=x(1);
dq1=x(2);
q2=x(3);
dq2=x(4);
q3=x(5);
dq3=x(6);
D11=0.46878*cos(q2 + q3) + 1.4064*cos(q2) + 0.46878*cos(q3) + 1.876;
D12=0.23439*cos(q2 + q3) + 0.70318*cos(q2) + 0.46878*cos(q3) + 0.78189;
D13=0.23439*cos(q2 + q3) + 0.23439*cos(q3) + 0.15655；
D21=0.23439*cos(q2 + q3) + 0.70318*cos(q2) + 0.46878*cos(q3) + 0.78189;
D22=0.46878*cos(q3) + 0.78189;
D23=0.23439*cos(q3) + 0.15655;
D31=0.23439*cos(q2 + q3) + 0.23439*cos(q3) + 0.15655;
D32=0.23439*cos(q3) + 0.15655;
D33=0.15655;
M=[D11,D12,D13;
D21,D22,D23;
D31,D32,D33];
C1=- 0.70318*dq2^2*sin(q2) - 0.23439*dq3^2*sin(q3) - 0.23439*dq2^2*sin(q2 + q3) - 0.23439*dq3^2*sin(q2 + q3) - 1.4064*dq1*dq2*sin(q2) - 0.46878*dq1*dq3*sin(q3) - 0.46878*dq2*dq3*sin(q3) - 0.46878*dq1*dq2*sin(q2 + q3) - 0.46878*dq1*dq3*sin(q2 + q3) - 0.46878*dq2*dq3*sin(q2 + q3);
C2=0.70318*dq1^2*sin(q2) - 0.23439*dq3^2*sin(q3) + 0.23439*dq1^2*sin(q2 + q3) - 0.46878*dq1*dq3*sin(q3) - 0.46878*dq2*dq3*sin(q3);
C3=0.46878*sin(q3)*((dq1 + dq2)*(0.5*dq1 + 0.5*dq2) + 0.5*dq1^2*cos(q2)) + 0.23439*dq1^2*cos(q3)*sin(q2);
C=[C1;C2;C3];
S=M\(tol-C);
sys(1)=x(2);
sys(2)=S(1);
sys(3)=x(4);
sys(4)=S(2);
sys(5)=x(6);
sys(6)=S(3);
% end mdlDerivatives
end
function sys=mdlUpdate(t,x,u)
sys = [];
% end mdlUpdate
end
%
%
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);
sys(5)=x(5);
sys(6)=x(6);
end
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
% end mdlGetTimeOfNextVarHit
end
%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
function sys=mdlTerminate(t,x,u)
sys = [];
% end mdlTerminate
end
创建SIMULINK，进行正动力学仿真：
仿真时间5s，输入力矩为0.2sint 0.1sint 0.1sint
创建SIMSCAPE，进行仿真验证：
结果一致：
