针对关节限位优化的7自由度机械臂逆运动学解法
胡奎, 张继文, 董云飞, 吴丹    
清华大学 机械工程系, 北京 100084
摘要:为解决机械臂运行过程中避让关节限位的问题,该文以S-R-S构型的7自由度机械臂为对象,提出了一种机械臂逆运动学优化算法。首先,引入臂角变量,推导得到含臂角变量的逆运动学封闭解,并在此基础上,将关节限位和臂角可行范围进行映射;其次,定义一种包含权重函数的避限位优化目标函数,其中权重函数取值与关节实际位置有关;最后,通过线性展开方法简化臂角与关节位置的关系式,从而将原有复杂形式的优化目标函数简化为二次函数,结合求得的臂角可行范围完成优化求解,实现关节避限位。计算结果表明:该算法可以有效避让关节限位,且直观、简单、运算效率较高,可以进行实时运算。
关键词冗余机械臂    逆运动学    避限位    7自由度    
Inverse kinematic optimization for 7-DoF serial manipulators with joint limits
HU Kui, ZHANG Jiwen, DONG Yunfei, WU Dan    
Department of Mechanical Engineering, Tsinghua University, Beijing 100084, China
Abstract: An optimized inverse kinematic algorithm was developed for 7 degrees of freedom (7-DoF) manipulators with an S-R-S kinematic structure that avoids joint limits. The first step determines the closed-form solution of the inverse kinematics model based on the arm angle parameter. The result is then used to determine feasible arm angle intervals for the given joint limits. Then, an optimal weighted target function is defined to avoid the joint limits with the weights determined according to the joint positions. The relations between the joint angles and the arm angles are then linearized to simplify the complex optimal target function to a quadratic function. The optimization problem that includes the feasible arm angle intervals then avoids the joint limit. This intuitive, efficient algorithm is effective, avoids the joint limits and can be computed online.
Key words: redundant manipulator    inverse kinematics    joint limit avoidance    7-DoF    

相比6自由度机械臂,7自由度机械臂在运动灵活性上的表现更为优异。由于冗余自由度的存在,7自由度机械臂的逆运动学求解,在保证末端运动精度的同时,还可以对其他目标进行优化,如避奇异[1]、防止关节极限[2-4]、避障[5-7]以及适应人类动作[8]等。但是,7自由度机械臂在提高机械臂灵活性的同时,也增加了逆运动学求解的难度[9]

针对冗余自由度机器人的逆运动学问题,一般的方法是结合速度层面的Jacobi矩阵,依赖优化方法迭代求解[10]。例如,Liegeois[11]提出梯度投影方法,将逆运动学问题的解分解为最小范数解和齐次解2部分,引入目标函数和放大系数进行解的优化;文[12]在此基础上完成了避让关节限位的优化。虽然迭代法求解逆运动学具有通用性的优点,但对于优化参数的选择,存在需要反复试凑的问题[13]

S-R-S(spherical-rotational-spherical)机械臂是一类具有特殊构型、应用广泛的7自由度机械臂结构,其与人的手臂的构型类似。诸多采用该结构的7轴机械臂包括KUKA公司的LBR iiwa机械臂、ABB公司的YuMi单臂机器人、新松公司的SCR5协作机器人等。S-R-S特殊结构体现在可以通过几何法[14-18]得到机械臂逆运动学的关节位置封闭解。这些方法的基本思想是引入一个参数代表冗余自由度,将逆运动学的关节位置解表示为该参数的解析函数,而冗余自由度参数的具体取值,则由需要优化的目标确定。由于这种方法能够获得逆运动学的封闭解形式,因此具有求解精度高、运算速度快、可用于机械臂实时控制的优点。针对具有S-R-S结构的冗余机械臂,为保证机械臂的运行安全,实现冗余机械臂躲避关节限位的二次目标,相关学者通过选取合适的冗余自由度参数对避免关节限位问题展开了一系列研究。

文[2]定义了机械臂的肘心,并通过最小化机械臂肘心的运动距离来间接避开关节限位;文[3]引入了表征机械臂自运动的参数,推导了逆运动学结果的表达形式,并且解析求解了机械臂的关节限位与自运动范围的映射关系,通过定义理想关节位置与实际关节位置之间旋转矩阵的差异,推导并优化了以自运动参数为变量的避限位的目标函数;文[4]则利用求得的自运动范围的边界与当前自运动位置的距离,提出了一种计算自运动间隔的方法,通过在自运动范围边界处取较小的自运动间隔来达到避限位的目的;文[19]采用人工势场法的思想,将关节极限位置视为斥力源,将避关节限位的优化指标转换为求虚拟转矩的问题。上述方法均能达到避限位的目的,但都采用间接方式避限位,不够直观,且由于都需要求解所有关节的运动范围与自运动范围的映射关系,因此均存在算法较复杂,计算量相对较大,不易于实际应用的问题。

本文针对特殊的S-R-S构型7自由度机械臂提出一种逆运动学求解优化算法以有效解决机械臂关节避限位问题。首先引入臂角变量,得到含臂角变量的逆运动学封闭解,并分析关节限位与臂角可行范围的映射关系;然后建立一种避限位优化目标函数及其简化表达式,以使优化求解过程直观而简便;最后进行计算分析以评价算法的有效性。

1 机械臂运动学 1.1 S-R-S机械臂结构及参数

