一种利用机械臂抑制航天器基座姿态干扰的方法与流程

本发明属于空间机器人轨迹规划技术领域；具体涉及一种利用机械臂抑制航天器基座姿态干扰的方法。

背景技术：

迄今为止，仍未实现完全自主的空间在轨服务任务，例如卫星维修、在轨组装、碎片清除等。由于空间机械臂具有多功能性、可扩展性等特性，成为一种实现在轨服务的优异手段。对于执行在轨服务任务的航天器，不仅仅具备机械臂等转动机构，还具备天线、太阳帆板等转动机构。因此，在轨服务航天器是一个复杂的多体系统。当天线、太阳帆板转动时，不可避免的对航天器本体造成姿态干扰，利用机械臂的运动消除这些姿态干扰非常重要，因为姿态干扰会影响相对敏感器的测量，亦可节省星上宝贵的燃料资源。因此，如何设计一种利用机械臂抑制航天器基座姿态干扰，具有重大的研究意义与实用价值。

空间机械臂本身作为一个运行在微重力环境中的多输入、多输出、非线性、强耦合的复杂系统，其运动规划问题起源于计算几何学的相关研究课题。文献中对此已有一系列相关研究，Yoshida等学者提出了零反作用空间的概念并在ETS-VII项目中进行了飞行验证，特别是针对运动冗余机械臂，在此空间内设计的机械臂轨迹可以最小化基座姿态干扰并移除操作过程中的关节速度约束。徐文福等人针对自由漂浮空间机器人提出了速度级逆运动学方程的方法，用于实现机械臂末端执行器连续位姿跟踪、基座姿态调整等任务。Kaigom等采用粒子群算法搜索参数化空间构建了基座姿态最小干扰的空间机械臂轨迹。基于约束最小二乘方法，Cocuzza等设计了机械臂轨迹跟踪过程中航天器局部最小动力学干扰的控制方法。王从庆等针对空间自由漂浮双臂机器人提出一种鲁棒协调控制方法，并保证内力跟踪误差的有界性。王明等提出一种基于机械臂耦合力矩评估的组合航天器姿态协调控制方法。

上述有关空间自由漂浮机器人的运动规划方法虽然能够完成最小基座姿态干扰的任务目标，但是一般仅考虑航天器带有单个机械臂或者两个完全相同的机械臂。然而，如上所述，航天器本体上可以安装有多个转动机构。每个转动机构的构型、质量分布特性不尽相同，这种情况下，如何协调不同转动机构间的运动十分重要。

技术实现要素：

本发明提供了一种利用机械臂抑制航天器基座姿态干扰的方法；结合动态平衡控制和零反作用空间的概念，提出了应用运动学冗余机械臂抑制其他转动机构引起的航天器基座姿态干扰的新方法。

本发明的技术方案是：一种利用机械臂抑制航天器基座姿态干扰的方法，包括以下步骤：

步骤1，建立空间机器人系统的动力学模型：

其中H_b,H_m,H_a分别表示基座、机械臂和天线的惯量矩阵；H_bm,H_ba分别表示机械臂与基座，天线与基座之间的动力学耦合矩阵；c_b,c_m,c_a分别表示基座、机械臂、天线的科里奥利力和离心力；τ_m,τ_a分别表示机械臂、天线关节电机的控制力矩；f_b,f_e分别表示作用于基座与机械臂末端执行器的广义外力；J_b,J_e为基座和末端执行器的Jacobian矩阵依据角动量守恒定律得到空间机器人的总动量为：其中I_s∈R^3×3为空间机器人的惯量矩阵,I_bm∈R^3×n和I_ba∈R^3×2为机械臂与基座、天线与基座的耦合惯量矩阵；

步骤2，设计动态平衡状态下的机械臂关节轨迹，其中包括最小化姿态干扰的任务和同时考虑末端执行器的任务以及最小化基座干扰的任务两种情形；

步骤3，设计机械臂关节速度的闭环逆运动学控制为：

其中和分别表示末端执行器的位置和姿态偏差，为一个正定控制增益矩阵，δx_e为跟踪误差，其中K_P的各个元素值越大，δx_e的范数越小；并且根据步骤2中最小化姿态干扰的任务和同时考虑末端执行器的任务以及最小化基座干扰的任务两种不同的情形，通过上述闭环逆运动学控制设计机械臂关节轨迹。

更进一步的，本发明的特点还在于：

其中步骤2中在最小化姿态干扰的任务的情形下机械臂关节轨迹为：其中是I_bm的零空间映射。

