CN106055810A - 用于在轨快速抓捕的姿轨臂一体化运动规划方法 - Google Patents

Abstract

本发明属于航天器运动规划领域，涉及一种用于在轨快速抓捕的姿轨臂一体化运动规划方法，本方法具有抓捕操作用时少、抓捕操作安全性好、求解趋于全局最优等优点。该方法首次提出对空间机器人的飞越式逼近目标过程与机械臂目标抓捕过程进行一体化规划，实现了区别于传统“停靠‑抓捕”方式的飞跃式动态抓捕。该方法首先提出了姿轨臂一体化运动规划的模型，主要是确立了空间机器人飞越接近和机械臂抓捕过程的约束条件模型；其次，针对设计变量的离散化问题，提出了基于切比雪夫多项式的离散方法；最后，针对一般优化算法的陷入局部最优的问题，提出了适用于该规划模型的混沌差分进化算法。

Description

用于在轨快速抓捕的姿轨臂一体化运动规划方法

技术领域

本发明涉及航天器系统动力学及控制技术领域，具体说涉及一种用于在轨快速抓捕的姿轨臂一体化运动规划方法。

背景技术

空间机器人在轨抓捕通常的做法是，先交会接近到停靠点然后再实施抓捕操作，这种方式将在轨抓捕过程分为交会和抓捕两个独立的阶段。对交会阶段，目前比较成熟的方式为沿V-轴或R-轴的准直线受迫运动，经实际应用证明对受控稳定目标是适合的。然而，对于更一般的在轨抓捕任务，目标通常是非合作的，抓捕点可能在任意方向而非V-轴或R-轴方向。对此类目标，直线交会策略难以满足任务要求。对抓捕阶段，目前大量的研究成果大都基于初始相对静止的假设，将抓捕规划问题转化为笛卡尔坐标系下定点转移的机械臂运动规划，而没有考虑其轨道动力学背景。况且，这种先停靠再抓捕的方式实施周期较长，不能满足某些快速在轨服务任务的需求。

文献(S.Matsumoto,S.Dubowsky,S.Jacobsen,Y.Ohkami.Fly-By Approach andGuidance for Uncontrolled Rotating Satellite Capture[C].AIAA Guidance,Navigation,and Control Conference and Exhibit.Austin,USA,2003:1-10.)提出一种服从轨道动力学的无碰撞接近策略，利用轨道相对运动规律，使空间机器人以自由漂浮模式飞越接近目标，若捕获失败又能够沿安全路径逃离目标。基于该接近策略若使空间机器人在相对运动的同时开展机械臂抓捕操作，不仅能够适用于非合作目标抓捕，而且可以实现更快速的在轨抓捕。但文献中仅研究了空间机器人与目标质心交会接近问题，没有考虑机械臂抓捕运动规划问题。对于飞越式逼近方式，抓捕时刻空间机器人相对目标体存在相对运动，不满足相对静止的条件，并且一旦进入飞越段的初始条件确定，空间机器人的逼近轨迹即严格确定，而空间机器人与目标的最近距离、交会速度以及可抓捕时间对机械臂能否成功抓捕目标有决定性影响。

发明内容

为解决空间机器人服从轨道动力学飞越式接近空间目标过程中的姿臂规划问题，本发明综合考虑接近运动与机械臂抓捕操作，提供一种用于在轨快速抓捕的姿轨臂一体化运动规划方法。

本发明的技术方案是：

本发明中空间目标主要适用于重力梯度稳定卫星和慢速自旋周期约为20s～360s的自旋卫星。

空间机器人的快速逼近策略如下：空间机器人相对目标在最终逼近阶段的运动过程，采取飞越式逼近的快速逼近方式，即空间机器人从相距目标几百米的位置开始转移机动，沿服从轨道动力学的无碰撞路径不施加轨道控制而以合适的速度接近目标位置，至捕获区域内实施机械臂抓捕操作。若接近过程中出现意外故障，则可以沿着一条安全轨迹逃离目标而不施加有源控制。飞越式逼近过程包括轨迹调整段、飞越段、捕获段和逃离段。其中轨迹调整段是指空间机器人由初始位置开始经机动到达设定的位置和速度条件，至轨道控制系统关机为止的飞行段；飞越段指空间机器人轨道控制系统关机至目标进入机械臂工作范围之前的飞行段；捕获段指目标进入机械臂的工作范围后空间机器人的飞行段；逃离段是指空间机器人在捕获段的抓捕操作未成功或飞行中出现意外故障而转入的飞行段。

空间机器人的快速抓捕策略如下：空间机器人完成轨迹调整后，相距目标的位置已相对较近，此时需避免使用喷气装置，以免对目标造成羽流污染。同时，为避免反作用飞轮等姿态控制系统的启动对机械臂操作造成扰动及带来碰撞风险，空间机器人将关闭星载姿态控制系统，飞越式逼近轨迹进入飞跃段与捕获段。成功抓捕前自由漂浮空间机器人相对目标处于相对运动中，机械臂的操作也在相对运动过程中完成。抓捕过程主要包括四个步骤，分别为：初始化、抓捕、稳定(锁定)、收拢撤离。

按照上述设计的策略，本发明一种用于在轨快速抓捕的姿轨臂一体化运动规划方法，包括以下步骤：

S1，确定规划设计变量：

其中表示飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的 位置、速度、姿态以及角速度矢量，分别表示机械臂末端的位置和姿态；

S2，确定运动规划约束条件

