基于避障伪距离的自由漂浮空间机器人规划-跟踪一体化控制

羊 帆，张国良，田 琦，王保明

(1. 火箭军工程大学，西安 710025； 2. 宝鸡市高新技术研究所，宝鸡 721000; 3. 成都信息工程大学，成都 610225)

0 引 言

伴随着人类探索太空进程的加快，利用空间机器人代替人类完成相关在轨服务任务，成为目前空间技术研究的方向之一。自由漂浮空间机器人(Free-Floating Space Robot, FFSR)由于其不需消耗额外能量稳定基座位姿，具有较好的长期在轨服务能力，成为目前空间机器人研究的主要对象[1-2]。

不同于地面机械臂系统， FFSR特有的漂浮基座特性使其规划和控制问题更为复杂。为此，一些学者开展了相关研究并取得了丰硕的研究成果。在控制问题的研究上，文献[3]在对FFSR运动学模型分析的基础上，提出了分解速度和加速度控制方法。文献[4]在FFSR动力学建模的基础上，利用PD控制实现了FFSR的计算力矩控制。文献[5]利用FFSR扩展动力学模型提出了规范形式扩展自适应控制方法方法。文献[6]利用积分降阶技术，将FFSR动力学方程进行低通滤波，改进了文献[5]所提方法，从而避免加速度信号的测量。文献[7]研究了同时存在运动学和动力学参数不确定情况的自适应控制方法。文献[8]以杆件最近距离为约束条件，基于NMPC理论提出了FFSR的避障控制方法。此外，一些智能控制方法如神经网络[9]、确定学习[10-11]、模糊控制[12]被研究者采用以实现FFSR的轨迹跟踪控制。另一方面，针对FFSR的非完整约束以及存在动力学奇异问题，其轨迹规划方法被学者所重视，提出了圆周运动方法[13]，双向规划法[14]，增强干扰图[15]等经典方法。在这些方法基础上，文献[16]利用PSO优化实现了FFSR在抓取后基座姿态的稳定，进一步在文献[17]提出了FFSR的自主路径规划方法。文献[18]提出了笛卡尔空间下FFSR点到点运动的规划方法。文献[19]提出了一种最小姿态干扰的目标接触轨迹规划方法。文献[20]基于人工势场法，对地面机械臂避障规划方法进行扩展用于FFSR的末端避障规划。文献[21]提出了一种针对滚动目标的避碰规划方法，实现了对滚动目标的末端同步接近规划。文献[22]将多项式插值与PSO方法相结合提出了一种FFSR关节空间下的非完整路径规划方法，从而保证关节轨迹的连续平滑。

虽然上述规划和控制方法的研究初步解决了FFSR运动控制问题，但是依然存在一些不足。首先，在规划方法上，多考虑在非完整约束下，FFSR的机械臂运动与基座姿态的相互关系以及动力学奇异回避问题，未考虑FFSR运动过程中的避障问题。其次，在运动控制上，基本遵循离线规划—轨迹跟踪思想，但是由于传感器探测距离，目标移动以及可能存在的避障需求，在工程上这一思路无法有效实现FFSR的运动控制。最后，少数如文献[8]提出的避障控制方法，也存在避障约束构造困难等问题。

为此，针对FFSR的避障规划与控制问题，本文提出一种FFSR的避障规划-跟踪一体化控制方法。首先，基于障碍物伪距离技术[23]，采用FFSR逆几何模型求解期望末端位姿下的连杆伪距离估计值构造避障约束条件，进而通过求解非线性优化问题，获得FFSR避障期望轨迹。其次，将全局轨迹规划与局部在线避障相结合，辅以离散状态依赖里卡提方程(Discrete State Dependent Riccati Equation DSDRE)控制方法实现FFSR的避障规划-跟踪一体化控制，以克服传统离线规划在跟踪所存在的对动态障碍，未知障碍避障能力不足等问题。

1 模型与假设

1.1 空间机器人模型

1)FFSR的运动学与动力学模型

FFSR一般由基座和机械臂组成，由于基座处于漂浮状态，系统无外力输入，满足动量守恒，基座运动与机械臂运动相耦合[3-4]。

典型的空间机器人运动学和动力学模型为

