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

技术特征：

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。