其中步骤3中最小化姿态干扰的任务结合闭环逆运动学控制设计机械臂关节轨迹为：

其中步骤2中同时考虑末端执行器的任务以及最小化基座干扰的任务的情形下，且最小化基座干扰的任务为主任务时，机械臂关节轨迹为：

其中

其中步骤3中在同时考虑末端执行器的任务以及最小化基座干扰的任务的情形下，且最小化基座干扰的任务为主任务时，结合闭环逆运动学控制设计机械臂关节轨迹为：

其中步骤2中同时考虑末端执行器的任务以及最小化基座干扰的任务的情形下，且机械臂末端执行器的任务为主任务时，机械臂关节轨迹为：

其中步骤3中同时考虑末端执行器的任务以及最小化基座干扰的任务的情形下，且机械臂末端执行器的任务为主任务时，结合闭环逆运动学控制设计机械臂关节轨迹为：

与现有技术相比，本发明的有益效果是：通过结合动态平衡控制和零反作用空间，针对星载转动天线对航天器基座的姿态干扰，提出了空间自由漂浮运动学冗余机械臂的运动规划方法，实现运动学冗余机械臂在空间环境下的关节轨迹规划，生成机械臂各关节的轨迹，进而实现对航天器基座姿态干扰的抑制；在步骤3中通过选择正定控制增益矩阵，并且通过Lyapunov函数分析使机械臂关节轨迹的指数稳定，从而解决了数据漂移的问题；同时本发明实现了根据任务优先级的不同分别设计机械臂关节轨迹，并且进而根据不同的任务与闭环逆运动学控制结合得到机械臂关节轨迹。

更进一步的，在最小化姿态干扰的任务，即在单任务的情形，在此任务情形下建立机械臂关节轨迹和机械臂关节运动轨迹。

更进一步的，在同时考虑末端执行器的任务以及最小化基座干扰的任务，即在多任务的情形，考虑了机械臂末端执行器的任务为主任务的情形下建立机械臂关节轨迹和机械臂关节运动轨迹。

附图说明

图1为本发明的闭环逆运动学控制原理图。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案进一步说明。

空间机器人系统由多个单自由度关节构成的机械臂系统、2自由度转动天线以及搭载机械臂系统的基座组成，假设机械臂共有n个关节，那么空间机器人系统共有n+3个可以运动的部件，由于基座的运动自由度为6，因此，整个系统的自由度为n+8。对于空间机器人系统，为了减少能量消耗，在机械臂运动过程中，不启用基座的姿轨控系统，因此整个系统是一个自由漂浮系统，且满足动量守恒定律。

据此本发明的基本步骤包括：首先，根据空间机器人的构型建立其多体动力学模型，并在此基础上分析机械臂、天线与基座的动量守恒问题；其次，考虑任务优先级，基于动态均衡控制和零反作用空间，设计空间机械臂的轨迹用于协调空间机械臂末端执行器的任务与抑制卫星基座姿态干扰；最后，基于上述分析，设计空间机械臂闭环逆运动学控制律，解决基座姿态抑制中由于误差积累逐渐增大的问题。

本发明利用机械臂抑制航天器基座姿态干扰的方法，包括以下步骤：

步骤1，空间机器人系统包括n+3个运动部件，其中3为机械臂、天线与基座，n为可增加的其他运动部件，n可以为0；参考Lagrange力学体系，空间机器人系统的动力学方程可以表示如下：

其中，H_b,H_m,H_a分别表示基座、机械臂、天线的惯量矩阵；H_bm,H_ba表示机械臂与基座，天线与基座之间的动力学耦合矩阵，c_b,c_m,c_a表示基座、机械臂、天线的科里奥利力和离心力；τ_m,τ_a表示机械臂、天线关节电机的控制力矩；f_b,f_e分别表示作用于基座与机械臂末端执行器的广义外力；J_b,J_e为基座和末端执行器的Jacobian矩阵。当没有外力作用在末端执行器f_e＝0，也没有外力作用在基座上时f_b＝0，整个系统被称为空间自由漂浮机器人。根据动量守恒定律，空间机器人系统的角动量可以表示如下：

其中I_s∈R^3×3为空间机器人的惯量矩阵,I_bm∈R^3×n和I_ba∈R^3×2为机械臂与基座、天线与基座的耦合惯量矩阵。由于存在耦合作用，使得天线转动时会对基座位姿造成干扰，机械臂的运动过程也会对基座位姿产生干扰。

一般而言，姿态的稳定更为重要，因此，如何利用系统的动力学耦合效应，抑制基座姿态的干扰，对于节省星上燃料、维持基座的稳定指向具有重要意义。

