一种高性能冗余度机械臂重复运动规划方法与装置与流程

本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种冗余度机械臂的逆运动学求解方法。

背景技术：

冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知冗余度机械臂末端位置，确定冗余度机械臂的关节角问题。当冗余度机械臂末端任务为一个封闭曲线时，其各个关节可能回不到初始位置，这种现象叫做关节角偏差，或称非重复运动问题。如果冗余度机械臂不能实现重复运动，则在运行过程中可能会产生不可预料的情况，甚至出现损害冗余度机械臂以及危害周围人员人身安全的问题；而重复运动规划方案就是要设计适当的性能参数，使得冗余度机械臂末端执行完封闭曲线任务时，各个关节角都能够回到其初始位置。

现有重复运动解析方法全都存在理论位置误差，而理论位置误差直接影响到了轨迹追踪任务的完成精度。具体而言，现有冗余度机械臂重复运动中的理论位置误差和关节角度误差存在耦合关系，由此会产生关节角度误差和理论位置误差之间的矛盾。例如：现有方法《一种冗余度机械臂重复运动规划的方法》(申请号：201010264141.6)中，关节角度反馈的增加会造成位置误差的增大，从而降低的精度。为了克服现有重复运动方案中出现理论位置误差偏大，从而对冗余度机械臂执行任务精度产生影响这一缺陷，本发明提出一种高性能冗余度机械臂重复运动规划方法与装置，有效地消除了执行任务时产生的理论误差，并能够更精确地完成冗余度机械臂的重复运动任务，这一点对于执行精密任务是非常有意义的。综上所述，本发明专利具有新颖性与实用性。

技术实现要素：

本发明所要解决的技术问题是提供一种高性能冗余度机械臂重复运动规划方法与装置。

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

1、一种高性能冗余度机械臂重复运动规划方法与装置包括如下步骤：

1)通过上位机采用二次型优化方案对冗余度机械臂的重复运动进行进行冗余度解析，设计的最小化性能指标为重复运动，受约束于雅可比等式，关节角度极限，关节角速度极限；

2)将步骤1)的二次型优化方案转化为二次规划；

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

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

2、一种高性能冗余度机械臂重复运动规划方法与装置设计为：最小化受约束于雅可比等式关节角度极限0^-≤θ≤θ⁺，关节角速度极上标t表示矩阵和向量的转置，上标表示矩阵的伪逆，θ表示冗余度机械臂的关节角度，θ0表示冗余度机械臂的初始关节角度，表示冗余度机械臂的关节角速度，κ＞0表示关于重复运动的反馈系数，rd表示设定的末端执行器的期望轨迹，表示设定的末端执行器期望速度，j表示冗余度机械臂的雅可比矩阵，i表示单位矩阵，表示本发明中不同于现存方法的重要特征，起到促使位置误差与关节角度误差解耦的作用，θ^±表示冗余度机械臂的关节角度极限，表示冗余度机械臂的关节角速度的上下限；

3、上述重复运动的二次型优化方案可以转化为一个二次规划，即最小化受约束于其中κ＞0表示关于重复运动的反馈系数，β^±为冗余度机械臂的关节速度上下限，它们的第i个元素分别定义为i表示关节序号，i＝1，2，3...n，正常数kα用来调节和保证关节速度有足够大的可行域。

4、此二次规划可以用二次规划求解器求解。

5、二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使冗余度机械臂重复运动。

6、通过控制装置的各模块之间的信息传递完成从求解器到冗余度机械臂重复运动的控制；

所述控制装置包括：

信息传递模块，用于获取二次规划求解器求解结果，并将求解结果传递给下位机控制冗余度机械臂的重复运动生成；

轨迹信息获取模块，用于获取所述目标冗余度机械臂设定的轨迹信息，并将轨迹信息传输给二次规划求解器；

等式构建模块，用于根据所述位置和速度信息，以及所述参考点轨迹信息，按照预设规则构建所述目标冗余度机械臂对应的雅可比矩阵等式；

控制信号确定模块，用于在所述雅可比矩阵等式、关节角度极限和关节角速度极限的约束下，根据二次型优化和标准二次规划方法确定所述目标冗余度机械臂的控制信号；

冗余度机械臂控制模块，用于根据所述控制信号控制目标冗余度机械臂，使目标冗余度机械臂实现重复运动。

与现有技术相比，本发明有如下优点：

现有的冗余度机械臂的重复运动方案都存在理论位置误差，理论位置误差与关节角度误差是成反比的关系，即关节角度误差越小，位置误差越大。本发明首次使末端执行器的位置误差与冗余度机械臂的关节角度误差解耦，并成功地消除了理论位置误差，令冗余度机械臂的重复运动能够达到更加精确的效果，这一点对于精密仪器是非常有意义的，这在实际应用中有重要作用和广阔前景。

附图说明

图1为本发明的流程图；

图2为实现本发明的冗余度机械臂三维模型图；

图3为实现本发明应用下的冗余度机械臂重复运动示意图；

图4为实现本发明应用下的冗余度机械臂重复运动过程图；

图5为现有重复运动方案应用下的冗余度机械臂重复运动末端执行器位置误差图和运动关节角度变化图；

图6为实现本发明应用下的冗余度机械臂重复运动末端执行器位置误差图和运动关节角度变化图；

具体实施方式

下面结合附图对本发明做进一步的说明。

