CN101927495A - 一种冗余度机械臂重复运动规划方法 - Google Patents

Abstract

本发明提供了一种冗余度机械臂重复运动规划的方法，包括如下步骤：1)通过上位机采用二次型优化在加速度层上对机械臂重复运动方案进行解析，设计最小化性能指标为重复运动，受约束于加速度雅可比等式、关节角极限、关节速度极限和关节加速度极限；2)将步骤1)的二次型优化转化为标准二次规划；3)将步骤2)的标准二次规划用基于线性投影方程的原对偶神经网络求解器或二次规划数值方法进行求解；4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。本发明在加速度层上实现机械臂的重复运动，对于不便于速度控制的冗余度机械臂具有较好的控制效果，且本发明考虑到多种关节极限，能有效防止机械臂超越极限产生物理损坏。

Description

一种冗余度机械臂重复运动规划方法

技术领域

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

背景技术

冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置，其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂末端任务为一个封闭曲线时，其各个关节可能回不到初始位置，这种现象叫做关节角偏差现象，或称非重复运动问题；而重复运动规划方案就是要设计适当的指标，使得机械臂末端执行完封闭曲线任务时，各个关节角都能够回到其初始位置。

以往的重复运动解析方法都是在速度层上进行，得到的结果均为速度值，这不能满足某些采用非速度控制的冗余度机械臂的要求，且速度层上的求解方法并不考虑加速度极限，机械臂在运动过程中就有可能因超越加速度极限而产生物理损坏。本发明能弥补这些不足。

发明内容

本发明所要解决的技术问题是提供一种计算量小、用于加速度控制的冗余度机械臂重复运动规划方法。

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

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

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

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

3)将步骤2)的标准二次规划用基于线性投影方程的原对偶神经网络求解器或二次规划数值方法进行求解；

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

2、冗余度机械臂重复运动加速度层方案设计为：最小化

受约束于

θ^-≤θ≤θ⁺、

其中表示关节加速度，W为合适维数的单位阵，

p^T表示p的转置，μ和v是用来控制关节位移幅值和收敛率的正设计参数，等式约束

表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，

是其时间导数，θ表示关节角，

表示关节速度，

表示机械臂末端执行器加速度矢量，θ^-≤θ≤θ⁺、

分别表示关节角极限、关节速度极限和关节加速度极限，θ^±表示关节角上下限，

表示关节速度上下限，

表示关节加速度上下限。

3、上述的二次型优化可以转化为一个标准二次规划，即最小化x^TWx/2+z^Tx，受约束于Cx＝d，ζ^-≤x≤ζ⁺，其中，

W为单位矩阵，

C＝J，

正常数k_μ、k_v、λ用来调节关节加速度可行域。

4、此标准二次规划可以转化为一个线性投影方程P_Ω[u-(Mu+q)]-u＝0的求解，其中P_Ω[·]为空间R^n+m到集合

的分段线性投影算子，m为笛卡尔空间的维数，n为关节空间的维数，u表示原对偶变量，u^-表示原对偶变量下极限，u⁺表示原对偶变量上极限，M、q为相关扩展矩阵。

5、该线性投影方程用原对偶神经网络求解器或二次规划数值方法求解。

附图说明

图1为本发明的流程图；

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

图3为机械臂非重复运动示意图；

图4为实现本发明的机械臂重复运动示意图。

具体实施方式

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

图1所示的冗余度机械臂重复运动规划方法主要由加速度层重复运动性能指标与约束1、标准二次规划2、基于线性投影方程的原对偶神经网络求解器或二次规划数值方法3、下位机控制器4和机械臂5组成。

图2所示实现本发明的机械臂为一个平面六自由度的机械臂。该机械臂由六个连杆所组成，通过关节9、关节10、关节11、关节12、关节13和关节14组成。

图3所示机械臂的非重复运动示意图。给定末端任务8为一个闭合曲线，机械臂从初始位置6开始执行，完成任务后，机械臂停在位置7，而并没有回复到初始位置1。它没有做到重复运动；即，完成任务时，机械臂未回复到初始位置，各个关节末态位置角不等于初始位置角。

图4为实现本发明的机械臂重复运动示意图。本发明设计的加速度层重复运动规划方案为

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

约束条件 $J\stackrel{\·\·}{\mathrm{\θ}}={\stackrel{\·\·}{r}}_a=\stackrel{\·\·}{r}-\stackrel{\·}{J}\stackrel{\·}{\mathrm{\θ}}---\left(2\right)$

θ^-≤θ≤θ⁺                                (3)

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

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

其中，

为加速度层重复运动性能指标，

表示关节加速度，W为合适维数的单位阵，

p^T表示p的转置，μ和v是用来控制关节位移幅值和收敛率的正设计参数，等式约束

表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，

是其时间导数，θ表示关节角，

表示关节速度，

表示机械臂末端执行器加速度矢量，θ^-≤θ≤θ⁺、分别表示关节角极限、关节速度极限和关节加速度极限，θ^±表示关节角上下限，

表示关节速度上下限，

表示关节加速度上下限。

考虑到上述优化问题是在加速度层上求解，因此需将机械臂的关节角度约束(3)、关节速度约束(4)，关节加速度约束(5)合并，从而可以得到基于加速度的双端不等式约束：

