基于SDRE控制的双臂空间机器人轨迹优化跟踪

顾建华，孙翠玲，贲能军，3

（1.盐城工业职业技术学院机电工程学院，江苏 盐城 224000；2.盐城师范学院信息工程学院，江苏 盐城 224001；3.安徽工业大学，安徽 马鞍山 243002）

1 引言

相比于单臂空间机器人系统，双臂空间机器人系统抓取物体载荷更大、精度更高、稳定性更好；同时，双臂空间机器人系统的运动规划和动力学控制过程，会由于所处的微重力环境和由其带来的强耦合及非线性增强等问题，而变得更加复杂。这些因素使得双臂空间机器人系统引起了国内外研究人员的广泛关注［1-3］。

近年来，绝大多数研究主要集中在空间机器人参数未知及外界扰动的问题上，文献［4］使用神经网络在线建模逼近系统中的非线性模型，并通过鲁棒控制器来消除逼近误差及外界有界扰动，以解决空间机器人高精度轨迹跟踪问题；文献［5］针对自由漂浮空间机器人系统惯性参数不确定问题，设计了一类基于逆动力学法的自适应控制器；文献［6］基于确定学习理论，设计了一种新的自适应神经网络控制算法，对未知闭环系统进行了局部准确逼近；梁捷［7］设计了一种基于标称力矩控制器附加模糊自适应补偿控制器的复合控制方案，克服了系统参数未知的影响。

然而，上述研究均没有具体给出衡量机械臂轨迹跟踪精度及各关节输出力矩能量的性能指标，难以对所设计控制方法的有效性进行定量分析，无法满足实际工程应用中的控制精度要求。状态依赖Riccati方程（State-dependent Riccati Equation，SDRE）方法是一种高效的非线性优化控制方法［8］，长期以来得到了十分重要的理论发展和实际应用［9-11］。在机械臂方面，文献［12］设计了一种基于SDRE的控制器，首次将该方法应用于工程实践中，并通过物理实验验证了该方法的可行性；文献［13］基于SDRE理论，设计了一种机械臂点到点运动控制器，并通过神经网络补偿控制器来克服未知参数带来的影响，以提高控制器的鲁棒性。然而，上述神经网络补偿控制器降低了控制输入的优化性能，且这些设计都仅仅针对的是地面机械臂，无法直接应用到模型更加复杂且存在动力学耦合的空间机器人中。

这里将SDRE理论应用于惯性参数未知条件下空间机械臂的轨迹跟踪问题中，提出一种离线标称SDRE控制器结合在线补偿SDRE控制器的轨迹跟踪控制方法。首先，建立双臂空间机器人自由漂浮状态下的关节空间动力学模型；而后将标称状态量和实际状态量作为标称动力学方程的反馈输入量，借助增广变量法，分别得到空间机器人关于关节角度和角速度的类线性标称状态方程和实际状态方程；根据标称状态方程设计离线标称SDRE控制器，并建立其与实际状态方程的误差模型，由此设计在线补偿SDRE控制器补偿标称控制器的跟踪误差。仿真结果验证了所设计控制器的有效性。

2 系统动力学建模

以双臂两连杆平面自由漂浮空间机器人系统为研究对象，如图1所示。

图1 双臂空间机器人系统Fig.1 Dual-Arm Space Robot System

设该系统由一个自由漂浮基座M0及四个机械臂M1、M2、M3、M4组成，建立各分体M（ii=0，1，2，3，4）的主轴坐标系（Oi-xiy）i，并设ei(i=1，2，3，4)为xi轴的基矢量，其中：O0与M0的质心Oc0重合，O1，O2，O3，O4分别为连接M1与M0、M2与M1、M3与M0、M4与M3的转动幅中心，xi(i=1，2，3，4)为机械臂的对称轴。设O0在O0e0轴上与O1的距离为l0，M（ii=0，1，2，3，4）沿轴ei(i=1，2，3，4)的长度为li(i=1，2，3，4)；质心Oci在轴ei上与Oi的距离为ai(i=1，2，3，4)；各分体的质量和中心惯量张量分别为mi和Ji，M=为系统总质量，O为系统总质心。

对上述自由漂浮空间机器人系统作出如下假设：（1）双臂空间机器人系统每个关节均受主动控制，且在同一平面内运动；（2）不考虑空间环境下的微弱重力影响，系统处于不受外力作用的自由漂浮状态；（3）各构件均为刚性体，系统为刚性系统。由系统的动量守恒关系及Lagrange第二类方程，根据扩展机械臂方法［14］，可导出自由漂浮双臂空间机器人系统的动力学方程为［15］：

式中：M(q)∈R4×4—系统质量矩阵；C(q，q)q∈R4×1—包含哥氏力和离心力的广义力矢量，τ机械臂4个关节幅的控制力矩组成的4维列矢量，τ=[τ1，τ2，τ3，τ4]T，q—系统坐标组成的列矢量，q=[θ1，θ2，θ3，θ4]T。

动力学方程式（1）具备如下性质：（1）惯性矩阵M(q)∈R4×4为对称正定矩阵。（2）通过选取合适的C(q，q)q，可以使(M-2C)为斜对称矩阵。

3 SDRE基本理论

考虑如下形式的非线性系统：

式中：x∈Rn—系统的状态向量；u∈Rn—输入向量，且t∈[0，∞)，B(x)≠0，f(0)=0。则可以给出具有如下形式的无限时间性能指标：

其中Q(x)>0，R(x)>0。

引理1［8］.基于扩展线性化方法，当式（2）满足：

（1）f(0)=0；

（2）在正常的取值范围内，f(x)具有一阶连续微分时；

则可将该状态方程转化为状态依赖参数SDC（Sdate Depen⁃dent Coefficient）的类线性结构方程：

