用MATLAB符号工具建立机器人的动力学模型

介绍

开发机器人过程中经常需要用牛顿-拉格朗日法建立机器人的动力学模型，表示为二阶微分方程组。本文以一个二杆系统为例，介绍如何用MATLAB符号工具得到微分方程表达式，只需要编辑好物点的位置公式和系统动能、势能，就能得到微分方程组，避免繁琐的手工推导工作。

代码功能演示

先放运行效果：没有外力：

施加5N向上外力：

采用《机器人学导论——分析、控制及应用（第二版）》一书中例4.4的自由二连杆，原例题和建模过程如下：


全部代码如下，直接复制到Matlab中即可运行，其中第一节是建模，得到加速度项的表达式，第二节是带入数据进行数值求解：

% 以自由二连杆为例，展示Matlab符号工具建立牛顿-拉格朗日动力学方程，并用ODE45函数数值求解的过程%% 建模
syms m1 m2 L1 L2 g % 结构常量
syms t x1(t) x2(t) % 将系统的广义坐标定义为时间函数IA=1/3*m1*L1^2; % 连杆1惯量
ID=1/12*m2*L2^2;% 连杆2惯量pD=[L1*cos(x1)+1/2*L2*cos(x1+x2); %连杆2质心位置L1*sin(x1)+1/2*L2*sin(x1+x2)];
vD=diff(pD,t); %对时间求导得到速度K=1/2*IA*diff(x1,t)^2+1/2*ID*(diff(x1,t)+diff(x2,t))^2+1/2*m2*sum(vD.^2); %系统动能P=m1*g*L1/2*sin(x1)+m2*g*(L1*sin(x1)+L2/2*sin(x1+x2)); % 系统势能L=K-P; %拉格朗日函数syms dx1 dx2 ddx1 ddx2
temp=diff(diff(L,diff(x1,t)),t)-diff(L,x1); % 展开拉格朗日函数，得到二阶微分式
temp=subs(temp,diff(x1,t,t),ddx1); % 用新定义的符号代替广义坐标的一二阶导数，简化公式表达
temp=subs(temp,diff(x2,t,t),ddx2);
temp=subs(temp,diff(x1,t),dx1);
temp=subs(temp,diff(x2,t),dx2);
diff1=collect(temp,[ddx1,ddx2,dx1^2,dx2^2,dx1*dx2,dx1,dx2]); % 合并同类项，整理成便于阅读的形式temp=diff(diff(L,diff(x2,t)),t)-diff(L,x2); % 对第二个广义坐标θ2做同样操作
temp=subs(temp,diff(x1,t,t),ddx1);
temp=subs(temp,diff(x2,t,t),ddx2);
temp=subs(temp,diff(x1,t),dx1);
temp=subs(temp,diff(x2,t),dx2);
diff2=collect(temp,[ddx1,ddx2,dx1^2,dx2^2,dx1*dx2,dx1,dx2]);syms T1 T2 Fx Fy
syms x1v x2v %求雅可比矩阵不能用时间函数x1(t)和x2(t)，因此先定义临时变量，求雅可比后再替换为x1和x2
p_end = [L1*cos(x1v)+L2*cos(x1v+x2v);L1*sin(x1v)+L2*sin(x1v+x2v)];% 末端位置
J_end = jacobian(p_end,[x1v;x2v]);% 末端雅可比矩阵
J_end = subs(J_end,{x1v,x2v},{x1,x2});
eqn = [diff1;diff2] == [T1;T2] + J_end.'*[Fx;Fy]; %构建动力学矩阵方程式
sol = solve(eqn,[ddx1;ddx2]); %求出二阶项[ddx1;ddx2]的解析解%输出求解结果，需要把结果表达式粘贴到新文件中，将其中的(t)全都删掉，然后粘贴到最下面odefun中v1和v2的表达式
fprintf("ddx1=");
disp(sol.ddx1);
fprintf("ddx2=");
disp(sol.ddx2);
fprintf("需要把结果表达式中的(t)全都删掉，然后粘贴到最下面odefun中ddx1和ddx2的表达式\n");%% 数值求解
clear;
global m1 m2 L1 L2 g d1 d2 Fx Fy
m1=1; m2=1; L1=0.5; L2=0.5; g=9.81;d1=0.8;d2=d1;Fx=0;Fy=0;%设置机器人参数,关节阻尼和外力tspan = 0:0.01:5; % 时间范围
[t,y] = ode45(@odefun,tspan,[0;0;0;0]); % 求解figure(1);clf; % 绘制运动动画
set(gcf,'Position',[0 300 600 350]);
for i = 1:10:size(y,1)x1=y(i,1);x2=y(i,2);x_loc = [0 L1*cos(x1) L1*cos(x1)+L2*cos(x1+x2)];y_loc = [0 L1*sin(x1) L1*sin(x1)+L2*sin(x1+x2)];clf; hold on;plot(x_loc,y_loc,'k - o','LineWidth',2);arrow_rate = 0.05;%箭头大小比例quiver(x_loc(end), y_loc(end), Fx*arrow_rate, Fy*arrow_rate, 0, 'LineWidth', 2, 'MaxHeadSize', 1, 'Color', 'r');  xlim([-1 1]);ylim([-1.22 0]);tit = sprintf("%.2f s",t(i));title(tit);
%     saveas(gcf,['Fig/',sprintf('%03d',size(y,1)-i),'.jpg']);pause(0.001);
endfigure(2); % 绘制关节角变化曲线
set(gcf,'Position',[0+600 300 600 500]);
subplot(211);
plot(t,rad2deg(y(:,1)+pi/2));
xlabel('Time (s)');ylabel('\theta_1');grid on;
subplot(212);
plot(t,rad2deg(y(:,2)));
xlabel('Time (s)');ylabel('\theta_2');grid on;% 为了求数值解需要化为一阶系统，以下为一阶系统的状态向量：
% x = [x1 x2 dx1 dx2]
% dxdt = [dx1 dx2 ddx1 ddx2]
function dxdt=odefun(t,x)global m1 m2 L1 L2 g d1 d2 Fx Fyx1=x(1);x2=x(2);dx1=x(3);dx2=x(4); %状态向量即为θ1，θ及其一阶导数T1 = -d1*dx1;T2 = -d2*dx2;% 根据符号工具的求解结果，得到θ1，θ2二阶导的表达式如下，需要删掉符号表达式中所有的(t)以免报错ddx1 = -(3*(2*L2*T2 - 2*L2*T1 - 6*L2*T1*sin(x1 + x2)^2 + 6*L2*T2*sin(x1 + x2)^2 - 6*L2*T1*cos(x1 + x2)^2 + 6*L2*T2*cos(x1 + x2)^2 + 12*L1*T2*sin(x1)*sin(x1 + x2) - 2*Fy*L1*L2*cos(x1)...  + 2*Fx*L1*L2*sin(x1) + 12*L1*T2*cos(x1)*cos(x1 + x2) + L1*L2*g*m1*cos(x1) + 2*L1*L2*g*m2*cos(x1) + 6*Fy*L1*L2*cos(x1)*cos(x1 + x2)^2 + 6*Fx*L1*L2*sin(x1)*cos(x1 + x2)...^2 - 6*Fy*L1*L2*cos(x1)*sin(x1 + x2)^2 - 6*Fx*L1*L2*sin(x1)*sin(x1 + x2)^2 + 3*L1*L2*g*m1*cos(x1)*cos(x1 + x2)^2 + 3*L1*L2*g*m1*cos(x1)*sin(x1 + x2)^2 + 6*L1*L2*g*m2*cos(x1)...    *sin(x1 + x2)^2 - L1*L2^2*dx1^2*m2*cos(x1)*sin(x1 + x2) + L1*L2^2*dx1^2*m2*sin(x1)*cos(x1 + x2) - L1*L2^2*dx2^2*m2*cos(x1)*sin(x1 + x2) + L1*L2^2*dx2^2*m2*sin(x1)*cos(x1 + x2)...  - 12*Fx*L1*L2*cos(x1)*cos(x1 + x2)*sin(x1 + x2) - 3*L1*L2^2*dx1^2*m2*cos(x1)*sin(x1 + x2)^3 + 3*L1*L2^2*dx1^2*m2*sin(x1)*cos(x1 + x2)^3 - 3*L1*L2^2*dx2^2*m2*cos(x1)*sin(x1 + x2)...^3 + 3*L1*L2^2*dx2^2*m2*sin(x1)*cos(x1 + x2)^3 + 12*Fy*L1*L2*sin(x1)*cos(x1 + x2)*sin(x1 + x2) + 6*L1^2*L2*dx1^2*m2*cos(x1)*sin(x1)*cos(x1 + x2)^2 - 6*L1^2*L2*dx1^2*m2*cos(x1)...  *sin(x1)*sin(x1 + x2)^2 - 6*L1*L2*g*m2*sin(x1)*cos(x1 + x2)*sin(x1 + x2) - 3*L1*L2^2*dx1^2*m2*cos(x1)*cos(x1 + x2)^2*sin(x1 + x2) - 6*L1^2*L2*dx1^2*m2*cos(x1)^2*cos(x1 + x2)...    *sin(x1 + x2) - 3*L1*L2^2*dx2^2*m2*cos(x1)*cos(x1 + x2)^2*sin(x1 + x2) - 2*L1*L2^2*dx1*dx2*m2*cos(x1)*sin(x1 + x2) + 2*L1*L2^2*dx1*dx2*m2*sin(x1)*cos(x1 + x2)...+ 3*L1*L2^2*dx1^2*m2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)^2 + 6*L1^2*L2*dx1^2*m2*sin(x1)^2*cos(x1 + x2)*sin(x1 + x2) + 3*L1*L2^2*dx2^2*m2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)...^2 - 6*L1*L2^2*dx1*dx2*m2*cos(x1)*sin(x1 + x2)^3 + 6*L1*L2^2*dx1*dx2*m2*sin(x1)*cos(x1 + x2)^3 - 6*L1*L2^2*dx1*dx2*m2*cos(x1)*cos(x1 + x2)^2*sin(x1 + x2)...+ 6*L1*L2^2*dx1*dx2*m2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)^2))/(2*(L1^2*L2*m1 + 3*L1^2*L2*m2*cos(x1)^2 + 3*L1^2*L2*m2*sin(x1)^2 + 3*L1^2*L2*m1*cos(x1 + x2)...^2 + 3*L1^2*L2*m1*sin(x1 + x2)^2 + 9*L1^2*L2*m2*cos(x1)^2*sin(x1 + x2)^2 + 9*L1^2*L2*m2*sin(x1)^2*cos(x1 + x2)^2 - 18*L1^2*L2*m2*cos(x1)*sin(x1)*cos(x1 + x2)*sin(x1 + x2)));ddx2 = (3*(8*L1^2*T2*m1 - 2*L2^2*T1*m2 + 2*L2^2*T2*m2 + 24*L1^2*T2*m2*cos(x1)^2 + 24*L1^2*T2*m2*sin(x1)^2 - 6*L2^2*T1*m2*cos(x1 + x2)^2 + 6*L2^2*T2*m2*cos(x1 + x2)...^2 - 6*L2^2*T1*m2*sin(x1 + x2)^2 + 6*L2^2*T2*m2*sin(x1 + x2)^2 - 2*Fy*L1*L2^2*m2*cos(x1) + 2*Fx*L1*L2^2*m2*sin(x1) + 8*Fy*L1^2*L2*m1*cos(x1 + x2) - 8*Fx*L1^2*L2*m1*sin(x1 + x2)...+ 2*L1*L2^2*g*m2^2*cos(x1) - 4*L1^2*L2*g*m1*m2*cos(x1 + x2) - 3*L1*L2^3*dx1^2*m2^2*cos(x1)*sin(x1 + x2)^3 + 3*L1*L2^3*dx1^2*m2^2*sin(x1)*cos(x1 + x2)...^3 - 12*L1^3*L2*dx1^2*m2^2*cos(x1)^3*sin(x1 + x2) + 12*L1^3*L2*dx1^2*m2^2*sin(x1)^3*cos(x1 + x2) - 3*L1*L2^3*dx2^2*m2^2*cos(x1)*sin(x1 + x2)^3 + 3*L1*L2^3*dx2^2*m2^2*sin(x1)...   *cos(x1 + x2)^3 + 6*Fy*L1*L2^2*m2*cos(x1)*cos(x1 + x2)^2 + 12*Fy*L1^2*L2*m2*cos(x1)^2*cos(x1 + x2) + 6*Fx*L1*L2^2*m2*sin(x1)*cos(x1 + x2)^2 - 24*Fx*L1^2*L2*m2*cos(x1)...^2*sin(x1 + x2) - 6*Fy*L1*L2^2*m2*cos(x1)*sin(x1 + x2)^2 + 24*Fy*L1^2*L2*m2*sin(x1)^2*cos(x1 + x2) - 6*Fx*L1*L2^2*m2*sin(x1)*sin(x1 + x2)^2 - 12*Fx*L1^2*L2*m2*sin(x1)...^2*sin(x1 + x2) - 12*L1*L2*T1*m2*cos(x1)*cos(x1 + x2) + 24*L1*L2*T2*m2*cos(x1)*cos(x1 + x2) - 12*L1*L2*T1*m2*sin(x1)*sin(x1 + x2) + 24*L1*L2*T2*m2*sin(x1)*sin(x1 + x2)...- L1*L2^3*dx1^2*m2^2*cos(x1)*sin(x1 + x2) + L1*L2^3*dx1^2*m2^2*sin(x1)*cos(x1 + x2) - L1*L2^3*dx2^2*m2^2*cos(x1)*sin(x1 + x2) + L1*L2^3*dx2^2*m2^2*sin(x1)*cos(x1 + x2)...+ 6*L1*L2^2*g*m2^2*cos(x1)*sin(x1 + x2)^2 - 12*L1^2*L2*g*m2^2*sin(x1)^2*cos(x1 + x2) + L1*L2^2*g*m1*m2*cos(x1) - 6*L1*L2^2*g*m2^2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)...- 12*L1^2*L2^2*dx1^2*m2^2*cos(x1)^2*cos(x1 + x2)*sin(x1 + x2) - 6*L1^2*L2^2*dx2^2*m2^2*cos(x1)^2*cos(x1 + x2)*sin(x1 + x2) - 6*L1*L2^3*dx1*dx2*m2^2*cos(x1)*sin(x1 + x2)...^3 + 6*L1*L2^3*dx1*dx2*m2^2*sin(x1)*cos(x1 + x2)^3 + 12*L1^2*L2^2*dx1^2*m2^2*sin(x1)^2*cos(x1 + x2)*sin(x1 + x2) + 6*L1^2*L2^2*dx2^2*m2^2*sin(x1)^2*cos(x1 + x2)*sin(x1 + x2)...+ 12*Fx*L1^2*L2*m2*cos(x1)*sin(x1)*cos(x1 + x2) - 12*Fy*L1^2*L2*m2*cos(x1)*sin(x1)*sin(x1 + x2) + 12*L1^3*L2*dx1^2*m2^2*cos(x1)^2*sin(x1)*cos(x1 + x2) - 12*Fx*L1*L2^2*m2*cos(x1)...*cos(x1 + x2)*sin(x1 + x2) - 12*L1^3*L2*dx1^2*m2^2*cos(x1)*sin(x1)^2*sin(x1 + x2) + 12*Fy*L1*L2^2*m2*sin(x1)*cos(x1 + x2)*sin(x1 + x2) - 3*L1*L2^3*dx1^2*m2^2*cos(x1)*cos(x1 + x2)...^2*sin(x1 + x2) - 3*L1*L2^3*dx2^2*m2^2*cos(x1)*cos(x1 + x2)^2*sin(x1 + x2) + 3*L1*L2^2*g*m1*m2*cos(x1)*cos(x1 + x2)^2 + 6*L1^2*L2*g*m1*m2*cos(x1)^2*cos(x1 + x2)...+ 12*L1^2*L2*g*m2^2*cos(x1)*sin(x1)*sin(x1 + x2) - 2*L1*L2^3*dx1*dx2*m2^2*cos(x1)*sin(x1 + x2) + 2*L1*L2^3*dx1*dx2*m2^2*sin(x1)*cos(x1 + x2) + 3*L1*L2^3*dx1^2*m2^2*sin(x1)...*cos(x1 + x2)*sin(x1 + x2)^2 + 3*L1*L2^3*dx2^2*m2^2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)^2 - 4*L1^3*L2*dx1^2*m1*m2*cos(x1)*sin(x1 + x2) + 4*L1^3*L2*dx1^2*m1*m2*sin(x1)*cos(x1 + x2)...+ 12*L1^2*L2^2*dx1^2*m2^2*cos(x1)*sin(x1)*cos(x1 + x2)^2 + 6*L1^2*L2^2*dx2^2*m2^2*cos(x1)*sin(x1)*cos(x1 + x2)^2 + 3*L1*L2^2*g*m1*m2*cos(x1)*sin(x1 + x2)...^2 - 12*L1^2*L2^2*dx1^2*m2^2*cos(x1)*sin(x1)*sin(x1 + x2)^2 - 6*L1^2*L2^2*dx2^2*m2^2*cos(x1)*sin(x1)*sin(x1 + x2)^2 - 12*L1^2*L2^2*dx1*dx2*m2^2*cos(x1)*sin(x1)*sin(x1 + x2)...^2 - 12*L1^2*L2^2*dx1*dx2*m2^2*cos(x1)^2*cos(x1 + x2)*sin(x1 + x2) + 12*L1^2*L2^2*dx1*dx2*m2^2*sin(x1)^2*cos(x1 + x2)*sin(x1 + x2) - 6*L1*L2^3*dx1*dx2*m2^2*cos(x1)*cos(x1 + x2)...^2*sin(x1 + x2) + 6*L1*L2^3*dx1*dx2*m2^2*sin(x1)*cos(x1 + x2)*sin(x1 + x2)^2 + 6*L1^2*L2*g*m1*m2*cos(x1)*sin(x1)*sin(x1 + x2) + 12*L1^2*L2^2*dx1*dx2*m2^2*cos(x1)*sin(x1)...*cos(x1 + x2)^2))/(2*(3*L1^2*L2^2*m2^2*cos(x1)^2 + 3*L1^2*L2^2*m2^2*sin(x1)^2 + L1^2*L2^2*m1*m2 + 9*L1^2*L2^2*m2^2*cos(x1)^2*sin(x1 + x2)^2 + 9*L1^2*L2^2*m2^2*sin(x1)...^2*cos(x1 + x2)^2 + 3*L1^2*L2^2*m1*m2*cos(x1 + x2)^2 + 3*L1^2*L2^2*m1*m2*sin(x1 + x2)^2 - 18*L1^2*L2^2*m2^2*cos(x1)*sin(x1)*cos(x1 + x2)*sin(x1 + x2)));dxdt=[dx1;dx2;ddx1;ddx2];%返回状态向量的一阶导
end

