机器人 / 系统课时
求解逆运动学并识别多组解
求解逆运动学并识别多组解
给定抓取点在基座坐标系中的位置和工具朝向,逆运动学要找出一组可执行的六关节角。本篇用理想化球腕六轴机械臂,先求腕心,再枚举肘部与腕部解,并用初始姿态、关节限位和正运动学检查结果。
学习目标
- 将 TCP 目标位姿拆解为腕心位置与腕部姿态。
- 计算肘部两组几何解,判断无解和腕翻转分支。
- 运行完整示例,以初始姿态筛选六轴解并验证误差。
从 TCP 位姿回推腕心
采用便于复算的理想化六轴模型:J1 绕基座竖直 Z 轴旋转,J2、J3 控制肩肘俯仰,J4~J6 的转轴交于同一腕心,腕部姿态按 Z-Y-Z 旋转分解。上臂 m,前臂 m,肩高 m,腕心到 TCP 沿末端局部 Z 轴的距离 m。
目标不只是一个三维坐标,而是 TCP 位置 和相对于基座的旋转矩阵 。由于球腕转动不改变腕心位置,可以先扣除工具轴向偏置:
这里 为腕心在基座系中的坐标。若实际 TCP 有侧向偏置、腕轴不共点或关节有结构偏置,不能照搬这个简化式,必须先用真实机械臂的运动学参数建立变换链。

用几何方程枚举肘部解
在 J1 确定的竖直平面内,令 、。由余弦定理得到肘角余弦 ;正、负两种肘角代表肘部的不同折叠方向:
随后按每个 分别求 ,并由腕心投影求出 :
例如腕心位于 m 时,、,肘部有 和 两组解。若 ,腕心超出两连杆可达范围;即使 ,关节限位、姿态约束和碰撞仍可能使目标不可执行。
组合腕部姿态并按初始角度选解
对于这个模型,前三轴旋转矩阵是 。每选定一组肩肘解,就计算 ,把剩余姿态分配给 J4~J6。非奇异的 Z-Y-Z 腕部通常有两种等效分解: 与 。
下面的 Python 3.8 及以上版本标准库示例先用正运动学生成一个已知可达的目标,再求逆解、按示例限位过滤,并寻找与当前关节角最接近的分支;它不是具体品牌机器人的控制程序。
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)))预期输出:
候选解数量: 4
限位内解数量: 4
最近解(度): (30.0, 40.0, -60.0, 20.0, 50.0, -30.0)
位置验证通过: True
姿态验证通过: True
超距目标解数量: 0判断多解、奇异和可执行性
本例固定 J1 的正径向分支,因此是“两种肘姿态 × 两种腕分解 = 四个候选”;其他六轴机构可能出现肩部左右分支,总数也可能变化,不能把“必有八解”当作通用规则。初始角 seed 只是选解依据:改成靠近另一肘姿态的数值,再运行即可观察最近解改变。
现场验收时,先检查每个候选角是否落在机器人实际软限位内,再通过正运动学计算 TCP 位置与姿态残差,最后在仿真器中沿完整轨迹检查碰撞、轴速度与奇异邻域。尤其当腕部 接近 或 ,J4/J6 的分配不再唯一;本示例会跳过该分支,生产求解器需专门处理,不能据此宣称目标不可达。