介紹

開發(fā)機(jī)器人過程中經(jīng)常需要用牛頓-拉格朗日法建立機(jī)器人的動(dòng)力學(xué)模型，表示為二階微分方程組。本文以一個(gè)二桿系統(tǒng)為例，介紹如何用MATLAB符號(hào)工具得到微分方程表達(dá)式，只需要編輯好物點(diǎn)的位置公式和系統(tǒng)動(dòng)能、勢(shì)能，就能得到微分方程組，避免繁瑣的手工推導(dǎo)工作。

代碼功能演示

先放運(yùn)行效果：沒有外力：

施加5N向上外力：

采用《機(jī)器人學(xué)導(dǎo)論——分析、控制及應(yīng)用（第二版）》一書中例4.4的自由二連桿，原例題和建模過程如下：


全部代碼如下，直接復(fù)制到Matlab中即可運(yùn)行，其中第一節(jié)是建模，得到加速度項(xiàng)的表達(dá)式，第二節(jié)是帶入數(shù)據(jù)進(jìn)行數(shù)值求解：

% 以自由二連桿為例，展示Matlab符號(hào)工具建立牛頓-拉格朗日動(dòng)力學(xué)方程，并用ODE45函數(shù)數(shù)值求解的過程%% 建模
syms m1 m2 L1 L2 g % 結(jié)構(gòu)常量
syms t x1(t) x2(t) % 將系統(tǒng)的廣義坐標(biāo)定義為時(shí)間函數(shù)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質(zhì)心位置L1*sin(x1)+1/2*L2*sin(x1+x2)];
vD=diff(pD,t); %對(duì)時(shí)間求導(dǎo)得到速度K=1/2*IA*diff(x1,t)^2+1/2*ID*(diff(x1,t)+diff(x2,t))^2+1/2*m2*sum(vD.^2); %系統(tǒng)動(dòng)能P=m1*g*L1/2*sin(x1)+m2*g*(L1*sin(x1)+L2/2*sin(x1+x2)); % 系統(tǒng)勢(shì)能L=K-P; %拉格朗日函數(shù)syms dx1 dx2 ddx1 ddx2
temp=diff(diff(L,diff(x1,t)),t)-diff(L,x1); % 展開拉格朗日函數(shù)，得到二階微分式
temp=subs(temp,diff(x1,t,t),ddx1); % 用新定義的符號(hào)代替廣義坐標(biāo)的一二階導(dǎo)數(shù)，簡化公式表達(dá)
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]); % 合并同類項(xiàng)，整理成便于閱讀的形式temp=diff(diff(L,diff(x2,t)),t)-diff(L,x2); % 對(duì)第二個(gè)廣義坐標(biāo)θ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 %求雅可比矩陣不能用時(shí)間函數(shù)x1(t)和x2(t)，因此先定義臨時(shí)變量，求雅可比后再替換為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]; %構(gòu)建動(dòng)力學(xué)矩陣方程式
sol = solve(eqn,[ddx1;ddx2]); %求出二階項(xiàng)[ddx1;ddx2]的解析解%輸出求解結(jié)果，需要把結(jié)果表達(dá)式粘貼到新文件中，將其中的(t)全都刪掉，然后粘貼到最下面odefun中v1和v2的表達(dá)式
fprintf("ddx1=");
disp(sol.ddx1);
fprintf("ddx2=");
disp(sol.ddx2);
fprintf("需要把結(jié)果表達(dá)式中的(t)全都刪掉，然后粘貼到最下面odefun中ddx1和ddx2的表達(dá)式\n");%% 數(shù)值求解
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;%設(shè)置機(jī)器人參數(shù),關(guān)節(jié)阻尼和外力tspan = 0:0.01:5; % 時(shí)間范圍
[t,y] = ode45(@odefun,tspan,[0;0;0;0]); % 求解figure(1);clf; % 繪制運(yùn)動(dòng)動(dòng)畫
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); % 繪制關(guān)節(jié)角變化曲線
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;% 為了求數(shù)值解需要化為一階系統(tǒng)，以下為一階系統(tǒng)的狀態(tài)向量：
% 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); %狀態(tài)向量即為θ1，θ及其一階導(dǎo)數(shù)T1 = -d1*dx1;T2 = -d2*dx2;% 根據(jù)符號(hào)工具的求解結(jié)果，得到θ1，θ2二階導(dǎo)的表達(dá)式如下，需要?jiǎng)h掉符號(hào)表達(dá)式中所有的(t)以免報(bào)錯(cuò)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];%返回狀態(tài)向量的一階導(dǎo)
end

拉格朗日方法回顧

