CN111452041A - 一种非球腕6r机器人逆运动学求取方法 - Google Patents

Abstract

本发明提供一种非球腕6R机器人逆运动学求取方法。包括以下步骤：S1、基于旋量理论的指数积公式建立机器人的正向运动学方程；S2、通过机器人正向运动学公式变形得出机器人可分解结论；S3、得到机器人分解点选择与重连的几何约束条件；S4、推导非线性封闭方程户所有关节角的求解公式，并用二值法进行求解。S5、仿真验证本方法的有效性。本发明相比于传统方法具有更加明确的几何意义，推导过程更加简便，求解过程中满足实时、高精度控制。

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

S1、基于旋量理论的指数积公式建立n自由度机器人的正向运动学方程；n 自由度机器人的正向运动学方程为：

S2、通过n自由度机器人正向运动学公式得出n自由度机器人可分解结论；

S3、通过机器人的分解，得到机器人重连的几何约束条件；

分解后子机器人的正向运动学方程为：

其中

机器人重连的几何约束条件为：

{T_L}与{T_R}位置重合，<x_L,y_R>＝π-β，z_R＝z_L；

S4、以步骤S3机器人得到的重连几何约束条件推导出一个只含θ₆的非线性 封闭方程和所有关节角的求解公式，并用二值法进行数值求解；

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

进一步地，在步骤S1中基于旋量理论的指数积公式为：

式中：g_ST(θ)——经刚体变换后机器人末端工具坐标系{T}相对于惯性坐标系{S}的位姿g_ST(θ)；

g_ST(0)——机器人位于初始位形时工具坐标系{T}相对于惯性坐标{S}的位姿。

进一步地，在步骤S2中n自由度机器人可分解结论为：n自由度机器人可 分解为两个低自由度的子机器人。

进一步的，在步骤S3中几何约束条件为：两子机器人的工具坐标系位姿重 合。

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

θ₄＝atan2(A₄,B₄)-atan2(C₄,0)；θ₅＝atan2(A₅,B₅)-atan2(C₅,0)。

进一步的，在步骤S4中θ₆的求解公式为：f(θ₆)＝A₆cosθ₆-B₆sinθ₆-C₆。

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

附图说明

图1为n自由度机器人；

图2为子机器人R基座标系图；

图3为子机器人R经g_ST(θ)g_ST ^-1(0)变换图；

图4为FanucP-200E结构简图；

图5为工具坐标系间几何关系；

图6为三余弦定理图；

图7为算法主流程图；

图8为数值算法流程图；

图9为目标位姿T₁对应的f(θ₆)曲线图；

图10为目标位姿T₂对应的f(θ₆)曲线图；

图11为目标位姿T₃对应的f(θ₆)曲线图。

具体实施方式

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

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

n自由度机器人运动学的旋量表示

旋量理论将刚体在空间的运动，描述为由绕某一定轴的旋转和平移复合而成。 设ω＝[ω_x,ω_y,ω_z]^T∈R³表示刚体旋转轴的方向，r为轴上一点的位置，则刚体的运 动旋量可用ξ＝[ω；v]表示，其中v＝r×ω，ξ同样可以被称为刚体的螺旋轴。ω的反 对称矩阵记为：

以反对称矩阵为推广，可将ξ的矩阵形式记为：

刚体的矩阵指数形式为：

其中e^θ[ω]＝I+[ω]sinθ+[ω]²(1-cosθ)

根据旋量理论，可以得出图1中n自由度机器人的正向运动学公式的指数积 表达形式：

式中：g_ST(θ)——经刚体变换后机器人末端工具坐标系{T}相对于惯性坐标系{S}的位姿g_ST(θ)。

g_ST(0)——机器人位于初始位形时工具坐标系{T}相对于惯性坐标{S}的位 姿。

n自由度机器人可分解说明

式(4)可变形为：

Q_L＝g_ST(θ)g_ST ^-1(0)Q_R (5)

其中

式中g_i(0)是一个可逆的位姿矩阵，其原点位于第i+1个关节轴轴线的原点，姿态与惯性坐标系一致，其中第i+1个关节轴线原点的定义为：关节i与关节i+1两 轴轴线的公垂线与关节i+1轴线的交点。

