一种任意关系的二阶子问题逆运动学求解方法

摘要

本发明公开了一种任意关系的二阶子问题逆运动学求解方法，属于机器人逆运动学领域，本发明涉及二阶子问题RR的逆解方法，该方法在指数积模型的基础上，利用旋量理论的基本性质和Rodrigues旋转矩阵表达，将几何方法与代数方法结合起来给出一种通用的关节角求解公式，不需要考虑关节轴线之间的关系，无论是相交、平行、还是异面都可以利用这种方法直接求出。本发明对机器人逆解的求解方法进行了拓展，扩大了适应范围，简化了求解过程，为机器人在实际的开发和应该提供了方便。

权利要求

1.一种任意关系的二阶子问题逆运动学求解方法，其特征在于，包括如下步骤：步骤1：求θ1

θ1是机器人第1关节绕轴旋转的角度；

二阶子问题RR可用公式表示为其中， 是p,q的齐次坐标，p,q分别是空间两点，前者是旋转前的点，后者是旋转后的点； i＝1,2由第i关节轴的轴方向向量ωi和轴上一点ri组成，这些参数均已知，根据旋量理论的距离相等原则可知：||c-r2||＝||p-r2||        (5)；

将 带入上式，

其中： 是单位方向向量 的反对称矩阵，可表示为：并利用 的Rodrigues旋转公式将其化简成关于θ1的三角函数方程：x1sinθ1+y1cosθ1＝z1      (9)；

其中 为已知参数，

从公式(9)可解得θ1的表达式：其中，

步骤2：求θ2

θ2是机器人第2关节绕轴旋转的角度；

根据已知的θ1可得c的值，而c还可表示为：其中， 是单位方向向量 的反对称矩阵，可表示为：将 的Rodrigues旋转公式带入上式整理可得：x2sinθ2+y2cosθ2＝z2     (14)；

其中 均为已知参数，

从公式(14)中可解的θ2的表达式：θ2角度的具体象限由 和 的符号决定，当相邻两关节相交的时候，两关节轴上的点r1和r2，必须满足r1≠r2≠r0，其中r0是两条轴线的交点。

说明书

一种任意关系的二阶子问题逆运动学求解方法

技术领域

[0001] 本发明属于机器人逆运动学领域，具体涉及一种任意关系的二阶子问题逆运动学求解方法。

背景技术

[0002] Paden-Kanhan子问题在机器人逆运动学应用非常广泛，因为它具有几何意义和数值稳定性，能够灵活的为多种机器人提供封闭解。Paden-Kanhan子问题主要分为三类：一阶子问题，二阶子问题，三阶子问题。其中一阶子问题是针对单关节的转动R或平移T运动的逆解问题；二阶子问题是针对两个关节逆解问题，包含了3种情况：RR，TT，RT/TR，其中RR又分为相交、平行、异面垂直等不同的类型；三阶子问题是针对三个关节的逆解问题，包含了6种情况。在实际中，由于加工、装配很多几何关系很难保证，比如：相交、平行，而且不同的结构需要选择不同的公式，这为实际应用带来很多不便。

发明内容

[0003] 针对现有技术中存在的上述技术问题，本发明提出了一种任意关系的二阶子问题逆运动学求解方法，设计合理，克服了现有技术的不足，具有良好的效果。

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

[0005] 一种任意关系的二阶子问题逆运动学求解方法，包括如下步骤：

[0006] 步骤1：求θ1

[0007] 二阶子问题RR可用公式表示为

[0008]

[0009] 其中， 是p,q的齐次坐标， 由第i关节轴的轴方向向量ωi和轴上一点ri组成，这些参数均已知。根据旋量理论的距离相等原则可知：

[0010] ||c-r2||＝||p-r2||              (5)；

[0011] 将 带入上式，并利用 的Rodrigues旋转公式将其化简成关于θ1的三角函数方程：

[0012] x1sinθ1+y1cosθ1＝z1             (9)；

[0013] 其中 为已知参数，从上式可解得θ1的表达式：

[0014]

[0015] 上式中需要通过调整r1和r2来保证

[0016] 步骤2：求θ2

[0017] 根据已知的θ1可得c的值，而c还可表示为：

[0018]

[0019] 将 的Rodrigues旋转公式带入上式整理可得：

[0020] x2sinθ2+y2cosθ2＝z2          (14)；

[0021] 其中 均为已知参数，从上式中可解的θ2的表达式：

[0022]

[0023] θ2角度的具体象限由 和 的符号决定，需注意的是当相邻两关节相交的时候，两关节轴上的点r1和r2，必须满足r1≠r2≠r0，其中r0是两条轴线的交点。

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

[0025] 1、计算效率高，给出了关节角度的封闭解，可利反三角函数直接求出，具有很高的计算效率；2、实现简单，每个关节的表达形式非常简单易懂，只需求解一次反三角函数即可；3、应用范围广，可应用于任意2R机器人中，不需要考虑其轴线之间的几何关系。

附图说明

[0026] 图1为任意关系的RR结构图。

具体实施方式

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

[0028] 一种任意关系的二阶子问题逆运动学求解方法，包括如下步骤：

[0029] 步骤1：求θ1

[0030] 如图1所示，二阶子问题RR可用公式表示为

[0031]

[0032] 其中，和 是空间点p和q的齐次坐标表示，且 点为初始点，绕轴ω2转θ2到点c，c点绕ω1旋转θ1到点q， 为运动旋量，由关节轴的单位方向向量 和轴上的任意一点构成， 是刚体变换的指数表达，对于转动关节其表达式为：

[0033]

[0034] 其中，I3×3为3×3的单位矩阵， 是旋转矩阵，可用Rodrigues表示为：

[0035]

[0036] 其中，是单位方向向量ω＝[ωx,ωy,ωz]T的反对称矩阵，可表示为：

[0037]

[0038] 根据旋量理论的距离相等原则可知：

[0039] ||c-r2||＝||p-r2||          (5)；

[0040] 根据旋量理论的基本原理可知：

[0041]

[0042] 上述两式相减可得：

[0043]

[0044] 将指数积公式 的表达式(2)带入式(6)可得：

[0045]

[0046] 将公式(7)带入公式(5)可得：

[0047]

[0048] 再将 的Rodrigues表达(3)带入式(8)，两边平方后，整理可得：

[0049] x1sinθ1+y1cosθ1＝z1           (9)；

[0050] 其中

[0051]

[0052]

[0053]

[0054] 设x1＝ρcosφ，y1＝ρsinφ，则 利用三角函数的积化和差公式，公式(9)可变为：

[0055]

[0056] 其中， 同理可以得到：

[0057]

[0058] 则关节角度θ1可表示为：

[0059]

[0060] 上式中需要通过调整r1和r2来保证

[0061] 步骤2：求θ2

[0062] 将θ1的值带入公式(7)中可得c的值，而c还可表示为：

[0063]

[0064] 将 的Rodrigues表达(3)带入式(13)，整理可得：

[0065] x2sinθ2+y2cosθ2＝z2          (14)；

[0066] 其中

[0067]

[0068]

[0069]

[0070] 由于 则在公式(14)两边分别同乘以 和 可得：

[0071]

[0072]

[0073] 则θ2可表示为：

[0074]

[0075] θ2角度的具体象限由 和 的符号决定，需要注意的是当相邻两关节相交的时候，两关节轴上的点r1和r2，必须满足r1≠r2≠r0，其中r0是两条轴线的交点。

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