(1)

FFSR在轨工作时，由于其姿态控制系统关闭，工作过程中无外力输入，由动量守恒原理，系统(1)还满足：

(2)

其中，Μ0表示系统的初始动量，包括初始线动量和初始角动量。

2)FFSR几何模型

考虑如图1所示的空间机器人一般模型，由n个连杆机械臂及作为其基座的航天器平台共n+1个刚体组成。其中，刚体B0代表空间机器人基座，刚体Bi(i=1,…,n)按照从与基座相连的第一个刚体开始向机械臂末端进行编号，依次代表机械臂的第i个连杆。连杆i-1与连杆i之间通过关节Ji相连接，第i个连杆质心记为Ci，基座质心记为C0。

则FFSR末端位姿在惯性系下可表示为：

ye=IA00A11A2…n-1AnΣE=
h(q1,q2,…,qn,qb)

(3)

其中，iAi+1为坐标系Σi+1到坐标系Σi的位姿转移矩阵；IA0为基座坐标系统到惯性系的坐标转移矩阵；ye为惯性系下末端位姿。

则式(3)为FFSR的几何模型，进一步可得FFSR逆几何模型

qm=h*(qb,ye)

一般而言，上式求解很复杂甚至无法求解，通常在已知末端与基座位姿情况采用迭代求解式(3),获得qm。

1.2 障碍建模

文献[23]提出了障碍物的伪距离建模方法，该方法将空间中的障碍规则化为空间中的椭球、椎体、圆柱、方体等几何体，进而通过求解规则化障碍物表面方程函数值作为确定机械臂与障碍物之间关系的依据。

由于椭圆，椭球具有较好的逼近真实障碍常常作为障碍描述的基本形状。若利用空间位置坐标x,y,z的标量方程替代椭球方程中的半轴参数a,b,c，则可在椭球方程基础上表征复杂障碍的表面方程，障碍物表面一般方程具有如下形式：

(4)

1)空间中点与障碍物关系

将空间中任意一点∀X0∈R3代入障碍物表面方程(4)中可得S(X0)，则点X0与障碍关系可由S(X0)函数值表示为：

若S(X0)=0，则点X0位于障碍物表面；

若S(X0)>0，则点X0位于障碍物外；

若S(X0) < 0，则点X0位于障碍物内；

且有S(X0)越大点与障碍物实际距离越远，故可将S(X0)称为点X0与障碍物的伪距离。

2)连杆与障碍物关系

如图2所示，t时刻考虑连杆Ji,Ji+1上点Xm可表示为如下关系：

Xm=ri+α(ri+1-ri)

(5)

其中，rm,ri,ri+1∈R3,为XmJi,Ji+1相对于参考系的位置矢量，标量α∈[0,1]为点位置参数。

考虑将障碍物ob规则化为一椭球体，由公式(4)，可得点Xm与障碍物之间的伪距离为：

(6)

其中，矩阵Qm,向量Bm，标量Cm为由椭球表面方程中半轴参数a,b,c导出。

由公式(5)、(6)并有U=ri-ri+1可得：

H(λ)= (ri+αU)TQm(ri+αU)+

(7)

则连杆Ji,Ji+1与障碍物ob之间的最短距离满足

(8)

进一步可获得连杆避障最小距离点位置参数：

(9)

1.3 问题描述及假设

本文一体化控制器设计主要目的是通过设计FFSR末端轨迹规划-跟踪控制器，实现FFSR由起点至目标的避障轨迹控制。

为便于问题研究设计且不失一般性，提出如下假设：

1)FFSR属于非固定基座机械臂系统，为便于规划器设计，假设在两次很短的采样间隔内基座位置姿态变化很小。即忽略两次采样间的基座位姿变化。

注1文献[4]中指出对于FFSR而言，基座相对于机械臂的质量越大，FFSR的运动学特性越接近地面固定机械臂，实际中通常基座本体质量远大于机械臂连杆质量，因此在很短的采样间隔内上述假设并不失一般性。

4)系统均为刚体，忽略微重力影响。

2 伪距离求解和避障约束

