机器人 / 系统课时

用雅可比迭代求末端目标位姿

用雅可比迭代求末端目标位姿

给定机械臂末端期望的位置和朝向,需要反推出一组关节角。直接求解析解并不总是方便,数值迭代又容易在伸直位形附近产生过大的关节变化。这里用平面三旋转关节机械臂演示:构造位姿误差,利用阻尼雅可比逐步更新,并以明确阈值判断是否成功。

学习目标

  • 将目标位姿拆成位置误差和最短角度误差。
  • 计算几何雅可比,并用阻尼最小二乘更新关节角。
  • 设置步长、误差阈值及最大迭代次数,识别未收敛结果。

把位置和朝向写成同一误差向量

取平面三旋转关节(3R)机械臂,连杆长度依次为 0.60、0.45、0.25 m,关节角 q=[q1,q2,q3]Tq=[q_1,q_2,q_3]^T 均以弧度表示。每段连杆的绝对方向是 θi=∑k=1iqkθ_i=\sum_{k=1}^{i}q_k,末端正运动学为:

x=∑i=13licos⁡θi,y=∑i=13lisin⁡θi,φ=q1+q2+q3x=\sum_{i=1}^{3}l_i\cos θ_i,\quad y=\sum_{i=1}^{3}l_i\sin θ_i,\quad φ=q_1+q_2+q_3

目标为 (xd,yd,φd)(x_d,y_d,φ_d)。位置直接相减;朝向不能直接采用未归一化的角度差,否则 179° 到 -179° 会被误判为转动 -358°。用下式取得 [−π,π][-π,π] 内的最短有符号旋转误差:

eφ=atan2⁡(sin⁡(φd−φ),cos⁡(φd−φ))e_φ=\operatorname{atan2}(\sin(φ_d-φ),\cos(φ_d-φ))

位置的单位是米、旋转的单位是弧度,因此示例给旋转误差乘以 wφ=0.30 m/radw_φ=0.30\ \mathrm{m/rad},构造 ew=[xd−x, yd−y, 0.30eφ]Te_w=[x_d-x,\ y_d-y,\ 0.30e_φ]^T。从 q=[0.15,−0.25,0.30]q=[0.15,-0.25,0.30] 出发,当前位姿约为 (1.2860,0.0944,0.2000)(1.2860,0.0944,0.2000);目标位姿为 (1.2129,0.1926,0.5000)(1.2129,0.1926,0.5000),初始加权误差约为 [−0.0731,0.0982,0.0900]T[-0.0731,0.0982,0.0900]^T。

用雅可比和阻尼求关节增量

每个关节影响其后全部连杆,所以雅可比的第 ii 列可由下游连杆贡献求和:

Jx,i=−∑k=i3lksin⁡θk,Jy,i=∑k=i3lkcos⁡θk,Jφ,i=1J_{x,i}=-\sum_{k=i}^{3}l_k\sin θ_k,\quad J_{y,i}=\sum_{k=i}^{3}l_k\cos θ_k,\quad J_{φ,i}=1

将雅可比第三行也乘以 0.30,得到 JwJ_w。直接使用伪逆 Δq=Jw+ewΔq=J_w^{+}e_w 可以迭代,但机械臂接近完全伸直时矩阵会病态,关节增量可能过大。这里使用阻尼最小二乘:

Δq=JwT(JwJwT+λ2I)−1ewΔq=J_w^T(J_wJ_w^T+λ^2I)^{-1}e_w

取 λ=0.05λ=0.05,并把每次各关节的变化限制在 [−0.20,0.20][-0.20,0.20] rad,再更新 qq。阻尼降低奇异位置附近的数值放大,但不保证不可达目标收敛;步长限制可以减少一次更新跨过目标的风险。

平面三关节逆运动学中的位姿误差与阻尼迭代闭环

用 NumPy 实现完整迭代

环境使用 Python 3.10+、NumPy 1.24+;先运行 python -m pip install numpy。为保证目标可达,示例先用一组已知关节角生成目标位姿,再从另一组初始角迭代求解。成功条件是位置误差小于 0.0001 m 且朝向误差小于 0.001 rad,最多执行 100 次关节更新。