拉格朗日方法回顾

系统广义坐标为两个关节角${\theta }_1$和${\theta }_2$，拉格朗日公式为：
$$\begin{gathered} L=K-P......(1) \\ \frac{d}{dt}(\frac{\partial L}{\partial {\dot{\theta }}_i})-\frac{\partial L}{\partial {\theta }_i}=Q_i,i=1,\mathrm{2......}(2) \end{gathered}$$
其中L是拉格朗日函数，K是系统动能，P是系统势能，$Q_i$表示关节力矩 + 所有外力等效到该关节的力矩。将公式(2)求导展开，并考虑机器人末端受力，会得到如下形式：
$$\mathbf{M}(\Theta )\ddot{\Theta }+\mathbf{C}(\Theta ,\dot{\Theta })=\mathbf{Q}+{\mathbf{J}}_e^{\text{T}}{\mathbf{F}}_e,\text{where }\Theta =\left(\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right)......(3)$$
其中M是加速度矩阵，C是低阶项，Q表示关节自身力矩，如关节阻尼力、电机驱动力等，Fe是末端受到的外力，Je是机器人末端雅可比矩阵，可以把笛卡尔坐标系下定义的外力映射到关节空间，化为等效的关节力矩。
实际上这个“末端”也可以是机器人身上任意一点，把每个受力点对应的$J^{\text{T}}F$项直接加在公式（3）右边就行了。本文案例中只有一个全局坐标系，Fe也是定义在这个坐标系下。

