04.3 从针孔成像手推视觉伺服：每一列雅可比在说什么

零基础入口：像素为什么能告诉我们该怎样运动

IBVS 是 image-based visual servoing，即直接用图像特征误差闭环控制。像素坐标是在图像上数格子；主点是光轴落到图像平面的位置；焦距把视线比例换成像素尺度；深度是目标沿相机光轴的距离，不是水深。针孔模型把光线视为穿过同一个小孔，用相似三角形描述投影；这里使用正向虚拟成像平面，已把真实底片倒像的符号约定吸收进模型。

先只看水平面：点横向离轴 $X=0.4$ m、光轴深度 $Z=2$ m，视线比例为 $x=X/Z=0.2$ 。取焦距 $f_x=500$ pixel、主点 $c_u=320$ pixel，像素为 $u=f_xx+c_u=420$ pixel。公式的目的，是把米制几何变成图上位置；远近都加倍时比例不变，所以单张单点图像不能独自确定距离。

链式法则在这里回答：图像变化经过了哪些中间量？ 相机运动先改变点在相机系中的 $X,Y,Z$ ，再通过透视除法改变 $x,y$ ，最后经焦距变成像素。只看分子会漏掉前进造成的放大。商法则中的 $\dot X/Z$ 是横向坐标变化， $-X\dot Z/Z^2$ 是深度变化；在上例，若 $\dot X=0$ 、 $\dot Z=-0.1$ m/s，则 $\dot x=0.01/\mathrm s$ ，目标向画面外侧移动。

雅可比矩阵是一张局部“运动→图像变化”倍率表。 它有两行，因为输出有水平和竖直两个像速；六列对应相机三种平移和三种旋转。读取一列时，把其余五种速度置零即可。下文会逐列推导，不能把“第几列”当作固定轴意义而不检查速度顺序。

单独打开的相机运动	第一行为什么变化	第二行为什么变化
沿右轴平移	静止点相对左移，系数负	不改竖向坐标或深度，为零
沿下轴平移	不改横向坐标或深度，为零	静止点相对上移，系数负
沿光轴前进	深度缩小，横向离轴点外扩	同样使竖向离轴点外扩
绕右轴转动	深度变化间接影响横向比值	竖向坐标与深度同时变
绕下轴转动	横向坐标与深度同时变	深度变化间接影响竖向比值
绕光轴转动	像点在平面内旋转	与横向变化配合成旋转，而非放大

有限运动也能检查导数的意义：上例相机纯右移 $0.1$ m/s，经过 $0.02$ s 移动 $0.002$ m，静止点的新相机横坐标是 $0.398$ m，新的像素是 $419.5$ pixel。它左移 $0.5$ pixel，与瞬时像速 $-25$ pixel/s 乘时间一致。纯横移且深度不变时这一计算恰好精确；旋转或深度改变时，导数乘时间只是短时间的一阶近似。

配套基础练习： 刚体杆臂与坐标变换帮助理解后文相机外参；当前九题目录没有独立 IBVS 判题，请使用本页折叠 C++ 的逐列有限差分测试，不把相邻题冒称视觉控制验证。

视觉检测输出“物体在图像哪里”，并不直接输出“机器人应向哪边施力”。本课把一个静止三维点投影成像素，再对相机的六维运动求导。推导完成后，你可以不用背表格，判断正偏航、正横移、正升沉怎样改变画面。输入为标定内参、已关联图像特征、近似光轴深度与外参；输出为受约束的相机或机体速度参考。

符号与适用条件

符号	含义	维数 / 单位	坐标或约定
$P^c=[X,Y,Z]^T$	静止世界点在相机系中的坐标	3×1 / m	c：右、下、前；Z>0
$u,v,f_x,f_y,c_u,c_v$	像素坐标、焦距、主点	各为标量 / pixel	图像；v不是本课速度符号
$x,y,s$	归一化像点、特征堆叠向量	无量纲；s为m×1	理想针孔平面
$\xi_c=[v_c^T,\omega_c^T]^T$	相机原点速度与角速度	6×1 / m/s、rad/s	表达在相机系
$L_s,e_s,\lambda_s$	交互矩阵、特征误差、收敛增益	m×6；m×1；1/s	$e_s=s-s^*$
$R_b^c,r_{bc}^b,A_{cb}$	机体到相机旋转、机体原点到相机杆臂、速度映射	3×3；3×1 m；6×6	b：FRD
$W,H,a,\tau_f$	图宽高、归一化框面积、滤波时间常数	pixel；pixel；无量纲；s	检测框特征
$u_c,v_c,w,h,K,s^*$	框中心像素、框宽高、面积比例常数、期望特征	pixel；pixel；pixel；pixel；m²；m×1 无量纲	此处框中心 $v_c$ 是标量，与带向量语境的相机速度分开
$V_s,\bar s,\Delta t_k$	特征误差能量、滤波特征、采样间隔	标量无量纲；m×1 无量纲；s	归一化特征；m 在矩阵维数中是特征个数

