matlab机器人工具箱学习笔记——状态空间方程

机器人动力学概述

对于机器人动力学分析，有两种经典方法：一种是牛顿—欧拉法，另一种是拉格朗日法。与机器人运动学相似，机器人动力学也有两个相反的问题：
（1）动力学正问题是已知机械臂各关节的作用力或力矩，求各关节的位移、速度和加速度，即机器人的运动轨迹$(\tau \to q,q^{\prime },q^{\prime \prime })$,这可以用于对机械臂的仿真
（2）动力学逆问题是已知机械臂的运动轨迹，即各关节的位移、速度和加速度，求各关节所需要的驱动力或力矩$(q^{\prime \prime },q^{\prime },q\to \tau )$，这可以用于对机械臂的控制。

状态空间方程

其中一种用状态空方程表示动力学方程。不考虑一切摩擦因素：
$$M(q)q^{\prime \prime }+C(q,q^{\prime })q^{\prime }+G(q)+F(q^{\prime })+J(q{)}^{T}f=\tau$$
$M(q)q^{\prime \prime }$，为机器人的惯性矩阵。
$C(q,q^{\prime })q^{\prime }$，为科里奥利矩阵。
$G(q)$，为重力矩阵。
$F(q^{\prime })$，为摩擦力矩。
$J(q{)}^{T}f$,表示关节力，$f$表示扭力，$J(q)$为雅克比矩阵。

各项参数的获取（正向动力学的计算）

1）运动学和动力学参数
使用SerialLink.dyn()来显示运动学参数和动力学参数。
输入命令：

>> mdl_puma560
>> p560.links(6).dyn
theta=q, d=          0, a=          0, alpha=          0, offset=          0 (R,stdDH)
  m    =        0.09
  r    =           0           0       0.032
  I    = |     0.00015           0           0 |
         |           0     0.00015           0 |
         |           0           0       4e-05 |
  Jm   =     3.3e-05
  Bm   =    3.67e-05
  Tc   =     0.00396(+)     -0.0105(-)
  G    =       76.69
  qlim = -4.642576 to 4.642576

2）惯性矩阵
当关节角为$q$时，使用SerialLink.inertia()获取机器人的惯性矩阵：
输入：

>> q=[0 0 0 0 0 0];
>> p560.inertia(q)

ans =

    3.9611   -0.1627   -0.1389    0.0016   -0.0004    0.0000
   -0.1627    4.4566    0.3727    0.0000    0.0019    0.0000
   -0.1389    0.3727    0.9387    0.0000    0.0019    0.0000
    0.0016    0.0000    0.0000    0.1924    0.0000    0.0000
   -0.0004    0.0019    0.0019    0.0000    0.1713    0.0000
    0.0000    0.0000    0.0000    0.0000    0.0000    0.1941

3）科里奥利矩阵
当机器人的关节角为$q$，关节角的速度为$q^{\prime }$，可以通过函数SerialLink.coriolis()获取科里奥利矩阵：
例如：

>> q=[0 0 0 0 0 0];
>>qd=[pi/6 pi/6 pi/6 pi/6 pi/6 pi/6];
>>> p560.coriolis(q,qd)

ans =

   -0.4206   -0.5773   -0.2121   -0.0007   -0.0014    0.0000
    0.2118   -0.2029   -0.4050   -0.0000   -0.0020         0
    0.2081    0.2021   -0.0000    0.0000   -0.0001         0
    0.0000    0.0000    0.0000         0         0         0
    0.0007    0.0007    0.0001         0         0         0
         0         0         0         0         0         0

4）重力矩阵
当机器人的关节角为$q$，可通过函数SerialLink.gravload()获取机器人的重力矩阵：

>> q=[0 0 0 0 0 0];
>>p560.gravload(q)
ans =

         0   37.4837    0.2489         0         0         0
G(q)=(0   37.4837    0.2489         0         0         0)'

5）摩擦力矩
使用SerialLink.dyn()
其中：
Bm为粘性摩擦系数
Tc为库仑摩擦系数
G为齿轮传动比

逆向动力学的计算

使用SerialLink.rne()计算动力学的逆问题，主要参数为$q$(关节角),$qd$（速度）,$qdd$（加速度）,$grav$（重力项，有默认值）
输入：

>> mdl_puma560;
>> q=[0 0 0 pi/6 pi/6 pi/6];
>> qd=[0 0 0 pi/18 pi/18 pi/18];
>> qdd=[0 0 0 0 0 0];
>> t1=p560.rne(q,qd,qdd)
t1 =

   -0.0000   37.4713    0.2366    0.9235    0.7265    0.3413%%%驱动力矩

当忽略状态方程的重力项时，输入语句：

>>t2=p560.rne(q,qd,qdd,[0 0 0]')
t2 =

   -0.0000   -0.0001   -0.0001    0.9235    0.7406    0.3413%%%驱动力矩

此外，函数SerialLink.rne()也可以计算机器人沿着一条轨迹时，每一时刻下的驱动力矩。
输入：

>> T1=transl(0.3,0.1,0)*trotx(pi);			%设置初始位姿
>>q1=p560.ikine6s(T1);						%计算对应关节角
>>T2=transl(0.2,0.4,0)*trotx(pi/2);		%设置最终位姿
>>q2=p560.ikine6s(T2);						%计算对应关节角
>>t=[0:0.1:6]';										%设置时间及步长
>>[q,qd,qdd]=jtraj(q1,q2,t);					%生成相应的轨迹
>>tu=p560.rne(q,qd,qdd);					%计算轨迹上每个点的驱动力矩

可以得到一个61×6的矩阵每一行对应某一个时间的驱动力矩。
我们可以
plot(t,tu(:1));…等
得到驱动力矩关于时间的图像

参考资料：
杨辰光, 李智军, 许扬，机器人仿真与编程技术[M].北京:清华大学出版社,2018