Matlab机器人工具箱——动力学
文章目录
- 前言
- 正向动力学
- 逆向动力学
- 参考的博客
前言
本文主要介绍串联机械臂的动力学与控制。机械臂上每一个连杆由前一个连杆的反作用力和力矩支撑，另外它还受自身的重力，以及来自它所支撑的后一个连杆的反作用力和力矩。
正向动力学
正向动力学是已知施加各个关节上电机提供的力/力矩，在此力矩作用下，关节如何运动，求对应各个关节角度、角速度、角加速度。
以Puma560机器人为例，直接在命令行输入代码：
mdl_puma560即可直接调用工具箱中的Puma560模型，在给定关节坐标(q)、关节速度(qd)和所施加的力矩(Q)的条件下，关节加速度可以用Serial-Link对象的accel方法计算得到：
qdd=p560.accel(q,qd,Q)该功能也封装在Simulink的Robot模块中，以下为其应用的一个示例，直接在命令行输入：
sl_ztorque如图1所示，施加给机器人的力矩为0，初始关节角被设定为Robot模块的一个参数，这里为“0角度位姿”。运行该仿真：
r=sim('sl_ztorque');关节角作为随时间变化的函数被返回到r对象中：
t=r.find('tout');
q=r.find('yout');我们用动画演示机器人的运动：
p560.plot(q,'trail','b-','movie','r.gif')
可以看到它在重力作用下崩溃，因为没有力矩来对抗重力来保持机器人直立。肩关节和肘关节都会垂下来并来回摆动。
我们还可以绘制出随时间变化的关节角度：
plot(t,q(:,1:3))
title('关节角度曲线');
legend('q1','q2','q3');
text(1.24,-1.352,'\leftarrow q3');
text(0.56,-0.9188,'\leftarrow q2');
text(1.04,0.1838,'\leftarrow q1');
legend('q1','q2','q3');
grid on如图3所示，fdyn方法可以被当作Simulink的一个无图形代替方法，它在在线文档中有描述。
但这个示例相当不现实。在实际应用中，关节力矩会作为机器人真实和期望关节角的某个函数被计算。
逆向动力学
逆运动学即已知一个轨迹点，以及关节速度、加速度、角加速度求出期望的关节力矩。
tua = R.rne(q,qd,qdd,option)是机器人R达到指定关节位置q(1xN)，速度qd(1XN)，加速度qdd(1XN)所需要的关节力矩，其中N为关节个数,
tua = R.rne(x,tions) 其中x=[q,qd,qdd] (1x3N)求各个关节需要提供的tua
[tau,wbase]=R.rne(x,grav,fext)其中grav是重力加速度,默认值是[0,0,9.81]
fext=[Fx,Fy,Fz,Mx,My,Mz ]
下面以VIPER7机械臂为例，进行机器人逆向动力学的编程演示：
%%逆动力学
clc
clear all
close all
deg = pi/180;
L1= Revolute('d', 0, 'a', 0, 'alpha', 0,'modified', ...
'I', [0.1183 -0.0001 0.0001;
-0.0001 0.1182 0.0001;
0.0001 0.0001 0.0140], ...
'r', [0.0002 0.0002 0.1264], ...
'm', 5.6431, ...
'Jm', 2.2e-4, ...
'G', 81, ...
'B', 1.48e-3, ...
'Tc', [0.395 -0.435], ...
'qlim', [-180 180]*deg );
L2 = Revolute('d', 0.06, 'a', 0, 'alpha', -pi/2,'modified', ...
'I', [0.0723,0.0000,-0.0051;0.0000,0.0784,0.0000;-0.0051,0.0000,0.0169;], ...
'r', [-0.0062,0.0001,0.1080], ...
'm', 5.0478, ...
'Jm', 2.2e-4, ...
'G', 121, ...
'B', .817e-3, ...
'Tc', [0.126 -0.071], ...
'qlim', [-105 105]*deg );
L3 = Revolute('d', -0.004, 'a', 0.332, 'alpha', 0, 'modified', ...
'I', [0.4263,0.0000,-0.0072;
0.0000,0.4334,0.0001;
-0.0072,0.0001,0.0191], ...
'r', [-0.0131,0.0001,0.2402], ...
'm', 5.7542, ...
'Jm', 2.2e-4, ...
'G', 81, ...
'B', 1.38e-3, ...
'Tc', [0.132, -0.105], ...
'qlim', [-225 45]*deg );
L4 = Revolute('d', -0.056, 'a', 0, 'alpha', pi/2, 'modified', ...
'I', [0.0821,0.0000,-0.0314;0.0000,0.1257,0.0001;-0.0314,0.0001,0.0451], ...
'r', [-0.0850,0.0003,0.1540], ...
'm', 3.0870, ...
'Jm', 2.2e-4, ...
'G', 81, ...
'B', 71.2e-6, ...
'Tc', [11.2e-3, -16.9e-3], ...
'qlim', [-110 110]*deg);
L5 = Revolute('d', 0.050, 'a', 0, 'alpha', -pi/2, 'modified', ...
'I', [0.0235,0.0000,-0.0002;0.0000,0.0253,0.0000;-0.0002,0.0000,0.0045], ...
'r', [0.0001,0.0002,0.0982], ...
'm', 2.0459, ...
'Jm', 2.2e-4, ...
'G', 81, ...
'B', 82.6e-6, ...
'Tc', [9.26e-3, -14.5e-3], ...
'qlim', [-115 115]*deg );
L6 = Revolute('d', -0.050, 'a', 0, 'alpha', pi/2, 'modified', ...
'I', [0.0684,0.0000,0.0001;0.0000,0.0696,-0.0001;0.0001,-0.0001,0.0047], ...
'r', [-0.0111,-0.0003,0.1366], ...
'm', 2.6317, ...
'Jm', 2.2e-4, ...
'G', 51, ...
'B', 36.7e-6, ...
'Tc', [3.96e-3, -10.5e-3], ...
'qlim', [-180 180]*deg );
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','VIPER7','comment','LL'); %SerialLink类函数
robot.display(); %Link类函数，显示建立机器人DH参数
%通过手动输入各个连杆转角，模型会自动运动到相应位置
theta1=[0 -pi/2 -pi/2 0 0 0]; %机器人伸直且垂直
robot.plot(theta1); %SerialLink类函数,显示机器人图像
theta2=[-pi/2 0 0 -pi/2 -pi/2 -pi/2];
robot.plot(theta2);
t=[0:0.01:2];
g=jtraj(theta1,theta2,t);%相当于具有tpoly插值的mtraj，但是对多轴情况进行了优化，还允许使用额外参数设置初始和最终速度
[q,qd,qdd]=jtraj(theta1,theta2,t);%通过可选的输出参数，获得随时间变化的关节速度加速度向量
figure
i=1:6;
subplot(2,3,1);
robot.plot(g)%绘制动画
subplot(2,3,2);
qplot(q(:,i));grid on;title('位置');%绘制每个关节位置
subplot(2,3,3);
qplot(qd(:,i));grid on;title('速度');%绘制每个关节速度
subplot(2,3,4);
qplot(qdd(:,i));grid on;title('加速度');%绘制每个关节加速度
Q = robot.rne(q,qd,qdd);%获得每个时间点所需要的关节力矩
subplot(2,3,5)
qplot(t,Q);grid on;title('关节力矩');%绘制每个关节的力矩最终得到的机械臂运动轨迹和运动学曲线分别如图4和图5所示。
关节控制
嵌套的控制回路，外环负责保持关节位置，确定使位置误差最小的关节速度，内环负责保持外环所要求的关节速度
参考的博客
这里是我学习过程中参考的一些博客
原文链接：
https://blog.csdn.net/weixin_43365751/article/details/100707493
https://blog.csdn.net/weixin_43502392/article/details/105634856