CN108422424A - 一种具有饱和特性的抗扰动机械臂重复运动规划方法 - Google Patents

Abstract

本发明提供了一种具有饱和特性的抗扰动机械臂重复运动规划方法，包括如下步骤：1)设计具有饱和特性的二次型形式的抗扰动机械臂重复运动性能指标，生成二次型优化解析方案；2)将步骤1)中的二次型优化解析方案转化为二次规划；3)将步骤2)中的二次规划运用二次规划求解器进行求解；4)将步骤3)中的求解结果传送给下位机控制器以驱动机械臂运动。本发明提供了在机械臂运动规划中性能指标的饱和设计，对偏差过大时导致的关节角速度过快现象有显著抑制作用。通过预防机械臂摆动速度过大进而有效规避其在运动过程中潜在的风险，此外本发明还增强了机械臂的抗扰动性能，这些改进在工程应用领域中均具有积极的意义和广泛的用途。

Description

一种具有饱和特性的抗扰动机械臂重复运动规划方法

技术领域

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

背景技术

抗扰动机械臂具有限制机械臂关节角速度的功能，同时具备抗扰动性能。该装置广泛应用于工业装备、医学治疗、安全防爆等涉及国民经济生产的领域。抗扰动机械臂的逆运动学问题是指已知机械臂末端的位置姿态，最终确定机械臂的关节角问题。当抗扰动机械臂末端的任务是走一个封闭曲线时，在末端到达初始位置时机械臂的各个关节最终可能回不到初始位置，这种现象叫做关节角偏差现象，或称非重复运动问题。如果抗扰动机械臂不能实现重复运动，那么它在运行过程中很可能会产生无法预期的结果，此外如果对速度不加以限制，那么当机械臂运行速度过大时，会很大程度降低机械臂某些性能的发挥，甚至可能会发生危及机械臂周围人员人身安全的事故；而重复运动规划方案就是要设计适当的性能指标，使得机械臂末端在执行完封闭曲线的任务时，各个关节都能够回到其初始位置，进而有效规避非重复运动规划方案中存在的缺陷。以往的重复运动解析方法虽然一部分也是在速度层上进行，但是这些速度层上的求解方法并没有考虑速度极限，因此机械臂在运动过程中就有可能因超越速度极限而产生物理损坏，那么再应用以前的方案就不能满足抗扰动机械臂在某些场合中所要求的运行精度。本发明人团队之前的发明《一种冗余度机械臂的新型加速度层重复运动规划的方法》(申请号：CN201010264141.6)则为一种加速度层的重复运动解析方法，可以克服上述加速度层控制的抗扰动机械臂的缺陷，但是，该发明方案没有将速度进行饱和度设计，并不能满足实际应用中对机械臂关节速度有限制的要求。本发明专利提出一种具有饱和特性的抗扰动机械臂重复运动规划方法，有效地克服了以往速度层重复运动规划中不具有饱和特性的缺陷，将从速度层进行饱和度解析。综上所述，本发明专利具有新颖性与实用性。

发明内容

本发明所要解决的技术问题是提供一种新型的、高效的、普遍用于速度饱和控制的抗扰动机械臂重复运动规划方法。

为解决之上所描述的技术问题，本发明通过以下技术方案加以实现：

1、一种具有饱和特性的抗扰动机械臂重复运动规划方法包括如下步骤：

1)通过上位机采用二次型优化方案在速度层上对抗扰动机械臂的重复运动进行逆运动学求解，设计的最小化性能指标为重复运动，受约束于速度雅可比等式、障碍物躲避不等式、关节角极限、关节角速度极限；

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

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

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

2、具有饱和特性的抗扰动机械臂重复运动方案设计为：

最小化

受约束于θ^-≤θ≤θ⁺、其中θ表示关节角，表示关节角速度，W为单位矩阵， 其中，P_μ表示一个映射P_μ(θ)＝argmin_Y∈μ‖Y-θ‖_F，0∈μ，θ和μ代表两个集合。简言之，P_μ(θ)是从集合θ到集合μ的映射。此外，p^T表示p的转置，λ>0和γ>0是设计参数，该参数是用来控制关节位移幅值和收敛率的。等式约束表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是其时间导数，表示关节角速度，表示机械臂末端执行器速度矢量，θ^-≤θ≤θ⁺、分别表示关节角极限、关节角速度极限，θ^±表示关节角上下限，表示关节角速度上下限，A和b是障碍物躲避参数。