求解符号表达式

首先用syms语句定义结构常量和广义坐标，需要把广义坐标定义为关于时间的函数，方便后面求导：

syms m1 m2 L1 L2 g % 结构常量
syms t x1(t) x2(t) % 将系统的广义坐标定义为时间函数

定义连杆转动惯量，后面要用：

IA=1/3*m1*L1^2; % 连杆1惯量
ID=1/12*m2*L2^2;% 连杆2惯量

定义连杆2中心的位置公式，用diff函数对时间求导得到速度：

pD=[L1*cos(x1)+1/2*L2*cos(x1+x2); %连杆2质心位置L1*sin(x1)+1/2*L2*sin(x1+x2)];
vD=diff(pD,t); %对时间求导得到速度

编辑动能和势能，得到拉格朗日函数：

K=1/2*IA*diff(x1,t)^2+1/2*ID*(diff(x1,t)+diff(x2,t))^2+1/2*m2*sum(vD.^2); %系统动能
P=m1*g*L1/2*sin(x1)+m2*g*(L1*sin(x1)+L2/2*sin(x1+x2)); % 系统势能
L=K-P; %拉格朗日函数

按公式2展开：

temp=diff(diff(L,diff(x1,t)),t)-diff(L,x1); % 展开拉格朗日函数，得到二阶微分式

