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

news2024/12/1 20:38:37

目录

  • 介绍
  • 代码功能演示
  • 拉格朗日方法回顾
  • 求解符号表达式
  • 数值求解

介绍

开发机器人过程中经常需要用牛顿-拉格朗日法建立机器人的动力学模型，表示为二阶微分方程组。本文以一个二杆系统为例，介绍如何用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随时间变化的数值，绘制成曲线如下：
在这里插入图片描述

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

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

相关文章

堆排序（含证明）

堆排序（含证明）

引言 前面我们讲过堆的基本操作的实现，现在给定一个int类型的数组，里面存放的数据是无序的，我们如何利用堆的思想来实现数组内数据的升序排列或降序排列呢？ 通过前面讲到的堆的实现，我们可以想到，我们再开…
阅读更多...
前海湾地铁的腾通数码大厦背后的临时免费停车点探寻

前海湾地铁的腾通数码大厦背后的临时免费停车点探寻

临时免费停车点：前海湾地铁的腾通数码大厦背后的桂湾大街，目前看不仅整条桂湾大街停了​车，而且还有工地餐点。可能是这个区域还是半工地状态，故暂时还不会有​罚单的情况出现。 中建三局腾讯数码大厦项目部A栋 广东省深圳市南山…
阅读更多...
Wend看源码-Durid

Wend看源码-Durid