假设目标点在世界静止、相机刚性安装、外参不随时间变化、畸变已校正。水下平窗折射会影响理想针孔近似，应在实际光学配置下使用相容标定；本课不把压力测得的水深当作目标的光轴深度。图像因缩放或填边进入检测器后，先把检测坐标映回原图，才能使用原内参。

从商的导数展开到 2×6 矩阵

\begin{aligned}u&=f_xX/Z+c_u,\quad v=f_yY/Z+c_v,\\x&=(u-c_u)/f_x=X/Z,\quad y=(v-c_v)/f_y=Y/Z,\\\dot x&=\frac{\dot XZ-X\dot Z}{Z^2}=\frac{\dot X-x\dot Z}{Z},\\\dot y&=\frac{\dot Y-y\dot Z}{Z}.\end{aligned}

(4.12)

第一步是针孔相似三角形，第二步把像素尺度除去，第三步仅用商法则。相机向右移动而世界点不动，点在相机坐标中向左移动；相机旋转也使坐标轴相对世界转动，因此点的坐标导数带负角速度叉乘。负号来自“运动的是坐标系”，不是控制器故意取负反馈。

\begin{aligned}\dot P^c&=-v_c-\omega_c\times P^c,\\\dot X&=-v_{cx}-\omega_{cy}Z+\omega_{cz}Y,\\\dot Y&=-v_{cy}-\omega_{cz}X+\omega_{cx}Z,\\\dot Z&=-v_{cz}-\omega_{cx}Y+\omega_{cy}X.\end{aligned}

(4.13)

\begin{aligned}\dot x&=-\frac{v_{cx}}Z+\frac{xv_{cz}}Z+xy\omega_{cx}-(1+x^2)\omega_{cy}+y\omega_{cz},\\\dot y&=-\frac{v_{cy}}Z+\frac{yv_{cz}}Z+(1+y^2)\omega_{cx}-xy\omega_{cy}-x\omega_{cz},\\L_{xy}&=\begin{bmatrix}-1/Z&0&x/Z&xy&-(1+x^2)&y\\0&-1/Z&y/Z&1+y^2&-xy&-x\end{bmatrix},\\\begin{bmatrix}\dot x\\\dot y\end{bmatrix}&=L_{xy}\xi_c.\end{aligned}

(4.14)

逐项检查第一行：相机右移使像点左移，故第一列为负；相机上下平移在理想针孔下不直接改变横像点，第二列为零；相机向前接近，离中心的点向外扩散，故第三列含 x/Z。绕相机 x 轴转动时，Y 与 Z 都变，Z 的变化经商法则带来 xy；绕相机 y 轴时，X 的直接变化给出负一，Z 的变化再给出负 x²；绕光轴旋转产生 y。第二行以同样方法逐项收集，光轴旋转对应负 x。平移三列单位为 1/m，乘 m/s 后得到 1/s；旋转列无量纲，弧度视为无量纲，所以也得到归一化像速 1/s。

数值例：光轴深度 2 m，归一化位置 (0.2,−0.1)，相机纯右移 0.1 m/s，则像点速度为 (−0.05,0)/s；焦距 500 pixel 时等于向左 25 pixel/s。若改为绕相机 y 轴正转 0.1 rad/s，则横像速为 −0.104/s，约向左 52 pixel/s。第二种运动与深度无关，这解释为什么远处目标仍明显响应转动，而对平移不敏感。

真正逐列展开，而不只把最终矩阵背下来

把点运动学代入商法则，先不合并项：

\begin{aligned} \dot x&=\frac{-v_{cx}-\omega_{cy}Z+\omega_{cz}Y -x(-v_{cz}-\omega_{cx}Y+\omega_{cy}X)}{Z},\\ \dot y&=\frac{-v_{cy}-\omega_{cz}X+\omega_{cx}Z -y(-v_{cz}-\omega_{cx}Y+\omega_{cy}X)}{Z}. \end{aligned}