为实现FFSR的避障规划，在本文中基于障碍物的伪距离模型，采用FFSR逆几何模型(IGM)求解FFSR的连杆最近伪距离估计，进而获得FFSR的避障约束条件。

2.1 基于IGM的连杆最近伪距离估计

在传统的6DOF固定基座机械臂系统中，若末端位姿确定，可通过迭代求解机械臂的IGM获得关节角运动的唯一解。由于FFSR特有的漂浮基座特性，使得该方法受到一定限制，由文献[4]可知，若FFSR基座相对于机械臂连杆质量越大其动力学特性越接近于固定机械臂系统。为此，本文在获得基座位姿反馈情况下，结合假设1条件，在一定误差允许范围内通过IGM求解连杆伪距离估计。

由假设1，可得t=kT时，期望关节角

qmr(k)=h*(qbk,yer(k))

(10)

其中，h*(qb,xe)为FFSR的IGM。

则迭代求解式(10)以及FFSR几何模型可求得t=kT时，第i关节期望位姿

怎样捕捉人物的“神”；各种灯光的打法；氛围营造；强大的后期处理；摄影美学理论；市场现状和流行趋势；现场拍摄及创作指导；犀利点评；获奖经验；等等。

(11)

其中，0hi(qb,qm)为惯性系中第i关节相对于惯性系的正向几何模型。

由式(6)，(9)，(11)可得，t=kT期望末端位姿的连杆最近伪距离估计为

(12)

其中，g(qbk,yer(k))为由式(11)，(9)推得的最近伪距离计算函数。

2.2 避障约束

为实现FFSR末端位姿运动过程中的避障，在本节中定义两个伪距离

定义1：FFSR障碍物安全伪距离，Duf=[duf1,duf2…dufn]T。

当连杆伪距离D≤Duf时，认为FFSR发生碰撞。

定义2：FFSR避障处理伪距离，Dif=[dif1,dif2…difn]T。

(本文中D≤Duf(if)表示D中每一个元素均不大于Duf(if)中每一个元素。)

则由定义1、定义2可知FFSR的末端及连杆运动可以进入由Dif构成的区域，但不能进入由Duf构成的区域。为了在末端轨迹规划器中引入避障约束，引入下述罚函数：

p(D)=[p1(d1),p2(d2)…pn(dn)]T

(13)

其中,

(14)

则，式(13)、(14)构成了避障规划的约束函数。

注2.式(13)、(14)说明当各连杆的pi(di)越小，则FFSR距离障碍物越远，反之越接近障碍，故避障问题求解中，只需满足各连杆pi(di)极小即可。

3 规划-跟踪控制器设计

本文中规划-跟踪控制器设计分为规划器和控制器两部分。首先根据目标点位姿通过规划器产生FFSR的末端避障轨迹规划，然后将规划轨迹结果引入FFSR的末端跟踪控制器中，实现FFSR的控制。最后基于FFSR相关运动反馈信息，计算伪距离，产生避障约束修正规划轨迹。

3.1 规划器设计

3.1.1全局规划器设计

若FFSR处于非避障工作空间，则规划当前点与终点之间的末端运动轨迹，可采用5次多项式(15)规划

(15)

满足如下约束条件：

(16)

则根据式(15)、(16)将全局轨迹规划问题，转化为式(17)所示的非线性寻优问题。

minf(x)=tf,x=[A0,A1,…A5,tf]T

s.t.

(17)

其中Ai为多项式系数矩阵。

求解式(17)的非线性优化问题，即可获得全局规划的轨迹的5次多项式系数，进而获得全局规划轨迹。则全局规划器为

(18)

3.1.2避障规划器设计

考虑tk时，FFSR任意连杆或末端伪距离di≤difi，则表明FFSR进入障碍影响区，原全局规划轨迹可能会造成FFSR的碰撞，因此需要进行避障轨迹规划重新确定yer(tk)使得FFSR既有一定的避障能力，同时又保证FFSR末端位姿向目标运动。

考虑yer(tk+1)与yer(tk)存在如下关系

(19)

其中，T=tk+1-tk为采样间隔。

由式(13)、(14)及(19)，可确定避障规划目标函数为

(20)

为保证控制力矩的连续性，以及使FFSR具有一定的避障能力，在式(20)目标函数的基础上给出相应的求解约束条件,

