[发明专利]一种基于突加度的障碍物躲避运动规划方法

摘要

本发明公开一种基于突加度的障碍物躲避运动规划方法，包括：1）设计基于突加度的障碍物躲避运动性能指标，所设计的运动性能指标受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限，生成二次型优化冗余度解析方案；2）将步骤1）中所生成的二次型优化冗余度解析方案转化为二次规划问题；3）将步骤2）中的二次规划问题运用二次规划求解器进行求解；4）将步骤3）的求解结果传递给下位机控制器驱动机械臂运动。本发明通过设计一种基于突加度的障碍物躲避运动性能指标，控制机械臂在突加度层上躲避障碍物，同时也使得机械臂完成给定的末端任务。

主权项

一种基于突加度的障碍物躲避运动规划方法，其特征在于，包括如下步骤：1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方案；其中基于突加度的障碍物躲避运动性能指标受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限，即最小化为受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^‑≤θ≤θ⁺、关节速度极限关节加速度极限和关节突加度极限${\stackrel{\·\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·\·}{\mathrm{\θ}}}^{+};$其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵，表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度；表示基于突加度的障碍物躲避不等式约束，J_z、和分别定义如下：其中，${\stackrel{\→}{\stackrel{\·\·\·}{r}}}_c=[x_c-x_o,y_c-y_o,z_c-z_o],$(x_c,y_c,z_c)和(x_o,y_o,z_o)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J_c(θ)表示判据点C处的雅可比矩阵，是J_c(θ)的时间导数，是的时间导数，向量与矩阵乘法操作定义如下：其中，列向量s＝[s₁,s₁,…,s_l]^T和行向量V_i表示矩阵V的第i行，i＝1,2,…,l；θ^±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，表示关节突加度上下限；2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题；3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。