此时只用 $X/Z=x$ 、 $Y/Z=y$ 整理。第二行第一列为零，因为沿相机右轴的平移既不直接改变 $Y$ ，也不改变 $Z$ ；第二列的 $-1/Z$ 来自相机下移后静止目标相对向上；第三列 $y/Z$ 来自前进造成的投影放大。第四列的 $1+y^2$ 有两个来源：绕相机右轴转动直接改变 $Y$ 给出正一，同时改变 $Z$ 再给出正 $y^2$ 。第五列 $-xy$ 只由分母的变化产生；第六列 $-x$ 是绕光轴转动时像点在平面内旋转。现在六列分别对应“只打开一种单位速度”的实验，而不是十二个没有来历的系数。

手算一整行。 取 $P^c=(0.4,-0.2,2)$ m，可得 $x=0.2,y=-0.1$ 。于是

L_{xy}=\begin{bmatrix} -0.5&0&0.1&-0.02&-1.04&-0.1\\ 0&-0.5&-0.05&1.01&0.02&-0.2 \end{bmatrix}.

前三列的数值单位为 $1/\mathrm m$ ，后三列无量纲。令相机速度同时有 $v_{cx}=0.1$ m/s、 $v_{cz}=0.2$ m/s、 $\omega_{cy}=0.1$ rad/s，其余分量为零，则 $\dot x=-0.05+0.02-0.104=-0.134/\mathrm s$ ， $\dot y=-0.01+0.002=-0.008/\mathrm s$ 。这说明“中心误差只影响 yaw”是一种主动选择的控制策略，不是成像几何天然解耦。

有限差分不是把矩阵公式换个写法再比一遍：参考代码只从 $\dot P=-v-\omega\times P$ 生成正、负微小点位移，再重新做透视除法，检查差分像速是否等于对应矩阵列。正负扰动抵消一阶截断误差之外的偶次项；步长太大有非线性误差，太小又会放大浮点相减误差。因此示例选 $10^{-7}$ 的小步，并用 $10^{-7}$ 的误差界检查，而不是要求双精度完全相等。

由特征误差到可执行自由度

\begin{aligned}e_s&=s-s^*,\qquad\dot e_s=L_s\xi_c,\\\xi_c&=-\lambda_s L_s^+e_s,\\\dot e_s&=-\lambda_s L_sL_s^+e_s,\\\dot V_s&=-\lambda_s e_s^TL_sL_s^+e_s\le0,\quad V_s=\tfrac12e_s^Te_s.\end{aligned}

(4.15)

这个下降结论使用真实的当前交互矩阵，并假设相机速度理想跟踪。实际把近似深度代入得到 $\widehat L_s$ 时，能量导数变为 $\dot V_s=-\lambda_s e_s^TL_s\widehat L_s^+e_s$ ，该乘积不再自动是正半定投影。因此近似深度、外参误差与速度饱和须进入实际闭环分析；不能把精确模型的非正结论原封不动套给任意估计矩阵。

这里视觉误差采用“当前减期望”，与前章位置误差“期望减当前”相反，所以速度律带负号。伪逆使像速尽量接近期望的负比例误差，并选取最小范数速度；矩阵乘积是特征空间的投影。只有误差处在可实现方向，或交互矩阵满行秩时，才得到完整指数误差收敛。单点只有两条标量约束，不能唯一决定六自由度；即使画面中心误差为零，相机仍可沿某些方向运动。多个点提供更多约束，也要避免几何退化和视场丢失。Chaumette 与 Hutchinson 的入门教程适合作为原式核对，TB04-S5。

实际 ROV 往往只让视觉控制前后、侧向、升沉、偏航，俯仰和横滚由姿态环稳定。设允许控制向量经固定映射产生机体速度，就先把交互矩阵与该映射相乘，再求受限问题，而不是求出任意六维相机速度后粗暴删掉不想要的角速度。阻尼最小二乘也必须先统一平移、转动和各特征的尺度，否则“最小范数”会隐含任意米与弧度权重。

伪逆背后的最小二乘问题，以及为什么不能随意删自由度

控制目标先写成一个线性方程：希望像速等于 $-\lambda_s e_s$ ，因此寻找 $L_s\xi_c=-\lambda_s e_s$ 的解。若方程无精确解，就最小化残差平方；若有很多解，伪逆选择其中最小范数者。对单点， $L_s$ 仅有两行，存在多维零空间；满足 $L_s\xi_0=0$ 的速度不会改变这个点的像坐标。画面中目标保持居中，并不意味着机器人已确定三维距离和全部姿态。

