视觉导引受限下空间绳系机器人最优逼近控制

胡永新，黄攀峰，孟中杰，刘正雄，张夷斋，董刚奇

(1. 西北工业大学航天学院智能机器人研究中心，西安 710072；2. 西北工业大学航天飞行动力学技术国家重点实验室，西安 710072)

0 引 言

空间绳系机器人由空间抓捕器，空间系绳，空间平台三部分构成，抓捕器通过系绳与平台相连，如图1所示，空间绳系机器人具有机动灵活，操作半径大，安全性高等优势[1-2]。空间绳系机器人可用于太空垃圾移除[3-4]，目标星辅助稳定[5-6]等任务，是一种非常具有应用前景的在轨操作工具，同时也是当前在轨服务技术发展的热点方向之一，国内外学者对该空间绳系系统进行了大量研究。

近年来有关空间绳系机器人的研究主要集中在动力学建模，逼近抓捕，抓捕后稳定控制，拖曳移除，以及回收控制等。在动力学建模方面，文献[7]介绍了基于无质量轻质杆模型，珠子模型，牛顿-欧拉方法，Ritz法，混合单元法，Galerkin法，Hamilton力学变分法等的空间系绳建模与求解方法。Yu等[8]针对空间柔性绳系卫星系统释放问题，采用简化弹性杆模型分析了系绳长度释放率，并在频域分析了其稳定性，得到了使绳系系统稳定释放的系绳伸长率的参数区域。Qi等[9]提出一种双系绳空间绳系机器人概念，并对其动力学建模及控制问题进行了研究。Zhang等[10]采用拉格朗日法对空间机器人、目标组合体进行建模，并对其动力学特性进行了分析。

在抓捕后稳定控制方面，William等[11]针对仅利用系绳的空间绳系机器人对目标抓捕后复合体的消旋稳定问题设计控制方法，该方法在系绳长度突然跳变情况下控制性能依然表现良好。文献[12]研究了空间绳系机器人对目标抓捕后复合体姿态接管控制问题，考虑了控制约束以及外部干扰，设计了有限时间终端滑模控制器，仿真结果校验了其控制器的良好性能。Zhai等[13]研究了空间绳系机器人系统中系绳对空间平台干扰下空间平台的姿态稳定控制问题，考虑弹性系绳的摆动，建立了平台姿态动力学方程，并设计了自适应姿态控制器对平台姿态进行控制。

在对目标拖曳移除方面，Chu等[14]针对空间绳系机器人大型空间垃圾移除问题，分析了系绳连接点偏置量的影响，并设计了安全准则来对该空间系绳系统系绳缠绕风险进行分析。Wen等[15]针对空间绳系机器人对目标离轨任务设计了仅有系绳长度反馈的系绳张力控制率，并实现了空间绳系机器人系统的稳定控制。Wang等[16]研究了空间绳系机器人对目标拖曳过程中的姿态稳定控制问题，引入了系绳连接点的移动机构，该机构能够实现2自由度的旋转和1自由度的伸缩，并设计了姿态前驱动抗饱和控制器，实现了抓捕复合体姿态的稳定控制。

空间绳系机器人对目标进行逼近是其进行其它任务的前提，在逼近抓捕任务方面，文献[17]以空间绳系机器人对非合作目标抓捕为背景，提出了一种新的目标定位方法，所采用非合作目标抓捕区域预测模型，大大减小了目标搜索区域。文献[18]针对空间绳系机器人近距离逼近问题设计了一种基于超扭滑膜观测器的动态拟控制方法，实现了空间绳系机器人的近距离位姿跟踪控制。孟中杰等[19]提出一种基于直线跟踪的混合视觉伺服控制方法，该方法能够保证在仅能获得帆板支架边缘线图像信息的情况下，空间绳系机器人稳定到达目标卫星的帆板支架处，并满足捕获条件。Botta等[20]对抓捕器与目标的接触碰撞过程进行研究，建立了接触碰撞动力学模型。黄攀峰等[21]针对空间绳系机器人目标抓捕过程中的稳定控制问题，设计了空间绳系机器人目标抓捕鲁棒自适应稳定控制器，该控制器能对模型中不确定性进行补偿，控制精度较高。Wang等[22]设计了一种协调控制机构通过控制系绳连接点的移动充分利用系绳张力提供姿态控制力矩，其中在该机构作用下，俯仰通道和偏航通道姿态控制均采用系绳张力力矩，滚转通道采用推力器推力力矩，大大节省了燃料消耗。Zhang等[23]针对非合作目标逼近问题提出了一种仅测角的逼近控制方案，该方案包括离线运动规划，模型预测控制和时延控制，仿真结果证明了该控制方案的有效性。

