CN113997288A - 一种求解非球腕6r工业机器人逆运动学的数值算法 - Google Patents

Abstract

本发明提供一种非球腕6R工业机器人逆运动学求取方法。包括以下步骤：S1、首先采用改进的断开‑重连方法和DIXON合成法，推导出具有连续性且只包含θ₆的非线性方程，以及其它关节变量的逆解公式；S2、然后证明相邻位姿点的逆解在相同唯一域中距离最小，并根据唯一域的判断，给出了逆解公式中正负符号确定的方法；S3、最后利用黄金分割法对非线性方程搜索求解；S4、仿真结果表明，算法无需得到非球腕6R机器人的所有逆解，也可保证各关节的位移最小；S5、算法平均求解时间约为12us，平均位姿误差范数约为7.07×10^‑12。本发明相比于传统方法具有更高的效率，推导过程更加简便。

Description

一种求解非球腕6R工业机器人逆运动学的数值算法

技术领域

本发明涉及一种逆解数值算法，更具体地，涉及一种非球腕6R工业机器人逆运动学的求解方法。

背景技术

机器人逆运动学求解作为机器人离线编程、轨迹规划、控制算法设计等其他课题研究的基础，一直是机器人学中的一个经典问题，同样也是研究热点。逆运动学求解的实质是完成机器人工作空间到关节空间的映射，逆运动学方程组具有高维、非线性的特点，求解复杂且不易求出。当机器人的结构满足PIEPER准则，即最后三个关节为轴线交于一点的球形腕部设计时，可以得到解析解。

腕部偏置型6R机器人与球形腕部6R机器人相比，前者具有更高的负载能力，更远的水平抵达距离和灵活性，因而在焊接、喷涂和材料处理等工业中得到更广泛的应用。腕部偏置型的结构虽然提高了机器人运动学等方面的性能，但也导致该类机器人无法得到逆运动学解析解，并使得机器人的逆运动学非线性方程组变得更复杂，耦合度更高。此时可以利用一般6R机器人的位姿反解成果求腕部偏置型6R机器人的逆运动学解，这些方法主要利用关节的半角正切，将运动学方程转化为1元16次多项式进行求解，但是这些方法的公式推导过程十分繁琐耗时。

现有运动学解法虽然可以解决这类机器人的逆运动学问题，但是仍然缺乏一种通用方法，既具有几何直观意义推导出逆解公式，数值求解过程中又可以满足实时、高精度控制。

发明内容

本发明针对现有技术中逆运动学解法推导过程十分繁琐耗时，无法保证离线求解和实时求解问题；提供一种非球腕6R工业机器人逆运动学求解数值算法。

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

一种商用非球腕6R工业机器人逆运动学求取方法，包括以下步骤：

S1、首先采用改进的断开-重连方法(improved method of disconnection andre-connection，IMDR)和DIXON合成法，推导出具有连续性且只包含θ₆的非线性方程，以及其它关节变量的逆解公式；

机器人末端相对于基坐标系的正向运动学公式可由各连杆齐次变换矩阵相乘得到：

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆)

建立正运动学方程后，可将上式变形为：

T_L＝T₂(θ₂)T₃(θ₃)T₄(θ₄)Rot(-α₄,x₄)

上式表示将原机器人分解为两个子链，子链L的末端位姿为T_L，它由关节2的原点至关节4和关节5的轴线交点部分组成，是一个平面结构。其余部分组成子链R，其末端位姿为T_R，其关节1和关节6之间的原点由T表示的虚拟连杆连接。

在子链末端处，只要两子链的位姿满足以下条件即可重连：

p_L＝p_R

式P_L＝P_R实际为三个方程，当满足上述6个方程的6个关节变量都被求解出来时，两子链将在断开连接的位置处重新结合为原机器人，而这些得到的关节变量就是机器人的逆解。前两个式子用于求θ₁、θ₂、θ₃和θ₆，第三个式子可求得θ₄，第四个式子可求得θ₅。

S2、然后证明相邻位姿点的逆解在相同唯一域中距离最小，并根据唯一域的判断，给出了逆解公式中正负符号确定的方法：

