【机器人学】平面2R机器人（六）——MATLAB仿真

机器人学，平面2R机器人完整建模及控制。

Guo_Zhanyu

3142人浏览 · 2021-10-21 18:24:09

Guo_Zhanyu · 2021-10-21 18:24:09 发布

前情回顾

【机器人学】平面2R机器人（一）——正运动学

【机器人学】平面2R机器人（二）——逆运动学

【机器人学】平面2R机器人（三）——速度雅可比矩阵

【机器人学】平面2R机器人（四）——正动力学

【机器人学】平面2R机器人（五）——逆动力学

解答

MATLAB编程仿真

问题：利用MATLAB编程仿真上述平面2R机器人的动力学系统。

在前面的文章中，计算得到平面2R机器人的动力学方程如下：

$\pmb{\tau}=\begin{bmatrix} M_{11} & M_{12} \\ M_{21} & M_{22}\end{bmatrix} \begin{bmatrix} \ddot{\theta}_1\\ \ddot{\theta}_2\end{bmatrix} + \begin{bmatrix} D_{11} & D_{12} \\ D_{21} & D_{22}\end{bmatrix} \begin{bmatrix} \dot{\theta}_1^2\\ \dot{\theta}_2^2\end{bmatrix} + \begin{bmatrix} C_1\\C_2 \end{bmatrix} +\begin{bmatrix} G_1\\G_2 \end{bmatrix}$

其中，

$M_{11}=\frac{5ml^2}{3}+ml^2\cos\theta_2,M_{12}=M_{21}=\frac{ml^2}{3}+\frac{ml^2}{2}\cos\theta_2,M_{22}= \frac{ml^2}{3}$

$D_{11} = D_{22} = 0,D_{12}=-\frac{ml^2}{2}\sin\theta_2,D_{21}=\frac{ml^2}{2}\sin\theta_2$

$C_1=-ml^2\sin\theta_2 \ \dot{\theta}_1\dot{\theta}_2,C_2=0$

$G_1=\frac{3mgl}{2}\sin\theta_1+\frac{mgl}{2}\sin(\theta_1+\theta_2),G_2=\frac{mgl}{2}\sin(\theta_1+\theta_2)$

进行MATLAB仿真的过程是：给定初始条件和外部施加的力矩 $\pmb \tau$ ，求解得到各个时刻的角度、角速度、角加速度等参数变化。使用关节角度进行正运动学求解可以得到末端的笛卡尔坐标；使用关节角速度进行速度雅可比矩阵的转换可以得到末端的笛卡尔坐标下的速度，又或者可以通过对求出的笛卡尔坐标进行差分得到。总而言之，要做的事情就是求解动力学微分方程组。使用到MATLAB内的函数 ode45 进行求数值解。

[t,y] = ode45(odefun,tspan,y0,options)

（其中 $tspan=[t0 \ tf]$ ）求微分方程组 $y \prime =f(t,y)$ 从 $t0$ 到 $tf$ 的积分，初始条件为 $y0$ ，还可以使用options来进行误差的设置或使用Mass选项提供质量矩阵。解数组 $y$ 中的每一行都与列向量 $t$ 中返回的值相对应。

由于本题中的微分方程组不好转换为 $y \prime =f(t,y)$ 的形式，参考了MATLAB帮助文档中的一个例子：求解抛向空中的短棒的运动方程，从中找到了使用质量矩阵Mass的方法，以求解比较复杂的微分方程组。

将动力学方程展开为方程组的形式：

