[发明专利]一种五自由度串联机器人的逆运动学通用求解方法

摘要

本发明公开了一种五自由度串联机器人的逆运动学通用求解方法，属于机器人逆运动学领域，本发明在指数积模型的基础上提出一种形式简单、易于计算的逆运动学通用求解方法，该发明主要将旋量理论的基本性质和Rodrigues旋转表达相结合简化了逆解求解过程，实现了在满足Pieper约束的条件下，不需要考虑前两个关节轴线之间的关系即可直接获得各关节的角度值，而且5个关节角度只需要2个表达式即可统一表达，为该类机器人在实际应用中提供了方便。

主权项

1.一种五自由度串联机器人的逆运动学通用求解方法，其特征在于，包括如下步骤：步骤1：求解机器人的肩部关节角度θ1根据指数积模型，机器人运动学方程可表示为：且其中，下标t和w分别表示末端工具坐标系与世界坐标系，θ是各关节的旋转角度向量θ＝[θ₁,...,θ₅]，g_wt(0)和g_wt(θ)分别表示在初始状态下和θ状态下末端工具坐标系相对世界坐标系的变换关系，为第i关节的运动旋量，包括关节轴的单位方向向量ω_i和轴上的任意一点r_i，ω_i和r_i被称为旋量参数，为第i关节坐标变换的指数表达形式，为是旋转矩阵的指数表达形式，其Rodrigues表达形式为：空间任一向量p的齐次坐标用表示；利用消元法消去机器人的腕部关节，设r₃是腕部关节的交点，将公式(1)两边同乘以可得：其中根据旋量理论中的距离相等原则可知：||c‑r2||＝||p‑r2||                  (6)；将带入公式(6)，两边平方后进行整理，并利用的Rodrigues旋转表达式将其化简为关于θ₁的三角函数公式：x1sinθ1+y1cosθ1＝z1                 (11)；其中x₁,y₁,为已知参数，则根据公式(11)可解得θ₁为：其中步骤2：求解机器人的肘部关节角度θ2将θ₁的值带入中可获得c的值，而c还可表示为：将的Rodrigues表达式(3)带入式(15)，整理可得：x2sinθ2+y2cosθ2＝z2              (16)；其中x₂，y₂，均已知，则根据公式(16)可解得θ₂的表达式为：θ₂角度的具体象限由和的符号决定；步骤3：求解机器人的腕部前两个关节角度θ3和θ4将θ1和θ2带入公式(1)，并将已知项移到公式(1)的左边，可得：将式(20)两边同乘以且r₅≠r₃，易知可得：其中，可见公式(21)与公式(5)的形式相同，则根据θ₁和θ₂的表达式可给出θ₃和θ₄的表达式；其中，且θ₄所在象限由和的符号来决定；步骤4：求解机器人的腕部末端关节角度θ5将θ1、θ2、θ3和θ4带入公式(1)，并将已知项移到公式(1)的左边，可得：将式(24)两边同乘以除r₅以外的点，这里取点r₄，其齐次坐标为可得：其中，易得：公式(26)与公式(15)形式相同，则可直接得出角度θ5的表达式其中，x₅,y₅,均已知，且θ₅所在象限由和的符号来决定。