首先由式l₄c₃-l₃s₃＝0(c₃表示cosθ₃，s₃表示sinθ₃)可直接得到θ₃的边界值：

从而

k∈Z。因此，θ₃的逆解值域与唯一域是一致的，k₂的值可由下式确定：

if l₄c₃-l₃s₃≤0

k₂＝1

else

k₂＝-1

k₁可由下式确定

if l₁+l₄c₂₃-l₃s₂₃-l₂s₂≥0

k₁＝1

else

k₁＝-1

由于机器人末端跟踪的是连续轨迹，k₁和k₂可由初始点代入上式中确定。

S3、最后利用黄金分割法对非线性方程搜索求解，得到θ₁、θ₂、θ₃、θ₄、θ₅的求解公式；

S4、仿真验证本方法的有效性。

进一步地，在步骤S1中相邻连杆间的齐次变换公式为：

其中:

θ_i——x_i-1轴和x_i轴之间关于z_i的夹角。

d_i——{i-1}坐标系原点沿着z_i-1轴到x_i轴的距离。

a_i——沿着x_i轴，z_i-1轴和z_i轴之间的距离。

α_i——z_i-1轴和z_i轴之间关于x_i的夹角。

进一步地，在步骤S1中采用Dixon合成法求解一元二次方程组：

假设一元二次方程组如下：

f(x)＝Ax²+Bx+C

g(x)＝Dx²+Ex+F

根据Dixon合成法的消元原则，可构造如下行列式：

上式可进一步写为：

其中α∈R是一个引入的新变量，U＝[α,1],V＝[x,1]^T，以及

|Σ|＝(AF-CD)²-(AE-BD)(BF-CE)

其中，|Σ|表示行列式，只要方程等于零，那么式f(x)和式g(x)中的x至少有一个共同的解。

进一步地，在步骤S2中：确定k₁可以保证θ₁的位移最小，确定k₂可以保证θ₃的位移最小，而初始点可以保证θ₆的位移最小。当θ₁、θ₃和θ₆都保证位移最小后，θ₂、θ₄和θ₅的最小位移将自然得到保证，因为它们的逆解由θ₁、θ₃和θ₆唯一得到。

进一步地，在步骤S3中θ₁、θ₂、θ₃、θ₄、θ₅的求解公式为：

θ₁＝atan2(k₁B₁,k₁A₁)；θ₂＝atan2(s₂,c₂)；

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄)；θ₅＝atan2(0,s_β)+atan2(B₅,A₅)。

本发明的有益效果为：相比于一般传统的6R机器人逆运动学求解的方法，本发明中的解法具有更明确的几何意义，推导过程更加便捷，满足实时、高精度及稳定性的要求。

附图说明

图1为手腕斜偏置工业机器人连杆坐标系；

图2为逆解算法流程图；

图3为各位姿对应的关节曲线；

图4为位姿误差范数；

图5为逆运动学求解时间；

图6为奇异位置附近的适应度函数曲线。

具体实施方式

下面结合具体实施方式对本发明作进一步的说明。

一种非球腕6R机器人逆运动学求取方法:

以喷涂机器人为研究对象。该机器人手腕存在斜偏置，在非球腕工业机器人中比较特殊，解决了它的逆运动学问题，其它手腕偏置型工业机器人的逆运动学问题都可得到解决。表1列举了该机器人的D-H参数，表中β＝60°。

表1工业机器人的D-H参数

相邻连杆间的齐次变换公式为：

其中:

θ_i——x_i-1轴和x_i轴之间关于z_i的夹角。

d_i——{i-1}坐标系原点沿着z_i-1轴到x_i轴的距离。

a_i——沿着x_i轴，z_i-1轴和z_i轴之间的距离。

α_i——z_i-1轴和z_i轴之间关于x_i的夹角。

机器人末端相对于基坐标系的正向运动学公式可由各连杆齐次变换矩阵相乘得到：

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆) (2)

建立正运动学方程后，可将式(2)变形为：