本文的研究对象为应用较为广泛的S-R-S构型的7自由度机械臂,机械臂结构如图 1所示,其中关节i的可运动范围为[θilθiu],采用D-H方法对机械臂进行建模,得到的D-H参数如表 1所示。图 1中,S为肩心,为前3个关节轴线的交点;W为腕心,为后3个关节轴线的交点;E为肘心,为第3关节和第5关节轴线的交点;B为基座坐标系原点;F为末端坐标系原点。

图 1 S-R-S构型的7自由度机械臂

表 1 机械臂D-H参数表
序号 ai di αi θi 关节范围/(°)
1 0 dBS - π/2 θ1 [θ1l, θ1u]
2 0 0 π/2 θ2 [θ2l, θ2u]
3 0 dSE - π/2 θ3 [θ3l, θ3u]
4 0 0 π/2 θ4 [θ4l, θ4u]
5 0 dEW - π/2 θ5 [θ5l, θ5u]
6 0 0 π/2 θ6 [θ6l, θ6u]
7 0 dWF 0 θ7 [θ7l, θ7u]

1.2 机械臂逆运动学

对于S-R-S构型的7自由度机械臂,可以定义臂角ψ求其逆运动学的封闭解,如图 2所示;当机械臂末端位姿给定后,即可确定腕心W和肩心S的位置,从而肘心E的位置可在一个圆上变化,记由腕心W、肩心S和肘心E确定的平面为臂角平面;设当θ3为0时,此时肘心位置为E′,由腕心W、肩心S和肘心E′确定的平面为参考平面,臂角平面与参考平面的夹角即为臂角ψ。臂角ψ的取值范围为(-π, π]。

图 2 臂角的定义

给定末端位姿,可以确定腕心W和肩心S的位置,从而得到‖dSW‖的值,由S-R-S机械臂的特殊几何构型,可知关节4角度θ4由ΔSEW唯一确定,由余弦定理可以推导出:

$ {\rm{cos}}{\theta _4} = \frac{{{{\left\| {{d_{SW}}} \right\|}^2} - d_{SE}^2 - d_{EW}^2}}{{2{d_{SE}}{d_{EW}}}}. $ (1)

可知θ4与臂角ψ无关,只与给定的末端位姿有关。引入臂角变量ψ,当θ3为0时,此时臂角变量ψ为0,肘心位置为E′,由腕心W、肩心S和肘心E′确定的平面为参考平面,对应的肘部位姿为参考位姿,记作0R4r,由末端位姿唯一确定。当臂角变量为ψ时,腕部位姿0R4可以认为是参考位姿0R4rSW轴旋转了ψ角度,即:

$ ^0{\mathit{\boldsymbol{R}}_4}{ = ^0}{\mathit{\boldsymbol{R}}_\psi }^0\mathit{\boldsymbol{R}}_4^{\rm{r}}. $ (2)

0Rψ表示绕SW轴旋转了ψ角度的旋转矩阵,记SW轴的单位向量为0uSW,其生成的反对称矩阵为[0uSW×],则0Rψ具体表达式为

$ ^0{\mathit{\boldsymbol{R}}_\psi } = {\mathit{\boldsymbol{I}}_3} + {\rm{sin}}\psi {[^0}{\mathit{\boldsymbol{u}}_{SW}} \times ] + (1 - {\rm{cos}}\psi ){{[^0}{\mathit{\boldsymbol{u}}_{SW}} \times ]^2}. $ (3)

由于关节4角度θ4与臂角ψ无关,故式(2)可以简化为

$ ^0{\mathit{\boldsymbol{R}}_3}{ = ^0}{\mathit{\boldsymbol{R}}_\psi }^0\mathit{\boldsymbol{R}}_3^{\rm{r}}. $ (4)

将式(3)代入式(4),可推导旋转矩阵0R3的表达式为

$ ^0{\mathit{\boldsymbol{R}}_3} = {\mathit{\boldsymbol{A}}_{\rm{s}}}{\rm{sin}}\psi + {\mathit{\boldsymbol{B}}_{\rm{s}}}{\rm{cos}}\psi + {\mathit{\boldsymbol{C}}_{\rm{s}}}. $ (5)

矩阵AsBsCs具体表达式可见文[19]。其中,矩阵AsBsCs仅与末端位姿和θ4有关。当给定末端位姿和臂角ψ,由式(5)即可求得AsBsCs0R3,结合正运动学,可以推导肩部3个关节θ1θ2θ3的表达式:

$ {\rm{cos}}{\theta _2} = - {a_{{\rm{s32}}}}{\rm{sin}}\psi - {b_{{\rm{s32}}}}{\rm{cos}}\psi - {c_{{\rm{s32}}}}. $ (6)
$ \begin{array}{*{20}{l}} {{\theta _1} = a{\rm{tan}}2\{ - {\rm{sign }}({\rm{sin}}{\theta _2})( - {a_{{\rm{s22}}}}{\rm{sin}}\psi - {b_{{\rm{s22}}}}{\rm{cos}}\psi - {c_{{\rm{s22}}}}),}\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - {\rm{sign}} ({\rm{sin}}{\theta _2})( - {a_{{\rm{s12}}}}{\rm{sin}}\psi - {b_{{\rm{s12}}}}{\rm{cos}}\psi - {c_{{\rm{s12}}}})\} .} \end{array} $ (7)
$ \begin{array}{*{20}{l}} {{\theta _3} = a{\rm{tan}}2\{ - {\rm{sign}} {\theta _2}({a_{{\rm{s33}}}}{\rm{sin}}\psi + {b_{{\rm{s33}}}}{\rm{cos}}\psi + {c_{{\rm{s33}}}}),}\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - {\rm{sign}}{\theta _2}( - {a_{{\rm{s31}}}}{\rm{sin}}\psi - {b_{{\rm{s31}}}}{\rm{cos}}\psi - {c_{{\rm{s31}}}})\} .} \end{array} $ (8)

