机器人 / 系统课时
筛选多组逆解并保持关节连续
筛选多组逆解并保持关节连续
面对同一个末端目标,逆运动学可能返回肘部朝向不同、或角度相差整圈的候选。直接取第一组解,可能让机械臂发生不必要的大幅回转。本篇通过关节限位检查、正运动学复核和运动距离评分,选出能够平滑接续当前关节配置的逆解。
学习目标
- 区分不同逆运动学分支与同一关节角的周期等价表示。
- 按关节限位、末端误差和当前关节距离筛选候选解。
- 实现可运行的逆解筛选程序,并处理连续关节的角度跨界与分支跳变。
同一个末端位置为什么有多组关节解
考虑一台平面双旋转关节机械臂,两段连杆长度均为 1 m。世界坐标系的 X 轴向右、Y 轴向上,角度以逆时针为正,第二关节角相对于第一连杆定义。
设目标末端位置为 m,只约束位置,不约束末端朝向。两组满足目标的关节角分别是:
- 候选 A:,,肘部位于 m。
- 候选 B:,,肘部位于 m。
两种肘部配置都能让末端到达 m,但连杆路径和关节姿态不同。如果机械臂当前处于 ,候选 A 显然更接近当前配置。

还要区分关节的物理角度与角度表示。对于可以连续旋转的关节, 与 表示同一方向,但从当前 出发,实际应优先考虑运动到 ,而不是执行一次 的回转。
对于存在机械限位的旋转关节,则不能直接套用最短圆周角差。只有等价角度落在允许行程内,并且连接当前角度的运动路径不越过限位,才属于可执行候选。
按约束逐层过滤候选解
假设第一关节是连续旋转关节,第二关节是有限位的旋转关节,允许范围为 。机械臂当前位置为 。
逆解候选的检查顺序可以固定为以下步骤:
- 检查关节数量及数值有效性。两关节模型必须返回两个有限实数,不能出现 NaN、无穷大或缺少关节角的候选。
- 处理角度等价关系。连续关节选择与当前实际角度最接近的整圈等价表示;有限位关节则在机械行程内寻找合法等价角,不能通过取模绕过硬限位。
- 检查关节限位。任何关节无法找到合法角度表示时,直接淘汰该候选;实际系统还需检查软限位和执行安全裕量。
- 使用正运动学重新计算末端位置和姿态。与目标的误差超过容差时,淘汰该候选。
- 对剩余候选计算关节运动代价,选择最小值,并将选中的角度交给轨迹规划器。
本例使用的关节代价为:
其中 是完成角度等价处理后的候选关节角, 是当前关节角。两个关节都使用度作为单位,权重均为 1,因此代价单位为度²。
候选 A 展开后是 ,变化量为 ,代价为 。候选 B 的变化量为 ,代价为 ,因此应选择 A。
使用 Python 筛选逆解候选
下面使用 Python 3 标准库实现完整示例。候选 A、B 是同一目标位置的两组解析解;另外人为加入 C、D 两条错误候选,用于验证几何误差与限位筛选是否真正生效。
本例约定有限位关节的候选角已由求解器规范到 ,因此第二关节可以直接进行范围检查。如果实际求解器返回多圈角度,应先枚举位于机械限位内的等价角,再执行检查。
from math import cos, sin, radians, hypot
current = (170.0, -85.0) # 当前关节角,单位:度
target = (-1.0, 1.0) # 目标末端位置,单位:m
candidates = {
'A': (-180.0, -90.0),
'B': (90.0, 90.0),
'C': (0.0, 0.0),
'D': (90.0, 150.0),
}
def nearest(theta, reference):
# 仅对连续关节寻找距离当前角度最近的等价圈数
return reference + (theta - reference + 180) % 360 - 180
def fk(q):
# 平面双连杆正运动学,两段连杆长度均为1m
a, b = map(radians, q)
return cos(a) + cos(a + b), sin(a) + sin(a + b)
valid = []
for label, raw in candidates.items():
q = (nearest(raw[0], current[0]), raw[1])
# 第二关节存在机械限位,不能按连续关节处理
if not -120 <= q[1] <= 120:
print(f'{label}: limit fail')
continue
x, y = fk(q)
error = hypot(x - target[0], y - target[1])
if error > 1e-3:
print(f'{label}: FK fail, error={error:.3f} m')
continue
cost = sum((value - now) ** 2 for value, now in zip(q, current))
valid.append((cost, label, q))
print(f'{label}: joints=({q[0]:.0f}, {q[1]:.0f}), cost={cost:.0f}')
if not valid:
raise RuntimeError('No feasible IK candidate')
# 代价相同时用候选名称保持确定性的选择顺序
_, label, q = min(valid, key=lambda item: (item[0], item[1]))
print(f'selected {label}: ({q[0]:.0f}, {q[1]:.0f})')预期输出:
A: joints=(180, -90), cost=125
B: joints=(90, 90), cost=37025
C: FK fail, error=3.162 m
D: limit fail
selected A: (180, -90)输出说明了四种处理结果。A 的第一关节从 等价展开为 ,因此代价很小;B 几何上有效,但距离当前关节配置过远;C 虽然没有越过关节限位,但末端计算位置是 m,与目标相差约 3.162 m;D 的第二关节达到 ,超过允许的 。
程序使用 0.001 m 作为末端位置容差。对于真实六轴机械臂的完整位姿求解,还应独立检查姿态误差,例如位置误差不超过 1 mm、姿态误差不超过 。这些数值只是示例验收阈值,实际容差必须依据任务精度、标定误差及控制器能力确定。
设有 组候选、 个关节,若每次正运动学计算的成本为 ,则整个候选筛选过程为 ;选择最小代价还需要 时间。本例候选规模很小,直接遍历即可,不需要额外的搜索优化。
在连续控制中保持逆解分支稳定
一次性选择最近解还不够。机械臂沿轨迹连续运动时,逆运动学可能在相邻控制周期返回不同顺序的候选。如果每次直接选择列表第一项,即使末端目标只移动几毫米,也可能突然从一种肘型切换到另一种肘型。
实际控制中应保存上一周期选中的分支标识及关节目标,并使用最新的关节反馈作为距离计算基准。不能每个周期都把连续关节角强行压回 ,否则跨越角度边界时,数值上会出现接近 的虚假跳变。
为减少两个相近候选之间的频繁切换,可以引入分支保持阈值。例如,在本例以度²计算的评分中,设切换改善阈值为 100 度²:只有新分支代价比原分支至少降低超过 100 度²,才允许切换。若原分支不再满足几何目标或安全约束,应立即重新筛选,而不能为了保持连续性继续使用失效解。
最终选中的关节角仍应经过轨迹规划。以本例为例,从 到 ,最大单轴角度变化是 。如果两轴最大允许速度均为 /s,那么仅按速度限制计算,运动时间至少为约 0.167 s;考虑加减速约束后,实际所需时间还会增加。
可用一个连续性测试验收实现:让第一关节从 逐步接近并越过 ,观察返回角度是否保持连续增加,而不是从 突然跳到 。同时检查第二关节是否始终位于 ,并确认所有执行目标均经过正运动学误差验证。
对于完整机器人系统,逆解筛选负责确定合法且接近当前配置的关节目标,轨迹规划负责验证到达该目标的运动过程。两者都通过检查,才能避免把数学上正确的逆解变成实际执行中的姿态跳变或越限动作。