[发明专利]一种针对闭链奇异点回避的双臂协同运动规划方法及系统

权利要求书

1.一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，包括：

获取被搬运物体的起始点和终止点；

根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；

根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；

对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。

2.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，根据被搬运物体的起始点和终止点，确定双臂闭链系统在任务空间中的路径；

根据双臂闭链系统在任务空间中的路径和闭链运动学模型，确定闭链系统的关节路径。

3.如权利要求2所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，将起始点和终止点分别分解为位置向量和姿态向量；

根据起止点的位置向量和终止点的位置向量，获得位置路径；

根据起止点的姿态向量和终止点的姿态向量，确定姿态路径；

将位置路径和姿态路径进行叠加，获得双臂闭链系统在任务空间中的路径。

4.如权利要求2所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，闭链运动学模型包括通过D-H法为每个单臂分别构建的机械臂运动学方程和双臂协同搬运运动学约束。

5.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，还包括：对于不处于奇异区域的路径点，通过雅克比逆映射，确定双臂的关节速度。

6.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，根据主臂的关节速度和从臂关节速度更新方程，确定从臂的关节速度，其中，从臂关节速度更新方程为：

其中，D₁和D₂为奇异区域判断系数，当系统位于奇异区域时，D₁＝1、D₂＝0；λ₁和λ₂为主从臂判断系数，当左臂为主臂时，λ₁＝1、λ₂＝0，反之，λ₁＝0、λ₂＝1；和分别为左右臂的关节速度，和分别为基于阻尼最小二乘法的左右臂的雅可比伪逆，和分别为根据主臂的近似解得到的从臂雅可比伪逆。

7.如权利要求1所述的一种针对闭链奇异点回避的双臂协同运动规划方法，其特征在于，以机械臂末端实际速度与期望速度误差的范数最小为目标，以机械臂关节速度变化范数小于等于设定阈值为约束，采用序列二次规划的方法确定阻尼最小二乘的雅可比伪逆法中的阻尼因子。

8.一种针对闭链奇异点回避的双臂协同运动规划系统，其特征在于，包括：

起止点获取模块，用于获取被搬运物体的起始点和终止点；

雅可比确定模块，用于根据被搬运物体的起始点和终止点，确定闭链系统的关节路径及雅可比；

规划路径奇异区域确定模块，用于根据闭链系统的雅可比确定闭链系统关节路径的奇异区域部分；

闭链系统无奇异点的规划路径确定模块，用于对于奇异区域部分，确定闭链系统的主臂和从臂，基于阻尼最小二乘的雅可比伪逆法确定主臂的关节速度；根据主臂的关节速度，确定从臂的关节速度；对各臂的关节速度进行积分，获得各臂的关节角度变化值；通过各臂的关节角度变化值，替换关节路径中的奇异区域部分，获得闭链系统无奇异点的规划路径。

9.一种电子设备，其特征在于，包括存储器和处理器以及存储在存储器上并在处理器上运行的计算机指令，所述计算机指令被处理器运行时，完成权利要求1-7任一项所述的一种针对闭链奇异点回避的双臂协同运动规划方法的步骤。

10.一种计算机可读存储介质，其特征在于，用于存储计算机指令，所述计算机指令被处理器执行时，完成权利要求1-7任一项所述的一种针对闭链奇异点回避的双臂协同运动规划方法的步骤。