${\mathrm{\ζ}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\mathrm{\ζ}}^{+}$

其中，ζ^-和ζ⁺中的第i个元素分别定义为：

正常数k_μ、k_v、λ用来调节关节加速度的可行域。用x表示机械臂的关节加速度

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

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

约束条件：Cx＝d                     (7)

ζ^-≤x≤ζ⁺                         (8)

其中，W为单位矩阵，

C＝J，

上述标准二次规划问题可以转化为一个线性投影方程P_Ω[u-(Mu+q)]-u＝0的求解，其中P_Ω[·]为空间R^n+m到集合的分段线性投影算子，m为笛卡尔空间的维数，n为关节空间的维数，u表示原对偶变量，u^-表示原对偶变量下极限，u⁺表示原对偶变量上极限，原对偶变量u及其上下限定义如下：

$u=\left[\begin{array}{c}x\\ y\end{array}\right],$

y∈R^m是对应于等式约束

的对偶决策向量，1_v：＝[1，...，1]^T是元素都为1的相应维数向量；

是足够大的常数，用于数值上替代无穷大+∞，而扩展矩阵M、q分别定义如下：

$M=\left[\begin{array}{cc}W& {-J}^T\\ J& 0\end{array}\right]\∈R^{(n+m)\×(n+m)},$ $q=\left[\begin{array}{c}z\\ -d\end{array}\right]\∈R^{n+m},$

接着，线性投影方程用原对偶神经网络求解器或二次规划数值方法求解。原对偶神经网络求解器的动力学***为

或

当使用二次规划数值方法时，设计其计算误差为e(u)＝u-P_Ω[u-(Mu+q)]，当误差为零时，对应的u值便为分段线性方程的解。给定初始值u⁰∈Rⁿ，通过如下的迭代来得到u使得误差e(u)达到预设的精度：u^k+1＝u^k-ρ(u^k)d(u^k)，其中k＝0，1，2，…，d(u^k)和ρ(u^k)定义为d(u^k)＝(M^T+I)e(u^k)，ρ(u^k)＝||e(u^k)||²/||d(u^k)||²，通过算法不断迭代，便可得到分段线性投影方程P_Ω(u-(Mu+p))-u＝0的解，从而得到加速度层重复运动二次规划的最优解。

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

在加速度层重复运动方案实施过程中，机械臂初始位置关节9-14角度设置为[π/3，π/6，π/6，π/3，π/6，π/6]^T弧度，角度上极限设置为θ⁺＝[2.775，0.750，3.142，2.967，0.035，3.142]^T弧度，角度下极限设置为θ^-＝[-2.775，-3.142，-0.906，-1.920，-1.745，-3.142]^T弧度，速度上极限设置为弧度/秒，速度下极限设置为

弧度/秒，加速度上极限设置为

弧度/秒²，加速度下极限设置为

弧度/秒²。所使用的基于线性投影方程的原对偶神经网络器动力学***为

将计算得到的加速度再传送给机械臂控制器从而控制机械臂的运动。

机械臂从初始位置6开始执行，完成任务后，机械臂回到了位置7，而位置7与位置6重合；也即，机械臂的各个末态关节角等于初始关节角，机械臂实现了重复运动。

Claims (5)

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

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

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

3)将步骤2)的标准二次规划用基于线性投影方程的原对偶神经网络求解器或二次规划数值方法进行求解；

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

2.根据权利要求1所述的冗余度机械臂重复运动规划方法，其特征在于所述步骤1)的加速度层重复运动方案设计为：最小化受约束于加速度雅可比等式

关节角极限θ^-≤θ≤θ⁺、关节速度极限

关节加速度极限

其中表示关节加速度，W为合适维数的单位阵，

p^T表示p的转置，μ和v是用来控制关节位移幅值和收敛率的正设计参数，等式约束

表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是其时间导数，θ表示关节角，

表示关节速度，

表示机械臂末端执行器加速度矢量，θ^-≤θ≤θ⁺、

分别表示关节角极限、关节速度极限和关节加速度极限，θ^±表示关节角上下限，

表示关节速度上下限，

表示关节加速度上下限。

3.根据权利要求2所述的冗余度机械臂重复运动规划方法，其特征在于所述步骤2)的二次型优化转化为一个标准二次规划，设计其性能指标为最小化x^TWx/2+z^Tx，受约束于Cx＝d，ζ^-≤x≤ζ⁺，其中，

W为单位阵，

C＝J，

正常数k_μ、k_v和λ用来调节关节加速度的可行域。

4.根据权利要求3所述的冗余度机械臂重复运动规划方法，其特征在于所述步骤3)的标准二次规划转化为一个线性投影方程P_Ω[u-(Mu+q)]-u＝0的求解，其中P_Ω[]为空间R^n+m到集合

的分段线性投影算子，m为笛卡尔空间的维数，n为关节空间的维数，u表示原对偶变量，u^-表示原对偶变量下极限，u⁺表示原对偶变量上极限，M、q为相关扩展矩阵。

5.根据权利要求4所述的冗余度机械臂重复运动规划方法，其特征在于所述步骤3)的线性投影方程，用原对偶神经网络求解器或二次规划数值方法求解。

Images