前情回顾

【机器人学】平面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+1alpha1(i)=(omega1(i)-omega1(i-1))/step;alpha2(i)=(omega2(i)-omega2(i-1))/step;
endx1=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');

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

【机器人学】平面2R机器人（五）——逆动力学
前情回顾 [机器人学]平面2R机器人(一)--正运动学 [机器人学]平面2R机器人(二)--逆运动学 [机器人学]平面2R机器人(三)--速度雅可比矩阵 [机器人学]平面2R机器人(四)--正动力学 ...
【机器人学】平面2R机器人（二）——逆运动学
前情回顾 [机器人学]平面2R机器人(一)--正运动学解答逆运动学求解尽管在题目要求上并没有计算逆运动学的步骤,但是在后续的MATLAB仿真中,需要从末端执行器的笛卡尔坐标得到各关节的角度,同时 ...
delta机器人与matlab仿真（一）
delta机器人与matlab仿真(一) delta机器人描述运动分析 matlab实现 delta机器人描述 Delta 机器人是典型的空间三自由度(X,Y,Z平移运动)的并联机构,特点是自重负荷 ...
SCARA机器人与工业六轴机器人的matlab仿真
最近因为课程原因对AR4215和FANUC R-0iB两个机械臂做了简单的matlab仿真,贴出来免得以后忘了. 关于Robotic toolbox工具箱的安装与使用请移步CSDN自行查找. SCAR ...
三自由度机器人运动学matlab仿真实验报告,3自由度机器人的正运动学建模研究...
. 3 MATLAB构建机器人仿真模型机器人学工具箱(Robotics Toolbox for MATLAB) 是MATLAB中专门用于机器人仿真的工具箱,在机器人建模.轨迹规划.控制.可视化方面使 ...
【鲁棒控制】平面2R型机器人的鲁棒逆推跟踪控制(matlab实现)
鲁棒跟踪逆推控制器设计前言:对于模型准确的对象,我们可以设计控制器直接进行控制,但通常实际情况下总是存在着种种不确定因素,如参数变化,未建模动态变化等,鲁棒控制就是在模型不精确和其他变化因素的条件下 ...
matlab光束,matlab仿真光束的传输特性
一.课程设计题目: 用matlab 仿真光束的传输特性. 二.任务和要求用matlab 仿真光束通过光学元件的变换. ① 设透镜材料为k9玻璃,对1064nm 波长的折射率为1.5062,镜片中心厚 ...
光束传输 matlab,matlab仿真光束的传输特性
一.课程设计题目: 用matlab仿真光束的传输特性. 二.任务和要求用matlab仿真光束通过光学元件的变换. ① 设透镜材料为k9玻璃,对1064nm波长的折射率为1.5062,镜片中心厚度为3 ...
matlab凸透镜防仿真,西安邮电大学matlab仿真解剖.doc
西安邮电大学专业课程设计报告书院系名称:电子工程学院学生姓名:李群学号专业名称:光信息科学与技术班级:光信1103实习时间:2014年4月8日至2014年4月18日课程设计题目: ...

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

前情回顾

解答

MATLAB编程仿真

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

最新文章

热门文章