[发明专利]基于新型数值求解器的冗余度机械臂重复运动规划方法

说明书

技术领域

本发明属于冗余度机械臂重复运动规划方法，特别是一种使用新型数值求解器求解，使求解结果离散化，降低控制难度的冗余度机械臂重复运动规划方法。

背景技术

机械手臂通常分为非冗余度机械手臂和冗余度机械手臂。因为有着更多的自由度，冗余度机械手臂可以在完成末端执行器的主要工作外还能完成额外的工作，例如障碍躲避，关节极限躲避和奇异状态躲避等。在工业生产活动中，机械手臂常被要求进行重复性的生产操作，这类重复运动要求机械手臂完成一次周期运动后机械臂各个关节位置回到初始状态。若未能回到初始状态，则在多次周期运动后机械手臂将会出现累计误差，导致明显的偏移，影响其工作效果或者需要额外的工作将其复位，所以非重复运动问题是冗余度机械手臂控制的关键问题之一。解决非重复运动问题本质上是在求解冗余度机械手臂的逆运动学问题。

冗余度机械手臂的逆运动学问题也称为冗余度解析问题。对于逆运动学问题，传统的求解方法是伪逆方法。伪逆方法在处理不等式约束上有困难，也不能获得在奇异情况下的可行解，而且计算量大，实时性差，在实际机械臂的应用中受到很大的制约。近年来，一种通过二次规划方案来解决冗余度机械手臂运动的方法被提出，能够解决带双端约束、不等式约束的问题。然而，目前存在的冗余度机械手臂运动规划方法使用的求解方法，大多数解得的是时间连续的解，难以直接应用于数字化电机的驱动和控制。

发明内容

本发明的目的在于针对冗余度机械臂非重复运动问题，提供一种使求解结果方便计算机控制和电机驱动的，基于新型数值求解器的冗余度机械臂重复运动规划方法。

为了实现上述发明目的，采用的技术方案如下。

基于新型数值求解器的冗余度机械臂重复运动规划方法,其包括如下步骤：

1)以重复性能指标为最小性能指标，将冗余度机械臂重复运动要求设计为二次规划问题；

2)将步骤1)的二次规划问题转化为分段线性投影方程；

3)将步骤2)的分段线性投影方程，用新型数值求解器求解；

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

进一步地，步骤1)所述冗余度机械臂重复运动要求设计为二次规划问题，具体是：在速度层上设计重复运动性能指标，其中θ为冗余度机械臂的关节角度向量，表示关节角速度，上标^T表示向量的转置，c＝λ(θ(t)-θ(0))，θ(t)和θ(0)分别为机械手臂运动过程中的关节角度和初始状态的关节角度，λ＞0为机械臂对关节位移θ(t)-θ(0)的响应系数；同时在重复运动中要考虑机械臂末端运动轨迹雅克比等式约束以及机械臂的关节物理角度极限和角速度极限：θ^-≤θ≤θ⁺，其中J(θ)表示机械臂的雅克比矩阵，表示机械臂末端轨迹方程对时间的导数，θ⁺和θ^-分别表示对关节角度的上下约束，和分别对关节角速度的上下约束；将角度极限和角速度极限统一为速度层上的双端约束为其中和分别表示对速度向量的上下约束向量，对这两个约束向量中的第i个分量即机械臂的第i个关节有系数v＞0是用来调节关节角速度的可行域；下标i表示冗余的机械臂的第i个关节对应的前述已经定义的相应参数，系数v＞0是用来调节关节角速度的可行域。

进一步地，步骤2)所述分段线性投影方程为：P_Ω(u-(Mu+q))-u＝0，其中P_Ω(·)是从R^n+m到集合的映射，u为待求解的原对偶平衡矢量，M∈R^(n+m)×(n+m)和q∈R^n+m为扩展矩阵，m是末端执行器的笛卡尔空间维数；n是机器人手臂的关节数；u⁺和u^-分别表示u的上下取值范围，在数学形式上定义为：

其中u的前n项x即为所求的机械臂关节的角速度，后m项y为使u的维数为n+m的量；l＝[1,…,1]^T∈R^m是一个全是1的矩阵，w＞＞0在数学上代替+∞；m是末端执行器的笛卡尔空间维数；n是机器人手臂的关节数；M和q为扩展系数矩阵，定义为：

其中J为机械臂的雅克比矩阵，J^T表示其转置，W＝I为单位矩阵，表示机械臂末端轨迹方程对时间的导数。