matlab生成的表达式中，会用diff(x1,t)表示速度，用diff(x1,t,t)表示加速度，为了便于后面代入数值，需要定义新的符号变量，表示关节角的速度和加速度，然后替换到公式里：

syms dx1 dx2 ddx1 ddx2
temp=subs(temp,diff(x1,t,t),ddx1); % 用新定义的符号代替广义坐标的一二阶导数，简化公式表达
temp=subs(temp,diff(x2,t,t),ddx2);
temp=subs(temp,diff(x1,t),dx1);
temp=subs(temp,diff(x2,t),dx2);

按关节角的加速度项、平方项、交叉项，对微分式进行合并同类项：

diff1=collect(temp,[ddx1,ddx2,dx1^2,dx2^2,dx1*dx2,dx1,dx2]); % 合并同类项，整理成便于阅读的形式

这时候用pretty(diff1)指令，可以看到微分式的内容：

matlab不会把sin^2+cos^2替换为1，不会合并因子，所以比较冗长，但也够用了。
对第二个关节角做同样操作，得到第二行微分式：

temp=diff(diff(L,diff(x2,t)),t)-diff(L,x2); % 对第二个广义坐标θ2做同样操作
temp=subs(temp,diff(x1,t,t),ddx1);
temp=subs(temp,diff(x2,t,t),ddx2);
temp=subs(temp,diff(x1,t),dx1);
temp=subs(temp,diff(x2,t),dx2);
diff2=collect(temp,[ddx1,ddx2,dx1^2,dx2^2,dx1*dx2,dx1,dx2]);

