🦉 猫头鹰都在卷机械臂逆解了，你确定不关注一下？

【机器人学习】小白学习第16讲：视觉伺服 Visual Servoing

一句话核心：让摄像头当机器人的"眼睛"，把图像上的误差直接送进控制回路，机器人才能在不确定的环境里干活。

• 本讲目标：讲清"三维世界的点怎么变成二维像素"，以及两种闭环路线 PBVS / IBVS 的区别

一、为什么需要这个

小讲 1：为什么编码器不够用——先看再动与持续闭环

编码器测不到外面的世界变成什么样

前面几讲的机器人都是"闭着眼睛干活"的：轨迹事先算好，靠编码器反馈保证关节走准。这套办法在一个完全已知、纹丝不动的世界里够用。可现实不是这样——零件放歪了、传送带上的物体一边动一边等抓取、装配公差累积了。（这些不确定性，编码器完全测不到，因为它只知道自己转了多少，不知道外面的世界变成什么样。）

两条路线：先看再动与边看边动

给的思路分两档：

• 先看再动（look-and-move）：开机照一张相，估出物体位姿，规划轨迹，然后开环执行。简单，但期间物体一动就抓空，机械臂定位精度差也救不回来。
• 视觉伺服（visual servoing）：视觉测量持续反馈进控制律，实时算出"当前是什么样"和"希望是什么样"之间的误差，闭环消除它。

视觉伺服和前面运动控制最大的不同在于：被控量不是传感器直接测出来的。相机给的是一张二维数组（每个像素的灰度值），位置和姿态是经过图像处理、位姿估计这一长串加工才算出来的中间量。这一长串加工，就是本讲的全部内容。

相机装在哪：eye-to-hand 与 eye-in-hand

eye-to-hand（眼在手外）
相机固定不动。视野稳定、精度恒定；缺点是机械臂容易挡住被观测的物体。eye-in-hand（眼在手上）
相机装在末端。靠近物体时精度更高、几乎不会遮挡；缺点是运动中视野变化大。本章只讨论这一种。
约定：单目、黑白、固定焦距相机。这些都是为了把问题限定清楚，不是因为别的情况不能做。

停下来想一想：传送带上的零件一边动一边等抓取，look-and-move 为什么一定会抓空？eye-in-hand 相机相比 eye-to-hand，最大的优点和最大的缺点各是什么？

二、核心概念

小讲 2：从像素到特征——分割、解释与相机标定

1. 图像加工：分割 + 解释

相机输出的是灰度矩阵I(XI, YI)，要变成能用的数字得分两步。**分割（segmentation）**把像素分组，让不同组对应不同物体；解释（interpretation）从每组里量出图像特征参数。

• 阈值化：灰度直方图有几个峰，就在峰之间的谷取阈值l，把图变成只有 0/1 的二值图像。–10.2 的阈值取l = 152。
• 边缘检测：边缘就是灰度突变处，算梯度即可。最简单的算子用前向差分，抗噪一点用 Roberts、Sobel 算子；梯度幅值超过阈值处就是边缘点。
• 矩（moments）：二值区域 R 上定义mij = Σ XIi YIj。m00就是面积（像素数）；x̄ = m10/m00、ȳ = m01/m00是质心，可以用它在图像上唯一定位这个物体。力学类比很好记：把区域看成一块密度等于灰度的薄板，m00是质量、质心是重心、二阶中心矩是转动惯量，惯量矩阵的特征向量就是主轴，主轴转角给出物体的朝向α。

2. 相机标定：内参与外参（、）

内参（intrinsic）：相机自身的"出厂脾气"，与相机摆在哪无关。矩阵 Ω 里装的是焦距f、像素比例因子αx, αy（米→像素的换算）、主点偏移X0, Y0（光轴与像平面的交点，通常不在图像正中央）。

外参（extrinsic）：相机此刻挂在世界的哪个位置、朝哪个方向。就是变换矩阵Tcb（eye-to-hand 时相对基座），或Tec（eye-in-hand 时相对末端）。

标定的做法是拿一块棋盘格标定板，上面点的位置已知得很准；把板摆 ≥3 个不同位姿拍照，先解平面单应H′，再用旋转矩阵列必须正交且单位长这两个约束解出 Ω，最后求外参。坦白说这套方法只是"概念性"的，真用还得加上镜头畸变模型和非线性优化。

小讲 3：从特征到位姿与速度——PnP、双目深度与交互矩阵

3. 位姿估计：PnP 问题