式(4)中Q_L、Q_R在图2中具有明确的几何意义。明显可以看出，Q_L表示一 个i自由度机器人的正向运动学，其基座标系与原n自由度机器人的惯性坐标系 (图中以{S_L}表示)重合，其工具坐标系(图中以{T_i}表示)则为g_i(0)Q_R则表示 一个(n-i)自由度机器人的正向运动学，其基坐标系(图中以{S_R}表示)位于关 节n的原点，姿态与{S_L}一致，其工具坐标系也为{T_i}，该机器人各关节的旋转 方向为负方向。根据矩阵乘法的结合律：

ABC＝A(BC) (6)

式(4)右端的整体可理解为：Q_R所表示的机器人基座标系做g_ST(θ)g_ST ^-1(0)变换 后的结果，如图3所示。

斜偏置6R机器人的分解

以Fanuc P-200E为例介绍腕部斜偏置6R机器人的具体分解方法，下文中将 关节1至分解点组成的部分记为子机器人L，分解点至关节6组成的部分记为子 机器人R。

图4描述了Fanuc P-200E机器人在零位时各关节的螺旋轴。选择{S}作为惯 性坐标系，{T}作为工具坐标系，{T}与螺旋轴的参数均以{S}为参考描述，且{T} 的原点与关节6的原点重合。机器人零位时工具坐标系的位姿为：

各关节的螺旋轴列于表1中。

表1各关节螺旋轴

Fanuc P-200E机器人的正向运动学公式为：

观察图4可知，机器人关节1至关节4组成部分与3R仿人臂结构类似，该 结构机器人的位置反解已有完备的解法，因此分解点选择为关节4的原点，将其 分为4R子机器人L，和2R子机器人R。此处并没有将原机器人分解为两个3R 子机器人，因为对于腕部斜偏置6R机器人，该分解方式并不利于后面的逆解公 式推导。

由于此分离点相邻关节不满足1节最后描述的三种关系，因此子机器人的末 端工具坐标系需重新设定，如图5所示。图5中原6自由度机器人的工具坐标系 变为了子机器人R的基座标系，关节5与关节6轴线的单位向量不变，但旋转 变为负方向，此时子机器人R的基座标系还未变换。{T_L}与{T_R}分别为两个子 机器人的工具坐标系，二者的原点实际是重合的，为了方便观察，人为将{T_L} 向右进行了平移。图中x_L与关节4的轴线重合，其方向不受θ₄的影响，y_R与关 节5的轴线重合，其方向不受θ₅的影响。并且x_L与y_R共面，<x_L,y_R>＝π-β，z_R与z_L重合。因此{T_R}可认为由{T_L}绕z_L旋转(π/2-β)所得，即T_R＝T_LRotz(π/2-β)。故 两个子机器人的正向运动学可如下表示：

其中

将表1中螺旋轴的参数代入式(1)、(2)、(3)、(9)、(10)可得：

式(10)中

为原机器人的目标位姿。

此时，两个子机器人可以重新结合为腕部斜偏置6R机器人的几何约束条件 变为：{T_L}与{T_R}位置重合，<x_L,y_R>＝π-β，z_R＝z_L。

逆向运动学求解

θ₁、θ₂、θ₃和θ₆的求解

子机器人L和子机器人R可以重新结合为6自由度机器人的第一个条件是 末端位置重合，故可令p_L＝p_R组成方程组

l₃s₁+c₁A₃＝p_R1 (13)

-l₃c₁+s₁A₃＝p_R2 (14)

l₄c₂₃+l₅s₂₃+l₂c₂＝p_R3-l₁ (15)

A₃＝l₅c₂₃-l₄s₂₃-l₂s₂ (16)

式(13～16)中s_ij、c_ij、s_i、c_i分别表示sin(θ_i+θ_j)、cos(θ_i+θ_j)、sinθ_i、cosθ_i，i，j 为关节的序号,下文与此相同。

首先求式(13～14)的平方和，得

式中K₁＝±1。

再求式(17)与式(15)的平方和，得

l₅s₃+l₄c₃＝B₃ (19)

由上式易得

式中K₂＝±1。

由式(17)、(20)可知,当C₃≥0且|B₃|≤1时θ₃才有解。

确定θ₃后，可通过以下方式计算θ₂。将式(15)、(16)、(17)组成方程组，得

A₂＝l₅cosθ₃-l₄sinθ₃ (24)

