自由漂浮空间机器人点到点避奇异运动控制方法

2018-09-29羊帆张国良张合新宋海涛

航空学报 2018年9期

羊帆，张国良，张合新，宋海涛

1. 火箭军工程大学导弹工程学院，西安 710025 2. 成都信息工程大学控制工程学院，成都 610225

随着人类空间活动的日益增多，以及空间在轨服务任务的需求，研究利用智能机器人执行复杂、危险的空间任务成为了当前空间技术研究的重要方面，因具有较好的长期在轨服务能力，自由漂浮空间机器人(Free Floating Space Robot, FFSR)日益成为空间机器人研究的重要对象[1-2]。

FFSR是典型的非完整多体动力学系统，其控制与规划问题一直是研究者关心的主要内容。其中，Umetani和Yoshida[3]对FFSR的运动学进行了相关研究，提出了广义雅克比矩阵(Generalized Jacobian Matrix, GJM)概念，实现了FFSR的分解速度、加速度控制。Dubowsky和Papadopoulos[4]开展了FFSR的动力学建模问题研究，基于PD控制提出了FFSR的计算力矩控制方法。Gu和Xu[5]在FFSR标准动力学模型基础上，将基座运动引入动力学方程，形成了FFSR的扩展动力学方程，并在此基础上提出了规范形式扩展自适应控制方法。但是，该方法需要系统的加速度信息。为此， Parlaktuna和Ozkan[6]、Wang[7]采用低通滤波器对系统进行积分降阶实现FFSR末端运动的自适应控制。Wang等[8]基于NMPC理论研究FFSR的避障跟踪控制方法。此外，如神经网络[9]、确定学习[10]、模糊控制[11]等智能控制方法亦被研究者用以实现FFSR的轨迹跟踪控制。至此，通过学者的大量工作，无论是针对关节空间还是任务空间的FFSR跟踪控制问题得到了基本解决。但是，也存在一些问题。具体来讲，由于非完整约束存在，类似于地面机械臂直接控制关节轨迹从而实现末端位姿的期望运动存在一定的困难即关节空间轨迹不能直接向任务空间映射。若在任务空间下直接跟踪末端期望轨迹，不可避免地存在奇异问题。徐文福[12]在其博士论文中分析指出当FFSR奇异发生时，将丧失末端位姿在某个或某几个方向上的控制自由度(无论如何施加控制力矩均无法完成某个方向的运动)。因此，上述任务空间中FFSR末端轨迹控制方法多假设控制过程中不存在奇异问题，以保证控制力矩不发生突变。

为解决FFSR的奇异问题，一些学者从规划角度出发开展了FFSR的避奇异规划研究。夏进军等[13]基于奇异转化思想，将FFSR的避动力学奇异问题转化为避虚拟机械臂的运动学奇异方法，实现了FFSR避奇异规划。Nanos和Papadopoulos[14-15]通过调整机械臂初始构型方法，首先实现了平面2自由度FFSR动力学奇异回避，其后将此方法扩展到多自由度FFSR动力学奇异回避中。张福海等[16]将可操作度变化作为依据，预测系统运动过程中的奇异位形并提出一种能构造无奇异运动轨迹的任务重构法。徐文福等[17]针对FFSR动力学与运动学耦合问题进行了混合建模与分析，提出姿态自由耦合空间、自由耦合空间以及耦合度等概念，并结合建模分析过程研究了FFSR非完整路径规划问题与动力学奇异回避问题。进一步，通过对奇异条件的分离，采用阻尼倒数代替导致矩阵奇异的普通倒数，从而消除奇异对关节角速度产生的影响[18]。上述方法，通过采用避奇异规划方法解决了FFSR在控制过程中的奇异问题。但是上述方法多考虑非冗余FFSR系统的奇异回避规划问题，利用广义逆(奇异倒数)代替GJM逆进行关节角规划时牺牲了FFSR在末端的规划精度，而采用调整初始位姿方法亦存在任务执行效率较低问题。为此，Wang等[19]基于贝塞尔曲线利用差分进化算法实现了7自由度(Degree of Freedom, DoF)的FFSR关节空间奇异回避规划。此外，Xu等[20]通过任务分解方法，采用增加位置平衡臂以及姿态平衡臂等方法利用冗余关节运动实现FFSR的奇异回避与基座位姿稳定规划与控制，取得较好的FFSR运动控制效果。虽然，当前关于FFSR奇异回避规划与控制问题相对成熟，但是也存在一些问题。采用阻尼倒数或广义逆实现奇异GJM代替法本质上牺牲了目标控制精度以实现奇异回避，采用初始位姿调整亦存在规划轨迹不连续等问题。文献[19]和文献[20]为冗余FFSR的奇异回避规划与控制提供了很好的思路但也存在计算分析过程复杂或者FFSR物理结构组成复杂等问题。