其中,asijbsijcsij分别表示矩阵AsBsCs的第(ij)个元素。在式(1)和式(5)的基础上,可推导旋转矩阵4R7的表达式为

$ ^4{\mathit{\boldsymbol{R}}_7} = {\mathit{\boldsymbol{A}}_{\rm{w}}}{\rm{sin}}\psi + {\mathit{\boldsymbol{B}}_{\rm{w}}}{\rm{cos}}\psi + {\mathit{\boldsymbol{C}}_{\rm{w}}}. $ (9)

其中,矩阵AwBwCw与末端位姿、θ4AsBsCs有关,其具体表达式见文[19],结合正运动学,可以得到腕部关节θ5θ6θ7的表达式:

$ {\rm{cos}}{\theta _6} = - {a_{{\rm{w32}}}}{\rm{sin}}\psi - {b_{{\rm{w32}}}}{\rm{cos}}\psi - {c_{{\rm{w32}}}}. $ (10)
$ \begin{array}{*{20}{l}} {{\theta _5} = a{\rm{tan}}2\{ - {\rm{sign}} {\theta _6}({a_{{\rm{w23}}}}{\rm{sin}}\psi + {b_{{\rm{w23}}}}{\rm{cos}}\psi + {c_{{\rm{w23}}}}),}\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - {\rm{sign}} {\theta _6}({a_{{\rm{w13}}}}{\rm{sin}}\psi + {b_{{\rm{w13}}}}{\rm{cos}}\psi + {c_{{\rm{w13}}}})\} .} \end{array} $ (11)
$ \begin{array}{*{20}{l}} {{\theta _7} = a{\rm{tan}}2\{ - {\rm{sign}} {\theta _6}({a_{{\rm{w32}}}}{\rm{sin}}\psi + {b_{{\rm{w32}}}}{\rm{cos}}\psi + {c_{{\rm{w32}}}}),}\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} - {\rm{sign}} {\theta _6}( - {a_{{\rm{w31}}}}{\rm{sin}}\psi - {b_{{\rm{w31}}}}{\rm{cos}}\psi - {c_{{\rm{w31}}}})\} .} \end{array} $ (12)

其中,awijbwijcwij分别表示矩阵AwBwCw的第(ij)个元素。由此即可求得7自由度的机械臂封闭逆运动学解。

2 臂角可行范围求解

机械臂实际运动过程中,会受到关节限位约束,即每个关节可运动的范围是有限的,由于臂角ψ和关节角度存在映射关系,因此臂角ψ也会存在一个可行范围,将关节i限位对应的臂角可行范围记作ψfi。对于一个给定的末端位姿,由式(1)、式(6)—(8)以及式(10)—(12)可知,除了关节4角度θ4与臂角ψ无关外,其他关节角度与臂角成余弦或者正切关系。下面分别分析这两种函数关系下关节限位与臂角可行范围之间的映射关系[4]

2.1 余弦函数形式

由式(6)和(10)可知,θ2θ6和臂角ψ之间的余弦函数关系式可统一写为

$ {\rm{cos}}{\theta _i} = a{\rm{sin}}\psi + b{\rm{cos}}\psi + c. $ (13)

a2+b2=0时,此时ab均为0,θi不随臂角ψ变化,θi的可行范围不会影响臂角ψ的可行范围。

a2+b2>0时,将式(10)进行整理,可得到

$ \frac{{{\rm{cos}}{\theta _i} - c}}{{\sqrt {{a^2} + {b^2}} }} = {\rm{sin}}(\psi + {\psi _0}). $ (14)

其中,ψ0=atan2(b, a),将等式左边看作一个整体,记作变量Ci,右边函数式记作h(ψ),由于θi的范围为[θil, θiu],易求得Ci的取值范围为[Cmin, Cmax];画出式(14)等号右边图像,如图 3所示。

图 3 函数Ci=h(ψ)图

根据图 3,对CminCmax进行分类讨论:

1) 当Cmin>0或者Cmax < 1时,臂角ψ的可行范围为空集;

2) 当Cmin < 1且Cmax>1时,臂角ψ的可行范围为(-π, π];

3) 当-1≤Cmin≤1且Cmax>1时,求解臂角ψ的可行范围需要解方程Cmin=h(ψ)并且比较Cminh(π)的大小;

4) 当-1≤Cmin≤1且-1≤Cmax≤1时,求解臂角ψ的可行范围需要解方程Cmin=h(ψ)以及Cmax=h(ψ)并比较CmaxCminh(π)的大小;

5) 当Cmin < 1且-1≤Cmax≤1时,求解臂角ψ的可行范围需要解方程Cmax=h(ψ)并且比较Cmaxh(π)的大小。

综上所述,可以求得余弦函数形式下关节i限位对应的臂角可行范围ψfi

2.2 正切函数形式

由式(7)—(8)、式(10)—(11)可知,θ1θ3θ5θ7和臂角ψ之间的正切函数关系式可统一写为

$ \begin{array}{*{20}{c}} {{\theta _i} = a{\rm{tan}}2({a_n}{\rm{sin}}\psi + {b_n}{\rm{sin}}\psi + {c_n},}\\ {{a_d}{\rm{sin}}\psi + {b_d}{\rm{sin}}\psi + {c_d}).} \end{array} $ (15)

为方便表述,在此记u=ansinψ+bnsinψ+cnv=adsinψ+bdsinψ+cd,将式(15)两边求导,得到