B₂＝l₅sinθ₃+l₄cosθ₃+l₂ (25)

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

θ₁可通过式(13)和式(14)组成的方程组计算，得

θ₁＝atan2(s₁,c₁) (29)

根据式(17)中K₁和式(21)中K₂的符号可知，θ₁、θ₂和θ₃可以得到四组关于θ₆的方程式。

子机器人L和子机器人R可以重新结合为6自由度机器人的第二个条件是： 轴线y_R与轴线x_L的夹角保持π-β，因|x_L|＝|y_R|＝1，故可得

x_Ly_R＝|x_L||y_R|cos(π-β)＝cos(π-β) (30)

因向量x_L与θ₁、θ₂和θ₃有关，而y_R只与θ₆有关，故式(30)是一个仅与θ₆有关 的非线性封闭方程，根据θ₁、θ₂和θ₃的求解公式可知式(30)有四个不同的方程， 求出这个四个方程的θ₆，便可以求出θ₁、θ₂和θ₃。

θ₄和θ₅的求解

从图5可以看出，一旦θ₁、θ₂、θ₃和θ₆确定后(即前面两个约束条件满足后)， 只要分别旋转θ₄和θ₅必然可以满足z_R＝z_L。但是直接用该约束条件求θ₄和θ₅，并 不方便。

观察图5，由于θ₁、θ₂、θ₃和θ₆确定后，x_L与y_R将保持不变，而x_L与x_R之 间应满足

<x_L,x_R>＝π/2-β，并且此时的x_L与x_R只有θ₅一个未知量，故可以由 x_Lx_R＝cos(π/2-β)求得θ₅：

式中根号部分实际上为零，θ₅只有一个解。以下给出证明。

图6中直线m在x_Ly_R所确定的平面内，x_R的射影直线在直线m上，γ₁为二 者的夹角；

另外，γ₂为m与x_L的夹角，γ₃为x_L与x_R的夹角。由三余弦定理有：

cosγ₃＝cosγ₁cosγ₂ (32)

因γ₂＝π/2-β是定值，故只有当γ₁＝0时才有γ₂＝γ₃，即θ₅在旋转过程中，只有使得x_R位于x_Ly_R所确定的平面内时才能满足约束条件。因此θ₅在最小正周期内只 有一个解，式(31)可改写为

θ₅＝atan2(A₅,B₅)-atan2(C₅,0) (33)

θ₅求解后，再考虑旋转θ₄使得<y_L,y_R>＝π/2-β,同理可以求得θ₄只有唯一解：

θ₄＝atan2(A₄,B₄)-atan2(C₄,0) (34)

式(33)、(34)中A₄、B₄、C₄、A₅、B₅、C₅的表达式为：

A₄＝-sinθ₁*(y₁*cosθ₆*sinβ-x₁*cosβ-z₁*sinβ*sinθ₆) +cosθ₁*(y₂*cosθ₆*sinβ-x₂*cosβ-z₂*sinβ*sinθ₆)；

B₄＝(cosθ₁*cosθ₂*sinθ₃+cosθ₁*cosθ₃*sinθ₂)*(y₁*cosθ₆*sinβ-x₁*cosβ-z₁*sinβ*sinθ₆) +(cosθ₂*sinθ₁*sinθ₃+cosθ₃*sinθ₁*sinθ₂)*(y₂*cosθ₆*sinβ-x₂*cosβ-z₂*sinβ*sinθ₆) -cos(θ₂+θ₃)*(y₃*cosθ₆*sinβ-x₃*coβ-z₃*sinβ*sinθ₆)；

C₄＝sinβ；

A₅＝(y₁*cosβ*cosθ₆-z₁*cosβ*sinθ₆+x₁*sinβ)*cos(θ₂+θ₃)*cosθ₁ +(y₂*cosβ*cosθ₆-z₂*cosβ*sinθ₆+x₂*sinβ)*cos(θ₂+θ₃)*sinθ₁ +(y₃*cosβ*cosθ₆-z₃*cosβ*sinθ₆+x₃*sinβ)*sin(θ₂+θ₃)；

B₅＝(z₁*cosθ₆+y₁*sinθ₆)*cos(θ₂+θ₃)*cosθ₁+(z₂*cosθ₆+y₂*sinθ₆)*cos(θ₂+θ₃)*sinθ₁ +(z₃*cosθ₆+y₃*sinθ₆)*sin(θ₂+θ₃)；

