机器人 / 系统课时

约束关节限位与迭代步长

约束关节限位与迭代步长

数值逆运动学算出的关节增量可能在数学上接近目标,却穿过机械限位;迭代过猛还会造成误差回升和来回振荡。下面用一台平面双旋转关节机械臂,把关节边界、最大单步角和误差回溯放进同一个求解循环,并分别验证可达与受限目标。

学习目标

  • 将关节上下限转换为求解器实际检查的角度边界。
  • 用整体步长缩放、限位投影和回溯搜索抑制越界与误差反弹。
  • 从末端误差和停止理由区分收敛、停滞与迭代超限。

将机械限位转换成数值边界

假设平面两段连杆长1.0m、0.8m,关节均绕纸面法线旋转。机械硬限位暂设为关节1的-65°~65°、关节2的-5°~105°;各侧预留5°后,求解器只允许关节1在-60°~60°、关节2在0°~100°内变化。

约束项 本例设置 检查标准
关节1软件限位 -60°~60° 更新后角度不得越界
关节2软件限位 0°~100° 更新后角度不得越界
单次关节角变化 最多5° 两关节分别都满足
末端位置误差 小于0.001m 才报告收敛
迭代次数 最多200次 超限报告失败

计算内部统一用弧度,只有输入与日志展示时换算为角度。上述机械硬限位是教学假设;实际项目应从机器人规格或URDF关节限制读取,再按速度、制动距离和标定误差留余量。求解器的软件裁剪不能代替驱动器限位和急停保护。

从大增量得到可接受的小步

以二维末端位置误差 e=p∗−f(q)e=p^*-f(q) 为输入,先用阻尼最小二乘计算关节增量:

Δq=(JTJ+λ2I)−1JTe\Delta q=(J^\mathsf{T}J+\lambda^2I)^{-1}J^\mathsf{T}e

本例取阻尼系数 λ=0.08\lambda=0.08。它能缓解雅可比病态导致的过大增量,但不会自动遵守关节范围。初始角度(-15°,20°)、目标(1.25,0.72)m时,原始增量约为(-49.05°,173.00°),显然不能直接执行。

先按最大角度分量等比例缩小整个增量向量:本次比例为5/173≈0.0289,得到约(-1.42°,5.00°)。不要把每个关节独立截成±5°,那会改变原始搜索方向,甚至使误差增大。之后按顺序执行:

  1. 设置回溯系数 α=1\alpha=1,形成候选角度 q+αΔqq+\alpha\Delta q。
  2. 把每个候选角度投影到各自的软件限位区间。
  3. 对投影后的角度重新运行正运动学,计算实际末端距离误差。
  4. 误差下降才接受;否则令 α\alpha 减半,最多尝试1、1/2、1/4直到1/128。全部失败则停止并报告停滞,而不是把越界或不下降的角度强行提交。

关节限位、单步增量缩放与误差回溯闭环

运行一个带关节约束的逆解器

以下Python示例只依赖标准库,按平面双关节的解析雅可比计算阻尼增量。位置误差在每次投影后重算;只有实际误差严格降低才接收候选角度。将代码保存为bounded_ik.py,使用Python 3.9或更新版本执行。

python
import math

L1, L2 = 1.0, 0.8
LOWER = [math.radians(-60), math.radians(0)]
UPPER = [math.radians(60), math.radians(100)]
MAX_STEP = math.radians(5)
DAMPING = 0.08
TOL = 1e-3

def fk(q):
    a, b = q
    return (L1 * math.cos(a) + L2 * math.cos(a + b),
            L1 * math.sin(a) + L2 * math.sin(a + b))

def dls(q, error):
    a, b = q
    j11 = -L1 * math.sin(a) - L2 * math.sin(a + b)
    j12 = -L2 * math.sin(a + b)
    j21 = L1 * math.cos(a) + L2 * math.cos(a + b)
    j22 = L2 * math.cos(a + b)
    # 求解 (J^T J + λ²I) Δq = J^T e 的 2×2 方程
    a11 = j11 * j11 + j21 * j21 + DAMPING ** 2
    a12 = j11 * j12 + j21 * j22
    a22 = j12 * j12 + j22 * j22 + DAMPING ** 2
    b1 = j11 * error[0] + j21 * error[1]
    b2 = j12 * error[0] + j22 * error[1]
    det = a11 * a22 - a12 * a12
    return [(a22 * b1 - a12 * b2) / det,
            (a11 * b2 - a12 * b1) / det]

def solve(target):
    q = [math.radians(-15), math.radians(20)]
    for k in range(201):
        x, y = fk(q)
        error = (target[0] - x, target[1] - y)
        distance = math.hypot(*error)
        if distance < TOL:
            return "收敛", k, q, distance
        if k == 200:
            return "达到迭代上限", k, q, distance

        dq = dls(q, error)
        # 按最大分量整体缩放,避免分别截断两个关节而扭曲方向
        biggest = max(abs(v) for v in dq)
        scale = min(1.0, MAX_STEP / biggest) if biggest else 1.0
        dq = [scale * v for v in dq]
        for backtrack in range(8):
            alpha = 0.5 ** backtrack
            # 先投影到限位,再以实际末端误差决定是否接受
            trial = [min(UPPER[i], max(LOWER[i], q[i] + alpha * dq[i]))
                     for i in range(2)]
            tx, ty = fk(trial)
            if math.hypot(target[0] - tx, target[1] - ty) < distance - 1e-12:
                q = trial
                break
        else:
            return "停滞", k, q, distance

for target in ((1.25, 0.72), (1.0, -1.2)):
    status, steps, angles, error = solve(target)
    degrees = [round(math.degrees(a), 2) for a in angles]
    print(f"目标={target} 状态={status} 步数={steps} 角度={degrees} 误差={error:.4f}m")

预期输出:

text
目标=(1.25, 0.72) 状态=收敛 步数=14 角度=[-2.28, 74.01] 误差=0.0001m
目标=(1.0, -1.2) 状态=停滞 步数=11 角度=[-60.0, 66.01] 误差=0.5117m

第一组目标用14步到达约(-2.28°,74.01°),位置误差0.0001m,小于设定的0.001m。第二组虽然没有走出关节范围,却在第11步停滞:关节1已到-60°边界,残余位置误差仍为0.5117m,必须按失败处理。

判断受限目标为何无法收敛

第二组目标(1.0,-1.2)m到基座的距离约1.562m,处于两连杆不考虑关节限位时的0.2~1.8m几何工作区间内。其两组无约束解析逆解约为(-76.52°,60°)和(-23.86°,-60°):前者违反关节1下限,后者违反关节2下限,因此这次确实是限位排除了几何上可达的目标。

若日志出现误差交替升降,先确认每次验收的是投影后的位置误差;再将单步上限从5°降到2°,或适度增加阻尼,并记录迭代序号、误差、实际角度和回溯次数。若连续碰到同一关节限位,应检查目标可行性,而不是无限增大迭代次数。

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

目录与阅读设置

阅读设置

17

记录一个想法

约束关节限位与迭代步长

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

登录哪都通