引理2［9］.当式（5）中的SDC矩阵A(x)和B(x)满足如下条件：

（1）{A(x)，B(x)}逐点可控；

（2）{A(x)，Q12(x)}逐点可观；

则上述状态依赖Riccati 方程（SDRE）可以看成是线性二次型调节器（LQR）方法在非线性控制的推广，针对由非线性状态方程（2）和性能指标函数（3）描述的无限时间状态调节问题，可设计局部渐进稳定优化控制律为：

其中P(x)满足代数Riccati方程：

需要指出的是，SDC矩阵的选取没有唯一的标准方法，同时可以通过调整权值矩阵Q(x)、R(x)，根据实际工程需求，对系统的跟踪误差和控制量的大小进行有效的折衷，这说明SDRE只是一种优化控制而并非最优控制，但这也体现了该方法的灵活性，是其它非线性方法所不具备的。

4 优化控制器设计

本节所设计的控制器由离线标称SDRE控制器和在线补偿SDRE 控制器两部分组成。根据给定的离线固定惯性参数获得系统标称动力学方程，并由此选取离线SDC 矩阵，设计离线SDRE控制器；而后，将实际状态量作为标称动力学方程的反馈输入量，得到实际状态方程，建立其与标称状态的误差方程，并由此设计在线补偿控制器。所设计的控制器较好的解决了惯性参数未知带来的系统模型不确定问题，并且给出了性能优化指标，是一种优化控制器。其控制系统框图，如图（2）所示。

图2 控制器系统框图Fig.2 Control System Framework

4.1 离线标称SDRE控制器设计

式中：qc—标称模型输出关节角速度，令状态量为：x1c=qc、x2c=qc，可得：

其中，

选取关节角度和角速度x=[x1c x2c]作为系统的增广变量，则该系统的标称状态方程变为：

则可设计离线标称SDRE控制律为：

式中：qd—双臂空间机器人系统机械臂各关节在关节空间的期望运动轨迹，P1(x)满足代数Riccati方程：

4.2 在线补偿控制器设计

在惯性参数未知条件下，给出双臂空间机器人系统的实际动力学模型：

式中：M(q)、C(q，q)—实际惯性矩阵。

根据实际模型的状态量，设计补偿控制器使得在标称条件下满足：

令状态变量为x1=q、x2=，根据式（15）可得实际状态方程：

将实际状态式（16）与标称模型状态式（9）相减，可得：

选取A2(ε)、B2(ε)作为误差模型中的SDC矩阵，选取优化指标为：

则可设计在线补偿SDRE控制器：

其中P2(ε)满足代数Riccati方程：

4.3 稳定性分析

定理1 由标称控制器式（12）和补偿控制器式（20）组成的闭环控制系统是局部渐进稳定的，即当t→∞时，e→0。

证明定义：

则由性质（1）可知空间机器人惯性矩阵M1为对称正定矩阵，可得出{A1(x)，B1(x)}满足逐点可控条件；同时由于SDC 矩阵A1(x)为离线可选定的标称惯性矩阵、权值矩阵Q1为一组可调整的常值矩阵，因此总能通过选取合适的标称惯性参数及Q1阵，使得{A1(x)，Q112}满足逐点可观条件。由引理2可得，当标称控制器中的SDC矩阵满足上述条件时，可保证标称控制器构成的闭环控制系统的局部渐进稳定性，即当t→∞时，e1→0。将矩阵A2(ε)写出分块矩阵的形式：

式中：D1(ε)、D2(ε)—一个4×4的矩阵。

定义：

式中：C11(ε)—一个4×5的零阵

5 仿真实例

以这里所提自由漂浮双臂空间机器人系统为对象，系统的标称模型参数与真实模型参数，如表1、表2所示。

表1 标称模型参数Tab.1 Nominal Model Parameters

表2 真实模型参数Tab.2 Actual Model Parameters

设双臂空间机器人系统机械臂各关节在关节空间的期望运动规律为：

选取权值矩阵：

考虑系统惯性参数未知情况，使用标称控制律式（12）联立补偿控制律式（20）对机械臂轨迹跟踪运动情况进行仿真，整个追踪过程耗时t=20.0s。自由漂浮双臂空间机器人左、右机械臂各关节轨迹跟踪情况，如图3（a）～图3（b）所示。对应的各关节轨迹跟踪误差，如图3（c）所示。各关节的输出力矩，如图4（a）～图4（b）所示。

图3 空间机器人系统各关节轨迹跟踪情况Fig.3 Trajectory Tracking Situation of Each Arm on Space Robot System

图4 空间机器人系统各关节输出力矩Fig.4 Control Torque of Each Joints on Space Robot System

图3表明，在t=2.5s的时间内，各关节基本能够对期望轨迹进行跟踪，为了进一步验证所设计的两级SDRE控制器对惯性参数未知情况下空间机器人轨迹跟踪的有效性，下面给出不加补偿控制器的轨迹跟踪仿真情况。由图5可以看出，当不加SDRE补偿控制器时，标称SDRE控制器的跟踪误差不收敛，无法实现对轨迹的精确跟踪。

图5 空间机器人系统各关节轨迹跟踪情况Fig.5 Trajectory Tracking Situation of Each Arm on Space Robot System

6 结论

这里将SDRE理论应用于惯性参数未知条件下空间机械臂的轨迹跟踪问题中，设计了一种离线标称SDRE控制器结合在线补偿SDRE控制器的轨迹跟踪优化控制方法，给出了衡量机械臂轨迹跟踪精度及各关节输出力矩能量的具体性能指标函数，所设计的优化控制器能够保证该优化指标最小，符合实际工程需求，通过数值仿真，验证了该方法的有效性。