前言
本文介绍机器人力矩控制的一种方法——动力学前馈加反馈线性化，将机器人各关节的目标角度、角速度与实际角度、角速度的偏差构成线性误差，并引入到前馈分量中，从而跟踪期望的轨迹。核心内容源于Modern Robotics这本书的第11章第4节，笔者重在实践该部分理论。
理论
前馈控制
轨迹控制的策略之一是使用机器人的动力学模型来产生力矩，机器人的动力学模型如下：
τ
=
M
~
(
θ
)
θ
¨
+
h
~
(
θ
˙
,
θ
¨
)
\tau = \tilde M(\theta )\ddot \theta + \tilde h(\dot \theta ,\ddot \theta )
τ
=
M
~
(
θ
)
θ
¨
+
h
~
(
θ
˙
,
θ
¨
)
上式中，
τ
\tau
τ
为机器人的力矩，当
M
(
θ
)
=
M
~
(
θ
)
M(\theta ) = \tilde M(\theta)
M
(
θ
)
=
M
~
(
θ
)
且
h
(
θ
)
=
h
~
(
θ
)
h(\theta ) = \tilde h(\theta)
h
(
θ
)
=
h
~
(
θ
)
时，模型是完美的，即机器人的动力学模型是精确的，那么，在没有初始状态误差的前提下，机器人是可以准确的跟踪轨迹的。但在现实中，始终存在建模误差和外界干扰，完全精确的动力学模型是无法获得的。而所有实用的控制器中都使用反馈，所以将前馈控制和反馈一起使用会有更好的效果。
前馈加反馈线性化
将PID控制与机器人动力学模型结合起来，使其能沿着任何轨迹：
θ
¨
e
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
=
0
（
1
）
{\ddot \theta _e} + {K_d}{\dot \theta _e} + {K_p}{\theta _e} + {K_i}\int {
{\theta _e}(t)dt = 0} （1）
θ
¨
e
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
=
0
（
1
）
通过上式并选取适当的PID增益能够确保轨迹误差的指数衰减，由于
θ
¨
e
=
θ
¨
d
−
θ
¨
{\ddot \theta _e} = {\ddot \theta _d} – \ddot \theta
θ
¨
e
=
θ
¨
d
−
θ
¨
,其中
θ
¨
e
\ddot \theta _e
θ
¨
e
角加速度误差，为实现误差动力学，为机器人选取如下指令加速度：
θ
¨
=
θ
¨
d
−
θ
¨
e
{\ddot \theta} = {\ddot \theta _d} – \ddot \theta_e
θ
¨
=
θ
¨
d
−
θ
¨
e
代入(1)式中：
θ
¨
=
θ
˙
d
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
（
2
）
{\ddot \theta}=\dot \theta _d+ {K_d}{\dot \theta _e} + {K_p}{\theta _e} + {K_i}\int {
{\theta _e}(t)dt }（2）
θ
¨
=
θ
˙
d
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
（
2
）
将（2）式代入机器人的动力学模型中，可得到反馈加前馈线性化控制器：
τ
=
M
~
(
θ
)
(
θ
˙
d
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
)
+
h
~
(
θ
˙
,
θ
¨
)
（
3
）
\tau = \tilde M(\theta )(\dot \theta _d+ {K_d}{\dot \theta _e} + {K_p}{\theta _e} + {K_i}\int {
{\theta _e}(t)dt }) + \tilde h(\dot \theta ,\ddot \theta ) （3）
τ
=
M
~
(
θ
)
(
θ
˙
d
+
K
d
θ
˙
e
+
K
p
θ
e
+
K
i
∫
θ
e
(
t
)
d
t
)
+
h
~
(
θ
˙
,
θ
¨
)
（
3
）
该控制器的框图如下图所示：
图1 控制框图
实践
本文基于simulink实现上述控制率，基于Simcape实现两连杆机械臂的模型搭建，从而完成轨迹跟踪。本文仅对简单的两连杆机械臂的实现，6自由度甚至更高自由度原理近似，有兴趣的读者可尝试去实现，不过高自由度机械臂动力学模型非常复杂，很难直接写出动力学方程表达式，可能需要递归逆运动学算法，其中包括正向迭代和逆向迭代阶段。
2连杆动力学模型
如图设2连杆的质心分别位于杆的中心处（图中质心为端点处，笔者设为了中心处），m1=m2=1kg，l1=l2=1m，2连杆动力学模型可基于拉格朗日方程推导出，详细推导过程见Modern Robotics第8章第1节，推导出方程为：
图2 连杆模型
τ
=
M
(
θ
)
θ
¨
+
c
(
θ
,
θ
˙
)
+
g
(
θ
)
⏟
h
(
θ
,
θ
˙
)
\tau=M(\theta) \ddot{\theta}+\underbrace{c(\theta, \dot{\theta})+g(\theta)}_{h(\theta, \dot{\theta})}
τ
=
M
(
θ
)
θ
¨
+
h
(
θ
,
θ
˙
)
c
(
θ
,
θ
˙
)
+
g
(
θ
)
M
(
θ
)
=
[
1
4
m
1
L
1
2
+
m
2
(
L
1
2
+
L
1
L
2
cos
θ
2
+
L
2
2
)
1
2
m
2
(
L
1
L
2
cos
θ
2
+
1
4
L
2
2
)
−
1
2
m
2
(
L
1
L
2
cos
θ
2
+
1
4
L
2
2
)
1
4
m
2
L
2
2
c
(
θ
,
θ
˙
)
=
[
−
m
2
L
1
L
2
sin
θ
2
(
θ
˙
1
θ
˙
2
+
1
2
θ
˙
2
2
)
1
2
m
2
L
1
L
2
θ
˙
1
2
sin
θ
2
]
g
(
θ
)
=
[
(
1
2
m
1
+
m
2
)
L
1
g
cos
θ
1
+
1
2
m
2
g
L
2
cos
(
θ
1
+
θ
2
)
1
2
m
2
g
L
2
cos
(
θ
1
+
θ
2
)
]
\begin{aligned} &M(\theta)=\left[\begin{array}{cc} \frac{1}{4} \mathfrak{m}_{1} L_{1}^{2}+\mathfrak{m}_{2}\left(L_{1}^{2}+L_{1} L_{2} \cos \theta_{2}+L_{2}^{2}\right) & \frac{1}{2} \mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+\frac{1}{4} L_{2}^{2}\right)^{-} \\ \frac{1}{2} \mathfrak{m}_{2}\left(L_{1} L_{2} \cos \theta_{2}+\frac{1}{4} L_{2}^{2}\right) & \frac{1}{4} \mathfrak{m}_{2} L_{2}^{2} \end{array}\right.\\ &c(\theta, \dot{\theta})=\left[\begin{array}{c} -\mathbf{m}_{2} L_{1} L_{2} \sin \theta_{2}\left(\dot{\theta}_{1} \dot{\theta}_{2}+\frac{1}{2} \dot{\theta}_{2}^{2}\right) \\ \frac{1}{2} \mathfrak{m}_{2} L_{1} L_{2} \dot{\theta}_{1}^{2} \sin \theta_{2} \end{array}\right]\\ &g(\theta)=\left[\begin{array}{c} \left(\frac{1}{2} \mathfrak{m}_{1}+\mathfrak{m}_{2}\right) L_{1} g \cos \theta_{1}+\frac{1}{2} \mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \\ \frac{1}{2} \mathfrak{m}_{2} g L_{2} \cos \left(\theta_{1}+\theta_{2}\right) \end{array}\right] \end{aligned}
M
(
θ
)
=
[
4
1
m
1
L
1
2
+
m
2
(
L
1
2
+
L
1
L
2
cos
θ
2
+
L
2
2
)
2
1
m
2
(
L
1
L
2
cos
θ
2
+
4
1
L
2
2
)
2
1
m
2
(
L
1
L
2
cos
θ
2
+
4
1
L
2
2
)
−
4
1
m
2
L
2
2
c
(
θ
,
θ
˙
)
=
[
−
m
2
L
1
L
2
sin
θ
2
(
θ
˙
1
θ
˙
2
+
2
1
θ
˙
2
2
)
2
1
m
2
L
1
L
2
θ
˙
1
2
sin
θ
2
]
g
(
θ
)
=
[
(
2
1
m
1
+
m
2
)
L
1
g
cos
θ
1
+
2
1
m
2
g
L
2
cos
(
θ
1
+
θ
2
)
2
1
m
2
g
L
2
cos
(
θ
1
+
θ
2
)
]
Simcape模型搭建
如图3所示，输入为关节力矩，输出为实时测量的角度、角速度和角加速度
图3 Simcape搭建
实际搭建出的模型如图4所示：
图4 物理模型
simulink控制率实现
图5为simulink的仿真图，联系了各关节的目标角度、角速度、角加速度与实际测得的角度、角速度，并将计算得到的力矩值输入到物理模型中产生所需的运动。
图5 控制率实现
实践结果
笔者所给的期望角度、角速度、角加速度为
θ
d
=
[
π
10
t
−
π
10
t
]
;
θ
d
˙
=
[
π
10
−
π
10
]
;
θ
d
¨
=
[
0
0
]
\theta_d=\left[\begin{array}{c} \frac{\pi}{10} t \\ -\frac{\pi}{10} t \end{array}\right] ; \dot{\theta_d}=\left[\begin{array}{c} \frac{\pi}{10} \\ -\frac{\pi}{10} \end{array}\right] ; \ddot{\theta_d}=\left[\begin{array}{l} 0 \\ 0 \end{array}\right]
θ
d
=
[
1
0
π
t
−
1
0
π
t
]
;
θ
d
˙
=
[
1
0
π
−
1
0
π
]
;
θ
d
¨
=
[
0
0
]
仿真的效果：
图6 动态轨迹追踪过程
图7为关节1和关节2的角度跟踪：
图7 角度跟踪
图8为关节1和关节2的角速度跟踪：
图8 角速度跟踪
图6与图7可看出，该控制率能较好的实现轨迹跟踪。
为更好的验证该控制律的适用性，故意将动力学模型部分的错误的写成m2=0.9kg，其他不变，图8和图9为非精确模型下的控制效果：
图9 非精确模型下的角度跟踪
图10 非精确模型下的角速度跟踪
可知基本实现了轨迹跟踪，在t=12s附近角度上出现稍许的不稳定，角速度出现了大的浮动，原因有待研究，可见，任何控制率都不是完美的，也希望大家能够一起积极探讨！
但在误差非常大的情况下，比如增大m1的误差，令m1=m2=0.9kg，结果为：
图11 大误差模型下的角度跟踪
图12 大误差模型下的角速度跟踪
此时轨迹跟踪效果欠佳，这个现象说明了此控制率的局限性，但笔者觉得可有更多的挖掘，比如PID增益的调整，欢迎大佬指点和讨论，笔者不胜感激！
展望与思考
- PID增益如何较好的设置，或有无动态调整的方法？
- 真实的机器人控制器是在离散环境下运算的，离散系统如何仿真并保证稳定性？
- 高自由度机器人动力学模型复杂情况下如何仿真？
- 真实机器人的动力学控制大多用的什么方法，怎么做到高效且稳定的？