1 引言（Introduction）

空间机器人在执行卫星维护、在轨装配和空间碎片移除等在轨任务方面，与航天员相比具有诸多优势。自20世纪80年代以来，国内外航天机构已在大型航天器和在轨服务任务中实施了一系列空间机器人项目，典型实例有“工程试验卫星7号”（Engineering Test Satellite VII，ETS-VII）、“轨道快车”（Orbit Express，OE）、“凤凰计划”（Phoe- nix）、“德国轨道服务任务”（Deutsche Orbitale Servicing Mission，DEOS）等^[1]。应用机器人执行未来的空间任务已成为航天领域的一大趋势^[2]。

空间机器人对非合作目标的抓捕是在轨服务中最具挑战性的工作之一。捕获过程包括一系列操作，可分为4个阶段。第1阶段为观测和规划阶段，服务航天器获取目标卫星的运动和物理特性信息，并规划机械臂如何抓取目标。第2阶段是控制机器人向规划的抓捕位置移动，使机械臂准备好抓取目标。第3阶段是实际捕获（物理拦截）阶段，机械臂抓取住目标卫星上的捕获设备。第4阶段是捕获后阶段，捕获的目标与服务系统作为一个整体稳定下来。图 1展示了利用空间机器人抓捕目标这一典型在轨操作的示意图。

完成捕获后，空间机器人的首要任务是消除目标卫星的自旋运动。为解决这一具有挑战性的问题，需要研究相应的规划和控制方法^[3]。Dimitrov和Yoshida ^[4-5]提出了一种分布式动量控制方法，用于在抓捕后阶段吸收和管理目标的角动量，通过对目标的动量进行管理，使其不会干扰服务卫星的基座姿态。Aghili ^[6-7]研究了机器人捕获翻滚卫星后的最优消旋控制策略，在末端执行器作用力和力矩满足约束的条件下使时间或能量的代价函数最小化。王明明等^[8-10]基于归一化时间或贝塞尔曲线设计了目标卫星的快速消旋策略，最优消旋时间由末端执行器处的相互作用力/力矩限制决定。Lampariello等^[11]提出了一种抑制目标相对运动的最优关节阻尼轨迹。Luo等^[12]提出了一种基于角动量分布的消旋镇定策略，在保持基座姿态稳定的前提下，将目标和机械臂的角动量传递给反作用轮，同时考虑关节速度的限制。Zong等^[13]研究了空间机械臂捕获翻滚目标后的最短时间消旋轨迹，考虑目标姿态运动边界和抓捕点处的交互力矩限制，并使用变分法求解。程靖和陈力^[14]针对空间机器人双臂捕获航天器后的姿态管理和辅助对接操作的协调控制问题，提出了基于极限学习机的自适应神经网络控制方案。此外，文[15-16]在调节服务卫星基座姿态的同时，利用服务卫星机械臂和卫星基座的协调控制来跟踪所需的消旋轨迹。

以上抓捕后的消旋策略是在目标惯性参数（质量、惯性张量和质心位置）已知的前提下发展起来的。这一假设的前提是，这些参数可以在抓捕之前通过参数辨识技术获得。参数辨识技术可分为基于视觉的^[17]、基于动量的^[18-20]和基于力的^{[18, 21]}技术。然而，这些参数辨识技术不能保证估计出的目标惯性参数在抓捕之前收敛到其真实值。Zhang等^[22]提出一种自适应滑模控制器来跟踪抓捕后阶段的参考轨迹，以使惯性参数不确定的非合作目标消旋。Zhu等^[23]提出了一种自适应滑模干扰观测器来估计未知目标产生的干扰，以便在跟踪期望的关节空间轨迹和调节服务卫星的基座姿态时对其进行补偿。Jayakody等^[24]应用自适应变结构控制方法，在服务卫星基座和机械臂之间实现鲁棒的协调控制，以解决服务卫星系统中的参数不确定性。上述策略没有考虑末端执行器处的相互作用力/力矩限制。文^{[8, 25]}使用末端执行器力/力矩测量值来实现阻抗控制，以跟踪期望的消旋轨迹。Oki等^[26]提出在不需要精确了解目标惯性参数的情况下描绘出一条消旋轨迹，并使用末端执行器处的力/力矩测量值修正轨迹，以遵守末端执行器的力/力矩限制。Nguyen-Huynh和Sharf^[27]提出了一种新的自适应算法，在空间机械臂捕获未知翻滚目标期间及捕获后产生零反作用运动。此外，Gangapersaud等^[28-30]提出一种解耦策略来调节服务位置的基座姿态，并跟踪末端执行器的参考力/力矩，从而解耦惯性参数未知但有界的非合作目标。

与单臂机器人相比，多臂机器人所能处理的载荷更多，灵活性也更高，但多臂协调操作的复杂度也更大。已有的消旋策略大多将末端执行器的力/力矩限制作为约束条件，而没有考虑服务卫星的力/力矩输入限制。在实际应用中，目标的可行运动受到空间机器人的力/力矩输入限制的影响。Yoshikawa^[31]提出动力学可操作度的概念以衡量力/力矩输入对目标加速度的映射能力。动力学可操作度可分别以操作因子和操作椭球衡量^[32]。在此基础上，Chiu^[33]提出任务相容性以衡量机械臂构形在给定任务方向上的可操作度。Xu等^[34]研究了自由飘浮多臂空间机器人的任务相容性及其在轨迹规划和构形优化等方面的应用。

本文旨在提出一种基于空间机器人对所抓捕目标的任务相容性的消旋策略。目标的期望加速度的方向取作其速度的反方向，其大小根据当前机械臂构形的动力学可操作度和任务相容性确定，从而在机器人系统力和力矩输入限制内实现最速消旋，且无需任何优化算法。基于协调双臂运动的内外双环式结构，对目标和机械臂末端分别建立柔顺度等式，提出了一种协调稳定控制方法，以应对目标惯性参数不确定性，并降低机械臂末端与目标间的内力。

2 动力学建模（Dynamic modeling）

在抓捕后阶段，空间机器人与所抓捕目标构成一个组合体。如图 2所示，双臂空间机器人系统由基座和2条$ n $自由度机械臂构成，共包含$ 2n+1 $个刚体，总自由度$ N=2n+6 $。表 1给出了组合体中运动学和动力学符号的含义，其中参数下标b、m、e和t分别表示与基座、机械臂、末端执行器和目标相关的项。在本文中，如无特别说明，所有参数都表示在与其固联的刚体坐标系中。$ \mathit{\boldsymbol{E}}_{n} $和$ \mathit{\boldsymbol{0}}_{n} $分别表示$ n\times n $维单位矩阵和零矩阵。

2.1 空间机器人动力学

根据拉格朗日力学原理^[17]，空间机器人的动力学方程表示如下：

$ \begin{align} \mathit{\boldsymbol{H\ddot{q}}}+\mathit{\boldsymbol{c}}=\mathit{\boldsymbol{u}}-\mathit{\boldsymbol{J}}_{\rm e}^{\rm T} \mathit{\boldsymbol{f}}_{\rm e} \end{align} $

(1)

方程可以分解为以下形式：

