机器人 / 系统课时

求解逆运动学并识别多组解

求解逆运动学并识别多组解

给定抓取点在基座坐标系中的位置和工具朝向,逆运动学要找出一组可执行的六关节角。本篇用理想化球腕六轴机械臂,先求腕心,再枚举肘部与腕部解,并用初始姿态、关节限位和正运动学检查结果。

学习目标

  • 将 TCP 目标位姿拆解为腕心位置与腕部姿态。
  • 计算肘部两组几何解,判断无解和腕翻转分支。
  • 运行完整示例,以初始姿态筛选六轴解并验证误差。

从 TCP 位姿回推腕心

采用便于复算的理想化六轴模型:J1 绕基座竖直 Z 轴旋转,J2、J3 控制肩肘俯仰,J4~J6 的转轴交于同一腕心,腕部姿态按 Z-Y-Z 旋转分解。上臂 A=0.40A=0.40 m,前臂 B=0.30B=0.30 m,肩高 H=0.20H=0.20 m,腕心到 TCP 沿末端局部 Z 轴的距离 d6=0.10d_6=0.10 m。

目标不只是一个三维坐标,而是 TCP 位置 pTCPp_{TCP} 和相对于基座的旋转矩阵 R06R_{06}。由于球腕转动不改变腕心位置,可以先扣除工具轴向偏置:

pw=pTCP−d6R06ez,ez=(0,0,1)Tp_w=p_{TCP}-d_6R_{06}e_z,\qquad e_z=(0,0,1)^T

这里 pw=(xw,yw,zw)p_w=(x_w,y_w,z_w) 为腕心在基座系中的坐标。若实际 TCP 有侧向偏置、腕轴不共点或关节有结构偏置,不能照搬这个简化式,必须先用真实机械臂的运动学参数建立变换链。

六轴机器人腕心定位与两组肘部逆解

用几何方程枚举肘部解

在 J1 确定的竖直平面内,令 r=xw2+yw2r=\sqrt{x_w^2+y_w^2}、v=zw−Hv=z_w-H。由余弦定理得到肘角余弦 c3c_3;正、负两种肘角代表肘部的不同折叠方向:

c3=r2+v2−A2−B22AB,q3=±arccos⁡(c3)c_3=\frac{r^2+v^2-A^2-B^2}{2AB},\qquad q_3=\pm\arccos(c_3)

随后按每个 q3q_3 分别求 q2q_2,并由腕心投影求出 q1q_1:

q1=atan2⁡(yw,xw),q2=atan2⁡(v,r)−atan2⁡(Bsin⁡q3,A+Bcos⁡q3)q_1=\operatorname{atan2}(y_w,x_w),\qquad q_2=\operatorname{atan2}(v,r)-\operatorname{atan2}(B\sin q_3,A+B\cos q_3)

例如腕心位于 (0.5095,0.2942,0.3545)(0.5095,0.2942,0.3545) m 时,q1=30°q_1=30°、c3≈0.5c_3\approx0.5,肘部有 (q2,q3)=(40°,−60°)(q_2,q_3)=(40°,-60°) 和 (−10.57°,60°)(-10.57°,60°) 两组解。若 ∣c3∣>1|c_3|>1,腕心超出两连杆可达范围;即使 ∣c3∣≤1|c_3|\le1,关节限位、姿态约束和碰撞仍可能使目标不可执行。

组合腕部姿态并按初始角度选解

对于这个模型,前三轴旋转矩阵是 R03=Rz(q1)Ry(−(q2+q3))R_{03}=R_z(q_1)R_y(-(q_2+q_3))。每选定一组肩肘解,就计算 R36=R03TR06R_{36}=R_{03}^TR_{06},把剩余姿态分配给 J4~J6。非奇异的 Z-Y-Z 腕部通常有两种等效分解:(q4,q5,q6)(q_4,q_5,q_6) 与 (q4+180°,−q5,q6+180°)(q_4+180°,-q_5,q_6+180°)。

下面的 Python 3.8 及以上版本标准库示例先用正运动学生成一个已知可达的目标,再求逆解、按示例限位过滤,并寻找与当前关节角最接近的分支;它不是具体品牌机器人的控制程序。

python
import math

A, B, H, D6 = 0.4, 0.3, 0.2, 0.1  # 所有长度均为米

def rz(t):
    c, s = math.cos(t), math.sin(t)
    return ((c, -s, 0), (s, c, 0), (0, 0, 1))

def ry(t):
    c, s = math.cos(t), math.sin(t)
    return ((c, 0, s), (0, 1, 0), (-s, 0, c))

def mul(a, b):
    return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3))
                       for j in range(3)) for i in range(3))