综合以上已有研究可以发现，空间绳系机器人的最大特点是可以利用系绳提供轨道控制力以及姿态控制力矩对空间绳系机器人的位姿进行控制，并且控制过程中系绳一般能够提供远大于其推力器推力的张力，充分利用系绳张力作为控制量不仅能够大大提高位置和姿态部分通道的控制的快速性，而且能够节省燃料消耗。但采用系绳进行控制时也会带来明显的问题，由于系绳张力矢量并不一直通过空间抓捕器的质心，利用系绳张力进行位置控制时，系绳张力会引入姿态干扰力矩，同时，运用系绳张力力矩进行姿态控制时，系绳张力会对抓捕器的轨道运动产生影响。因而考虑这种操作耦合下姿轨一体化最优控制很有必要。

在逼近任务过程中，抓捕器采用自身测量系统提供位姿信息进行逼近，但限于空间绳系机器人的体积和重量，一般仅采用单/双目相机来实现对目标的测量[1]。一般情况下，远距离采用单目相机对目标进行测量，近距离采用双目相机，但无论单目相机还是双目相机，其相机视场角均有限。而任务一般要求在逼近过程中目标一直处于空间抓捕器的相机视场范围。但空间抓捕器的姿态运动以及位置运动均有可能使目标脱离相机视场范围。

另外需指出的是，当逼近目标为非合作目标时，应用单目相机进行位置导航时，仅视线面内面外角是单目相机可以直接测量的量，抓捕器相对目标距离不可直接测量，因而这种相对导航信息不完全的逼近控制问题很有实际研究意义。

通过以上分析可知，空间绳系机器人对目标最优逼近的问题需同时兼顾轨道运动和姿态运动，并且在控制器设计中需考虑导航信息不全的影响。

本文结构如下：第1节采用牛顿-欧拉法建立了空间绳系机器人动力学模型，以及逼近目标相对轨道动力学模型。第2节采用高斯伪谱法进行姿轨最优轨迹一体化规划，并设计了闭环控制率对最优轨迹进行跟踪。第3节进行了仿真校验。第4节对全文进行了总结。

1 空间绳系机器人系统动力学模型

首先，在模型建立过程中采用以下假设：

1)平台质量远大于系绳和抓捕器的质量，且推力器以及系绳张力对平台的影响可以忽略不计。

2)平台在任务期间保持姿态稳定，将平台视为质点忽略其姿态对系绳及抓捕器的影响。

3)逼近过程中，系绳连接点在抓捕器本体系下不移动。

图2 空间绳系机器人对目标逼近Fig.2 The gripper approaching the target

1.1 逼近目标相对轨道动力学模型

(1)

式中:ωvI为抓捕器视线坐标系相对惯性坐标系的旋转角速度。

fg为作用在抓捕器上的空间干扰力矢量，ft为作用在目标上的空间干扰力矢量，Tg为作用在抓捕器上空间系绳张力矢量，Fg为作用在抓捕器上推力器推力矢量，Ft为作用在目标上推力器推力矢量，mt为目标质量，mg为抓捕器质量，μ为地球引力常数，则目标及抓捕器的相对轨道动力学方程为：

(2)

其中，Δggt为抓捕器和目标的引力加速度差。将Δggt投影到抓捕器视线坐标系可得：

(3)

式中:Δgvgt,rvg和rvt分别为Δggt,rg和rt在抓捕器视线坐标系下的投影，且rvg=rvt-l0。

将式(3)在l0=0处展开，并忽略高阶项可得：

(4)

式中：a,n,e及θ分别为目标轨道的长半轴、角速度、偏心率、真近点角。

结合式(1)和式(4)，式(2)在抓捕器视线系下描述为：

(5)

式中:vft，vFt，vfg，vTg，vFg分别为ft,Ft,fg,Tg,Fg在抓捕器视线坐标系下的投影。

1.2 空间系绳无质量弹性杆模型

平台受到系绳张力矢量为-Tg，根据假设可以忽略。抓捕器质心受到系绳张力矢量为Tg，受到自身执行器控制力矢量为Fg。则该部分动力学方程为：

(6)

(7)

(8)

(9)

其中，Δgpgt为Δgpg在系绳坐标系下的投影。

则式(6)在系绳坐标系下的投影为：

(10)

式中：tFg，tfeg，tTg=[-Tl,0,0]T分别为系绳坐标系下抓捕器控制力、环境干扰力、系绳控制力。

由于系绳仅有张紧力没有压紧力，因而：

(11)

式中:E为空间系绳拉伸弹性模量，A为空间系绳横截面积，σ=(l1-ls)/ls,ls为已释放系绳的自然长度，c为空间系绳等效黏性阻尼系数。

1.3 抓捕器姿态动力学模型

任务过程中，抓捕器受到的力矩主要有：抓捕器控制力矩、系绳张力力矩以及环境干扰力矩，有：

(12)