$ \begin{align} \begin{bmatrix} {\mathit{\boldsymbol{H}}_{\rm b}} & {\mathit{\boldsymbol{H}}_{\rm{bm}}} \\ {\mathit{\boldsymbol{H}}_{\rm{bm}}^{\rm T}} & {\mathit{\boldsymbol{H}}_{\rm m}} \end{bmatrix} \begin{bmatrix} {\ddot{\mathit{\boldsymbol{x}}}_{\rm b}} \\ {\ddot{{\mathit{\boldsymbol{\theta}}}}} \end{bmatrix}+ \begin{bmatrix} {\mathit{\boldsymbol{c}}_{\rm b}} \\ {\mathit{\boldsymbol{c}}_{\rm m}} \end{bmatrix} = \begin{bmatrix} {\mathit{\boldsymbol{f}}_{\rm b}} \\ {\mathit{\boldsymbol{\tau}}} \end{bmatrix}- \begin{bmatrix} {\mathit{\boldsymbol{J}}_{\rm b}^{\rm T}} \\ {\mathit{\boldsymbol{J}}_{\rm m}^{\rm T}} \end{bmatrix}\mathit{\boldsymbol{f}}_{\rm e} \end{align} $

(2)

或：

$ \begin{align} & \begin{bmatrix} {\mathit{\boldsymbol{H}}_{\rm b}} & {\mathit{\boldsymbol{H}}_{\rm{bm}}^{a}} & {\mathit{\boldsymbol{H}}_{\rm{bm}}^{b}} \\ {\mathit{\boldsymbol{H}}_{\rm{bm}}^{a\rm T}} & {\mathit{\boldsymbol{H}}_{\rm m}^{a}} & {{{\boldsymbol{0}}}_{n}} \\ {\mathit{\boldsymbol{H}}_{\rm{bm}}^{b\rm T}} & {{{\boldsymbol{0}}}_{n}} & {\mathit{\boldsymbol{H}}_{\rm m}^{b}} \end{bmatrix} \begin{bmatrix} {\ddot{\mathit{\boldsymbol{x}}}_{\rm b}} \\ {\ddot{{\mathit{\boldsymbol{\theta}}}}^{a}} \\ {\ddot{{\mathit{\boldsymbol{\theta}}}}^{b}} \end{bmatrix}+ \begin{bmatrix} {\mathit{\boldsymbol{c}}_{\rm b}} \\ {\mathit{\boldsymbol{c}}_{\rm m}^{a}} \\ {\mathit{\boldsymbol{c}}_{\rm m}^{b}} \end{bmatrix} \\ =& \begin{bmatrix} {\mathit{\boldsymbol{f}}_{\rm b}} \\ {{\mathit{\boldsymbol{\tau}}}^{a}} \\ {{\mathit{\boldsymbol{\tau}}}^{b}} \end{bmatrix}- \begin{bmatrix} {\mathit{\boldsymbol{J}}_{\rm b}^{a\rm T}} & {\mathit{\boldsymbol{J}}_{\rm b}^{b\rm T}} \\ {\mathit{\boldsymbol{J}}_{\rm m}^{a\rm T}} & {{{\boldsymbol{0}}}_{n\times 6}} \\ {{{\boldsymbol{0}}}_{n\times 6}} & {\mathit{\boldsymbol{J}}_{\rm m}^{b\rm T}} \end{bmatrix} \begin{bmatrix} {\mathit{\boldsymbol{f}}_{\rm e}^{a}} \\ {\mathit{\boldsymbol{f}}_{\rm e}^{b}} \end{bmatrix} \end{align} $

(3)

其中参数上标$ a $和$ b $分别表示与臂$ a $或臂$ b $相关的项。$ \mathit{\boldsymbol{H}}\in \mathbb{R}^{N\times N} $为空间机器人的广义惯性矩阵，由基座项$ \mathit{\boldsymbol{H}}_{\rm b} $、机械臂项$ \mathit{\boldsymbol{H}}_{\rm m} $以及耦合项$ \mathit{\boldsymbol{H}}_{\rm{bm}} $组成。$ \ddot{\mathit{\boldsymbol{q}}}\in \mathbb{R}^{N} $为广义加速度，包括基座加速度$ \ddot{\mathit{\boldsymbol{x}}}_{\rm b} $和关节加速度$ \ddot{\mathit{\boldsymbol{\theta}}} $。$ \mathit{\boldsymbol{c}}\in \mathbb{R}^{N} $为广义偏差力，由基座项$ \mathit{\boldsymbol{c}}_{\rm b} $和机械臂项$ \mathit{\boldsymbol{c}}_{\rm m} $组成。$ \mathit{\boldsymbol{u}}\in \mathbb{R}^{N} $为机器人系统的广义输入，包括基座输入力和力矩$ \mathit{\boldsymbol{f}}_{\rm b} $以及关节扭矩$ \mathit{\boldsymbol{\tau}} $。$ \mathit{\boldsymbol{J}}_{\rm e} \in \mathbb{R}^{12\times N} $为雅可比矩阵，由基座项$ \mathit{\boldsymbol{J}}_{\rm b} $和机械臂项$ \mathit{\boldsymbol{J}}_{\rm m} $组成。$ \mathit{\boldsymbol{f}}_{\rm e} \in \mathbb{R}^{12} $为末端施加的力和力矩。

机械臂末端速度可以表示为

$ \begin{align} \dot{\mathit{\boldsymbol{x}}}_{\rm e} = \begin{bmatrix} {\dot{\mathit{\boldsymbol{x}}}_{\rm e}^{a}} \\ {\dot{\mathit{\boldsymbol{x}}}_{\rm e}^{b}} \end{bmatrix} =[{\mathit{\boldsymbol{J}}_{\rm b}} \; \; {\mathit{\boldsymbol{J}}_{\rm m}} ] \begin{bmatrix} {\dot{\mathit{\boldsymbol{x}}}_{\rm b}} \\ {\dot{{\mathit{\boldsymbol{\theta}}}}} \end{bmatrix} = \begin{bmatrix} {\mathit{\boldsymbol{J}}_{\rm b}^{a} \dot{\mathit{\boldsymbol{x}}}_{\rm b} +\mathit{\boldsymbol{J}}_{\rm m}^{a} \dot{{\mathit{\boldsymbol{\theta}}}}^{a}} \\ {\mathit{\boldsymbol{J}}_{\rm b}^{b} \dot{\mathit{\boldsymbol{x}}}_{\rm b} +\mathit{\boldsymbol{J}}_{\rm m}^{b} \dot{{\mathit{\boldsymbol{\theta}}}}^{b}} \end{bmatrix} \end{align} $

(4)

刚体速度$ \dot{\mathit{\boldsymbol{x}}}=[{\mathit{\boldsymbol{\omega}}^{\rm T}} \; \; {\mathit{\boldsymbol{\upsilon}}^{\rm T}} ]^{\rm T} $由角速度和线速度组成。线速度$ \mathit{\boldsymbol{\upsilon}} =\dot{\mathit{\boldsymbol{r}}} $即为刚体位置向量的导数，而角速度与其姿态欧拉角$ \mathit{\boldsymbol{\phi}} =[\alpha \; \; \beta \; \; \gamma ]^{\rm T} $通过以下公式联系（欧拉角旋转顺序为$ X $-$ Y $-$ Z $）：

$ \begin{align} \mathit{\boldsymbol{\omega}} =\mathit{\boldsymbol{J}}_{\phi} \dot{\mathit{\boldsymbol{\phi}}} \end{align} $

(5)

其中：

$ \begin{align} \mathit{\boldsymbol{J}}_{\phi} = \begin{bmatrix} {\cos \beta \cos \gamma} & {\sin \gamma} & 0 \\ {-\cos \beta \sin \gamma} & {\cos \gamma} & 0 \\ {\sin \beta} & 0 & 1 \end{bmatrix} \end{align} $

(6)

2.2 目标动力学

目标卫星的动力学方程表示如下：