def trans(a):
    return tuple(zip(*a))

def fk(q_deg):
    q1, q2, q3, q4, q5, q6 = map(math.radians, q_deg)
    radial = A * math.cos(q2) + B * math.cos(q2 + q3)
    wc = (radial * math.cos(q1), radial * math.sin(q1),
          H + A * math.sin(q2) + B * math.sin(q2 + q3))
    r03 = mul(rz(q1), ry(-(q2 + q3)))
    r06 = mul(mul(mul(r03, rz(q4)), ry(q5)), rz(q6))
    p = tuple(wc[i] + D6 * r06[i][2] for i in range(3))
    return p, r06

def ik(p, r06):
    # TCP 沿自身 Z 轴退回腕心,再用余弦定理寻找肘部两分支。
    wc = [p[i] - D6 * r06[i][2] for i in range(3)]
    r, v = math.hypot(wc[0], wc[1]), wc[2] - H
    c3 = (r*r + v*v - A*A - B*B) / (2*A*B)
    if abs(c3) > 1 + 1e-10:
        return []
    c3 = max(-1.0, min(1.0, c3))
    q1 = math.atan2(wc[1], wc[0])
    solutions = []
    for sign in (1, -1):
        q3 = math.atan2(sign * math.sqrt(max(0.0, 1-c3*c3)), c3)
        q2 = math.atan2(v, r) - math.atan2(B*math.sin(q3), A+B*math.cos(q3))
        r03 = mul(rz(q1), ry(-(q2 + q3)))
        r36 = mul(trans(r03), r06)
        s5 = math.hypot(r36[0][2], r36[1][2])
        if s5 < 1e-9:
            continue  # 腕部奇异分解需另行处理,不能当作无解。
        q4 = math.atan2(r36[1][2], r36[0][2])
        q5 = math.atan2(s5, r36[2][2])
        q6 = math.atan2(r36[2][1], -r36[2][0])
        for a4, a5, a6 in ((q4, q5, q6),
                           (q4 + math.pi, -q5, q6 + math.pi)):
            angles = (q1, q2, q3, a4, a5, a6)
            solutions.append(tuple(math.degrees((t+math.pi) %
                               (2*math.pi)-math.pi) for t in angles))
    return solutions

goal_p, goal_r = fk((30, 40, -60, 20, 50, -30))
candidates = ik(goal_p, goal_r)
limits = ((-170, 170), (-120, 120), (-150, 150),
          (-180, 180), (-130, 130), (-180, 180))
valid = [q for q in candidates if all(lo <= x <= hi
         for x, (lo, hi) in zip(q, limits))]
seed = (30, 35, -55, 10, 45, -25)

def cost(q):
    # 使用关节角的最短环绕差,避免把 179° 与 -179° 误判为远离。
    return sum(((x-y+180) % 360-180)**2 for x, y in zip(q, seed))

best = min(valid, key=cost)
check_p, check_r = fk(best)
pos_ok = math.dist(check_p, goal_p) < 1e-9
rot_ok = max(abs(check_r[i][j]-goal_r[i][j])
             for i in range(3) for j in range(3)) < 1e-9
print("候选解数量:", len(candidates))
print("限位内解数量:", len(valid))
print("最近解(度):", tuple(round(x, 1) for x in best))
print("位置验证通过:", pos_ok)
print("姿态验证通过:", rot_ok)
print("超距目标解数量:", len(ik((2.0, 0.0, 0.2), goal_r)))

预期输出:

text
候选解数量: 4
限位内解数量: 4
最近解(度): (30.0, 40.0, -60.0, 20.0, 50.0, -30.0)
位置验证通过: True
姿态验证通过: True
超距目标解数量: 0

判断多解、奇异和可执行性

本例固定 J1 的正径向分支,因此是“两种肘姿态 × 两种腕分解 = 四个候选”;其他六轴机构可能出现肩部左右分支,总数也可能变化,不能把“必有八解”当作通用规则。初始角 seed 只是选解依据:改成靠近另一肘姿态的数值,再运行即可观察最近解改变。

现场验收时,先检查每个候选角是否落在机器人实际软限位内,再通过正运动学计算 TCP 位置与姿态残差,最后在仿真器中沿完整轨迹检查碰撞、轴速度与奇异邻域。尤其当腕部 q5q_5 接近 0°0° 或 180°180°,J4/J6 的分配不再唯一;本示例会跳过该分支,生产求解器需专门处理,不能据此宣称目标不可达。

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

目录与阅读设置

阅读设置

17

记录一个想法

求解逆运动学并识别多组解

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

登录哪都通