图1所示的一种高性能冗余度机械臂重复运动规划方法与装置首先提出重复运动性能指标与约束；然后将其转化为相应的二次规划；接着应用二次规划求解器求解；最后下位机控制器根据求解结果控制冗余度机械臂的运动。

图2所示实现本发明的冗余度机械臂为一个空间六自由度的puma560机械臂。该冗余度机械臂由六个连杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成。

本发明设计的重复运动规划方案为最小化:

约束条件:

θ^-≤θ≤θ⁺(3)

其中，为重复运动的性能指标，上标^t表示矩阵和向量的转置，上标表示矩阵的伪逆，θ表示冗余度机械臂的关节角度，θ0表示冗余度机械臂的初始关节角度，表示冗余度机械臂的关节角速度，κ＞0表示关于重复运动的反馈系数，表示设定的期望轨迹，j表示冗余度机械臂的雅可比矩阵，i表示单位矩阵，表示冗余度机械臂的关节角速度的上下限，θ^±表示冗余度机械臂的关节角度的上下限。

带物理约束的冗余度机械臂的二次型优化方案(1)-(4)便可描述为如下的二次规划方案：

最小化：

约束条件：

其中，κ＞0表示关于重复运动的反馈系数，β^±为冗余度机械臂的关节速度上下限，它们的第i个元素分别定义为i表示关节序号，i＝1，2，3...n，正常数kα用来调节和保证关节速度足够大的可行域。下面给出一个可行的求解器的例子(本发明不限于该求解器例子)，通过使用梯度下降法加上速度补偿的方法，上述问题又可以等价为分段线性投影方程组：

其中，γ＞0表示末端执行器位置误差反馈系数，r表示末端执行器实际轨迹，rd-r表示末端执行器的位置误差，w表示连接(8)和(9)的辅助变量，j^tw表示速度补偿，v＞0表示式子(9)的系数，

本发明的理论误差等于零，并且与关节误差解耦。然而对于现有的方案如《一种冗余度机械臂重复运动规划的方法》(申请号：201010264141.6)位置误差为通过这个位置误差表达式，可以看出对于现有的重复运动方案当增加了关节角度误差反馈系数k时，关节角度误差会减小，可是位置误差会增大，这种矛盾的关系是现存冗余度机械臂重复运动方案的不足之处。由此可见，现存的冗余度机械臂重复运动规划方案理论上存在与关节角度误差有关的位置误差，然而所提出的一种高性能冗余度机械臂重复运动规划方法与装置不存在理论上的位置误差。对本发明进行求解器的求解，从而得到高性能冗余度机械臂重复运动规划的精确解。

通过上位机的二次规划求解器得到该二次规划的解后，再将结果转化为电机驱动所需要的控制信号，并通过该冗余度机械臂的控制器来驱动冗余度机械臂各关节电机运动，从而驱动各关节电机使冗余度机械臂重复运动。

现结合一个具体的实例操作对本发明的工作流程进行如下说明。

利用matlab软件对本发明方法进行六自由度的puma560冗余度机械臂重复运动轨迹追踪的实验仿真，并在相同参数设置与现存的方法进行结果对比。具体参数设置如下：γ＝2000，k＝10，v＝0.001，冗余度机械臂的关节速度极限为β⁺＝-β^-＝[0.9,0.9,0.9,0.9,0.9,0.9]弧度/秒,计算机仿真时间设为10秒，执行任务为三尖瓣图形，初始的角度设置为q0＝[0,0,0.5,0,0,0]弧度。通过二次规划求解器求解，将计算得到的结果再传送给冗余度机械臂控制器从而控制冗余度机械臂的运动。结果相比之下，一种高性能冗余度机械臂重复运动规划方法与装置在完成冗余度机械臂轨迹任务时位置误差更小，能更加精确地完成任务。

图3为实现本发明应用下的冗余度机械臂重复运动示意图。从图3可以看出来所给定的三尖瓣轨迹追踪任务顺利被完成。

图4为实现本发明应用下的冗余度机械臂重复运动过程图。子图1为初始冗余度机械臂的状态，子图2，3，4为中间过程中冗余度机械臂的状态，子图5为一次任务循环结束时冗余度机械臂的状态，子图6为一个新的重复运动循环的冗余度机械臂的状态。图4体现了在一个任务循环中，结束时冗余度机械臂的状态和初始冗余度机械臂状态完全相同，最终完成了重复运动任务。

图5为现有重复运动方案应用下的冗余度机械臂重复运动末端执行器位置误差图和运动关节角度变化图。从图5看出，当角度反馈系数较大时，现有重复运动方案应用《一种冗余度机械臂重复运动规划的方法》(申请号：201010264141.6)中的冗余度重复运动末端执行器在整个运动过程中位置误差比较大，最大误差甚至接近0.01米，会影响任务完成的质量。另外，冗余度机械臂重复运动关节角度结束状态和初始状态相同，满足重复运动的要求。

图6为实现本发明应用下的冗余度机械臂重复运动末端执行器位置误差图和运动关节角度变化图。从图6看出，当角度反馈系数较大时，本发明应用下的冗余度机械臂重复运动末端执行器位置误差比较小，并整体保持在较小的状态下，最大位置误差小于2×10^-4米。另外，冗余度机械臂重复运动关节角度结束状态和初始状态相同，满足重复运动的要求。