运动规划约束条件包括飞越式逼近轨迹约束条件和快速抓捕约束条件，其中飞越式逼近轨迹约束条件包括安全操作距离约束、凸曲线约束和相对速度约束；

(1)安全操作距离约束

其中为空间机器人质心相对于目标抓捕点的最小安全距离，R_w,max表示空间机器人操作空间的最大范围，d_cp(t)为空间机器人质心相对目标抓捕点的距离；t_r为捕获段的起始时刻；t_mf为接触点时刻。

定义罚函数

(2)凸曲线约束

其中，^Hω_T为目标抓捕点自旋角速度向量，^Hx_cp是指相对运动位置向量；为相对运动加速度向量；

(3)相对速度约束

其中分别为目标抓捕点的速度分量，分别机械臂末端的速度分量V_ε为安全操作最大相对速度；

所述的快速抓捕轨迹约束条件包括抓捕操作时间约束、机械臂末端位置约 束、机械臂末端姿态约束、机械臂物理约束、工作空间约束和接触扰动最小约束；

(1)抓捕操作时间约束

t_mf＝t_xmin＜t_p (5)

其中t_mf为接触点时刻，t_xmin为轨道相对运动速度最小或距离最近时刻，t_p表示逃离段初始时刻；

(2)机械臂末端位置约束

其中^HR_GE,0表示初始时刻机械臂末端与目标点的距离，^HR_E(t_m0)、^HR_E(t_mf)分别表示抓捕操作起始与终止时刻机械臂末端相对空间机器人质心的位置，t_m0表示抓捕操作起始时刻，t_mf表示抓捕操作终止时刻；^Hρ_r,0和^Hρ_r,f分别表示抓捕操作起始与终止时刻的空间机器人质心与目标卫星质心的相对位置；^HR_G表示抓捕点相对目标卫星质心的位置矢量。

(3)机械臂末端姿态约束

其中^HΩ_E,0和^HΩ_E,f分别为抓捕操作起始和终止时刻的机械臂末端姿态；

(4)机械臂物理约束

其中q_m,max和分别表示关节角和角速率允许的最大值，q_mi表示机械臂第i个关节的关节角；进一步地，定义罚函数如下：

(5)工作空间约束，用罚函数的形式表示为：

其中ε为小于1的正数，；ε_o为大于100的正数，；⁰J_g(q_m)表示空间机器人的广义雅克比矩阵，|·|表示某矩阵的行列式，q_m表示机械臂关节角向量；R_E表示机械臂末端位置；PIW和PDW分别表示为自由漂浮空间机器人工作空间中的路径无关工作空间和路径相关工作空间，可参见文献(Papadopoulos E,Dubowsky S.Dynamic Singularity in Free-FloatingSpace Robots[J].JDyn Syst Meas Contr,1993,115(1):44-52.)

(6)接触扰动最小约束

其中，表示接触前空间机器人的速度，为使得接触引起的基座角速度变化为零的机械臂末端相对抓捕点的速度，为接触前抓捕点的速度；

S3，确定运动规划目标函数

结合约束条件中罚函数指标，令一体化运动规划的目标函数设定为

min J＝J_ω+J_ctrs (11)

其中J_ctrs＝J_d+J_q+J_v+J_s为相应的罚函数指标；J_ω表示基座扰动指标(基座扰动指标是做空间机器人运动规划的基本指标，目的在于使空间机器人机械臂运动时对基座扰动最小，从而保持基座对地定向等，从而使机械臂运动不影响对地通信等任务的进行。)，表示如下：

这里是指空间机器人基座的角速度矢量。

S4，机械臂末端轨迹参数化

对待规划时间段t∈[t_m0,t_mf]，令ξ∈[0,1]；机械臂末端位置变量可以表示为：

^HR_E＝R(ξ),ξ∈[0,1] (13)

机械臂末端姿态由规划初始时刻t_m0的机械臂末端姿态^HΩ_E,0绕单位向量κ旋转Φ_f角后调整到^HΩ_E,f，0≤Φ≤180°，因此机械臂末端姿态变量可以表示为

^HΩ_E＝[κ Φ(ξ)],ξ∈[0,1] (14)

其中由^HΩ_E,0和^HΩ_E,f可以计算出κ与Φ_f，使满足Φ(0)＝0,Φ(1)＝Φ_f；

假设机械臂的期望运动轨迹连续可微，则其末端的速度轨迹可以表示为

由式(13)-(15)，机械臂末端的位置与姿态轨迹最终由R(ξ)和Φ(ξ)描述，因此对机械臂运动规划问题，设计变量可以统一表示为

x_i(ξ)＝[R^T(ξ) Φ^T(ξ)]^T,i＝1,2,…,ξ∈[0,1] (16)

其中x_i设计变量在Hill坐标系的第i个分量；在Hill坐标系中对x_i进行参数化表示：

其中表示N阶多项式形式的x_i(ξ)，N根据等式约束条件(式(6)、(7)、(10))的数量选取，N>等式约束条件的数量；P_k(t)表示关于时间变量t的切比雪夫多项式；b_ik表示P_k(t)的权系数。

对式(17)式求导，得到参数化形式的速度与加速度变量：

对应时间条件t＝t_m0和t＝t_mf分别取ξ＝0和ξ＝1，根据等式约束式(6)、(7)和(10)，对每一个i，可以得到5个关于参数b_ik的方程w_j(b_ik)＝0，j＝1,2,…5，由此可知，参数b_i0～b_iN中只有N-5是独立变量。不妨假设b_i6～b_iN为独立变量，机械臂运动规划问题转变为优化配置b_i6～b_iN使相应的目标函数最小；

