[发明专利]一种基于突加度的冗余度机械臂重复运动规划方法

说明书

技术领域

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

背景技术

冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用 于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆 运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机 械臂末端任务为一个封闭曲线时，其各个关节可能回不到初始位置，这种现象 叫做关节角偏差现象，或称非重复运动问题，如果冗余度机械臂不能实现重复 运动，则在运行过程中会产生不能预料甚至是危险有损机械臂与周围人员安全 的问题；而重复运动规划方案就是要设计适当的性能指标，使得机械臂末端执 行完封闭曲线任务时，各个关节角都能够回到其初始位置。以往的重复运动解 析方法大多都是在速度层(或加速度层)上进行，得到的结果为速度值(或加 速度值)，这不能满足某些采用非速度(或非加速度)控制的冗余度机械臂的 要求，且它们的求解方法并不考虑突加度极限，机械臂在运动过程中就有可能 因超越突加度极限而产生物理损坏。本发明专利提出一种基于突加度的冗余度 机械臂重复运动规划方法，有效地克服了以往速度层(或加速度层)重复运动 规划中的局限，具有新颖性与实用性。

发明内容

本发明所要解决的技术问题是提供一种新型的、计算量小、普遍用于突加度 控制的冗余度机械臂重复运动规划方法。

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

一种突加度的冗余度机械臂重复运动规划方法，包括如下步骤：

1)通过上位机采用二次型优化在突加度层上对机械臂的重复运动进行逆运 动学解析，得到基于突加度的重复运动二次型优化冗余度解析方案；具体步骤为： 设计的最小化性能指标为重复运动，重复运动受约束于基于突加度的雅可比矩阵 等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加 速度极限和关节突加度极限；

2)将步骤1)中所生成的基于突加度的重复运动二次型优化冗余度解析方案 转化为标准二次规划；

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

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

基于突加度的重复运动性能指标设计为：最小化性能指标受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限和关节突加度极限其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵，α、β、χ为用来控制关节位移幅值和收敛率的正设计参数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ(0)表示机械臂的初始关节角度；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵，表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数，C与d表示基于突加度的障碍物躲避参数，θ^±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，表示关节突加度上下限；

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

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

上述的标准二次规划运用二次规划求解器进行求解。

将上述的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而 驱动各关节电机使机械臂完成障碍物躲避运动。