CN106272443A - 多自由度空间机械臂非完整路径规划方法 - Google Patents

Abstract

本发明公开了一种利用遗传算法进行多自由度空间机械臂非完整路径规划方法。其步骤包括：一、机械臂各关节运动规律函数化；二、利用两端约束条件化简待定参数；三、基于终端时刻控制精度和过程约束要求确定适应度目标函数；四、利用遗传算法对待定参数寻优。本发明基于带操作臂的自由漂浮航天器本体‑机械臂耦合动力学建模，运用遗传算法寻找最优的机械臂的运动路径，实现本体姿态及机械臂关节角同时达到期望状态，节省卫星本体姿态控制的能量消耗与任务执行时间，同时避免传统逆运动学关系求解过程中可能造成的动力学奇异问题，可运用到以在轨服务为背景的目标捕获后机械臂与本体综合控制任务。

Description

多自由度空间机械臂非完整路径规划方法

技术领域

本发明涉及航天器在轨服务技术，具体涉及一种基于耦合动力学建模的平台无扰动空间机械臂运动路径规划方法。

背景技术

随着航天技术发展对航天器的性能、结构、组成提出的日趋复杂要求，航天器在复杂的空间环境中持久、可靠运行成为完成复杂任务的前提条件。针对这一需求，目前正在大力发展的在轨服务技术将成为解决上述重要问题的重要途径：通过针对复杂大型航天器开展的在轨捕获、模块维修与部件更换等操作，可显著提升航天器在轨运行的可靠性，降低重新发射替代航天器的时间和资金成本，因而具有广泛应用前景。

在轨操作通常需要服务航天器在经历远程变轨、近程导引和超近距离逼近后，利用其上配置的机械臂对目标上的部件进行抓取操作。然而，由于机械臂和航天器本体的动力学耦合效应，地面固定基座机械臂的运动规划算法不适用于空间机械臂系统，目标捕获后空间机械臂的运动通常都会对本体的姿态稳定形成干扰，这对星上能源消耗、GNC系统的运行、对地通信等任务都形成安全隐患。因此，有必要在机械臂运动与航天器本体姿态转动耦合关系分析的基础上，寻找运动学约束条件下对本体姿态无扰动的机械臂最优运动路径规划方法，使该方法同样适于利用机械臂运动对本体进行辅助姿态控制方案的设计。

发明内容

为了解决对航天器本体无扰动的空间机械臂的非完整路径规划问题，本发明提供一种多自由度空间机械臂非完整路径规划方法，利用遗传算法对机械臂各关节运动参数寻优，以确定特定运动学约束条件下机械臂最优运动路径。

本发明所述的多自由度空间机械臂非完整路径规划方法，包括如下步骤：

步骤一，机械臂各关节运动规律函数化：采用正弦函数对关节角进行参数化设计；

步骤二，化简待定参数：利用初始、终端、过程状态约束条件将未知参数用待定参数表示，以减少待定参数个数；

步骤三，设计适应度目标函数：基于终端时刻本体控制精度和过程约束要求设计合适的适应度目标函数；

步骤四，待定参数的遗传算法寻优：基于本体-臂耦合动力学关系寻找使适应度目标函数最小的待定参数组合，据此确定机械臂完整运动路径。

与现有技术相比，本发明具有以下优点：

1、采用遗传算法可以搜索得到机械臂运动待定参数矩阵的全局最优解，可保证机械臂各关节运动期望角度的同时实现本体稳定或达到期望姿态，实现本体姿态的机械臂运动辅助控制，节省卫星本体姿态控制的能量消耗与任务执行时间。

2、仅用到了表示机械臂关节角速度-本体姿态角速度关系的正运动学方程，且根据实际终端及过程运动约束，采用正弦函数对关节路径进行参数化设计，得到了平滑的关节运动形式，适合于实际执行机构对机械臂控制的工程应用。

附图说明

以下将结合附图和具体实施方式对本发明作进一步的说明。

图1是基于遗传算法的平台无扰动空间机械臂运动参数寻优流程图。

具体实施方式

下文中，结合附图和实施例对本发明做进一步阐述。

如图1所示，本实施例的多自由度空间机械臂非完整路径规划方法，包括如下步骤：

步骤一、机械臂各关节运动规律函数化；

步骤二、利用两端约束条件化简待定参数；