S5，混沌差分进化求解

S5.1：初始化参数

设定种群规模N_p，个体维数dim，最大进化代数Iter_max，终止条误差件ε，混沌搜索次数k_c，调节因子F_s，交叉因子Cr；

S5.2：种群初始化

每个种群个体包含以下变量：即飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置、速度、姿态以及角速度矢量以及机械臂末端的位置的轨迹和姿态的轨迹。其中的维数为4×3＝12，和的各个维度可以由式(17)离散为n_r个变量，又因为存在5个等式约束条件，所以共n＝12+3×(n_r-5)个独立参数，因此随机产生N_p个参数向量i＝1,2,…,N_p，分别表示该设计变量的各个独立参数，构成的初始种群pop(0)；

S5.3：计算个体适应度

首先将种群参数代入相对轨道运动方程(相对轨道运动方程是本领域的一个通用方程式)，其次在相对运动基础上由式(17)、(18)得到机械臂末端标准轨迹，利用广义雅克比矩阵计算空间机器人的基座速度和位姿变化(空间机器人由基座和机械臂组成)，代入目标函数式(11)得到个体适应度指标；

S5.4：精英选择

对种群适应度进行评价，找出适应度最佳个体B_best(即式11计算得到的值最小的个体)，记录最佳适应度值及个体在种群中的索引位置；B_best作为当代种群的精英；

S5.5：混沌局部搜索

以B_best作为混沌搜索的初值，在B_best附近的区域进行k_c次混沌搜索：

B_k＝B_best+F_s·β_k (19)

其中β_k为Logistic混沌方程的迭代变量，F_s为调节因子，可使B_k向着正反两个方向变化；从得到的个体B_k中找出适应度最佳的B'_best，若B'_bset优于B_best，则将B'_best随机取代种群中个体后进入下一步骤；若B_best优于B'_best，则直接进入下一步骤；

S5.6：判断终止条件

判断是否满足终止准则(如最大进化代数或最佳适应度均值的更新误差)，若满足则终止计算，输出最佳个体B_gbest；若不满足则进入S5.7，且Iter＝Iter+1；

S5.7：进化操作

按照DE/local-to-best/1进化模式的规则进行相应的选择和进化操作，生成新一代种群pop(Iter)，返回S5.3；

重复S5.3至S5.7的过程直至满足终止条件，输出最佳个体B_gbest及其适应度的值。

本发明的有益技术效果是：

本方法具有抓捕操作用时少、抓捕操作安全性好、求解趋于全局最优等优点。该方法首次提出对空间机器人的飞越式逼近目标过程与机械臂目标抓捕过程进行一体化规划，实现了区别于传统“停靠-抓捕”方式的飞跃式动态抓捕。该方法首先提出了姿轨臂一体化运动规划的模型，主要是确立了空间机器人飞越接近和机械臂抓捕过程的约束条件模型；其次，针对设计变量的离散化问题， 提出了基于切比雪夫多项式的离散方法；最后，针对一般优化算法的陷入局部最优的问题，提出了适用于该规划模型的混沌差分进化算法。

附图说明

图1是空间机器人飞越式逼近目标过程示意图。

图2是抓捕操作流程示意图。

图3是混沌差分算法流程图。

图4是空间机器人轨道平面内的逼近路径及机械臂末端抓捕轨迹。

图5是参考惯性系内空间机器人的运动过程。

图6是抓捕过程中基座姿态轨迹。

图7是抓捕过程中机械臂末端速度轨迹。

图8是抓捕过程中机械臂关节轨迹。

图9是机械臂末端姿态变化轨迹。

具体实施方式

为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

图1描述了空间机器人的快速逼近策略。空间机器人相对目标在最终逼近阶段的运动过程，采取飞越式逼近的快速逼近方式，即空间机器人从相距目标几百米的位置开始转移机动，沿服从轨道动力学的无碰撞路径不施加轨道控制而以合适的速度接近目标位置，至捕获区域内实施机械臂抓捕操作。若接近过程中出现意外故障，则可以沿着一条安全轨迹逃离目标而不施加有源控制。飞越式逼近过程包括轨迹调整段S₀～S₁、飞越段S₁～S₂、捕获段S₂～S₃和逃离段S₃～S₄。

图2描述了空间机器人的快速抓捕策略。空间机器人完成轨迹调整后，相距目标的位置已相对较近，此时需避免使用喷气装置，以免对目标造成羽流污染。同时，为避免反作用飞轮等姿态控制系统的启动对机械臂操作造成扰动及带来碰撞风险，空间机器人将关闭星载姿态控制系统，飞越式逼近轨迹进入飞跃段与捕获段。成功抓捕前自由漂浮空间机器人相对目标处于相对运动中，机械臂的操作也在相对运动过程中完成。抓捕过程主要包括四个步骤，分别为：初始化、抓捕、稳定(锁定)、收拢撤离。

按照上述设计的策略，本发明提供一种用于在轨快速抓捕的姿轨臂一体化运动规划方法，包括以下步骤：

第一步，确定设计变量。选择规划设计变量：

