目录
- 介绍
- 代码功能演示
- 拉格朗日方法回顾
- 求解符号表达式
- 数值求解
介绍
开发机器人过程中经常需要用牛顿-拉格朗日法建立机器人的动力学模型，表示为二阶微分方程组。本文以一个二杆系统为例，介绍如何用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
拉格朗日方法回顾
系统广义坐标为两个关节角 θ 1 \theta_1 θ1和 θ 2 \theta_2 θ2，拉格朗日公式为：
L = K − P . . . . . . ( 1 ) d d t ( ∂ L ∂ θ ˙ i ) − ∂ L ∂ θ i = Q i , i = 1 , 2...... ( 2 ) L=K-P......(1)\newline \frac{d}{dt}(\frac{\partial L}{\partial \dot \theta_i})-\frac{\partial L}{\partial \theta_i}=Q_i, i=1,2 ......(2) L=K−P......(1)dtd(∂θ˙i∂L)−∂θi∂L=Qi,i=1,2......(2)
其中L是拉格朗日函数，K是系统动能，P是系统势能， Q i Q_i Qi表示关节力矩 + 所有外力等效到该关节的力矩。将公式(2)求导展开，并考虑机器人末端受力，会得到如下形式：
M ( Θ ) Θ ¨ + C ( Θ , Θ ˙ ) = Q + J e T F e , where Θ = ( θ 1 θ 2 ) . . . . . . ( 3 ) \bold M(\Theta)\ddot\Theta +\bold C(\Theta,\dot\Theta)=\bold Q+\bold J_{e}^\text T\bold F_{e}, \text{where } \Theta=\begin{pmatrix} \ \theta_1 \\ \ \theta_2 \end{pmatrix}......(3) M(Θ)Θ¨+C(Θ,Θ˙)=Q+JeTFe,where Θ=( θ1 θ2)......(3)
其中M是加速度矩阵，C是低阶项，Q表示关节自身力矩，如关节阻尼力、电机驱动力等，Fe是末端受到的外力，Je是机器人末端雅可比矩阵，可以把笛卡尔坐标系下定义的外力映射到关节空间，化为等效的关节力矩。
实际上这个“末端”也可以是机器人身上任意一点，把每个受力点对应的 J T F J^\text{T}F JTF项直接加在公式（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)化为一阶系统，新的状态向量是：
x = [ θ 1 θ 2 θ ˙ 1 θ ˙ 2 ] \bold{x}= \begin{bmatrix} \ \theta_1 \\ \ \theta_2 \\ \ \dot\theta_1 \\ \ \dot\theta_2 \end{bmatrix} x= θ1 θ2 θ˙1 θ˙2
一阶系统方程为
x ˙ = [ θ ˙ 1 θ ˙ 2 θ ¨ 1 θ ¨ 2 ] = [ x ( 3 ) x ( 4 ) θ ¨ 1 θ ¨ 2 ] \bold{\dot x}= \begin{bmatrix} \ \dot\theta_1 \\ \ \dot\theta_2 \\ \ \ddot\theta_1 \\ \ \ddot\theta_2 \end{bmatrix}= \begin{bmatrix} \ \bold{x}(3) \\ \ \bold{x}(4) \\ \ \ddot\theta_1 \\ \ \ddot\theta_2 \end{bmatrix} x˙= θ˙1 θ˙2 θ¨1 θ¨2 = x(3) x(4) θ¨1 θ¨2
其中 θ ¨ 1 \ddot\theta_1 θ¨1和 θ ¨ 2 \ddot\theta_2 θ¨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随时间变化的数值，绘制成曲线如下：