1)加速度约束

由式(20)可知加速度a的搜索范围一方面决定了FFSR避障规划的可行解空间,另一方面也影响着进入障碍影响区域后FFSR进行避障运动的控制力矩大小。若a的搜索范围较大，则FFSR可以获得更大的避障能力，相应的控制力矩也较大。反之，若a选择较小的搜索范围，则FFSR的控制力矩更光滑，其避障能力相对减弱。为此，给定如下加速度约束。

(21)

注4.通过调整kl,kA的值可以实现加速度搜索空间的调整，kl,kA可根据FFSR控制力矩输出限制、Dif和Duf所确定的障碍区域大小以及FFSR运动时间综合考虑。

2)锥形搜索区域约束

FFSR的期望避障规划轨迹，不仅应使FFSR具有一定的避障能力，同时又要保证其趋向于目标位姿运动。虽然在式(20)的目标函数中通过权系数γ,βi调整可以在一定程度上满足要求，为缩短FFSR末端运动时间,在加速度约束的基础上增加锥形搜索约束。

(22)

至此，则根据式(20)、(21)以及(22)将避障规划问题，转化为如下非线性优化问题。

s.t.

(23)

3.2 控制器设计

由FFSR运动学和动力学模型，可得

(24)

则采用欧拉法对式(24)，进行离散化可得：

(25)

其中Ad=TA+I，Bd=TB，Cd=C，xk=x(kT)，I为单位阵，T为采样间隔。

由SDRE相关理论可知，当J*可逆时，Ad,Bd,Cd为系统(24)的状态依赖系数(State Dependent Coefficient, SDC)矩阵[24]。

为设计DSDRE优化跟踪控制器，引入滑膜变量z。

定义3：滑膜变量z满足

(26)

其中，ek表示跟踪误差。λ为滑膜增益，其满足λ=diag(λ1,λ2,…,λn),λi>0。

由式(24)，(26),系统输出方程可写为

(27)

则由式(27)可重写系统(25)的期望跟踪轨迹如下：

(28)

设计目标函数为

(29)

则由DSDRE方法可得控制律为

(30)

注5：关于Pk+1，sk的求解，根据DSDRE理论可按照无限时间调节器理论方法求解上述黎卡提方程，此时认为Pk+1=Pk,sk+1=sk。由式(25)表明控制律(30)使得limt→∞z→0，则FFSR末端运动轨迹能够实现对期望末端轨迹的跟踪。Q,R为适当维数的正定矩阵。

定理1. 对于具有不确定参数的FFSR系统(25)，满足假设1)～4)及相应的SDC条件，则存在控制律(30)使得FFSR末端运动轨迹渐进稳定跟踪期望轨迹。

证. 由系统(25)及控制律(30)，系统闭环方程可写为

(31)

闭环方程(31)包含反馈项与前馈项，由于前馈项不影响Pk的求解，闭环系统稳定性仅与反馈项相关，故可得系统闭环反馈方程为：

xk+1=Adkxk-AdkFkxk=Adk(I-Fk)xk

(32)

构造Lyapunov 函数

其中V(xk)>0,∀xk≠0并且有xk=0，V(xk)=0。则有

Pk+1FkAdk]xk。

由于R>0，故可得V(xk+1)-V(xk)≤0。表明系统在标称控制律作用下可实现闭环反馈系统的渐进稳定。

注6.定理1成立需要系统(1)满足SDC条件，即保证FFSR的广义雅克比矩阵非奇异。为此，本文将奇异点等效为一个障碍，计算跟踪过程中广义雅克比矩阵的最小奇异值作为等效障碍的伪距离。通过选取合适等效避障处理伪距离，采用在线避障规划方法实现回避奇异。

3.3 规划-跟踪一体化控制算法

限于传感器观测范围等因素，采用传统离线规划再跟踪控制方法无法有效实现FFSR的避障运动控制，为此本文将避障规划和优化跟踪相结合提出FFSR规划-跟踪一体化控制算法，流程如图3所示。

为了进一步说明FFSR的规划-跟踪一体化控制方法，给出算法步骤：

5)利用式(26)求解优化跟踪控制律，控制FFSR运动。