$ \frac{{{\rm{d}}{\theta _i}}}{{{\rm{d}}\psi }} = \frac{{{a_t}{\rm{sin}}\psi + {b_t}{\rm{sin}}\psi + {c_t}}}{{{u^2} + {v^2}}}. $ (16)

其中:

$ \begin{array}{*{20}{l}} {{a_t} = {c_n}{b_d} - {b_n}{c_d},}\\ {{b_t} = {a_n}{c_d} - {c_n}{a_d},}\\ {{c_t} = {a_n}{b_d} - {b_n}{a_d}.} \end{array} $

设关节i由于关节限位最大值为θi, max,最小值为θi, min,需要求其对应的臂角ψ范围。

at2+bt2ct2≤0时,式(16)满足非负或者非正,即关节角度θi随臂角ψ单调变化。此时分为2种情况:第1种情况为随着臂角ψ的变化,关节角度θi始终位于(-π, π]范围内,如图 4a所示;第2种情况为当关节位置位于-π或者π附近时,随着臂角ψ的连续变化,关节位置超出了(-π, π]的范围,此时关节位置会存在一个2π大小的突变,如从-π突变为π,如图 4b所示。

图 4 at2+bt2ct2≤0时关节角度随臂角变化图

at2+bt2ct2>0时,式(16)存在2个零点,其表达式为

$ {\psi _s} = 2{\rm{arctan}}\left( {\frac{{{a_t} \pm \sqrt {a_t^2 + b_t^2 - c_t^2} }}{{{b_t} - {c_t}}}} \right). $ (17)

此时同样分为2种情况:第1种情况为随着臂角ψ的变化,关节角度θi始终位于(-π, π]范围内,如图 5a所示;第2种情况为随着臂角ψ的连续变化,关节位置超出了(-π, π]的范围,关节位置存在一个2π大小的突变,如图 5b所示。

图 5 at2+bt2ct2>0时关节角度随着臂角的变化

仿照余弦函数形式的分类讨论方法,可以求得正切函数形式下关节i限位对应的臂角可行范围ψfi

3 考虑关节限位的运动学优化

机械臂实际运动过程中,会受到关节限位约束,为保证机械臂运动的安全性,机械臂运行过程中关节角度应该在关节限位内且远离关节限位[θilθiu]。

3.1 考虑关节限位的目标函数

对于一条连续的机械臂作业轨迹,通过轨迹规划将其离散为包含时间信息的末端位姿序列,对于t时刻的末端位姿Tt,对应的机械臂关节i的实际位置为θi, t,臂角为ψt,对于初始位置,可通过三角函数知识反解式(6)和式(10)求出对应的臂角;给定t+1时刻的末端位姿Tt+1,需要选取臂角ψt+1,求解关节i的位置θi, t+1,使得θi, t+1在关节限位范围内尽可能远离关节限位。实际上,如果θi, t离关节限位θiuθil较远,则基本不需要考虑关节限位问题;如果θi, t离关节限位θiuθil较近,则需要通过合理选取臂角ψt+1来求解θi, t+1,使θi, t+1尽可能避免靠近关节限位θiuθil,保证机械臂安全。

由于每个关节的限位值不同,为统一衡量各关节距离其限位的大小,对关节实际位置θi, t按下式进行归一化处理:

$ {x_{i,t}} = \frac{2}{{\theta _i^{\rm{u}} - \theta _i^{\rm{l}}}}\left( {{\theta _{i,t}} - \frac{{\theta _i^{\rm{u}} + \theta _i^{\rm{l}}}}{2}} \right). $ (18)

其中,xi, t是对应θi, t的归一化结果。显然,θiu映射为1,θil映射为-1;xi, t的取值范围为[-1, 1];xi, t越靠近1或者-1,表示越接近关节限位。

从避限位的角度来说,各关节需离限位越远越好。为此,本文提出如下优化目标函数:

$ f({\psi _{t + 1}}) = \sum\limits_{i = 1}^7 {{w_i}} ({x_{i,t}}){[{\theta _{i,t + 1}}({\psi _{i + 1}}) - {\theta _{i,t}}]^2}. $ (19)

其中,wi(xi, t)表示关节i的权重,为xi, t的函数。通过最小化该目标函数,以实现机械臂的关节避限位功能。其中,权重函数应具有如下特点:

1) 当距离关节限位较远时,wi较小;当距离关节限位较近时,wi较大;当达到关节限位时,wi为无穷大。

2) 当与关节限位距离较远时,wi增长较为缓慢;当与关节限位距离较近时,wi急剧增大,直至无穷大。

3) 当所有关节均离关节限位较远时,各关节权重大小应基本相同,这样能使机械臂关节运动总体最少,从而减小机械臂运行能量;而当某一个关节距离关节限位较近时,该关节权重应远大于其他关节,目标函数会重点优化该关节的关节位置,使其与上一时刻的关节位置尽可能接近,即减缓该关节靠近关节限位的速度,以达到避限位的目的。

根据上述分析权重函数应有的性质,构造如下形式的权重函数:

