机器人 / 系统课时
分离位置与姿态求球腕逆解
分离位置与姿态求球腕逆解
给定六轴机械臂的目标末端位置和方向,直接联立六个关节角往往难以求解。对后三根关节轴交于一点的球腕结构,可以先回推腕心、求前三轴位置,再求后三轴姿态。本篇给出可运行的八候选求解与回代检查示例。
学习目标
- 判断球腕分解的几何前提,并从目标位姿计算腕心。
- 用平面双连杆几何和 ZYZ 旋转分解求出关节角候选解。
- 按关节限位、当前姿态距离与正运动学误差筛选结果。
确认球腕条件并回推腕心
球腕要求第 4、5、6 轴的转动轴线交于同一固定点 。这些关节改变工具方向,却不改变 在基座坐标系中的位置。这里进一步约定:第 6 坐标系原点 位于腕心沿其局部正 轴距离 处,且没有其他横向偏置。
设目标位姿为位置 和旋转矩阵 ,则:
这一步必须用目标方向旋转偏移向量,不能直接从世界坐标的 分量减去 。如果给定的是工具中心点(TCP)位姿,应先用已标定的法兰到 TCP 变换换算出第 6 坐标系目标,再回推腕心。

用腕心位置求前三个关节角
采用一个明确的示例机型:第 1 轴绕基座 轴旋转,第 2、3 轴绕各自局部负 轴转动;肩部高度 m,两段连杆长度 m、 m。对腕心 ,先计算 、。
基座角有两个候选: 时取有符号半径 ; 增加 时取 。每种基座方向再用余弦定理求肘部双解:
若 且超过数值容差,说明腕心在该两连杆模型中不可达;在容差范围内才可把 截断到 。后面的示例腕心约为 m,对应 m、 m、,故肘角为约 。