系統(tǒng)廣義坐標(biāo)為兩個(gè)關(guān)節(jié)角${\theta }_1$和${\theta }_2$，拉格朗日公式為：
$$\begin{gathered} L=K?P......(1) \\ \frac{d}{dt}(\frac{?L}{?{\dot{\theta }}_i})?\frac{?L}{?{\theta }_i}=Q_i,i=1,\mathrm{2......}(2) \end{gathered}$$
其中L是拉格朗日函數(shù)，K是系統(tǒng)動(dòng)能，P是系統(tǒng)勢(shì)能，$Q_i$表示關(guān)節(jié)力矩 + 所有外力等效到該關(guān)節(jié)的力矩。將公式(2)求導(dǎo)展開，并考慮機(jī)器人末端受力，會(huì)得到如下形式：
$$\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是低階項(xiàng)，Q表示關(guān)節(jié)自身力矩，如關(guān)節(jié)阻尼力、電機(jī)驅(qū)動(dòng)力等，Fe是末端受到的外力，Je是機(jī)器人末端雅可比矩陣，可以把笛卡爾坐標(biāo)系下定義的外力映射到關(guān)節(jié)空間，化為等效的關(guān)節(jié)力矩。
實(shí)際上這個(gè)“末端”也可以是機(jī)器人身上任意一點(diǎn)，把每個(gè)受力點(diǎn)對(duì)應(yīng)的$J^{\text{T}}F$項(xiàng)直接加在公式（3）右邊就行了。本文案例中只有一個(gè)全局坐標(biāo)系，Fe也是定義在這個(gè)坐標(biāo)系下。

求解符號(hào)表達(dá)式

首先用syms語句定義結(jié)構(gòu)常量和廣義坐標(biāo)，需要把廣義坐標(biāo)定義為關(guān)于時(shí)間的函數(shù)，方便后面求導(dǎo)：

syms m1 m2 L1 L2 g % 結(jié)構(gòu)常量
syms t x1(t) x2(t) % 將系統(tǒng)的廣義坐標(biāo)定義為時(shí)間函數(shù)

定義連桿轉(zhuǎn)動(dòng)慣量，后面要用：

IA=1/3*m1*L1^2; % 連桿1慣量
ID=1/12*m2*L2^2;% 連桿2慣量

定義連桿2中心的位置公式，用diff函數(shù)對(duì)時(shí)間求導(dǎo)得到速度：

pD=[L1*cos(x1)+1/2*L2*cos(x1+x2); %連桿2質(zhì)心位置L1*sin(x1)+1/2*L2*sin(x1+x2)];
vD=diff(pD,t); %對(duì)時(shí)間求導(dǎo)得到速度

編輯動(dòng)能和勢(shì)能，得到拉格朗日函數(shù)：

K=1/2*IA*diff(x1,t)^2+1/2*ID*(diff(x1,t)+diff(x2,t))^2+1/2*m2*sum(vD.^2); %系統(tǒng)動(dòng)能
P=m1*g*L1/2*sin(x1)+m2*g*(L1*sin(x1)+L2/2*sin(x1+x2)); % 系統(tǒng)勢(shì)能
L=K-P; %拉格朗日函數(shù)

按公式2展開：

temp=diff(diff(L,diff(x1,t)),t)-diff(L,x1); % 展開拉格朗日函數(shù)，得到二階微分式

matlab生成的表達(dá)式中，會(huì)用diff(x1,t)表示速度，用diff(x1,t,t)表示加速度，為了便于后面代入數(shù)值，需要定義新的符號(hào)變量，表示關(guān)節(jié)角的速度和加速度，然后替換到公式里：

syms dx1 dx2 ddx1 ddx2
temp=subs(temp,diff(x1,t,t),ddx1); % 用新定義的符號(hào)代替廣義坐標(biāo)的一二階導(dǎo)數(shù)，簡化公式表達(dá)
temp=subs(temp,diff(x2,t,t),ddx2);
temp=subs(temp,diff(x1,t),dx1);
temp=subs(temp,diff(x2,t),dx2);

按關(guān)節(jié)角的加速度項(xiàng)、平方項(xiàng)、交叉項(xiàng)，對(duì)微分式進(jìn)行合并同類項(xiàng)：

diff1=collect(temp,[ddx1,ddx2,dx1^2,dx2^2,dx1*dx2,dx1,dx2]); % 合并同類項(xiàng)，整理成便于閱讀的形式

這時(shí)候用pretty(diff1)指令，可以看到微分式的內(nèi)容：

matlab不會(huì)把sin^2+cos^2替換為1，不會(huì)合并因子，所以比較冗長，但也夠用了。
對(duì)第二個(gè)關(guān)節(jié)角做同樣操作，得到第二行微分式：

temp=diff(diff(L,diff(x2,t)),t)-diff(L,x2); % 對(duì)第二個(gè)廣義坐標(biāo)θ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]);

為了加入外力，需要求末端雅可比矩陣，這里有個(gè)技巧，如果直接用剛才定義的x1(t), x2(t)會(huì)導(dǎo)致 jacobian() 返回一個(gè)時(shí)間函數(shù)，不能參與矩陣運(yùn)算了，因此需要先定義普通符號(hào)變量x1v, x2v，求出雅可比后再用x1(t)和x2(t)替換。