式中：Ib为抓捕器在其本体系下的惯性张量；bτc，bτe分别为抓捕器本体系下抓捕器控制力矩，系绳张力力矩以及环境干扰力矩。Cbt为抓捕器本体系到系绳坐标系的旋转矩阵。

2 逼近目标最优控制

2.1 最优轨迹规划问题描述

空间平台距目标100 m停靠并调整位姿，确保目标在抓捕器相机视场中心时释放抓捕器，抓捕器以一定初速向目标逼近，逼近至距目标一定距离。假设目标不进行自主机动，且其轨道偏心率e=0，忽略环境干扰等影响，研究空间绳系机器人抓捕器从逼近到距目标20 m的逼近最优轨道设计。

1)指标函数

(13)

其中，up=[mgQl2,mgQα,mgQβ]T为轨道控制输入推力器部分，ua=[Qφ,Qθ,Qψ]T为姿态控制力矩，κ1,κ2∈R3×3为系数矩阵，且需根据实际的推力及推力力矩关系进行选择。

2)约束函数

优化问题中约束主要有动力学方程约束，边界条件约束，控制约束以及状态约束。

式(5)、式(10)和式(12)为该问题的动力学方程约束。

令x0为空间绳系机器人系统的初始状态，则：

x(t0)=x0

(14)

(15)

(16)

当γ≤ζ时，目标一直处于抓捕器相机视场范围。显然任务中α0,β0,θ,ψ的幅值均小于0.25ζ时，可以保证γ≤ζ一直成立。因而建立下列状态约束：

(17)

同时需要考虑控制约束

uL≤u≤uR

(18)

其中，u∈R7为控制变量，uL与uR分别为控制变量的下限与上限。

2.2 高斯伪谱法规划模型建立

对于最优轨迹规划问题，求解方法分为间接法和直接法[26]。间接法精度高但对初值估计困难。直接法收敛域宽，对初值估计要求不高。高斯伪谱法作为一种求解最优化问题的直接法，具有较高求解精度，在一系列最优控制问题中得到采用。文献[27]采用高斯伪谱法对失效航天器超近距逼近问题规划最优避免碰撞逼近轨迹。文献[28]将高斯伪谱法应用于液体运载火箭抛罩结束到入轨飞行段的制导方法中得到了满意的结果。文献[29]将高斯伪谱法应用于多UAV协同航迹规划，获得了满足任务要求的可行飞行轨迹。本文采用高斯伪谱法对空间绳系机器人最优逼近问题进行求解。利用高斯伪谱法将最优轨迹规划问题离散化并转化为NLP问题。

Gauss伪谱法通过时域变换、Lagrange多项式差值将时间t∈[t0,tf]上的最优控制问题转化为τ∈[-1,1]上的非线性规划问题。

(19)

其中，τ1～τK为Legendre-Gauss多项式的K个零点，X为离散状态变量，U为离散输入变量，Dki为微分矩阵，且满足如下关系：

(20)

末端状态约束可以转化为：

(21)

式中:ωk为第k处的高斯积分系数。

目标函数可以转化为：

(22)

状态变量和控制变量在各离散点处的约束为：

(23)

2.3 闭环控制器设计

在空间绳系机器人系统抓捕机构在远距离逼近时仅采用单目相机作为相对位置测量装备，但单目相机仅能够直接测量目标方位角信息。因而本文所设计控制器利用目标方位角，系绳长度以及绝对姿态角的测量信息实现空间绳系机器人的位姿跟踪控制。所设计控制器结构如图3所示。

图3 闭环最优控制系统框架Fig.3 The closed-loop optimal control system

3 仿真校验

3.1 仿真条件

空间绳系机器人的基本参数如下：空间绳系机器人质量36 kg，惯量：Ix=0.6 kg·m2,Iy=2.1 kg·m2,Iz=2.2 kg·m2；连接点在抓捕器本体系下位置为[-0.25 m，0，0]T；轨控推力器推力幅值2 N，姿控推力器推力力矩为0.1 Nm，抓捕器控制力矩幅值为0.1 Nm；抓捕器单目相机视场角为0.4 rad；系绳杨氏模量130 Gpa，系绳直径1 mm，等效黏性阻尼系数为0.1，系绳张力范围为0.02 N～10 N；为保证系绳一直处于张紧状态，令系绳一直有张力且张力最小为0.02 N。抓捕器脱离平台后，向目标进行逼近，实现对目标从100 m到20 m的逼近。

表1 仿真关键参数列表Table 1 Key simulation parameters

3.2 仿真算例一

在MATLAB环境下对高斯伪谱法规划模型进行求解得到理想控制输入和理想状态轨迹，将理想控制输入进行样条差值，并将差值得到的结果作为实际控制量，得到仿真结果如图4～图10所示。其中图4～图6为控制输入曲线，可以看出所规划控制输入均满足控制变量上下限约束。在轨道控制方面，x轴方向上的减速主要由系绳张力实现，节省了推力器燃料消耗。u2～u7均得到的很好的拟合，但u1拟合效果并不理想，在13 s～18 s以及95 s～100 s期间实际输入与期望之间存在有较大的偏差。

