MATLAB机器人工具箱（四）动力学

正动力学：
已知各个关节上电机提供的力/力矩，在此力矩作用下，关节如何运动，求对应各个关节角度、角速度、角加速度。
逆动力学：
已知一个轨迹点，以及关节速度、加速度、角加速度求出期望的关节力矩
机器人动力学参数查看：

robot.links(1).dyn

 theta=q, d=          0, a=          0, alpha=          0, offset=          0 (R,modDH)
  r    =           0           0           0
  I    = |           0           0           0 |
         |           0           0           0 |
         |           0           0           0 |
  Bm   =           0
  Tc   =           0(+)           0(-)
  G    =           0

按顺序分别是运动学参数，连杆质量，质心位置，惯量矩阵，

正动力学

使用SerialLink.fdyn()计算正向动力学
(T,q,qd)=R.fdyn(tmax,ftfun)将机器人在0~tmax时间内的动力学进行积分，得到时间T，关节位置q、关节速度qd，初始关节位置和速度为0，施加在关节上的扭矩由控制函数ftfun计算
TAU=FTFUN(T,Q,QD)
其中q和qd分别为关节位置和关节速度，T为当前时间
[T,q,qd] = R.fdyn(T1,ftfun,q0,qd0,ARG1,ARG2…)允许给定关节位置q0和速度qd0的初始值
例如采用PD控制：
首先定义函数估算控制
function tau = myftfun(t,q,qd,qstar,P,D)
tau=P*(qstar-q) + D*qd;
然后将控制与动力学相结合：
[t,q]=robot.fdyn(10,@myftfun,qstar,P,D);
若fifun没有指定，或者为0或[]，那么对机器人关节施加0力矩

逆动力学SerialLink.rne

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 ]

%%逆动力学
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);

关节控制
嵌套的控制回路，外环负责保持关节位置，确定使位置误差最小的关节速度，内环负责保持外环所要求的关节速度