syms x1v x2v %求雅可比矩陣不能用時(shí)間函數(shù)x1(t)和x2(t)，因此先定義臨時(shí)變量，求雅可比后再替換為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}); %替換時(shí)間變量

定義符號(hào)變量表示關(guān)節(jié)力矩和外力，然后構(gòu)建微分方程組，即為機(jī)器人動(dòng)力學(xué)模型：

syms T1 T2 Fx Fy
eqn = [diff1;diff2] == [T1;T2] + J_end.'*[Fx;Fy]; %構(gòu)建動(dòng)力學(xué)矩陣方程式

這時(shí)候標(biāo)準(zhǔn)做法是通過對(duì)比公式(3)得到矩陣M，C的表達(dá)式，在后面數(shù)值求解函數(shù)中帶入數(shù)據(jù)得到M，C的數(shù)值，再解出$\ddot{\Theta }$。
但是對(duì)于自由度較少的系統(tǒng)，可以直接讓matlab解出$\ddot{\Theta }$的解析式，更加省事：

sol = solve(eqn,[ddx1;ddx2]); %求出二階項(xiàng)[ddx1;ddx2]的解析解
fprintf("ddx1=");%輸出求解結(jié)果
disp(sol.ddx1);
fprintf("ddx2=");
disp(sol.ddx2);

會(huì)得到很長的代數(shù)式，為了方面后面使用，在GPT幫助下生成了一段python代碼，它先把表達(dá)式中所有"(t)"去掉，再把公式拆分為多行，python代碼如下：

# 把很長的Matlab符號(hào)表達(dá)式拆分為多行def format_matlab_expression(expression, line_length=180):  # 定義運(yùn)算符  operators = ['+', '-', '*', '/', '(', ')']  # 初始化變量  formatted_expression = ""  current_line = ""  # 遍歷表達(dá)式中的每個(gè)字符  for char in expression:  current_line += char  # 檢查當(dāng)前行的長度  if len(current_line) >= line_length:  # 找到最近的運(yùn)算符  for op in reversed(operators):  if op in current_line:  # 找到運(yùn)算符的位置  op_index = current_line.rfind(op)  # 在運(yùn)算符前換行  formatted_expression += current_line[:op_index + 1] + "...\n"  # 更新當(dāng)前行  current_line = current_line[op_index + 1:].lstrip()  # 去掉運(yùn)算符前的空格  break  # 添加最后一行（如果有剩余內(nèi)容）  if current_line:  formatted_expression += current_linereturn formatted_expression  # 示例 MATLAB 表達(dá)式  
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)
# 格式化表達(dá)式  
formatted = format_matlab_expression(matlab_expression)  # 打印結(jié)果  
print(formatted)

把python腳本輸出的表達(dá)式放在最后的自定義函數(shù)odefun中，用來數(shù)值求解。

數(shù)值求解

網(wǎng)上已經(jīng)有很多ode45函數(shù)的用法了。在這部分是先把公式中的參數(shù)定義為全局變量并賦值，以便被腳本代碼和odefun函數(shù)共享：

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;%設(shè)置機(jī)器人參數(shù),關(guān)節(jié)阻尼和外力

然后設(shè)置好時(shí)間向量和初始狀態(tài)，執(zhí)行ode45函數(shù)：

tspan = 0:0.01:5; % 時(shí)間范圍
initial_state = [0;0;0;0]; % 初始狀態(tài)
[t,y] = ode45(@odefun,tspan,initial_state); % 求解

為了用ode45求解，需要把公式(3)化為一階系統(tǒng)，新的狀態(tài)向量是：
$$\mathbf{x}=\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$$
一階系統(tǒng)方程為
$$\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$就來自前面求解得到的加速度項(xiàng)解析式。

此外在odefun中需要指定關(guān)節(jié)力矩和外力的數(shù)值，示例代碼中是給兩個(gè)關(guān)節(jié)添加了阻尼力矩，給末端添加了一個(gè)恒定的外力。實(shí)際項(xiàng)目中可以根據(jù)需要，設(shè)置變化的力。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); %狀態(tài)向量即為θ1，θ及其一階導(dǎo)數(shù)T1 = -d1*dx1;T2 = -d2*dx2;% 根據(jù)符號(hào)工具的求解結(jié)果，得到θ1，θ2二階導(dǎo)的表達(dá)式如下，需要?jiǎng)h掉符號(hào)表達(dá)式中所有的(t)以免報(bào)錯(cuò)ddx1 = （省略）ddx2 = （省略）dxdt=[dx1;dx2;ddx1;ddx2];%返回狀態(tài)向量的一階導(dǎo)
end

ode45返回的y的兩列數(shù)據(jù)即為廣義坐標(biāo)θ1，θ2隨時(shí)間變化的數(shù)值，繪制成曲線如下：