CN107203653B - 一种六自由度串联机器人的逆运动学通用求解方法 - Google Patents

Abstract

本发明公开了一种六自由度串联机器人的逆运动学通用求解方法，属于机器人逆运动学领域，该方法是在指数积模型的基础上，提出的一种计算过程简单、易于实现的封闭解求解方法，其主要利用旋量理论的基本性质、Rodrigues旋转公式以及特殊的几何结构，将复杂的逆解求解问题转化为简单的三角函数方程进行求解，使得6个关节角度只需要两个表达式即可表示，形式简单、方便记忆。该发明实用范围广，可应用于任意满足Pieper原则且前三个关节中相邻两个轴之间具有相交或平行关系的机器人中，该发明促进机器人的应用推广、简化了应用过程。

Description

一种六自由度串联机器人的逆运动学通用求解方法

技术领域

本发明属于机器人逆运动学领域，具体涉及一种六自由度串联机器人的逆运动学通用求解方法。

背景技术

6R型机器人是目前工业中常用的一类机器人，主要因为该类机器人能够获得有效逆解，目前已有的机器人主要采用H-D模型建立机器人的运动学模型，并通过Paul和Pieper等人提出的方法进行机器人逆运动学的求解，该类方法完全依赖机器人的机械结构，而且针对不同的机器人需要重新计算，计算过程复杂，每个角度的表达形式也是各异。这为机器人二次开发带来很多不便。目前，在机器人运动学的研究中，更多的是采用指数积运动学模型，该模型是由旋量理论和指数积公式相结合建立的，建立过程简单、灵活而且是一种完备的机器人模型，只需要两个坐标系，可避免奇异性。针对这种模型Paden-Kahan等人提出一种子问题求解方法，即将6自由度机器人逆解问题化简成多个子问题进行求解。总共分为三阶：一阶，二阶，三阶，几阶对应几个自由度，由于旋量理论的性质，在机器人满足Pieper原则时，可将其分解成3自由度一下的问题，该原则证明了相邻三个关节相交于一点或相互平行则存在逆解。目前这些子问题的求解方法主要基于机器人结构中特殊的几何关系，即具有明显的几何意义又具有数值稳定性，广受好评。但每一种几何关系都有多种表达形式，这为实际应用带来很多不便。

发明内容

针对现有技术中存在的上述技术问题，本发明提出了一种六自由度串联机器人的逆运动学通用求解方法，设计合理，克服了现有技术的不足，具有良好的效果。

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

一种六自由度串联机器人的逆运动学通用求解方法，包括如下步骤：

步骤1：求解机器人的腰部关节角度θ₁、肩部关节角度θ₂和肘部关节角度θ₃

根据指数积模型，机器人运动学方程可表示为：

其中，

其中，下标t和w分别表示末端工具坐标系与世界坐标系，θ是各关节的旋转角度向量θ＝[θ₁,...,θ₅]，g_wt(0)和g_wt(θ)分别表示在初始状态下和θ状态下末端工具坐标系相对世界坐标系的变换关系，

为第i关节的运动旋量，

包括关节轴的单位方向向量ω_i和轴上的任意一点r_i，ω_i和r_i被称为旋量参数，

为第i关节坐标变换的指数表达，

是旋转矩阵的指数表达，其Rodrigues旋转公式可表示为：

为了方便叙述，统一将空间任一点p的齐次坐标表示为

首先，利用消元法将机器人的后三个关节消去；然后分别计算几个子问题来求解。设

是腕部关节的交点，将公式(1)两边同乘以

可得：

若轴2和轴3平行，存在如下等式：

(p₂-r₄)^Tω₂＝0 (5)；

若轴2和轴3相交，满足r₂＝r₃，则存在如下等式：

||p₂-r₂||＝||r₄-r₂|| (6)；

将

分别带入公式(5)和(6)，并利用Rodrigues旋转矩阵公式来表示

最终将公式(5)和(6)简化为下面的表达形式：

x₁sinθ₁+y₁cosθ₁＝z₁ (10)；

对于平行和相交情况，其中的已知参数

的值不同，且均需满足

则根据公式(10)可解得θ₁的值：

θ₁的值一旦确定，p₂就可根据

来获得，再根据距离不变原则可知：

||p₁-r₃||＝||r₄-r₃|| (14)；

将

带入公式(14)，利用Rodrigues公式表示

并将公式(14)两边平方后整理可得：

x₂sinθ₂+y₂cosθ₂＝z₂ (17)；

其中，

均为已知参数，且

则根据公式(17)可解得θ₂的值：

同样p₁的值可根据公式

来获得，而p₁还可表示为：

将

的Rodrigues旋转矩阵公式带入公式(19)，整理可得：

x₃sinθ₃+y₃cosθ₃＝z₃ (20)；

式中

均为已知参数，则根据公式(20)可解得θ₃的值：

θ₃所在象限可根据

和

的符号决定；

步骤2：求解机器人的腕部前两个关节角度θ₄和θ₅