为了有效解决冗余FFSR系统点到点的避奇异规划与跟踪控制问题。本文提出一种冗余FFSR末端点到点避奇异运动控制方法。该方法首先基于离散状态依赖李卡提方程(Discrete State Dependence Riccati Equation, DSDRE)控制器设计方法，利用FFSR的动力学和运动学方程实现了FFSR系统方程的伪线性重构。然后，在重构系统的基础上利用DSDRE状态调节器设计方法实现了FFSR的关节角速度和末端位姿的同时跟踪控制。其次，根据控制器设计需求提出FFSR的避奇异约束函数，进而结合关节角约束实现FFSR在线避奇异规划器设计，最后将控制方法与规划方法相结合实现了冗余FFSR的点到点的避奇异运动控制。其整体结构框图如图1所示。图中：yf为期望末端目标位姿；τm为控制力矩；xdes为期望状态变量；ye为末端位姿；qb为基座的位姿向量；qm为关节角向量；sk为GJM的最小奇异值。

1 模型与假设

1.1 空间机器人模型

典型的FFSR扩展动力学方程可表述为[4]

(1)

由于基座处于漂浮状态，且忽略了重力影响，FFSR系统满足动量守恒，故FFSR系统还满足

(2)

式中：M0为系统的初始动量。

进一步，将式(2)代入式(1)可得FFSR的标准动力学方程为

(3)

对式(1)求导并结合角动量及线动量守恒方程，可得FFSR的运动学方程为[4]

(4)

式中：J*(qb,qm)为FFSR的GJM，考虑本文以冗余FFSR系统为研究对象，则有J*(qb,qm)∈Rm×n,m

1.2 问题描述及假设

为了便于问题研究且不失一般性，作如下假设：

1) 一般而言，FFSR的基座质量远大于杆件质量，且为保证安全FFSR运动一般较为缓慢，故假设在两次数据采样间基座位姿变化很小，故以当前基座位姿采样值作为下一时刻基座运动的估计值。

3) 系统均为刚体，忽略微重力影响，系统初始动量M0=0。

4) FFSR初始关节配置，不为奇异位型，即FFSR初始状态不存在奇异。

2 冗余FFSR的跟踪控制器设计

为了实现FFSR的末端运动控制，本节中将基于DSDRE的调节器设计方法，设计冗余FFSR的末端轨迹跟踪控制器。

2.1 FFSR系统方程的伪线性重构

由式(2)～式(4)可得：

(5)

(6)

式中：A(x)∈R(n+m)×(n+m)、B(x)∈R(n+m)×n为系统的状态依赖系数(State Dependence Coefficient, SDC)矩阵[22]，表达式分别为

(7)

(8)

进一步，对伪线性状态方程式(6)进行离散化可得

xk+1=Ad(xk)xk+Bd(xk)τmk

(9)

式中：xk为tk时刻的状态变量；Ad(xk)、Bd(xk)为离散系统的状态矩阵和输入矩阵，分别满足

Ad(xk)=TA(xk)+I=

(10)

(11)

式中：T为采样周期；I为适当维数的单位矩阵；k={0,1,2,…,∞}表示采样序列。

则由式(10)和式(11)可得：

(12)

式(9)为由FFSR系统方程重构的离线系统伪线性方程。

2.2 FFSR跟踪控制器设计

由FFSR的伪线性重构状态方程式(9)及式(12)可知，若FFSR满足GJM行满秩，则其SDC矩阵[AdBd]满足逐点可控，则可根据DSDRE相关理论设计FFSR系统的优化跟踪控制器。

ek=xk-xdesk

(13)

进一步，定义性能指标函数：

(14)

式中：Q、R为适当维数的正定对称矩阵。

则可得反馈控制律为

(15)

式中：反馈增益为

(16)

Pk是李卡提方程的解：

(17)

定理1：若FFSR的GJM满足行满秩，即有rank(J*(qb,qm))=m，且满足假设2)及假设3)，存在优化跟踪控制律式(15)使得FFSR末端位姿运动稳定跟踪期望位姿轨迹。

证明：因rank(J*(qb,qm))=m，则由式(12)可得SDC矩阵{Ad,Bd}满足逐点可控。进一步，由优化理论可知对无限时间离散李卡提方程式(17)有唯一正定解。

由伪线性系统式(9)及控制律式(15)可得系统闭环方程

(18)

则闭环系统式(18)包含反馈项与前馈项，由于前馈项不影响系统的稳定性。故考虑闭环系统反馈环节为

(Adk-Fk)xk

(19)

则可构造Lyapunov函数：

满足V(xk)≥0,∀xk≠0，仅当xk=0,V(xk)=0。则由式(19)可得：

(20)

3 冗余FFSR系统的避奇异运动规划