步骤三、基于终端时刻控制精度和过程约束要求确定适应度目标函数；

步骤四、利用遗传算法对待定参数寻优。

步骤一，机械臂各关节运动规律函数化：采用正弦函数对关节角进行参数化设计；

采用了关于时间的五次多项式正弦函数对关节角进行参数化设计，直接约束了关节角的运动范围，同时保证了机械臂运动过程的平滑，该函数关系表示为：

θ_i(t)＝A_i1sin(a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0)+A_i2 (1)

其中，变量下标i对应机械臂系统的第i个关节。

对上式求时间导数，可得：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_i\left(t\right)=A_{i1}\cos (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}t+a_{i0})\\ \·(7a_{i7}{t}^6+6a_{i6}{t}^5+5a_{i5}{t}^4+4a_{i4}{t}^3+3a_{i3}{t}^2+2a_{i2}t+a_{i1})\end{array}---\left(8\right)$

$\begin{array}{c}{\stackrel{\·\·}{\mathrm{\θ}}}_i\left(t\right)=-A_{i1}\sin (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}t+a_{i0})\\ \·{(7a_{i7}{t}^6+6a_{i6}{t}^5+5a_{i5}{t}^4+4a_{i4}{t}^3+3a_{i3}{t}^2+2a_{i2}t+a_{i1})}^2\\ +A_{i1}\cos (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}t+a_{i0})\\ \·(42a_{i7}{t}^5+30a_{i6}{t}^4+20a_{i5}{t}^3+12a_{i4}{t}^2+6a_{i3}t+2a_{i2})\end{array}---\left(9\right)$

步骤二，化简待定参数：利用初始、终端、过程状态约束条件将未知参数用待定参数表示，以减少待定参数个数；

根据如下约束条件化简待定参数：