$ w(x) = \left\{ {\begin{array}{*{20}{l}} {\frac{{bx}}{{{{\rm{e}}^{a(1 - x)}} - 1}},}&{0 \le x < 1;}\\ {\frac{{ - bx}}{{{{\rm{e}}^{a(1 + x)}} - 1}},}&{ - 1 < x < 0;} \end{array}\quad a > 0,b > 0.} \right. $ (20)

权重函数w(x)为偶函数,故只对其0≤x < 1区间函数进行分析。当0≤x < 1,a>0,b>0时:

$ {w(x) = \frac{{bx}}{{{{\rm{e}}^{a(1 - x)}} - 1}},} $ (21)
$ {{w^\prime }(x) = \frac{{b({{\rm{e}}^{a(1 - x)}} - 1) + abx{{\rm{e}}^{a(1 - x)}}}}{{{{({{\rm{e}}^{a(1 - x)}} - 1)}^2}}},} $ (22)
$ \begin{array}{*{20}{c}} {{w^{\prime \prime }}(x) = }\\ {\frac{{{{\rm{e}}^{a(1 - x)}}[2ab({{\rm{e}}^{a(1 - x)}} - 1) + {a^2}bx{{\rm{e}}^{a(1 - x)}} + {a^2}bx]}}{{{{({{\rm{e}}^{a(1 - x)}} - 1)}^3}}}.} \end{array} $ (23)

进一步分析可得:

$ {w(0) = 0,w(1) = + \infty ,w(x) > 0;} $ (24)
$ {{w^\prime }(x) > 0,{w^\prime }(1) = + \infty ;} $ (25)
$ {{w^{\prime \prime }}(x) > 0.} $ (26)

由于w′(x)>0,因此w(x)单调递增,由0增加到无穷大;由于w″(x)>0,因此w(x)单调递增的速度也是递增的,一直递增到无穷大,满足前文所述权重函数应有的性质。

权重函数中有可以调节的参数ab,需要根据实际情况选取合适的ab。基本原则为选取的ab能够使得在关节安全区域内取得相对较小的权重值和较小的权重增长速度,在关节靠近极限位置时能够取得较大的权重值和较大的权重增长速度。本文提出一种参数ab的参考选取方法如下所示。

将关节位置θi归一化后,得到其对应的归一化结果xi,可以设定当-0.5 < xi < 0.5时,关节位于安全区域;当xi>0.8或xi < -0.8时,关节位于危险区域。由于无论参数ab取何值,当xi=0时,权重值为0;当xi=±1时,权重值为正无穷大,且权重单调递增,权重单调递增的速度也是递增的;故选定xi=0.5处的权重值可以大致控制xi在[0,0.5]区间内的权重增长速度,选定xi=0.8处的权重值可以大致控制xi在区间[0.5,0.8]和区间[0.8,1]内的增长速度。由于权重函数为偶函数,通过选定xi=0.5和xi=0.8处的权重值,求得其对应的参数ab,可以控制整个权重函数的变化趋势。

本文结合一个实例阐述上述参数ab的选取方法。设某一个关节的限位是[-120°, 120°],则关节位置在[-60°, 60°]范围内为安全区域,在[-96°, 96°]范围内为危险区域。将关节位置进行标准化后,当x=0.5时,取较小权重值w(x)=0.5;当x=0.8时,取较大权重值w(x)=3,解如下方程:

$ \left\{ {\begin{array}{*{20}{l}} {\frac{{0.5b}}{{{{\rm{e}}^{a(1 - 0.5)}} - 1}} = 0.5,}\\ {\frac{{0.8b}}{{{{\rm{e}}^{a(1 - 0.8)}} - 1}} = 3.} \end{array}} \right. $ (27)

利用MATLAB数值方程求解工具,可以求得a=2.38,b=2.28,即确定了参数ab,对应的权重函数w(x)的图像如图 6所示。显然,权重函数满足前述3个特点。

图 6 权重函数(a=2.38,b=2.28)

3.2 目标函数的简化

由式(6)—(8)和式(10)—(12)可知,臂角与关节位置的关系十分复杂,使得目标函数的表达式十分复杂,无法通过解析方法进行求解,因此需要对优化函数进行简化。

机械臂在作直线或圆弧运动时,每次插补位姿变化很小,各个关节的插补位移也很小[20],且由于机械臂的关节速度限制,轨迹上相邻两点逆运动学求解得到的关节角度不会相差较大,由于臂角与关节位置的关系式可导,故相邻2次求解的臂角也不会相差较大,因此,可以将臂角与关节位置的关系式在当前关节位置处进行线性展开,在一个较小的臂角范围认为臂角与关节位置的关系式是线性的,从而简化目标函数。由于实际臂角与关节位置的关系十分复杂,求导过程较为复杂,因此采用差分法完成线性化过程[20]。线性化步骤如下。

1) 采用t时刻的臂角ψtt+1时刻的末端位姿Tt+1,可计算得到一组关节位置θi, t+10

2) 在ψt的基础上加上一个小量Δψt,得到新的臂角ψt1=ψtψt,通过ψt1Tt+1可计算得到新的一组关节位置θi, t+11

3) 则θi, t+1ψt+1的线性表达式为

$ {\theta _{i,t + 1}} = \theta _{i,t + 1}^0 + \frac{{\theta _{i,t + 1}^1 - \theta _{i,t + 1}^0}}{{\Delta {\psi _t}}}({\psi _{i + 1}} - {\psi _t}). $ (28)

此时,θi, t+1ψt+1呈线性关系,线性系数为$\frac{\theta_{i, t+1}^{1}-\theta_{i, t+1}^{0}}{\Delta \psi_{t}}$,为方便表示,将$\frac{\theta_{i, t+1}^{1}-\theta_{i, t+1}^{0}}{\Delta \psi_{t}}$记作ki,则式(28)可写作:

$ {\theta _{i,t + 1}} = \theta _{i,t + 1}^0 + {k_i}({\psi _{t + 1}} - {\psi _t}). $ (29)