图7为惯性系下抓捕器相对目标运动轨迹，可以看出实际轨迹逐渐偏离期望轨迹。图8为抓捕器绝对姿态变化曲线，可以看出绝对姿态在三个通道跟踪效果均不理想，特别是姿态角θ在任务期间最大值为0.2322 rad，大于规划中设置上限0.1 rad。从图9可以看出，系绳张力力矩存在较大偏差，该偏差直接导致姿态跟踪偏差。可见系绳控制输入的拟合偏差对逼近过程具有较大影响，特别是系绳控制输入拟合偏差不仅对轨道跟踪产生影响，其产生的系绳力矩偏差会对姿态轨迹跟踪产生较大影响。

图4 系绳控制力Fig.4 The control force of tether

图5 轨道控制中推力器控制力部分Fig.5 The control force of thrusters

图6 姿态控制力矩Fig.6 The control torques

图7 惯性坐标系下抓捕器与目标之间相对运动轨迹Fig.7 The trajectory of the gripper relative to target with respect to oIxIyIzI

任务期间抓捕器视线角如图10所示，可以看出在整个任务过程中视线角均小于0.22 rad，目标全程均在抓捕器相机的视场范围内，满足任务要求，但可以看出视线角在52 s左右达到峰值，这是由姿态角θ和ψ在52 s左右达到极值导致。最终抓捕器与目标的相对位置为[20.77 m,0.08 m,0.04 m]，与所规划的期望最终位置之间的偏差为[0.77 m, 0.08 m,0.04 m]，最终姿态角与期望姿态角的偏差为[0.024 rad,0.2 rad,-0.04 rad],可见采用该开环控制方案，轨迹跟踪存在较大末端控制偏差。

图8 抓捕器绝对姿态变化曲线Fig.8 The attitude angle of the gripper

图9 系绳张力力矩Fig.9 Torques produced by the tether

图10 抓捕器视线角变化曲线Fig.10 The sight-line angle of the gripper

3.3 仿真算例二

对第2.3节所设计的闭环控制框架进行了仿真校验，仿真结果如图11～图16所示。其中图11～图13为控制输入曲线，可以看出控制输入均满足幅值约束条件，控制输入u1,u2,u6的轨迹与期望轨迹存在较大偏差，其中u1的偏差和仿真算例一中相同，主要为拟合偏差。u2和u6的偏差为PD控制器产生的控制量对状态偏差进行补偿控制。

图14为惯性坐标系下抓捕器与目标之间相对运动的轨迹。图15为抓捕器绝对姿态角变化曲线，可以看出实际轨迹与期望轨迹重合度较高。最终抓捕器与目标的相对位置为：[19.99 m, 3.8×10-4m, -1.4×10-3m]，与所规划的期望最终位置之间的偏差为[-0.01 m, 3.8×10-4m, -1.4×10-3m]，最终姿态角与期望姿态角的偏差为[-1×10-6rad, 2×10-3rad, 5×10-4rad]。从图16可以看出，在整个任务中抓捕器视线角变化轨迹与期望轨迹基本一致且均小于0.18 rad，满足相机视场角约束。

图11 系绳控制力Fig.11 The control force of the tether

图12 轨道控制中推力器控制力部分Fig.12 The control force of thrusters

图13 姿态控制力矩Fig.13 The control torques

图14 惯性坐标系下抓捕器与目标之间相对运动轨迹Fig.14 The trajectory of the gripper relative to target with respect to oIxIyIzI

图15 抓捕器绝对姿态变化曲线Fig.15 The attitude angle of the gripper

图16 抓捕器视线角变化曲线Fig.16 The sight-line angle of the gripper

4 结 论

本文针对空间绳系机器人对非合作目标最优逼近问题进行研究。考虑抓捕器视线角约束，对空间绳系机器人姿轨最优轨迹一体规划，并设计了闭环最优控制系统对规划轨迹进行跟踪控制，该方案中位置跟踪仅需目标方位角信息和系绳长度信息，克服了单目相机距离信息测量不足的缺点，实现了无需测量抓捕器与目标相对距离的相对位置逼近。仿真结果表明,该闭环控制系统能够保证目标在整个任务期间一直处于抓捕器相机视场范围内，且具有较高的末端控制精度。需要指出的是：在本文的研究中，系绳弹性杆模型较为简化，忽略了系绳质量的影响以及空间平台位姿机动控制的影响，下一步研究工作将把系绳珠点模型以及平台位姿机动控制考虑进绳系机器人系统模型中，分析其对绳系机器人逼近任务的影响。