$ \begin{align} \bar{\mathit{\boldsymbol{I}}}_{\rm t} \ddot{{\mathit{\boldsymbol{x}}}}_{\rm t} +\dot{{\mathit{\boldsymbol{x}}}}_{\rm t} \times^{*} \bar{\mathit{\boldsymbol{I}}}_{\rm t} \dot{{\mathit{\boldsymbol{x}}}}_{\rm t} ={\mathit{\boldsymbol{J}}}_{\rm t}^{\rm T} {\mathit{\boldsymbol{f}}}_{\rm e} \end{align} $

(7)

其中$ \bar{\mathit{\boldsymbol{I}}}_{\rm t} ={\rm diag}({\mathit{\boldsymbol{I}}}_{\rm t}, m_{\rm t} {\mathit{\boldsymbol{E}}}_{3})\in \mathit{\boldsymbol{\mathbb{R}}}^{6\times 6} $为目标转动惯量和质量组成的广义惯性矩阵。$ \dot{\mathit{\boldsymbol{x}}}_{\rm t} =[{\omega_{\rm t}^{\rm T}} \; \; {\upsilon_{\rm t}^{\rm T}} ]^{\rm T}\in \mathit{\boldsymbol{\mathbb{R}}}^{6} $为目标质心速度。$ \mathit{\boldsymbol{J}}_{\rm t} =[{\mathit{\boldsymbol{J}}_{\rm t}^{a\rm T}} \; \; {\mathit{\boldsymbol{J}}_{\rm t}^{b\rm T}} ]^{\rm T}\in \mathbb{R}^{12\times 6} $为目标雅可比矩阵，由目标质心到各抓捕点的转换矩阵组成。$ \times^{*} $表示6维空间向量的叉乘因子，对于$ \mathit{\boldsymbol{x}}=[{\mathit{\boldsymbol{x}}_{1}^{\rm T}} \; \; {\mathit{\boldsymbol{x}}_{2}^{\rm T}} ]^{\rm T}\in \mathbb{R}^{6} $，其具体形式为

$ \begin{align} \mathit{\boldsymbol{x}}\times^{*} = \begin{bmatrix} {\mathit{\boldsymbol{x}}_{1}} \\ {\mathit{\boldsymbol{x}}_{2}} \end{bmatrix}\times^{*} = \begin{bmatrix} {\mathit{\boldsymbol{x}}_{1} \times} & {\mathit{\boldsymbol{x}}_{2} \times} \\ {\mathit{\boldsymbol{0}}_{3}} & {\mathit{\boldsymbol{x}}_{1} \times} \end{bmatrix} \end{align} $

(8)

对于3维向量$ \mathit{\boldsymbol{x}}=[{x_{1}} \; \; {x_{2}} \; \; {x_{3}} ]^{\rm T}\in \mathbb{R}^{3} $，叉乘因子$ \times $的形式为

$ \begin{align} \mathit{\boldsymbol{x}}\times = \begin{bmatrix} {x_{1}} \\ {x_{2}} \\ {x_{3}} \end{bmatrix}\times = \begin{bmatrix} 0 & {-x_{3}} & {x_{2}} \\ {x_{3}} & 0 & {-x_{1}} \\ {-x_{2}} & {x_{1}} & 0 \end{bmatrix} \end{align} $

(9)

目标上的抓捕点速度可以表示为

$ \begin{align} \dot{\mathit{\boldsymbol{x}}}_{\rm g} = \begin{bmatrix} {\dot{\mathit{\boldsymbol{x}}}_{\rm g}^{a}} \\ {\dot{\mathit{\boldsymbol{x}}}_{\rm g}^{b}} \end{bmatrix}=\mathit{\boldsymbol{J}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t} = \begin{bmatrix} {\mathit{\boldsymbol{J}}_{\rm t}^{a} \dot{\mathit{\boldsymbol{x}}}_{\rm t}} \\ {\mathit{\boldsymbol{J}}_{\rm t}^{b} \dot{\mathit{\boldsymbol{x}}}_{\rm t}} \end{bmatrix} \end{align} $

(10)

空间机器人完成对目标的捕获后，末端执行器和抓捕点的相对速度应为0，即：

$ \begin{align} \mathit{\boldsymbol{J}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}=\mathit{\boldsymbol{J}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t} \end{align} $

(11)

3 消旋策略（Detumbling strategy）

空间机器人完成捕获后，末端执行器与抓捕点建立了稳固连接，如果不对目标实施消旋，目标卫星的旋转运动将会传递至空间机器人，从而引起整个组合体不可预测的运动。本节的主要内容是在满足服务卫星的力/力矩输入约束的条件下，如何在最优时间内使目标卫星在给定的初始角速度下实现停靠，同时需保证目标运动轨迹的连续性并尽量减小运动过程中的非期望影响。这里借助动力学可操作度及其衍生的任务相容性的概念，提出一种充分利用机器人系统对目标的操作能力的消旋策略。不失一般性，本文中变量的上标s、d和f分别代表变量的初始、期望和最终值。

3.1 动力学可操作度

动力学可操作度^[31]是衡量机械臂关节扭矩输入对操作点加速度的映射能力的指标。机器人与目标之间的加速度约束可通过对式(11)两边求导得到：

$ \begin{align} \mathit{\boldsymbol{J}}_{\rm e} \ddot{\mathit{\boldsymbol{q}}}+\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}=\mathit{\boldsymbol{J}}_{\rm t} \ddot{\mathit{\boldsymbol{x}}}_{\rm t} \end{align} $

(12)

由式(1)和式(7)分别可得：

$ \begin{align} \ddot{\mathit{\boldsymbol{q}}}& =\mathit{\boldsymbol{H}}^{-1}(\mathit{\boldsymbol{u}}-\mathit{\boldsymbol{c}}-\mathit{\boldsymbol{J}}_{\rm e}^{\rm T} \mathit{\boldsymbol{f}}_{\rm e}) \end{align} $

(13)

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm t} & =\bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} (\mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{f}}_{\rm e} -\dot{\mathit{\boldsymbol{x}}}_{\rm t} \times^{*} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t}) \end{align} $

(14)

将以上两式代入式(12)，经整理得：

$ \begin{align} & (\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}\mathit{\boldsymbol{J}}_{\rm e}^{\rm T} +\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T})\mathit{\boldsymbol{f}}_{\rm e} \\ =&\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}(\mathit{\boldsymbol{u}}-\mathit{\boldsymbol{c}})+\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}+\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \dot{\mathit{\boldsymbol{x}}}_{\rm t} \times^{*} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t} \end{align} $

(15)

令$ \mathit{\boldsymbol{A}}=\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}\mathit{\boldsymbol{J}}_{\rm e}^{\rm T} +\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} $，则有：

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm e} =\mathit{\boldsymbol{A}}^{-1}[{\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}(\mathit{\boldsymbol{u}}-\mathit{\boldsymbol{c}})+\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}+\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \dot{\mathit{\boldsymbol{x}}}_{\rm t} \times^{*} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t}}] \end{align} $

(16)

将式(16)代入式(14)得：

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm t} =\;& \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{A}}^{-1}[{\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}(\mathit{\boldsymbol{u}}-\mathit{\boldsymbol{c}})+\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}}] + \\ & (\bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{A}}^{-1}\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} -\bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1})\dot{\mathit{\boldsymbol{x}}}_{\rm t} \times^{*} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t} \\ =\;& \mathit{\boldsymbol{A}}_{\rm t} \mathit{\boldsymbol{u}}+\mathit{\boldsymbol{b}}_{\rm t} \end{align} $

(17)

