CN107756400B - 一种基于旋量理论的6r机器人逆运动学几何求解方法 - Google Patents

Abstract

本发明公开了一种基于旋量理论的6R机器人逆运动学几何求解方法，属于机器人运动学逆解方法研究领域。建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型。将6R机器人的前三个关节的逆解运动分解描述并建立六元二次方程组。基于旋量正运动学模型求解初始位置q_s1对应的目标位置q_e1。该方法将几何描述与旋量理论结合，几何意义更加明确，通过简化逆解算法为代数方程组求解，有效提升了计算效率，能够为机器人运动实时控制提供一种新的逆解处理方法。

Description

一种基于旋量理论的6R机器人逆运动学几何求解方法

技术领域

本发明属于机器人运动学逆解方法研究领域，尤其涉及一种基于旋量理论的6R机器人逆运动学几何求解方法。

背景技术

运动学分析是实现运动控制的基础，主要建立关节变量与末端位姿的映射模型。其中逆解问题，即已知末端位姿求解关节变量的问题，是机器人领域研究热点之一，其求解效率直接影响着机器人运动控制的实时性能。目前机器人运动学逆解建模主要基于D-H法和旋量理论，研究者们对比了两种方法并发现后者的应用具有以下优点：可避免建立局部坐标系，简化计算模型并克服了局部参数产生的奇异性；其几何意义明确，可方便的确定产生多解的条件和个数。目前基于旋量描述的Paden-Kahan子问题被广泛的机器人逆向求解问题中，但该方法不适用于任意构型的六自由度机器人，因此多数学者基于三种基本子问题进行了延伸并提出了一些新的子问题模型，如Tan对子问题二进行了改进建立了针对“绕两个不相交轴的旋转运动”子问题的数学模型；Chen描述了一种“绕三个轴线旋转的运动，其中两个轴线平行且与第三轴线异面”的子问题，针对其逆解进行了详细的旋量描述。基于这些子问题模型在六自由度串联机器人逆解问题中得到了广泛应用，研究表明当6R机器人满足Piper准则时其逆运动学问题具有封闭解。Sariyildi等基于三个子问题实现了6R机器人的逆运动学求解；吕世增等通过在旋量法中引入吴方法减少了对子问题的依赖性，并将6R机器人的逆解问题转化为六元八次方程组的求解问题。为了进一步简化6R机器人逆解模型，本方法通过几何描述方法将6R机器人的逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，从而使其几何意义更加清晰，求解过程更加简单。

发明内容

本发明的目的旨在提供一种基于旋量理论的6R机器人逆运动学几何求解方法。该方法的主要特点是通过结合几何描述方法和旋量理论，针对6R机器人运动学逆解问题提出一种几何意义清晰、求解过程简单的逆运动学模型。

为实现上述目的，本发明采用的技术手段为一种基于旋量理论的6R机器人逆运动学几何求解方法，该方法的实现过程如下：

S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型。

S2、如图1，将6R机器人的前三个关节的逆解运动分解描述：由点q_e到点c₂＝[x₂ y₂z₂]绕关节1的旋转运动；点c₂到点c₁＝[x₁ y₁ z₁]绕关节2的旋转运动；点c₁到点q_s绕关节3的旋转运动。其中q_s，q_e分别为机器人末端的初始和目标位置，根据图1所示的几何关系描述建立以x₁,y₁,z₁,x₂,y₂和z₂为变量的六元二次方程组。

S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c₁和c₂的坐标矢量。

S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ₁，θ₂和θ₃的显式求解模型，并进行求解。

S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置q_s1，并基于旋量正运动学模型求解初始位置q_s1对应的目标位置q_e1。

S6、如图2，将关节4和5的逆解运动分解描述：由点q_e1到点c₃＝[x₃y₃z₃]的旋转运动；由点c₃到点q_s1的旋转运动。根据图2示，根据几何关系建立以x₃,y₃和z₃为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c₃的坐标矢量。

S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ₄和θ₅显式求解模型，并进行求解。