其中表示表示飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置和速度矢量；分别表示机械臂末端的位置和姿态。空间机器人逼近与抓捕过程的运动规划问题可以表述为：求解t∈[t_f,t_p]时间内的逼近运动轨迹^HX_f(t)和t∈[t_m0,t_mf]时间内的机械臂抓捕运动轨迹[^HR_E(t)^T,^HΩ_E(t)^T]^T，使满足逼近与抓捕过程的位形约束、物理约束和安全约束等约束条件。

第二步，确定运动规划约束条件。运动规划的约束条件包括飞越式逼近轨迹约束条件和快速抓捕约束条件。其中所述的飞越式逼近轨迹约束条件包括安全操作距离约束、凸曲线约束、相对速度约束等。

所述的安全操作距离约束表述如下：机械臂的工作空间为Ω_R＝{R|R_w,min≤R≤R_w,max}，设空间机器人质心相对目标抓捕点的距离为d_cp(t)。考虑捕获段的定义，当t∈[t_r,t_p]时，存在约束条件：

d_cp(t)＜R_w,max (2)

同时，考虑机械臂操作安全性，应在空间机器人与目标抓捕点之间留有一 定的安全距离以调整操作偏差，设空间机器人质心相对于目标抓捕点的最小安全距离为则约束条件调整为：

同时定义罚函数

凸曲线约束可表示为：为保证空间机器人逼近轨迹为凸曲线，有如下不等式成立：

其中，^Hω_T为目标抓捕点自旋角速度向量，^Hx_cp是指相对运动位置向量，为相对运动加速度向量；

相对速度约束可表示为：定义z_H方向为目标星轨道平面的法线方向，令空间机器人的质心在z_H方向无相对运动，因此仅考虑空间机器人质心相对目标星的速度矢量在目标卫星轨道平面内的速度分量空间机器人轨道相对运动速度表示为：

其中，x_cf表示飞跃段初始时刻空间机器人质心相对目标卫星的位置矢量在目标卫星轨道平面上的某个分量，该分量的方向由地心指向目标卫星。分别表示飞跃段初始时刻空间机器人质心相对于目标卫星的速度矢量在目标卫星轨道平面内的速度分量。

为保证抓捕操作安全性，在抓捕时间内空间机器人相对抓捕点的速度应不超过安全抓捕所允许的最大速度V_ε：

其中分别为目标抓捕点的速度分量，分别空间机器人质心相对于目标卫星的速度在轨道平面上分量，V_ε为安全操作最大相对速度。

所述的快速抓捕轨迹约束条件包括抓捕操作时间约束、机械臂末端位置约束、机械臂末端姿态约束、机械臂物理约束、工作空间约束、接触扰动最小约束等。

抓捕操作时间约束可表示为：在轨抓捕的操作时间受轨道相对运动动力学的约束，与逼近轨迹紧密相关。受机械臂工作范围的限制，抓捕操作必须在捕获段内完成。t_p表示逃离段初始时刻，则成功抓捕必须满足：

t_mf≤t_p (7)

接触时刻的选择还需要考虑目标自旋周期等约束，若选择轨道相对运动速度最小或距离最近时刻t_xmin，则须满足

t_mf＝t_xmin (8)

机械臂末端位置约束可表示为：假设机械臂末端相对目标抓捕点的初始距离为^HR_GE,0，机械臂末端的位置约束条件：

其中^HR_GE,0表示初始时刻机械臂末端与目标点的距离，^HR_E(t_m0)、^HR_E(t_mf)分别表示抓捕操作起始与终止时刻机械臂末端相对空间机器人质心的位置，t_m0表示抓捕操作起始时刻，t_mf表示抓捕操作终止时刻；^Hρ_r,0和^Hρ_r,f分别表示抓捕操作起始与终止时刻的空间机器人质心与目标卫星质心的相对位置；^HR_G表示抓捕点相对目标卫星质心的位置矢量。

所述的机械臂末端姿态约束可表示为：

其中^HΩ_E,0和^HΩ_E,f分别为抓捕操作起始和终止时刻的机械臂末端姿态。

所述的机械臂物理约束可表示为：

其中q_m,max和分别表示关节角和角速率允许的最大值。qmi表示机械臂第i个关节的关节角；同时定义罚函数如下：

工作空间约束可表示为：规划设计变量为机械臂末端在笛卡尔空间中的位置，因此设计变量须满足空间机器人工作空间的限制。机械臂末端在相对参考惯性系的可达空间范围内运动，则得到：

R_w,min≤|R_E(t)|≤R_w,max (12)

进一步，若机械臂末端进入PDW空间，则运动中会出现动力学奇异点，将给控制系统设计增加困难。应尽量使机械臂末端在PIW范围内运动。但这一约束缩小了机械臂的工作范围，对某些工况可能无法满足任务要求。因此，考虑在PIW内无最优解时允许机械臂末端进入PDW内。定义惩罚因子J_s：

其中ε为很小的正数，ε_o为比较大的正数；⁰J_g(q_m)表示空间机器人的广义雅克比矩阵，|·|表示某矩阵的行列式，q_m表示机械臂关节角向量；R_E表示机械臂末端位置；PIW和PDW分别表示为自由漂浮空间机器人工作空间中的路径无关工作 空间和路径相关工作空间。

接触扰动最小约束可表示为：设表示目标抓捕点的广义速度向量，v_G表示抓捕点的线速度，ω_G表示抓捕点的角速度，有：

其中，为目标卫星的广义速度矢量，为抓捕点的广义速度矢量，A_S表示抓捕点到目标质心的定常转移矩阵。同理，忽略目标卫星姿态运动的非线性项，将目标卫星的动力学方程在Δt→0时间内积分得到：