此即为目标加速度的动力学可操作度公式。其中$ \mathit{\boldsymbol{A}}_{\rm t} $和$ \mathit{\boldsymbol{b}}_{\rm t} $分别为可操作度矩阵和偏差加速度：

$ \begin{equation} \begin{aligned} \mathit{\boldsymbol{A}}_{\rm t} =\;& \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{A}}^{-1}\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1} \\ \mathit{\boldsymbol{b}}_{\rm t} =\;& (\bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{A}}^{-1}\mathit{\boldsymbol{J}}_{\rm t} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} -\bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1})\dot{\mathit{\boldsymbol{x}}}_{\rm t} \times^{*} \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t} \dot{\mathit{\boldsymbol{x}}}_{\rm t}+ \\ & \bar{{{\mathit{\boldsymbol{I}}}}}_{\rm t}^{-1} \mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{A}}^{-1}(\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}-\mathit{\boldsymbol{J}}_{\rm e} \mathit{\boldsymbol{H}}^{-1}\mathit{\boldsymbol{c}}) \end{aligned} \end{equation} $

(18)

式(17)可转化为

$ \begin{align} \mathit{\boldsymbol{u}}=\mathit{\boldsymbol{A}}_{\rm t}^{+} (\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t}) \end{align} $

(19)

其中上标$ + $表示矩阵的广义逆。令$ \mathit{\boldsymbol{u}} = [ {\mathit{\boldsymbol{f}}_{\rm b}^{\rm T}} \; \; {{\mathit{\boldsymbol{\tau}}}^{\rm T}} ]^{\rm T} $满足如下输入限制：

$ \begin{equation} \begin{aligned} | {u_{i}} | & \leqslant u_{i, \max} \\ \mathit{\boldsymbol{u}}_{\max} & = [ {\mathit{\boldsymbol{f}}_{\rm b, \max}^{\rm T}} \; \; {{\mathit{\boldsymbol{\tau}}}_{\rm b, \max}^{\rm T}} ]^{\rm T} \end{aligned} \end{equation} $

(20)

其中$ \mathit{\boldsymbol{f}}_{\rm b, \max} $和$ \mathit{\boldsymbol{\tau}}_{\rm b, \max} $分别为基座力/力矩和关节扭矩的限制。考虑由各输入边界构成的权重矩阵$ \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}} $ $ ={\rm diag}(\mathit{\boldsymbol{u}}_{\max}) $，可将$ \mathit{\boldsymbol{u}} $按比例缩放为归一化向量：

$ \begin{align} \mathit{\boldsymbol{u}}=\mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}} \hat{\mathit{\boldsymbol{u}}}, \quad | {\hat{u}_{i}} |\leqslant 1 \end{align} $

(21)

于是有：

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm t} =\mathit{\boldsymbol{A}}_{\rm t} \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}} \hat{\mathit{\boldsymbol{u}}}+\mathit{\boldsymbol{b}}_{\rm t} \end{align} $

(22)

以及

$ \begin{align} \hat{\mathit{\boldsymbol{u}}}=\mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}}^{-1} \mathit{\boldsymbol{A}}_{\rm t}^{+} (\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t}) \end{align} $

(23)

动力学可操作度可分别以操作因子和操作椭球衡量^[32]。首先，对$ \mathit{\boldsymbol{A}}_{\rm t} $进行奇异值分解：

$ \begin{align} \mathit{\boldsymbol{A}}_{\rm t} =[{\mathit{\boldsymbol{U}}_{1}} \; \; {\mathit{\boldsymbol{U}}_{2}} ] \begin{bmatrix} {\mathit{\boldsymbol{\varSigma}}} & {\mathit{\boldsymbol{0}}} \\ {\mathit{\boldsymbol{0}}} & {\mathit{\boldsymbol{0}}} \end{bmatrix} \begin{bmatrix} {\mathit{\boldsymbol{V}}_{1}^{\rm T}} \\ {\mathit{\boldsymbol{V}}_{2}^{\rm T}} \end{bmatrix} \end{align} $

(24)

其中$ \mathit{\boldsymbol{U}}_{1} \in \mathbb{R}^{6\times r} $（$ r $为$ \mathit{\boldsymbol{A}}_{\rm t} $的秩），$ \mathit{\boldsymbol{U}}_{2} \in \mathbb{R}^{6\times (6-r)} $，$ \mathit{\boldsymbol{V}}_{1} \in \mathbb{R}^{N\times r} $，$ \mathit{\boldsymbol{V}}_{2} \in \mathbb{R}^{N\times (N-r)} $。$ \mathit{\boldsymbol{\varSigma}} ={\rm diag}(\sigma_{1}, \cdots, \sigma_{r})\in \mathbb{R}^{r\times r} $为$ \mathit{\boldsymbol{A}}_{\rm t} $的各非零奇异值组成的对角矩阵。考虑到偏差加速度的影响，定义偏差因子：

$ \begin{align} \eta =\sqrt{\sum\limits_{i=1}^6 \left(\frac{p_{i}} {\sigma_{i}}\right)^{2}} \end{align} $

(25)

其中$ \mathit{\boldsymbol{p}}=\mathit{\boldsymbol{U}}^{-1}\mathit{\boldsymbol{b}} $。综上所述，考虑偏差的动力学可操作度因子定义为

$ \begin{align} ma_{\rm t} = \begin{cases} {(1-\eta)\det ({\mathit{\boldsymbol{\varSigma}}} (\mathit{\boldsymbol{A}}_{\rm t})\mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}})}, & {\eta <1} \\ 0, & {\eta \geqslant 1} \end{cases} \end{align} $

(26)

假设归一化输入$ \hat{\mathit{\boldsymbol{u}}} $满足单位球约束：

$ \begin{align} \hat{\mathit{\boldsymbol{u}}}^{\rm T}\hat{\mathit{\boldsymbol{u}}}\leqslant 1 \end{align} $

(27)

根据式(23)，目标加速度满足约束：

$ \begin{equation} \rm{或}\; \; \begin{aligned} &(\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})^{\rm T}\mathit{\boldsymbol{A}}_{\rm t}^{+\rm T} \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}}^{-2} \mathit{\boldsymbol{A}}_{\rm t}^{+} (\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})\leqslant 1 \\ & (\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})^{\rm T}(\mathit{\boldsymbol{A}}_{\rm t} \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}}^{2} \mathit{\boldsymbol{A}}_{\rm t}^{\rm T})^{-1}(\ddot{\mathit{\boldsymbol{x}}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})\leqslant 1 \end{aligned} \end{equation} $

(28)

这一约束在几何上表示为一个球心位于$ \mathit{\boldsymbol{b}}_{\rm t} $的6维椭球，即动力学可操作度椭球。椭球的各轴长分别对应$ \mathit{\boldsymbol{A}}_{\rm t} $的各奇异值大小，在不考虑偏差加速度的情况下，椭球的体积与可操作度因子成正比。

3.2 任务相容性

任务相容性^[33]可以看作期望任务方向上的可操作度衡量。这里以单位向量$ \mathit{\boldsymbol{k}}_{\rm t} $表示期望任务方向，于是目标加速度可以表示为

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm t} =a\mathit{\boldsymbol{k}}_{\rm t} \end{align} $

(29)

其中标量$ a>0 $为$ \ddot{\mathit{\boldsymbol{x}}}_{\rm t} $在$ \mathit{\boldsymbol{k}}_{\rm t} $方向的比例值。

任务相容性有椭球法和比例因子法两种衡量方法^[34]。将式(29)代入式(28)，可得到机器人输入的单位球约束下，目标加速度比例值$ a $所满足的约束：