考虑机械臂的前向运动学方程该方程构建了机械臂末端执行器的速度与机械臂各关节角速度间的联系。对于描述最小化基座姿态干扰的一种有效手段是采用基座旋转角速度的范数。因此，问题可以转化为找到合适的机械臂关节轨迹来完成机械臂末端的任务，同时抑制基座的姿态干扰。该问题表述如下：

其中第二个等式约束描述了空间机器人系统的角动量守恒。因此在第二步中，充分利用系统的角动量守恒定律，设计机械臂关节轨迹以抑制空间机构转动以及机械臂运动对基座造成的姿态干扰。

步骤2，航天器上转动机构(如卫星指向机构)的运动，导致了整个空间机器人系统的相互动力学干扰。为了稳定基座姿态，拓展动态平衡控制的应用。关于动态平衡控制，考虑以下3个假设：1.星载姿态控制系统在姿态干扰抑制中全程关闭；2.机械臂与天线的控制执行机构完全独立；3.由于天线的转动可以预先计算，因此天线对基座的影响可以经计算获得。

首先考虑最小化姿态干扰的任务，也就是单任务的情形。由于I_s可逆，自由漂浮状态下由机械臂运动与天线转动引起基座的转动角速度ω_b可以由下式计算：

给定天线初始指向以及航天器与通信卫星之间的相对轨道关系，那么可以设计天线指向机构各关节的轨迹。因此，如果设计合理的机械臂关节轨迹，就可以动态地稳定基座姿态。考虑耦合惯量矩阵I_bm的伪逆与零空间，动态平衡的机械臂关节轨迹如下：的零空间映射，称之为零反作用空间，意味着机械臂关节在零反作用空间中的运动不会对基座产生干扰。该概念对机械臂关节轨迹规划具有重要意义。

另外，同时考虑末端执行器任务以及最小化基座干扰任务，需要对两个任务根据优先级的排序从而确定机械臂关节轨迹，当最小化基座干扰任务为主任务时，机械臂关节轨迹为：其中相应的如果末端执行器任务为主任务时，机械臂关节轨迹为：其中

步骤3，步骤2中介绍了3种规划机械臂关节轨迹的情形。为规划离散系统下的机械臂关节轨迹，需要采用适当的积分算法来计算θ(t)，传统的积分算法包括Euler、Runge-Kutta和Dormand-Prince等算法。本方案提供了一种闭环逆运动学控制算法，通过控制期望值与当前的任务变量之间的差值，以消除数值漂移问题，具体为：利用四元数用于描述末端执行器的姿态，和定义为期望的末端执行器任务，考虑到机械臂关节速度的闭环逆运动学控制表示为：其中分别代表末端执行器的位置与姿态偏差。是一个正定控制增益矩阵，基于Lyapunov函数分析保证上式中机械臂关节速度的闭环逆运动学控制指数稳定，跟踪误差δx_e依赖于增益矩阵K_P的选择，K_P的各个元素值越大，δx_e的范数就越小，解决了数据漂移的问题。

重新考虑动态平衡控制，应用闭环逆运动学控制求解最小化基座干扰，即单任务情形下的机械臂关节轨迹可以表示为：

同时考虑末端执行器的任务以及最小化基座干扰的任务的情形下，最小化基座干扰的任务为主任务时，结合闭环逆运动学控制设计机械臂关节轨迹为：

机械臂末端执行器任务为主任务时，结合闭环逆运动学控制设计机械臂关节轨迹为：

如图1所示，本发明中，空间机械臂关节轨迹设计主要由三部分组成：一是计算通信卫星与服务航天器间的相对轨道关系，从而计算服务航天器上卫星天线调向所需的运动指令，得到卫星天线调向关节的轨迹，即可获得卫星天线转动对基座的干扰；二是基于动态平衡控制的高优先级任务的逆运动学控制算法，主要考虑针对单个任务的情形，如何设计机械臂关节轨迹从而消除卫星天线转动对基座的姿态干扰；三是在零反作用空间利用逆运动学控制设计空间机械臂的关节轨迹，这部分涉及到空间机械臂的正向运动学计算，用于完成次优先级的任务。综合高优先级的任务与次优先级的任务生成的机械臂关节轨迹，可以得到空间运动学冗余机械臂关节轨迹，进而实现对基座姿态干扰的抑制和末端执行器位姿的跟踪。

仅考虑单项任务时，利用动态平衡控制与闭环逆运动学控制方法；当存在多项任务时，根据任务的优先级采用闭环逆运动学控制生成机械臂关节轨迹。