其中H_S表示目标卫星的广义质量矩阵，表示机械臂末端与抓捕点接触时的相互作用力；

机械臂末端与目标抓捕点接触后目标抓捕点的速度增量：

假设下标“α”表示接触前的物理量，下标“β”表示接触后的物理量，将 代入(16)式，整理得到：

由空间机器人运动学关系，得：

将(18)代入(17)中，整理得到：

其中 表示机械臂末端与目标抓捕点的相对速度。

由(19)可得：

式(20)表明，接触引起的基座角速度变化与接触前机械臂末端相对目标抓捕点的速度有关。因此，若使基座角度扰动尽量小，可以通过优化使得相对速度最小来实现。

假设接触碰撞对基座角速度没有扰动，即Δω₀＝0，则有两种可行的实现方式，而“直臂抓捕”方式较难实现，因此采取第(2)种方式，即将Δω₀＝0代入式(20)中可以解得：

其中(·)⁺表示矩阵的Moore-Penrose伪逆，为任意速度向量。显然，为式(20)的零解，由式(21)确定的相对速度可以使接触对基座角速度的扰动严格为零。因接触前速度是定值，可以得到接触扰动最小的速度约束条件：

其中，表示接触前空间机器人的速度，为使得接触引起的基座角速度变化为零的机械臂末端相对目标抓捕点的速度，为接触前抓捕点的速度。

第三步，确定运动规划目标函数。

飞越式抓捕过程对基座姿态产生扰动最小是规划期望的最终目标，基座受到的扰动因素主要有两项。一项是由机械臂运动产生的耦合扰动，由于空间机器人系统的非完整性，基座姿态与关节变量没有位置级约束关系，而仅存在速度级约束，因此，基座扰动指标表示为

其中是指空间机器人基座的角速度矢量。

另一项则为机械臂末端接触目标产生的扰动影响，接触扰动最小问题可转化为对终端时刻的机械臂速度约束。因此结合约束条件中罚函数指标，令一体 化运动规划的目标函数设定为

min J＝J_ω+J_ctrs (24)

其中J_ctrs＝J_d+J_q+J_v+J_s为相应的罚函数指标。

第四步，机械臂末端轨迹参数化。

对待规划时间段t∈[t_m0,t_mf]，令ξ∈[0,1]。机械臂末端位置变量可以表示为：

^HR_E＝R(ξ),ξ∈[0,1] (25)

机械臂末端姿态由规划初始时刻t_m0的机械臂末端姿态^HΩ_E,0绕单位向量κ旋转Φ_f角后调整到^HΩ_E,f，因此机械臂末端姿态变量可以表示为

^HΩ_E＝[κ Φ(ξ)],ξ∈[0,1] (26)

其中由^HΩ_E,0和^HΩ_E,f可以计算出κ与Φ_f，使满足Φ(0)＝0,Φ(1)＝Φ_f。

假设机械臂的期望运动轨迹连续可微，则其末端的速度轨迹可以表示为

由式(25)-(27)，机械臂末端的位置与姿态轨迹最终由R(ξ)和Φ(ξ)描述，因此对机械臂运动规划问题，设计变量可以统一表示为

x_i(ξ)＝[R^T(ξ) Φ^T(ξ)]^T,i＝1,2,…,ξ∈[0,1] (28)

在Hill坐标系中对x_i进行参数化表示：

其中表示N阶多项式形式的x_i(ξ)，N根据等式约束条件的数量选取，N>等式约束条件的数量；P_k(t)表示关于时间变量t的切比雪夫多项式；b_ik表示P_k(t)的权系数。

对式(29)式求导，得到参数化形式的速度与加速度变量：

对应时间条件t＝t_m0和t＝t_mf分别取ξ＝0和ξ＝1，根据等式约束式(9)、(10)和(22)，对每一个i，可以得到5个关于参数b_ik的方程w_j(b_ik)＝0，j＝1,2,…5，由此可知，参数b_i0～b_iN中只有N-5是独立变量。不妨假设b_i6～b_iN为独立变量，机械臂运动规划问题转变为优化配置b_i6～b_iN使相应的目标函数最小。

第五步，混沌差分进化求解，具体步骤如下：

步骤1：初始化参数

设定种群规模N_p，个体维数dim，最大进化代数Iter_max，终止条误差件ε，混沌搜索次数k_c，调节因子F_s，交叉因子Cr。

步骤2：种群初始化

每个种群个体包含以下变量：飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置、速度、姿态以及角速度矢量机械臂末端的位置的轨迹和姿态的轨迹；其中的维数为4×3＝12，和的各个维度可以由式(29)离散为n_r个变量，因还存在5个等式约束条件，所以共n＝12+3×(n_r-5)个独立参数，因此随机产生N_p个参数向量i＝1,2,…,N_p，分别表示该设计变量的各个独立参数，构成的初始种群pop(0)。

步骤3：计算个体适应度。首先将种群参数代入相对轨道运动方程，其次在相对运动基础上由式(29)、(30)得到机械臂末端标准轨迹，利用广义雅克比矩阵计算基座速度和位姿变化，代入目标函数式(24)得到个体适应度指标。

步骤4：精英选择

对种群适应度进行评价，找出适应度最佳个体B_best，记录最佳适应度值及个体在种群中的索引位置。B_best作为当代种群的精英。

步骤5：混沌局部搜索