C₅＝sinβ。

逆运动学数值解法

虽然式(30)只含有θ₆一个变量，但直接求θ₆的解析值很困难，本节将以二分 法求θ₆的数值解。

令式(30)为

f(θ₆)＝A₆cosθ₆-B₆sinθ₆-C₆ (35)

A₆、B₆、C₆均是关于θ₆的方程，其表达式为：

A₆＝y₁*sinβ*cos(θ₂+θ₃)*cos(θ₁)+y₂*sinβ*cos(θ₂+θ₃)*sin(θ₁)+y₃*sinβ*sin(θ₂+θ₃)；

B₆＝z₁*sinβ*cos(θ₂+θ₃)*cos(θ₁)+z₂*sinβ*cos(θ₂+θ₃)*sin(θ₁)+z₃*sinβ*sin(θ₂+θ₃)；

C₆＝x₁*cosβ*cos(θ₂+θ₃)*cos(θ₁)+x₂*cosβ*cos(θ₂+θ₃)*sin(θ₁)+x₃*cosβ*sin(θ₂+θ₃)+cos(π-β)；

此时只需求出式(35)中的零点，因求解过程中均为数值解，故只需求θ₆使得 |f(θ₆)|≤ε即可，ε为设定的阈值。算法具体如下：

图7是本文所使用的数值算法的主流程图，该算法的主要思路是：首先初始 化θ₆，阈值ε₁、ε₂，迭代次数k，目标位姿T以及步长h；然后计算式(13)～(20)， 如果式(17)和式(20)不满足“C₃≥0且|B₃|≤1”，则重置k＝1，θ₆增加一个步长重新开 始计算；如果式(17)和式(20)满足“C₃≥0且|B₃|≤1”，则计算式(21)～(31)。并将得到 的f(θ₆)进行判定：

I.以“k≥2且f(^(k)θ₆)f(^(k-1)θ₆)<0”做判定，如果判定为“是”，则转入图8中的子算法。在子算法中以[^(k-1)θ₆,^(k)θ₆]为区间，用二分法求解符合|f(θ₆)|≤ε₁的θ₆；

II.若I中判定为否，则进行下一个判定“^(k)θ₆＝2π或|f(^(k)θ₆)|≤ε₁”，该判定为“是” 则输出θ₆，为“否”则令“k＝k+1，^(k+1)θ₆＝^(k)θ₆+h”，重新从式(13)开始计算。

算法求得的θ₆最后反代回式(13)～(29)以及(33)～(34)，即可求得θ₁、θ₂、θ₃、 θ₄和θ₅。由K₁和K₂的符号可知，上述算法需单独运行四次。

仿真算例

为验证3.2节中逆运动学算法的有效性，本节将设置三组位姿进行求解。算 法的计算软件为Matlab2014b，PC配置如下：处理器为Intel(R)Core(TM)i5-9400F， CPU速度为2.90GHz，安装内存为16.00GB。

图4中Fanuc P-200E机器人的连杆参数分别为l₁＝1510mm，l₂＝1400mm， l₃＝455mm，l₄＝150mm，l₅＝1400mm，l₆＝100mm，β＝π/3。算法参数设置为：⁽⁰⁾θ₆＝0， ε₁＝ε₂＝10^-14，k＝1，h＝π/180。根据3.1节公式推导的结果，每组位姿将会得到4 个不同的f(θ₆)，为方便区分，对每个f(θ₆)做表2中的定义。

表2 f(θ₆)的定义

数值实验结果

本次实验的目标位姿分别为:

3组位姿分别求得的逆运动学解列于表3～5，图8为3组位姿求解过程中分 别得到的f_i(θ₆)曲线图。图中“-”表示f₁(θ₆)，“--”表示f₂(θ₆)，“-.”表示f₃(θ₆)，“.”表 示f₄(θ₆)。

表3为三组位姿中四条曲线各自独立求解1000次的平均速度。

表3三组位姿的平均求解速度

图9、10中，虽然f(θ₆)的曲线都是连续的，但是图9中的曲线有8个零点， 而图10中只有4个零点，表明Fanuc P-200E机器人的两组位姿分别有8组和4 组逆运动学解。图11中f(θ₆)不连续，存在间断点，表明算法在搜索过程中存在 不满足“C₃≥0且|B₃|≤1”条件的点，但是此位姿下，机器人仍然有8组逆运动学解。