将θ₁，θ₂，θ₃带入公式(1)，并将已知项移到公式(1)的左边，可得：

将式(24)两边同乘以轴6上的一点

且r₆≠r₄，可消去一项，方程变为：

根据旋量理论性质中的距离相等公式可知：

||c-r₅||＝||r₆-r₅|| (26)；

其中，

r₄₀表示轴4上不同于r₄的任意点，参照θ₁的求解过程可直接给出θ₄的表达式：

其中，

均为已知参数，且

将θ₄的值带入

中后可获得c的值，而c还可以表示为：

公式(28)与公式(19)形式相同，则参考θ₃的求解过程，可直接得出θ₅的表达式：

其中，

均为已知参数，θ₅的象限可根据

和

的符号决定；

步骤3：求解机器人的末端关节角θ₆

将前面计算得到的θ₁，θ₂，θ₃，θ₄和θ₅带入公式(1)，并将已知项移到公式(1)的左边，可得：

将式(30)两边同乘以轴6以外的点，这里取点p，可得：

其中，

易得：

公式(32)与公式(19)形式相同，则可直接给出角度θ₆的表达式：

其中，

均为已知参数，θ₆的象限可根据

和

的符号决定。

本发明所带来的有益技术效果：

1、计算精度高；给出了各关节角度的封闭解，可利反三角函数直接求出，具有很高的计算精度；2、实现简单；每个关节的表达形式非常简单易懂，只需求解一次反三角函数即可；3、形式统一；6个关节可用两个表达式统一表达，容易记忆，方便应用。

附图说明

图1为6自由度机器人运动学简图。

图2为肩关节轴与肘关节轴平行的示意图。

图3为第一腕关节轴与第二腕关节轴的逆解示意图。

图4为第三腕关节轴的逆解示意图。

其中，1-腰关节轴；2-肩关节轴；3-肘关节轴；4-第一腕关节轴；5-第二腕关节轴；6-第三腕关节轴。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1，针对6R机器人，包括腰关节轴1，肩关节轴2，肘关节轴3，第一腕关节轴4，第二腕关节轴5和第三腕关节轴6，腕部三条关节轴线相交于一点r₄，轴1与轴2相互异面，轴2与轴3为平行。本发明是在机器人指数积模型基础上实现的，指数积模型科表示为：

其中，

其中，下标t和w分别表示末端工具坐标系与世界坐标系，θ是各关节的旋转角度向量θ＝[θ₁,...,θ₅]，g_wt(0)和g_wt(θ)分别表示在初始状态下和θ状态下末端工具坐标系相对世界坐标系的变换关系，

为第i关节的运动旋量，

包括关节轴的单位方向向量ω_i和轴上的任意一点r_i，ω_i和r_i被称为旋量参数，

为第i关节坐标变换的指数表达，

是旋转矩阵的指数表达，其Rodrigues表达为：

为了叙述方便，假设空间任一点p的齐次坐标可表示为

实施过程分为三步，第一步求解机器人的腰部、肩部和肘部关节角度，第二步求解机器人的腕部前两个关节角度，最后求解机器人的末端关节角。

Step1：求θ₁，θ₂和θ₃。

首先，针对图1所示机器人，利用消元法消去机器人的腕部关节，根据旋量理论的性质，轴上一点绕轴旋转后的位置保持不变，即

则将公式(1)两边同乘以腕部关节的交点

可得：

如图2，对于两个平行轴线，存在：

(p₂-r₄)^Tω₂＝0 (5)；

而对于两个相交轴，由于r₂＝r₃，则存在

||p₂-r₂||＝||r₄-r₃|| (6)；

其中

且根据轴上的点绕轴做旋转运动其位置保持不变的性质，可得

这两个表达式相减，并将

的表达式(2)带入易得：

将上式带入公式(5)和(6)可得：

其中

用Rodrigues旋转公式表示，带入后两边平方均可整理得：

x₁sinθ₁+y₁cosθ₁＝z₁ (10)；

当平行时，

当相交时，

设x₁＝ρcosφ，y₁＝ρsinφ，则

利用三角函数的积化和差公式，公式(10)可变为：

其中

同样可以得到：

则关节角θ₁度可表示为：

需要注意的是，在上式中需选择合适的r₁和r₂来保证

将θ₁的值带入公式(8)中可获得p₂的值。如图2，可得

||p₁-r₃||＝||r₄-r₃|| (14)；

参考公式(8)p₁可表示为

将上式带入公式(14)得：

其中

用Rodrigues表达后带入上式，两边平方整理可得：

x₂sinθ₂+y₂cosθ₂＝z₂ (17)；

其中

根据θ₁的求解过程，可直接从上式中解出θ₂

将θ₂的值带入公式(15)中可得p₁，而p₁还可表示为：

将

的Rodrigues旋转公式带入，整理可得：

x₃sinθ₃+y₃cosθ₃＝z₃ (20)；

其中

由于

则对公式(20)两边分别同乘以

和

可得：

则θ₃可表示为：

其中θ₃角度值的最终取值取决于

和

的符号。

