[发明专利]一种针对运动学冗余双臂空间机器人的协同路径规划方法

权利要求书

1.一种针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，包括以下步骤：

步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；

步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；

步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；

步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划；

步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。

2.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤1的具体方法为：

基于拉格朗日原理建立空间机器人系统的动力学方程：

其中，是基座的线加速度和角加速度，表示机械臂a的角加速度，表示机械臂b的角加速度，上标的a和b分别表示机械臂a和机械臂b；H_b为基座的惯量矩阵，和分别为两机械臂的与基座的耦合惯量矩阵，和分别为两机械臂的惯量矩阵，c_b为基座的非线性项，和分别为两机械臂的非线性项；f_b为施加于基座的广义力，和分别为施加于各关节的力矩；和分别为两机械臂对基座的Jacobian矩阵的转置矩阵；和分别为两机械臂的Jacobian矩阵的转置矩阵；和分别为施加于两机械臂末端的广义力；

根据动量守恒定律，整个空间机器人系统的线动量和角动量的守恒方程：

其中，M₀为空间机械臂系统的总动量；P₀和L₀为空间机械臂系统的线动量和角动量；m_b和m_i为基座和各连杆的质量；和为基座和各连杆质心的速度；I_b为基座的惯量矩阵，ω_b和ω_i为基座和各连杆的旋转角速度，为基座的线速度与角速度，为机械臂b的关节角速，为机械臂a的关节角速度，r_b为基座的位置向量，I_i为机械臂连杆的惯量矩阵，r_Ci为机械臂连杆的位置向量；

由于H_b可逆；得到基座运动方程：

其中，J_a为基座的广义Jacobian矩阵，为机械臂的关节角速度向量；

将基座运动方程代入末端执行器a的运动学链式方程和末端执行器b的运动学链式方程中，得到末端执行器运动学方程：

其中，J_d是描述系统初始动量对机械臂运动影响的耦合矩阵，J_g是广义雅克比矩阵，为机械臂a的末端执行器速度，为机械臂a对基座影响的Jacobian矩阵，为机械臂b的末端执行器速度，为机械臂b对基座影响的Jacobian矩阵，为机械臂a的末端执行器Jacobian矩阵，为机械臂b的末端执行器Jacobian矩阵，为机械臂a的广义Jacobian矩阵，为机械臂b的广义Jacobian矩阵。

3.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤2的具体方法为：

通过动量守恒方程得到空间机器人系统满足的非完整约束方程：

其中，I_s为整个机器人系统的惯性矩阵，和为基座和机械臂之间的耦合惯性矩阵；非完整约束方程为关节速度约束方程；

根据末端执行器的运动学方程得到末端执行器的逆运动学冗余解，末端执行器的逆运动学冗余解由伪逆特解和齐次方程的一般解组成：

其中，是J_g的Moore-Penrose伪逆，E_n为单位矩阵，h是任意矢量；

将末端执行器的逆运动学冗余解代入关节速度约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程：