用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

拉格朗日方法回顾

系统广义坐标为两个关节角 θ 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+JeT​Fe​,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随时间变化的数值，绘制成曲线如下：
在这里插入图片描述

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/bicheng/62670.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

SpringAi整合大模型(进阶版)

SpringAi整合大模型(进阶版)

进阶版是在基础的对话版之上进行新增功能。 如果还没弄出基础版的，请参考 https://blog.csdn.net/weixin_54925172/article/details/144143523?sharetypeblogdetail&sharerId144143523&sharereferPC&sharesourceweixin_54925172&spm1011.2480.30…
阅读更多...
Android电视项目焦点跨层级流转

Android电视项目焦点跨层级流转

1. 背景 在智家电视项目中，主要操作方式不是触摸，而是遥控器，通过Focus进行移动，确定点击进行的交互，所以在电视项目中焦点、选中、确定、返回这几个交互比较重要。由于电视屏比较大，在一些复杂页面中会存…
阅读更多...
yolo辅助我们健身锻炼

yolo辅助我们健身锻炼

使用软件辅助健身能够大大提升运动效果并帮助你更轻松地达成健身目标。确保每次锻炼都更加高效且针对性强，精确记录你的训练进度，帮助你更清晰地看到自己的进步，避免无效训练。 借助YOLO11的尖端计算机视觉技术，跟踪和分析锻炼变得异常简单。它可以无缝检测和监控多种锻炼…
阅读更多...
Flume 与 Kafka 整合实战

Flume 与 Kafka 整合实战

目录 一、Kafka 作为 Source【数据进入到kafka中，抽取出来】 （一）环境准备与配置文件创建 （二）创建主题 （三）测试步骤 二、Kafka 作为 Sink数据从别的地方抽取到kafka里面】 （…
阅读更多...
SRS搭建直播推流服务

SRS搭建直播推流服务

学习链接 5分钟教你搭建SRS流媒体服务器 - B站视频 SRS Stack 入门B站合集视频 - SRS官方教程 SRS官网 SRS官网文档 ossrs/srs github SRS for window - 可以安装windows版本的srs，SRS 5.0.89正式支持Windows，每个5.0的版本都会提供安装包 文章目录…
阅读更多...
css—轮播图实现

css—轮播图实现

一、背景 最近和朋友在一起讨论的时候，我们提出了这样的一个提问，难道轮播图的效果只能通过js来实现吗？经过我们的一系列的争论，发现了这是可以通过纯css来实现这一效果的，CSS轮播图也是一种常见的网页展示方式&#x…
阅读更多...
nacos安装部署

nacos安装部署

nacos安装部署 1.安装nacos 1.安装nacos nacos的安装很简单下载后解压启动即可，但是在启动前请确保jdk环境正常； 1.首先我们要下载nacos安装包：可以到官网下载，注意我这里使用的是2.1.0版本； 2.下载完成后&#xff0…
阅读更多...
tomcat 8.5.35安装及配置

tomcat 8.5.35安装及配置

安装包地址： 1.Index of /dist/tomcat/tomcat-8/v8.5.35/binhttps://archive.apache.org/dist/tomcat/tomcat-8/v8.5.35/bin/ 2.通过网盘分享的文件：tomcat 链接: https://pan.baidu.com/s/1z9bD4rIuIRvzQ4okm3iRzw?pwdp24p 提取码: p24p 3.通过官网…
阅读更多...
YOLO系列论文综述（从YOLOv1到YOLOv11）【第12篇：YOLOv9——可编程梯度信息(PGI)+广义高效层聚合网络(GELAN)】

YOLO系列论文综述（从YOLOv1到YOLOv11）【第12篇：YOLOv9——可编程梯度信息(PGI)+广义高效层聚合网络(GELAN)】

YOLOv9 1 摘要2 改进点3 网络架构 YOLO系列博文： 【第1篇：概述物体检测算法发展史、YOLO应用领域、评价指标和NMS】【第2篇：YOLO系列论文、代码和主要优缺点汇总】【第3篇：YOLOv1——YOLO的开山之作】【第4篇：YOLOv2—…
阅读更多...
机器学习提高电子病历主要诊断编码正确率的路径分析

机器学习提高电子病历主要诊断编码正确率的路径分析

摘要 本研究探讨机器学习在强化病历书写质量和提高主要诊断编码正确率方面的应用。介绍了基于机器学习的病历质量分析方法、AI病历质控应用、智能预问诊系统和诊室听译机器人等在病历书写质量提升中的作用，以及基于机器学习的ICD智能诊断编码方法和重症病人ICD自动…
阅读更多...
鸿蒙征文｜鸿蒙技术分享：使用到的开发框架和技术概览

