CN104526695B - 一种最小化基座碰撞扰动的空间机械臂轨迹规划方法 - Google Patents

Abstract

本发明公开了一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，属于机械臂控制技术领域。在建立空间机械臂运动学方程、动力学方程的基础上，推导出机械臂基座姿态扰动方程；在考虑空间机械臂末端捕获位姿精度、关节限位的情况下，设计综合优化算子，并在零空间内对机械臂构型优化以实现由碰撞引起的基座扰动最小化；最后，采用粒子群优化算法实现空间机械臂由初始位姿到理想位姿的碰前轨迹规划。本发明解决了空间机械臂碰前轨迹规划问题，其控制过程简单易懂，综合优化算子设计新颖实用，通过该规划方法可以实现在确保机械臂末端捕获位姿精确，关节角度不超限的前提下尽量减小碰撞引起的基座姿态扰动。

Description

一种最小化基座碰撞扰动的空间机械臂轨迹规划方法

技术领域

本发明涉及一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，属于机械臂控制技术领域。

背景技术

空间机械臂执行在轨组装或者抓捕等接触任务时，一般经历三个阶段：预碰撞、接触碰撞及碰撞后控制。预碰撞阶段不仅要完成机械臂末端抓捕位姿的调整，通常还需要进行机械臂构型及轨迹的优化，以实现机械臂碰撞脉冲最小化或者碰撞对基座扰动最小等目标。针对基座惯量较小的机械臂系统，当受到碰撞冲击时，基座会受到较大的扰动，而基座的姿态往往是维持空间机械臂与地面通信的关键，因此，研究如何减小碰撞引起的基座扰动对空间机械臂执行在轨捕获或者装配任务很有必要。目前，解决此问题通常有两种方法：一是采用附加部件，如采用反作用轮或者是增加一条平衡臂来抵消基座的扰动，但是此类方法会显著的增加空间机械臂系统的质量，并且平衡能力受到其机构的影响；二是采用控制算法，如规划机械臂碰撞前的构型、轨迹，或者减小碰撞相对速度等，但是目前的方法通常存在几个问题：①优化能力有限；②未考虑到捕获位姿；③未考虑到机械臂关节角实际的限制。本发明采用控制算法实现由碰撞引起的基座扰动最小化，为了解决上述问题，一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，其设计思路新颖、控制过程简单，能够同时解决上述三个问题。

发明内容

本发明的目的是针对空间机械臂执行在轨组装或者抓捕等接触任务，提供一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，以在考虑关节限位、捕获位姿的前提下最大限度的减小碰撞引起的基座扰动。

一种最小化基座碰撞扰动的空间机械臂轨迹规划方法由以下步骤完成：

步骤一、采用拉格朗日方法建立空间机械臂动力学模型，消去基座线速度变量，推导出含有关节角变量和基座角速度变量的空间机械臂动力学方程(见式(2))；

步骤二、在极短时间内对碰撞动力学方程积分，消去关节角变量，结合碰撞脉冲表达式导出基座姿态扰动与空间机械臂运动学参数、动力学参数、碰撞方向、碰撞相对速度及恢复系数的关系(见式(10))；

步骤三、设计综合考虑关节限位及基座扰动的优化因子，在冗余度机械臂的零空间内进行优化，直到达到平衡稳定的状态，得到此时机械臂碰撞前的最优构型；

步骤四、采用正弦七次多项式拟合机械臂关节角度，利用粒子群算法求得相应的控制参数，完成空间机械臂从初始构型到理想构型的规划。

本发明的优点

本发明主要涉及一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，针对空间机械臂的捕获任务，通过采用设计的优化因子可以实现在确保机械臂末端捕获位姿精确，关节角度不超限的前提下尽量减小碰撞引起的基座姿态扰动(见实例1)。

附图说明

图1是本发明实施例1中的七自由度空间机械臂模型；

图2是本发明实施例1中零空间优化过程中关节角度变化；

图3是本发明实施例1中零空间优化过程中基座扰动值变化；

图4是本发明实施例1中碰前关节角度变化；

图5是本发明实施例1中碰前关节角速度变化；

具体实施方式

