00.6｜旋转如何求导：从小扰动到 ESKF、机械臂与视觉雅可比

区分时间导数、参数导数、局部扰动雅可比与机械臂几何雅可比，逐步推导正逆旋转、SE(3) 点变换和相机重投影导数。

1. 先问：到底对什么求导

看到“旋转的雅可比”，必须先写清输入、输出、坐标系和扰动约定。至少有四种常见问题：

时间导数：机器人现在怎么转，求 $\dot R$ ，单位含每秒。
参数导数：改变欧拉角或旋转向量的某个分量，对矩阵或测量有多大影响。
局部扰动雅可比：在当前姿态附近乘一个小旋转，测量如何变化。这是 ESKF、SLAM 最常用的形式。
机械臂几何雅可比：关节速度如何映射为末端线速度与角速度，输入维数是关节数。

它们都叫雅可比，却不是同一矩阵。本节遵守 $R=R_{nb}$ 、列向量、右局部扰动 $R'=R\operatorname{Exp}(\delta\theta)$ 。 $\delta\theta$ 在机体系表达，单位 rad。暂时只保留增量的一阶项，高阶项在增量趋零时消失。

2. 正向旋转一个向量：负号从哪里来

设已知机体系向量 $a$ ，预测导航系向量 $y=Ra$ 。例如 $a$ 是去偏置后的 IMU 比力， $y$ 是旋转到导航系的比力。把姿态扰动代入：

y'=R\operatorname{Exp}(\delta\theta)a \simeq R(I+[\delta\theta]_\times)a =Ra+R(\delta\theta\times a).

用叉乘反交换关系 $\delta\theta\times a=-a\times\delta\theta$ ：

y'-y\simeq-R[a]_\times\delta\theta, \qquad J_{y,\theta}=-R[a]_\times.

矩阵为 $3\times3$ ，输入三个转角增量，输出三个向量分量变化。其单位是输出量单位/rad。例如 $a$ 为 m/s² 时，雅可比单位是 m/s²/rad。

用最简单的例子判断符号： $R=I$ 、 $a=[1,0,0]^T$ ，绕 $z$ 轴增加一个正小角 $\epsilon$ ，向量应向正 $y$ 方向转，故 $\delta y=[0,\epsilon,0]^T$ 。代入 $-[a]_\times[0,0,\epsilon]^T$ 正好如此。

如果改用左扰动 $R'=\operatorname{Exp}(\delta\theta^n)R$ ，同样展开会得到 $J=-[Ra]_\times$ 。两个式子并不矛盾，因为 $\delta\theta^n=R\delta\theta^b$ ，于是 $-[Ra]_\times R=-R[a]_\times$ 。看清输入坐标，才能判断两个论文公式是否真的不同。

3. 逆向旋转：为什么 DVL 的符号经常相反

设导航系速度为 $v^n$ ，DVL 预测机体系速度 $y=R^Tv^n$ ，先忽略杆臂与外参。右扰动后

y'=\big(R\operatorname{Exp}(\delta\theta)\big)^Tv^n =\operatorname{Exp}(-\delta\theta)R^Tv^n \simeq y-[\delta\theta]_\times y.

再次利用叉乘关系，得到

\delta y=[y]_\times\delta\theta+R^T\delta v^n, \qquad H_\theta=[R^Tv^n]_\times,\quad H_v=R^T.

这里正号不是“DVL 的特殊规定”，而是对逆旋转求导得到的结果。完整误差状态顺序为 $[\delta p,\delta v,\delta\theta,\delta b_a,\delta b_g]$ 时，无杆臂的三维速度观测矩阵是

H=\begin{bmatrix}0_{3\times3}&R^T&[R^Tv^n]_\times&0&0\end{bmatrix}\in\mathbb R^{3\times15}.

这个矩阵把十五维误差投影成三维测量误差。三维观测并不要求系统只有三维状态。加入 DVL 安装旋转、杆臂、角速度后，分别通过链式法则增加相应块，详见第二章传感器建模；不要在这里凭直觉把零块全部填满。

若滤波器定义新息 $r=z-h(\hat x)$ ，使用的是测量函数导数 $H=\partial h/\partial\delta x$ ，因为 $r\simeq H\delta x+n$ 。若优化器把残差定义成 $h(x)-z$ 或 $z-h(x)$ ，残差导数符号可能相反。先区分测量模型雅可比与残差雅可比，再讨论正负号。

4. 位姿同时扰动：三维点的六维雅可比

定义 $T=[R,p;0,1]$ 把机体系坐标变为导航系坐标，位姿右扰动采用平移在前的 $\delta\xi=[\delta\rho^T,\delta\phi^T]^T$ 。由 SE(3) 的一阶指数