由定理1可知，FFSR的GJM行满秩式是保证跟踪控制方法的有效的前提，但是，动基座使得GJM的奇异性与FFSR末端路径运动密切相关，使得采用离线路径规划方法回避GJM奇异具有一定的难度。此外，实际应用中由于关节角及关节角速度约束存在也使FFSR离线避奇异规划显得异常复杂。为此，本节将利用冗余FFSR具有多逆运动学解这一特点研究冗余FFSR的在线轨迹规划器。

3.1 FFSR的奇异性判定与避奇异约束

1) GJM的奇异性判定

考虑本文研究对象为冗余FFSR系统，其GJM为非方矩阵，为保证[AdBd]逐点可控，需要FFSR的GJM始终行满秩。为使这一条件满足，考虑以式(21)计算GJM奇异值，从而实现GJM奇异性判断。

(21)

式中：函数fsvd(·)表示计算矩阵的最小奇异值。

根据式(21)计算结果，可给出如下FFSR奇异性定义。

定义1FFSR的奇异区域阈值Suf>0。若i时刻式(21)计算结果si≤Suf，则认为此时FFSR处于奇异位型，相应的控制律式(15)失去对系统的控制运动的控制能力。

定义2进入奇异区域阈值Sif>Suf。若i时刻式(21)计算结果si≤Sif，则认为FFSR运动路径将发生奇异，需要进行适当的避奇异处理，以保证控制律式(15)的有效。

2) GJM奇异性估计与避奇异约束

(22)

为避免系统进入奇异区域以及进入奇异后迅速进行奇异回避运动，根据式(22)给出的期望奇异值，设计奇异回避约束函数为

(23)

3.2 在线轨迹规划器设计

为实现FFSR点到点运动控制，结合定义的避奇异约束函数，定义规划目标函数为

(24)

式中：α为规划参数；γ、β为权系数，为保证FFSR进入奇异后迅速离开奇异区域保证控制器有效性，通常权系数选取时应满足β≫γ。

式(24)的规划目标函数分为两项，其分别保证FFSR末端趋向目标点运动和FFSR的GJM行满秩使得跟踪控制方法始终有效。

由于式(24)中并不显含规划参数，现就规划参数与目标函数中各项关系以及其他规划约束条件，进行分析。

(25)

由式(4)可计算期望末端位姿速度为

(26)

进一步，可得期望末端位姿及期望关节角为

(27)

由式(25)可以得到目标函数中的优化参数本质上为期望关节运动的角加速度。实际中由于自身结构限制，各关节关节角总是限制在某个区域内，为此，必须对目标函数式(24)中的搜索空间进行约束以保证关节角及角加速度合理性。

定义3目标函数参数搜索空间

(28)

则有

(29)

由式(22)、式(23)以及式(24)～式(29)可将FFSR的点到点运动规划问题总结为非线性优化问题：

(30)

采用经典的内点法求解优化问题式(30)，即可获得控制器期望状态变量xdesk。

4 FFSR末端点到点避奇异运动控制

为实现FFSR的点到点运动控制本节中将对上述跟踪控制方法与在线规划方法进行综合以实现冗余FFSR点到点避奇异运动控制。

定义4FFSR终点精度控制函数

(31)

式中：etrack为目标位姿控制精度。

则由式(31)可得当满足etrack≤ε时认为达到设定目标位姿控制精度ε， FFSR运动停止。

整个点到点运动控制流程如图2所示，为进一步说明本文方法，同时给出计算步骤：

步骤2tk时，获得机器人关节角，基座位姿，末端位姿等初始值。

步骤3 计算式(21)判断GJM是否奇异，若sk≤Suf说明FFSR处于奇异位型控制器失效，则停止；反之转步骤4。

步骤5 利用规划所得期望状态量xdesk代入控制律式(15)，产生关节控制力矩τmk。

步骤6 根据式(31)计算停止精度条件，若满足etrack≤ε表明达到目标位姿停止，反之转步骤7。

步骤7 判断FFSR运动时间是否超过设定时间，若超过则停止。反之转步骤8。

步骤8k+1→k。转步骤2。

定理2 若非线性优化问题(30)始终有解，且满足假设1)～4)，则存在优化控制律(15)使得FFSR末端能够到达目标位姿。

证明：

由假设4)可知初始时GJM非奇异，则控制律式(15)在初始有效，又非线性优化问题式(30)始终有解，表明在FFSR运动过程GJM始终行满秩，由定理1可知FFSR末端能够稳定跟踪期望运动轨迹则由limk→∞x=xdes进一步可得k→∞,ye→yer。

5 数值仿真

5.1 方法有效性验证

为验证所提点到点控制方法有效性，同时简化仿真计算复杂度。采用如图3所示的平面4连杆冗余FFSR模型进行仿真验证。