S8、取不在关节6轴线上的任意一点q_s2，基于旋量正运动学模型求解q_s2对应的目标位置q_e2，则关节6的逆解运动始末位置已知。同理基于子问题1求解θ₆。

本发明的特点在于基于几何描述方法将6R机器人的运动学逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，简化了逆解模型，且几何意义更加明确，为6R机器人的实时运动控制提供一定的方法支撑。该方法将几何描述与旋量理论结合，几何意义更加明确，通过简化逆解算法为代数方程组求解，有效提升了计算效率，能够为机器人运动实时控制提供一种新的逆解处理方法。

附图说明

图1关节1、2和3的逆解运动几何描述；

图2关节4和5的逆解运动几何描述；

图3某6R机器人参数坐标系。

具体实施方式

以下结合附图1-3对本发明进行详细说明。

S1确定6R机器人运动学参数并建立正运动学模型

如图3所示，已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：

其中r_i，1≤i≤6表示i关节在基坐标系的位置矢量，ω_i表示i关节的旋转矢量。

基于旋量理论，该6R机器人正运动学模型表示为，

其中g_st(θ),g_st(0)分别表示机器人末端的初始位姿与目标位姿，

表示i关节旋转运动的指数积形式，

式中θ_i为第i个关节角位移；

是i关节旋转矢量ω_i的另一种表示形式，由ω_i＝[ω₁ ω₂ ω₃]定义为

则

ν_i是i关节运动的旋转线速度，ν_i＝-ω_i×r_i。

给定目标位姿，

S2针对前三个关节建立六元二次方程组

基于旋量理论前三个关节的旋转运动描述为，

式中q′_s和q′_e分别表示6R机器人末端在该旋转运动的初始位置与目标位置。由S1可知，

q′_s＝[x_s y_s z_s 1]＝[0 744 940 1]

q′_e＝[x_e y_e z_e 1]＝[-936.6611 631.7859 570.0752 1]

设运动经过点c₁和c₂,根据图1所示几何描述建立以下关系式组，

其中q₁，q₂和q₃分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q₁＝[0 0 0]，q₂＝[0 150 250]和q₃＝[0 150 800]，那么式(7)表示为如下方程组，

采用MATLAB中“SOLVE”函数求解方程组(8)，得到x₁,y₁,z₁,x₂,y₂和z₂的解。

S3计算关节角位移θ₁，θ₂和θ₃

得到过程点坐标c₁＝[x₁ y₁ z₁]和c₂＝[x₂ y₂ z₂]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，

步骤(4)针对关节4和关节5建立三元二次方程组

由式(3)知，

其中

