求解6轴串联机器人后三关节逆运动学问题求助

问题核心分析 #

你当前的代码存在公式错误、变量混淆（误写j3应为j6）、三角函数调用不规范以及奇异情况未处理等问题，导致无法得到正确的关节角度。以下是针对性的修正步骤：

关键前提确认 #

腕部（位置a）到末端执行器的相对旋转矩阵计算逻辑正确，但旋转矩阵的逆可替换为转置（利用旋转矩阵正交性），更高效稳定：

R_rel = R0.T @ R1  # 替代np.linalg.inv(R0) @ R1

修正后的逆解代码（适配球腕结构） #

假设你的机器人后三关节为标准球腕结构（j4绕x轴、j5绕y轴、j6绕x轴，与DH表定义一致），修正后的代码如下：

import numpy as np

# 计算腕部到末端的相对旋转矩阵
R_rel = R0.T @ R1

# 求解j5（θ5）
sin_j5 = R_rel[1, 2]
# 数值截断避免浮点误差导致arcsin报错
j5 = np.arcsin(np.clip(sin_j5, -1.0, 1.0))

# 分情况求解j4和j6，处理奇异点
if abs(np.cos(j5)) > 1e-6:  # 非奇异情况，cos(j5)不接近0
    j4 = np.arctan2(-R_rel[0, 2], R_rel[2, 2])
    j6 = np.arctan2(R_rel[1, 0], R_rel[1, 1])
else:
    # 奇异情况（j5=±π/2），j4与j6耦合，固定j4为0（可根据需求调整）
    j4 = 0.0
    if j5 == np.pi / 2:
        j6 = np.arctan2(-R_rel[2, 0], R_rel[0, 0]) - j4
    else:
        j6 = j4 - np.arctan2(R_rel[2, 0], R_rel[0, 0])

# 可选：将角度统一转换到[-π, π]范围
j4 = np.arctan2(np.sin(j4), np.cos(j4))
j5 = np.arctan2(np.sin(j5), np.cos(j5))
j6 = np.arctan2(np.sin(j6), np.cos(j6))

代码修正说明 #

1. 变量修正：原代码中的j3应为j6，属于笔误。
2. 三角函数规范：全部使用numpy的三角函数（np.arcsin/np.arctan2），避免原生math函数的维度不兼容问题。
3. 数值稳定性：用np.clip限制正弦值范围，防止浮点运算误差导致arcsin抛出异常。
4. 奇异点处理：当j5接近±π/2时，球腕进入奇异状态，此时固定一个关节角度（如j4=0）求解另一个，适配实际应用场景。

验证方法 #

将求解得到的j4/j5/j6代入正运动学模型，计算末端姿态并与给定的R1对比，若姿态误差小于1e-6（弧度/米级别），则说明逆解正确。

内容的提问来源于stack exchange，提问作者Nico Waser