用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);
end

figure(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 Fy
    x1=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_line
    
    return 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 Fy
    x1=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随时间变化的数值，绘制成曲线如下：
在这里插入图片描述

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

MongoDB集群分片安装部署手册

MongoDB集群分片安装部署手册

文章目录 一、集群规划1.1 集群安装规划1.2 端口规划1.3 目录创建 二、mongodb安装（三台均需要操作）2.1 下载、解压2.2 配置环境变量 三、mongodb组件配置3.1 配置config server的副本集3.1.1 config配置文件3.1.2 config server启动3.1.3 初始化config …
阅读更多...
java 调用 k8s crd 生成 crd model

java 调用 k8s crd 生成 crd model

k8s官方提供了自动生成Java模型代码的工具，使用指南： https://github.com/kubernetes-client/java提供有两种方法： github action远程生成本地docker镜像生成 本地docker镜像生成很简单，跟着官方指南下载镜像执行命令即可&…
阅读更多...
36 基于单片机的电磁炉系统设计

36 基于单片机的电磁炉系统设计

目录 一、主要功能 二、硬件资源 三、程序编程 四、实现现象 一、主要功能 基于51单片机，通过DS18B20温度传感器检测温度，通过八位数码管显示， 如果温度超过阈值，则蜂鸣器报警，红灯亮起；若不超过阈值&…
阅读更多...
IoTDB 常见问题 QA 第一期

IoTDB 常见问题 QA 第一期

开始！关于 IoTDB 的 Q&A 我们将定期汇总社区讨论频繁的问题，并展开进行详细回答，通过积累常见问题“小百科”，方便大家使用 IoTDB。 Q1：WAL 堆积导致写入失败 问题及现象 集群报错： The write is rejec…
阅读更多...
buildroot 制作Linux嵌入式文件系统，并添加telnet 以及ssh

buildroot 制作Linux嵌入式文件系统，并添加telnet 以及ssh

在开始配置前，我们需要了解SSH和Telnet的基本概念。SSH（Secure Shell）为加密的网络协议，用于在不安全的网络中执行命令并管理网络服务。相对于SSH，Telnet是一个老旧且非加密的协议，用于进行远程登录 sshd 服…
阅读更多...
Simulink的SIL软件在环测试

Simulink的SIL软件在环测试

以基于模型的设计（MBD）的软件开发时，需要进行SIL（软件在环测试）。SIL测试就是在PC上验证模型是否与代码功能一致。在项目开展中，用在需要将控制器生成移植到硬件前，把控制器的模块生成代码&…
阅读更多...
【赵渝强老师】PostgreSQL中的模式

【赵渝强老师】PostgreSQL中的模式

在PostgreSQL中，所有的数据库对象都是属于模式中的对象。这里的数据库对象包括：表、索引、视图、存储过程、触发器等等。所有数据库对象都有各自的对象标识符oid（object identifiers）,它是一个无符号的四字节整数，相关…
阅读更多...
【分页查询】.NET开源 ORM 框架 SqlSugar 系列

【分页查询】.NET开源 ORM 框架 SqlSugar 系列

.NET开源 ORM 框架 SqlSugar 系列 【开篇】.NET开源 ORM 框架 SqlSugar 系列【入门必看】.NET开源 ORM 框架 SqlSugar 系列【实体配置】.NET开源 ORM 框架 SqlSugar 系列【Db First】.NET开源 ORM 框架 SqlSugar 系列【Code First】.NET开源 ORM 框架 SqlSugar 系列【数据事务…
阅读更多...
Android 图形系统之四：Choreographer

Android 图形系统之四：Choreographer

Choreographer 是 Android 系统中负责帧同步的核心组件，它协调输入事件、动画和绘制任务，以确保界面以固定频率（通常是每 16ms，一帧）流畅渲染。通过管理 VSYNC 信号和调度任务，Choreographer 是实现流畅 UI…
阅读更多...
计算机毕业设计Python异常流量检测 流量分类 流量分析 网络流量分析与可视化系统 网络安全 信息安全 机器学习 深度学习

计算机毕业设计Python异常流量检测 流量分类 流量分析 网络流量分析与可视化系统 网络安全 信息安全 机器学习 深度学习

温馨提示：文末有 CSDN 平台官方提供的学长联系方式的名片！ 温馨提示：文末有 CSDN 平台官方提供的学长联系方式的名片！ 温馨提示：文末有 CSDN 平台官方提供的学长联系方式的名片！ 作者简介：Java领…
阅读更多...
关于扩散方程的解

关于扩散方程的解

1-D 扩散方程的形式 Cauchy齐次方程 这个解无积分无级数，很简单的形式 美其名曰：基本解。 把基本解和初值做卷积，就得到cauchy方程的解。
阅读更多...
零基础学安全--Burp Suite（4）proxy模块以及漏洞测试理论

零基础学安全--Burp Suite（4）proxy模块以及漏洞测试理论

目录 学习连接 一些思路 proxy模块 所在位置 功能简介 使用例子 抓包有一个很重要的点，就是我们可以看到一些在浏览器中看不到的传参点，传参点越多就意味着攻击面越广 学习连接 声明！ 学习视频来自B站up主 **泷羽sec** 有兴趣的师傅可…
阅读更多...
python打包深度学习虚拟环境

python打包深度学习虚拟环境

今天师兄让我把环境打包发给他，我才知道可以直接打包深度学习虚拟环境，这样另一个人就不用辛辛苦苦的去装环境了，我们都知道有些论文他需要的环境很难装上。比如装Apex，装 DCN，mmcv-full 我现在把3090机子上的ppft虚拟…
阅读更多...
M4V 视频是一种什么格式？如何把 M4V 转为 MP4 格式？

M4V 视频是一种什么格式？如何把 M4V 转为 MP4 格式？

M4V 是一种视频文件格式，主要由苹果公司用于其产品和服务中，如 iTunes Store 上的电影和电视节目。这种格式可以包含受版权保护的内容，并且通常与苹果的 DRM（数字版权管理）技术结合使用，以限制内容的复制和…
阅读更多...
【C++】从零到一掌握红黑树：数据结构中的平衡之道

【C++】从零到一掌握红黑树：数据结构中的平衡之道

个人主页: 起名字真南的CSDN博客 个人专栏: 【数据结构初阶】 📘 基础数据结构【C语言】 💻 C语言编程技巧【C】 🚀 进阶C【OJ题解】 📝 题解精讲 目录 前言1 红黑树的概念**红黑树的五大性质** 2 红黑树的实现2.1 红黑树的结构…
阅读更多...
webpack（react）基本构建

webpack（react）基本构建

文章目录 概要整体架构流程技术名词解释技术细节小结 概要 Webpack 是一个现代 JavaScript 应用程序的静态模块打包工具。它的主要功能是将各种资源（如 JavaScript、CSS、图片等）视为模块，并将它们打包成一个或多个输出文件，以便…
阅读更多...
C++STL（四）--＞vector 的模拟实现

C++STL（四）--＞vector 的模拟实现

1.vector的各函数接口&#xff1a; namespace cl {//模拟实现vectortemplate<class T>class vector{public:typedef T* iterator;typedef const T* const_iterator;//默认成员函数vector(); //构造函数vector(size_t n, cons…
阅读更多...
机器学习实战：泰坦尼克号乘客生存率预测（数据处理+特征工程+建模预测）

机器学习实战：泰坦尼克号乘客生存率预测（数据处理+特征工程+建模预测）

项目描述 任务&#xff1a;根据训练集数据中的数据预测泰坦尼克号上哪些乘客能生存下来 数据源&#xff1a;csv文件&#xff08;train.csv&#xff09; 目标变量&#xff1a;Survived&#xff08;0-1变量&#xff09; 数据集预览&#xff1a; 1、英文描述&#xff1a; 2、…
阅读更多...
MATLAB不动点迭代法求单变量非线性方程的根程序加实例

MATLAB不动点迭代法求单变量非线性方程的根程序加实例

不动点迭代法用于单变量线性方程近似根&#xff0c;首先确定一个方程根附近的近似初始值&#xff0c;采用逐次逼近的方法&#xff0c;使用迭代公式不断地更新这个初始值&#xff0c;使这个初始值不断趋近于准确值。 1.不动点迭代法自定义函数 fixed_point.m是一个MATLAB函数&a…
阅读更多...
BurpSuite使用篇--抓包方法与解码器

BurpSuite使用篇--抓包方法与解码器

BurpSuite是一个web渗透利器&#xff0c;可以抓包改包也可以扫描漏洞&#xff0c;将漏洞扫描和利用集成化一体&#xff0c;更可以支持外部插件拓展&#xff0c;非常牛的工具。 那我们怎么使用呢&#xff1f; 使用BurpSuite 老版本中BurpSuite需要在浏览器中配置本地代理&…
阅读更多...
最新文章

Copyright @ 2022~2023