式(3)表示将原机器人分解为两个子链，子链L的末端位姿为T_L，它由关节2的原点至关节4和关节5的轴线交点部分组成，是一个平面结构。其余部分组成子链R，其末端位姿为T_R，其关节1和关节6之间的原点由T表示的虚拟连杆连接。

在子链末端处，只要两子链的位姿满足以下条件即可重连：

p_L＝p_R (4)

式(4)实际为三个方程，当满足上述6个方程的6个关节变量都被求解出来时，两子链将在断开连接的位置处重新结合为原机器人，而这些得到的关节变量就是机器人的逆解。式(4)-(5)用于求θ₁、θ₂、θ₃和θ₆，式(6)可求得θ₄，式(7)可求得θ₅。

令目标位姿为：

将各连杆参数的符号变量代入式(3)，并由式(5)可得，

c₂₃(-c₁A₂₃-s₁B₂₃)+s₂₃C₂₃-c_β＝0 (9)

式中

A₂₃＝o_xc₆s_β-a_xc_β+n_xs_βs₆

B₂₃＝o_yc₆s_β-a_yc_β+n_ys_βs₆

C₂₃＝a_zc_β-o_zc₆s_β-n_zs_βs₆

从现在开始s_i、c_i、s_i+j、c_i+j、s_β和c_β分别表示sinθ_i、cosθ_i、sin(θ_i+θ_j)、cos(θ_i+θ_j)、sinβ和cosβ。

由式(4)得，

l₄c₂₃-l₃s₂₃-l₂s₂＝s₁B₁+c₁A₁-l₁ (10)

l₃c₂₃+l₄s₂₃+l₂c₂＝C₁ (11)

0＝s₁A₁-c₁B₁ (12)

式中

A₁＝p_x-a_xl₆+l₅A₂₃

B₁＝p_y-a_yl₆+l₅B₂₃

C₁＝p_z-a_zl₆-l₅C₂₃

利用式(9-12)可得到只包含θ₆的非线性方程。首先，利用(12)以及三角函数恒等式 sin²θ+cos²θ＝1，得：

s₁＝k₁B₁/K (13)

c₁＝k₁A₁/K (14)

式中k₁＝±1，

k₁可由下式确定

if l₁+l₄c₂₃-l₃s₂₃-l₂s₂≥0

k₁＝1

else

k₁＝-1

然后，将式(10)和式(11)中包含(θ₂+θ₃)的项移至等式右边平方求和，并将式(13)和(14) 代入其中，得：

因s₁和c₁都以θ₆表示，因此式(9)和(15)此时都只包含变量(θ₂+θ₃)和θ₆。为了使得非线性方程连续，可将式(9)和式(15)看作关于s₂₃和c₂₃的一元二次方程组，由解向量法得：

式中A、B、C、D、E和F都只与θ₆有关，它们分别为：

A＝-2l₄C₁-2l₃(l₁-k₁K)

B＝-2l₃C₁+2l₄(l₁-k₁K)

C＝c₂₃K

D＝-k₁(A₁A₂₃+B₁B₂₃)

F＝c_βK

然后结合三角恒等式整理得：

f＝(DE-BF)²+(AF-CE)²-(AD-BC)² (17)

θ₆一旦得到解决，其它关节变量可采用以下方法得到。

由式(13)和(14)，得：

θ₁＝atan2(k₁B₁,k₁A₁) (18)

对式(10)和(11)平方求和，得：

式中，k₂＝±1，p₁表示式(10)中等号的右侧部分。

A₃＝2l₂l₃

B₃＝2l₂l₄

首先由式l₄c₃-l₃s₃＝0可直接得到θ₃的边界值：

从而

k∈Z。因此，θ₃的逆解值域与唯一域是一致的，k₂的值可由下式确定：

if l₄c₃-l₃s₃≤0

k₂＝1

else

k₂＝-1

得到θ₃后，式(10)和(11)则只包含θ₂一个变量，将其展开，以s₂和c₂分别合并同类项，视作二元一次方程组，解得：

θ₂＝atan2(s₂,c₂) (21)

式中

a＝-(l₂+l₃c₃+l₄s₃)

b＝l₄c₃-l₃s₃