已知物体的三维模型（CAD 里有），又看到它在图像上的投影，反推物体相对相机的位姿Tco——这就是摄影测量里的**PnP（Perspective-n-Point）**问题。解的个数很有讲究：

• P3P（3 个不共线点）：4 个解，必须额外判断哪个才是真的。
• P4P、P5P：非共面时至少 2 个解；若是≥4 个共面点且无三点共线，解唯一。
• 非共面且 n ≥ 6：唯一解。

共面是最常用的情况：把式两边左乘反对称矩阵S(·)消掉深度 λ，得到平面单应方程S(s̃i)H[rx,i ry,i 1]T = 0，它对未知矩阵 H 是线性的，堆 4 个点得到 12 个方程 9 个未知量，用 SVD 取最小奇异值对应的右特征向量即可（这招叫直接线性变换 DLT）。

注意：因为有噪声，直接解出来的 Q 通常不是严格的正交矩阵。要"矫正"成真正的旋转矩阵，得再对 Q 做 SVD 并按式（10.12）取最接近的 R——这一步在代码里经常被忘。

4. 立体视觉

单张二维图像里没有深度信息，只能用已知的物体几何去推。两张从不同位置拍的图可以解出深度，这就是立体视觉（stereo vision）。两个基本问题：

• 对应问题：左图那个点是右图哪个点？难，但极线约束把二维搜索降成一维——左图点 s₁ 的对应点必然落在右图的极线 ℓ₂ 上。
• 三维重建：两相机参数已知时，用**三角化（triangulation）**求两条视线的交点。

最实用的平行双目：pz = b/(X1 − X2)，即深度 = 基线 ÷ 视差。由此得到那条工程直觉：基线越长精度越高、盲区越大。视差 = b/p_z，b 越大同样深度的差别越明显、越好量；但 b 一大，两台相机能同时看见的重叠区域就变小，近处反而"看不见"了，而且匹配更容易认错。

5. 图像雅可比与交互矩阵

"我动一下，图像上的点往哪边跑多少？"这个问题的答案就是交互矩阵 L_s（interaction matrix，也称 image Jacobian 的近亲）。它把相机速度线性映射到特征速度：

$$\dot{s}=J_s(s,T_o^c)v_{c,o}^c=J_s{v}_o^c+L_s{v}_c^c\text{，物体静止时}\dot{s}=L_s{v}_c^c$$

符号	含义
s	特征向量（k×1），如 n 个点的归一化坐标堆起来 (10.5)
Js	图像雅可比（k×6）：相对运动 → 特征速度
Ls	交互矩阵（k×6）：相机自身运动 → 特征速度，两者关系 Js = Ls Γ(−occ,o)
vcc	相机在自身坐标系下表达的绝对速度 [ν ω]T

单个点的交互矩阵显式地只依赖特征坐标 s 和深度 z_c：

$$\begin{array}{r}{L}_s=[-1/z_c,0,X/z_c,XY,-(1+X^2),Y;\\ 0,-1/z_c,Y/z_c,1+Y^2,-XY,-X]\end{array}$$

**直觉：**前三项说"相机怎么平移，点就怎么在图上滑"——离得越远（z_c大），同样平移引起的图上位移越小；后三项说"相机怎么转，点怎么动"，它是二次的、和位置无关。

停下来想一想：阈值化时把阈值 l 取在灰度直方图的什么位置？用 3 个不共线的点做 PnP 会解出几个解，改成怎样的取点方式才能让解唯一？

图 1 PBVS 与 IBVS 的闭环结构对比。差别只有一句话：误差定义在哪个空间

三、关键公式

小讲 4：透视投影公式，以及误差定义在哪个空间

1. 透视投影：三维点怎么落到像素上（5.36–5.42）

$$\begin{array}{r}{X}_f=f\cdot x_c/z_c\text{，}{Y}_f=f\cdot y_c/z_c\\ X_I={\alpha }_{x}f\cdot x_c/z_c+X_0\text{，}{Y}_I={\alpha }_{y}f\cdot y_c/z_c+Y_0\\ \lambda [X_I{Y}_{I}1{]}^T=\Omega \prod [x_c{y}_c{z}_{c}1{]}^T\text{，}\Xi =\Omega \prod T_b^c\text{（相机标定矩阵）}\end{array}$$