本发明的技术方案能够实现利用运动学冗余机械臂在空间自由漂浮模式下抑制基座姿态干扰的问题，特别是由基座上其他转动机构引起的基座姿态干扰，并且针对不同的任务优先级设计相应的关节轨迹。下面在两种仿真场景下，采用本发明的技术方案，针对7自由度机械臂关节轨迹设计效果分别如下：

1.仅抑制基座姿态作为执行任务的情形。卫星天线的初始位置为φ⁰＝(0,0)，希望天线在8s内能够达到期望位置φ^f＝(π,π/2)，空间运动学冗余机械臂的初始位置为：其中基座的初始姿态与惯性坐标系重合。首先，不抑制基座的姿态干扰时，由于卫星天线的转动，当天线转动至期望位置后，基座姿态在三轴上的干扰分别为(ZYX欧拉角)-0.45、-0.82、0.36(deg)；其次，采用本发明设计的基于动态平衡控制(无闭环逆运动学控制)的机械臂关节轨迹，得到的基座姿态在三轴上的干扰分别为(ZYX欧拉角)0.058、0.052、-0.09(deg)，可以看出，采用动态平衡控制已经很好地抑制了卫星天线转动对基座造成的姿态干扰；最后，结合动态平衡控制与闭环逆运动学控制，设计了新的机械臂关节轨迹，得到的基座姿态在三轴上的干扰分别为(ZYX欧拉角)-0.0014、0.002、-0.0012(deg)，可以看出，当利用闭环逆运动学控制后，由于积分截断误差引起的数值漂移问题也得到了很好的消除。

2.要求末端执行器跟踪路径同时抑制基座姿态干扰的情形。该情形与上述1中的机械臂相同，且基座初始构型也相同，并要求卫星天线指向通信卫星，但同时考虑保持末端执行器的方位保持不变。这主要是为了保持住末端执行器的方位，便于利用末端执行器上的探测设备对目标进行观测。本仿真中要求抑制基座姿态干扰作为主要任务。由于存在多个任务，采用本发明中提出的基于任务优先级的机械臂关节轨迹设计。对仿真2中的双任务情形分别采用本发明提出的动态平衡控制与闭环逆运动学控制，对基座姿态的干扰抑制结果与仿真1完全一致，这是由于保持末端执行器的方位作为次优先级任务，因此该任务的机械臂关节轨迹是在零反作用空间中进行设计，也就是说，完成次优先级任务的机械臂关节运动对于基座的姿态不产生干扰。尽机械臂关节能力的最大可能保持机械臂末端执行器方位的次优先级任务。不采用闭环逆运动学控制时，末端执行器的方位偏差(四元数表示)为0.0006、0.0005、-0.0008，采用闭环逆运动学控制后，末端执行器的方位偏差(四元数表示)为0.003、-0.0005、0.03，可以看出次优先级任务的误差有所增大，这是因为闭环逆运动学控制要优先满足高优先级任务的完成。

可以看出，针对所述的两种仿真场景，采用本发明设计的机械臂关节运动方案，可以同步抑制卫星天线转动对基座姿态造成的影响，并且抑制的效果显著，特别是当采用闭环逆运动学控制方式时，由于保证了系统的闭环稳定性，使得最终的基座姿态干扰抑制效果显著。另外，针对任务个数不同，采用基于任务优先级的策略，分别设计用于动态平衡控制以及在零反作用空间中的关节轨迹，所设计的机械臂关节轨迹可以完成针对多个任务的情形，并对高优先级的任务优先完成，由此说明了本发明所给出的关节轨迹规划方案对于空间复杂机械臂执行空间任务的有效性，同时验证了本发明所提轨迹规划的可行性。

通过上述仿真分别验证了针对单个任务、不同优先级的多个任务采用动态平衡控制以及逆运动学控制的方案。下面我们通过本技术方案在卫星天线展开的整个流程中的运用进一步说明。在初始状态下，天线处于折叠状态下，机械臂处于略微展开状态；当命令要求天线调向机构驱动电机展开天线，并使其指向通信卫星；在天线展开的过程中，机械臂的各关节按照本发明所设计的轨迹进行调节，抑制由卫星天线转动对基座造成的姿态干扰。在卫星天线展开的整个流程中，卫星天线转动时，机械臂利用本发明所设计的轨迹成功地实现了对基座姿态干扰的抑制，本发明对于实现星载燃料节省以及后续的多机械臂协同运动具有重要的参考价值与应用意义。