将式(29)代入到目标函数中,此时目标函数的表达式为

$ \begin{array}{*{20}{c}} {f({\psi _{i + 1}}) = }\\ {\sum\limits_{i = 1}^7 {{w_i}} ({x_{i,t}}){{[\theta _{i,t + 1}^0 - {\theta _{i,t}} + {k_i}({\psi _{i + 1}} - {\psi _t})]}^2}.} \end{array} $ (30)

其中,θi, tθi, t+10kiψtwi(xi, t)均为已知,且易知$\sum\limits_{i=1}^{7} w_{i}\left(x_{i, t}\right) k_{i}^{2}>0, f\left(\psi_{i+1}\right)$是关于ψt+1的二次函数。

3.3 目标函数的优化

在进行优化之前,需要确定优化区间。关节限位和臂角可行范围存在映射关系,给定末端位姿Tt+1,当关节位置θi, t距离关节限位较近时,此时该关节较为危险,对应的臂角ψi, t也会距离其可行区域的边界较近;反之,当关节位置θi, t距离关节限位较远时,此时该关节较为安全,对应的臂角ψi, t也会距离其可行区域的边界较远。考虑到对于一条连续轨迹,相邻2次逆运动学求解对应的关节角度之间以及臂角之间相差较小,故只需求解危险关节的关节限位对应的臂角可行范围,并在此范围内选取ψt+1,即可保证该关节的安全,从而保证机械臂的安全,如果各个关节均不在危险区域,则可以不计算优化区间。危险关节可采用xi, t的绝对值和1之间的距离定义。这样,只需分析少数的危险关节,通常为0或者1个,而不是所有关节的关节限位对应的臂角可行范围,能够减小应用难度,提高计算效率。

t+1时刻,危险关节的数目为n,为便于表述,对危险关节进行编号,分别为m1, m2,…,mn。对于mi关节,其关节限位对应的臂角可行范围为ψi

$ {\psi _i} = \bigcup\limits_{j = 1}^{{n_j}} {{\psi _{i,j}}} . $ (31)

其中,ψi, j为子集合,nj为子集合的个数。考虑所有的危险关节,得到最终的臂角可行范围为

$ {\psi _{f,t + 1}} = \bigcap\limits_{i = 1}^n {{\psi _i}} . $ (32)

由式(30)可知,目标函数是关于ψt+1的二次函数,需要确定合适的ψt+1使目标函数取得最小值,将目标函数进行系数整合,可以得到

$ f({\psi _{t + 1}}) = A\psi _{t + 1}^2 + B{\psi _{t + 1}} + C. $ (33)

其中,

$ \begin{array}{*{20}{c}} {A = \sum\limits_{i = 1}^7 {{w_i}} ({x_{i,t}})k_i^2,}\\ {B = \sum\limits_{i = 1}^7 2 {w_i}({x_{i,t}}){k_i}(\theta _{i,t + 1}^0 - {\theta _{i,t}} - {k_i}{\psi _t}),}\\ {C = \sum\limits_{i = 1}^7 {{w_i}} ({x_{i,t}}){{(\theta _{i,t + 1}^0 - {\theta _{i,t}} - {k_i}{\psi _t})}^2}.} \end{array} $

显然,由于A>0,由二次函数的相关性质可知,f(ψt+1)具有全局最小值,最小值在ψt+1opt=-B/2A处取得,且距离ψt+1opt越近,函数值越小。

ψt+1opt在臂角可行范围ψf, t+1内时,ψt+1opt即是最终的优化臂角ψt+1final。但实际上,当关节极为接近关节限位时,ψt+1opt可能在臂角可行范围ψf, t+1外,此时由于二次函数的性质,臂角可行范围内距离ψt+1opt最近的臂角值即为最终优化后的臂角ψt+1final,如图 7所示。

图 7 当优化臂角ψt+1opt不在可行范围内的情况

ψt+1final和Tt+1代入式(6)—(8)和式(10)—(12),可以求得优化后的θi, t+1final。循环上述过程,可以完成轨迹上所有末端位姿点的求解。

4 实验计算 4.1 实验条件

本文实验计算的对象为实验室自主研制的机械臂,如图 8所示。其基本结构参数dBSdSEdEWdWF分别为33、420、380和160 mm,奇数关节的运动范围为[-150°,150°],偶数关节的运动范围为[-120°,120°]。机械臂控制系统上位机处理器为Intel® CoreTM i7-4770 CPU @ 3.40 GHz,内存为8 GB,采用EtherCAT总线通信,控制周期为1 ms。

图 8 7自由度机械臂

本文采用圆弧轨迹对算法进行计算以验证算法的有效性,圆弧轨迹起始位姿为T0, arc,终止位姿为T1, arc,圆弧轨迹经过位置坐标为[750 mm,0,450 mm]的点,采取梯形速度插补方法对圆弧轨迹进行位置和姿态插补。

$ {\mathit{\boldsymbol{T}}_{{\rm{0,arc}}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&{700}\\ 0&{0.809{\kern 1pt} {\kern 1pt} 0}&{0.587{\kern 1pt} {\kern 1pt} {\kern 1pt} 8}&{ - 200}\\ 0&{ - 0.587{\kern 1pt} {\kern 1pt} 8}&{0.809{\kern 1pt} {\kern 1pt} 0}&{400}\\ 0&0&0&1 \end{array}} \right], $ (34)
$ {\mathit{\boldsymbol{T}}_{{\rm{1,arc}}}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&{700}\\ 0&{0.809{\kern 1pt} {\kern 1pt} 0}&{ - 0.587{\kern 1pt} {\kern 1pt} 8}&{200}\\ 0&{0.587{\kern 1pt} {\kern 1pt} 8}&{0.809{\kern 1pt} {\kern 1pt} 0}&{400}\\ 0&0&0&1 \end{array}} \right]. $ (35)