符号	含义
(xc, yc, zc)	点在相机坐标系下的坐标，zc 是到相机的深度
f	焦距；把像平面搬到 zc = f 的"虚像平面"上，这样像不再倒立
αx, αy	每米多少像素（photosite 尺寸决定）
X0, Y0	主点偏移：光轴穿过像平面的那个点在像素坐标里的位置

**直觉：**分母那个 z_c就是一切麻烦的根源——远处的东西在图上被压缩，所以"图上近，实际不一定近"，深度信息在投影时被丢掉了。

图 2 针孔（透视）投影模型：三维点 P 与光心 Oc 的连线穿过像平面，交点就是它的像 s。取 zc = f 处的"虚像平面"，像就不倒过来了（对应）

2. 两种伺服路线的控制律

KaTeX parse error: Undefined control sequence: \Deltax at position 89: …J^{T}_{A d}(q, \̲D̲e̲l̲t̲a̲x̲) ( K_{P} \Delt…

符号	含义
Δx	任务空间误差 = −[odd,c ; φd,c]，由 Tdc = Tdo(Tco)−1 得到（10.68–10.69）
es	图像空间误差 = sd − s（10.79），单位就是"图上差多少"
JA d	任务空间解析雅可比（10.73），把关节速度映射到 Δẋ
JL	JL = Ls · blockdiag(RTc, RTc) · J(q)（10.83），把关节速度映射到 ės
g(q)	重力补偿项，和你在第 8 讲见过的完全一样

**直觉：**两条式子的形状和第 8 讲的"任务空间 PD + 重力补偿"一模一样，只是把误差换了个地方定义：PBVS 用 Δx（米和弧度），IBVS 用 e_s（图像单位）。ės + Ks es = 0是一阶指数收敛——这正是你在《自动控制原理》里看熟了的一阶系统。

停下来想一想

为什么要有"解速度"这一档？相机帧率（25 Hz 起）比运动控制回路低一个数量级。直接把视觉量塞进 PD，为了保稳不得不把增益压得很低。解速度方案干脆假定下层已有高增益运动控制器（q(t) ≈ qr(t)），视觉只负责生成参考速度再去积分，于是闭环性能不再被相机帧率卡住。

停下来想一想：PBVS 的误差 Δx 和 IBVS 的误差 e_s分别用什么单位来度量？ės + Ks es = 0的收敛快慢由矩阵 K_s的什么决定？

小讲 5：两张图——深度怎么丢掉，两种闭环差在哪

停下来想一想：图 1 里从光心 O_c到物点 P 的连线中，哪一段的长度就是深度 z_c？图 2 里标定误差分别落在 PBVS 和 IBVS 的哪条通道上，为什么抑制能力不同？

四、例题 / 动手

小讲 6：两道动手题——手算透视投影与双目测距精度

例 1 亲手算一遍透视投影

边长为 0.1 m 的正方形物体，四个顶点在物体系里是 P₁(0,0,0)、P₂(0.1,0,0)、P₃(0.1,0.1,0)、P₄(0,0.1,0)。相机看它：相对姿态是绕 y 轴转 π/4，位置 o^c_c,o= [0, 0, 0.5]^Tm。求四个点在归一化像平面（f = 1）上的坐标。

**第 1 步：**写出旋转矩阵 R^c_o= R_y(π/4)：

$$R=\left[\begin{array}{c}\mathrm{0.707100.7071}\\ 010\\ -\mathrm{0.707100.7071}\end{array}\right]$$

**第 2 步：**把点变换到相机系：rc = R ro + o。以 P₂为例：
R·(0.1,0,0)T = (0.07071, 0, −0.07071)T，加上(0,0,0.5)得(0.07071, 0, 0.42929)。

**第 3 步：**做透视除法（X = x_c/z_c，Y = y_c/z_c）：

$$\begin{array}{r}{P}_2:X=0.07071/0.42929=0.1647\text{，}Y=0\\ P_4:X=0\text{，}Y=0.1/0.5=0.2\\ P_3:X=0.07071/0.42929=0.1647\text{，}Y=0.1/0.42929=0.2329\\ P_1:X=0\text{，}Y=0\end{array}$$

和给出的 s₁…s₄完全一致。反过来做——已知这 8 个图像坐标和物体的四个顶点位置，去反解Tco——就是 PnP：堆出 12×9 的 A 矩阵，做 SVD，取最小奇异值对应的右特征向量，得到 ζH，再用第一列的范数定出 |ζ| = 1.5。

动手建议：用 numpy 把这 10 行写出来跑一遍，再给四个点的图像坐标加上 ±0.005 的高斯噪声，看看解出来的位置偏多少。这一步会直观地告诉你：为什么工程上一定要用远多于 4 个的对应点，再去解最小二乘。

例 2 双目视差能测多准？

平行双目，基线 b，同一物点在左右图的归一化横坐标是 X₁、X₂，式（10.53）：

$$p_z=b/(X_1-X_2)=b/d\text{，}d\text{为视差}$$

对 d 求扰动，得到相对误差：

KaTeX parse error: Undefined control sequence: \Deltap at position 2: \̲D̲e̲l̲t̲a̲p̲_{z}/p_{z} = - …

读出来的三件事：（1）深度越远，相对误差线性发散——远处靠眼睛（同样靠视觉）是量不准的；（2）基线 b 越大，误差越小，这正是"基线越长精度越高"；（3）但 b 一大，两台相机共同可见的重叠区域变小，太近的物体反而有一台看不见——这就是"盲区越大"。工程上只能折着取。

停下来想一想：例 1 里 P₃的归一化坐标 X、Y 分别由相机系里的哪两个数相除得到？例 2 里基线 b 变大时，深度的相对误差和近处盲区各自怎么变？

五、常见误区

用视觉伺服不等于不用标定相机

常见误区

误区 1：“用视觉伺服就不用标定相机了。”

正好反过来。PBVS 每一步都要先用图像反解位姿，内参错了像素→归一化坐标的换算就错，外参错了相机→末端的换算就错。的说法很形象：PBVS 里标定误差等于是加在反馈通道上的扰动，反馈通道的扰动抑制能力很差，所以 PBVS 对标定误差很敏感；IBVS 里误差量全在图像平面上定义、期望值也是用同一台相机"示教"拍下来的，标定误差等效为加在前向通道的扰动，抑制能力强，所以IBVS 对标定误差更鲁棒。IBVS 里唯一还需要的是深度 z_c（用来算 L_s），可以用初值或常值近似。

图像上收敛得漂亮，不等于机器人走得漂亮

常见误区

误区 2：“IBVS 里视觉量收敛得漂亮，机器人就一定走得漂亮。”

不一定。图像空间到任务空间是非线性映射，图像特征走直线，相机在笛卡儿空间里可能绕一大圈。 的仿真很扎眼：IBVS 的 PD 方案（方案 C）在 z 方向出现了300% 的超调，相机先是大幅后退再回来；想要 180° 大转动时，它会一路退到无穷远并失稳。PBVS 正好相反：任务空间（尤其是解速度方案）可以指派漂亮的指数轨迹，但特征是"被动"的结果，中途物体可能滑出视野，反馈断掉就麻烦了。

盯住一个特征点控不住六维运动

常见误区

误区 3：“看住一个特征点就够了。”

不够。单个点的交互矩阵是 2×6，秩只有 2，零空间维数是 4：沿视线方向的平移、绕视线的转动，图像坐标纹丝不动。位姿解的唯一性也一样苛刻：P3P 有 4 个解。

停下来想一想：单个点的交互矩阵是 2×6，它的零空间维数是几？哪两种相机运动在图像上完全看不出来？

六、本讲小结

停下来想一想：从图像到控制律，中间那串加工依次是哪几步？IBVS 里唯一还需要的那个三维量是什么？

要点回顾

1. 视觉让机器人不接触就能获得环境的几何信息；look-and-move 是开环的，视觉伺服把视觉测量闭环进控制器。本章只考虑 eye-in-hand、单目、黑白、定焦相机。
2. 流水线是：图像 →（分割/解释）→ 特征参数 s →（PnP 或交互矩阵）→ 位姿或速度 → 控制律。
3. 投影的本质是X = f xc/zc：除以深度这一步丢了深度，恢复深度要么靠已知物体几何，要么靠双目视差 / 三角化。
4. **交互矩阵 L_s**是图像版的雅可比，把相机速度线性映射到特征速度ṡ = Ls vcc；它依赖特征坐标 s 和深度 z_c。
5. PBVS三维空间：需要位姿估计与物体模型，可以对任务空间指派期望动态，可能丢失视野，对标定敏感。IBVS图像空间：不需要位姿估计与精确模型，能保住视野、对标定鲁棒，但相机三维路径不可预测、有奇异性。还给了折中的混合视觉伺服。

**一句话总结：**视觉伺服就是"把 PID 该作用的那个误差，换成从图像里算出来的误差"——PBVS 把它定义在三维，IBVS 把它定义在二维。