以B_best作为混沌搜索的初值，在B_best附近的区域进行k_c次混沌搜索：

B_k＝B_best+F_s·β_k (31)

其中β_k为Logistic混沌方程的迭代变量，F_s为调节因子，可使B_k向着正反两个方向变化。从得到的个体B_k中找出适应度最佳的B'_best，若B'_bset优于B_best，则将B'_best随机取代种群中个体后进入步骤6；若B_best优于B'_best，则直接进入步骤6。

步骤6：判断终止条件

判断是否满足终止准则(本章中为最大进化代数或最佳适应度均值的更新误差)，若满足则终止计算，输出最佳个体B_gbest；若不满足则进入步骤7，且Iter＝Iter+1。

步骤7：进化操作

按照DE/local-to-best/1进化模式的规则进行相应的选择和进化操作，生成新一代种群pop(Iter)，返回步骤3。

重复步骤3至步骤7的过程直至满足终止条件，输出最佳个体B_gbest及其适应度的值。

下面以某一仿真实例来说明本发明的具体实施效果。

以自由漂浮三关节空间机器人模型为例进行仿真计算，系统物理参数如下表所示。设定种群规模N_p＝200，个体维数dim＝13，进化次数上限Iter_max＝1000，混沌搜索次数k_c＝30。

实例中的仿真条件分别为：待抓捕目标轨道半径r_T＝7000km，轨道角速率ω_T＝1.078×10^-3rad/s，自旋角速率ω_S＝0.15rad/s，抓捕点位置坐标^SR_G＝[-1,1,0]^T；空间机器人相距目标初始位置坐标ρ_r＝[0,-100,0]^T，相对参考惯性系的初始姿态q_b＝[0,0,0]^T，q_m＝[0,-π/6,-π/6]^T。考虑飞越式逼近时间t_c≤300s，逼近最小安全距离4m，抓捕操作完成时间t_m≤100s。

经计算得到飞越段初始点位置坐标^Hx_cf＝[-8.9120,29.0342,0]^T，速度 ^Hv_cf＝[0.0576,-0.1374,0]^T，飞越式逼近过程时间为200.068s，机械臂抓捕操作时间100s。最终生成的空间机器人轨道平面内逼近路径及抓捕轨迹如图4所示。图中虚线表示空间机器人飞越式逼近轨迹，实线为机械臂末端抓捕运动轨迹，带叉的点划线为空间机器人撤离轨迹。可以看出，空间机器人实现了凸路径逼近运动，逼近中机械臂沿光滑连续轨迹执行抓捕操作，若此过程中出现意外故障，空间机器人可继续沿无碰撞轨迹撤离。因此，规划得到的轨迹满足模型要求，是合理有效的。

图5为参考惯性系内空间机器人末端规划轨迹及运动过程，图6为抓捕过程中基座姿态变化轨迹。由图5和图6可以看出机械臂末端抓捕运动轨迹光滑连续，且抓捕过程中基座姿态受到的扰动很小。图7为抓捕过程中机械臂末端的速度轨迹，可以看出速度变化是连续的，且恰在接触时刻减小到零。图8为抓捕过程中机械臂关节运动轨迹，图9为机械臂末端姿态规划轨迹。可以看出关节及末端姿态变化轨迹符合规划模型中的约束条件，是有效的可行解。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应该提出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (1)

1.一种用于在轨快速抓捕的姿轨臂一体化运动规划方法，其特征在于：包括以下步骤：

S1，确定规划设计变量：

$X\left(t\right)={\left[\begin{array}{ccc}X_f^T{}_H& R_E^T{}_H& \mathrm{\Ω}_E^T{}_H\end{array}\right]}^T---\left(1\right)$

其中表示飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置和速度矢量，分别表示机械臂末端的位置和姿态；

S2，确定运动规划约束条件

运动规划约束条件包括飞越式逼近轨迹约束条件和快速抓捕约束条件，其中飞越式逼近轨迹约束条件包括安全操作距离约束、凸曲线约束和相对速度约束；

(1)安全操作距离约束

${\stackrel{\&OverBar;}{R}}_{w,\min }<d_{cp}\left(t\right)<R_{w,max},t\&Element;\&lsqb;t_r,t_{mf}\&rsqb;---\left(2\right)$

其中为空间机器人质心相对于目标抓捕点的最小安全距离，R_w,max表示空间机器人操作空间的最大范围，d_cp(t)为空间机器人质心相对目标抓捕点的距离，t_r为捕获段的起始时刻；t_mf为接触点时刻；

同时定义罚函数