T'=T\operatorname{Exp}_{SE(3)}(\delta\xi) \simeq\begin{bmatrix}R(I+[\delta\phi]_\times)&p+R\delta\rho\\0&1\end{bmatrix}.

所以对固定机体系点 $a$ ， $y=Ra+p$ 的变化为

\delta y=R\delta\rho-R[a]_\times\delta\phi, \qquad J_{y,\xi}=\begin{bmatrix}R&-R[a]_\times\end{bmatrix}.

注意这里平移扰动 $\delta\rho$ 在机体系，不是直接加在导航系位置上的 $\delta p^n$ 。若选的是“导航系加性位置误差加右姿态误差”的混合 ESKF 状态，位置块就应是 $I$ 而不是 $R$ 。这是一个常见陷阱：同样六维，不代表同一种参数化。

反过来，把固定的世界点 $P$ 变到机体系： $y=R^T(P-p)$ 。从 $T'^{-1}=\operatorname{Exp}(-\delta\xi)T^{-1}$ 出发，直接展开

y'\simeq (I-[\delta\phi]_\times)y-\delta\rho, \qquad J_{y,\xi}=\begin{bmatrix}-I&[y]_\times\end{bmatrix}.

这个 $3\times6$ 矩阵是视觉重投影、点云配准和标定中极常见的基础块。它成立的前提是 $T$ 表示相机/机体到世界的位姿，而不是反向变换；变换方向改变后不能直接照搬。

5. 从三维点到像素：链式法则真的怎么用

设相机中的点 $y=[X,Y,Z]^T$ ， $Z>0$ 。先用无畸变针孔模型

u=f_x\frac{X}{Z}+c_x,\qquad v=f_y\frac{Y}{Z}+c_y.

这里 $u,v,c_x,c_y$ 单位是像素， $f_x,f_y$ 是以像素表示的焦距， $X,Y,Z$ 可以统一用 m。对三个坐标分别求偏导：

J_\pi=\frac{\partial(u,v)}{\partial(X,Y,Z)} =\begin{bmatrix} f_x/Z&0&-f_xX/Z^2\\ 0&f_y/Z&-f_yY/Z^2 \end{bmatrix}.

例如 $\partial u/\partial Z=-f_xX/Z^2$ ：同一个侧向距离 $X$ ，离相机更远时像素偏移更小，因此是负号。它的单位是像素/m。

若待估计位姿是相机到世界 $T_{wc}$ ，固定地标为 $P^w$ ，上节的逆变换雅可比适用。定义残差为预测像素减观测像素 $e=\pi(T_{wc}^{-1}P^w)-z$ ，得到

J_e=J_\pi\begin{bmatrix}-I&[y]_\times\end{bmatrix}\in\mathbb R^{2\times6}.

链式法则在这里就是先算“位姿动一点，三维点坐标动多少”，再算“三维点动一点，像素动多少”。两块相乘，三维中间变量消去。若残差改成 $z-\pi(\cdot)$ ，则整个矩阵取负，正规方程中的右端也应同步。

数例： $f_x=f_y=500$ px， $y=[1,0,5]^T$ m，主点先取零。预测像素为 $[100,0]^T$ px。如果相机沿自己的正 $x$ 方向移动 $\delta\rho_x=0.01$ m，静止地标相对相机应向负 $x$ 方向移动，故像素约变化 $-1$ px。由 $J_\pi[-I,\cdots]$ 计算也是 $-(500/5)\times0.01=-1$ px。

真实相机还可能有畸变、滚动快门、相机与 IMU 外参及水下折射。它们应增加到测量函数里再求导；把针孔公式套到所有水下光学结构上并不会自动正确。

6. 旋转向量雅可比为何还有左右两种

局部扰动不是旋转向量的直接相加。设当前旋转是 $R(\phi)=\operatorname{Exp}(\phi)$ ，改变它的坐标为 $\phi+\delta\phi$ 。我们希望找到等效的右乘小角 $\delta\theta$ ：

\operatorname{Exp}(\phi+\delta\phi) \simeq\operatorname{Exp}(\phi)\operatorname{Exp}(J_r(\phi)\delta\phi).

$J_r$ 将“旋转向量的坐标增量”变成“当前姿态处的右局部增量”。对指数矩阵的微分，可将变化均匀插入旋转过程各时刻，得到积分表达

J_r(\phi)=\int_0^1\operatorname{Exp}(-s\phi)\,ds =I-\frac{1-\cos\theta}{\theta^2}[\phi]_\times +\frac{\theta-\sin\theta}{\theta^3}[\phi]_\times^2.