求得θ₁、θ₂、θ₃和θ₆后，θ₄和θ₅的解可由式(6)和(7)分别得到。

由式(6)，得：

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄) (22)

式中

A₄＝B₂₃c₁-A₂₃s₁

B₄＝-C₂₃c₂₃-s₂₃(A₂₃c₁+B₂₃s₁)

由式(7)，得:

θ₅＝atan2(0,s_β)+atan2(B₅,A₅) (23)

式中

A₅＝c₂₃(c₁(n_xc₆-o_xs₆)+s₁(n_yc₆-o_ys₆))+s₂₃(n_zc₆-o_zs₆)

式(17)是由关于θ₆的三角函数所组成的一元非线性方程，在复数域内，它是可以因式分解的，由于难以得到它的求根公式，所以这种分解很难得到。假设它能因式分解，可将适应度函数设计为下述式子:

式中，K_fit1＝-1或1，K_fit2＝1或2，它们默认为1。

为检验本文所提算法的性能，在VS2017仿真环境下展开试验。试验的硬件环境为Intel(R)Core(TM)i7-9750H CPU@2.60GHz，运行内存为16GB，操作系统为Windows 10 家庭中文版。

实际运动中，机器人都是从零位开始运动到目标点，因此将Q₀＝[0,0,0,0,0,0]^T代入式 (2)得到的位姿设为初始位姿T₀。目标轨迹采用下式得到：

式中，P₀是初始位姿T₀中的位置，N为采样点总数，j是采样点的序数，j∈Z且j ∈[1,N]。目标轨迹的姿态均等于T₀中姿态。

逆解算法中，第一个位姿用于搜索的初始关节角为Q₀，随后每个位姿求解时的初始关节角均为上一个位姿点的逆解。采样点总数N＝10000，步长h＝π/180，搜索次数k初始为1，阈值E＝1×10^-12，ε＝10^-8。此次逆运动学求解，共得到以下数据：

(1)各位姿对应的关节曲线，如图3；

(2)逆解对应的位姿与目标位姿之间的二范数，如图4；

(3)每个位姿的逆运动学求解时间，如图5。

图4中最大位姿误差范数约为1.2509×10^-12mm，其平均值约为7.07×10^-12mm，标准差约为1.6776×10^-13mm。

图5为目标轨迹上各采样点的逆解时间，其中耗时最长的时间约为90.4μs，平均时间约为12μs。

另外，令Q＝[0,0,atan(l₄/l₃)+0.13,0,0,0]^T代入式(2)得到目标位姿T，然后将其与k₁＝1， K_fit1＝1和K_fit2＝1代入k₂确定公式得到该位姿处的适应度函数曲线，如图6。

关节角Q对应的非球腕机器人的雅可比矩阵行列式为6.5×10^-8，由此可判断该位置位于奇异位置附近。图6中，从局部放大图可以看出，θ₆在[-2.032,-2.026]间存在两个十分接近的根，而这两个根之间的距离小于设定的步长h。采用本文所提出的算法，得到该处逆解为:

[0.89755491,0.14028032,1.09887203,3.06847901,-2.99804717,-2.02615162]

对应的位姿误差为4.5475×10^-13，平均求解时间约为20.7μs。

从图3中可看出各个关节的位移是平滑连续的，表明逆解公式中系数k₁和k₂由唯一域确定，可以保证非球腕工业机器人末端轨迹跟踪时各个关节的位移最小，同时也验证了本文所推导的逆解公式中包含唯一域的划分。图4的位姿误差范数表明算法可以得到高精度的逆解，且求解稳定性高。图5中算法的平均求解时间达到12μs,该求解时间可与球腕工业机器人的时间比肩。另外，算法只启动了线性插值，表明所求轨迹上的位置点均远离奇异位置。

图6中的目标位姿是接近奇异位置的，正如之前所分析，该处的两个根十分接近，由于步长的原因，可能导致线性插值法无法启动，而启动黄金分割法。在启动黄金分割法后，适应度函数会被平方，这样搜索区间中实际是一个双峰寻极值问题。因为黄金分割法本身自带确定搜索区间的功能，所以在这个双峰极值中，算法必然会收敛于一处极值。综上，即使位姿处于或接近奇异位置，本文算法依然有效，且具有高精度的解。