注 1：采用平面4连杆模型进行仿真主要为降低仿真计算复杂度，由于本文采用FFSR的一般模型进行研究故所提方法并不限于平面4连杆模型的点到点控制。其中，ai为连杆质心到下一关节的几何长度；bi为前一关节到下一连杆质心几何长度；Ii为连杆i的转动惯量；mi为连杆i的质量；xE、yE为末端位置；qi为关节i的关节角；θ0为基座姿态角。系统的模型参数如表1所示。

杆件号ai/mbi/mmi/kgIi/(kg·m2)00.5255.66710.50.550.33320.50.540.33330.50.530.2540.130.15

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

给定系统初始末端位姿速度为0，基座初始位姿为0,关节角初始值为

qm0=[0.774 10.420 3 -1.024 8 1.247 6]Trad

控制器选取权值矩阵为

仿真采用MATLAB Spacedyn工具箱[22]，仿真时间为30 s，结果如图4～图9所示。其中图4为控制力矩，图5表示规划以及控制运动的末端位姿轨迹曲线。图6为末端轨迹跟踪的跟踪误差，图7为式(21)的最小奇异值变化情况，图8为基座的位姿运动情况，图9为终点精度控制函数曲线。图10为优化参数状态变化曲线。

仿真结果表明本文所提FFSR点到点避奇异运动控制方法，能够实现FFSR末端由初始点到目标点位姿运动控制。

图7反映了跟踪过程中式(22)计算的GJM最小奇异值的变化情况，表明跟踪过程在5 s前后两次进入了预设的奇异处理区域。由于规划目标函数式(30)中避奇异约束函数作用使得系统在很短的时间内离开了奇异区域从而保证了系统控制方法的持续有效性。图8中FFSR的基座运动展现了FFSR基座因关节运动反作用力矩而发生位姿改变的特点。此外，图9中终点精度曲线在5 s左右出现的跳跃变化从侧面表明了当系统进行奇异回避规划时为了保证最大的避奇异能力，规划器适当降低了向目标运动的趋势。图10反映了规划目标函数式(24)中规划参数α的转态变化情况，从图中可以看到由于在规划过程施加了参数搜索空间的约束条件，使得α的状态曲线呈现上下界限制特点。

5.2 与广义逆(Moore-Penrose Pseudo Invers)规划方法的对比

由于FFSR系统具有冗余机械臂，其GJM行长大于列长，故无法对GJM直接求逆从而获得关节角速度进行关节空间运动规划，通常解决此问题的最简单也是最常用方法是利用其广义逆(Moore-Penrose pseudo invers)求解FFSR的逆运动学从而实现关节轨迹的规划。进一步，利用适当的控制方法跟踪规划所得关节轨迹实现FFSR的运动控制。现就本文方法与广义逆规划方法在避奇异及点到点运动控制的能力进行比较。

为比较本文方法与利用广义逆规划方法(利用广义逆求解逆运动学解)在点到点避奇异运动控制方法的性能。在利用广义逆规划方法规划关节角轨迹时，采用与本文方法相同的期望末端轨迹即图5中规划末端位姿轨迹。此外，利用广义逆规划方法得到避奇异规划轨迹后，需要选取合适关节轨迹跟踪控制方法实现关节轨迹的跟踪控制。为避免跟踪误差的影响，假设选取控制方法的关节轨迹跟踪误差为0，即利用规划关节轨迹直接计算末端运动轨迹。

给定系统初始末端位姿速度为0，基座初始位姿为0,关节角初始值为

qm0=[0.774 10.420 3-1.024 81.247 6]Trad

仿真结果如图11和图12所示，其中图11为末端位姿轨迹对比。图12为基座变化对比。

由图11的末端位姿轨迹结果看本文方法明显优于直接使用广义规划方法，其中广义逆规划方法远远偏离了期望的末端目标位姿，这主要是由于采用广义逆求解逆运动学时本质上是利用最小二乘法求解如下优化问题，从而获得关节运动轨迹。

(32)

由式(32)可以看出，在利用广义逆矩阵求解关节角轨迹时不可避免的存在规划误差，且当GJM存在奇异时，规划误差存在异常增大现象。故广义逆规划方法实际上是以牺牲运动控制精度为代价，实现关节期望轨迹的求解。因此，图11中利用直接求逆法控制末端运动在终点控制精度上达不到预期效果。

此外，图11中亦可以看到广义逆方法在FFSR末端运动的初期与本文方法，但随着仿真时间增长广义逆规划方法规划误差的不断累计使得两者逐渐产生不同最终远离了期望目标位姿，特别的当5 s左右GJM产生奇异，由于本文方法进行了避奇异处理，而广义逆方法因未考虑GJM奇异带来的影响，从而产生了较大的规划误差。