[发明专利]用于在轨快速抓捕的姿轨臂一体化运动规划方法

摘要

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

主权项

一种用于在轨快速抓捕的姿轨臂一体化运动规划方法，其特征在于：包括以下步骤：S1，确定规划设计变量：$X\left(t\right)={\left[\begin{array}{ccc}{}^{H}X_f^T& {}^{H}R_E^T& {}^H\mathrm{\Ω}_E^T\end{array}\right]}^T---\left(1\right)$其中表示飞越段初始时刻，空间机器人在Hill坐标系中相对轨道运动的位置和速度矢量，分别表示机械臂末端的位置和姿态；S2，确定运动规划约束条件运动规划约束条件包括飞越式逼近轨迹约束条件和快速抓捕约束条件，其中飞越式逼近轨迹约束条件包括安全操作距离约束、凸曲线约束和相对速度约束；(1)安全操作距离约束${\stackrel{\‾}{R}}_{w,\min }\<d_{cp}\left(t\right)\<R_{w,max},t\∈\[t_r,t_{mf}\]---\left(2\right)$其中为空间机器人质心相对于目标抓捕点的最小安全距离，R_w,max表示空间机器人操作空间的最大范围，d_cp(t)为空间机器人质心相对目标抓捕点的距离，t_r为捕获段的起始时刻；t_mf为接触点时刻；同时定义罚函数$J_d=\left\{\begin{array}{cc}0& d_{cp}\∈\[{\stackrel{\‾}{R}}_{w,\min },R_{w,\max }\]\\ +\mathrm{\∞}& d_{cp}\∉\[{\stackrel{\‾}{R}}_{w,\min },R_{w,\max }\]\end{array}\right.$(2)凸曲线约束$(\frac{{{}^H\mathrm{\ω}}_T\·{{}^{H}x}_{cp}}{\left|{{}^H\mathrm{\ω}}_T\right|}\·\frac{{{}^H\mathrm{\ω}}_T}{\left|{{}^H\mathrm{\ω}}_T\right|})\·{{}^H\stackrel{\·\·}{x}}_{cp}\>0---\left(3\right)$其中，^Hω_T为目标抓捕点自旋角速度向量，^Hx_cp是指相对运动位置向量，为相对运动加速度向量；(3)相对速度约束$\sqrt{{({{}^H\stackrel{\·}{x}}_r-{{}^H\stackrel{\·}{x}}_G)}^2+{({{}^H\stackrel{\·}{y}}_r-{{}^H\stackrel{\·}{y}}_G)}^2}\≤V_{\mathrm{\ϵ}}---\left(4\right)$其中分别为目标抓捕点的速度分量，分别机械臂末端的速度分量，V_ε为安全操作最大相对速度；所述的快速抓捕轨迹约束条件包括抓捕操作时间约束、机械臂末端位置约束、机械臂末端姿态约束、机械臂物理约束、工作空间约束和接触扰动最小约束；(1)抓捕操作时间约束t_mf＝t_xmin＜t_p    (5)其中t_mf为接触点时刻，t_xmin为轨道相对运动速度最小或距离最近时刻，t_p表示逃离段初始时刻；(2)机械臂末端位置约束$\{\begin{array}{c}{{}^{H}R}_E\left(t_{m0}\right)={{}^{H}R}_{GE,0}+{{}^{H}R}_G-{{}^H\mathrm{\ρ}}_{r,0}\\ {{}^{H}R}_E\left(t_{mf}\right)={{}^{H}R}_G-{{}^H\mathrm{\ρ}}_{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}{{}^H\mathrm{\Ω}}_E\left(t_{m0}\right)={{}^H\mathrm{\Ω}}_{E,0}\\ {{}^H\mathrm{\Ω}}_E\left(t_{mf}\right)={{}^H\mathrm{\Ω}}_{E,f}\end{array}---\left(7\right)$其中^HΩ_E,0和^HΩ_E,f分别为抓捕操作起始和终止时刻的机械臂末端姿态；(4)机械臂物理约束$\left\{\begin{array}{c}\left|q_{mi}\right|\≤q_{m,\max }\\ \left|{\stackrel{\·}{q}}_{mi}\right|\≤{\stackrel{\·}{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|\≤q_{m,\max }\\ +\mathrm{\∞}& \left|q_{mi}\right|\>q_{m,\max }\end{array},i=1,2,\mathrm{...}n$$J_v=\{\begin{array}{cc}0& \left|{\stackrel{\·}{q}}_{mi}\right|\≤{\stackrel{\·}{q}}_{m,\max }\\ +\mathrm{\∞}& \left|{\stackrel{\·}{q}}_{mi}\right|\>{\stackrel{\·}{q}}_{m,\max }\end{array},i=1,2,\mathrm{...}n$(5)工作空间约束，用罚函数的形式表示为：$J_s=\left\{\begin{array}{cc}0& R_E\∈PIW\\ +\mathrm{\∞}& \left|{{}^{0}J}_g\left(q_m\right)\right|\<\mathrm{\ϵ}\\ {\mathrm{\ϵ}}_0& R_E\∈PDW\end{array}\right.---\left(9\right)$其中ε为小于1的正数，；ε_o为大于100的正数；⁰J_g(q_m)表示空间机器人的广义雅克比矩阵，|·|表示某矩阵的行列式，q_m表示机械臂关节角向量；R_E表示机械臂末端位置；PIW和PDW分别表示为自由漂浮空间机器人工作空间中的路径无关工作空间和路径相关工作空间；(6)接触扰动最小约束${\stackrel{\·}{x}}_{E,\mathrm{\α}}\left(t_{mf}\right)={\stackrel{\·}{x}}_{GE}^{null}+{\stackrel{\·}{x}}_{G,\mathrm{\α}}---\left(10\right)$其中，表示接触前空间机器人的速度，为使得接触引起的基座角速度变化为零的机械臂末端相对抓捕点的速度，为接触前抓捕点的速度；S3，确定运动规划目标函数结合约束条件中罚函数指标，令一体化运动规划的目标函数设定为min J＝J_ω+J_ctrs    (11)其中J_ctrs＝J_d+J_q+J_v+J_s为相应的罚函数指标；J_ω表示基座扰动指标，表示如下：$J_{\mathrm{\ω}}={\∫}_{t_{m0}}^{t_{mf}}{\stackrel{\·}{q}}_b^T{\stackrel{\·}{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}{{}^{H}v}_E=\frac{d}{d\mathrm{\ξ}}R\left(\mathrm{\ξ}\right)\\ {{}^H\mathrm{\ω}}_E=\mathrm{\κ}\·\frac{d}{d\mathrm{\ξ}}\mathrm{\Φ}\left(\mathrm{\ξ}\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{\ξ}\right)=\underset{k=0}{\overset{N}{\Σ}}{b}_{ik}\·P_k\left(t\right){|}_{t=\mathrm{\ξ}(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{\ξ}\right)}{d\mathrm{\ξ}}=\underset{k=0}{\overset{8}{\Σ}}{b}_{ik}{P}_k^{\′}\left(t\right){|}_{t=\mathrm{\ξ}(t_{mf}-t_{m0})+t_{m0}}\\ \frac{d^2{x}_i^N\left(\mathrm{\ξ}\right)}{{\mathrm{d\ξ}}^2}=\underset{k=0}{\overset{8}{\Σ}}{b}_{ik}{P}_k^{\′\′}\left(t\right){|}_{t=\mathrm{\ξ}(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及其适应度的值。