机器人动力学概述
对于机器人动力学分析，有两种经典方法：一种是牛顿—欧拉法，另一种是拉格朗日法。与机器人运动学相似，机器人动力学也有两个相反的问题：
(1)动力学正问题是已知机械臂各关节的作用力或力矩，求各关节的位移、速度和加速度，即机器人的运动轨迹( τ → q , q ′ , q ′ ′ ) (\tau\to q,q',q'')(τ→q,q′,q′′),这可以用于对机械臂的仿真
(2)动力学逆问题是已知机械臂的运动轨迹，即各关节的位移、速度和加速度，求各关节所需要的驱动力或力矩( q ′ ′ , q ′ , q → τ ) (q'',q',q \to\tau)(q′′,q′,q→τ)，这可以用于对机械臂的控制。
状态空间方程
其中一种用状态空方程表示动力学方程。不考虑一切摩擦因素：
M ( q ) q ′ ′ + C ( q , q ′ ) q ′ + G ( q ) + F ( q ′ ) + J ( q ) T f = τ M(q)q''+C(q,q')q'+G(q)+F(q')+J(q)^Tf=\tauM(q)q′′+C(q,q′)q′+G(q)+F(q′)+J(q)Tf=τ
M ( q ) q ′ ′ M(q)q''M(q)q′′，为机器人的惯性矩阵。
C ( q , q ′ ) q ′ C(q,q')q'C(q,q′)q′，为科里奥利矩阵。
G ( q ) G(q)G(q)，为重力矩阵。
F ( q ′ ) F(q')F(q′)，为摩擦力矩。
J ( q ) T f J(q)^TfJ(q)Tf,表示关节力，f ff表示扭力，J ( q ) J(q)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 qq时，使用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 qq，关节角的速度为q ′ q'q′，可以通过函数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. -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 qq，可通过函数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 qq(关节角),q d qdqd(速度),q d d qddqdd(加速度),g r a v gravgrav(重力项，有默认值)
输入：
>> 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].北京:清华大学出版社,