$\left\{\begin{array}{c}\mathrm{\Θ}\left(t_0\right)={\mathrm{\Θ}}_0,\mathrm{\Θ}\left(t_f\right)={\mathrm{\Θ}}_d\\ \stackrel{\·}{\mathrm{\Θ}}\left(t_0\right)=0,\stackrel{\·}{\mathrm{\Θ}}\left(t_f\right)=0\\ \stackrel{\·}{\mathrm{\Θ}}\left(t_0\right)=0,\stackrel{\·\·}{\mathrm{\Θ}}\left(t_f\right)=0\\ {\mathrm{\θ}}_{i\_\min }\≤{\mathrm{\θ}}_i\left(t\right)\≤{\mathrm{\θ}}_{i\_\max }\end{array}\right.---\left(2\right)$

其中，Θ＝[θ₁ θ₂ ... θ_n]^T，n为机械臂关节总数，1≤i≤n，且t₀≤t≤t_f。

将(2)式定义的两端与过程约束条件代入(1)、(8)、(9)式，可计算得到：

$\left\{\begin{array}{c}{A}_{i1}=\frac{{\mathrm{\θ}}_{i\_\max }-{\mathrm{\θ}}_{i\_\min }}{2},A_{i2}=\frac{{\mathrm{\θ}}_{i\_\max }+{\mathrm{\θ}}_{i\_\min }}{2}\\ a_{i0}={\sin }^{-1}\[({\mathrm{\θ}}_{i0}-A_{i2})/A_{i1}\]\\ a_{i1}=a_{i2}=0\\ a_{i3}=-\frac{3a_{i7}{t}_f^7+a_{i6}{t}_f^6-10(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^3}\\ a_{i4}=-\frac{8a_{i7}{t}_f^7+3a_{i6}{t}_f^6-15(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^4}\\ a_{i5}=-\frac{6a_{i7}{t}_f^7+3a_{i6}{t}_f^6-6(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^5}\end{array}\right.---\left(3\right)$

由此，a_i6、a_i7为待定参数，其余未知参数均用待定参数表示，定义：

$A=\left[\begin{array}{cccc}{a}_{16}& a_{26}& ...& a_{n6}\\ a_{17}& a_{27}& ...& a_{n7}\end{array}\right]---\left(4\right)$

由(4)式可见，通过对A中待定参数组合的设定，可以调节机械臂关节空间从初始状态Θ₀运动到期望终端状态Θ_d的路径，进而对本体姿态运动加以干预。

步骤三，设计适应度目标函数：基于终端时刻本体控制精度和过程约束要求设计合适的适应度目标函数；

适应度目标函数定义为：

$J=\frac{\left|\right|\mathrm{\δ}q\left|\right|}{K_q}+\frac{J_{\stackrel{\·}{\mathrm{\Θ}}}}{K_{\stackrel{\·}{\mathrm{\Θ}}}}+\frac{J_{\stackrel{\·\·}{\mathrm{\Θ}}}}{K_{\stackrel{\·\·}{\mathrm{\Θ}}}}+\frac{J_{\mathrm{\Δ}q}}{K_{\mathrm{\Δ}q}}---\left(5\right)$

δq为本体姿态四元数终值误差，||·||为求范数运算，K_q为根据本体姿态控制精度要求设定的阈值，只要||δq||＜K_q，即认为结果满足要求。 表示关节角速度和角加速度超出其允许值的百分比， 为相应阈值。为机械臂运动过程中星本体姿态q(t)相对于初值q(t₀)最大变化量超出其允许值Δq_limit的百分比，K_Δq为相应阈值。以上叙述中相应参数的表达式如下：

$\begin{array}{c}{J}_{{\stackrel{\·}{\mathrm{\θ}}}_i}=\left\{\begin{array}{cc}0;& {\stackrel{\·}{\mathrm{\θ}}}_{i\_\max }\≤{\stackrel{\·}{\mathrm{\θ}}}_{i\_\lim it}\\ \frac{{\stackrel{\·}{\mathrm{\θ}}}_{i\_\max }-{\stackrel{\·}{\mathrm{\θ}}}_{i\_\lim it}}{{\stackrel{\·}{\mathrm{\θ}}}_{i\_\lim it}};& {\stackrel{\·}{\mathrm{\θ}}}_{i\_\max }>{\stackrel{\·}{\mathrm{\θ}}}_{i\_\lim it}\end{array}\right.\\ J_{{\stackrel{\·\·}{\mathrm{\θ}}}_i}=\left\{\begin{array}{cc}0;& {\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\max }\≤{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\lim it}\\ \frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\max }-{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\lim it}}{{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\lim it}};& {\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\max }>{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\lim it}\end{array}\right.\\ J_{\mathrm{\Δ}q}=\left\{\begin{array}{cc}0;& {\mathrm{\Δq}}_{\max }\≤{\mathrm{\Δq}}_{\lim it}\\ \frac{{\mathrm{\Δq}}_{\max }-{\mathrm{\Δq}}_{\lim it}}{{\mathrm{\Δq}}_{\lim it}};& {\mathrm{\Δq}}_{\max }>{\mathrm{\Δq}}_{\lim it}\end{array}\right.\end{array}---\left(10\right)$

步骤四，待定参数的遗传算法寻优：基于本体-臂耦合动力学关系寻找使适应度目标函数最小的待定参数组合，据此确定机械臂完整运动路径。

首先，星本体-机械臂耦合动力学与运动学关系表示为：

${\mathrm{\ω}}_0=J_{ba\_\mathrm{\ω}}\stackrel{\·}{\mathrm{\Θ}}---\left(6\right)$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_0\end{array}\right]---\left(7\right)$

其中，J_{ba_ω}为本体角速度-机械臂关节角速度雅可比矩阵，需根据和机械臂各关节的历史构型进行实时计算：

$J_{ba\_\mathrm{\ω}}=-{\[M{\tilde{r}}_{0g}{\tilde{r}}_{0g}+\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(I_i+m_i{\tilde{r}}_{0i}^T{\tilde{r}}_{0i})+I_0\]}^{-1}\[\underset{i=1}{\overset{n}{\mathrm{\Σ}}}(I_i{J}_{Ri}+m_i{\tilde{r}}_{0i}{J}_{Ti})-{\tilde{r}}_{0g}{J}_{tw}\]---\left(11\right)$

其中，M—系统总质量，r_0g—本体质心到系统质心矢量，I_i—第i节臂的惯量矩阵(I₀为本体惯量阵)，m_i—第i节臂的质量，r_0i—本体质心到第i节臂质心矢量，J_Ri、J_Ti、J_tw是由各关节旋转方向单位矢量、各节臂质心位置矢量、各关节位置矢量的矩阵运算组成。

其次，适应度目标函数按如下流程进行计算：

1)对于一组确定的A值，代入(8)式计算各关节角速度，进而确定以及(5)式中的

2)根据(6)式计算J_{ba_ω}，进而计算本体的角速度ω₀；

3)根据(7)式计算本体四元数的时间导数

4)计算本体四元数终值以及(5)式中J_Δq；

5)根据(5)式计算适应度目标函数。