本发明提供了一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，下面结合附图对本发明作进一步说明。

一、基座姿态扰动方程

采用拉格朗日法建立空间机械臂动力学一般方程为：

其中， $H_{\mathrm{\ω}}=\left[\begin{array}{cc}ME& {{\mathrm{Mr}}_{og}^{\×}}^T\\ {\mathrm{Mr}}_{og}^{\×}& H_{\mathrm{\ω}}\end{array}\right]$ 和H_f分别为基座和机械臂的惯性矩阵， $H_{\mathrm{\ω}\mathrm{\φ}}={\left[\begin{array}{cc}{H}_{v\mathrm{\φ}}^T& H_{\mathrm{\ω}\mathrm{\φ}}^T\end{array}\right]}^T$ 为耦合惯性矩阵，分别为基座的线加速度和角加速度，c_v,c_ω分别为线速度依赖项和角速度依赖项，F_b＝[f_b,τ_b]为基座所受外力/力矩，τ_m为关节输出力矩，F_e＝[f_e,τ_e]为机械臂末端所受外力/力矩，为基座雅克比矩阵，为机械臂雅克比矩阵，M为机械臂系统整体质量，为基座质心到系统质心向量的反对称矩阵。

消去线加速度可得：

$\left[\begin{array}{cc}{\tilde{H}}_{\mathrm{\ω}}& {\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}\\ {\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}^T& {\tilde{H}}_{\mathrm{\φ}}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_b\\ \stackrel{\·\·}{\mathrm{\θ}}\end{array}\right]+\left[\begin{array}{c}{\tilde{c}}_{\mathrm{\ω}}\\ {\tilde{c}}_m\end{array}\right]=\left[\begin{array}{c}{\widetilde{\mathrm{\τ}}}_b\\ {\widetilde{\mathrm{\τ}}}_m\end{array}\right]+\left[\begin{array}{c}{\tilde{J}}_{b\mathrm{\ω}}^T\\ {\tilde{J}}_m^T\end{array}\right]F_e---\left(2\right)$

其中， ${\tilde{H}}_{\mathrm{\ω}}=H_{\mathrm{\ω}}+{\mathrm{Mr}}_{og}^{\×}{r}_{og}^{\×},{\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}=H_{\mathrm{\ω}\mathrm{\φ}}-r_{og}^{\×}{H}_{v\mathrm{\φ}},{\tilde{H}}_{\mathrm{\φ}}=H_{\mathrm{\φ}}-\frac{H_{v\mathrm{\φ}}^T{H}_{v\mathrm{\φ}}}{M},{\tilde{c}}_{\mathrm{\ω}}=c_{\mathrm{\ω}}-r_{og}^{\×}{c}_v,$ ${\tilde{c}}_m=c_m-\frac{H_{v\mathrm{\φ}}^T}{M}{c}_v,{\widetilde{\mathrm{\τ}}}_b={\mathrm{\τ}}_b-r_{og}^{\×}{f}_b,{\widetilde{\mathrm{\τ}}}_m={\mathrm{\τ}}_m-\frac{H_{v\mathrm{\φ}}^T}{M}{f}_b,{\tilde{J}}_{b\mathrm{\ω}}^T-J_{b\mathrm{\ω}}^T-r_{og}^{\×}{J}_{bv}^T,{\tilde{J}}_m^T=J_m^T-\frac{H_{v\mathrm{\φ}}^T}{M}{J}_{bv}^T.$

对式(2)在极短时间内积分，消去惯性项及速度依赖项，将所有的加速度项替换成极短时间内的速度改变量，可得：

$\left[\begin{array}{cc}{\tilde{H}}_{\mathrm{\ω}}& {\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}\\ {\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}^T& {\tilde{H}}_{\mathrm{\φ}}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}{\mathrm{\ω}}_b\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}{\tilde{J}}_{b\mathrm{\ω}}^T\\ {\tilde{J}}_m^T\end{array}\right]{\stackrel{\‾}{F}}_e---\left(3\right)$

其中， ${\stackrel{\‾}{F}}_e={\∫}_{t_0}^{t_0+dt}{F}_{e}dt.$

消去关节角速度变化量可得基座姿态扰动量表达式：