项目地址 GitHub - alibaba/druid: 阿里云计算平台DataWorks(https://help.aliyun.com/document_detail/137663.html) 团队出品，为监控而生的数据库连接池 简介 Druid连接池是阿里巴巴开源的数据库连接池项目，自2011年开源以来，它因其卓越的…
阅读更多...
GY302光照传感器模块详解

GY302光照传感器模块详解

目录 一、引言 二、功能特点 三、工作原理 四、引脚功能 五、应用场景 六、使用方法 七、总结 一、引言 在当今科技飞速发展的时代，传感器技术在各个领域都发挥着至关重要的作用。光照传感器作为一种能够感知环境光照强度的设备，广泛应用于农业、…
阅读更多...
分布式事务调研

分布式事务调研

目录 需求背景： 本地事务 分布式基本理论 1、CAP 定理 2、BASE理论 分布式事务方案 #2PC #1. 运行过程 #1.1 准备阶段 #1.2 提交阶段 #2. 存在的问题 #2.1 同步阻塞 #2.2 单点问题 #2.3 数据不一致 #2.4 太过保守 3PC #本地消息表 TCC TCC原理 …
阅读更多...
win10系统部署RAGFLOW+Ollama教程

win10系统部署RAGFLOW+Ollama教程

本篇主要基于linux服务器部署ragflowollama，其他操作系统稍有差异但是大体一样。 一、先决条件 CPU ≥ 4核； RAM ≥ 16 GB； 磁盘 ≥ 50 GB； Docker ≥ 24.0.0 & Docker Compose ≥ v2.26.1。 如果尚未在本地计算机&#xff…
阅读更多...
自然语言处理期末试题汇总

自然语言处理期末试题汇总

建议自己做，写完再来对答案。答案可能存在极小部分错误，不保证一定正确。 一、选择题 1-10、C A D B D B C D A A 11-20、A A A C A B D B B A 21-30、B C C D D A C A C B 31-40、B B B C D A B B A A 41-50、B D B C A B B B B C 51-60、A D D …
阅读更多...
Android Studio的AI工具插件使用介绍

Android Studio的AI工具插件使用介绍

Android Studio的AI工具插件使用介绍 一、前言 Android Studio 的 AI 工具插件具有诸多重要作用，以下是一些常见的方面： 代码生成与自动补全 代码优化与重构 代码解读 学习与知识获取 智能搜索与资源推荐实际使用中可以添加注释，解读某段代…
阅读更多...
iQOO Neo10系列携三大蓝科技亮相，性能与续航全面升级

iQOO Neo10系列携三大蓝科技亮相，性能与续航全面升级

11月29日，iQOO Neo10系列正式登场。作为iQOO Neo系列的最新力作，Neo10系列不仅延续了该系列一贯的“双芯”特色，更在性能、续航、屏幕、影像等多个方面实现了全面升级，为用户带来前所未有的使用体验。此次发布的Neo10系列共有两款…
阅读更多...
172页PPT集团数字化转型采购供应链及财务管控业务流程指南

172页PPT集团数字化转型采购供应链及财务管控业务流程指南

一、供应商管理与数字化转型 1.1供应商管理数字化的重要性与挑战 重要性： 效率提升： 数字化可以提高供应商管理的效率，通过自动化流程减少手动操作，加快决策速度。透明度增强： 数字化工具可以提供实时数据&#xff…
阅读更多...
springboot338it职业生涯规划系统--论文pf(论文+源码)_kaic

springboot338it职业生涯规划系统--论文pf(论文+源码)_kaic

毕 业 设 计（论 文） 题目：it职业生涯规划系统的设计与实现 摘 要 互联网发展至今，无论是其理论还是技术都已经成熟，而且它广泛参与在社会中的方方面面。它让信息都可以通过网络传播，搭配信息管理工具可以…
阅读更多...
【深度学习基础】一篇入门模型评估指标（分类篇）

【深度学习基础】一篇入门模型评估指标（分类篇）

🌈 个人主页：十二月的猫-CSDN博客 🔥 系列专栏： 🏀深度学习_十二月的猫的博客-CSDN博客 💪🏻 十二月的寒冬阻挡不了春天的脚步，十二点的黑夜遮蔽不住黎明的曙光 目录 1. 前言 2. 模…
阅读更多...
【STM32学习】TB6612FNG驱动芯片的学习，驱动电路的学习

【STM32学习】TB6612FNG驱动芯片的学习，驱动电路的学习

目录 1、TB6612电机驱动芯片 1.1如下是芯片的引脚图： 1.2如下图是电机的控制逻辑： 1.3MOS管运转逻辑 1.3典型应用电路 2、H桥驱动电路 2.1、单极模式 2.2、双极模式 2.3、高低端MOS管导通条件 2.4、H桥电路设计 2.5、自举电路 3、电气特性 3…
阅读更多...
STM32 HAL库开发学习3.STM32启动浅析

STM32 HAL库开发学习3.STM32启动浅析

STM32 HAL库开发学习3.STM32启动浅析 一、STM32启动模式（也称自举模式）1. MSP与PC指针赋值2. F1系列的启动模式：3. F4系列启动模式4. F7系列启动模式5. H7系列启动模式 二、STM32启动过程1. MSP 栈顶地址2. PC值3. Reset_Handler4. 启动文件内…
阅读更多...
FCBP 认证考试要点摘要

FCBP 认证考试要点摘要

理论知识 数据处理与分析：包括数据的收集、清洗、转换、存储等基础操作，以及数据分析方法，如描述性统计分析、相关性分析、数据挖掘算法等的理解和应用 。数据可视化：涉及图表类型的选择与应用，如柱状图、折线图、饼图…
阅读更多...
xv6前置知识

xv6前置知识

fork函数 一个进程，包括代码、数据和分配给进程的资源。fork（）函数通过系统调用创建一个与原来进程几乎完全相同的进程，也就是两个进程可以做完全相同的事，但如果初始参数或者传入的变量不同，两个进程也可以做不同的事。 一个进程调用fork（）函数后，系统先给新的进程分…
阅读更多...
ECharts柱状图-极坐标系下的堆叠柱状图，附视频讲解与代码下载

ECharts柱状图-极坐标系下的堆叠柱状图，附视频讲解与代码下载

引言： 在数据可视化的世界里，ECharts凭借其丰富的图表类型和强大的配置能力，成为了众多开发者的首选。今天，我将带大家一起实现一个柱状图图表，通过该图表我们可以直观地展示和分析数据。此外，我还将提供…
阅读更多...
监控视频汇聚平台：Liveweb视频监控管理平台方案详细介绍

监控视频汇聚平台：Liveweb视频监控管理平台方案详细介绍

Liveweb国标视频综合管理平台是一款以视频为核心的智慧物联应用平台。它基于分布式、负载均衡等流媒体技术进行开发，提供广泛兼容、安全可靠、开放共享的视频综合服务。该平台具备多种功能，包括视频直播、录像、回放、检索、云存储、告警上报、语音对讲、…
阅读更多...
MySQL源码编译

MySQL源码编译

华子目录 下载源码包上传并解压安装cmake环境检测make编译make install安装 部署复制编译文件到别的主机上 下载源码包 下载相应源码包mysql5.7编译安装需要boost库，这里官网下载含boost的源码包https://downloads.mysql.com/archives/community/ 上传并解压 [roo…
阅读更多...
请求(request)

请求(request)

目录 前言 request概述 request的使用 获取前端传递的数据 实例 请求转发 特点 语法 实例 实例1 实例2 【关联实例1】 域对象 组成 作用范围： 生命周期： 使用场景： 使用步骤 存储数据对象 获得数据对象 移除域中的键值…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023