基于动力学前馈加反馈线性化的机器人动力学控制实现

前言

  本文介绍机器人力矩控制的一种方法——动力学前馈加反馈线性化，将机器人各关节的目标角度、角速度与实际角度、角速度的偏差构成线性误差，并引入到前馈分量中，从而跟踪期望的轨迹。核心内容源于Modern Robotics这本书的第11章第4节，笔者重在实践该部分理论。

理论

前馈控制

  轨迹控制的策略之一是使用机器人的动力学模型来产生力矩，机器人的动力学模型如下：
$$\tau =\tilde{M}(\theta )\ddot{\theta }+\tilde{h}(\dot{\theta },\ddot{\theta })$$
  上式中，$\tau$为机器人的力矩，当$M(\theta )=\tilde{M}(\theta )$且$h(\theta )=\tilde{h}(\theta )$时，模型是完美的，即机器人的动力学模型是精确的，那么，在没有初始状态误差的前提下，机器人是可以准确的跟踪轨迹的。但在现实中，始终存在建模误差和外界干扰，完全精确的动力学模型是无法获得的。而所有实用的控制器中都使用反馈，所以将前馈控制和反馈一起使用会有更好的效果。

前馈加反馈线性化

  将PID控制与机器人动力学模型结合起来，使其能沿着任何轨迹：
$${\ddot{\theta }}_e+K_d{\dot{\theta }}_e+K_p{\theta }_e+K_i\int {\theta }_e(t)dt=0（1）$$
  通过上式并选取适当的PID增益能够确保轨迹误差的指数衰减，由于${\ddot{\theta }}_e={\ddot{\theta }}_d-\ddot{\theta }$,其中${\ddot{\theta }}_e$角加速度误差，为实现误差动力学，为机器人选取如下指令加速度：
$$\ddot{\theta }={\ddot{\theta }}_d-{\ddot{\theta }}_e$$
  代入(1)式中：
$$\ddot{\theta }={\dot{\theta }}_d+K_d{\dot{\theta }}_e+K_p{\theta }_e+K_i\int {\theta }_e(t)dt（2）$$
  将（2）式代入机器人的动力学模型中，可得到反馈加前馈线性化控制器：
$$\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）$$
  该控制器的框图如下图所示：

图1 控制框图

实践

  本文基于simulink实现上述控制率，基于Simcape实现两连杆机械臂的模型搭建，从而完成轨迹跟踪。本文仅对简单的两连杆机械臂的实现，6自由度甚至更高自由度原理近似，有兴趣的读者可尝试去实现，不过高自由度机械臂动力学模型非常复杂，很难直接写出动力学方程表达式，可能需要递归逆运动学算法，其中包括正向迭代和逆向迭代阶段。

2连杆动力学模型

  如图设2连杆的质心分别位于杆的中心处（图中质心为端点处，笔者设为了中心处），m1=m2=1kg，l1=l2=1m，2连杆动力学模型可基于拉格朗日方程推导出，详细推导过程见Modern Robotics第8章第1节，推导出方程为：

图2 连杆模型

$$\tau =M(\theta )\ddot{\theta }+\underset{h(\theta ,\dot{\theta })}{\underbrace{c(\theta ,\dot{\theta })+g(\theta )}}$$

$$\begin{array}{rl}& M(\theta )=[\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}\\ & 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{array}$$

Simcape模型搭建

  如图3所示，输入为关节力矩，输出为实时测量的角度、角速度和角加速度

图3 Simcape搭建   实际搭建出的模型如图4所示：

图4 物理模型

simulink控制率实现

  图5为simulink的仿真图，联系了各关节的目标角度、角速度、角加速度与实际测得的角度、角速度，并将计算得到的力矩值输入到物理模型中产生所需的运动。

图5 控制率实现

实践结果

  笔者所给的期望角度、角速度、角加速度为$${\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]$$
  仿真的效果：

图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增益的调整，欢迎大佬指点和讨论，笔者不胜感激！

展望与思考

1. PID增益如何较好的设置，或有无动态调整的方法？
2. 真实的机器人控制器是在离散环境下运算的，离散系统如何仿真并保证稳定性？
3. 高自由度机器人动力学模型复杂情况下如何仿真？
4. 真实机器人的动力学控制大多用的什么方法，怎么做到高效且稳定的？