${\mathrm{\δ\ω}}_b={({\tilde{H}}_{\mathrm{\ω}}-{\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}{\tilde{H}}_{\mathrm{\φ}}^{-1}{\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}^T)}^{-1}({\tilde{J}}_{b\mathrm{\ω}}^T-{\tilde{H}}_{\mathrm{\ω}\mathrm{\φ}}{\tilde{H}}_{\mathrm{\φ}}^{-1}{\tilde{J}}_m^T){\stackrel{\‾}{F}}_e---\left(4\right)$

至此，推得基座扰动与碰撞脉冲的关系，又由式(3)消去基座姿态项，可得：

$\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=H^{*-1}{J}^{*T}{\stackrel{\‾}{F}}_e---\left(5\right)$

其中， $H^{*}=H_{\mathrm{\φ}}-H_{\mathrm{\ω}\mathrm{\φ}}^T{H}_{\mathrm{\ω}}^{-1}{H}_{\mathrm{\ω}\mathrm{\φ}},J^{*}=J_m-J_b{H}_{\mathrm{\ω}}^{-1}{H}_{\mathrm{\ω}\mathrm{\φ}}$ 为广义雅克比矩阵。

假设机械臂初始的角动量和线动量均为零，则速度级的运动学方程为：

${\stackrel{\·}{x}}_e=J^{*}\stackrel{\·}{\mathrm{\θ}}---\left(6\right)$

对式(6)在极短时间内积分：

$\mathrm{\δ}{\stackrel{\·}{x}}_e=J^{*}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=J^{*}{H}^{*-1}{J}^{*T}{\stackrel{\‾}{F}}_e=D{\stackrel{\‾}{F}}_e---\left(7\right)$

将碰撞过程分为压缩阶段和恢复阶段，在压缩阶段最后时刻两物体接触面的法向相对速度为0。假设碰撞前速度分为υ_e1、υ_e2，至压缩最终阶段速度变化为δυ_e1、δυ_e2，则在压缩最终阶段有如下式子：

N^T(υ_e2+δυ_e2-υ_e1-δυ_e1)＝0(8)

其中，N为碰撞处法向向量。

结合式(7)、式(8)以及恢复系数定义可推导出碰撞脉冲表达式：

${\stackrel{\‾}{F}}_e=-(1+e)\frac{{\mathrm{\υ}}_r}{N^T(D_1+D_2)N}---\left(9\right)$

其中，υ_r＝υ_e1-υ_e2为碰撞相对速度，e为材料的恢复系数，D₁、D₂分别为机械臂和目标物的雅克比惯性矩阵。

将式(9)代入到式(4)中，可得最终的基座姿态扰动表达式：

δω_b＝f(θ,N,υ_r)(10)

二、基于零空间的基座姿态扰动最小化

由式(6)可计算关节角速度为：

其中，称作零空间，k为增益系数，为任意速度级向量。

为优化基座扰动项，引入函数g：

g＝||δω_b||(12)

考虑到机械臂运行过程中关节限位的要求，有如下函数：

$H\left(\mathrm{\θ}\right)=\mathrm{\ρ}\underset{i=1}{\overset{n}{\Σ}}\frac{{({\mathrm{\θ}}_{imax}-{\mathrm{\θ}}_{imin})}^2}{({\mathrm{\θ}}_{i\max }-{\mathrm{\θ}}_i)({\mathrm{\θ}}_i-{\mathrm{\θ}}_{imin})}---\left(13\right)$

其中，θ_imax、θ_imin为第i个关节角的上、下限，ρ为调节系数。

为保证在关节未超限的前提下，尽最大可能优化基座姿态扰动，设计如下优化因子：

其中， ρ₂＝bH(θ)，a、b为常量。

这样设计可以满足：