3、上述的二次型优化方案可以转化为一个二次规划，即最小化x^TWx/2+p^Tx，受约束于Cx＝d，Ax≤b，其中，W为单位矩阵，其中，P_μ表示一个映射P_μ(θ)＝arg min_Y∈μ‖Y-θ‖_F，0∈μ，θ和μ代表两个集合。简言之，P_μ(θ)是从集合θ到集合μ的映射。此外，C＝J， 和分别表示合成双端约束的上下极限，它们第i个元素分别定义为 和 其中为裕度，i表示关节序号，i＝1,2,…,n，正常数k_λ和k_γ用来调节和确保关节角速度的可行域足够大。

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

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

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

以往发明，针对抗扰动机械臂的速度层控制不具备饱和特性，这给抗扰动机械臂在速度层控制方案中增加了不可控因素。本发明给出了一个速度层饱和的重复运动方案，使得机械臂的运行更加安全有效，这在实际应用中具有重要意义和广阔前景。

附图说明

图1为本发明的流程图；

图2为实现本发明的机械臂三维模型。

具体实施方式

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

图1所示的抗扰动机械臂重复运动规划方法首先提出速度层饱和的重复运动性能指标与约束；然后将其转化为相应的二次规划；接着应用二次规划求解器求解；最后下位机控制器根据求解器求解出的结果对机械臂进行控制，最终实现重复运动。

图2所示为实现本发明的机械臂，其为一个具有六个自由度的PUMA560机械臂。该机械臂由六个连杆组成，具体是由关节1、关节2、关节3、关节4、关节5和关节6组成。

本发明设计的速度层重复运动规划方案为

最小化

约束条件

θ^-≤θ≤θ⁺, (4)

其中，为速度层重复运动性能指标，表示关节角速度，W为单位矩阵， 其中，P_μ表示一个映射P_μ(θ)＝arg min_Y∈μ‖Y-θ‖_F，0∈μ，θ和μ代表两个集合。简言之，P_μ(θ)是从集合θ到集合μ的映射。进一步而言，在具体实施中，我们选取“bound”激励函数为选取“ball”激励函数为两激励函数表达式分别为，

“bound”激励函数：

其中μ₁＝θ∈R^n×n，θ^-≤θ≤θ⁺。

“ball”激励函数：

其中μ₂＝θ∈R^n×n，‖θ‖_F≤η，η>0。

p^T表示p的转置，λ>0和γ>0是用来控制关节位移幅值和收敛率的设计参数，等式约束表示机械臂末端的运动轨迹，J表示机械臂的雅可比矩阵，是其时间导数，θ表示关节角，表示关节角速度，表示机械臂末端执行器速度矢量，θ-≤θ≤θ+、分别表示关节角极限、关节角速度极限，θ±表示关节角上下限，表示关节角速度上下限，A和b是障碍物躲避参数。考虑到上述优化问题是在速度层上求解，因此需将机械臂的关节角度约束(4)、关节速度约束(5)和关节速度约束(6)进行合并，从而可以得到基于速度的双端不等式约束：

其中，和分别表示合成双端约束的上下限，它们的第i个元素分别定义为和 其中为裕度，i表示关节序号，i＝1,2,…,n，正常数用k_λ和k_γ来调节和保证关节速度足够大的可行域。用x表示机械臂的关节速度带物理约束机械臂的新形式的二次型优化方案(1)-(6)便可描述为如下的二次规划方案：

最小化：x^TWx/2+z^Tx, (7)

约束条件：Cx＝d, (8)

Ax≤b, (9)

其中，W为单位矩阵， 其中，P_μ表示一个映射P_μ(θ)＝arg min_Y∈μ‖Y-θ‖_F，0∈μ，θ和μ代表两个集合。简言之，P_μ(θ)是从集合θ到集合μ的映射。进一步而言，在具体实施中，我们选取“bound”激励函数为选取“ball”激励函数为两激励函数表达式分别为，