在关节6的轴线上取一点q_s1＝[x_s1 y_s1 z_s1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，

其中q′_e1＝g₁q′_s1＝[x_e1 y_e1 z_e1 1]。

设运动经过点坐标为c₃＝[x₃ y₃ z₃]，根据图2所示几何描述建立以下关系式组，

其中q₄＝[0 744 940]。则式(14)表示为如下方程，

采用MATLAB中“SOLVE”函数求解方程组(15)，得到x₃,y₃和z₃的解。

步骤(5)计算关节角位移θ₄和θ₅

得到过程点坐标c₃＝[x₃ y₃ z₃]后，将绕关节4和关节5的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移θ₄和θ₅的显示表达式如下，

步骤(6)计算关节角位移θ₆

取不在关节6轴线上一点q_s2＝[0 750 940]，其绕关节6的旋转运动描述为，

其中

同理基于Paden-Kahan子问题1，得到关节角位移θ₆的显示表达式如下，

通过以上求解得到八组运动学逆解如表1所示。

表1八组运动学逆解

Claims (2)

1.一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：该方法的实现过程如下：

S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型；

S2、将6R机器人的前三个关节的逆解运动分解描述：由点q_e到点c₂＝[x₂ y₂ z₂]绕关节1的旋转运动；点c₂到点c₁＝[x₁ y₁ z₁]绕关节2的旋转运动；点c₁到点q_s绕关节3的旋转运动；其中q_s，q_e分别为机器人前三个关节末端的初始和目标位置，根据几何关系描述建立以x₁,y₁,z₁,x₂,y₂和z₂为变量的六元二次方程组；

S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c₁和c₂的坐标矢量；

S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ₁，θ₂和θ₃的显式求解模型，并进行求解；

S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置q_s1，并基于旋量正运动学模型求解初始位置q_s1对应的目标位置q_e1；

S6、将关节4和5的逆解运动分解描述：由点q_e1到点c₃＝[x₃ y₃ z₃]的旋转运动；由点c₃到点q_s1的旋转运动；根据几何关系建立以x₃,y₃和z₃为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c₃的坐标矢量；

S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ₄和θ₅显式求解模型，并进行求解；

S8、取不在关节6轴线上的任意一点q_s2，基于旋量正运动学模型求解q_s2对应的目标位置q_e2，则关节6的逆解运动始末位置已知；同理基于子问题1求解θ₆。

2.根据权利要求1所述的一种基于旋量理论的6R机器人逆运动学几何求解方法，其特征在于：S1确定6R机器人运动学参数并建立正运动学模型

已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：

其中r_i，1≤i≤6表示i关节在基坐标系的位置矢量，ω_i表示i关节的旋转矢量；

基于旋量理论，该6R机器人正运动学模型表示为，

其中g_st(0),g_st(θ)分别表示机器人末端的初始位姿与目标位姿，

表示i关节旋转运动的指数积形式，

式中θ_i为第i个关节角位移；

是i关节旋转矢量ω_i的另一种表示形式，由ω_i＝[ω₁ω₂ ω₃]定义为

则

ν_i是i关节运动的旋转线速度，ν_i＝-ω_i×r_i；

给定目标位姿，

S2针对前三个关节建立六元二次方程组

基于旋量理论前三个关节的旋转运动描述为，

式中q′_s和q′_e分别表示6R机器人末端在该旋转运动的初始位置与目标位置；由S1可知，

q′_s＝[x_s y_s z_s 1]＝[0 744 940 1]

q′_e＝[x_e y_e z_e 1]＝[-936.6611 631.7859 570.0752 1]

设运动经过点c₁和c₂,根据几何描述建立以下关系式组，

其中q₁，q₂和q₃分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q₁＝[0 0 0]，q₂＝[0 150 250]和q₃＝[0 150 800]，那么式(7)表示为如下方程组，

采用MATLAB中“SOLVE”函数求解方程组(8)，得到x₁,y₁,z₁,x₂,y₂和z₂的解；

S3计算关节角位移θ₁，θ₂和θ₃

得到过程点坐标c₁＝[x₁ y₁ z₁]和c₂＝[x₂ y₂ z₂]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，

S4针对关节4和关节5建立三元二次方程组

由式(3)知，

其中

在关节6的轴线上取一点q_s1＝[x_s1 y_s1 z_s1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，

其中q′_e1＝g₁q′_s1＝[x_e1 y_e1 z_e1 1]；

设运动经过点坐标为c₃＝[x₃ y₃ z₃]，根据几何描述建立以下关系式组，

其中q₄＝[0 744 940]；则式(14)表示为如下方程，

采用MATLAB中“SOLVE”函数求解方程组(15)，得到x₃,y₃和z₃的解；

S5计算关节角位移θ₄和θ₅

得到过程点坐标c₃＝[x₃ y₃ z₃]后，将绕关节4和关节5的旋转运动分别描述如下，

基于Paden-Kahan子问题1，得到关节角位移θ₄和θ₅的显示表达式如下，

S6计算关节角位移θ₆

取不在关节6轴线上一点q_s2＝[0 750 940]，其绕关节6的旋转运动描述为，

其中

同理基于Paden-Kahan子问题1，得到关节角位移θ₆的显示表达式如下，

通过以上求解得到八组运动学逆解。

Images