算法中各关节采用的权重函数均为w(x),其中参数ab均取2,线性化中取的小量Δψt为0.001 rad,初始点对应的臂角为1 rad。

4.2 实验结果

通过实验计算,不同算法对应的圆弧轨迹逆运动学计算结果如图 9所示,其中,本文优化算法得到的圆弧轨迹的逆运动学结果如图 9a所示,为了证明算法的可行性,对于相同的轨迹以及初始臂角,固定初始臂角进行逆运动学计算得到不考虑关节限位的非优化逆运动学结果,如图 9b所示。为了证明算法的优越性,采用文[3]中的算法进行对比,文中的参数rsrw分别取值为0.4和0.6,该算法计算的逆运动学结果如图 9c所示。各算法对应的轨迹最终关节位置如表 2所示;采用C++编程计算,本文优化算法计算单个点的计算时间小于0.1 ms,可以满足实时性要求。

图 9 不同算法对应的圆弧轨迹逆运动学计算结果

表 2 不同算法计算的圆弧轨迹最终关节位置表
关节 圆弧轨迹最终关节位置/(°)
非优化算法 本文算法 文[3]算法
1 8.69 6.27 26.16
2 85.95 98.68 78.63
3 57.17 101.31 -11.72
4 35.39 35.39 35.39
5 -26.49 -75.30 52.30
6 -124.31 -113.16 -119.59
7 -24.39 -34.68 -0.47

4.3 计算结果分析

分析计算结果可知,本文提出的考虑关节限位的逆运动学算法能够保证机械臂运动的连续性,这对算法的实用性十分重要[21]。可以发现,对于圆弧轨迹,6轴相对比较危险,其他轴均离关节极限较远。不采用考虑关节限位的逆运动学方法,6轴最终会达到-124.31°,已经超过关节极限,在实际情况中认为不可运行; 文[3]算法最终6轴达到-119.59°,尚在关节限位范围内,但已经和关节限位十分接近; 而采用本文算法后,6轴最终只会达到-113.16°,距离关节限位还有一些空间,机械臂可以正常运行,同时其他关节距离关节极限较远。因此,可以证明本文提出的考虑关节限位的逆运动学算法在避关节限位问题上是有效的且相对优越的。由于采用本文算法计算单个点的时间约为0.1 ms,远小于机械臂的控制周期,因此可以进行实时计算。

本文提出的算法可以有效解决限位问题。实际上,对本文算法稍作修改,还可以解决其他问题,如在关节奇异位置处加上较大的权重,即可以实现机械臂避奇异。同时,权重函数的选取也不唯一,可以根据实际情况进行调整,如考虑到各关节电机功率不同,希望功率大的关节运动幅度尽可能小,可以在现有权重函数的基础上对各关节再加上一个表征电机功率大小的权重。

5 结论

为了解决机械臂避限位问题,本文以S-R-S构型的7自由度机械臂为对象,提出了一种考虑关节限位的逆运动学优化方法和算法。

对于S-R-S构型的机械臂,通过引入臂角变量,可采用几何法得到其逆运动学封闭求解形式,给定末端位姿和臂角大小,即可解析求得对应的各关节的位置。可以采用解析计算方法通过合理选取臂角实现关节位置的优化,保证关节靠近关节限位时,能减小靠近速度,直至关节最终静止。

由于关节位置和臂角存在映射关系,因此关节限位会对应臂角的可行范围,对于余弦映射关系和正切映射关系,可结合映射关系图像对关节限位进行分类讨论求得臂角可行范围,只要臂角的选取在臂角可行范围内,即可以保证关节角度位于关节限位内。

为实现避限位,本文算法提出了一种带权重函数的避限位优化目标函数;权重函数的设计保证关节靠近限位时取较大的值,关节到达限位时可达到正无穷大。为了简化优化计算过程,本文采用线性展开方法简化各关节位置与臂角的关系式,从而将原本复杂形式的优化目标函数简化为简单的二次函数,极大地提高了优化计算效率,结合求得的臂角可行范围,可以快速获得优化的臂角及各关节位置,实现关节避限位。

计算结果表明,本文提出的7自由度逆运动学优化算法可以有效实现关节避限位;算法直观、简单,并可实现实时计算,这对机械臂在线路径规划与解算具有重要意义。算法还具有较好的通用性和拓展性,通过合理选取权重函数及其表达形式,可以在逆运动学解算时考虑各关节驱动电机功率、机械臂避奇异等其他问题,从而使机械臂获得更佳的运动性能。

