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

Abstract

本发明提供了一种冗余度机械臂运动规划方法，包括如下步骤：1)通过上位机采用二次型优化在速度层上对机械臂的逆运动学解析，设计的最小性能指标可为速度范数、重复运动或动能，受约束于速度雅可比等式、不等式和关节角速度极限，该角速度极限是随关节角度变化的；2)将步骤1)的二次型优化转化为二次规划问题；3)将步骤2)的二次规划问题用线性变分不等式原对偶神经网络求解器或数值方法求解；4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。本发明基于线性变分不等式的原对偶神经网络具有全局指数收敛性，且没有涉及到矩阵求逆等复杂运算，大大地提高了计算效率，同时实时性强且能适应关节角速度极限变化。

Description

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

技术领域

本发明属于冗余度机械臂运动规划方法，特别是涉及一种关节角速度变极限的冗余度机械臂运动规划方法。

背景技术

冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置，其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。传统的冗余度解析方法以及工业机械臂控制方法主要是基于伪逆的方法：即，把问题的解转化成求一个最小范数解加上一个同类解。次目标可以被指定到同类解上，去控制机械臂的自运动以躲避障碍物、关节极限、奇异点和优化其它目标函数。其缺点是在处理不等式约束上有困难，计算量大，实时性差，而且它会遇到奇异情况而生成不可行解，在实际机械臂的应用中受到很大的制约。

发明内容

本发明的目的在于克服现有技术的不足，提供一种计算量小、实时性强且能适应关节角速度极限变化的冗余度机械臂运动规划方法。

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

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

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

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

3)将步骤2)的二次规划问题用线性变分不等式原对偶神经网络求解器或数值方法求解；

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

本发明基于线性变分不等式的原对偶神经网络具有全局指数收敛性，且没有涉及到矩阵求逆等复杂运算，大大地提高了计算效率，同时实时性强且能适应关节角速度极限变化。

附图说明

图1为本发明的流程图；

图2为实现本发明的机械臂结构主视图；

图3为实现本发明的机械臂结构俯视图；

图4为机械臂局部示意图。

具体实施方式

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

图1所示的冗余度机械臂运动规划方法主要由目标问题1、二次规划问题2、基于线性变分不等式原对偶神经网络求解器或二次规划数值算法3、下位机控制器4和机械臂5组成。先将逆运动学求解在速度层上设计为最小化

且受约束于

θ^-≤θ≤θ⁺，

欲优化的性能指标

可以是最小速度范数函数

重复运动指标

或最小动能函数

将上述的各种冗余度解析方案转化为通用的二次型优化标准形式2，再使用基于线性变分不等式的原对偶神经网络或二次规划数值方法3求解，并将求解结果传递给下位机控制器4驱动机械臂5运动。

图2和图3所示的机械臂由机械臂连杆1、推杆2、关节3、关节与推杆施力点连结部4和基座5组成。其中推杆2的存在使其不同于传统无推杆串联机械臂的角速度恒定极限方式，而是一种变极限机械臂。在实时计算机械臂的逆解时，这个角速度极限是角度的函数。因此，通过改变二次规划的约束条件

从而实现变极限的控制。

图4所示机械臂局部示意图，在通常的设计中，认为机械的动力产生于内部电机力矩，即假设动力不是来自于推杆，或者认为4非常小，几乎可以忽略，这样1和2就会重合在一起。本发明涉及的机械臂设计中，推杆是存在的，即4不可能忽略。这样，由于推杆的存在使得原来的角速度极限在每时每刻都会变化，是角度的函数。假设1的长度为a，4的长度为b，2的长度为c。具体推导过程如下：

$c^2=a^2+b^2-2a*b*\cos (\frac{\mathrm{\π}}{2}+\mathrm{\θ});---\left(1\right)$

将上述公式对时间t求导，得

$2c*\frac{\mathrm{dc}}{\mathrm{dt}}=2a*b*\sin (\frac{\mathrm{\π}}{2}+\mathrm{\θ})*\stackrel{\·}{\mathrm{\θ}};---\left(2\right)$

其中，c＝c₀+v*Δt*Δc，c₀为初始的c边长，v为步进电机的转速，Δc为电机转一圈对应的电推杆伸长量。进一步可得

$2c*(v*\mathrm{\Δc})=2a*b*\sin (\frac{\mathrm{\π}}{2}+\mathrm{\θ})*\stackrel{\·}{\mathrm{\θ}}=2a*b*\cos \mathrm{\θ}*\stackrel{\·}{\mathrm{\θ}};---\left(3\right)$

即 $c*\mathrm{\Δc}*v=a*b*\cos \mathrm{\θ}*\stackrel{\·}{\mathrm{\θ}};$ (4)

所以 $\stackrel{\·}{\mathrm{\θ}}=\frac{c*\mathrm{\Δc}*v}{a*b*\cos \mathrm{\θ}}=\frac{\mathrm{\Δc}*v*\sqrt{a^2+b^2-2a*b*\cos (\frac{\mathrm{\π}}{2}+\mathrm{\θ})}}{a*b*\cos \mathrm{\θ}},---\left(5\right)$

