前言

本文介绍机器人力矩控制的一种方法——动力学前馈加反馈线性化，将机器人各关节的目标角度、角速度与实际角度、角速度的偏差构成线性误差，并引入到前馈分量中，从而跟踪期望的轨迹。核心内容源于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+Kdθ˙e+Kpθe+Ki∫θe(t)dt=0（1）{\ddot \theta _e} + {K_d}{\dot \theta _e} + {K_p}{\theta _e} + {K_i}\int {{\theta _e}(t)dt = 0} （1）θ¨e+Kdθ˙e+Kpθe+Ki∫θe(t)dt=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+Kdθ˙e+Kpθe+Ki∫θe(t)dt（2）{\ddot \theta}=\dot \theta _d+ {K_d}{\dot \theta _e} + {K_p}{\theta _e} + {K_i}\int {{\theta _e}(t)dt }（2）θ¨=θ˙d+Kdθ˙e+Kpθe+Ki∫θe(t)dt（2）
将（2）式代入机器人的动力学模型中，可得到反馈加前馈线性化控制器：
τ=M~(θ)(θ˙d+Kdθ˙e+Kpθe+Ki∫θe(t)dt)+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+Kdθ˙e+Kpθe+Ki∫θe(t)dt)+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(θ)=[14m1L12+m2(L12+L1L2cos⁡θ2+L22)12m2(L1L2cos⁡θ2+14L22)−12m2(L1L2cos⁡θ2+14L22)14m2L22c(θ,θ˙)=[−m2L1L2sin⁡θ2(θ˙1θ˙2+12θ˙22)12m2L1L2θ˙12sin⁡θ2]g(θ)=[(12m1+m2)L1gcos⁡θ1+12m2gL2cos⁡(θ1+θ2)12m2gL2cos⁡(θ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(θ)=[41m1L12+m2(L12+L1L2cosθ2+L22)21m2(L1L2cosθ2+41L22)21m2(L1L2cosθ2+41L22)−41m2L22c(θ,θ˙)=[−m2L1L2sinθ2(θ˙1θ˙2+21θ˙22)21m2L1L2θ˙12sinθ2]g(θ)=[(21m1+m2)L1gcosθ1+21m2gL2cos(θ1+θ2)21m2gL2cos(θ1+θ2)]

Simcape模型搭建

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

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

图4 物理模型

simulink控制率实现

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

图5 控制率实现

实践结果

笔者所给的期望角度、角速度、角加速度为θd=[π10t−π10t];θd˙=[π10−π10];θd¨=[00]\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=[10πt−10πt];θd˙=[10π−10π];θd¨=[00]
仿真的效果：

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

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

[论文]基于强化学习的无模型水下机器人深度控制
基于强化学习的无模型水下机器人深度控制摘要介绍问题公式 A.水下机器人的坐标框架 B.深度控制问题马尔科夫模型 A.马尔科夫决策 B.恒定深度控制MDP C.弯曲深度控制MDP D.海底追踪的 ...
【机器人算法】机器人动力学、运动学和基于动力学模型的仿真与控制
文章目录运动学机器人运动学参数标定(DH参数标定) 运动学逆解解析解数值解工作空间分析可操纵性轨迹规划路径规划静力学动力学动力学模型动力学仿真自由下落遇到跳床重力补偿动 ...
机器人动力学与控制学习笔记（九）————基于模糊自适应增益调整的机器人滑模控制
九.基于模糊自适应增益调整的机器人滑模控制采用自适应模糊系统,可实现机器人滑模控制中切换增益的自适应逼近,从而消除滑模控制中的抖振.本文设计一类基于模糊自适应增益调整的机器人滑模控制设计方法. 9. ...
2、前馈和反馈神经网络
前馈和反馈神经网络神经网络分类一.CNN **1.结构** **2.应用** **3.CNN的类型综述** 二.RNN **1.结构** **2.应用** **3.RNN类型** 三.GAN ** ...
机器人动力学与控制_大负载协作机器人难关怎么攻克？3大技术细节解析！
对传统机器人来说,大负载的机器人控制难度会更大,这极大影响了机器人的效率和稳定性. 其主要原因如下: (1)传统的机器人编码器安装在减速机输入侧,但是减速机多少都有背隙,随着臂展的变长,背隙造成末端点 ...
基于强化学习的自我完善聊天机器人
Elena Ricciardelli, Debmalya Biswas 埃琳娜·里恰德利(Elena Ricciardelli) Abstract. We present a Reinforcemen ...
（一）基于物联网的智能安防监控机器人2207231212569
基于物联网的智能安防监控机器人2207231212569 项目摘要: 机器人是人类一直期待的东西,但自动化的东西有点不同.理想情况下,机器人能够做的事情比自动化机器人想做的要多得多.自动化机器人希望实 ...
反馈线性化类有哪些最新发表的毕业论文呢？
一.总体简介反馈线性化的相关文献在1992年到2020年内共计627篇,主要集中在自动化技术.计算机技术.电工技术.航天(宇宙航行) 等领域,其中期刊论文521篇.会议论文69篇.专利文献37篇: ...
python开发酷q插件_基于python和酷Q的QQ机器人开发实践（1）
基本框架 (1) 酷Q:目前最常用的QQ机器人软件(尤其是在smartQQ失效以后). (2) 酷Q HTTP插件:通过HTTP对酷Q的事件进行上报以及接收HTTP请求来调用酷Q的DLL接口,从而可以 ...
机器人动力学简述 Robot Dynamics
机器人动力学有两个问题需要解决: 动力学正问题--根据关节驱动力矩或力,计算机器人的运动(关节位移.速度和加速度): 动力学逆问题--已知轨迹运动对应的关节位移.速度和加速度,求出所需要的关节力矩或力 ...

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

前言

理论