- Authors
- Name
- Youngju Kim
- @fjvbn20031
- 引言 —— 把「把那个杯子放这里」变成六个角度
- 逆运动学为什么比正运动学难
- 两连杆手臂的解析解 —— 用余弦定理一路解到底
- 雅可比矩阵 —— 把关节速度带到末端速度的那个矩阵
- 用伪逆矩阵反复迭代求数值解
- 奇异点 —— 物理上究竟发生了什么
- 阻尼最小二乘法 —— 在奇异点附近活下来
- 冗余自由度与零空间
- 结语 —— 逆运动学不是求答案的问题,而是选答案的问题
引言 —— 把「把那个杯子放这里」变成六个角度
我们在正运动学那篇里搭出来的计算器是这么用的:输入肩部 30 度、肘部 45 度,它就会告诉你末端在 (0.212028, 0.244889)。随便输入什么角度都会得到唯一一个答案,而且几次乘法就能算完。
但实际上想做的事情正好相反。杯子在 (0.25, 0.15),想让末端到那个位置去,而关节该是多少度,得靠自己反推出来。
这个方向就是逆运动学(IK),而且比正运动学难得多。
关键在于,这种难度有好几种不同的形态。不是计算变复杂了,而是问题本身的性质变了。有些坐标根本没有答案,有些坐标有两个答案,有些手臂上则有无穷多个答案。而且即便存在答案,答案周围的性质在不同地方也不一样:在某个姿态下,把末端移动 1 毫米只需要关节转 0.15 度;在另一个姿态下,同样的 1 毫米却需要转 20.8 度。
这篇文章逐一讲这四种难度,最后给出真正能用的代码。
逆运动学为什么比正运动学难
正运动学是一个函数。输入关节角度向量,输出恰好一个末端位置。定义域里的每一个点,都对应着恰好一个值。
逆运动学是求这个函数的反函数的问题——而这个函数既不是单射,也不是满射。
无解的情形。 比连杆长度之和还远的点,不管用什么角度都够不到。一条上臂 0.20 米、前臂 0.15 米的手臂,最大可达距离是 0.35 米。要求够到 0.40 米外的点,方程就没有实数解。内侧也存在够不到的区域:比两根连杆长度之差(0.05 米)还近的点,不管肘部怎么折都够不到。
多解的情形。 大多数可达的点恰好有两个解:肘部朝上弯,或者肘部朝下弯。在 6 自由度的工业机器人上,这种分支会在三个地方发生(肩、肘、腕),最多产生八个解。
无穷多解的情形。 当关节数超过任务的维度时,解会连续地有无穷多个。用三连杆平面手臂只指定一个二维位置,剩下那一个自由度就能让手臂自由地扭动。人的手臂就是这样——把手固定在一个地方,上下摆动肘部,立刻就能感受到。
没有闭式解的情形。 一般的 6 自由度手臂的逆运动学,代数上求解会得到一个 16 次方程,闭式解只在特定条件下存在。手腕三根轴交于一点的结构(球形手腕)是这些条件里用得最广的一种,这也是大多数工业机器人采用这种结构的实际原因。没有这种结构的手臂,就必须用数值方法求解。
| 性质 | 正运动学 | 逆运动学 |
|---|---|---|
| 解的个数 | 永远恰好一个 | 0个、多个,或无穷个 |
| 闭式解 | 永远存在 | 取决于结构 |
| 计算方式 | 几次矩阵乘法 | 解析解或迭代数值解 |
| 计算时间 | 恒定 | 随姿态和初值变化 |
| 失败条件 | 无 | 超出工作空间、收敛失败、奇异点 |
| 姿态选择 | 不需要 | 必须从多个解里挑一个 |
最后一行在实务中造成的事故最多。有两个解时选哪一个,决定了整条手臂的姿态,如果每个周期都独立求解,一旦选择发生切换,手臂就会瞬间翻转过去。
两连杆手臂的解析解 —— 用余弦定理一路解到底
先把最简单的情形彻底解出来。两根连杆,两个转动关节,平面上的一个位置。两个未知数,两个方程。
正运动学是这样的:
x = L1·cos(θ1) + L2·cos(θ1 + θ2)
y = L1·sin(θ1) + L2·sin(θ1 + θ2)
核心技巧是先消去 θ1。把两个方程分别平方后相加。
x² + y² = L1² + L2² + 2·L1·L2·[cos(θ1)cos(θ1+θ2) + sin(θ1)sin(θ1+θ2)]
方括号里是余弦差角公式,所以变成 cos(θ2)。
x² + y² = L1² + L2² + 2·L1·L2·cos(θ2)
这正是三角形的余弦定理。两根连杆和肩到末端的连线构成一个三角形,θ2 是那个夹角的补角。整理一下:
D = cos(θ2) = (x² + y² - L1² - L2²) / (2·L1·L2)
代入数字。把目标设为 (0.212028, 0.244889),
x² + y² = 0.0449559 + 0.0599706 = 0.1049264
L1² + L2² = 0.0400000 + 0.0225000 = 0.0625000
2·L1·L2 = 2 × 0.20 × 0.15 = 0.06
D = (0.1049264 - 0.0625000) / 0.06 = 0.0424264 / 0.06 = 0.7071068
D 是 0.7071068,也就是 cos(45°)。
这里不能直接用 θ2 = arccos(D) 结束。arccos 只返回 0 度到 180 度之间的值,会丢掉负值那个解。应该把正弦也一并求出来,一起代入 atan2。
sin(θ2) = ±√(1 - D²)
θ2 = atan2(±√(1 - D²), D)
两个符号正好对应两个解。
√(1 - 0.7071068²) = √(1 - 0.5) = √0.5 = 0.7071068
θ2 = atan2(+0.7071068, 0.7071068) = +45°
θ2 = atan2(-0.7071068, 0.7071068) = -45°
接下来是 θ1。取从肩部看向末端的方向,再减去三角形内角所占的那部分。
θ1 = atan2(y, x) - atan2(L2·sin(θ2), L1 + L2·cos(θ2))
前一项是肩部看向末端的角度,后一项是上臂相对这条视线偏开了多少。用 θ2 = +45° 来算,
atan2(0.244889, 0.212028) = 49.1136°
atan2(0.15 × 0.7071068, 0.20 + 0.15 × 0.7071068)
= atan2(0.1060660, 0.3060660) = 19.1136°
θ1 = 49.1136 - 19.1136 = 30.0000°
正好算出 30 度——正是正运动学里出发时用的那个角度。
代入 θ2 = -45°,后一项就变成 atan2(-0.1060660, 0.3060660) = -19.1136°,于是
θ1 = 49.1136 - (-19.1136) = 68.2271°
第二个解是 (68.2271°, -44.9999°)。肘部朝相反方向弯曲,末端却落在完全相同的位置。
import numpy as np
L1, L2 = 0.20, 0.15
def ik_2link(x, y, elbow=+1):
"""两连杆平面手臂的解析解。elbow=+1是elbow-down,-1是elbow-up。"""
D = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
inner = 1.0 - D * D
if inner < 0:
raise ValueError(f"超出工作空间。D={D:.6f}, 1-D²={inner:.6f}")
theta2 = np.arctan2(elbow * np.sqrt(inner), D)
theta1 = np.arctan2(y, x) - np.arctan2(L2 * np.sin(theta2), L1 + L2 * np.cos(theta2))
return np.array([theta1, theta2])
def fk_2link(q):
return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])
target = np.array([0.212028, 0.244889])
for elbow, name in ((+1, "elbow-down"), (-1, "elbow-up ")):
q = ik_2link(*target, elbow=elbow)
print(f"{name} θ1={np.degrees(q[0]):9.4f}° θ2={np.degrees(q[1]):9.4f}° FK校验={np.round(fk_2link(q), 6)}")
try:
ik_2link(0.40, 0.0)
except ValueError as e:
print("(0.40, 0.00) ->", e)
运行结果如下。
elbow-down θ1= 30.0001° θ2= 44.9999° FK校验=[0.212028 0.244889]
elbow-up θ1= 68.2271° θ2= -44.9999° FK校验=[0.212028 0.244889]
(0.40, 0.00) -> 超出工作空间。D=1.625000, 1-D²=-1.640625
最后一行展示了超出工作空间的坐标在方程里是怎么表现出来的。代入 (0.40, 0),
D = (0.16 - 0.0625) / 0.06 = 1.625
1 - D² = 1 - 2.640625 = -1.640625
D 超过了 1。既然余弦不可能超过 1,这就意味着没有任何角度能满足这个目标,平方根内部变成负数,计算也就中止了。
跳过这个检查会发生什么,这一点很关键。在 numpy 里,负数的平方根不是抛异常,而是变成 nan,而 nan 会悄悄地扩散开。θ2 变成 nan,θ1 也跟着变成 nan,nan 最终会传到把角度写给舵机的那个函数里。在 C 系语言里,把 nan 转换成整数是未定义行为,如果这个值在 Arduino 上传进了 servo.write(),关节就会跳到一个无法预测的位置。逆运动学函数的第一行,永远应该是可达性检查。
两个解里选哪一个,也得由代码来决定。实务中常见的规则有三条。第一,丢弃超出硬件关节极限的解。第二,在剩下的解里,选择相对当前姿态关节角度变化量最小的那一个。第三,如果还有多个,就固定选用某个姿态(比如永远 elbow-up)。第二条规则尤其重要,没有它,解就可能在轨迹执行到一半时切换,手臂随之翻转。
雅可比矩阵 —— 把关节速度带到末端速度的那个矩阵
解析解很优雅,但只在两连杆手臂或球形手腕这类结构特殊的手臂上才求得出来。一般的手臂需要另一种方法,起点就是雅可比矩阵。
思路是这样的。对正运动学 p = f(q) 求微分,
ṗ = J(q) · q̇
这里的 J 是把末端坐标对关节角度求偏导后收集起来的矩阵。按分量写就是,
J[i][j] = ∂(p的第i个分量) / ∂(q的第j个分量)
雅可比矩阵不过是把一堆偏导数排成网格而已,并没有另外藏着什么更复杂的定义。
来手工推导两连杆手臂的雅可比矩阵。把 x 对 θ1 求导,
∂x/∂θ1 = -L1·sin(θ1) - L2·sin(θ1 + θ2)
∂x/∂θ2 = -L2·sin(θ1 + θ2)
∂y/∂θ1 = L1·cos(θ1) + L2·cos(θ1 + θ2)
∂y/∂θ2 = L2·cos(θ1 + θ2)
收集成矩阵,
J = [ -L1·s1 - L2·s12 -L2·s12 ]
[ L1·c1 + L2·c12 L2·c12 ]
s1 是 sin(θ1) 的简写,s12 是 sin(θ1+θ2) 的简写。
算一下这个矩阵的行列式,会得到一个出乎意料简单的结果。
det(J) = (-L1·s1 - L2·s12)(L2·c12) - (-L2·s12)(L1·c1 + L2·c12)
= -L1·L2·s1·c12 - L2²·s12·c12 + L1·L2·s12·c1 + L2²·s12·c12
= L1·L2·(s12·c1 - s1·c12)
= L1·L2·sin(θ2)
这个行列式完全不依赖 θ1,只依赖 θ2。 意味着不管肩部转到哪个方向,手臂的性质都完全由肘部角度决定,这在物理上是理所当然的——转动肩部只是把整条手臂旋转了一下而已。
而当 θ2 = 0 时,行列式为零。这正是手臂完全伸直的姿态。雅可比矩阵在这里变成奇异的。
用伪逆矩阵反复迭代求数值解
有了雅可比矩阵,就能把逆运动学变成一个微分方程来求解。
设当前关节角度是 q,末端在 f(q),目标是 p_target,那么剩下的误差是 e = p_target - f(q)。要消除这个误差,末端需要移动 e,而关节需要移动多少才能做到这一点,由 J·Δq = e 给出。
如果 J 是方阵且可逆,Δq = J⁻¹·e。但如果关节数和任务维度不一致,J 就不是方阵。这时候用的是摩尔-彭若斯伪逆矩阵 J⁺。
当关节数多于任务维度(也就是存在冗余)时,J⁺ = Jᵀ(J·Jᵀ)⁻¹,这个解是让 ‖Δq‖ 最小的解。当关节数不够时,J⁺ = (Jᵀ·J)⁻¹Jᵀ,这个解是让误差最小的解。两种情况,numpy.linalg.pinv 都会自动处理。
整个算法是这样的:
1. 选定初始角度q(通常是当前姿态)
2. 计算 e = p_target - f(q)
3. 若‖e‖足够小,结束
4. Δq = J(q)⁺ · e
5. q = q + Δq,回到第2步
一行行看下来,这就是多元牛顿法——一个在末端误差这个非线性函数上寻找零点的迭代过程。
这个方法的优点是完全不在乎手臂的结构。只要能算出雅可比矩阵,不管有多少个关节、按什么顺序连接,它的运作方式都一样。缺点有三个:不保证收敛,不同初值会落到不同的解上,以及在奇异点附近会发散。
最后这个问题足够严重,值得单独拿一节来讲。
奇异点 —— 物理上究竟发生了什么
奇异点是雅可比矩阵秩下降的姿态。单看定义很抽象,但物理上发生的事情非常具体。
末端会在某个方向上完全动不了。 把手臂完全伸直,试着把末端朝远离肩部的方向再推一把。不管怎么转动任何一个关节,那个方向上连一毫米都动不了——因为已经伸到最长了。雅可比矩阵的像从二维退化成一维,说的正是这个情形。
6 自由度手臂上的奇异点分为三种。
| 类型 | 姿态 | 会发生什么 |
|---|---|---|
| 肘部奇异点 | 手臂完全伸直 | 无法沿径向移动 |
| 肩部奇异点 | 腕部中心落在第1轴的旋转轴上 | 不管第1轴转向哪里,腕部位置都不变 |
| 腕部奇异点 | 腕部第4轴与第6轴共线 | 两根轴产生相同的旋转,其中一个变得多余 |
第三种是实务中最常遇到的问题。手腕两根轴一旦重合,它们就会做同一件事,控制器于是开始拼命地把两者朝相反方向猛转。工业机器人示教过程中手腕突然转上一整圈的事故,大多数都是这个原因。
奇异点不是非此即彼的二元问题,而是一个连续的程度问题。衡量这个程度的标准尺度,是吉川的可操作度指标。
w = √(det(J·Jᵀ))
对方阵雅可比矩阵而言,这等于 |det(J)|。而更实用的尺度,来自奇异值分解给出的最小奇异值和条件数。用前面搭的两连杆手臂来确认一下。
import numpy as np
L1, L2 = 0.20, 0.15
def jacobian(q):
s1, c1 = np.sin(q[0]), np.cos(q[0])
s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
[ L1 * c1 + L2 * c12, L2 * c12]])
print(" θ2 det(J) L1·L2·sin(θ2) σ1 σ2 条件数")
for d2 in (90, 45, 10, 2, 0):
J = jacobian(np.radians([30, d2]))
sigma = np.linalg.svd(J, compute_uv=False)
cond = sigma[0] / sigma[1] if sigma[1] > 1e-12 else float("inf")
print(f"{d2:4}° {np.linalg.det(J):10.7f} {L1*L2*np.sin(np.radians(d2)):12.7f}"
f" {sigma[0]:8.6f} {sigma[1]:8.6f} {cond:10.1f}")
运行结果如下。
θ2 det(J) L1·L2·sin(θ2) σ1 σ2 条件数
90° 0.0300000 0.0300000 0.269451 0.111337 2.4
45° 0.0212132 0.0212132 0.351840 0.060292 5.8
10° 0.0052094 0.0052094 0.379341 0.013733 27.6
2° 0.0010470 0.0010470 0.380731 0.002750 138.5
0° -0.0000000 0.0000000 0.380789 0.000000 inf
第二列和第三列在每一行都一致。手工推出的 det(J) = L1·L2·sin(θ2) 是对的。最后一行的 -0.0000000 是浮点数里带符号的零,值就是零。
该看的是右边三列。σ1 随肘部角度变化时大致保持在 0.27 到 0.38 之间,几乎不变;但 σ2 会从 0.111 一路跌到 0。条件数从 2.4 一路涨到无穷大。
较小的奇异值,就是那个方向上的「传动比」。 σ2 = 0.00275 的意思是,要沿那个方向把末端移动 1 米,关节需要转动 1/0.00275 = 364 弧度。换算成 1 毫米就是 0.364 弧度,也就是 20.8 度。而另一个方向(σ1 = 0.3807)上,1 毫米只需要 0.0026 弧度,0.15 度就够了。同样是 1 毫米,不同方向能相差 139 倍。
阻尼最小二乘法 —— 在奇异点附近活下来
现在很清楚前面那个迭代法为什么会发散了。在 Δq = J⁺·e 中,J⁺ 里包含奇异值的倒数,一旦某个奇异值趋于零,它的倒数就会发散。
用数字来看看。在 θ2 = 1° 的姿态下,命令末端沿 x 方向移动 1 毫米,
伪逆矩阵给出的|Δq| = 0.626520 rad = 35.8970°
为了这 1 毫米,关节要转 35.9 度。如果控制周期是 100Hz,这就是要求 10 毫秒内完成,也就是每秒 3590 度。常见业余舵机的空载转速大约是每秒 400 度左右,这个要求超出了九倍还多。实际情况是,舵机会以最大速度硬冲,完全偏离轨迹,要么撞上关节极限,要么电源被拖垮、主板复位。
解决办法是放弃一点精度。在求逆矩阵时,给对角线加上一个小值。
Δq = Jᵀ·(J·Jᵀ + λ²·I)⁻¹·e
这就是阻尼最小二乘法(DLS)——在机器人学里以中村、花房、Wampler 1986 年的研究闻名,在一般数值分析领域则被称为 Levenberg-Marquardt 方法。
从奇异值的角度看,λ 起的作用就很清楚了。原本每个奇异值会被反转成 1/σ,加上阻尼之后就变成 σ/(σ² + λ²)。当 σ 比 λ 大得多时,这个值几乎等于 1/σ;当 σ 趋近于零时,这个值也趋近于零。它只挑出小奇异值方向上的放大效应加以抑制,其余方向不受影响。
import numpy as np
L1, L2 = 0.20, 0.15
def fk(q):
return np.array([L1 * np.cos(q[0]) + L2 * np.cos(q[0] + q[1]),
L1 * np.sin(q[0]) + L2 * np.sin(q[0] + q[1])])
def jacobian(q):
s1, c1 = np.sin(q[0]), np.cos(q[0])
s12, c12 = np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
return np.array([[-L1 * s1 - L2 * s12, -L2 * s12],
[ L1 * c1 + L2 * c12, L2 * c12]])
J = jacobian(np.radians([30, 1])) # 几乎完全伸直、紧挨着奇异点的姿态
dx = np.array([0.001, 0.0]) # 想沿x方向移动1mm
dq = np.linalg.pinv(J) @ dx
print(f"伪逆矩阵 |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}°"
f" 换算100Hz {np.degrees(np.linalg.norm(dq))/0.01:8.1f}°/s"
f" 实际移动 {np.linalg.norm(J @ dq)*1000:.4f}mm")
for lam in (0.001, 0.005, 0.01, 0.05):
dq = J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), dx)
print(f"DLS λ={lam:<6} |Δq|={np.linalg.norm(dq):.6f} rad = {np.degrees(np.linalg.norm(dq)):8.4f}°"
f" 换算100Hz {np.degrees(np.linalg.norm(dq))/0.01:8.1f}°/s"
f" 实际移动 {np.linalg.norm(J @ dq)*1000:.4f}mm")
运行结果如下。
伪逆矩阵 |Δq|=0.626520 rad = 35.8970° 换算100Hz 3589.7°/s 实际移动 1.0000mm
DLS λ=0.001 |Δq|=0.409784 rad = 23.4789° 换算100Hz 2347.9°/s 实际移动 0.7585mm
DLS λ=0.005 |Δq|=0.044071 rad = 2.5251° 换算100Hz 252.5°/s 实际移动 0.5113mm
DLS λ=0.01 |Δq|=0.011702 rad = 0.6705° 换算100Hz 67.0°/s 实际移动 0.5077mm
DLS λ=0.05 |Δq|=0.001394 rad = 0.0799° 换算100Hz 8.0°/s 实际移动 0.4992mm
这张表把这里的取舍原原本本地展示了出来。
伪逆矩阵能精确地做出要求的那 1 毫米。代价是关节要转动 35.9 度。
当 λ = 0.01 时,关节运动降到 0.67 度,缩小了 54 倍。 代价是实际移动只有 0.51 毫米,大约是要求值的一半。
只走了一半,看起来像是失败,但其实不然。这只是一个周期的故事。下一个周期,会再看一次剩下的误差,再走大约一半。末端朝目标靠近的速度慢了一点,但很可靠。相比之下,伪逆矩阵想要一个周期就到位,结果关节超出了物理极限,末端非但没到目标,反而跑到了完全不相干的地方。
选 λ 有实用的指导原则。如果用固定值,就从所需的作业精度和最大关节速度反推出来。更好的办法是可变阻尼:远离奇异点时 λ = 0,一旦最小奇异值跌破某个阈值,才开始把 λ 调大。这样既不会牺牲平时的精度,又能在奇异点附近获得保护。
冗余自由度与零空间
最后一块内容。当关节数超过任务维度时会怎样?
假设用三连杆平面手臂只指定一个二维位置。雅可比矩阵是 2 行 3 列,秩为 2。按照秩-零化度定理,零空间的维度是 3 - 2 = 1。
零空间中的关节速度向量满足 J·Δq = 0。关节在动,末端却停在原地。 这正是把手固定在桌子上、上下摆动肘部的那个动作。
利用这一点的方法叫零空间投影。
Δq = J⁺·e + (I - J⁺·J)·z
第一项把末端带向目标,第二项在完全不影响末端的前提下,朝 z 想要的方向推动姿态。(I - J⁺·J) 就是投影到零空间的矩阵。
z 里填什么,决定了冗余自由度怎么用。填入一个随着关节偏离活动范围中心而增大的值,手臂就会自动避开关节极限;填入到障碍物的距离,就能在保持末端路径不变的同时避开自身躯干;填入可操作度指标,手臂就会自动远离奇异点。
import numpy as np
LINKS = (0.20, 0.15, 0.10)
def fk3(q):
a = np.cumsum(q)
return np.array([sum(L * np.cos(t) for L, t in zip(LINKS, a)),
sum(L * np.sin(t) for L, t in zip(LINKS, a))])
def jacobian3(q):
a = np.cumsum(q)
J = np.zeros((2, 3))
for j in range(3):
J[0, j] = -sum(LINKS[i] * np.sin(a[i]) for i in range(j, 3))
J[1, j] = sum(LINKS[i] * np.cos(a[i]) for i in range(j, 3))
return J
def solve_ik(target, q0, lam=0.05, max_iter=200, tol=1e-6):
"""阻尼最小二乘法迭代。即便经过奇异点也不会发散。"""
q = np.array(q0, dtype=float)
for k in range(max_iter):
error = target - fk3(q)
if np.linalg.norm(error) < tol:
return q, k
J = jacobian3(q)
q = q + J.T @ np.linalg.solve(J @ J.T + lam * lam * np.eye(2), error)
return q, max_iter
target = np.array([0.30, 0.20])
q, iters = solve_ik(target, [0.1, 0.5, 0.0])
print(f"收敛 {iters}次 θ={np.round(np.degrees(q), 4)}° FK校验={np.round(fk3(q), 8)}")
J = jacobian3(q)
print(f"J的形状 {J.shape},秩 {np.linalg.matrix_rank(J)},零空间维度 {3 - np.linalg.matrix_rank(J)}")
N = np.eye(3) - np.linalg.pinv(J) @ J # 零空间投影矩阵
z = np.array([1.0, 1.0, 1.0]) # 试着填入任意方向
dq_null = N @ z
print(f"零空间方向 Δq = {np.round(dq_null, 6)}")
print(f"这个方向上的末端速度 J·Δq = {np.round(J @ dq_null, 12)}")
step = 0.05 * dq_null / np.linalg.norm(dq_null)
print(f"关节移动 {np.round(np.degrees(step), 4)}° 后")
print(f" 末端位移 = {np.linalg.norm(fk3(q + step) - fk3(q)) * 1000:.6f} mm")
运行结果如下。
收敛 6次 θ=[-7.5005 67.2642 15.0619]° FK校验=[0.29999971 0.19999971]
J的形状 (2, 3),秩 2,零空间维度 1
零空间方向 Δq = [ 0.038069 -0.231649 0.463807]
这个方向上的末端速度 J·Δq = [0. 0.]
关节移动 [ 0.2098 -1.2766 2.556 ]° 后
末端位移 = 0.059555 mm
三个关节分别移动了 0.21°、-1.28°、2.56°,末端却几乎没动,只有 0.06 毫米。之所以不是精确的零,是因为零空间是那一点上的切线方向,有限步长会留下二阶误差。把步长减半,这份误差就会变成四分之一。
收敛只用了 6 次迭代,这一点也值得留意。虽然初始姿态离目标相当远,六次迭代就能逼近到微米级别。牛顿类方法的收敛速度就是这么快。当然,一旦经过奇异点,或者目标落在工作空间之外,情况就不一样了,所以实际代码里必须有迭代次数上限和最终误差检查。如果你注意到上面这个函数就算到达了 max_iter 也会默默返回一个值,那你注意对了——实战代码必须让调用方能够区分这种情况。
结语 —— 逆运动学不是求答案的问题,而是选答案的问题
贯穿这篇文章的是一个事实:逆运动学的答案要么有好几个,要么一个都没有。
所以实际要做的事,大部分不是解方程,而是明确说清楚自己想要哪一个答案。elbow-up 还是 elbow-down?超出关节极限的解要不要丢弃?要不要选离上一个姿态最近的那个解?如果有冗余自由度,多出来的自由度用来做什么?在奇异点附近,精度和关节速度要放弃哪一个?
如果不在代码里把这些选择明确写出来,选择并不会消失,而是会由浮点运算的顺序替你决定。 而这个选择每个周期都在变,正是手臂突然翻转的原因。
用数值方法时最后要记住的一点是:迭代收敛了,和答案能不能用,是两回事。收敛出来的角度是否落在关节极限之内,途中的路径会不会穿过自己的身体,那个姿态的条件数是否在可控范围内——这些都需要单独检查。
以上就是「该往哪里去」的答案。真正平滑地移动到那个角度,留待控制回路那篇继续讲。而这篇文章里出现的伪逆矩阵、奇异值分解、条件数这些工具该学到什么程度,已经按顺序整理在机器人学所需的数学那篇里了。