一种具有抗噪特性的冗余度机械臂重复运动规划方法与流程

本发明涉及冗余度机械臂的规划及控制领域，具体涉及一种具有抗噪特性的冗余度机械臂重复运动规划方法。

背景技术：

冗余度机械臂是一种末端能动的机械装置，其拥有的自由度要多于执行末端任务所需的最少自由度；已广泛应用于装备制造和产品加工等国民经济生产活动中。冗余度机械臂的规划和控制中容易存在的一个问题是关节角偏差问题，即，当机械臂末端的运动轨迹是闭合的，在完成给定的规划任务后，机械臂各关节角变量在运动空间中的轨迹不是闭合的。为了解决关节角偏差问题(或称，非重复运动问题)，目前已有多种有效的重复运动规划方案被提出。然而，这些方案都是在不考虑噪声的情况下进行研究的；一旦存在噪声，那么它们将失效，从而无法使机械臂能够正常/成功地完成所给定的规划任务。

技术实现要素：

本发明的目的在于克服现有方法的不足，提供一种具有抗噪特性的冗余度机械臂重复运动规划方法。

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

一种具有抗噪特性的冗余度机械臂重复运动规划方法，包括以下步骤：

根据机械臂的雅可比矩阵等式，通过引入位置误差反馈，设计具有抗噪特性的新型雅可比矩阵等式；

结合所需要优化的最小化性能指标，建立具有抗噪特性的重复运动规划方案，所述的规划方案受约束于新型雅可比矩阵等式、关节角度极限和关节速度极限；

将具有抗噪特性的重复运动规划方案转化为一个统一的二次型优化问题，并通过数值算法求解器对该优化问题进行求解；

下位机控制器根据二次型优化问题的求解结果，驱动机械臂使其完成给定的规划任务。

具有抗噪特性的新型雅可比矩阵等式设计为：

其中，设计参数α>0，β>0，且两者满足α²>β；θ表示机械臂关节角度，表示机械臂关节速度，J(θ)表示机械臂的雅可比矩阵；e(t)表示机械臂的位置误差且

e(t)＝f(θ)-r_d，t表示时间，f表示一个非线性映射函数，r_d表示机械臂末端期望的运动轨迹；表示r_d的时间导数，τ表示积分变量。

具有抗噪特性的重复运动规划方案设计为：

最小化

受约束于θ-≤θ≤θ+，

其中，设计参数λ>0，θ(0)表示机械臂关节的初始状态，上标^T表示矩阵或向量的转置；等式约束对应于机械臂在速度层的运动轨迹；θ^±和分别表示关节角度极限和关节速度极限。

将具有抗噪特性的重复运动规划方案转化为一个统一的二次型优化问题，其性能指标为x^Tx/2+p^Tx，约束条件为Ax＝b，x^-≤x≤x⁺，其中，p＝λ(θ-θ(0))，A＝J(θ)，x^±表示x的上下限。

通过数值算法求解器对二次型优化问题进行求解，具体为：将所述二次型优化问题进一步变换为分段线性投影方程，从而构造相应的数值算法求解器进行求解。

本发明与先有方法相比，具有以下优点：

本发明能有效克服现有方法的不足，提供了一种能使得冗余度机械臂在具有噪声的情况下仍可完成给定任务的重复运动规划方法。

附图说明

图1为本发明的流程图。

具体实施方式

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

图1所示的一种具有抗噪特性的冗余度机械臂重复运动规划方法主要由设计具有抗噪特性的新型雅克比矩阵等式1、建立重复运动规划方案2、转为二次型优化问题3、数值算法求解器4、下位机控制器5、冗余度机械臂6这六个部分组成。

首先根据机械臂的雅可比矩阵等式，通过引入位置误差反馈，设计具有抗噪特性的新型雅可比矩阵等式；然后结合所需要优化的最小化性能指标，建立相应的重复运动规划方案，并将其转化为一个统一的二次型优化问题，从而构造相应的数值算法求解器来求解该优化问题；最后将求解结果用于驱动机械臂的各个关节以使机械臂完成给定的规划任务。

根据机械臂的雅可比矩阵等式，通过引入位置误差反馈，具有抗噪特性的新型雅可比矩阵等式可设计为：

其中设计参数α>0，β>0，且两者满足α²>β；θ表示机械臂关节角度，表示机械臂关节速度，J(θ)表示机械臂的雅可比矩阵；e(t)表示机械臂的位置误差且e(t)＝f(θ)-r_d，t表示时间，f表示一个非线性映射函数，r_d表示机械臂末端期望的运动轨迹；表示r_d的时间导数，τ表示积分变量。

基于上述等式(1)，结合所需要优化的性能指标，便可建立如下的重复运动规划方案：

最小化

约束条件：

θ-≤θ≤θ⁺， (4)

其中，设计参数λ>0，θ(0)表示机械臂关节的初始状态，上标^T表示矩阵或向量的转置；等式约束对应于机械臂在速度层的运动轨迹；θ^±和分别表示关节角度极限和关节速度极限。

对于上述带物理约束的重复运动规划方案(2)-(5)，其可转化为如下的二次型优化问题：

最小化x^Tx/2+p^Tx， (6)

约束条件：Ax＝b， (7)

x^-≤x≤x⁺， (8)

其中，p＝λ(θ-θ(0))，A＝J(θ)，x^±表示x的上下限。

并且，上述的二次型优化问题(6)-(8)等价于如下的分段线性投影方程：

P_Ω(y-(My+q))-y＝0， (9)

其中，P_Ω(·)表示分段线性投影算子。分段线性投影方程(9)中的原对偶决策变向量y，增广系数矩阵M和向量q分别定义如下：

其中，I表示单位矩阵，对偶决策变量u对应于等式约束(7)。对于上述的分段线性投影方程(9)和二次型优化问题(6)-(8)，可采用如下的数值算法来进行求解：

e(y^k)＝y^k-P_Ω(y^k-(My^k+q))，

y^k+1＝y^k-ρ(y^k)φ(y^k)，

φ(y^k)＝(M^T+I)e(y^k)，

其中，||·||₂表示向量的二范数，迭代次数k＝0,1,2,…。给定一个初始值y⁰，通过该算法的不断迭代，便可得到分段线性投影方程(9)的解，从而得到二次型优化问题(6)-(8)的最优解，也即前文所述的具有抗噪特性的重复运动规划方案(2)-(5)的最优解。

通过数值算法求解器得到该二次型优化问题的最优解之后，再将求解结果传递给下位机控制器驱动机械臂的运动，从而使得机械臂完成所给定的规划任务。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。