联合仿真下机械臂终端约束模型预测控制方法
时间：2022-12-10 09:40:04 来源：雅意学习网 本文已影响 人 
张 蕾，李义帅，王晓华，王文杰
(1.西安工程大学 电子信息学院，陕西 西安 710048；
2.陕西省人工智能联合实验室 西安工程大学分部，陕西 西安 710048)
随着全球新冠疫情的反复以及人口老龄化所造成的劳动力短缺与用工难等问题，机械臂的应用需求日益旺盛[1]。机械臂逐步替代人类生产生活中具有枯燥重复及恶劣危险性质的工作，而完成这些工作对机械臂的性能具有极高的考验，对机械臂的本体设计制造与控制方法的设计都是挑战。机械臂采用传统的瀑布式流程设计开发，存在着开发效率低下、成本较高等问题，且机械臂的轨迹跟踪控制是机械臂大规模应用的基础性问题[2-3]。
针对机械臂的轨迹跟踪控制问题，国内外众多学者对此深入研究并取得颇多建设性的成果[4-5]。在日常应用中，传统的PID控制方法是机械臂运动控制应用最广泛的控制方法之一，但是多关节机械臂存在着非线性耦合现象，其各个连杆之间存在着相互力作用，会影响到机械臂高速运动时的控制精度以及稳定性，且无法解决机械臂关节运动存在的约束问题，会造成较大的跟踪误差以及抖动现象[6-9]。模型预测控制(model predictive control，MPC)是目前主流的先进控制方法之一，具备在复杂条件下处理约束的能力，在机械臂运动控制的领域不断发展，在学术界和工业界都有着大量的研究与应用，已成为解决机械臂控制问题的流行方法[10-11]。HASSAN等针对柔性单关节机械臂振动抑制的问题，采用有限元法建模，基于阶跃响应实现动态矩阵控制，但阶跃响应并不能很好地应用在多关节机械臂控制[12]；
HEDJAR等针对双关节机械臂轨迹跟踪控制问题，采用泰勒展开法对机械臂模型进行线性化处理，设计固定控制律，同时加入积分环节确保系统的鲁棒性，但是对机械臂所建立的模型与本体之间的精确度要求高，且未对约束条件进行处理[13]；
BELAD等针对工业并联机械臂，对外界噪声扰动进行估计实现调整模型预测控制方法的参数，虽然抑制了系统扰动造成的误差，但是忽略了约束条件，并不能应用在真实硬件环境上[14]。在进行控制方法设计时，通常情况采用理论计算来研究机械臂的动力学控制问题，但是存在计算量过大的弊端，而且专家学者们对机械臂模型进行了简化，使得理论仿真结果并不理想[15]。另外，真实的机械臂测试成本较高且在实验室环境下存在一定危险，建立具有更好的技术基础设施的高度复杂的实验台需要巨大的成本。为克服这些问题，可以使用仿真环境模拟、可视化和验证控制方法。
针对机械臂传统开发过程存在的跟踪误差较大、无法处理约束、效率低下、成本较高等问题，本文提出一种基于终端约束的机械臂轨迹跟踪模型预测控制方法。该控制方法通过设置目标函数及终端状态等式约束条件将机械臂轨迹跟踪问题转化成模型预测控制的数值规划问题。再基于模型在环的仿真思想，采用轻量化低成本设备树莓派Raspberry Pi搭建基于Gazebo-Simulink的机械臂联合仿真平台，验证本文所提控制方法的有效性。
良好的机器人仿真系统能够构造接近真实世界的物理环境，快速测试机器人设计可靠性以及控制方法的可用性[16-17]。Gazebo是一款开源软件，对于复杂的多刚体仿真速度快、实时性强，相比于传统的ADAMS软件，更适合对机械臂进行动力学仿真。通过与Simulink进行协同工作，借助Simulink的图形化编程可以实现快速编程，降低开发难度，提高开发效率[18]。
本文研究的被控对象不是机械臂的数学模型，是Gazebo中的机械臂物理模型，其模型文件包含了机械臂的运动学与动力学参数，Gazebo中的机械臂物理模型具备真实机械臂的动力学特性，需要根据动力学方程所呈现出来的机械臂系统的驱动力矩与关节位移的输入输出关系进行控制方法设计。
接下来建立SoildWorks中的虚拟物理样机模型，并导出机械臂的统一机器人描述格式(unified robot description format ，URDF)文件，为后续的Gazebo机械臂物理仿真的实现提供建模基础；
然后，基于拉格朗日法建立三关节机械臂的动力学方程，为后续进行机械臂轨迹跟踪控制方法的设计提供理论基础。
1.1 三关节机械臂物理建模
以实验室与中科深谷公司共同开发的三关节机械臂ROCR3为被控对象进行分析研究，该三关节机械臂本体由连接部件、底座以及末端部件3个可重组的关节模组组成，是典型的三关节拟人臂。该三关节机械臂实体及尺寸参数如图1所示。
(a) 三关节机械臂实体 (b)机械臂尺寸参数图 1 三关节机械臂及尺寸参数Fig.1 Three-joint manipulator and its parameters
机械臂必须在满足本体参数限制的条件下运行，若负载超限制则会损坏机械臂。机械臂控制方法设计必须符合机械臂的物理特性满足约束条件，否则也将会严重损坏机械臂，甚至会对使用者的人身安全造成威胁。本文严格参考机械臂的真实数据，依照机械臂的CAD文件，通过SolidWorks建立其虚拟物理样机模型。SolidWorks中的虚拟物理样机模型包含了机械臂的质量属性、尺寸等参数。Solidworks中的虚拟物理样机的建立为下一步建立机械臂的动力学方程做了初步准备，同时也为下一步建立Gazebo中的机械臂物理模型做建模准备。通过SolidWorks的sw_urdf_exporter插件配置并导出机械臂的URDF文件，通过此插件导出机械臂的URDF文件，相比基于文本的编程方式建立机械臂的URDF文件，可以大大提高机械臂模型建立的效率，节省时间并减少建模过程中的错误。机械臂的URDF文件内容包含机械臂的连杆、关节名称、运动学参数、动力学参数、可视化模型、碰撞检测模型等[19]。首先通过check_urdf命令验证URDF文件的正确性，然后在Gazebo中通过可视化仿真验证机械臂的建模正确性。图2为SolidWorks中的虚拟物理样机模型以及Gazebo中的机械臂物理模型。
(a) SolidWorks虚拟样机 (b) Gazebo物理模型图 2 机械臂物理仿真模型Fig.2 Physical model of manipulator
从图2可以看出，Gazebo中所建立的机械臂模型和SolidWorks中的虚拟物理样机模型以及真实机械臂关节及连杆具有一致性，证明Gazebo中的机械臂物理模型建模正确。
1.2 三关节机械臂动力学方程建立
采用拉格朗日方程法建立三关节机械臂的动力学模型。为建模方便，对其进行简似化处理，可得三关节机械臂的简化动力学方程[20-21]为
(1)
通过对SolidWorks内搭建的三关节机械臂的虚拟物理样机模型进行测量计算得到机械臂动力学方程的参数，可以减少建模误差，实现更精准的控制，将测量得到的动力学方程参数代入机械臂动力学方程式(1)中可以得到更精确的机械臂动力学模型，精确的动力学模型的建立使得数学模型更接近于真实机械臂，从而设计符合动力学条件的控制方法。
通过Simulink中的Robotics System工具箱，使Gazebo和Simulink协同工作，搭建联合仿真平台，进行模型在环测试。联合仿真的过程相当于闭环的数据交换，实现机械臂的实时闭环控制。
该联合仿真平台由Matlab中的Simulink仿真控制器和Gazebo中的仿真被控对象机械臂组成，在Matlab中显示仿真数据曲线，在Gazebo中显示三维仿真结果。
2.1 联合仿真建立
联合仿真的仿真对象由2部分组成:一部分是控制器模型的仿真，一部分是被控对象模型的仿真。借助于Simulink库中Gazebo Co-Simulation的通信模块，实现发布与订阅话题，从而实现Simulink与Gazebo闭环数据交换[22]。
树莓派性能优异足以完成机械臂的仿真且成本较低，因此本文采用便携式的微型单板计算机树莓派(Raspberry Pi)搭载Gazebo实现被控对象机械臂的仿真。Gazebo中的物理模型机械臂在图2中已经建立，控制器仿真模型如图3所示。
图 3 控制器仿真模型Fig.3 Structure diagram of controller simulation model
图3中的控制器仿真模型主要分为4部分：期望轨迹输入模块可以自定义关节期望轨迹，进行不同期望轨迹的测试与对比；
控制器模块可以通过S函数或者已有Simulink模块库设计控制方法；
仿真效果可视化模块可以通过Scope示波器模块实时显示机械臂的运行状态，包括关节的角度、角速度以及加速度和力矩变化情况等；
Gazebo通信模块通过主从分布式的话题消息通信机制实现交互通信。
2.2 联合仿真有效性验证
(2)
式中：Kp为比例环节的参数项；
Ki为积分环节的参数项；
Kd为微分环节的参数项；
t为时间参数；
u(t)为输入控制力矩；
r(t)为期望轨迹；
q(t)为机械臂反馈回来的实际轨迹；
轨迹跟踪误差e(t)=r(t)-q(t)。
设置仿真环境以及参数。3个关节的期望轨迹均为y=(π/3)sint，单位为rad，初始角弧度为0 rad，角速度为0 rad/s，Gazebo中机械臂为站立姿态。Simulink中机械臂控制器采样时间设置为Ts1=0.01 s，仿真总时间设置为Ts2=20 s。前馈补偿PID控制方法的PID参数分别设置为Kp=[100 60 10]，Ki=[10 6 1]，Kd=[5 1 0.1]。Gazebo中重力加速度为9.8 m/s2，环境温度为26 ℃，风力干扰为0 N。首先在树莓派上启动Gazebo实现机械臂可视化，然后在Simulink中运行联合仿真，在本文所设置实验条件下进行多次重复实验，实验结果一致。
图4(a)、(b)、(c)依次为前馈补偿PID控制方法应用在机械臂关节1、2、3的实际关节轨迹跟踪期望轨迹的位置响应曲线以及关节角度跟踪误差曲线。图中蓝色为真实值，红色为参考值。
(a) 关节1
(b) 关节2
(c) 关节3图 4 前馈补偿PID控制方法下关节位置响应 及关节角度跟踪误差曲线Fig.4 Joint position response and joint angle tracking error curve under feedforward compensation PID control method
从图4(a)～(c)可以看出，在三关节机械臂上应用前馈补偿PID控制方法时，关节1在2.05 s左右跟踪误差趋于稳态，波形平稳后关节1跟踪误差在[-0.002 8，0.003 3]之间；
关节2在2.48 s左右跟踪误差趋于稳态，波形平稳后关节2跟踪误差在[-0.063 3，0.065 6]之间；
关节3在2.48 s左右跟踪误差趋于稳态，波形平稳后关节3跟踪误差在[-0.115 9，0.116 5]之间。可以得出，尽管各关节跟踪误差最后收敛在一定范围内，但是机械臂运行初期误差波动较大，在Gazebo中也能观察到对应的抖动现象；
受到各关节互相作用力的影响，关节2、3的跟踪误差较大。
图5为三关节机械臂的控制力矩变化曲线。u1、u2、u3分别代表机械臂关节1、2、3的力矩变化曲线。
图 5 关节1、2、3控制力矩曲线Fig.5 Control torque curve of joint 1, 2 and 3
从图5可以看出，各关节力矩在运行初期振荡明显，波动较大，进入稳态后呈周期性变化，无明显振荡现象，且受本身机械结构影响，关节2的峰值力矩最大，关节1的峰值力矩最小，关节3的峰值力矩较小。结合图4(a)～(c)可以得出，运行初期关节力矩频繁调节导致振荡现象，造成初始误差波动，从而导致Gazebo中机械臂存在抖动现象，在控制力矩进入稳态后，跟踪误差也随之进入稳态，呈现周期性变化。
综合以上仿真结果可以得出，在联合仿真中，Gazebo中的机械臂按照期望轨迹运动，Simulink中获取到了Gazebo中机械臂反馈回来的数据，实现了闭环内联合仿真软件之间的实时数据交互。结果证明联合仿真的流程与设计思路是正确的，该联合仿真是成功有效的。本文所搭建的联合仿真平台能够更直观地验证控制效果，是一种更接近于真实环境的仿真模型，弥补了传统开发方法的不足，具有成本低、快速验证的优点。
但是在三关节机械臂上应用前馈补偿PID控制方法存在着控制精度较差，响应速度较慢以及初期力矩变化振荡的问题。
为解决传统的前馈补偿PID控制方法在三关节机械臂上应用时存在的控制精度较差以及无法处理约束条件等问题，本文提出一种基于终端约束的机械臂轨迹跟踪模型预测控制方法。采用雅可比线性化方法对机械臂动力学方程进行线性化处理[10]，再进行离散化得到机械臂的离散线性动力学方程，最后设计基于终端约束的模型预测控制方法(model predictive control method with terminal constraints，TMPC)实现轨迹跟踪控制。
3.1 机械臂动力学方程线性离散化
(3)
接下来对式(3)进行雅可比线性化处理，根据上文中的状态向量，求出其分量的导数函数f的雅可比子矩阵，其表达式为
(4)
首先定义线性化后的状态向量为xM，控制向量为uM，则该系统方程为
(5)
式(5)中的常数项可以用D(D∈R+)来表示，则系统线性化后的方程为
(6)
根据连续离散化的基本约定，可以得到离散时间线性化状态空间方程的简化形式，即
(7)
3.2 终端约束模型预测控制方法设计
预测控制的核心思想是根据被控对象的系统模型建立预测模型，并设计包含控制输入量与预测误差量的目标函数，添加约束条件后通过求解最小化目标函数的数值规划问题求得最优控制输入序列，从而进一步实现机械臂轨迹跟踪控制。
在预测模型建立的过程中，假设在时刻k时，给定一组控制输入U(k)，预测模型向后预测Np步可以得到k时刻的预测输入输出以及状态序列。
预测模型的输出序列为
(8)
预测模型的状态序列为
(9)
预测模型的控制输入序列为
(10)
给定的参考轨迹期望输入序列为
(11)
预测模型的误差状态序列为
E(k)=Y(k+1|k)-R(k)=
(12)
根据模型预测控制的最优性能指标定义，选择预测输出序列与参考轨迹期望输入序列的误差量和控制输入量作为性能指标，则TMPC控制器的目标函数可设置为
(13)
式中：e(k+i|k)=y(k+i|k)-r(k+i|k)；
Q为TMPC控制器的误差权重对角矩阵；
R为TMPC控制器的控制输入权重对角矩阵，均为正定矩阵。不同的权重矩阵的设置将会影响被控对象的动态性能。
为使机械臂能够跟踪期望轨迹，本文考虑加入终端状态等式约束条件，定义预测误差状态序列E(k)处于终端约束内，为终端状态量。特别地，强制E(k)=0即零终端情况下会导致预测控制问题无可行解[24]。故本文在每个预测时域的(k+Np)终端时刻，令E(k+Np|k)=0即预测的最后序列使得预测误差收敛于零，此时系统无需再调节控制输入即预测控制输入量U(k+Np|k)=0，保证机械臂在每个预测时域内能够有效地跟踪期望轨迹。则所提TMPC的优化控制问题可以描述为
(14)
满足x(k+1)=Ax(k)+Bu(k)+c，y(k)=Cx(k),同时umin(k+i|k)≤u(k+i|k)≤umax(k+i|k),i=0,1,2,…,Np-1；
ymin(k+i|k)≤y(k+i|k)≤ymax(k+i|k),i=1,2,…,Np；
这2个式子为控制输入与输出的约束条件。本文所提控制方法具体的实现流程分为3个步骤。
1) 根据系统约束与终端约束条件求解最优性能指标目标函数J，计算最优控制输入量U*(k)。
2) 将U*(k)序列中第一个控制向量u*(k|k)作用于被控对象机械臂可以得到测量值x(k+1|k)。
3) (k+1)时刻，令系统平衡点xr=x(k+1|k)，更新预测模型系统状态量，返回第1)步，实现滚动优化。
在Matlab中可以通过调用Hybrid Toolbox工具箱实现TMPC控制器的设计，该工具箱支持自动生成C++代码，实现嵌入式开发与测试，数值规划问题的求解可以使用YALMIP工具箱调用其他开源求解器，例如CasADi等，加快求解速度[25]。
为验证本文所提TMPC控制方法的有效性，在联合仿真环境下对三关节机械臂进行了仿真研究，仿真环境参数设置同上文2.2节。图6为本文所提出的TMPC控制方法作用在三关节机械臂上关节1、2、3的实际关节轨迹跟踪期望轨迹的位置响应曲线以及关节角度跟踪误差曲线。图中蓝色代表真实值，红色代表参考值。
(a) 关节1
(b) 关节2
(c) 关节3图 6 关节位置响应曲线及关节角度跟踪误差曲线Fig.6 Joint position response curve and joint angle tracking error curve
从图6(a)～(c)可以看出，在0.31 s左右，关节1、2、3的跟踪误差进入稳态，呈周期性变化。波形平稳后，关节1跟踪误差在[-0.005 5，0.000 3]之间，关节2跟踪误差在[-0.008 9，0.001 8]之间，关节3跟踪误差在[-0.002 8，0.035 8]之间。
在本文所设置的仿真参数与条件下，应用所提控制方法，与图4(a)～(c)中应用前馈补偿PID控制方法进行对比后可以发现，各关节跟踪误差均有减小，并且Gazebo中机械臂按照预定的期望轨迹进行平稳运动。各个关节调节时间较快，实际位置在较快的时间内跟踪上了期望位置，且运行稳定后关节1、2跟踪误差在10-3rad左右，有着较好的控制精度。
图7为本文所提出的TMPC控制方法作用在三关节机械臂关节1、2、3的控制力矩曲线图。
图 7 关节1、2、3控制力矩曲线Fig.7 Control torque curve of joint 1,2 and 3
从图7可以看出，虽然各关节初始控制力矩较大，但是处于约束条件内，能够实现快速调节，进入稳态后呈周期性变化，相比于图5中应用前馈补偿PID控制方法，本文所提控制方法的力矩变化曲线初期振荡减少且调节快速，结合图6(a)～(c)可以得出，运行初期控制力矩快速调节使得轨迹跟踪快速响应，且跟踪误差较小。
综合以上结果可以得出：应用本文所提控制方法时，机械臂在更短的时间跟踪上期望位置并趋于平稳，且运行平稳后的跟踪误差更小，跟踪精度有所提高，控制力矩变化波动较小，初始阶段的振荡现象明显减弱，证明本文所提的TMPC控制方法在三关节机械臂的轨迹跟踪上更具有优势。
本文采用Gazebo与Simulink搭建三关节机械臂联合仿真平台，并提出一种基于终端约束的机械臂轨迹跟踪模型预测控制方法。所搭建的联合仿真平台将控制方法、可视化工具以及通信标准高效地集成，降低了机械臂初期开发成本，提高了开发效率；
所提出的轨迹跟踪控制方法在机械臂轨迹跟踪控制性能上有所提升，能够实现快速调节，降低了跟踪误差，提高了控制平稳性。本文所搭建的联合仿真平台与所提控制方法，具有一定的工程应用与理论研究价值。