$ \begin{align} (a\mathit{\boldsymbol{k}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})^{\rm T}(\mathit{\boldsymbol{A}}_{\rm t} \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}}^{2} \mathit{\boldsymbol{A}}_{\rm t}^{\rm T})^{-1}(a\mathit{\boldsymbol{k}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})\leqslant 1 \end{align} $

(30)

上式可转化为如下二元一次方程：

$ \begin{align} \bar{\alpha} a^{2}+2\bar{\beta} a+\bar{\gamma} \leqslant 0 \end{align} $

(31)

其中$ \bar{\alpha} =\mathit{\boldsymbol{k}}_{\rm t}^{\rm T} \mathit{\boldsymbol{Bk}}_{\rm t} $，$ \bar{\beta}=-\mathit{\boldsymbol{k}}_{\rm t}^{\rm T} \mathit{\boldsymbol{Bb}}_{\rm t} $，$ \bar{\gamma}=\mathit{\boldsymbol{b}}_{\rm t}^{\rm T} \mathit{\boldsymbol{Bb}}_{\rm t} -1 $，$ \mathit{\boldsymbol{B}}=(\mathit{\boldsymbol{A}}_{\rm t} \mathit{\boldsymbol{W}}_{{\mathit{\boldsymbol{u}}}}^{2} \mathit{\boldsymbol{A}}_{\rm t}^{\rm T})^{-1} $。求解该方程可得$ a $的最大值：

$ \begin{align} {}^{\rm e}a_{\max} =\frac{-\bar{\beta} \pm \sqrt{\bar{\beta}^{2}-\bar{\alpha} \bar{\gamma}}}{\bar{\alpha}} \end{align} $

(32)

$ {}^{\rm e}a_{\max} $的几何意义为可操作度椭球球心沿$ \mathit{\boldsymbol{k}}_{\rm t} $方向与椭球表面的距离。

与椭球法相比，比例因子法可以更精确地描述任务的相容性。将式(29)代入式(19)和式(20)，可得：

$ \begin{align} -\mathit{\boldsymbol{u}}_{\max} \leqslant \mathit{\boldsymbol{A}}_{\rm t}^{+} ({}^{\rm s}a\mathit{\boldsymbol{k}}_{\rm t} -\mathit{\boldsymbol{b}}_{\rm t})\leqslant \mathit{\boldsymbol{u}}_{\max} \end{align} $

(33)

该式可改写为

$ \begin{equation} \begin{aligned} & -u_{1, \max} \leqslant (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{1} {}^{\rm s}a-(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{1} \leqslant u_{1, \max} \\ & \qquad\qquad\qquad\qquad \vdots \\ & -u_{N, \max} \leqslant (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{N} {}^{\rm s}a-(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{N} \leqslant u_{N, \max} \end{aligned} \end{equation} $

(34)

根据以上不等式约束组，可以得到：

$ \begin{align} \begin{cases} \frac{-u_{i, \max} +(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i}} {(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i}} \leqslant {}^{\rm s}a\leqslant \frac{u_{i, \max} +(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i}} {(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i}}, \\ \quad (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i} >0 \\ {}^{\rm s}a=+\infty, \quad (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i} =0, \; \; (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i} \leqslant u_{i, \max} \\ {}^{\rm s}a=-\infty, \quad (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i} =0, \; \; (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i} >u_{i, \max} \\ \frac{u_{i, \max} +(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i}} {(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i}} \leqslant {}^{\rm s}a\leqslant \frac{-u_{i, \max} +(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{b}}_{\rm t})_{i}} {(\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i}}, \\\quad (\mathit{\boldsymbol{A}}_{\rm t}^{+} \mathit{\boldsymbol{k}}_{\rm t})_{i} <0 \end{cases} \end{align} $

(35)

由各不等式约束获得的比例因子$ ^{\rm s}a_{i} $为

$ \begin{align} &{}^{\rm s}a_{i}=\\ & \begin{cases} {\frac{u_{i, \max} +({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{b}}}}_{\rm t})_{i}} {({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i}}}, &({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i} >0 \\ {+\infty}, &({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i} =0, \; \; ({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{b}}}}_{\rm t})_{i} \leqslant u_{i, \max} \\ {-\infty}, &({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i} =0, \; \; ({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{b}}}}_{\rm t})_{i} >u_{i, \max} \\ {\frac{-u_{i, \max} +({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{b}}}}_{\rm t})_{i}} {({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i}}}, &({{\mathit{\boldsymbol{A}}}}_{\rm t}^{+} {{\mathit{\boldsymbol{k}}}}_{\rm t})_{i} <0 \end{cases} \end{align} $

(36)

因此，比例因子的最大值可以取作$ ^{\rm s}a_{i} $的最小值，即

$ \begin{align} {}^{\rm s}a_{\max} =\min {}^{\rm s}a_{i}, \quad i=1, \cdots, N \end{align} $

(37)

$ ^{\rm s}a_{\max} $表示机器人系统输入约束所允许的期望任务方向上的最大幅值。

3.3 消旋规划

针对自旋目标卫星的消旋策略是在给定的约束条件下，寻找一条从初始状态至静止状态的最优路径。目标转动的初始状态$ \mathit{\boldsymbol{\phi}}_{\rm t}^{\rm s} $和$ \dot{\mathit{\boldsymbol{\phi}}}{}_{\rm t}^{\rm s} $可由星上传感器确定，消旋策略的目的是使$ \mathit{\boldsymbol{\omega}}_{\rm t}^{\rm f} =\dot{\mathit{\boldsymbol{\phi}}}{}_{\rm t}^{\rm f} ={{\boldsymbol{0}}} $以及$ \dot{\mathit{\boldsymbol{\omega}}}{}_{\rm t}^{\rm f} =\ddot{\mathit{\boldsymbol{\phi}}}{}_{\rm t}^{\rm f} ={{\boldsymbol{0}}} $，而终止时间和状态是自由的。

已知目标在某一时刻的运动状态，为了尽快消减目标自旋，可以将目标在该时刻的期望加速度方向（即期望任务方向）设定为速度的反方向，即：

$ \begin{align} \mathit{\boldsymbol{k}}_{\rm t} =-\frac{\dot{\mathit{\boldsymbol{x}}}_{\rm t}} {\| {\dot{\mathit{\boldsymbol{x}}}_{\rm t}} \|} \end{align} $

(38)

根据3.2节内容，在满足机器人系统输入约束式(20)的条件下，目标在期望任务方向上的最大加速度可通过比例因子法获得。为保证目标运动轨迹的连续性，并避免在接近稳定时出现运动振荡，这里令目标期望加速度幅值随速度大小成比例下降，于是目标期望加速度为

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm{td}} =\frac{\| {\dot{\mathit{\boldsymbol{x}}}_{\rm t}} \|}{\| {\dot{\mathit{\boldsymbol{x}}}_{\rm{ts}}} \|}{}^{\rm s}a_{i} \mathit{\boldsymbol{k}}_{\rm t} =-{}^{\rm s}a_{i} \frac{\dot{\mathit{\boldsymbol{x}}}_{\rm t}} {\| {\dot{\mathit{\boldsymbol{x}}}_{\rm{ts}}} \|} \end{align} $

(39)

根据式(36)~(39)所确定的消旋策略，能够在机器人系统输入约束下，充分利用机器人的操作能力，以实现目标的尽快消旋，同时保证目标运动轨迹连续。目标期望运动轨迹的获取无需任何优化算法，从而能够快速计算实现。已知目标的期望运动轨迹，空间机器人的期望加速度可通过逆运动学得到：