从图9、图10、图11中可以明显看出，本文提出的二值法对每组位姿只需 运行四次算法，即可求出所有解。值得注意的是，由于步长和阈值设置的原因， 在每次算法运行过程中，有些解可能无需通过二值法便可求得。通常，算法设定 的阈值越大，步长越小，则出现这种情况的可能性会越大，但不影响算法整体运 行的结果，因为输出结果是符合设定阈值的。

结合图9、图10、图11和表3可以知道：当f_i(θ₆)连续且存在2个零点时， 算法运行一次最多需要0.334ms；当f_i(θ₆)连续且无零点时，因算法无需进入子算 法——二值法环节，故此时运行一次最多需要0.198ms；当f_i(θ₆)存在间断点时， 间断部分会跳过计算f_i(θ₆)以及二值法环节，故此时运行一次最多需要0.252ms。 综上可知，只有当f_i(θ₆)连续且存在多个零点时，算法运行的时间才会最多，但 依然能满足机器人实时控制的要求。表4中各目标位姿求得的逆运动学解代入式 (1)中，所得位姿与各自目标位姿之间的绝对误差e最大值仍然在10^-12数量级， 表明算法的求解精度高。

表3得到的算法运行时间，都以θ₆从0到2π做单向搜索得到，从图8可以 看出，有些解无需θ₆搜索至2π即可结束算法，从而节省算法的运行时间。这需 要进一步研究斜偏置6R机器人目标位姿对应的逆解数目，如果能明确每个位姿 的f_i(θ₆)有多少个零点，那么算法将可以以一种更优的方式结束一次运行，从而 提高求解速度。

本文的方法在公式推导过程中有更明确的几何意义。本文采用固定的初始迭 代点，算法在固定运行次数内便可求出所有逆解；同时本文的方法具有更强的理 论推广性，同时给出了其他关节角的具体求解方法；本文算法部分所使用的二值 法易于编程实现，与常用的牛顿-拉夫森算法相比，无需求雅可比矩阵，求解过 程中不受奇异位置的影响。

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

Claims (6)

1.一种非球腕6R机器人逆运动学求取方法，其特征在于，包括以下步骤：

S1、基于旋量理论的指数积公式建立n自由度机器人的正向运动学方程；n自由度机器人的正向运动学方程为：

S2、通过n自由度机器人正向运动学公式得出n自由度机器人可分解结论；

S3、通过机器人的分解，得到6R机器人重连的几何约束条件；

分解后子机器人的正向运动学方程为：

其中

机器人重连的几何约束条件为：

{T_L}与{T_R}位置重合，<x_L,y_R>＝π-β，z_R＝z_L；

S4、以步骤S3机器人得到的重连几何约束条件推导出一个只含θ₆的非线性封闭方程和所有关节角的求解公式，并用二值法进行数值求解；

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

2.根据权利要求1所述的一种非球腕6R机器人逆运动学求取方法，其特征在于，在步骤S1中基于旋量理论的指数积公式为：

式中：g_ST(θ)——经刚体变换后机器人末端工具坐标系{T}相对于惯性坐标系{S}的位姿g_ST(θ)；

g_ST(0)——机器人位于初始位形时工具坐标系{T}相对于惯性坐标{S}的位姿。

3.根据权利要求1所述的一种非球腕6R机器人逆运动学求取方法，其特征在于，在步骤S2中n自由度机器人可分解结论为：n自由度机器人可分解为两个低自由度的子机器人。

4.根据权利要求1所述的一种非球腕6R机器人逆运动学求取方法，其特征在于，在步骤S3中几何约束条件为：两子机器人的工具坐标系位姿重合。

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

θ₁＝atan2(s₁,c₁)；θ₂＝atan2(s₂,c₂)；

θ₄＝atan2(A₄,B₄)-atan2(C₄,0)；θ₅＝atan2(A₅,B₅)-atan2(C₅,0)。

6.根据权利要求1所述的一种非球腕6R机器人逆运动学求取方法，其特征在于，在步骤S4中θ₆的求解公式为：f(θ₆)＝A₆cosθ₆-B₆sinθ₆-C₆。

Images