CN103231381B - 一种冗余度机械臂的新型加速度层重复运动规划方法 - Google Patents

Abstract

本发明提供了一种冗余度机械臂的新型加速度层重复运动规划方法，包括如下步骤：1)设计新型二次型形式的加速度层重复运动性能指标，生成二次型优化冗余度解析方案；2)将步骤1)的二次型优化冗余度解析方案转化为QP(二次规划)；3)将步骤2)的QP运用QP求解器进行求解；4)将步骤3)的求解结果传递给下位机控制器驱动机械臂运动。本发明突破了以往参数选择中的局限，可以较大范围地选取正的设计参数，使得冗余度重复运动规划可选择的方案显著增加，这在工程应用领域具有良好前景。

Description

一种冗余度机械臂的新型加速度层重复运动规划方法

技术领域

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

背景技术

冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂末端任务为一个封闭曲线时，其各个关节可能回不到初始位置，这种现象叫做关节角偏差现象，或称非重复运动问题，如果冗余度机械臂不能实现重复运动，则在运行过程中会产生不能预料甚至是危险有损机械臂与周围人员安全的问题；而重复运动规划方案就是要设计适当的性能指标，使得机械臂末端执行完封闭曲线任务时，各个关节角都能够回到其初始位置。

以往的重复运动解析方法大多都是在速度层上进行，得到的结果为速度值，这不能满足某些采用非速度控制的冗余度机械臂的要求，且速度层上的求解方法并不考虑加速度极限，机械臂在运动过程中就有可能因超越加速度极限而产生物理损坏。本发明人团队之前的发明《一种冗余度机械臂重复运动规划的方法》(申请号：201010264141.6)则为一种加速度层的重复运动解析方法，可以克服上述速度层控制的冗余度机械臂的缺陷，但是，该发明方案单一，并不能满足实际应用中对不同类型加速度层重复运动方案参数选择的需求。本发明专利提出一种新型的加速度层重复运动规划方案，有效地克服了以往加速度层重复运动规划中参数选择的局限，具有新颖性与实用性。

发明内容

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

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

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

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

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

3)将步骤2)的QP用QP求解器进行求解；

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