$ \begin{align} \ddot{\mathit{\boldsymbol{q}}}_{\rm d} =\mathit{\boldsymbol{J}}_{\rm e}^{+} (\mathit{\boldsymbol{J}}_{\rm t} \ddot{\mathit{\boldsymbol{x}}}_{\rm{td}} -\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}) \end{align} $

(40)

进一步地，通过积分可得到目标和机械臂的期望速度和构形。

4 柔顺控制（Compliant control）

目标精确的惯性参数通常很难在捕获前阶段确定。由于目标期望运动轨迹规划需要目标惯性参数的估值，如果直接跟踪这一轨迹，可能造成末端执行器施加的控制力过大而产生安全问题。当机械臂与外部环境或其他机械臂发生相互作用时，通过施加柔顺策略，顺应机器人系统的动力学特性，可以有效避免接触力和力矩过大。Hogen^[35]提出的阻抗控制可在目标与外部环境存在相互作用力或内力的情况下应用于协调操作。其基本思想是在末端执行器与目标连接的抓捕点处加入虚拟的机械阻抗（质量－弹簧－阻尼）。阻抗控制的优点是可以适当地设计末端执行器的阻抗参数以应对目标参数的不确定性。本文设计了一种具有内外双环结构的柔顺控制方案，以应对目标不确定性和内力的影响。在外环和内环分别建立阻抗关系式，对目标和末端执行器期望运动轨迹进行调整，最终得到机器人的柔顺运动轨迹。控制方法的结构如图 3所示。

4.1 载荷分配

由式(7)可知，作用于目标质心的合外力/力矩$ \mathit{\boldsymbol{f}}_{\rm t} $与末端接触力/力矩$ \mathit{\boldsymbol{f}}_{\rm e} $的关系为

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm t} =\mathit{\boldsymbol{J}}_{\rm t}^{\rm T} \mathit{\boldsymbol{f}}_{\rm e} \end{align} $

(41)

与单臂机器人不同，当多臂空间机器人的$ m $条臂同时作用于目标时，在目标雅可比矩阵的零空间内会产生对目标运动没有影响的内力。末端接触力由操作力和内力两部分组成：

$ \begin{align} \mathit{\boldsymbol{f}}_{\rm e} =(\mathit{\boldsymbol{J}}_{\rm t}^{\rm T})^{+}\mathit{\boldsymbol{f}}_{\rm t} +[{\mathit{\boldsymbol{E}}-(\mathit{\boldsymbol{J}}_{\rm t}^{\rm T})^{+}\mathit{\boldsymbol{J}}_{\rm t}^{\rm T}}]{\mathit{\boldsymbol{\xi }}}\end{align} $

(42)

其中$ \mathit{\boldsymbol{\xi}} $为与$ \mathit{\boldsymbol{f}}_{\rm e} $维度相同的任意向量，可取作$ \mathit{\boldsymbol{0}} $。$ (\mathit{\boldsymbol{J}}_{\rm t}^{\rm T})^{+} $的具体形式为

$ \begin{align} (\mathit{\boldsymbol{J}}_{\rm t}^{\rm T})^{+}={\mathit{\boldsymbol{\varDelta}}} \mathit{\boldsymbol{J}}_{\rm t} (\mathit{\boldsymbol{J}}_{\rm t}^{\rm T} {\mathit{\boldsymbol{\varDelta}}} \mathit{\boldsymbol{J}}_{\rm t})^{-1} \end{align} $

(43)

其中$ \mathit{\boldsymbol{\varDelta}} $为某一正定矩阵。

内力反映了机械臂与目标之间的拉扯和挤压，为了保证末端接触的安全，需要在控制空间机器人的过程中尽量减小内力。Walker等^[36-37]证明，通过适当选择$ \mathit{\boldsymbol{\varDelta}} $，可以消除操作力中产生拉扯或挤压的分量。这里取作：

$ \begin{align} \mathit{\boldsymbol{\varDelta}} = \begin{bmatrix} {\mathit{\boldsymbol{0}}_{3}} & {k_{1} \mathit{\boldsymbol{E}}_{3}} & & & & & \\ {k_{1} \mathit{\boldsymbol{E}}_{3}} & {\mathit{\boldsymbol{0}}_{3}} & & & & {\mathit{\boldsymbol{0}}} & \\ & & {\mathit{\boldsymbol{0}}_{3}} & {k_{2} \mathit{\boldsymbol{E}}_{3}} & & & \\ & & {k_{2} \mathit{\boldsymbol{E}}_{3}} & {\mathit{\boldsymbol{0}}_{3}} & & & \\ & & & & \ddots & & \\ & {\mathit{\boldsymbol{0}}} & & & & {\mathit{\boldsymbol{0}}_{3}} & {k_{m} \mathit{\boldsymbol{E}}_{3}} \\ & & & & & {k_{m} \mathit{\boldsymbol{E}}_{3}} & {\mathit{\boldsymbol{0}}_{3}} \end{bmatrix} \end{align} $

(44)

式中，$ k_{i} $（$ i=1, 2, \cdots, m $）为各机械臂的末端对目标施加的力/力矩在目标所受合外力/力矩中所占的权重。由此计算得到的末端期望接触力/力矩$ \mathit{\boldsymbol{f}}_{\rm{ed}} $以及目标期望合外力/力矩$ \mathit{\boldsymbol{f}}_{\rm{td}} $将作为柔顺控制的输入。

4.2 柔顺轨迹生成

为控制目标惯性参数不确定性对稳定过程的影响，需要根据受不确定性影响的目标合外力和力矩，对目标期望运动轨迹进行调整。利用目标实际合外力/力矩$ \mathit{\boldsymbol{f}}_{\rm t} $（由末端接触力/力矩的测量值经式(41)获得），构造调整目标期望运动轨迹的外环柔顺等式：

$ \begin{align} \mathit{\boldsymbol{M}}_{\rm t} δ \ddot{\mathit{\boldsymbol{x}}}_{\rm{tdc}} +\mathit{\boldsymbol{D}}_{\rm t} δ \dot{\mathit{\boldsymbol{x}}}_{\rm{tdc}} +\mathit{\boldsymbol{K}}_{\rm t} δ \mathit{\boldsymbol{x}}_{\rm{tdc}} =δ \mathit{\boldsymbol{f}}_{\rm t} \end{align} $

(45)

其中$ δ \mathit{\boldsymbol{x}}_{\rm{tdc}} =\mathit{\boldsymbol{x}}_{\rm{td}} -\mathit{\boldsymbol{x}}_{\rm{tc}} $，$ δ \mathit{\boldsymbol{f}}_{\rm t} =\mathit{\boldsymbol{f}}_{\rm{td}} -\mathit{\boldsymbol{f}}_{\rm t} $。$ \mathit{\boldsymbol{x}}_{\rm{tc}} $为经过调整后的目标柔顺运动轨迹，可通过下式的积分得到：

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm{tc}} =\ddot{\mathit{\boldsymbol{x}}}_{\rm{td}} +\mathit{\boldsymbol{M}}_{\rm t}^{-1} (\mathit{\boldsymbol{D}}_{\rm t} δ \dot{\mathit{\boldsymbol{x}}}_{\rm{tdc}} +\mathit{\boldsymbol{K}}_{\rm t} δ \mathit{\boldsymbol{x}}_{\rm{tdc}} -δ \mathit{\boldsymbol{f}}_{\rm t}) \end{align} $

(46)