因此可得关节速度的变极限为

$\frac{\mathrm{\Δc}*v^{-}*\sqrt{a^2+b^2-2a*b*\cos (\frac{\mathrm{\π}}{2}+\mathrm{\θ})}}{a*b*\cos \mathrm{\θ}}:={\stackrel{\·}{\mathrm{\θ}}}^{-}\left(\mathrm{\θ}\right)\≤\stackrel{\·}{\mathrm{\θ}}$ (6)

$\≤{\stackrel{\·}{\mathrm{\θ}}}^{+}\left(\mathrm{\θ}\right):=\frac{\mathrm{\Δc}*v^{+}*\sqrt{a^2+b^2-2a*b*\cos (\frac{\mathrm{\π}}{2}+\mathrm{\θ})}}{a*b*\cos \mathrm{\θ}},$

其中，v⁺和v^-分别为相关关节步进电机的转速正极限和负极限。通过考虑角速度的极限为随角度变化的函数，在设计控制方法时，修改相应的约束条件，组成带有变极限参数的角速度极限条件，从而实现对变极限问题的解决。

基于前面的分析，机械臂的逆运动学求解在速度层上可设计为：

$\min \mathrm{\φ}(\mathrm{\θ},\stackrel{\·}{\mathrm{\θ}})---\left(7\right)$

$s.t.J\stackrel{\·}{\mathrm{\θ}}=\stackrel{\·}{r},---\left(8\right)$

$A\stackrel{\·}{\mathrm{\θ}}\≤b,---\left(9\right)$

θ^-≤θ≤θ⁺，                           (10)

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

其中，

代表欲优化的性能指标；等式约束

表述机械臂末端运动轨迹；不等式约束

可以用于环境障碍物的躲避或其它性能约束；θ^-≤θ≤θ⁺、

分别是关节角度极限、关节角速度极限。

欲优化的性能指标

可以设计为各种冗余度解析方案的优化判据。其可以是最小速度范数函数，即

也可是重复运动指标，即

其中z＝λ(θ-θ(0))，λ＞0是用来控制关节位移幅值的正设计参数；还可以是最小动能函数

等。

如图1所示的步骤1，将上述问题转化为一个标准的二次规划问题去求解才能应用到机械臂的控制上去。该二次规划问题可写为如下通用形式：

minx^TWx/2+q^Tx，                (12)

s.t.Jx＝d，                    (13)

Ax≤b，                        (14)

x^-≤x≤x⁺。                    (15)

其中，决策变量x可以被定义为

W，q，J，d，A，b，x^-，x⁺为已知的相对应的系数矩阵和向量，比如，在最小速度范数方案中，W为单位矩阵，q＝0，J为雅可比矩阵，

而A，b可以是障碍物躲避参数或者由优化指标转化得到的不等式约束，x^-，x⁺由公式(10)、(11)通过变换获得。

下面说明关节物理极限的处理和变换过程，即如何由公式(10)、(11)转换得到公式(15)。在速度层上解析的时候，需要将(10)式转换为速度层

上的表达形式：

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

其中系数μ＞0是用来调节关节角速度的可行域，系数μ的选取应该使(16)式转换后的可行域比原来的关节角速度可行域在一般情况下略大。由此，双端约束公式(10)和(11)可以合并为一个统一的双端约束：x^-≤x≤x⁺，其中x^-和x⁺的第i个元素分别定义如下：

$x_i^{-}=\max \{{\stackrel{\·}{\mathrm{\θ}}}_i^{-}\left(\mathrm{\θ}\right),\mathrm{\μ}({\mathrm{\θ}}_i^{-}-{\mathrm{\θ}}_i)\},---\left(17\right)$

$x_i^{+}=\min \{{\stackrel{\·}{\mathrm{\θ}}}_i^{+}\left(\mathrm{\θ}\right),\mathrm{\μ}({\mathrm{\θ}}_i^{+}-{\mathrm{\θ}}_i)\}.---\left(18\right)$

本发明用双端不等式来表述关节物理极限的躲避，以上相关参数μ的选择可以基于理论分析或基于经验。

得到上述的二次规划问题(12)-(15)式后，本发明的求解方法是采用基于线性变分不等式的原对偶神经网络或二次规划数值算法来实时求解此二次规划问题。

以下就是基于线性变分不等式的原对偶神经网络来求解带约束的二次规划问题(12)-(15)的神经网络求解器的构造过程。

首先，将二次规划问题(12)-(15)式转化为一个线性变分不等式，即求一个原对偶变量

使得

(y-y^*)^T(My^*+p)≥0，                    (19)

其中，原对偶变量y及其上下限定义如下：

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

对偶变量u和v分别与等式约束(13)和不等式约束(14)相对应；1_v：＝[1，...，1]^T是元素都为1的相应维数向量；是足够大的常数，用于数值上替代无穷大+∞，而扩展矩阵M、p分别定义如下：

