机器人 / 系统课时
用雅可比迭代求末端目标位姿
用雅可比迭代求末端目标位姿
给定机械臂末端期望的位置和朝向,需要反推出一组关节角。直接求解析解并不总是方便,数值迭代又容易在伸直位形附近产生过大的关节变化。这里用平面三旋转关节机械臂演示:构造位姿误差,利用阻尼雅可比逐步更新,并以明确阈值判断是否成功。
学习目标
- 将目标位姿拆成位置误差和最短角度误差。
- 计算几何雅可比,并用阻尼最小二乘更新关节角。
- 设置步长、误差阈值及最大迭代次数,识别未收敛结果。
把位置和朝向写成同一误差向量
取平面三旋转关节(3R)机械臂,连杆长度依次为 0.60、0.45、0.25 m,关节角 均以弧度表示。每段连杆的绝对方向是 ,末端正运动学为:
目标为 。位置直接相减;朝向不能直接采用未归一化的角度差,否则 179° 到 -179° 会被误判为转动 -358°。用下式取得 内的最短有符号旋转误差:
位置的单位是米、旋转的单位是弧度,因此示例给旋转误差乘以 ,构造 。从 出发,当前位姿约为 ;目标位姿为 ,初始加权误差约为 。
用雅可比和阻尼求关节增量
每个关节影响其后全部连杆,所以雅可比的第 列可由下游连杆贡献求和:
将雅可比第三行也乘以 0.30,得到 。直接使用伪逆 可以迭代,但机械臂接近完全伸直时矩阵会病态,关节增量可能过大。这里使用阻尼最小二乘:
取 ,并把每次各关节的变化限制在 rad,再更新 。阻尼降低奇异位置附近的数值放大,但不保证不可达目标收敛;步长限制可以减少一次更新跨过目标的风险。

用 NumPy 实现完整迭代
环境使用 Python 3.10+、NumPy 1.24+;先运行 python -m pip install numpy。为保证目标可达,示例先用一组已知关节角生成目标位姿,再从另一组初始角迭代求解。成功条件是位置误差小于 0.0001 m 且朝向误差小于 0.001 rad,最多执行 100 次关节更新。
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")预期输出:
目标: x=1.2129 m, y=0.1926 m, phi=0.5000 rad
求解: 4 次迭代, 收敛=True
位置误差: 0.000025 m
角度误差: 0.000100 rad这里返回的关节角不要求与生成目标时的角度完全相同;逆运动学可能存在多解。验收依据应是重新执行正运动学后的末端误差,而非关节向量是否与某个参考解一致。
处理奇异、限位与未收敛
将初始角改为全零,可测试完全伸直的奇异附近行为。此时 不能独立控制所有微小的位姿变化,普通伪逆容易对误差敏感;保留 的阻尼、0.20 rad 的单关节步长限制,并检查最后是否真正满足两个阈值。若返回 False,只能报告未收敛,不能把最后一组角度当作有效目标配置。
本示例没有关节行程、碰撞和自碰撞约束。实际机械臂需要为每个关节设置最小/最大角度,对候选更新执行限位检查,再重新计算末端误差;若受限后持续无改进,应报告不可达或受限,而不是无限重试。也可在连续 10 次迭代误差几乎不下降时提前停止。
空间机械臂把单一平面朝向误差扩展为三维旋转向量,例如用 (旋转矩阵的对数映射)构造角度误差,并使用同一基坐标系下的 几何雅可比。大角度误差仍要限制单次更新量;成功标准继续分别检查位置、姿态以及实际关节约束。