为了加入外力，需要求末端雅可比矩阵，这里有个技巧，如果直接用刚才定义的x1(t), x2(t)会导致 jacobian() 返回一个时间函数，不能参与矩阵运算了，因此需要先定义普通符号变量x1v, x2v，求出雅可比后再用x1(t)和x2(t)替换。

syms x1v x2v %求雅可比矩阵不能用时间函数x1(t)和x2(t)，因此先定义临时变量，求雅可比后再替换为x1和x2
p_end = [L1*cos(x1v)+L2*cos(x1v+x2v);L1*sin(x1v)+L2*sin(x1v+x2v)]; % 末端位置
J_end = jacobian(p_end,[x1v;x2v]); % 末端雅可比矩阵
J_end = subs(J_end,{x1v,x2v},{x1,x2}); %替换时间变量

定义符号变量表示关节力矩和外力，然后构建微分方程组，即为机器人动力学模型：

syms T1 T2 Fx Fy
eqn = [diff1;diff2] == [T1;T2] + J_end.'*[Fx;Fy]; %构建动力学矩阵方程式

这时候标准做法是通过对比公式(3)得到矩阵M，C的表达式，在后面数值求解函数中带入数据得到M，C的数值，再解出$\ddot{\Theta }$。
但是对于自由度较少的系统，可以直接让matlab解出$\ddot{\Theta }$的解析式，更加省事：