python
import numpy as np

LINKS = np.array([0.60, 0.45, 0.25])
ORIENTATION_SCALE = 0.30

def fk_and_jac(q):
    theta = np.cumsum(q)
    sin_t, cos_t = np.sin(theta), np.cos(theta)
    pose = np.array([LINKS @ cos_t, LINKS @ sin_t, theta[-1]])

    # 每个转动关节影响自身及其下游连杆
    jac = np.vstack([
        -np.cumsum((LINKS * sin_t)[::-1])[::-1],
        np.cumsum((LINKS * cos_t)[::-1])[::-1],
        np.ones(3),
    ])
    jac[2] *= ORIENTATION_SCALE
    return pose, jac

def angle_error(target, current):
    # 将角度差约束到 [-pi, pi]
    return np.arctan2(np.sin(target - current),
                      np.cos(target - current))

def solve_ik(target, q0, max_iters=100):
    q = q0.copy()
    for iteration in range(max_iters + 1):
        pose, jac = fk_and_jac(q)
        dp = target[:2] - pose[:2]
        da = angle_error(target[2], pose[2])
        if np.linalg.norm(dp) < 1e-4 and abs(da) < 1e-3:
            return q, iteration, True
        if iteration == max_iters:
            break

        error = np.array([dp[0], dp[1], ORIENTATION_SCALE * da])
        # 阻尼使近奇异位形的更新更温和
        damping = 0.05
        dq = jac.T @ np.linalg.solve(
            jac @ jac.T + damping**2 * np.eye(3), error
        )
        q += np.clip(dq, -0.20, 0.20)
    return q, max_iters, False

if __name__ == "__main__":
    target, _ = fk_and_jac(np.array([0.35, -0.65, 0.80]))
    q, iterations, ok = solve_ik(target, np.array([0.15, -0.25, 0.30]))
    actual, _ = fk_and_jac(q)
    print(f"目标: x={target[0]:.4f} m, y={target[1]:.4f} m, phi={target[2]:.4f} rad")
    print(f"求解: {iterations} 次迭代, 收敛={ok}")
    print(f"位置误差: {np.linalg.norm(target[:2]-actual[:2]):.6f} m")
    print(f"角度误差: {abs(angle_error(target[2], actual[2])):.6f} rad")

预期输出:

text
目标: x=1.2129 m, y=0.1926 m, phi=0.5000 rad
求解: 4 次迭代, 收敛=True
位置误差: 0.000025 m
角度误差: 0.000100 rad

这里返回的关节角不要求与生成目标时的角度完全相同;逆运动学可能存在多解。验收依据应是重新执行正运动学后的末端误差,而非关节向量是否与某个参考解一致。

处理奇异、限位与未收敛

将初始角改为全零,可测试完全伸直的奇异附近行为。此时 JwJ_w 不能独立控制所有微小的位姿变化,普通伪逆容易对误差敏感;保留 λ=0.05λ=0.05 的阻尼、0.20 rad 的单关节步长限制,并检查最后是否真正满足两个阈值。若返回 False,只能报告未收敛,不能把最后一组角度当作有效目标配置。

本示例没有关节行程、碰撞和自碰撞约束。实际机械臂需要为每个关节设置最小/最大角度,对候选更新执行限位检查,再重新计算末端误差;若受限后持续无改进,应报告不可达或受限,而不是无限重试。也可在连续 10 次迭代误差几乎不下降时提前停止。

空间机械臂把单一平面朝向误差扩展为三维旋转向量,例如用 eR=Log⁡(RdR(q)T)∨e_R=\operatorname{Log}(R_dR(q)^T)^\vee(旋转矩阵的对数映射)构造角度误差,并使用同一基坐标系下的 6×n6\times n 几何雅可比。大角度误差仍要限制单次更新量;成功标准继续分别检查位置、姿态以及实际关节约束。

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

目录与阅读设置

阅读设置

17

记录一个想法

用雅可比迭代求末端目标位姿

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

登录哪都通