$\left\{\begin{array}{cc}{\mathrm{\ρ}}_1\↓,{\mathrm{\ρ}}_2\↑& \begin{array}{cc}if& {\mathrm{\θ}}_i\→{\mathrm{\θ}}_{imax}{\mathrm{or\θ}}_i\→{\mathrm{\θ}}_{imin}\end{array}\\ {\mathrm{\ρ}}_1\↑,{\mathrm{\ρ}}_2\↓& else\end{array}\right.$

同时为了确保抓捕位姿的精确，在整个优化过程中令由此可得机械臂在优化过程中的关节角速度为：

为确保初始时刻机械臂关节角速度不发生突变，设计k如下：

$k=\left\{\begin{array}{cc}0& 0\&le;t\&le;T_R\\ -\sin (\mathrm{\&tau;}-\frac{\mathrm{\&pi;}}{2})-1& T_R<t\&le;T_m\\ -2& T_m<t\end{array}\right.---\left(16\right)$

其中，T_R、T_m分别为过渡开始和结束的时刻。

三、空间机械臂碰前轨迹规划

首先采用正弦7次多项式插值法，建立机械臂关节角的表达式如下：

θ_i(t)＝△_i1sin(a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t+a_i0)+△_i2(17)

其中，a_i7,a_i6,...,a_i0为多项式的系数，

同时，机械臂在运行过程中需要满足如下约束条件：

$\begin{array}{c}\mathrm{\&theta;}\left(t_0\right)={\mathrm{\&Theta;}}_0,\mathrm{\&theta;}\left(t_f\right)={\mathrm{\&Theta;}}_d,\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(t_0\right)=0,\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\left(t_0\right)=0,\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\left(t_f\right)=0,\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\left(t_f\right)=0\\ {\mathrm{\&theta;}}_{i\min }\&le;{\mathrm{\&theta;}}_i\left(t\right)\&le;{\mathrm{\&theta;}}_{i\max }\end{array}---\left(18\right)$

其中，t₀,t_f分别为开始和结束时刻，Θ₀,Θ_d分别为初始构型和理想构型，

通过以上约束解得：

$a_{i0}=\arcsin \left(\frac{{\mathrm{\&theta;}}_{i0}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}}\right)$

a_i1＝a_i2＝0

$a_{i3}=-(3a_{i7}{t}_f^7+a_{i6}{t}_f^6-10\&lsqb;\arcsin (\frac{{\mathrm{\&theta;}}_{id}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})-\arcsin (\frac{{\mathrm{\&theta;}}_{i0}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})\&rsqb;)/{t_f}^3---\left(19\right)$

$a_{i4}=(8a_{i7}{t}_f^7+3a_{i6}{t}_f^6-15\&lsqb;\arcsin (\frac{{\mathrm{\&theta;}}_{id}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})-\arcsin (\frac{{\mathrm{\&theta;}}_{i0}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})\&rsqb;)/{t_f}^4$

$a_{i5}=-(6a_{i7}{t}_f^7+3a_{i6}{t}_f^6-6\&lsqb;\arcsin (\frac{{\mathrm{\&theta;}}_{id}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})-\arcsin (\frac{{\mathrm{\&theta;}}_{i0}-{\mathrm{\&Delta;}}_{i2}}{{\mathrm{\&Delta;}}_{i1}})\&rsqb;)/{t_f}^5$

由式(15)可以看到所有的关节角度就只与(a_i6,a_i7)有关，故设计a＝[a₁₆,a₁₇,a₂₆,a₂₇,...,a_n6,a_n7]为粒子群算法中的粒子。然后设计目标函数，以机械臂最终末端捕获位姿为目标，其中末端位置采用速度积分公式：

$P_e\left(t\right)={\&Integral;}_0^{t_f}{J}_{mv}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}dt---\left(20\right)$

末端姿态采用四元数：

Q＝η+q₁i+q₂j+q₃k(21)

其变化率为：

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}\left[\begin{array}{c}-q^T\\ \mathrm{\&eta;}E-q^{\&times;}\end{array}\right]J_{m\mathrm{\&omega;}}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}---\left(22\right)$

其中，q＝[q₁,q₂,q₃]^T。

设定{η_ef,q_ef},{η_ed,q_ed}分别为机械臂末端初始和理想的位姿，其偏差为：

${\mathrm{\&delta;q}}_e={\mathrm{\&eta;}}_{ef}{q}_{ed}-{\mathrm{\&eta;}}_{ed}{q}_{ef}-q_{ef}^{\&times;}{q}_{ed}---\left(23\right)$