sol = solve(eqn,[ddx1;ddx2]); %求出二阶项[ddx1;ddx2]的解析解
fprintf("ddx1=");%输出求解结果
disp(sol.ddx1);
fprintf("ddx2=");
disp(sol.ddx2);

会得到很长的代数式，为了方面后面使用，在GPT帮助下生成了一段python代码，它先把表达式中所有"(t)"去掉，再把公式拆分为多行，python代码如下：

# 把很长的Matlab符号表达式拆分为多行def format_matlab_expression(expression, line_length=180):  # 定义运算符  operators = ['+', '-', '*', '/', '(', ')']  # 初始化变量  formatted_expression = ""  current_line = ""  # 遍历表达式中的每个字符  for char in expression:  current_line += char  # 检查当前行的长度  if len(current_line) >= line_length:  # 找到最近的运算符  for op in reversed(operators):  if op in current_line:  # 找到运算符的位置  op_index = current_line.rfind(op)  # 在运算符前换行  formatted_expression += current_line[:op_index + 1] + "...\n"  # 更新当前行  current_line = current_line[op_index + 1:].lstrip()  # 去掉运算符前的空格  break  # 添加最后一行（如果有剩余内容）  if current_line:  formatted_expression += current_linereturn formatted_expression  # 示例 MATLAB 表达式  
matlab_expression = "-(3*(2*L2*T2 - 2*L2*T1 - 6*L2*T1*sin(x1(t) + x2(t))^2 + 6*L2*T2*sin(x1(t) + x2(t))^2 - 6*L2*T1*cos(x1(t) + x2(t))^2 + 6*L2*T2*cos(x1(t) + x2(t))^2 + 12*L1*T2*sin(x1(t))*sin(x1(t) + x2(t)) - 2*Fy*L1*L2*cos(x1(t)) + 2*Fx*L1*L2*sin(x1(t)) + 12*L1*T2*cos(x1(t))*cos(x1(t) + x2(t)) + L1*L2*g*m1*cos(x1(t)) + 2*L1*L2*g*m2*cos(x1(t)) + 6*Fy*L1*L2*cos(x1(t))*cos(x1(t) + x2(t))^2 + 6*Fx*L1*L2*sin(x1(t))*cos(x1(t) + x2(t))^2 - 6*Fy*L1*L2*cos(x1(t))*sin(x1(t) + x2(t))^2 - 6*Fx*L1*L2*sin(x1(t))*sin(x1(t) + x2(t))^2 + 3*L1*L2*g*m1*cos(x1(t))*cos(x1(t) + x2(t))^2 + 3*L1*L2*g*m1*cos(x1(t))*sin(x1(t) + x2(t))^2 + 6*L1*L2*g*m2*cos(x1(t))*sin(x1(t) + x2(t))^2 - L1*L2^2*dx1^2*m2*cos(x1(t))*sin(x1(t) + x2(t)) + L1*L2^2*dx1^2*m2*sin(x1(t))*cos(x1(t) + x2(t)) - L1*L2^2*dx2^2*m2*cos(x1(t))*sin(x1(t) + x2(t)) + L1*L2^2*dx2^2*m2*sin(x1(t))*cos(x1(t) + x2(t)) - 12*Fx*L1*L2*cos(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t)) - 3*L1*L2^2*dx1^2*m2*cos(x1(t))*sin(x1(t) + x2(t))^3 + 3*L1*L2^2*dx1^2*m2*sin(x1(t))*cos(x1(t) + x2(t))^3 - 3*L1*L2^2*dx2^2*m2*cos(x1(t))*sin(x1(t) + x2(t))^3 + 3*L1*L2^2*dx2^2*m2*sin(x1(t))*cos(x1(t) + x2(t))^3 + 12*Fy*L1*L2*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t)) + 6*L1^2*L2*dx1^2*m2*cos(x1(t))*sin(x1(t))*cos(x1(t) + x2(t))^2 - 6*L1^2*L2*dx1^2*m2*cos(x1(t))*sin(x1(t))*sin(x1(t) + x2(t))^2 - 6*L1*L2*g*m2*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t)) - 3*L1*L2^2*dx1^2*m2*cos(x1(t))*cos(x1(t) + x2(t))^2*sin(x1(t) + x2(t)) - 6*L1^2*L2*dx1^2*m2*cos(x1(t))^2*cos(x1(t) + x2(t))*sin(x1(t) + x2(t)) - 3*L1*L2^2*dx2^2*m2*cos(x1(t))*cos(x1(t) + x2(t))^2*sin(x1(t) + x2(t)) - 2*L1*L2^2*dx1*dx2*m2*cos(x1(t))*sin(x1(t) + x2(t)) + 2*L1*L2^2*dx1*dx2*m2*sin(x1(t))*cos(x1(t) + x2(t)) + 3*L1*L2^2*dx1^2*m2*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t))^2 + 6*L1^2*L2*dx1^2*m2*sin(x1(t))^2*cos(x1(t) + x2(t))*sin(x1(t) + x2(t)) + 3*L1*L2^2*dx2^2*m2*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t))^2 - 6*L1*L2^2*dx1*dx2*m2*cos(x1(t))*sin(x1(t) + x2(t))^3 + 6*L1*L2^2*dx1*dx2*m2*sin(x1(t))*cos(x1(t) + x2(t))^3 - 6*L1*L2^2*dx1*dx2*m2*cos(x1(t))*cos(x1(t) + x2(t))^2*sin(x1(t) + x2(t)) + 6*L1*L2^2*dx1*dx2*m2*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t))^2))/(2*(L1^2*L2*m1 + 3*L1^2*L2*m2*cos(x1(t))^2 + 3*L1^2*L2*m2*sin(x1(t))^2 + 3*L1^2*L2*m1*cos(x1(t) + x2(t))^2 + 3*L1^2*L2*m1*sin(x1(t) + x2(t))^2 + 9*L1^2*L2*m2*cos(x1(t))^2*sin(x1(t) + x2(t))^2 + 9*L1^2*L2*m2*sin(x1(t))^2*cos(x1(t) + x2(t))^2 - 18*L1^2*L2*m2*cos(x1(t))*sin(x1(t))*cos(x1(t) + x2(t))*sin(x1(t) + x2(t))))"
matlab_expression = matlab_expression.replace("(t)", "") # 删掉所有(t)
# 格式化表达式  
formatted = format_matlab_expression(matlab_expression)  # 打印结果  
print(formatted)