$J_d=\left\{\begin{array}{cc}0& d_{cp}\&Element;\&lsqb;{\stackrel{\&OverBar;}{R}}_{w,\min },R_{w,\max }\&rsqb;\\ +\mathrm{\&infin;}& d_{cp}\&NotElement;\&lsqb;{\stackrel{\&OverBar;}{R}}_{w,\min },R_{w,\max }\&rsqb;\end{array}\right.$

(2)凸曲线约束

$(\frac{{\mathrm{\&omega;}_H}_T\&CenterDot;{x_H}_{cp}}{\left|{\mathrm{\&omega;}_H}_T\right|}\&CenterDot;\frac{{\mathrm{\&omega;}_H}_T}{\left|{\mathrm{\&omega;}_H}_T\right|})\&CenterDot;{\stackrel{\&CenterDot;\&CenterDot;}{x}_H}_{cp}>0---\left(3\right)$

其中，^Hω_T为目标抓捕点自旋角速度向量，^Hx_cp是指相对运动位置向量，为相对运动加速度向量；

(3)相对速度约束

$\sqrt{{({\stackrel{\&CenterDot;}{x}_H}_r-{\stackrel{\&CenterDot;}{x}_H}_G)}^2+{({\stackrel{\&CenterDot;}{y}_H}_r-{\stackrel{\&CenterDot;}{y}_H}_G)}^2}\&le;V_{\mathrm{\&epsiv;}}---\left(4\right)$

其中分别为目标抓捕点的速度分量，分别机械臂末端的速度分量，V_ε为安全操作最大相对速度；

所述的快速抓捕轨迹约束条件包括抓捕操作时间约束、机械臂末端位置约束、机械臂末端姿态约束、机械臂物理约束、工作空间约束和接触扰动最小约束；

(1)抓捕操作时间约束

t_mf＝t_xmin＜t_p (5)

其中t_mf为接触点时刻，t_xmin为轨道相对运动速度最小或距离最近时刻，t_p表示逃离段初始时刻；

(2)机械臂末端位置约束

$\{\begin{array}{c}{R_H}_E\left(t_{m0}\right)={R_H}_{GE,0}+{R_H}_G-{\mathrm{\&rho;}_H}_{r,0}\\ {R_H}_E\left(t_{mf}\right)={R_H}_G-{\mathrm{\&rho;}_H}_{r,f}\end{array}---\left(6\right)$

其中^HR_GE,0表示初始时刻机械臂末端与目标点的距离，^HR_E(t_m0)、^HR_E(t_mf)分别表示抓捕操作起始与终止时刻机械臂末端相对空间机器人质心的位置，t_m0表示抓捕操作起始时刻，t_mf表示抓捕操作终止时刻；^Hρ_r,0和^Hρ_r,f分别表示抓捕操作起始与终止时刻的空间机器人质心与目标卫星质心的相对位置；^HR_G表示抓捕点相对目标卫星质心的位置矢量；

(3)机械臂末端姿态约束

$\{\begin{array}{c}{\mathrm{\&Omega;}_H}_E\left(t_{m0}\right)={\mathrm{\&Omega;}_H}_{E,0}\\ {\mathrm{\&Omega;}_H}_E\left(t_{mf}\right)={\mathrm{\&Omega;}_H}_{E,f}\end{array}---\left(7\right)$

其中^HΩ_E,0和^HΩ_E,f分别为抓捕操作起始和终止时刻的机械臂末端姿态；

(4)机械臂物理约束

$\left\{\begin{array}{c}\left|q_{mi}\right|\&le;q_{m,\max }\\ \left|{\stackrel{\&CenterDot;}{q}}_{mi}\right|\&le;{\stackrel{\&CenterDot;}{q}}_{m,\max }\end{array}\right.,i=1,2,\mathrm{...}n---\left(8\right)$

其中q_m,max和分别表示关节角和角速率允许的最大值，q_mi表示机械臂第i个关节的关节角；同时定义罚函数如下：

$J_q=\{\begin{array}{cc}0& \left|q_{mi}\right|\&le;q_{m,\max }\\ +\mathrm{\&infin;}& \left|q_{mi}\right|>q_{m,\max }\end{array},i=1,2,\mathrm{...}n$

$J_v=\{\begin{array}{cc}0& \left|{\stackrel{\&CenterDot;}{q}}_{mi}\right|\&le;{\stackrel{\&CenterDot;}{q}}_{m,\max }\\ +\mathrm{\&infin;}& \left|{\stackrel{\&CenterDot;}{q}}_{mi}\right|>{\stackrel{\&CenterDot;}{q}}_{m,\max }\end{array},i=1,2,\mathrm{...}n$

(5)工作空间约束，用罚函数的形式表示为：

$J_s=\left\{\begin{array}{cc}0& R_E\&Element;PIW\\ +\mathrm{\&infin;}& \left|{J_0}_g\left(q_m\right)\right|<\mathrm{\&epsiv;}\\ {\mathrm{\&epsiv;}}_0& R_E\&Element;PDW\end{array}\right.---\left(9\right)$

其中ε为小于1的正数，；ε_o为大于100的正数；⁰J_g(q_m)表示空间机器人的广义雅克比矩阵，|·|表示某矩阵的行列式，q_m表示机械臂关节角向量；R_E表示机械臂末端位置；PIW和PDW分别表示为自由漂浮空间机器人工作空间中的路径无关工作空间和路径相关工作空间；

(6)接触扰动最小约束

${\stackrel{\&CenterDot;}{x}}_{E,\mathrm{\&alpha;}}\left(t_{mf}\right)={\stackrel{\&CenterDot;}{x}}_{GE}^{null}+{\stackrel{\&CenterDot;}{x}}_{G,\mathrm{\&alpha;}}---\left(10\right)$

其中，表示接触前空间机器人的速度，为使得接触引起的基座角速度变化为零的机械臂末端相对抓捕点的速度，为接触前抓捕点的速度；

S3，确定运动规划目标函数

结合约束条件中罚函数指标，令一体化运动规划的目标函数设定为

min J＝J_ω+J_ctrs (11)

其中J_ctrs＝J_d+J_q+J_v+J_s为相应的罚函数指标；J_ω表示基座扰动指标，表示如下：

$J_{\mathrm{\&omega;}}={\&Integral;}_{t_{m0}}^{t_{mf}}{\stackrel{\&CenterDot;}{q}}_b^T{\stackrel{\&CenterDot;}{q}}_{b}dt---\left(12\right)$

其中是指空间机器人基座的角速度矢量；

S4，机械臂末端轨迹参数化

对待规划时间段t∈[t_m0,t_mf]，令ξ∈[0,1]；机械臂末端位置变量可以表示为：

^HR_E＝R(ξ),ξ∈[0,1] (13)

机械臂末端姿态由规划初始时刻t_m0的机械臂末端姿态^HΩ_E,0绕单位向量κ旋转Φ_f角后调整到^HΩ_E,f，0≤Φ≤180°，因此机械臂末端姿态变量可以表示为

^HΩ_E＝[κ Φ(ξ)],ξ∈[0,1] (14)

其中由^HΩ_E,0和^HΩ_E,f可以计算出κ与Φ_f，使满足Φ(0)＝0,Φ(1)＝Φ_f；

假设机械臂的期望运动轨迹连续可微，则其末端的速度轨迹可以表示为

$\left\{\begin{array}{c}{v_H}_E=\frac{d}{d\mathrm{\&xi;}}R\left(\mathrm{\&xi;}\right)\\ {\mathrm{\&omega;}_H}_E=\mathrm{\&kappa;}\&CenterDot;\frac{d}{d\mathrm{\&xi;}}\mathrm{\&Phi;}\left(\mathrm{\&xi;}\right)\end{array}\right.---\left(15\right)$

由式(13)-(15)，机械臂末端的位置与姿态轨迹最终由R(ξ)和Φ(ξ)描述，因此对机械臂运动规划问题，设计变量可以统一表示为

x_i(ξ)＝[R^T(ξ) Φ^T(ξ)]^T,i＝1,2,…,ξ∈[0,1] (16)

在Hill坐标系中对x_i进行参数化表示：

$X_i^N\left(\mathrm{\&xi;}\right)=\underset{k=0}{\overset{N}{\&Sigma;}}{b}_{ik}\&CenterDot;P_k\left(t\right){|}_{t=\mathrm{\&xi;}(t_{mf}-t_{m0})+t_{m0}}---\left(17\right)$

其中表示N阶多项式形式的x_i(ξ)，N根据等式约束条件的数量选取，N>等式约束条件的数量；P_k(t)表示关于时间变量t的切比雪夫多项式；b_ik表示P_k(t)的权系数；

对式(17)式求导，得到参数化形式的速度与加速度变量：

$\{\begin{array}{c}\frac{{\mathrm{dx}}_i^N\left(\mathrm{\&xi;}\right)}{d\mathrm{\&xi;}}=\underset{k=0}{\overset{8}{\&Sigma;}}{b}_{ik}{P}_k^{\&prime;}\left(t\right){|}_{t=\mathrm{\&xi;}(t_{mf}-t_{m0})+t_{m0}}\\ \frac{d^2{x}_i^N\left(\mathrm{\&xi;}\right)}{{\mathrm{d\&xi;}}^2}=\underset{k=0}{\overset{8}{\&Sigma;}}{b}_{ik}{P}_k^{\&prime;\&prime;}\left(t\right){|}_{t=\mathrm{\&xi;}(t_{mf}-t_{m0})+t_{m0}}\end{array}---\left(18\right)$

S5，混沌差分进化求解

S5.1：初始化参数

设定种群规模N_p，个体维数dim，最大进化代数Iter_max，终止条误差件ε，混沌搜索次数k_c，调节因子F_s，交叉因子Cr；

S5.2：种群初始化

每个种群个体包含以下变量：飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置、速度、姿态以及角速度矢量机械臂末端的位置的轨迹和姿态的轨迹；其中的维数为4×3＝12，和的各个维度可以由式(17)离散为n_r个变量，因还存在5个等式约束条件，所以共n＝12+3×(n_r-5)个独立参数，因此随机产生N_p个参数向量i＝1,2,…,N_p，分别表示该设计变量的各个独立参数，构成的初始种群pop(0)；

S5.3：计算个体适应度

首先将种群参数代入相对轨道运动方程，其次在相对运动基础上由式(17)、(18)得到机械臂末端标准轨迹，利用广义雅克比矩阵计算基座速度和位姿变化，代入目标函数式(11)得到个体适应度指标；

S5.4：精英选择

对种群适应度进行评价，找出适应度最佳个体B_best，记录最佳适应度值及个体在种群中的索引位置；B_best作为当代种群的精英；

S5.5：混沌局部搜索

以B_best作为混沌搜索的初值，在B_best附近的区域进行k_c次混沌搜索：

B_k＝B_best+F_s·β_k (19)

其中β_k为Logistic混沌方程的迭代变量，F_s为调节因子，可使B_k向着正反两个方向变化；从得到的个体B_k中找出适应度最佳的B'_best，若B'_bset优于B_best，则将B'_best随机取代种群中个体后转S5.6；若B_best优于B'_best，则直接转S5.6；

S5.6：判断终止条件

判断是否满足终止准则，若满足则终止计算，输出最佳个体B_gbest；若不满足则进入S5.7，且Iter＝Iter+1；

S5.7：进化操作

按照DE/local-to-best/1进化模式的规则进行相应的选择和进化操作，生成新一代种群pop(Iter)，返回S5.3；

重复S5.3至S5.7的过程直至满足终止条件，输出最佳个体B_gbest及其适应度的值。