积分式说明：扰动作用在不同瞬间，其方向要先旋转到统一的最终局部坐标才能相加。对 Rodrigues 公式逐项积分即可得到右边闭式。左侧版本对应

\operatorname{Exp}(\phi+\delta\phi) \simeq\operatorname{Exp}(J_l(\phi)\delta\phi)\operatorname{Exp}(\phi), \quad J_l(\phi)=J_r(-\phi)=\operatorname{Exp}(\phi)J_r(\phi).

因此 $J_l$ 正是上一节 SE(3) 平移积分中的 $V$ 。这里的 $J_r,J_l$ 为 $3\times3$ ；它们不是机械臂的 $6\times n$ 几何雅可比，也不是某个传感器的 $m\times15$ 观测矩阵。

当角度趋零， $J_r\simeq I-\tfrac12[\phi]_\times$ ， $J_l\simeq I+\tfrac12[\phi]_\times$ 。而有限非零角度下，如果要对 $y=\operatorname{Exp}(\phi)a$ 的旋转向量参数求导，必须再连一层：

\frac{\partial y}{\partial\phi} =-R[a]_\times J_r(\phi).

直接写 $-R[a]_\times$ 相当于改成了右局部扰动参数，不是同一个导数。

Log 的导数和数值分支

在选定的良好局部分支内，右雅可比可逆，

\operatorname{Log}(R\operatorname{Exp}(\delta\theta)) \simeq\phi+J_r^{-1}(\phi)\delta\theta, \qquad \phi=\operatorname{Log}(R).

对 $0<\theta<\pi$ 可写成

J_r^{-1}(\phi)=I+\frac12[\phi]_\times+ \left(\frac1{\theta^2}-\frac{1+\cos\theta}{2\theta\sin\theta}\right)[\phi]_\times^2.

小角时括号系数趋近 $1/12$ ，应采用级数；接近 $\pi$ 时可改用半角 cot 形式改善计算，但这不会消除主值 Log 本身的分支不连续。在接近 $2\pi$ 等非主值旋转向量处雅可比可能奇异，不能随意求逆。

7. ESKF 注入后为什么还要“重置协方差”

滤波求得小姿态修正 $d$ 后，名义旋转从 $\hat R$ 改成 $\hat R^+=\hat R\operatorname{Exp}(d)$ 。真实姿态不因为软件改了名义值而变化；旧误差 $\delta\theta$ 与新误差 $\delta\theta^+$ 应满足

\hat R\operatorname{Exp}(\delta\theta) =\hat R\operatorname{Exp}(d)\operatorname{Exp}(\delta\theta^+), \quad \operatorname{Exp}(\delta\theta^+)=\operatorname{Exp}(-d)\operatorname{Exp}(\delta\theta).

把旧误差写成修正中心加剩余误差 $\delta\theta=d+\epsilon$ 。利用上一节右雅可比定义：

\operatorname{Exp}(d+\epsilon) \simeq\operatorname{Exp}(d)\operatorname{Exp}(J_r(d)\epsilon).

左乘 $\operatorname{Exp}(-d)$ 后得到 $\delta\theta^+\simeq J_r(d)\epsilon$ 。因此姿态误差坐标已经换了，协方差也必须按雅可比变换：

P_{\mathrm{new}}=G\,P_{\mathrm{corrected}}G^T, \quad G_{\theta\theta}=J_r(d)\simeq I-\frac12[d]_\times.

对本教材的加性位置、速度、偏置误差，其余对角块为单位阵；应对整张协方差做变换，让姿态与其他状态的交叉块也同步变化。这里的 $P_{\mathrm{corrected}}$ 是测量更新后的误差协方差，不是传感器噪声 $R$ 。

“误差均值清零”是把修正吸收到名义状态中，不是宣称真实误差变成零，更不是把协方差清零。大姿态误差时，一次局部线性修正可能不够，仍需重线性化或更合适的初始化。

8. 机械臂的几何雅可比与这些导数怎样连接

设第 $i$ 个转动关节的轴方向在基座系中为 $a_i$ ，轴上一点为 $o_i$ ，末端位置为 $p_e$ 。转速 $\dot q_i$ 造成的末端速度是刚体旋转关系：

\dot p_e=a_i\times(p_e-o_i)\dot q_i, \qquad \omega_e=a_i\dot q_i.

于是线速度在前的几何雅可比第 $i$ 列为

J_i=\begin{bmatrix}a_i\times(p_e-o_i)\\a_i\end{bmatrix}.

直线关节第 $i$ 列则为 $[a_i^T,0^T]^T$ 。把所有列拼起来， $[\dot p_e^T,\omega_e^T]^T=J(q)\dot q$ 。这里上半部分确实是末端点速度，不能与某些空间 twist 表示中的 $v_s$ 混淆。