6)是否到达目标位姿。若到达目标位姿停止，反之转步骤7)。

7)k+1→k。转步骤2)。

4 数值仿真

以文献[25]所提的6R空间机器人为例，验证所提方法的有效性。机器人结构及参数与文献[25]中相同。

给定系统初始与终止末端位姿分别为：

y0=[1.5950,-0.0095,-0.2166,0,-1.3962]T

yf=[1.0,0.0520,0,1,-1.3963,0]T

给定系统初始末端速度为0，基座初始位姿为0；关节角初始值为：qm0=[0,0,-1.04 rad,0,-0.34 rad,0]T

假定障碍中心坐标为

PO=[1.3205m,0.06m,0.1195m]T;

设定障碍物安全伪距离为Duf=[1,1,1,1,1,1]T设定避障处理伪距离为Dif=[3,3,3,3,3,3]T

控制器选取权值矩阵：

仿真采用MATLAB Spacedyn工具箱[26]，仿真时间为25 s，结果如图4～8所示。图4为控制力矩;图5为末端轨迹跟踪误差;图6为机器人跟踪过程中连杆运动示意图;图7为腕部和末端的伪距离变化;图8为基座的位姿运动情况。

从图5中可以看出一体化控制方法能够实现末端轨迹对期望轨迹的稳定跟踪，且能够满足跟踪精度。若采用下式计算末端位姿控制精度，则etrack≤1×10-3。

其中，yef为实际终点末端位姿，yf为期望终点末端位姿。

图4中可以明显看出在4 s和8 s左右控制力矩发生了两次振荡，对比图7可以发现FFSR在3.4 s左右其腕部进入了障碍影响范围，在8.8 s左右末端进入障碍影响范围。这反映出由于进行在线避障规划使控制力矩发生了较强烈的变化，从而控制FFSR运动进行避障。从图7可看到在短时间内FFSR离开了障碍区域，实现了避障。

图6的连杆示意图中整体反映了FFSR避障的杆件运动过程。图8中的基座位姿变化可以看出，由于基座质量相对较大， FFSR杆件运动导致基座位姿运动缓慢，基座位置变化较小。

5 结 论

针对FFSR的避障规划与控制问题，本文提出一种基于避障伪距离的FFSR的规划-跟踪一体化控制方法。将FFSR的在线避障规划与优化跟踪控制相结合，解决了FFSR点到点的避障规划与控制问题。

[1] Flores-Abad A, Ma O, Pham K, et al. A review of space robotics technologies for on-orbit servicing[J]. Progress in Aerospace Sciences, 2014,68(8): 1-26.

[2] Xu W, Liang B, Xu Y. Survey of modeling, planning, and ground verification of space robotic systems[J]. Acta Astronautica, 2011,68(11-12): 1629-1649.

[3] Umetani Y, Yoshida K. Resolved motion rate control of space manipulators with generalized Jacobian matrix[J]. IEEE Transactions on Robotics & Automation, 2002,5(3): 303-314.

[4] Dubowsky S, Papadopoulos E. The kinematics, dynamics, and control of free-flying and free-floating space robotic systems[J]. Robotics & Automation IEEE Transactions 1993,9(5): 531-543.

[5] Gu YL, Xu Y. A normal form augmentation approach to adaptive control of space robot systems[J]. Dynamics & Control, 1995,5(3): 275-294.

[6] Parlaktuna O, Ozkan M. Adaptive control of free-floating space robots in Cartesian coordinates[J]. Advanced Robotics, 2004,18(9): 943-959.

[7] Wang H, Xie Y. Passivity based adaptive Jacobian tracking for free-floating space manipulators without using spacecraft acceleration[J]. Automatica, 2009,45(6): 1510-1517.

[8] Wang M, Luo J, Walter U. A non-linear model predictive controller with obstacle avoidance for a space robot[J]. Advances in Space Research, 2016,57(8): 1737-1746.

[9] 史也. 空间机器人自主捕获目标的轨迹规划与控制研究[D]: 哈尔滨工业大学, 2013. [Shi Ye. Research on trajectory planning and control of Space Robot for autonomous capturing target[D].Harbin： Institute of Technology, 2013]