最后，采用遗传算法对待定参数矩阵A寻优的流程如下(算法参数：种群大小n_p＝40，复制概率p_c＝0.8，交叉概率p_m＝0.08，有效基因数n_e＝4，进化总代数N_{g_max}＝300)：

1)随机产生含n_p个个体的初始种群P₀；

2)计算每个个体的适应度目标函数：若存在某个体，其适应度目标函数(5)式每一项均小于1，则终止寻优，确定该个体为待定参数矩阵A的最优值，并根据此最优值确定的(1)式θ_i(t)和(6)式中计算机械臂最优运动路径；否则转3)；

3)确定当代种群的进化代数N_g：若N_g＝N_{g_max}，将当代种群中适应度目标函数最小的个体作为待定参数矩阵A的次优估计值，并根据此次优估计值确定的(1)式θ_i(t)和(6)式中计算机械臂次优运动路径；否则转4)；

4)进行复制、交叉、变异操作生成后代种群，N_g＝N_g+1，并转2)。

综上所述，本发明基于带操作臂的自由漂浮航天器本体-机械臂耦合动力学建模，运用遗传算法寻找最优的机械臂的运动路径，实现本体姿态及机械臂关节角同时达到期望状态，节省卫星本体姿态控制的能量消耗与任务执行时间，同时避免传统逆运动学关系求解过程中可能造成的动力学奇异问题，可运用到以在轨服务为背景的目标捕获后机械臂与本体综合控制任务。

Claims (7)

1.一种多自由度空间机械臂非完整路径规划方法，其特征在于，该方法包括如下的步骤：

步骤一，机械臂各关节运动规律函数化，采用正弦函数对关节角进行参数化设计；

步骤二，利用初始、终端、过程状态约束条件，将步骤一所述正弦函数中的未知参数用待定参数表示，以减少待定参数个数；

步骤三，基于终端时刻本体控制精度和过程约束要求，得到适应度目标函数；

步骤四，进行待定参数的遗传算法寻优，基于本体-臂耦合动力学关系寻找使适应度目标函数最小的待定参数组合，据此确定机械臂完整运动路径。

2.根据权利要求1所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤一中，采用了关于时间的五次多项式正弦函数对关节角进行参数化设计，以约束关节角的运动范围，并保证机械臂运动过程的平滑，函数关系表示为：

θ_i(t)＝A_i1sin(a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0)+A_i2 (1)

其中，变量下标i对应机械臂系统的第i个关节；

所述步骤二中，根据如下约束条件化简待定参数：

