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

权利要求书

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)形式相同，则可直接给出角度θ₆的表达式：

其中，均为已知参数，θ₆的象限可根据和的符号决定。