${\mathrm{\&delta;P}}_e=P_{ed}-P_{ef}=P_{ed}-{\&Integral;}_0^{t_f}{J}_{mv}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}dt---\left(24\right)$

则粒子群算法的目标函数设计为：

f＝Wα(25)

其中，α＝[||δq_e||||δP_e||]^T，W＝[w₁w₂]为权重系数。

实施例1：

根据本发明所提供的一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，以如图1所示的七自由度空间机械臂为研究对象展开验证，机械臂的D-H参数以及动力学参数如表1、表2所示。

表1空间机械臂D-H参数表

表2空间机械臂动力学参数

用说明书所述的方法推导基座姿态扰动方程，并利用设计的综合优化因子在空间机械臂零空间对其构型进行优化，最后采用粒子群优化方法规划出从初始位姿到理想位姿的碰前轨迹。设定相关参数如下：

理想的末端位姿：[7.00m,0.00m,3.00m,-1.00rad,-0.50rad,-2.00rad]；

空间机械臂关节角度：Θ₀＝[-50°,-170°,150°,-60°,130°,170°,0°]；

关节限位：Θ_max＝[160°,200°,180°,170°,200°,200°,180°]；

Θ_min＝[-160°,-220°,-180°,-150°,-180°,-226°,-80°]；

碰撞方向：N＝[0.30,-0.30,0.91]^T；

碰撞目标物质量：m_o＝30kg；

碰撞时相对速度大小：υ_r＝0.05m/s；

材料恢复系数：e＝0.8；

粒子群算法相关参数：加速常数c₁＝c₂＝2.0，种群数24，搜寻200代，目标函数权重系数w₁＝w₂＝50。

利用说明书中的式(11)，可计算在保证末端捕获位姿精度、考虑关节限位的前提下，使得碰撞引起的基座扰动最小的理想构型：

[-94.74°,-171.80°,136.50°,-89.82°,195.76°,198.96°,-22.02°]；

优化过程中关节角度的变化如图2所示，基座扰动值(式(12))的变化如图3所示，幅值降低达50.57％，利用粒子群优化方法规划机械臂从初始位姿到理想位姿的关节角及关节角速度变化分别如图4、图5所示。

Claims (3)

1.一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，其特征在于：所述方法由以下步骤完成：

步骤一、采用拉格朗日方法建立空间机械臂动力学模型，消去基座线速度变量，推导出含有关节角变量和基座角速度变量的空间机械臂动力学方程；

步骤二、在极短时间内对碰撞动力学方程积分，消去关节角变量，结合碰撞脉冲表达式导出基座姿态扰动与空间机械臂运动学参数、动力学参数、碰撞方向、碰撞相对速度及恢复系数的关系；

步骤三、设计综合考虑关节限位及基座扰动的优化因子，在冗余度机械臂的零空间内进行优化，直到达到平衡稳定的状态，得到此时机械臂碰撞前的最优构型；

步骤四、采用正弦七次多项式拟合机械臂关节角度，利用粒子群算法求得相应的控制参数，完成空间机械臂从初始构型到理想构型的规划。

2.根据权利要求1所述的一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，其特征在于：

在步骤二中结合空间机械臂基座姿态扰动方程和碰撞脉冲函数，导出基座姿态扰动与空间机械臂运动学参数、动力学参数、碰撞方向、碰撞相对速度及恢复系数的关系，其过程为：

采用拉格朗日法建立空间机械臂动力学一般方程为：

其中， $H_{\mathrm{\&omega;}}=\left[\begin{array}{cc}ME& {\mathrm{Mr}}_{og}^{\&times;T}\\ {\mathrm{Mr}}_{og}^{\&times;}& H_{\mathrm{\&omega;}}\end{array}\right]$ 和H_f分别为基座和机械臂的惯性矩阵， $H_{\mathrm{\&omega;}\mathrm{\&phi;}}={\left[\begin{array}{cc}{H}_{v\mathrm{\&phi;}}^T& H_{\mathrm{\&omega;}\mathrm{\&phi;}}^T\end{array}\right]}^T$ 为耦合惯性矩阵，分别为基座的线加速度和角加速度，c_v,c_ω分别为线速度依赖项和角速度依赖项，F_b＝[f_b,τ_b]为基座所受外力/力矩，τ_m为关节输出力矩，F_e＝[f_e,τ_e]为机械臂末端所受外力/力矩，为基座雅克比矩阵，为机械臂雅克比矩阵，M为机械臂系统整体质量，为基座质心到系统质心向量的反对称矩阵；

