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

说明书

技术领域

本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种基于突加度(加速度的导数，Jerk)的障碍物躲避运动规划方法。

背景技术

冗余度机械臂是一种所具有的自由度数量大于执行任务所需最小自由度数量的机械臂，广泛地应用于各种国民经济生产活动中。冗余度解析是操作冗余度机械臂中的一个重要的问题，即是依据不同的优化性能指标和二级任务，通过冗余度机械臂的末端位置和姿态来确定机械臂各个关节最优角度的问题。冗余度机械臂的二级任务包括障碍物躲避、关节极限角躲避和重复运动，通常被描述成等式约束、不等式约束或者双端约束。障碍物躲避是冗余度解析问题中的一个重要的问题。目前有效的障碍物躲避运动规划方法都是在速度层或者加速度层上进行解析的。然而，这不能满足某些采用突加度控制冗余度机械臂的要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。

发明内容

本发明所要解决的技术问题是提供一种基于突加度的障碍物躲避运动规划方法，该方法具有计算量小的特点。

为解决上述技术问题，本发明通过以下技术方案予以实现：

一种基于突加度的障碍物躲避运动规划方法，包括如下步骤：

1)设计基于突加度的障碍物躲避运动性能指标，生成二次型优化冗余度解析方案；其中基于突加度的障碍物躲避运动性能指标受约束于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限；

2)将步骤1)中所生成的基于突加度的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题；

3)将步骤2)中的二次规划问题运用二次规划求解器进行求解；

4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。

基于突加度的障碍物躲避运动性能指标设计为：最小化受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限关节突加度极限其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵，表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度；表示基于突加度的障碍物躲避不等式约束，J_z、和分别定义如下：

其中，(x_c,y_c,z_c)和(x_o,y_o,z_o)分别是判据点C和障碍物点O相对于机械臂基座的坐标，J_c(θ)表示判据点C处的雅可比矩阵，是J_c(θ)的时间导数，是的时间导数，向量与矩阵乘法操作◇定义如下：

s◇V＝[s₁V₁,s₂V₂,…s_lV_l]^T

其中，列向量s＝[s₁,s₁,…,s_l]^T和行向量V_i表示矩阵V的第i行，i＝1,2,…,l；θ^±表示关节角上下限，表示关节速度上下限，表示关节加速度上下限表示关节突加度上下限。

上述的障碍物躲避二次型优化冗余度解析方案转化为二次规划问题，设计其性能指标为最小化x^TWx/2，受约束于Ax＝b，Cx≤d，ζ^-≤x≤ζ⁺，其中，W为单位矩阵，A＝J_e，C＝J_z，ζ^-和ζ⁺分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：

其中，j表示关节序号，j＝1,2,...,n，n为关节数目，和ε>0为关节转换裕度，关节极限转换参数k_p>0，k_v>0和k_a>0用来调节和保证关节突加度够大的可行域。