上文所述的$ \mathit{\boldsymbol{f}}_{\rm{td}} $为利用目标动力学方程(7)和期望运动轨迹得到的目标期望合外力/力矩。$ \mathit{\boldsymbol{M}}_{\rm t} $、$ \mathit{\boldsymbol{D}}_{\rm t} $和$ \mathit{\boldsymbol{K}}_{\rm t} $为对称正定矩阵。由于$ \mathit{\boldsymbol{x}}_{\rm{tc}} $与$ \mathit{\boldsymbol{f}}_{\rm t} $直接满足上述柔顺等式，因此当目标以$ \mathit{\boldsymbol{x}}_{\rm{tc}} $对应的轨迹运动时，受目标不确定性影响的末端接触力和力矩会产生相应的调整。

为控制内力对末端接触的影响，需要根据末端期望力和力矩，对经外环调整后得到的目标柔顺运动进行进一步的调整。根据运动约束(12)，机械臂末端的期望运动为

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm{ed}} =\mathit{\boldsymbol{J}}_{\rm t} \ddot{\mathit{\boldsymbol{x}}}_{\rm{tc}} \end{align} $

(47)

利用末端接触力/力矩$ \mathit{\boldsymbol{f}}_{\rm e} $的测量值，构造调整末端期望运动轨迹的内环柔顺等式：

$ \begin{align} \mathit{\boldsymbol{M}}_{\rm e} δ \ddot{\mathit{\boldsymbol{x}}}_{\rm{edc}} +\mathit{\boldsymbol{D}}_{\rm e} δ \dot{\mathit{\boldsymbol{x}}}_{\rm{edc}} +\mathit{\boldsymbol{K}}_{\rm e} δ \mathit{\boldsymbol{x}}_{\rm{edc}} =δ \mathit{\boldsymbol{f}}_{\rm e} \end{align} $

(48)

其中$ δ \mathit{\boldsymbol{x}}_{\rm{edc}} =\mathit{\boldsymbol{x}}_{\rm{ed}} -\mathit{\boldsymbol{x}}_{\rm{ec}} $，$ δ \mathit{\boldsymbol{f}}_{\rm e} =\mathit{\boldsymbol{f}}_{\rm{ed}} -\mathit{\boldsymbol{f}}_{\rm e} $。$ \mathit{\boldsymbol{x}}_{\rm{ec}} $为经过调整后的末端柔顺运动轨迹，可通过下式的积分得到：

$ \begin{align} \ddot{\mathit{\boldsymbol{x}}}_{\rm{ec}} =\ddot{\mathit{\boldsymbol{x}}}_{\rm{ed}} +\mathit{\boldsymbol{M}}_{\rm e}^{-1} (\mathit{\boldsymbol{D}}_{\rm e} δ \dot{\mathit{\boldsymbol{x}}}_{\rm{edc}} +\mathit{\boldsymbol{K}}_{\rm e} δ \mathit{\boldsymbol{x}}_{\rm{edc}} -δ \mathit{\boldsymbol{f}}_{\rm e}) \end{align} $

(49)

上文所述的$ \mathit{\boldsymbol{f}}_{\rm{ed}} $可根据式(42)~(44)所制定的载荷分配方案由$ \mathit{\boldsymbol{f}}_{\rm{td}} $得到。类似地，由于$ \mathit{\boldsymbol{x}}_{\rm{ec}} $与$ \mathit{\boldsymbol{f}}_{\rm e} $直接满足上述的柔顺等式，因此当末端沿轨迹$ \mathit{\boldsymbol{x}}_{\rm{ec}} $运动时，受内力影响的末端接触力和力矩将会产生相应的调整。

4.3 跟踪控制

机器人期望运动轨迹可通过下式的积分得到：

$ \begin{align} \ddot{\mathit{\boldsymbol{q}}}_{\rm d} =\mathit{\boldsymbol{J}}_{\rm e}^{+} (\ddot{\mathit{\boldsymbol{x}}}_{\rm{ec}} -\dot{\mathit{\boldsymbol{J}}}_{\rm e} \dot{\mathit{\boldsymbol{q}}}) \end{align} $

(50)

为利用空间机器人协调地镇定被抓捕的非合作目标，基于空间机器人动力学方程(1)，设计如下PD（比例－微分）控制器以跟踪期望运动轨迹$ \mathit{\boldsymbol{q}}_{\rm d} $：

$ \begin{align} \mathit{\boldsymbol{u}}=\mathit{\boldsymbol{H}}[{\ddot{\mathit{\boldsymbol{q}}}_{\rm d} +\mathit{\boldsymbol{M}}_{\rm c}^{-1} (\mathit{\boldsymbol{D}}_{\rm c} δ \dot{\mathit{\boldsymbol{q}}}+\mathit{\boldsymbol{K}}_{\rm c} δ \mathit{\boldsymbol{q}})} ]+\mathit{\boldsymbol{c}}+\mathit{\boldsymbol{J}}_{\rm e}^{\rm T} \mathit{\boldsymbol{f}}_{\rm e} \end{align} $

(51)

其中$ δ \mathit{\boldsymbol{q}}=\mathit{\boldsymbol{q}}_{\rm d} -\mathit{\boldsymbol{q}} $。$ \mathit{\boldsymbol{M}}_{\rm c} $、$ \mathit{\boldsymbol{D}}_{\rm c} $和$ \mathit{\boldsymbol{K}}_{\rm c} $为PD控制器的控制参数。将式(51)代入式(1)，可得：

$ \begin{align} \mathit{\boldsymbol{M}}_{\rm c} δ \ddot{\mathit{\boldsymbol{q}}}+\mathit{\boldsymbol{D}}_{\rm c} δ \dot{\mathit{\boldsymbol{q}}}+\mathit{\boldsymbol{K}}_{\rm c} δ \mathit{\boldsymbol{q}}={{\boldsymbol{0}}} \end{align} $

(52)

经过柔顺等式(45)(48)的调整，最终得到的期望运动轨迹$ \mathit{\boldsymbol{q}}_{\rm d} $考虑了目标不确定性以及内力的影响。通过跟踪该轨迹，空间机器人能够同时有针对性地控制内力和目标不确定性对末端接触所产生的影响，在机械臂末端与目标的交互过程中实现柔顺接触。

5 仿真结果（Simulation results）

本节通过双臂空间机器人捕获自旋目标的案例来验证所提出的消旋策略。如图 2所示，基座具有6个自由度，2只抓捕机械臂都具有7个自由度。空间机器人的运动学与动力学参数如表 2所示，其中$ \mathit{\boldsymbol{a}}_{i} $、$ \mathit{\boldsymbol{b}}_{i} $和$ \mathit{\boldsymbol{I}}_{i} $是在与其固联的本体坐标系中进行表示的。

如表 3所示，在消旋规划和控制中，引入目标惯性参数的不确定性，以验证所提的消旋策略在此情况下的有效性。在抓捕前仅通过测量目标卫星的旋转运动是无法辨识出其惯性参数的，但可以获得目标的无量纲惯性参数。在此基础上，目标参数的估计值可通过$ {\rm tr}\; \mathit{\boldsymbol{\bar{I}}}_{\rm t} $获得。机器人基座与机械臂关节输入需满足约束$ | {u_{i}} |\leqslant u_{i, \max} $。根据式(16)(17)，较大的目标惯性参数估计值会降低机器人系统输入对目标的动力学可操作度，从而增加目标的消旋时间。因此，在目标惯性参数尚不确定的情况下，将其估计值设计得较为保守是比较合理的。