$\left\{\begin{array}{c}\mathrm{\Θ}\left(t_0\right)={\mathrm{\Θ}}_0,\mathrm{\Θ}\left(t_f\right)={\mathrm{\Θ}}_d\\ \stackrel{\·}{\mathrm{\Θ}}\left(t_0\right)=0,\stackrel{\·}{\mathrm{\Θ}}\left(t_f\right)=0\\ \stackrel{\·\·}{\mathrm{\Θ}}\left(t_0\right)=0,\stackrel{\·\·}{\mathrm{\Θ}}\left(t_f\right)=0\\ {\mathrm{\θ}}_{i\_\min }\≤{\mathrm{\θ}}_i\left(t\right)\≤{\mathrm{\θ}}_{i\_max}\end{array}\right.---\left(2\right)$

其中，Θ＝[θ₁ θ₂ ... θ_n]^T，n为机械臂关节总数，1≤i≤n，且t₀≤t≤t_f；

将(1)式求一阶及二阶导数，并将(2)式定义的约束条件代入，得到：

$\left\{\begin{array}{c}{A}_{i1}=\frac{{\mathrm{\θ}}_{i\_\max }-{\mathrm{\θ}}_{i\_\min }}{2},A_{i2}=\frac{{\mathrm{\θ}}_{i\_\max }+{\mathrm{\θ}}_{i\_\min }}{2}\\ a_{i0}={\sin }^{-1}\[({\mathrm{\θ}}_{i0}-A_{i2})/A_{i1}\]\\ a_{i1}=a_{i2}=0\\ a_{i3}=-\frac{3a_{i7}{t}_f^7+a_{i6}{t}_f^6-10(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^3}\\ a_{i4}=-\frac{8a_{i7}{t}_f^7+3a_{i6}{t}_f^6-15(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^4}\\ a_{i5}=-\frac{6a_{i7}{t}_f^7+3a_{i6}{t}_f^6-6(\arcsin \frac{{\mathrm{\θ}}_{id}-A_{i2}}{A_{i1}}-\arcsin \frac{{\mathrm{\θ}}_{i0}-A_{i2}}{A_{i1}})}{t_f^5}\end{array}\right.---\left(3\right)$

由此，a_i6、a_i7为待定参数，其余未知参数均用待定参数表示，定义：

$A=\left[\begin{array}{cccc}{a}_{16}& a_{26}& ...& a_{n6}\\ a_{17}& a_{\mathrm{27}}& ...& a_{n7}\end{array}\right]---\left(4\right)$

根据(4)式，通过对A中待定参数组合的设定，来调节机械臂关节空间从初始状态Θ₀运动到期望终端状态Θ_d的路径，进而对本体姿态运动加以干预。

3.根据权利要求2所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤三中，适应度目标函数表示为：

$\mathrm{J=}\frac{\mathrm{||}\mathrm{\δ}q\left|\right|}{K_q}+\frac{J_{\stackrel{\·}{\mathrm{\Θ}}}}{K_{\stackrel{\·}{\mathrm{\Θ}}}}+\frac{J_{\stackrel{\·\·}{\mathrm{\Θ}}}}{K_{\stackrel{\·\·}{\mathrm{\Θ}}}}+\frac{J_{\mathrm{\Δ}q}}{K_{\mathrm{\Δ}q}}---\left(5\right)$

其中，δq为本体姿态四元数终值误差，||·||为求范数运算，K_q为根据本体姿态控制精度要求设定的阈值，只要||δq||＜K_q，即认为结果满足要求； 分别表示关节角速度和角加速度超出其允许值的百分比，分别为与对应的阈值；J_Δq为机械臂运动过程中星本体姿态相对于初值最大变化量的百分比，K_Δq为与J_Δq对应的阈值。

4.根据权利要求3所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤四中，星本体-机械臂耦合动力学与运动学关系表示为：

${\mathrm{\ω}}_0=J_{ba\_\mathrm{\ω}}\stackrel{\·}{\mathrm{\Θ}}---\left(6\right)$

$\stackrel{\·}{q}=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_0\end{array}\right]---\left(7\right)$

其中，J_{ba_ω}为本体角速度-机械臂关节角速度雅可比矩阵，是根据和机械臂各关节的历史构型实时计算得到；q为本体姿态四元数。

5.根据权利要求4所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤四中，对适应度目标函数按如下流程进行计算：

S11、对于一组确定的A值，代入(1)式的一阶导数计算各关节角速度，进而确定以及(5)式中的

S12、根据(6)式计算本体的角速度ω₀；

S13、根据(7)式计算本体四元数的时间导数

S14、计算本体四元数终值以及(5)式中J_Δq；

S15、根据(5)式计算适应度目标函数。

6.根据权利要求5所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤四中，采用遗传算法对待定参数矩阵A寻优的流程如下：

S21、随机产生含n_p个个体的初始种群P₀；n_p为种群大小；

S22、计算每个个体的适应度目标函数：若存在任意个体的适应度目标函数的每一项均小于1，则终止寻优，确定该个体为待定参数矩阵A的最优值，并根据由此最优值确定的(1)式θ_i(t)和(6)式中计算机械臂最优运动路径；否则转S23；

S23、确定当代种群的进化代数N_g：若N_g＝N_{g_max}，将当代种群中适应度目标函数最小的个体作为待定参数矩阵A的次优估计值，并根据此次优估计值确定的(1)式θ_i(t)和(6)式中计算机械臂次优运动路径；否则转S24；

S24、进行复制、交叉、变异操作生成后代种群，N_g＝N_g+1，并转S22。

7.根据权利要求5所述的多自由度空间机械臂非完整路径规划方法，其特征在于，所述步骤四中，遗传算法的算法参数：种群大小n_p＝40，复制概率p_c＝0.8，交叉概率p_m＝0.08，有效基因数n_e＝4，进化总代数N_{g_max}＝300。