这与旋转扰动导数的联系是：关节改变先产生局部位姿变化，局部位姿变化再影响末端点或相机像素。把这些函数链相乘，就是视觉伺服或手眼标定需要的雅可比。具体机械臂变换链与求逆将在后续运动学节展开。

9. 如何用有限差分独立验算导数

公式推导正确仍可能在代码中写反索引。对函数 $f$ 和第 $j$ 个局部方向 $e_j$ ，采用中心差分

J_{:,j}^{\mathrm{FD}} =\frac{f(R\operatorname{Exp}(\epsilon e_j))-f(R\operatorname{Exp}(-\epsilon e_j))}{2\epsilon}.

这里要用合法的旋转扰动，不能直接给旋转矩阵某个元素加 $\epsilon$ ，那样测试的是另一个问题。通常可尝试 $10^{-4}$ 到 $10^{-7}$ rad 的多个步长；太大有截断误差，太小有消减舍入误差。中心差分与解析式接近，不替代定义检查，因为两段代码可能共享同一错误约定。

展开 C++17：独立检查正向、逆向旋转的雅可比

将下面代码与 00.5 的矩阵辅助函数一起使用：保留其 V、M、identity、skew、mm、mv、polynomial、norm、expSO3、leftJ 定义，替换原来的 main，而不是在同一文件保留两个 main。

M trans(M a){
 M b{};
 for(int i=0;i<3;i++){for(int j=0;j<3;j++){b[i][j]=a[j][i];}}
 return b;
}
V sub(V a,V b){return {a[0]-b[0],a[1]-b[1],a[2]-b[2]};}
int main(){M r=expSO3({.3,-.2,.7});V a{1.2,-.4,2.0};
 M jf=mm(r,skew(a));for(auto& row:jf)for(double& x:row)x=-x;
 V y=mv(trans(r),a);M ji=skew(y);double eps=1e-6;
 for(int j=0;j<3;j++){
  V dp{},dm{};dp[j]=eps;dm[j]=-eps;
  M rp=mm(r,expSO3(dp)),rm=mm(r,expSO3(dm));
  V df=sub(mv(rp,a),mv(rm,a));
  V di=sub(mv(trans(rp),a),mv(trans(rm),a));
  for(int i=0;i<3;i++){
   assert(std::abs(df[i]/(2*eps)-jf[i][j])<1e-8);
   assert(std::abs(di[i]/(2*eps)-ji[i][j])<1e-8);
  }
 }
 // 参数加法的导数必须额外乘右雅可比。
 V phi{.3,-.2,.7};M jr=leftJ({-phi[0],-phi[1],-phi[2]});
 M jp=mm(jf,jr);
 for(int j=0;j<3;j++){
  V pp=phi,pm=phi;pp[j]+=eps;pm[j]-=eps;
  V d=sub(mv(expSO3(pp),a),mv(expSO3(pm),a));
  for(int i=0;i<3;i++)assert(std::abs(d[i]/(2*eps)-jp[i][j])<1e-8);
 }
}

建议自行加两组测试：相机三维点到像素的链式雅可比；位姿原点更改时的伴随变换。先写数值测试再接入飞控，可避免在实机上用 PID 调参掩盖坐标导数错误。

10. 阅读代码时的检查清单

四元数/矩阵表示机体到世界还是世界到机体？
增量左乘还是右乘，向量在哪个系表达？
待估计的是普通位置加右姿态，还是完整 SE(3) 右扰动？
雅可比对局部增量求导，还是对旋转向量、欧拉角参数求导？
使用测量函数的导数，还是预测减观测的残差导数？
twist 与几何速度的排列是线在前还是角在前，平移分量是否等于末端点速度？
有限差分是否使用了同一种扰动和单位？

这些问题都明确后，EKF、图优化、机械臂逆解和视觉伺服中的许多“完全不同的公式”，其实只是同一个链式法则作用于不同输入输出。

原始资料

Solà 等：A micro Lie theory for state estimation in robotics：对照局部雅可比、链式法则、群作用和附录公式。
Solà：Quaternion kinematics for the error-state Kalman filter：核对右局部误差、注入与重置。
Modern Robotics 第五章：Velocity Kinematics and Statics：区分几何速度、空间/本体雅可比与奇异性。

本节给出的传感器、像素与数值差分案例是按统一约定独立展开的教学算例；具体项目仍应对其实际测量函数逐项求导，而不是只替换变量名。

旋转如何求导：从小扰动到 ESKF、机械臂与视觉雅可比