机器人 / 系统课时
判断目标位姿是否超出工作空间
判断目标位姿是否超出工作空间
给定末端目标后,机械臂即使能够碰到目标位置,也未必能以指定朝向到达。本篇用两段连杆的平面双旋转关节机械臂,依次检查几何距离、关节限位和工具朝向,并用脚本把不可达原因分开报告。
学习目标
- 用连杆长度计算位置可达的几何必要条件。
- 枚举两支逆运动学解,并检查关节角范围。
- 区分位置不可达、位置可达但姿态不可达、完整位姿可达。
用半径筛掉明显越界的目标
以基座为原点,世界 轴向右、 轴向上,连杆长 、。先只看末端位置 ,不考虑关节限位时,必须满足:
因此理想几何工作空间是半径 的环带。目标 D 为 ,距离 ,大于最大伸展长度,直接判为位置不可达。此检查只是必要条件:半径合格不代表关节能转到那里。
加入关节限位并枚举逆解
设第一关节允许 ,第二关节允许 。对半径合格的位置,用余弦定理求第二关节,再分别计算肘上、肘下两支解:
求出的角度要按关节实际编码约定处理后逐个比对限位。目标 C 为 ,虽然距离 通过环带筛选,但两支解的第一关节角约为 ,均超出允许范围,仍是位置不可达。
只因一支逆解越界就拒绝目标也不对;必须检查两支。比如 虽在理论环带内,但需要第二关节弯曲约 ,超过本例的 限位。
单独检查末端的目标朝向
对没有额外腕部关节、且工具安装角偏置为零的平面 2R 机械臂,末端朝向被关节角之和决定:
目标 B 的位置 有两组合法位置逆解,末端朝向分别约为 和 。若指定 ,位置能到,但朝向误差都超出本例的 容差,应标记为位置可达,姿态不可达。目标 A 为 ,可由 、 满足。

用 Python 验证四种目标
下面脚本仅用 Python 3 标准库;输入单位为米和度。它先检查环带,再枚举两支位置逆解,最后把朝向误差折返到 后验收。
import math
L1, L2 = 1.0, 0.7
def wrap(deg):
return (deg + 180) % 360 - 180
def reachability(x, y, target_phi):
r = math.hypot(x, y)
# 不限位时的几何距离是第一道必要条件
if r < abs(L1 - L2) - 1e-9 or r > L1 + L2 + 1e-9:
return "位置不可达:超出连杆几何范围"
c2 = (r * r - L1 * L1 - L2 * L2) / (2 * L1 * L2)
c2 = max(-1.0, min(1.0, c2)) # 抑制边界浮点误差
valid = []
for t2 in (math.acos(c2), -math.acos(c2)):
t1 = math.atan2(y, x) - math.atan2(
L2 * math.sin(t2), L1 + L2 * math.cos(t2)
)
q1, q2 = wrap(math.degrees(t1)), wrap(math.degrees(t2))
if -90 <= q1 <= 90 and -120 <= q2 <= 120:
valid.append((q1, q2))
if not valid:
return "位置不可达:关节限位"
# 圆周角误差不可直接用普通减法的绝对值
for q1, q2 in valid:
if abs(wrap(q1 + q2 - target_phi)) <= 0.5:
return f"位姿可达:θ1={q1:.1f}°,θ2={q2:.1f}°"
return "位置可达,姿态不可达"
targets = [
("A", 1.1791, 0.9998, 70),
("B", 1.20, 0.40, 0),
("C", -1.30, 0, 0),
("D", 1.80, 0, 0),
]
for name, x, y, phi in targets:
print(f"{name}:{reachability(x, y, phi)}")预期输出:
A:位姿可达:θ1=20.0°,θ2=50.0°
B:位置可达,姿态不可达
C:位置不可达:关节限位
D:位置不可达:超出连杆几何范围该判定只覆盖本例的平面双关节模型、角度限位和朝向容差;“位姿可达”表示至少存在一组满足这些条件的关节角,不代表整段运动路径安全。
在实机上校正验收边界
若目标给的是工具中心点 TCP,而运动学模型求的是法兰原点,必须先扣除工具偏置。例如工具沿自身 轴伸出 ,则法兰目标应为 、,不能直接把 TCP 坐标代入法兰逆解。
上线前建议把三个检查分开记录:位置逆解是否存在、姿态是否在允许误差内、关节运动和连杆是否碰撞。可先用 关节限位余量及 障碍物安全余量做教学测试,再依设备精度与安全要求调整。靠近手臂伸直的 位形时还要检查奇异性和速度放大;空间多自由度机械臂则需额外检查 坐标与完整三维姿态。