当控制通道是 $u\in\mathbb R^r$ ，其单位按通道分别为 m/s 或 rad/s，已知映射 $S\in\mathbb R^{6\times r}$ 将它转成机体速度，则应先写

\dot e_s=L_s A_{cb}S u=J_u u, \qquad u=-\lambda_s J_u^+e_s.

$A_{cb}$ 在下一节推导，负责原点与坐标转换； $S$ 选择或组合真正允许的自由度。假设这里只允许相机右移和下移， $u=[v_{cx},v_{cy}]^T$ ，那么 $J_u=\operatorname{diag}(-1/Z,-1/Z)$ ，控制律直接为 $u=\lambda_s Z e_s$ 。目标在右侧时，应使相机向右移，静止目标在画面中便向左回中心。取 $Z=2$ m、 $e_s=(0.2,-0.1)$ 、 $\lambda_s=0.5/\mathrm s$ ，得到速度 $(0.2,-0.1)$ m/s。这个简单例子可以手算求逆；一般多点问题则用数值分解求解，不用显式求大矩阵逆。

精确矩阵时， $L_sL_s^+$ 是对可实现像速子空间的正交投影，故能量导数非正。使用估计矩阵时， $L_s\widehat L_s^+$ 不再是该正交投影；深度取错甚至坐标符号写反，可能产生错误反馈。参考代码检查的是运动学雅可比，不是在声称近似模型、执行滞后与饱和下的完整视觉系统已被证明稳定。

相机到 FRD：为何不能只转三维向量

\begin{aligned}R_c^b&=\begin{bmatrix}0&0&1\\1&0&0\\0&1&0\end{bmatrix},\qquad R_b^c=(R_c^b)^T,\\v_c&=R_b^c(v^b+\omega^b\times r_{bc}^b),\quad\omega_c=R_b^c\omega^b,\\\xi_c&=A_{cb}\xi_b,\\A_{cb}&=\begin{bmatrix}R_b^c&-R_b^c[r_{bc}^b]_\times\\0_{3\times3}&R_b^c\end{bmatrix},\qquad L_b=L_sA_{cb}.\end{aligned}

(4.16)

矩阵仅适用于相机光轴沿机体前方、相机右向沿机体右方、相机下向沿机体下方的正装情况。杆臂从机体参考点指向相机；刚体转动时相机原点速度多出角速度叉乘杆臂，交换叉乘次序得到右上角负叉乘矩阵。这就是刚体速度的伴随变换，包含旋转和不同原点，不是仅把三个轴换名。若直接求逆映射，也要保留杆臂项。

举例：相机位于机体前方 0.2 m，机体原点不动而正偏航 0.1 rad/s。相机在机体系产生向右 0.02 m/s 速度，再加相机 y 轴方向的正角速度。目标位于光轴中心、Z=2 m 时，像点横速度为 −0.01−0.1=−0.11/s。漏掉杆臂会少算约 9.1% 的该瞬时像速；这也说明“原地转艏”不是“相机只有旋转”。

框面积、身份与时间滤波

\begin{aligned}e_x&=\frac{u_c-W/2}{W/2},\quad e_y=\frac{v_c-H/2}{H/2},\quad a=\frac{wh}{WH},\\a&\approx K/Z^2,\qquad\frac Z{Z^*}\approx\sqrt{\frac{a^*}{a}},\\\alpha_k&=1-\exp(-\Delta t_k/\tau_f),\quad\bar s_k=(1-\alpha_k)\bar s_{k-1}+\alpha_ks_k.\end{aligned}

(4.17)

这里框中心归一化与针孔归一化是两套不同变量：一个除图像半宽，另一个除标定焦距，数值不应混用。面积公式要求目标尺寸、投影朝向和可见比例近似固定。面积缩为四分之一可能是距离翻倍，也可能是遮挡或目标转身；中心加面积只提供三个标量，不会自动恢复六维米制位姿。对同类多目标，应先做身份关联，再滤波特征，否则滤波器只是把两个目标混成一个虚构中间点。最后一行由一阶低通在采样区间内输入不变的精确解得到，改变帧率也能保持相近的物理时间常数。

项目 decision_logic.py::make_observation 使用中心与面积特征；640×480 图像、中心 (384,216)、框 80×120 给出 (0.2,−0.1,0.03125)。orbit_logic.py 的中心距离、面积比例与连续候选确认承担轻量关联。它们是可解释的特征策略，不必误称为整套六自由度 IBVS；上面的雅可比用来分析符号、耦合与可观测方向。