消去线加速度可得：

$\left[\begin{array}{cc}{\tilde{H}}_{\mathrm{\&omega;}}& {\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}\\ {\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}^T& {\tilde{H}}_{\mathrm{\&phi;}}\end{array}\right]\left[\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&omega;}}}_b\\ \stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]+\left[\begin{array}{c}{\tilde{c}}_{\mathrm{\&omega;}}\\ {\tilde{c}}_m\end{array}\right]=\left[\begin{array}{c}{\widetilde{\mathrm{\&tau;}}}_b\\ {\widetilde{\mathrm{\&tau;}}}_m\end{array}\right]+\left[\begin{array}{c}{\tilde{J}}_{b\mathrm{\&omega;}}^T\\ {\tilde{J}}_m^T\end{array}\right]F_e---\left(2\right)$

其中， ${\tilde{H}}_{\mathrm{\&omega;}}=H_{\mathrm{\&omega;}}+{\mathrm{Mr}}_{og}^{\&times;}{r}_{og}^{\&times;},$ ${\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}=H_{\mathrm{\&omega;}\mathrm{\&phi;}}-r_{og}^{\&times;}{H}_{v\mathrm{\&phi;}},$ ${\tilde{H}}_{\mathrm{\&phi;}}=H_{\mathrm{\&phi;}}-\frac{H_{v\mathrm{\&phi;}}^T{H}_{v\mathrm{\&phi;}}}{M},$ ${\tilde{c}}_{\mathrm{\&omega;}}=c_{\mathrm{\&omega;}}-r_{og}^{\&times;}{c}_v,$ ${\tilde{c}}_m=c_m-\frac{H_{v\mathrm{\&phi;}}^T}{M}{c}_v,$ ${\widetilde{\mathrm{\&tau;}}}_b={\mathrm{\&tau;}}_b-r_{og}^{\&times;}{f}_b,$ ${\widetilde{\mathrm{\&tau;}}}_m={\mathrm{\&tau;}}_m-\frac{H_{v\mathrm{\&phi;}}^T}{M}{f}_b,$ ${\tilde{J}}_{b\mathrm{\&omega;}}^T-J_{b\mathrm{\&omega;}}^T-r_{og}^{\&times;}{J}_{bv}^T,$ ${\tilde{J}}_m^T-J_m^T-\frac{H_{v\mathrm{\&phi;}}^T}{M}{J}_{bv}^T;$

对式(2)在极短时间内积分，消去惯性项及速度依赖项，将所有的加速度项替换成极短时间内的速度改变量，可得：

$\left[\begin{array}{cc}{\tilde{H}}_{\mathrm{\&omega;}}& {\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}\\ {\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}^T& {\tilde{H}}_{\mathrm{\&phi;}}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}{\mathrm{\&omega;}}_b\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]=\left[\begin{array}{c}{\tilde{J}}_{b\mathrm{\&omega;}}^T\\ {\tilde{J}}_m^T\end{array}\right]{\stackrel{\&OverBar;}{F}}_e---\left(3\right)$

其中， ${\stackrel{\&OverBar;}{F}}_e={\&Integral;}_{t_0}^{t_0+dt}{F}_{e}dt;$

消去关节角速度变化量可得基座姿态扰动量表达式：

${\mathrm{\&delta;\&omega;}}_b={({\tilde{H}}_{\mathrm{\&omega;}}-{\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}{\tilde{H}}_{\mathrm{\&phi;}}^{-1}{\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}^T)}^{-1}({\tilde{J}}_{b\mathrm{\&omega;}}^T-{\tilde{H}}_{\mathrm{\&omega;}\mathrm{\&phi;}}{\tilde{H}}_{\mathrm{\&phi;}}^{-1}{\tilde{J}}_m^T){\stackrel{\&OverBar;}{F}}_e---\left(4\right)$