$\left \{ \begin{array}{lcl} \tau_1=M_{11}\ddot\theta_1+M_{12}\ddot\theta_2+D_{11}\dot\theta_1^2+D_{12}\dot\theta_2^2+C_1+G_1 \\ \tau_2=M_{21}\ddot\theta_1+M_{22}\ddot\theta_2+D_{21}\dot\theta_1^2+D_{22}\dot\theta_2^2+C_2+G_2\end{array} \right.$

ode45求解器要求将方程写作 $\dot q=f(t,q)$ 的形式，其中 $\dot q$ 是每个分量的一阶导数。在该问题中，解向量有 4 个分量： $\theta_1,\dot \theta_1,\theta_2,\dot \theta_2$ 。

$q=\begin{bmatrix} q_1 \\ q_2 \\q_3\\q_4\end{bmatrix}=\begin{bmatrix} \theta_1 \\ \dot \theta_1 \\\theta_2\\\dot\theta_2\end{bmatrix}$

重写方程组得：

$\left \{ \begin{array}{lcl} \tau_1=M_{11}\dot q_2+M_{12}\dot q_4+D_{11}q_2^2+D_{12}q_4^2+C_1+G_1 \\ \tau_2=M_{21}\dot q_2+M_{22}\dot q_4+D_{21}q_2^2+D_{22}q_4^2+C_2+G_2\end{array} \right.$

但在方程一侧还存在不止一项的一阶导数。出现这种情况时，必须使用质量矩阵来表示方程的左侧。通过矩阵表示法，使用 $M(q)\dot q=f(t,q)$ 形式的质量矩阵将运动方程重写为包含 4 个方程的方程组。在此形式中，方程组变为：

$\begin{bmatrix}1&0&0&0\\0&M_{11}&0&M_{12}\\0&0&1&0\\0&M_{21}&0&M_{22} \end{bmatrix} \begin{bmatrix} \dot q_1 \\ \dot q_2 \\ \dot q_3 \\ \dot q_4\end{bmatrix} = \begin{bmatrix} q_2\\\tau_1-D_{11}q_2^2-D_{12}q_4^2-C_1-G_1\\ q_4 \\ \tau_2-D_{21}q_2^2-D_{22}q_4^2-C_2-G_2\end{bmatrix}$

根据方程组，编写一个用于计算质量矩阵的函数mass如下：

function M=mass(t,q,P)
    m=P.m;
    l=P.l;
    M=zeros(4,4);
    M(1,1)=1;
    M(2,2)=5*m*l^2/3+m*l^2*cos(q(3));
    M(2,4)=m*l^2/3+m*l^2/2*cos(q(3));
    M(3,3)=1;
    M(4,2)=m*l^2/3+m*l^2/2*cos(q(3));
    M(4,4)=m*l^2/3; 
end

该函数接受 3 个输入： $t$ 和解向量 $q$ （两者为必须），以及额外输入的结构体P，在这里用以传输连杆质量和长度。接下来为方程组 $M(q)\dot q=f(t,q)$ 的右侧编写一个函数，一样需要接受3个输入： $t$ 和解向量 $q$ （两者为必须），以及额外输入的结构体P，此处结构体P还传递了两个旋转关节驱动力矩，函数f如下：

function dydt=f(t,q,P)
    m=P.m;
    l=P.l;
    g=P.g;
    t1=P.t1;
    t2=P.t2;
    dydt=[q(2)
          t1+m*l^2/2*sin(q(3))*q(4)^2+m*l^2*sin(q(3))*q(2)*q(4)-3*m*g*l/2*sin(q(1))-m*g*l/2*sin(q(1)+q(3))
          q(4)
          t2-m*l^2/2*sin(q(3))*q(2)^2-m*g*l/2*sin(q(1)+q(3))];
end

针对题目的(1)(2)，完整代码如下

clc;clear;

P.m=5;
P.l=1;
P.g=9.8;
P.t1=2;
P.t2=0.5;
N=2;
step=0.01;
tspan=linspace(0,N,N/step+1);
y0=[pi 0 0 0];

opts=odeset('Mass',@(t,q) mass(t,q,P));
[t,q]=ode45(@(t,q) f(t,q,P),tspan,y0,opts);

theta1=q(:,1);
theta2=q(:,3);
omega1=q(:,2);
omega2=q(:,4);
alpha1=zeros(N/step+1,1);
alpha2=zeros(N/step+1,1);
for i=2:N/step+1
    alpha1(i)=(omega1(i)-omega1(i-1))/step;
    alpha2(i)=(omega2(i)-omega2(i-1))/step;
end

x1=sin(theta1);
y1=-cos(theta1);
x2=sin(theta1+theta2)+sin(theta1);
y2=-cos(theta1+theta2)-cos(theta1);

% theta
figure(1);subplot(221);set(gcf,'position',[310,0,1200,768]);
plot(t,theta1,'-r',t,theta2,'-b');grid on;title('角度');
legend('$\theta_1$','$\theta_2$','Interpreter','latex');
xlabel('time(s)','Interpreter','latex');ylabel('$\theta(rad)$','Interpreter','latex');

% omega
figure(1);subplot(222);
plot(t,omega1,'-r',t,omega2,'-b');grid on;title('角速度');
legend('$\dot{\theta}_1$','$\dot{\theta}_2$','Interpreter','latex');
xlabel('time(s)','Interpreter','latex');ylabel('$\dot{\theta}(rad/s)$','Interpreter','latex');

% alpha
figure(1);subplot(223);
plot(t,alpha1,'-r',t,alpha2,'-b');grid on;title('角加速度');
legend('$\ddot{\theta}_1$','$\ddot{\theta}_2$','Interpreter','latex');
xlabel('time(s)','Interpreter','latex');ylabel('$\ddot{\theta}(rad/s^2)$','Interpreter','latex');

pause(0.5);

% x-y
figure(2);set(gcf,'position',[10,50,600,600]);

line([0,x1(1)],[0,y1(1)],'color','r','linewidth',2);
line([x1(1),x2(1)],[y1(1),y2(1)],'color','b','linewidth',2);
axis([-2.2 2.2 -2.2 2.2]);grid on;

for i=1:length(t)
    clf;
    line([0,x1(i)],[0,y1(i)],'color','r','linewidth',2);
    line([x1(i),x2(i)],[y1(i),y2(i)],'color','b','linewidth',2);
    axis([-2.2 2.2 -2.2 2.2]);grid on;title(['Time:',num2str(i/100-0.01,'%.2f'),'s']);
    pause(0.01);
end
hold on;
plot(x2,y2,'-o');