先手写，再展开参考实现

将本课公式写成一个 C++17 单文件程序。先列输入单位与退化分支，再写核心循环；不要先看答案再照抄。参考程序只依赖标准库，可用 g++ -std=c++17 -O2 lesson.cpp -o lesson && ./lesson 离线运行，不访问机器人。断言是对上文手算结果的检查，不是用程序输出代替推导。

参考解答（展开查看）

// 教学实现：逐列中心差分验证 L，不连接相机或机器人。
#include <array>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <cassert>
using V=std::array<double,3>;using F=std::array<double,2>;
using Mat=std::array<std::array<double,6>,2>;
V cross(V a,V b){return {a[1]*b[2]-a[2]*b[1],a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]};}
F project(V P){assert(P[2]>0);return {P[0]/P[2],P[1]/P[2]};}
Mat interaction(V P){
    auto f=project(P);double x=f[0],y=f[1],z=P[2];
    return {{{-1/z,0,x/z,x*y,-(1+x*x),y},
             {0,-1/z,y/z,1+y*y,-x*y,-x}}};
}
int main(){
    V P{.4,-.2,2};auto L=interaction(P);double eps=1e-7,maxError=0;
    for(int j=0;j<6;++j){
        V v{},w{};if(j<3)v[j]=1;else w[j-3]=1;
        V wxP=cross(w,P),plus=P,minus=P;
        for(int k=0;k<3;++k){
            double dP=-v[k]-wxP[k];plus[k]+=eps*dP;minus[k]-=eps*dP;
        }
        F fp=project(plus),fm=project(minus);
        for(int row=0;row<2;++row)
            maxError=std::max(maxError,std::abs((fp[row]-fm[row])/(2*eps)-L[row][j]));
    }
    assert(maxError<1e-7);
    assert(std::abs(L[0][0]*.1+.05)<1e-12);
    // 正装相机 R_b^c 将 FRD 向量变为 [right,down,front]。
    V rb{.2,0,0},omega_b{0,0,.1};V vc_b=cross(omega_b,rb);
    V vc{vc_b[1],vc_b[2],vc_b[0]},wc{omega_b[1],omega_b[2],omega_b[0]};
    Mat center=interaction({0,0,2});double xdot=0;
    for(int k=0;k<3;++k)xdot+=center[0][k]*vc[k]+center[0][k+3]*wc[k];
    assert(std::abs(xdot+.11)<1e-12);
    std::cout<<"Jacobian error "<<maxError<<"; lever-arm xdot "<<xdot<<'\n';
}

练习与答案线索

只用商法则与点运动学，重推交互矩阵第二行第六列。
深度从 $2$ m 改为 $20$ m，哪些列改变、哪些列不变？
构造两个检测框交叉序列，比较最高置信度选择与身份连续选择。
将正装相机外参误设成单位阵，会把哪条物理轴解释错？

参考解答（展开查看）

第一题遮住公式，仅从商法则推导第二行第六列。答案：绕光轴时 Z 不变，而 Y 的导数为负角速度乘 X，除以 Z 得负 x。第二题将深度从 2 m 改到 20 m：平移三列缩小十倍，旋转三列不变。第三题写两个框交叉的序列，比较最高分选择与带中心/尺度门限的身份连续选择；答案应讨论关联失败，而不是把低通调得无限慢。第四题将外参误当恒等阵：相机向右的线速度会被当作机体前速，物理轴错了，任何增益整定都不能修复该错误。

从检测框到控制器：每个阶段只做一件事

图像先保留曝光时刻；检测框从网络缩放坐标映回原图；身份关联选定目标；标定模型把像素变成特征；特征及其深度估计决定交互矩阵；外参伴随与允许自由度决定可执行雅可比；制导求受限参考；最后由桥接按飞控模式解释。项目的 decision_logic.py::make_observation 位于“框到特征”，orbit_logic.py 同时承担身份连续、任务策略和参考整形，OrbitPublisher 给参考附加会话与时间。它们不等于飞控内部速度 PID。把视觉中心反馈的输出究竟是 rad/s、m/s 还是归一化命令写在接口旁，才能正确把本课几何与实际 ArduSub 通道连接起来。

TB04-S5 · Chaumette & Hutchinson：Visual Servo Control, Part I: Basic Approaches

2006 原论文，优先读 Basic Components、Interaction Matrix、Approximating the Interaction Matrix。本课按指定坐标自行展开每一列，并用差分检查。