把python脚本输出的表达式放在最后的自定义函数odefun中，用来数值求解。

数值求解

网上已经有很多ode45函数的用法了。在这部分是先把公式中的参数定义为全局变量并赋值，以便被脚本代码和odefun函数共享：

global m1 m2 L1 L2 g d1 d2 Fx Fy
m1=1; m2=1; L1=0.5; L2=0.5; g=9.81;d1=0.8;d2=d1;Fx=0;Fy=0;%设置机器人参数,关节阻尼和外力

然后设置好时间向量和初始状态，执行ode45函数：

tspan = 0:0.01:5; % 时间范围
initial_state = [0;0;0;0]; % 初始状态
[t,y] = ode45(@odefun,tspan,initial_state); % 求解

为了用ode45求解，需要把公式(3)化为一阶系统，新的状态向量是：
$$\mathbf{x}=\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$$
一阶系统方程为
$$\dot{\mathbf{x}}=\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\\ {\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]=\left[\begin{array}{c}\mathbf{x}(3)\\ \mathbf{x}(4)\\ {\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]$$
其中${\ddot{\theta }}_1$和${\ddot{\theta }}_2$就来自前面求解得到的加速度项解析式。

此外在odefun中需要指定关节力矩和外力的数值，示例代码中是给两个关节添加了阻尼力矩，给末端添加了一个恒定的外力。实际项目中可以根据需要，设置变化的力。odefun如下：

function dxdt=odefun(t,x)global m1 m2 L1 L2 g d1 d2 Fx Fyx1=x(1);x2=x(2);dx1=x(3);dx2=x(4); %状态向量即为θ1，θ及其一阶导数T1 = -d1*dx1;T2 = -d2*dx2;% 根据符号工具的求解结果，得到θ1，θ2二阶导的表达式如下，需要删掉符号表达式中所有的(t)以免报错ddx1 = （省略）ddx2 = （省略）dxdt=[dx1;dx2;ddx1;ddx2];%返回状态向量的一阶导
end

ode45返回的y的两列数据即为广义坐标θ1，θ2随时间变化的数值，绘制成曲线如下：