$M=\left[\begin{array}{ccc}W& {-J}^T& A^T\\ J& 0& 0\\ -A& 0& 0\end{array}\right],$ $p=\left[\begin{array}{c}q\\ -d\\ b\end{array}\right];$

由此可总结归纳为：至少存在一个最优解x^*时，二次规划问题(12)-(15)可转化为线性变分不等式问题(19)。

其次，线性变分不等式问题(19)又等价于线性投影方程，即P_Ω(y-(My+p))-y＝0，其中P_Ω(·)为空间R^{dim(x)+dim(d)+dim(b)}到集合Ω的分段线性投影算子，P_Ω(y)的第i个计算单元定义为

$\left\{\begin{array}{ccc}{y}_i^{-}& \mathrm{if}& y_i<y_i^{-}\\ y_i& \mathrm{if}& y_i^{-}\&le;y_i\&le;y_i^{+}\\ y_i^{+}& \mathrm{if}& y_i>y_i^{+}\end{array}\right.,\&ForAll;i\&Element;\{\mathrm{1,2},...,\dim \left(x\right)+\dim \left(d\right)+\dim \left(b\right)\}.$

接着，用下面的动力学***(作为基于线性变分不等式的原对偶神经网络的动力学描述形式，如图1的步骤3)来求解上述线性变分不等式问题及二次规划问题：

$\stackrel{\&CenterDot;}{y}=\mathrm{\&gamma;}(I+M^T)\{P_{\mathrm{\&Omega;}}(y-(\mathrm{My}+p))-y\},---\left(20\right)$

其中，设计参数γ＞0用来调节网络的收敛性，γ越大该网络收敛得越快。此外，当(12)-(15)至少存在一个最优解x^*时，从任何初始状态出发，线性变分不等式原对偶神经网络(20)的状态向量y(t)都将收敛至某平衡点y^*，其前dim(x)个元素组成了二次规划问题(12)-(15)的最优解x^*。如果存在一个常数ρ＞0，使得||y-P_Ω(y-(My+p))||²≥ρ||y-y^*||²成立，则神经网络(20)全局指数收敛于平衡点y^*和问题最优解x^*(其收敛率正比于γρ)。将计算得到的角速度再传送给下位机控制器从而控制机械臂的运动，实现本发明的方法。

Claims (4)

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

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

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

3)将步骤2)的二次规划问题用基于线性变分不等式的原对偶神经网络求解器或二次规划数值方法进行求解；

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

2.根据权利要求1所述的冗余度机械臂运动规划方法，其特征在于所述步骤1)的二次型优化的冗余度解析方案将机械臂的逆运动学求解在速度层上设计为：最小化

受约束于

θ^-≤θ≤θ⁺，

其中

代表欲优化的性能指标，等式约束

表述机械臂末端运动轨迹，不等式约束

表示用于环境障碍物的躲避性能约束，θ^-≤θ≤θ⁺、分别表示关节角度极限、关节角速度极限；

所述欲优化的性能指标

为各种冗余度解析方案的优化判据，在速度层上

采用最小速度范数函数，即

或重复运动指标，即

其中z＝λ(θ-θ(0))，λ＞0是用来控制关节位移幅值的正设计参数，或最小动能函数

其中H为机械臂惯性矩阵。

3.根据权利要求2所述的冗余度机械臂运动规划方法，其特征在于所述步骤2)的二次规划问题可以转化为一个线性变分不等式，即求一个原对偶变量使得

(y-y^*)^T(My^*+p)≥0，其中，原对偶变量y及其上下限定义如下：

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

对偶变量u和v分别与等式约束(7)和不等式约束(8)相对应；1_v：＝[1，...，1]^T是元素都为1的相应维数向量；是足够大的常数，用于数值上替代无穷大+∞，而扩展矩阵M、p分别定义如下：

$M=\left[\begin{array}{ccc}W& -J^T& A^T\\ J& 0& 0\\ -A& 0& 0\end{array}\right],$ $p=\left[\begin{array}{c}q\\ -d\\ b\end{array}\right].$

4.根据权利要求3所述的线性变分不等式，其特征在于等价于线性投影方程P_Ω(y-(My+p))-y＝0，其中P_Ω(·)为空间R^{dim(x)+dim(d)+dim(b)}到集合Ω的分段线性投影算子，P_Ω(y)的第i个计算单元定义为

$\left\{\begin{array}{ccc}{y}_i^{-}& \mathrm{if}& y_i<y_i^{-}\\ y_i& \mathrm{if}& y_i^{-}\&le;y_i\&le;y_i^{+}\\ y_i^{+}& \mathrm{if}& y_i>y_i^{+}\end{array}\right.,$ $\&ForAll;i\&Element;\{\mathrm{1,2,3},...,\dim \left(x\right)+\dim \left(d\right)+\dim \left(b\right)\}.$

接着，用动力学***

解上述线性变分不等式问题及二次规划问题，其中，设计参数γ＞0用来调节网络的收敛性，γ越大该网络收敛得越快。

Images