Step2：求θ₄和θ₅。

将θ₁，θ₂和θ₃带入机器人指数积模型公式(1)，并将已知项移到公式的左边，

将上式两边同乘以轴6上的一点

且r₆≠r₄，则可消去轴6的指数变换矩阵，

其中

如图3，根据旋量理论性质中的距离相等公式可知：

||c-r₅||＝||r₆-r₅|| (26)；

其中

r₄₀表示轴4上不同于r₄的任意点，参照θ₁的求解过程可直接给出结果：

其中

需注意的是4和5相交，所以上式必须满足r₄₀≠r₅≠r₄。θ₄已知后，c的值可以确定，再利用c的另一个表达式：

上式与公式(19)的形式相同，则可参考θ₃的求解过程，直接给出θ₅结果：

其中

Step3：求θ₆

将前面求解的θ₁，θ₂，θ₃，θ₄和θ₅带入机器人指数积模型中，并将已知参数移到方程的左边：

两边同乘以轴6以外的点，记为p，可得，

其中

如图1，易得：

比较上式与公式(19)形式相同，则可直接给出角度θ₅的表达式，

其中

这里θ₆所在象限由

和

的符号来决定。该发明还可推广到前两个关节轴线平行或相交，且与第三个关节轴线关系任意的情况，推导过程类似。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (1)

1.一种六自由度串联机器人的逆运动学通用求解方法，其特征在于：包括如下步骤：

步骤1：求解机器人的腰部关节角度θ₁、肩部关节角度θ₂和肘部关节角度θ₃

根据指数积模型，机器人运动学方程可表示为：

其中，

其中，下标t和w分别表示末端工具坐标系与世界坐标系，θ是各关节的旋转角度向量θ＝[θ₁,...,θ₅]，g_wt(0)和g_wt(θ)分别表示在初始状态下和θ状态下末端工具坐标系相对世界坐标系的变换关系，

为第i关节的运动旋量，

包括关节轴的单位方向向量ω_i和轴上的任意一点r_i，ω_i和r_i被称为旋量参数，

为第i关节坐标变换的指数表达，

是旋转矩阵的指数表达，其Rodrigues旋转公式可表示为：

为了方便叙述，统一将空间任一点p的齐次坐标表示为

首先，利用消元法将机器人的后三个关节消去；设

是腕部关节的交点，将公式(1)两边同乘以

可得：

若轴2和轴3平行，存在如下等式：

(p₂-r₄)^Tω₂＝0 (5)；

若轴2和轴3相交，满足r₂＝r₃，则存在如下等式：

||p₂-r₂||＝||r₄-r₂|| (6)；

将

分别带入公式(5)和(6)，并利用Rodrigues旋转矩阵公式来表示

最终将公式(5)和(6)简化为下面的表达形式：

x₁sinθ₁+y₁cosθ₁＝z₁ (10)；

对于平行和相交情况，其中的已知参数

的值不同，且均需满足

则根据公式(10)可解得θ₁的值：

θ₁的值一旦确定，p₂就可根据

来获得，再根据距离不变原则可知：

||p₁-r₃||＝||r₄-r₃|| (14)；

将

带入公式(14)，利用Rodrigues公式表示

并将公式(14)两边平方后整理可得：

x₂sinθ₂+y₂cosθ₂＝z₂ (17)；

其中，

均为已知参数，且

则根据公式(17)可解得θ₂的值：

同样p₁的值可根据公式

来获得，而p₁还可表示为：

将

的Rodrigues旋转矩阵公式带入公式(19)，整理可得：

x₃sinθ₃+y₃cosθ₃＝z₃ (20)；

式中

均为已知参数，则根据公式(20)可解得θ₃的值：

θ₃所在象限可根据

和

的符号决定；

步骤2：求解机器人的腕部前两个关节角度θ₄和θ₅

将θ₁，θ₂，θ₃带入公式(1)，并将已知项移到公式(1)的左边，可得：

将式(24)两边同乘以轴6上的一点

且r₆≠r₄，可消去一项，方程变为：

根据旋量理论性质中的距离相等公式可知：

||c-r₅||＝||r₆-r₅|| (26)；

其中，

r₄₀表示轴4上不同于r₄的任意点，参照θ₁的求解过程可直接给出θ₄的表达式：

其中，

均为已知参数，且

将θ₄的值带入

中后可获得c的值，而c还可以表示为：

公式(28)与公式(19)形式相同，则参考θ₃的求解过程，可直接得出θ₅的表达式：

其中，

均为已知参数，θ₅的象限可根据

和

的符号决定；

步骤3：求解机器人的末端关节角θ₆

将前面计算得到的θ₁，θ₂，θ₃，θ₄和θ₅带入公式(1)，并将已知项移到公式(1)的左边，可得：

将式(30)两边同乘以轴6以外的点，这里取点p，可得：

其中，

易得：

公式(32)与公式(19)形式相同，则可直接给出角度θ₆的表达式：

其中，

均为已知参数，θ₆的象限可根据

和

的符号决定。

Images