2、冗余度机械臂加速度层重复运动方案设计为：最小化受约束于 $J\stackrel{\·\·}{\mathrm{\θ}}={\stackrel{\·\·}{r}}_a,$ $A\stackrel{\·\·}{\mathrm{\θ}}\≤b,{\mathrm{\θ}}^{-}\≤\mathrm{\θ}\≤{\mathrm{\θ}}^{+},{\stackrel{\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·}{\mathrm{\θ}}\≤{\stackrel{\·}{\mathrm{\θ}}}^{+},{\stackrel{\·\·}{\mathrm{\θ}}}^{-}\≤\stackrel{\·\·}{\mathrm{\θ}}\≤{\stackrel{\·\·}{\mathrm{\θ}}}^{+},$ 其中表示关节加速度，W为单位阵， p^T表示p的转置，α＞0和β＞0且α²＜4β是用来控制关节位移幅值和收敛率的设计参数，等式约束表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是其时间导数，θ表示关节角，表示关节速度，表示机械臂末端执行器加速度矢量，θ^-≤θ≤θ⁺、 分别表示关节角极限、关节速度极限和关节加速度极限，θ^±表示关节角上下限，表示关节速度上下限，表示关节加速度上下限，A和b是障碍物躲避参数。

3、上述的二次型优化方案可以转化为一个QP，即最小化x^TWx/2+p^Tx，受约束于Cx＝d，Ax≤b，ζ^-≤x≤ζ⁺，其中，W为单位矩阵，C＝J，ζ⁺和ζ^-分别表示合成双端约束的上下极限，它们第i个元素分别定义为和其中为裕度，i表示关节序号，i＝1，2，...，n，正常数k_α和k_β用来调节和保证关节加速度足够大的可行域。

4、此QP可以用QP求解器求解。

5、QP求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂重复运动。

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

以往，对冗余度机械臂的加速度层控制只局限于某一固定参数形式，这局限了冗余度机械臂的加速度层控制方案的应用，本发明给出了一个新型的加速度层重复运动方案，使得参数可以根据实际需要配置，这在实际应用中有重要作用和广阔前景。

附图说明

图1为本发明的流程图；

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

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

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

具体实施方式

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

图1所示的新型冗余度机械臂重复运动规划方法首先提出加速度层重复运动性能指标与约束；然后将其转化为相应的QP；接着应用QP求解器求解：最后下位机控制器根据求解结果控制机械臂的运动。

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

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

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

最小化 ${\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)$

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

θ^-≤θ≤θ⁺    (4)

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

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

其中，为新的加速度层重复运动性能指标，表示关节加速度，W为单位矩阵，p^T表示p的转置，α＞0和β＞0且α²＜4β是用来控制关节位移幅值和收敛率的设计参数，等式约束表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，为其时间导数，θ表示关节角，表示关节速度，表示机械臂末端执行器加速度矢量，θ^±表示关节角上下限，表示关节速度上下限，表示关节加速度上下限，A和b为障碍物躲避参数。

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

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

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

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

约束条件：Cx＝d    (8)

Ax≤b    (9)

ζ^-≤x≤ζ⁺    (10)

其中， $x=\stackrel{\·\·}{\mathrm{\θ}},$ W为单位矩阵， $z=\mathrm{\α}\stackrel{\·}{\mathrm{\θ}}+\mathrm{\β}[\mathrm{\θ}-\mathrm{\θ}\left(0\right)],$ C＝J， $d={\stackrel{\·\·}{r}}_a=\stackrel{\·\·}{r}-\stackrel{\·}{J}\stackrel{\·}{\mathrm{\θ}}.$ 上述QP问题求解等价于寻求一个向量使之满足其中y＝[x，u，v]^T， 1_u和1_v分别表示由1组成的相应维数向量，是足够大的常数，用于数值上替代无穷大+∞，u∈R^m和v∈R^dim(b)表示由等式约束(8)和不等式约束(9)得到的向量， $H=\left[\begin{array}{ccc}W& -J^T& A^T\\ J& 0& 0\\ -A& 0& 0\end{array}\right],$ $g=\left[\begin{array}{c}z\\ -d\\ b\end{array}\right]$

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

${\left[P_{\mathrm{\&Omega;}}\left(y\right)\right]}_i=\left\{\begin{array}{cc}{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},\&CenterDot;\&CenterDot;\&CenterDot;,n+m+\dim \left(b\right)\}.$

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

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

其中d(y^k)：＝H^Te(y^k)+Hy^k+g和 $\mathrm{\&rho;}\left(y^k\right):={\left|\right|e\left(y^k\right)\left|\right|}_2^2/{\left|\right|(H^T+I)e\left(y^k\right)\left|\right|}_2^2,k=\mathrm{0,1,2},\&CenterDot;\&CenterDot;\&CenterDot;.$ 通过算法不断迭代，得到分段线性投影方程P_Ω(y-(Hy+g))-y＝0的解，从而得到冗余度机械臂运动规划的最优解。

通过上位机的QP求解器得到该QP的解后，再将结果转化为电机驱动所需要的控制信号，并通过该冗余度机械臂的控制器来驱动机械臂各个关节电机运动，从而驱动各关节电机使机械臂重复运动。

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

在加速度层重复运动方案实施过程中远离障碍物，α＝1，β＝3，机械臂初始位置关节1-6角度设置为[0，-π/4，0，π/2，-π/4，0]^T弧度，角度上极限设置为θ⁺＝[2.775，0.750，4.049，2.967，0.035，3.142]^T弧度，角度下极限设置为θ^-＝[-2.775，-3.892，-0.905，-1.919，-1.745，-4.625]^T弧度，速度上极限设置为弧度/秒，速度下极限设置为弧度/秒，加速度上极限设置为 ${\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}^{+}={\left[\mathrm{6,6,6,6,6,6}\right]}^T$ 弧度/秒²，加速度下极限设置为 ${\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}^{-}={[-6,-6,-6,-6,-6,-6]}^T$ 弧度/秒²。通过QP求解器求解，将计算得到的结果再传送给机械臂控制器从而控制机械臂的运动。

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

Claims (4)

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

1)通过上位机采用新型二次型优化方案在加速度层上对机械臂的重复运动进行冗余度解析，设计的新型最小化性能指标为重复运动，受约束于加速度雅可比等式、障碍物躲避不等式、关节角极限、关节速度极限和关节加速度极限，即为最小化受约束于加速度雅可比等式障碍物躲避不等式约束关节角极限θ^-≤θ≤θ⁺、关节速度极限关节加速度极限其中表示机械臂关节加速度，W为单位矩阵， 上标^T表示矩阵和向量的转置，α＞0和β＞0且α²＜4β是用来控制关节位移幅值和收敛率的设计参数，等式约束表示机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是其时间导数，θ表示机械臂的关节角，θ(0)表示其初始关节角，表示机械臂的关节速度，表示机械臂末端执行器加速度矢量，θ^±表示关节角上下限，表示关节速度上下限，表示关节加速度上下限，A和b是障碍物躲避参数；

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

3)将步骤2)的QP用数值算法(也即QP求解器)进行求解；

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

2.根据权利要求1所述的一种冗余度机械臂的新型加速度层重复运动规划方法，其特征在于所述步骤2)的新型二次型优化转化为一个QP，设计其性能指标为最小化x^TWx/2+p^Tx，受约束于Cx＝d，Ax≤b，其中，W为单位矩阵， $p=\mathrm{\&alpha;}\stackrel{.}{\mathrm{\&theta;}}+\mathrm{\&beta;}[\mathrm{\&theta;}-\mathrm{\&theta;}\left(0\right)],$ C＝J， $d={\stackrel{..}{r}}_a=\stackrel{..}{r}-\stackrel{.}{J}\stackrel{.}{\mathrm{\&theta;}},$ A，b为障碍物躲避参数，和分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和其中为裕度，i表示关节序号，i＝1，2，...，n，正常数k_α和k_β用来调节和保证关节加速度足够大的可行域。

3.根据权利要求2所述的一种冗余度机械臂的新型加速度层重复运动规划方法，其特征在于所述步骤3)的QP求解器求解。

4.根据权利要求3所述的一种冗余度机械臂的新型加速度层重复运动规划方法，其特征在于所述步骤3)的QP求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂重复运动。