本文针对捕获后的组合体系统，设计了消旋策略及柔顺控制方法，以消除目标的自旋运动并管理接触力和力矩。目标上的2个抓捕点相对于目标质心坐标系的位置分别为$ \mathit{\boldsymbol{r}}_{\rm g} =[0, \pm 0.1006, 0] $ m。基座力和力矩输入限制为$ | {f_{{\rm b}, i}} |\leqslant 10 $ N和$ | {n_{{\rm b}, i}} |\leqslant 10 $ N$ \cdot $m，关节扭矩输入限制为$ | {{\mathit{\boldsymbol{\tau}}}_{i}} |\leqslant 5 $ N$ \cdot $m。期望接触力和力矩的权重为$ k_{1} =k_{2} =0.5 $。在捕获瞬间（即消旋运动开始的时刻），目标相对于惯性坐标系的运动状态为

$ \begin{equation} \begin{aligned} \mathit{\boldsymbol{\phi}}_{\rm t}^{\rm s} & =[0, 0, 0]^{\circ} \\ \dot{\mathit{\boldsymbol{\phi}}}{}_{\rm t}^{\rm s} & =[3, -8, 4]^{\circ}\rm{/s} \\ {\mathit{\boldsymbol{r}}}_{\rm t}^{\rm s} & =[3.5561, 0, 0.1680] \rm{m} \\ \mathit{\boldsymbol{\upsilon}}_{\rm t}^{\rm s} & =[0, 0, 0] \rm{m/s} \end{aligned} \end{equation} $

(53)

空间机器人的基座与机械臂的运动状态为

$ \begin{equation} \begin{aligned} \mathit{\boldsymbol{\phi}}_{\rm b}^{\rm s} =\;& [0, 0, 0]^{\circ} \\ \dot{\mathit{\boldsymbol{\phi}}}{}_{\rm b}^{\rm s} =\;& [2.8495, 1.0630, 0.1649]^{\circ}\rm{/s} \\ {\mathit{\boldsymbol{r}}}_{\rm b}^{\rm s} =\;& [0, 0, 0] \rm{m} \\ \mathit{\boldsymbol{\upsilon}}_{\rm b}^{\rm s} =\;& [0.0142, -0.0479, -0.0414] \rm{m/s} \\ \mathit{\boldsymbol{\theta}}^{\rm{as}}=\;& [0, -30, 0, -30, 0, -45, 0]^{\circ} \\ \mathit{\boldsymbol{\theta}}^{\rm{bs}}=\;& [0, +30, 0, +30, 0, +45, 0]^{\circ} \\ \dot{\mathit{\boldsymbol{\theta}}}^{\rm{as}}=\, & [2.3003, -2.5690, 2, 4870, -1.8419, \\ & -3.5160, 3.1081, -1.5457]^{\circ}\rm{/s} \\ \dot{\mathit{\boldsymbol{\theta}}}^{\rm{bs}}=\, & [-2.6172, -1.4544, -2.3229, -0.7601, \\ & 3.1222, 3.1408, 1.8554]^{\circ}\rm{/s} \end{aligned} \end{equation} $

(54)

外环柔顺等式、内环柔顺等式和运动跟踪控制的惯性、阻尼和刚度矩阵的选择如表 4所示。

图 4~图 6给出了标准目标参数下的仿真结果。图 4展示了目标的角运动（欧拉角、角速度和角加速度）和线运动（位置、线速度和线加速度），其中红线表示根据本文提出的消旋策略规划的期望运动轨迹，黑线表示采用本文设计的柔顺控制得到的实际运动轨迹。以$ \| {\mathit{\boldsymbol{\omega}}_{\rm t}} \|_{\infty} \leqslant 10^{-3} $ rad/s为标准，目标的期望运动用时约6.1 s实现消旋稳定，稳定姿态欧拉角为$ \mathit{\boldsymbol{\phi}}_{\rm t}^{\rm f} =[0.074, -0.172, 0.094] $ rad。在本文设计的柔顺控制方案下，目标的$ \alpha $和$ \beta $角很快跟踪上期望值，$ \gamma $角由开始时的振荡逐渐收敛至期望值，用约12.9 s实现消旋稳定，最终实现对期望状态的精确跟踪。此过程中，目标位置基本保持稳定，其线速度大小限制于$ 8\times 10^{-3} $ m/s以下。图 5展示了空间机器人的机械臂末端接触力矩和力，红线和黑线分别为由载荷分配得到的期望值和实际值。由于预计目标仅做角运动，其期望接触力为0，接触力矩与加速度成正比。在实际情况下，由于运动约束的影响，难以同时实现对期望运动轨迹和期望力/力矩的精确跟踪。末端接触力矩和力在开始时出现偏离期望值的较大值（最大值分别为0.96 N$ \cdot $m和1.52 N），而后随着目标消旋稳定逐步收敛至0附近。图 6展示了仿真过程中基座力矩和力输入分量以及各关节扭矩输入的最大值（黑线）与其输入限制（红线）。开始时基座力矩和力输入达到或接近限制值，后续输入随着目标稳定而逐渐减小。这样，在满足机器人系统力/力矩输入约束以及运动轨迹连续性的前提下，在最短时间内实现了对目标的镇定消旋，并避免了末端接触力和力矩过大。

图 7~图 9给出了目标参数估计存在20%偏差时的仿真结果。由于对目标惯性参数作了较大的估计，因此目标期望加速度有所降低，消旋时间有所延长。目标的期望运动用时约6.2 s实现消旋稳定，稳定姿态欧拉角增大至$ \mathit{\boldsymbol{\phi}}_{\rm t}^{\rm f} =[0.077, -0.178, $ $ 0.098] $ rad。在目标参数估计存在偏差的情况下，本文的柔顺控制方案依然能使目标运动轨迹收敛至期望值，用时约13.0 s实现消旋稳定。末端接触力矩和力的最大值分别为0.94 N$ \cdot $m和1.49 N，而后逐步收敛至0附近。类似地，机器人系统输入由开始时的达到或接近限制值，随着目标稳定而逐渐减小。

图 10~图 12给出了目标参数估计存在50%偏差时的仿真结果。因为选择了较大的目标惯性参数估计，目标的期望运动用时约6.7 s实现消旋稳定，稳定姿态欧拉角进一步增大至$ \mathit{\boldsymbol{\phi}}_{\rm t}^{\rm f} =[0.082, -0.189, $ $ 0.105] $ rad。目标实际运动用时约13.1 s实现消旋稳定，并能够使目标运动轨迹收敛至期望值。末端接触力矩和力的最大值分别为0.91 N$ \cdot $m和1.46 N，而后逐步收敛至0附近。机器人系统同样满足输入限制要求。综上所述，即使目标参数具有不确定性，所提的消旋策略和柔顺控制方法依然可以完成目标消旋与稳定控制的任务。

6 结论（Conclusion）

在空间机器人捕获非合作目标后的阶段，为使自旋的目标卫星尽快稳定，考虑机器人系统的输入限制，本文提出了一种基于任务相容性的消旋策略和柔顺控制方法。主要工作总结如下：

1) 基于任务相容性理论，提出了一种充分利用空间机器人操作能力的消旋策略。目标的期望加速度的方向取作其速度的反方向，其大小根据比例因子法确定，从而在机器人系统力和力矩输入限制内实现最速消旋，且无需任何优化算法。

2) 为应对目标不确定性和内力的影响，设计了一种内外双环式柔顺控制方法，在外环和内环分别建立阻抗关系式，对目标和末端执行器的期望运动轨迹进行调整，最终得到机器人的柔顺运动轨迹。

仿真结果证明了消旋策略及柔顺控制的可行性与有效性，并展示了该方法能够在目标惯性参数估计存在较大偏差的情况下消除目标自旋，并避免末端接触力和力矩过大。对目标惯性参数的精确辨识与空间操作的自适应控制问题将成为未来研究工作的重点。