本文的方法在公式推导过程中有更明确的几何意义。本文采用固定的初始迭代点，算法在固定运行次数内便可求出所有逆解；同时本文的方法具有更强的理论推广性，同时给出了其他关节角的具体求解方法；本文算法部分所使用的算法易于编程实现。

以上所述实例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。

Claims (5)

1.一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，包括以下步骤：

S1、首先采用改进的断开-重连方法和DIXON合成法，推导出具有连续性且只包含θ₆的非线性方程，以及其它关节变量的逆解公式；

机器人末端相对于基坐标系的正向运动学公式可由各连杆齐次变换矩阵相乘得到：

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆) (1)

建立正运动学方程后，可将上式变形为：

式(2)表示将原机器人分解为两个子链，子链L的末端位姿为T_L，它由关节2的原点至关节4和关节5的轴线交点部分组成，是一个平面结构；其余部分组成子链R，其末端位姿为T_R，其关节1和关节6之间的原点由T表示的虚拟连杆连接；在子链末端处，只要两子链的位姿满足以下条件即可重连：

p_L＝p_R (3)

式(3)实际为三个方程，当满足上述6个方程的6个关节变量都被求解出来时，两子链将在断开连接的位置处重新结合为原机器人，而这些得到的关节变量就是机器人的逆解；式(3)-(4)用于求θ₁、θ₂、θ₃和θ₆，式(5)可求得θ₄，式(6)可求得θ₅；

S2、然后证明相邻位姿点的逆解在相同唯一域中距离最小，并根据唯一域的判断，给出了逆解公式中正负符号确定的方法：

首先由式l₄c₃-l₃s₃＝0(c₃表示cosθ_3，s₃表示sinθ₃)可直接得到θ₃的边界值：

从而

因此，θ₃的逆解值域与唯一域是一致的，k₂的值可由下式确定：

k₁可由下式确定

由于机器人末端跟踪的是连续轨迹，k₁和k₂可由初始点代入上式(7)和(8)中确定；

S3、最后利用黄金分割法对非线性方程搜索求解，得到θ₁、θ₂、θ₃、θ₄、θ₅的求解公式；

S4、仿真验证本方法的有效性。

2.根据权利要求2所述的一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，在步骤S1中相邻连杆间的齐次变换公式为：

其中

θ_i——x_i-1轴和x_i轴之间关于z_i的夹角；

d_i——{i-1}坐标系原点沿着z_i-1轴到x_i轴的距离；

a_i——沿着x_i轴，z_i-1轴和z_i轴之间的距离；

α_i——z_i-1轴和z_i轴之间关于x_i的夹角。

3.根据权利要求1所述的一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，在步骤S1中采用Dixon合成法求解一元二次方程组：

假设一元二次方程组如下：

f(x)＝Ax²+Bx+C (10)

g(x)＝Dx²+Ex+F (11)

根据Dixon合成法的消元原则，可构造如下行列式：

式(12)可进一步写为：

其中α∈R是一个引入的新变量，U＝[α,1],V＝[x,1]^T，以及

|Σ|＝(AF-CD)²-(AE-BD)(BF-CE) (14)

其中，|Σ|表示行列式；式(14)是式(10)和式(11)的系数的方程，只要方程等于零，那么式(10)和式(11)中的x至少有一个共同的解。

4.根据权利要求1所述的一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，在步骤S2中：确定k₁可以保证θ₁的位移最小，确定k₂可以保证θ₃的位移最小，而初始点可以保证θ₆的位移最小。

5.根据权利要求1所述的一种求解非球腕6R工业机器人逆运动学的数值算法，其特征在于，在步骤S3中θ₁、θ₂、θ₃、θ₄、θ₅的求解公式为：

θ₁＝atan2(k₁B₁,k₁A₁)；θ₂＝atan2(s₂,c₂)；

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄)；θ₅＝atan2(0,s_β)+atan2(B₅,A₅)。

Images