CN105538327A - 一种基于突加度的冗余度机械臂重复运动规划方法 - Google Patents

Abstract

本发明公开一种基于突加度的冗余度机械臂重复运动规划方法，包括：1)通过上位机采用二次型优化在突加度层上对机械臂重复运动方案进行解析，设计最小化性能指标为重复运动；2)将步骤1)中的二次型优化转化为标准二次规划；3)将步骤2)中的标准二次规划运用二次规划求解器进行求解；4)将步骤3)的求解结果传递给下位机控制器驱动冗余度机械臂运动。本发明通过设计一种基于突加度的冗余度机械臂重复运动规划性能指标，控制机械臂在突加度层上实现重复运动，同时也使得机械臂完成给定的末端任务。对于不便于速度与加速度控制的冗余度机械臂具有较好的控制效果，且本发明考虑到多种关节极限，能有效防止机械臂超越极限产生物理损坏。

Description

一种基于突加度的冗余度机械臂重复运动规划方法

技术领域

本发明涉及冗余度机械臂运动规划及控制领域，具体涉及一种基于突加度(加速度的导数，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用来调节和保证关节突加度够大的可行域。

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

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

与现有技术相比，本发明有如下优点：现有有效的重复运动规划方法都是在速度层或者加速度层上进行解析的，这不能满足某些采用突加度控制的冗余度机械臂的要求，而且不能考虑到突加度极限，从而可能导致机械臂在运动过程中超出突加度极限。本发明是一种计算量小的基于突加度的重复运动规划方法。

附图说明

图1为本发明的流程图。

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

具体实施方式

下面结合附图对本发明做进一步的描述，但本发明的实施方式并不限于此。

图1所示的一种基于突加度的冗余度机械臂重复运动规划方法，首先提出基于突加度的重复运动性能指标与约束，生成二次型优化冗余度解析方案；然后将二次型优化冗余度解析方案转化为标准二次规划；接着运用二次规划求解器求解；最后下位机控制器根据求解结果控制机械臂的运动。

图2所示实现本发明的机械臂为一个六自由度的机械臂。该机械臂由六个连杆组成，通过第一关节1,第二关节2,第三关节3,第四关节4,第五关节5,第六关节6组成，机械臂连接在基座7上，关节6的末端连接有末端执行器8。

本发明基于突加度的障碍物躲避运动规划方法的二次型优化冗余度解析方案设计为：

最小化： ${\stackrel{\·\·\·}{\mathrm{\θ}}}^{T}W\stackrel{\·\·\·}{\mathrm{\θ}}/2+p^T\stackrel{\·\·\·}{\mathrm{\θ}}---\left(1\right)$

约束条件： $J_e\stackrel{\·\·\·}{\mathrm{\θ}}={\stackrel{\·\·\·}{r}}_{\mathrm{\α}}---\left(2\right)$

$C\stackrel{\·\·\·}{\mathrm{\θ}}\≤d---\left(3\right)$

θ^-≤θ≤θ⁺(4)

${\stackrel{\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·}{\mathrm{\θ}}\≤{\stackrel{\·}{\mathrm{\θ}}}^{+}---\left(5\right)$

${\stackrel{\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·}{\mathrm{\θ}}}^{+}---\left(6\right)$

${\stackrel{\·\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·\·}{\mathrm{\θ}}}^{+}---\left(7\right)$

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

由于上述的二次型优化冗余度解析方案是在突加度层上求解，因此需将机械臂的关节角度约束(4)、关节速度约束(5)、关节加速度约束(6)和关节突加度约束(7)合并，从而可以得到基于突加度的双端不等式约束：其中，ζ^-和ζ⁺分别表示合成双端约束的上下极限，它们的第j个元素分别定义为：

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

进而，使用x表示机械臂的关节突加度上述带物理约束的基于突加度的障碍物躲避运动规划方法(1)-(7)便可描述为如下的二次规划问题：

最小化：x^TWx/2+p^Tx(8)

约束条件：Ax＝b(9)

Cx≤d(10)

ζ^-≤x≤ζ⁺(11)

其中，W为单位矩阵，A＝J_e，另外，上述的二次型规划问题(8)-(11)等价于如下的分段线性投影方程：

P_Ω(y-(My+q))-y＝0(12)

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

$y=\left[\begin{array}{c}x\\ u\\ v\end{array}\right],M=\left[\begin{array}{ccc}W& -A^T& C^T\\ A& 0& 0\\ -C& 0& 0\end{array}\right],q=\left[\begin{array}{c}p\\ -b\\ d\end{array}\right]$

其中，对偶决策变量u和v分别对应于等式约束(9)和不等式约束(10)。对于上述的分段线性投影方程(12)和二次型规划问题(8)-(11)，进而可以采用如下的二次型规划数值算法(即，二次型规划求解器)来求解：

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

φ(y^k)＝M^Te(y^k)+My^k+q

其中，迭代次数k＝0,1,2,…。给定初始值y⁰，通过该二次型规划数值算法的不断迭代，便可得到分段线性投影方程(12)的解，得到二次型规划问题(8)-(11)的最优解，也即是二次型优化冗余度解析方案(1)-(7)的最优解，从而得到基于突加度的障碍物躲避运动规划方法的最优解。

最后，通过二次型规划求解器得到该二次型规划问题的解后，将求解结果传递给下位机控制器驱动机械臂的运动，控制机械臂在突加度层上实现重复运动，同时也使得机械臂完成给定的末端任务。

以上所述的本发明的实施方式，并不构成对本发明保护范围的限定。任何在本发明的精神原则之内所作出的修改、等同替换和改进等，均应包含在本发明的权利要求保护范围之内。

Claims (4)

1.一种基于突加度的冗余度机械臂重复运动规划方法，其特征在于，包括如下步骤：

步骤1)通过上位机采用二次型优化在突加度层上对机械臂的重复运动进行逆运动学解析，得到基于突加度的重复运动二次型优化冗余度解析方案；具体步骤为：设计最小化性能指标为重复运动，重复运动受约束于基于突加度的雅可比矩阵等式、基于突加度的障碍物躲避不等式、关节角度极限、关节速度极限、关节加速度极限和关节突加度极限；即重复运动最小化为受约束于基于突加度的雅可比矩阵等式基于突加度的障碍物躲避不等式关节角度极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限 ${\stackrel{\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·}{\mathrm{\θ}}}^{+}$ 和关节突加度极限 ${\stackrel{\·\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·\·}{\mathrm{\θ}}}^{+};$ 其中，表示优化性能指标，表示机械臂关节突加度，上标T表示矩阵和向量的转置，W为单位矩阵，α、β、χ为用来控制关节位移幅值和收敛率的正设计参数，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ(0)表示机械臂的初始关节角度；等式约束表示机械臂末端运动轨迹，J_e表示机械臂末端执行器的雅可比矩阵， 表示机械臂末端执行器突加度向量，是J_e的时间导数，是的时间导数；C与d表示基于突加度的障碍物躲避参数，θ^±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，表示关节突加度上下限；

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

步骤3)将标准二次规划运用二次规划求解器进行求解，得到求解结果；

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

2.根据权利要求1所述的基于突加度的冗余度机械臂重复运动规划方法，其特征在于，所述步骤2)的重复运动二次型优化冗余度解析方案转化为标准二次规划，设计其性能指标为最小化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用来调节和保证关节突加度够大的可行域。

3.根据权利要求2所述的基于突加度的冗余度机械臂重复运动规划方法，其特征在于，所述步骤3)运用二次规划求解器进行求解。

4.根据权利要求3所述的基于突加度的冗余度机械臂重复运动规划方法，其特征在于，所述步骤4)将二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂完成障碍物躲避运动。