鸿蒙征文｜鸿蒙技术分享：使用到的开发框架和技术概览

目录 每日一句正能量前言正文1. 开发环境搭建关键技术：2. 用户界面开发关键技术：3. 应用逻辑开发关键技术：4. 应用测试关键技术：5. 应用签名和打包关键技术：6. 上架流程关键技术：7. 后续维护和更新关键技术…
阅读更多...
C++类中多线程的编码方式

C++类中多线程的编码方式

问题 在C++代码中，一般的代码是需要封装在类里面，比如对象，方法等。否则就不能很好的利用C++面向对象的能力了。 但是这个方式在处理线程时会碰到一个问题。 考虑下面一个简单的场景： class demoC { public:std::thread t;int x;void threadFunc(){std::cout<<x&…
阅读更多...
Android开发仿qq详情下拉头像变大

Android开发仿qq详情下拉头像变大

Android开发仿qq详情下拉头像变大 个人详情界面&#xff0c;很多都有下拉头像变大的效果&#xff0c;其实我觉得这效果还不如点击头像看大图呢 一、思路&#xff1a; 自定义ScrollView 二、效果图&#xff1a; 看视频更直观点&#xff1a; Android开发教程案例分享-仿qq详情…
阅读更多...
深入解析下oracle date底层存储方式

深入解析下oracle date底层存储方式

之前我们介绍了varchar2和char的数据库底层存储格式&#xff0c;今天我们介绍下date类型的数据存储格式&#xff0c;并通过测试程序快速获取一个日期。 一、环境搭建 1.1&#xff0c;创建表 我们还是创建一个测试表t_code&#xff0c;并插入数据&#xff1a; 1.2&#xff0c;…
阅读更多...
golang版本管理工具：scoop使用

golang版本管理工具：scoop使用

安装 Scoophttps://scoop.sh/根据官方文档安装。 第一步&#xff1a;打开PowerShell。(注意不要使用管理员方式打开&#xff0c;否则在执行安装Scoop的过程中&#xff0c;会报错。) 第二步&#xff1a;切到C盘根目录下。 第三步&#xff1a; Set-ExecutionPolicy -Executi…
阅读更多...
时装购物系统

时装购物系统

私信我获取源码和万字论文&#xff0c;制作不易&#xff0c;感谢点赞支持。 摘 要 随着科学技术的飞速发展&#xff0c;社会的方方面面、各行各业都在努力与现代的先进技术接轨&#xff0c;通过科技手段来提高自身的优势&#xff0c;时装购物系统当然也不能排除在外。时装购物…
阅读更多...
架构04-透明多级分流系统

架构04-透明多级分流系统

零、文章目录 架构04-透明多级分流系统 1、透明多级分流系统 &#xff08;1&#xff09;概述 **定义&#xff1a;**透明多级分流系统是指在用户请求从客户端发出到最终查询或修改数据库信息的过程中&#xff0c;通过多个技术部件对流量进行合理分配&#xff0c;以提高系统的…
阅读更多...
【AI】数据，算力，算法和应用（3）

【AI】数据，算力，算法和应用（3）

三、算法 算法这个词&#xff0c;我们都不陌生。 从接触计算机&#xff0c;就知道有“算法”这样一个神秘的名词存在。象征着专业、权威、神秘、高难等等。 算法是一组有序的解决问题的规则和指令&#xff0c;用于解决特定问题的一系列步骤。算法可以被看作是解决问题的方法…
阅读更多...
YOLO系列论文综述（从YOLOv1到YOLOv11）【第14篇：YOLOv11——在速度和准确性方面具有无与伦比的性能】

YOLO系列论文综述（从YOLOv1到YOLOv11）【第14篇：YOLOv11——在速度和准确性方面具有无与伦比的性能】

YOLOv11 1 摘要2 改进点3 模型性能4 模型架构 YOLO系列博文&#xff1a; 【第1篇&#xff1a;概述物体检测算法发展史、YOLO应用领域、评价指标和NMS】【第2篇&#xff1a;YOLO系列论文、代码和主要优缺点汇总】【第3篇&#xff1a;YOLOv1——YOLO的开山之作】【第4篇&#xff…
阅读更多...
最短路径(Floyd-Warshall、Dijkstra、Bellman-Ford)

最短路径(Floyd-Warshall、Dijkstra、Bellman-Ford)

图的遍历&#xff0c;通过算法优雅实现。 上次使用遍历的方法求得最短路径&#xff08;图的遍历。-CSDN博客&#xff09;&#xff0c;这样虽然可以解决问题&#xff0c;但还是不够优雅&#xff0c;有一些弊端&#xff0c;时间复杂度和空间复杂度都比较高。本博客主要描述三种求…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023