[10] 吴玉香, 王聪. 基于确定学习的机器人任务空间自适应神经网络控制[J]. 自动化学报, 2013,39(6): 806-815. [Wu Yu-Xiang, Wang Cong. Deterministic learning based adaptive network control of robot in task space[J]. Acta Automatica Sinica, 2013, 39(6): 806-815.]

[11] 杨飞飞, 王聪, 曾玮. 空间机械臂的确定性学习控制[J]. 宇航学报, 2015,(07): 811-818.[Yang Fei-fei, Wang Cong, Zeng Wei. Learning from Adaptive Control of a Class of Space Manipulator Systems[J]. Journal of Astronautics, 2015,(07): 811-818.]

[12] 张福海, 付宜利, 王树国. 惯性参数不确定的自由漂浮空间机器人自适应控制研究[J]. 航空学报, 2012,33(12): 2347-2354. [Zhang Fu-hai, Fu Yi-li, Wang Shu-guo. Adaptive control of free-floating space robot with inertia parameter uncertainties. Acta Aeronautica et Astronautica Sinica, 2012, 33(12):2347-2354.]

[13] Vafa Z, Dubowsky S. On the dynamics of space manipulators using the virtual manipulator, with applications to path planning[J]. 1993,38(4): 45-76.

[14] Nakamura Y, Mukherjee R. Nonholonomic path planning of space robots via a bidirectional approach[J]. IEEE Transactions on Robotics & Automation, 2002,7(4): 500-514.

[15] Dubowsky S, Torres M A. Path planning for space manipulators to minimize spacecraft attitude disturbances[C]. Proceedings of the IEEE International Conference on Robotics and Automation, Sacramento,CA,1991.

[16] Xu W, Li C, Liang B, et al. Target berthing and base reorientation of free-floating space robotic system after capturing[J]. Acta Astronautica, 2009,64(2): 109-126.

[17] Xu W, Liang B, Li C, et al. Autonomous target capturing of free-floating space robot: theory and experiments[J]. Robotica, 2009,27(3): 425-445.

[18] Tortopidis I, Papadopoulos E. On point-to-point motion planning for underactuated space manipulator systems[J]. Robotics & Autonomous Systems, 2007,55(2): 122-131.

[19] Zhang L, Jia Q, Chen G, et al. Pre-impact trajectory planning for minimizing base attitude disturbance in space manipulator systems for a capture task[J]. Chinese Journal of Aeronautics, 2015,28(4): 1199-1208.

[20] Liu S, Zhang Q, Zhou D. Obstacle avoidance path planning of space manipulator based on improved artificial potential field method[J]. Journal of the Institution of Engineers, 2014,95(1): 31-39.

[21] Luo J, Zong L, Wang M, et al. Optimal capture occasion determination and trajectory generation for space robots grasping tumbling objects[J]. Acta Astronautica, 2017,136: 380-386.

[22] 石忠, 王永智, 胡庆雷. 基于多项式插值的自由漂浮空间机器人轨迹规划粒子群优化算法[J]. 宇航学报, 2011,32(7): 1516-1521.[Shi Zhong, Wang Yong-zhi, Hu Qing-lei. A polynomial interpolation based particle swarm optimization algorithm for trajectory planning of free-floating space robot[J]. Journal of Astronautics, 2011,32(7): 1516-1521.]

[23] Perdereau V, Passi C, Drouin M. Real-time control of redundant robotic manipulators for mobile obstacle avoidance[J]. Robotics & Autonomous Systems, 2002,41(1): 41-59.

[24] Çimen T. State-dependent riccati equation (SDRE) control: A survey[J]. Ifac Proceedings Volumes, 2008,41(2): 3761-3775.

[25] 徐文福. 空间机器人目标捕获的路径规划与实验研究[D].哈尔滨: 哈尔滨工业大学, 2007. [ Xu Wen-fu, Path planning and experiment study of space robot for target capturing[D].Harbin: Harbin Institute of Technology, 2007.]

[26] Yoshida K. The SpaceDyn : A MATLA toolbox for space and mobile robots[J]. Journal of the Society of Instrument & Control Engineers, 1999,38(7): 138-143.