机器人 / 系统课时

判断目标位姿是否超出工作空间

判断目标位姿是否超出工作空间

给定末端目标后,机械臂即使能够碰到目标位置,也未必能以指定朝向到达。本篇用两段连杆的平面双旋转关节机械臂,依次检查几何距离、关节限位和工具朝向,并用脚本把不可达原因分开报告。

学习目标

  • 用连杆长度计算位置可达的几何必要条件。
  • 枚举两支逆运动学解,并检查关节角范围。
  • 区分位置不可达、位置可达但姿态不可达、完整位姿可达。

用半径筛掉明显越界的目标

以基座为原点,世界 xx 轴向右、yy 轴向上,连杆长 l1=1.0 ml_1=1.0\,\text{m}、l2=0.7 ml_2=0.7\,\text{m}。先只看末端位置 (x,y)(x,y),不考虑关节限位时,必须满足:

∣l1−l2∣≤r=x2+y2≤l1+l2|l_1-l_2|\le r=\sqrt{x^2+y^2}\le l_1+l_2

因此理想几何工作空间是半径 0.3∼1.7 m0.3\sim1.7\,\text{m} 的环带。目标 D 为 (1.80,0)(1.80,0),距离 1.80 m1.80\,\text{m},大于最大伸展长度,直接判为位置不可达。此检查只是必要条件:半径合格不代表关节能转到那里。

加入关节限位并枚举逆解

设第一关节允许 θ1∈[−90∘,90∘]\theta_1\in[-90^\circ,90^\circ],第二关节允许 θ2∈[−120∘,120∘]\theta_2\in[-120^\circ,120^\circ]。对半径合格的位置,用余弦定理求第二关节,再分别计算肘上、肘下两支解:

c2=x2+y2−l12−l222l1l2,θ2=±arccos⁡(c2)c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2},\quad\theta_2=\pm\arccos(c_2)
θ1=atan2⁡(y,x)−atan2⁡(l2sin⁡θ2,l1+l2cos⁡θ2)\theta_1=\operatorname{atan2}(y,x)-\operatorname{atan2}(l_2\sin\theta_2,l_1+l_2\cos\theta_2)

求出的角度要按关节实际编码约定处理后逐个比对限位。目标 C 为 (−1.30,0)(-1.30,0),虽然距离 1.30 m1.30\,\text{m} 通过环带筛选,但两支解的第一关节角约为 ±147.8∘\pm147.8^\circ,均超出允许范围,仍是位置不可达。

只因一支逆解越界就拒绝目标也不对;必须检查两支。比如 (0.60,0)(0.60,0) 虽在理论环带内,但需要第二关节弯曲约 143.8∘143.8^\circ,超过本例的 120∘120^\circ 限位。

单独检查末端的目标朝向

对没有额外腕部关节、且工具安装角偏置为零的平面 2R 机械臂,末端朝向被关节角之和决定:

ϕ=θ1+θ2\phi=\theta_1+\theta_2

目标 B 的位置 (1.20,0.40)(1.20,0.40) 有两组合法位置逆解,末端朝向分别约为 70.4∘70.4^\circ 和 −33.6∘-33.6^\circ。若指定 ϕ=0∘\phi=0^\circ,位置能到,但朝向误差都超出本例的 ±0.5∘\pm0.5^\circ 容差,应标记为位置可达,姿态不可达。目标 A 为 (1.1791,0.9998,70∘)(1.1791,0.9998,70^\circ),可由 θ1≈20∘\theta_1\approx20^\circ、θ2≈50∘\theta_2\approx50^\circ 满足。

平面双关节机械臂的几何工作空间、限位与朝向可达性

用 Python 验证四种目标

下面脚本仅用 Python 3 标准库;输入单位为米和度。它先检查环带,再枚举两支位置逆解,最后把朝向误差折返到 [−180∘,180∘)[-180^\circ,180^\circ) 后验收。

python
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)}")

预期输出:

text
A:位姿可达:θ1=20.0°,θ2=50.0°
B:位置可达,姿态不可达
C:位置不可达:关节限位
D:位置不可达:超出连杆几何范围

该判定只覆盖本例的平面双关节模型、角度限位和朝向容差;“位姿可达”表示至少存在一组满足这些条件的关节角,不代表整段运动路径安全。

在实机上校正验收边界

若目标给的是工具中心点 TCP,而运动学模型求的是法兰原点,必须先扣除工具偏置。例如工具沿自身 xx 轴伸出 0.10 m0.10\,\text{m},则法兰目标应为 xf=xTCP−0.10cos⁡ϕx_f=x_{TCP}-0.10\cos\phi、yf=yTCP−0.10sin⁡ϕy_f=y_{TCP}-0.10\sin\phi,不能直接把 TCP 坐标代入法兰逆解。

上线前建议把三个检查分开记录:位置逆解是否存在、姿态是否在允许误差内、关节运动和连杆是否碰撞。可先用 2∘2^\circ 关节限位余量及 5 mm5\,\text{mm} 障碍物安全余量做教学测试,再依设备精度与安全要求调整。靠近手臂伸直的 θ2=0∘\theta_2=0^\circ 位形时还要检查奇异性和速度放大;空间多自由度机械臂则需额外检查 zz 坐标与完整三维姿态。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

判断目标位姿是否超出工作空间

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通