利用剩余旋转求腕部三轴
对前三轴的每个位置解,计算腕心处第 3 坐标系方向。本模型采用 ,球腕采用 ZYZ 顺序:
将 的矩阵元素记为 。对非奇异腕部,取:
另一组等效姿态解为 ,需将角度归一化并检查限位。因此一般位置会形成基座两支、肘部两支、腕部两支,最多八组代数候选;具体机型存在肩偏置时,前三轴公式要按它的真实运动学重推。
当 接近零时发生腕部奇异: 仅约束 , 仅约束 。不能继续使用上述两个 atan2 独立恢复角度,应固定一个腕角(例如取接近当前值)再求另一个。
运行完整的候选解算例
环境为 Python 3.10+、NumPy;可通过 python -m pip install numpy 安装。下面固定连杆长度和球腕转轴顺序,先由已知关节角生成一个合法目标位姿,再完全只用目标位置、旋转矩阵求逆解,以便进行闭环验算。保存为 wrist_ik.py 后运行 python wrist_ik.py。
import numpy as np
D1, A2, A3, D6 = 0.4, 0.5, 0.4, 0.1
def rz(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
def ry(a):
c, s = np.cos(a), np.sin(a)
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
def forward(q):
q1, q2, q3, q4, q5, q6 = q
radial = A2 * np.cos(q2) + A3 * np.cos(q2 + q3)
pw = np.array([radial * np.cos(q1), radial * np.sin(q1),
D1 + A2 * np.sin(q2) + A3 * np.sin(q2 + q3)])
r03 = rz(q1) @ ry(-(q2 + q3))
r06 = r03 @ rz(q4) @ ry(q5) @ rz(q6)
return pw + D6 * r06[:, 2], r06
def inverse(p, r06):
# 先沿目标工具轴回推腕心,再枚举基座与肘部支路。
pw = p - D6 * r06[:, 2]
rho = np.hypot(pw[0], pw[1])
h = pw[2] - D1
c3 = (rho**2 + h**2 - A2**2 - A3**2) / (2 * A2 * A3)
if abs(c3) > 1 + 1e-9:
return []
c3 = np.clip(c3, -1, 1)
solutions = []
for base_sign in (1, -1):
q1 = np.arctan2(pw[1], pw[0]) + (0 if base_sign == 1 else np.pi)
r = base_sign * rho
for elbow_sign in (1, -1):
q3 = elbow_sign * np.arccos(c3)
q2 = np.arctan2(h, r) - np.arctan2(
A3 * np.sin(q3), A2 + A3 * np.cos(q3))
r03 = rz(q1) @ ry(-(q2 + q3))
r36 = r03.T @ r06
sy = np.hypot(r36[0, 2], r36[1, 2])
# 奇异腕支路需另外求耦合角,此简例不强行拆分。
if sy < 1e-8:
continue
q4 = np.arctan2(r36[1, 2], r36[0, 2])
q5 = np.arctan2(sy, r36[2, 2])
q6 = np.arctan2(r36[2, 1], -r36[2, 0])
for flip in (False, True):
q = np.array([q1, q2, q3, q4 + (np.pi if flip else 0),
-q5 if flip else q5,
q6 + (np.pi if flip else 0)])
solutions.append((q + np.pi) % (2 * np.pi) - np.pi)
return solutions
# 用正运动学生成可复核的目标,逆解过程不读取原关节角。
target_q = np.deg2rad([30, 20, -40, 40, 50, -30])
p, r06 = forward(target_q)
solutions = inverse(p, r06)
limits = np.array([[-170, 170], [-120, 120], [-150, 150],
[-170, 170], [-120, 120], [-175, 175]])
valid = [q for q in solutions if np.all(
(np.rad2deg(q) >= limits[:, 0]) & (np.rad2deg(q) <= limits[:, 1]))]
if not valid:
raise RuntimeError("没有符合关节限位的非奇异候选解")
# 用相对于当前关节角的最短角差选解,最后回代检查。
seed = np.deg2rad([28, 18, -35, 38, 48, -25])
angle_diff = lambda a, b: (a - b + np.pi) % (2 * np.pi) - np.pi
best = min(valid, key=lambda q: np.linalg.norm(angle_diff(q, seed)))
p_check, r_check = forward(best)
print("腕心位置(m):", np.round(p - D6 * r06[:, 2], 4))
print("代数候选数:", len(solutions))
print("关节限位内候选数:", len(valid))
print("选中关节角(度):", np.round(np.rad2deg(best), 1))
print("位置误差小于1e-9m:", np.linalg.norm(p_check - p) < 1e-9)
print("旋转矩阵误差小于1e-9:", np.linalg.norm(r_check - r06) < 1e-9)预期输出:
腕心位置(m): [0.7324 0.4229 0.4342]
代数候选数: 8
关节限位内候选数: 3
选中关节角(度): [ 30. 20. -40. 40. 50. -30.]
位置误差小于1e-9m: True
旋转矩阵误差小于1e-9: True在真机执行前筛选与复核
八组是几何上的候选数,不代表八组都可以执行。本例按各轴限位留下三组,再选与当前关节配置最接近的一组。回代误差用目标末端位置的欧氏距离和目标旋转矩阵的差异检验;示例中的 仅是数值闭环测试阈值,不是机器人实际标定精度。
实际集成时,按以下顺序执行:先验证目标旋转矩阵正交且行列式接近 ,再用真实连杆尺寸求候选并排除超限、奇异或碰撞配置;随后用机器人真实正运动学回代位置和姿态,最后结合当前关节角及规划器检查运动路径。对多圈关节还要保留允许的旋转圈数,不能直接照搬示例的 归一化方式。
这套闭式解依赖特定球腕结构、坐标轴定义和准确的 TCP 标定。如果第 4 至第 6 轴并非共点,或前臂带有与示例不同的偏置,就应使用该机器人的完整运动学模型求解,不能只修改连杆长度后沿用这些三角公式。