至此，推得基座扰动与碰撞脉冲的关系，又由式(3)消去基座姿态项，可得：

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}=H^{*-1}{J}^{*T}{\stackrel{\&OverBar;}{F}}_e---\left(5\right)$

其中， $H^{*}=H_{\mathrm{\&phi;}}-H_{\mathrm{\&omega;}\mathrm{\&phi;}}^T{H}_{\mathrm{\&omega;}}^{-1}{H}_{\mathrm{\&omega;}\mathrm{\&phi;}},$ $J^{*}=J_m-J_b{H}_{\mathrm{\&omega;}}^{-1}{H}_{\mathrm{\&omega;}\mathrm{\&phi;}}$ 为广义雅克比矩阵；

假设机械臂初始的角动量和线动量均为零，则速度级的运动学方程为：

${\stackrel{\&CenterDot;}{x}}_e=J^{*}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}---\left(6\right)$

对式(6)在极短时间内积分：

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{x}}_e=J^{*}\mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&theta;}}=J^{*}{H}^{*-1}{J}^{*T}{\stackrel{\&OverBar;}{F}}_e=D{\stackrel{\&OverBar;}{F}}_e---\left(7\right)$

将碰撞过程分为压缩阶段和恢复阶段，在压缩阶段最后时刻两物体接触面的法向相对速度为0，假设碰撞前速度分为υ_e1、υ_e2，至压缩最终阶段速度变化为δυ_e1、δυ_e2，则在压缩最终阶段有如下式子：

N^T(υ_e2+δυ_e2-υ_e1-δυ_e1)＝0(8)

其中，N为碰撞处法向向量；

结合式(7)、式(8)以及恢复系数定义可推导出碰撞脉冲表达式：

${\stackrel{\&OverBar;}{F}}_e=-(1+e)\frac{{\mathrm{\&upsi;}}_r}{N^T(D_1+D_2)N}---\left(9\right)$

其中，υ_r＝υ_e1-υ_e2为碰撞相对速度，e为材料的恢复系数，D₁、D₂分别为机械臂和目标物的雅克比惯性矩阵；

将式(9)代入到式(4)中，可得最终的基座姿态扰动表达式：

δω_b＝f(θ,N,υ_r)(10)。

3.根据权利要求1所述的一种最小化基座碰撞扰动的空间机械臂轨迹规划方法，其特征在于：

在步骤三中综合考虑基座碰撞扰动最小及关节限位，设计了综合优化算子，其过程为：

设计基座碰撞扰动函数：

g＝||δω_b||(11)

考虑到机械臂运行过程中关节限位的要求，有如下函数：

$H\left(\mathrm{\&theta;}\right)=\mathrm{\&rho;}\underset{i=1}{\overset{n}{\&Sigma;}}\frac{{({\mathrm{\&theta;}}_{imax}-{\mathrm{\&theta;}}_{imin})}^2}{({\mathrm{\&theta;}}_{imax}-{\mathrm{\&theta;}}_i)({\mathrm{\&theta;}}_i-{\mathrm{\&theta;}}_{imin})}---\left(12\right)$

其中，θ_imax、θ_imin为第i个关节角的上、下限，ρ为调节系数；

为保证在关节未超限的前提下，尽最大可能优化基座姿态扰动，设计如下优化因子：

其中， ρ₂＝bH(θ)，a、b为常量；

这样设计可以满足：

$\left\{\begin{array}{cc}{\mathrm{\&rho;}}_1\&DownArrow;,{\mathrm{\&rho;}}_2\&UpArrow;& if{\mathrm{\&theta;}}_i\&RightArrow;{\mathrm{\&theta;}}_{imax}or{\mathrm{\&theta;}}_i\&RightArrow;{\mathrm{\&theta;}}_{imin}\\ {\mathrm{\&rho;}}_1\&UpArrow;,{\mathrm{\&rho;}}_2\&DownArrow;& else\end{array}\right.$

保证空间机械臂关节在不超限的情况，尽量减小由碰撞引起的基座扰动。