“bound”激励函数：

其中μ₁＝θ∈R^n×n，θ^-≤θ≤θ⁺。

“ball”激励函数：

其中μ₂＝θ∈R^n×n，‖θ‖_F≤η，η>0。

约束条件中上述二次规划问题求解等价于寻求一个向量 使之满足(y-y^*)^T(Hy^*+g)≥0,其中y＝[x,u,v]^T，y^-＝[ξ^-,-l_Uω,0]^T，y⁺＝[ξ⁺,+l_Uω,l_vω]^T，l_U和l_v分别表示由1组成的相应维数向量，ω>>0是足够大的常数，用于数值上替代无穷大+∞，u∈Rⁿ和v∈R^dim(b)表示由等式约束(8)和不等式约束(9)得到的向量，

上述问题又可以等价为分段线性投影方程p_Ω(y-(Hy+g))-y＝0，其中

设计其计算误差为e(y):＝y-P_Ω(y-(Hy+g)):当误差为零时，其对应的y值便为分段线性方程的解，其前n个元素组成二次规划的解x。给定初始值y₀∈R^n+n+dim(b)，通过如下的迭代来得到y使得误差e(y)达到预设的精度，运行以下迭代：

y^k+1＝R(y^k-ρ(y^k)d(y^k))，

其中d(y^k)：＝H^Te(y^k)+Hy^k+g和0,1,2,…。通过算法不断迭代，得到分段线性投影方程p_Ω(y-(Hy+g))-y＝0的解，从而得到抗扰动机械臂重复运动规划的最优解。

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

Claims (4)

1.一种具有饱和特性的抗扰动机械臂重复运动规划方法，其内容包括如下步骤：

1)通过上位机采用二次型优化方案在速度层上对机械臂的重复运动进行饱和度解析，设计的最小化性能指标：饱和特性约束下的重复运动性能指标，受速度雅可比恒等式、障碍物躲避不等式、关节角极限以及关节角速度极限等约束，即最小化性能指标为受约束于速度雅可比恒等式障碍物躲避不等式约束关节角极限θ^-≤θ≤θ⁺、关节角速度极限其中θ表示机械臂的关节角，表示机械臂的关节角速度，W为单位矩阵，上标T表示矩阵和向量的转置，J表示机械臂的雅可比矩阵， 其中，P_μ表示一个映射P_μ(θ)＝arg mmin_Y∈μ||Y-θ||_F，0∈μ，θ和μ代表两个集合，简言之，P_μ(θ)是从集合θ到集合μ的映射，此外，λ＞0和γ＞0是来控制关节位移幅值和收敛率的设计参数，等式约束表示机械臂末端运动轨迹，是J的时间导数，θ(0)表示其初始关节角，θ±表示关节角的上下限，表示关节角速度的上下限，A和b是障碍物躲避参数；

2)将步骤1)中的面向速度层重复运动的二次型优化方案转化为二次规划；

3)将步骤2)中的二次规划应用数值算法(即二次规划求解器)进行求解；

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

2.根据权利要求1所述的具有饱和特性的抗扰动机械臂重复运动规划方法，其特征在于所述步骤2)的将二次型优化转化为一个二次规划，设计其性能指标为最小化受约束于Cx＝d，Ax≤b，其中，W为单位矩阵， 其中，P_μ表示一个映射P_μ(θ)＝arg min_Y∈μ||Y-θ||_F，0∈μ，θ和μ代表两个集合，简言之，P_μ(θ)是从集合θ到集合μ的映射，C＝JA，b为障碍物躲避参数，和分别表示合成双端约束的上下极限，它们的第i个元素分别定义为 和其中为裕度，i表示关节序号，i＝1，2，...，n，正常数k_λ和k_γ用来调节和保证关节速度足够大的可行域。

3.根据权利要求2所述的具有饱和特性的抗扰动机械臂重复运动规划方法，其特征在于所述步骤3)的二次规划求解器求解。

4.根据权利要求3所述的具有饱和特性的抗扰动机械臂重复运动规划方法，其特征在于所述步骤3)的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各个关节电机使机械臂重复运动。