参考文献
[1]
YAN L, MU Z G, XU W F. Analytical inverse kinematics of a class of redundant manipulator based on dual arm-angle parameterization[C]//2014 IEEE International Conference on Systems, Man, and Cybernetics. San Diego, USA: IEEE, 2014: 3744-3749.
[2]
WAN J, WU H T, MA R, et al. A study on avoiding joint limits for inverse kinematics of redundant manipulators using improved clamping weighted least-norm method[J]. Journal of Mechanical Science and Technology, 2018, 32(3): 1367-1378. DOI:10.1007/s12206-018-0240-7
[3]
SHIMIZU M, YOON W K, KITAGAKI K. A practical redundancy resolution for 7-DoF redundant manipulators with joint limits[C]//Proceedings of the 2007 IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2007: 4510-4516.
[4]
FARIA C, FERREIRA F, ERLHAGEN W, et al. Position-based kinematics for 7-DoF serial manipulators with global configuration control, joint limit and singularity avoidance[J]. Mechanism and Machine Theory, 2018, 121: 317-334. DOI:10.1016/j.mechmachtheory.2017.10.025
[5]
WANG W R, GU J L, ZHU M C, et al. An obstacle avoidance method for redundant manipulators based on artificial potential field[C]//2018 IEEE International Conference on Mechatronics and Automation (ICMA). Changchun, China: IEEE, 2018: 2151-2156.
[6]
GONG M D, LI X D, ZHANG L. Analytical inverse kinematics and self-motion application for 7-DoF redundant manipulator[J]. IEEE Access, 2019, 7: 18662-18674. DOI:10.1109/ACCESS.2019.2895741
[7]
ZHAO L L, ZHAO J D, LIU H, et al. Efficient inverse kinematics for redundant manipulators with collision avoidance in dynamic scenes[C]//2018 IEEE International Conference on Robotics and Biomimetics (ROBIO). Kuala Lumpur, Malaysia: IEEE, 2018: 2502-2507.
[8]
KIM H, ROSEN J. Predicting redundancy of a 7-DoF upper limb exoskeleton toward improved transparency between human and robot[J]. Journal of Intelligent & Robotic Systems, 2015, 80(1): 99-119.
[9]
段晋军, 甘亚辉, 戴先中, 等. 基于可操作度评价的冗余机器人逆解求解方法[J]. 华中科技大学学报(自然科学版), 2015, 43(S1): 45-48, 57.
DUAN J J, GAN Y H, DAI X Z, et al. Method of inverse kinematics solution for a redundant manipulator based on manipulability[J]. Journal of Huazhong University of Science and Technology (Nature Science Edition), 2015, 43(S1): 45-48, 57. (in Chinese)
[10]
ZHAO J, XU T, FANG Q Q, et al. A synthetic inverse kinematic algorithm for 7-DoF redundant manipulator[C]//2018 IEEE International Conference on Real-time Computing and Robotics (RCAR). Kandima, Maldives: IEEE, 2018: 112-117.
[11]
LIÉGEOIS A. Automatic supervisory control of the configuration and behavior of multibody mechanisms[J]. IEEE Transactions on Systems, Man, and Cybernetics, 1977, 7(12): 868-871. DOI:10.1109/TSMC.1977.4309644
[12]
WANG J G, LI Y M, ZHAO X H. Inverse kinematics and control of a 7-DoF redundant manipulator based on the closed-loop algorithm[J]. International Journal of Advanced Robotic Systems, 2010, 7(4): 1-9.
[13]
曹鹏飞, 甘亚辉, 戴先中, 等. 物理受限冗余机械臂逆运动学凸优化求解[J]. 机器人, 2016, 38(3): 257-264.
CAO P F, GAN Y H, DAI X Z, et al. Convex optimization solution for inverse kinematics of a physically constrained redundant manipulator[J]. Robot, 2016, 38(3): 257-264. (in Chinese)
[14]
SINGH G K, CLAASSENS J. An analytical solution for the inverse kinematics of a redundant 7-DoF manipulator with link offsets[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei, China: IEEE, 2010: 2976-2982.
[15]
陈鹏, 刘璐, 余飞, 等. 一种仿人机械臂的运动学逆解的几何求解方法[J]. 机器人, 2012, 34(2): 211-216.
CHEN P, LIU L, YU F, et al. A geometrical method for inverse kinematics of a kind of humanoid manipulator[J]. Robot, 2012, 34(2): 211-216. (in Chinese)
[16]
CHEN B C, CAO G Z, LI W B, et al. An analytical solution of inverse kinematics for a 7-DoF redundant manipulator[C]//2018 15th International Conference on Ubiquitous Robots (UR). Honolulu, HI, USA: IEEE, 2018: 523-527.
[17]
SINHA A, CHAKRABORTY N. Geometric search-based inverse kinematics of 7-DoF redundant manipulator with multiple joint offsets[C]//2019 International Conference on Robotics and Automation (ICRA). Montreal, Canada: IEEE, 2019: 5592-5598.
[18]
LI S P, WANG Z J, ZHANG Q, et al. Solving inverse kinematics model for 7-DoF robot arms based on space vector[C]//2018 International Conference on Control and Robots (ICCR). Hong Kong, China: IEEE, 2018: 1-5.
[19]
霍希建, 刘伊威, 姜力, 等. 具有关节限位的7R仿人机械臂逆运动学优化[J]. 吉林大学学报(工学版), 2016, 46(1): 213-220.
HUO X J, LIU Y W, JIANG L, et al. Inverse kinematic optimization of 7R humanoid arm with joint limits[J]. Journal of Jilin University (Engineering and Technology Edition), 2016, 46(1): 213-220. (in Chinese)
[20]
徐俊虎, 栾楠, 张诗雷, 等. 7自由度机械臂的运动学逆解与优化[J]. 机电一体化, 2011, 17(6): 28-33.
XU J H, LUAN N, ZHANG S L, et al. Inverse kinematics solution and optimization of a 7-DoF robot[J]. Mechatronics, 2011, 17(6): 28-33. (in Chinese)
[21]
KUHLEMANN I, SCHWEIKARD A, JAUER P, et al. Robust inverse kinematics by configuration control for redundant manipulators with seven DoF[C]//20162nd International Conference on Control, Automation and Robotics (ICCAR). Hong Kong, China: IEEE, 2016: 49: 55.