[发明专利]一种最小化基座碰撞扰动的空间机械臂轨迹规划方法

权利要求书

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

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

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

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

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

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

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

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

其中，Hω=MEMrog×TMrog×Hω]]>和H_φ分别为基座和机械臂的惯性矩阵，Hωφ=HvφTHωφTT]]>为耦合惯性矩阵，分别为基座的线加速度和角加速度，c_v,c_ω分别为线速度依赖项和角速度依赖项，F_b＝[f_b,τ_b]为基座所受外力/力矩，τ_m为关节输出力矩，F_e＝[f_e,τ_e]为机械臂末端所受外力/力矩，为基座雅克比矩阵，为机械臂雅克比矩阵，M为机械臂系统整体质量，为基座质心到系统质心向量的反对称矩阵。

消去线加速度可得：

H~ωH~ωφH~ωφTH~φω·bθ··+c~ωc~m=τ~bτ~m+J~bωTJ~mTFe---(2)]]>

其中，H~ω=Hω+Mrog×rog×,]]>H~ωφ=Hωφ-rog×Hvφ,]]>H~φ=Hφ-HvφTHvφM,]]>c~ω=cω-rog×cv,]]>c~m=cm-HvφTMcv,]]>τ~b=τb-rog×fb,]]>τ~m=τm-HvφTMfb,]]>J~bωT=JbωT-rog×JbvT,]]>J~mT=JmT-HvφTMJbvT.]]>

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

H~ωH~ωφH~ωφTH~φδωbδθ·=J~bωTJ~mTF‾e---(3)]]>

其中，F‾e=∫t0t0+dtFedt.]]>

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

δωb=(H~ω-H~ωφH~φ-1H~ωφT)-1(J~bωT-H~ωφH~φ-1J~mT)F‾e---(4)]]>

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

δθ·=H*-1J*TF‾e---(5)]]>

其中，H*=Hφ-HωφTHω-1Hωφ,]]>J*=Jm-JbHω-1Hωφ]]>为广义雅克比矩阵。

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

x·e=J*θ·---(6)]]>

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

δx·e=J*δθ·=J*H*-1J*TF‾e=DF‾e---(7)]]>

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

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

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

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

F‾e=-(1+e)υrNT(D1+D2)N---(9)]]>

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

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

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

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

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

设计基座碰撞扰动函数：

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

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

H(θ)=ρΣi=1n(θimax-θimin)2(θimax-θi)(θi-θimin)---(12)]]>

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

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

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

这样设计可以满足：

ρ1↓,ρ2↑ifθi→θimaxorθi→θiminρ1↑,ρ2↓else]]>

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