激光SLAM后端优化——雅克比矩阵推导

本文详细介绍了激光SLAM后端优化中雅克比矩阵的推导过程,以及如何扩展系统状态变量。讨论了雅克比矩阵在EKF系统中的应用,同时阐述了状态变量包括IMU、相机和激光雷达的状态,并展示了状态扩展时协方差矩阵的更新规则。通过实例解释了雅克比矩阵在多维函数中的作用,最后给出了Jacobian矩阵的具体形式。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

激光SLAM后端优化——雅克比矩阵推导

Jacobi Matrix

In the EKF system, the maintained state quantities include three categories: IMU state, camera state, and Lidar state, which are expressed as follows:

X i m u = [ n q b T b g T n v b T b a T n p b T ] X_{imu}=\begin{bmatrix}^nq^T_b &b^T_g &^nv^T_b &b^T_a & ^np^T_b \end{bmatrix} Ximu=[nqbTbgTnvbTbaTnpbT]

X c a m = [ G q c 0 T G p c 0 T ] X_{cam}=\begin{bmatrix}^Gq^T_{c_0} & ^Gp^T_{c_0} \end{bmatrix} Xcam=[Gqc0TGpc0T]

X l i d a r = [ G q L T G p L T ] X_{lidar}=\begin{bmatrix}^Gq_L^T & ^Gp^T_L \end{bmatrix} Xlidar=[GqLTGpLT]

Note: q stands for attitude and p stands for position.Normally,G stands for Earth-Centered,Earth-Fixed Coordinate System,n stands for Navigation System.

We combine the above three state variables into a total state variable,which are expressed as follows:

X o l d = [ X i m u ( X c a m ) N ( X l i d a r ) M ] X_{old}=\begin{bmatrix}X_{imu} & (X_{cam})_N & (X_{lidar})_M \end{bmatrix} Xold=[Ximu(Xcam)N(Xlidar)M]

So far, we have obtained the initial state variables of the entire system. And then,Let’s try to expand our state variables.

If we get a new lidar scan,we need to expand X X X just like this:

X n e w = [ X i m u ( X c a m ) N ( X l i d a r ) M X l i d a r n e w ] = [ X o l d X l i d a r n e w ] X_{new}=\begin{bmatrix} X_{imu} & (X_{cam})_N & (X_{lidar})_M & X_{lidar}^{new} \end{bmatrix}=\begin{bmatrix} X_{old}& X_{lidar}^{new} \end{bmatrix} Xnew=[Ximu(Xcam)N(Xlidar)MXlidarnew]=[XoldXlidarnew]

For the covariance matrix, it can be expressed in the form of the following figure, in which the green blocks represent non-zero blocks, the blue blocks are all zero matrixes, and the orange blocks represent the content to be obtained when extending the lidar state of the k+1 th frame.
加入一阵激光雷达后状态扩维

The expanded covariance is:

P n e w = [ P o l d P O L k + 1 P L k + 1 O P L L l + 1 ] P_{new}=\begin{bmatrix}P_{old} & P_{OL_{k+1}} \\ P_{L_{k+1}O} & P_{LL_{l+1}} \end{bmatrix} Pnew=[PoldPLk+1OPOLk+1PLLl+1]

📌 Let’s discuss some knowledge about the Jacobian matrix.For simplicity, we use a one-dimensional function as an example.If there is function curve y=f(x,t),and this is a point (x_0,y_0) in the curve i.e. y_0=f(x_0,t_0).

If I want to know the value of a point at t 1 t_1 t1 on the curve that is very close to the point ( x 0 , y 0 ) (x_0,y_0) (x0,y0).If I know the X coordinate of this point is x 1 x_1 x1,so how can I get the Y coordinate of this point if the function at t 1 t_1 t1 is very similar to that at t 0 t_0 t0. I can get Y as follows:

f ( x 1 , t 1 ) = y 0 + d f ( x , t 0 ) d x ∣ x = x 0 ( x 1 − x 0 ) f(x_1,t_1)=y_0+\frac {df(x,t_0)}{dx}|_{x=x0}(x_1-x_0) f(x1,t1)=y0+dxdf(x,t0)x=x0(x1x0)

For n-dimensional functions we use the Jacobian matrix J J J to represent the partial derivatives.We can get this:

f ( X 1 , t 1 ) = f ( X 0 , t 0 ) + J ( X 1 − X 0 ) f(X_1,t_1)=f(X_0,t_0)+J(X_1-X_0) f(X1,t1)=f(X0,t0)+J(X1X0)

From the law of covariance propagation, we can draw the following conclusions:

D X 1 X 1 = J D X 0 X 0 J T D_{X_1X_1}=JD_{X_0X_0}J^T DX1X1=JDX0X0JT

D X 0 X 1 = D X 0 X 0 J T D_{X_0X_1}=D_{X_0X_0}J^T DX0X1=DX0X0JT

D X 1 X 0 = J D x 0 x 0 D_{X_1X_0}=JD_{x_0x_0} DX1X0=JDx0x0

So,We can get:

P n e w = [ P o l d P O L k + 1 P L k + 1 O P L L l + 1 ]   = [ P o l d P o l d J T J P o l d J P o l d J T ]   = [ I J ] P o l d [ I T J T ] P_{new}=\begin{bmatrix}P_{old} & P_{OL_{k+1}} \\ P_{L_{k+1}O} & P_{LL_{l+1}} \end{bmatrix} \\ \,\qquad =\begin{bmatrix}P_{old} & P_{old}J^T \\ JP_{old} & JP_{old}J^T \end{bmatrix} \\ \,\qquad =\begin{bmatrix}I \\J\end{bmatrix}P_{old}\begin{bmatrix}I^T & J^T\end{bmatrix} Pnew=[PoldPLk+1OPOLk+1PLLl+1]=[PoldJPoldPoldJTJPoldJT]=[IJ]Pold[ITJT]

Obviously, J J J is the Jacobian matrix of the newly added lidar state variable X l i d a r n e w X_{lidar}^{new} Xlidarnew about the previous state variable X o l d X_{old} Xold. Mathematically, the newly added state variable X l i d a r n e w X_{lidar}^{new} Xlidarnewis only related to the state variable of imu.
First, construct the position and attitude of the lidar:

R ( G q L ) = R ( G q n ) R ( n q b ) R L b R(^Gq_L)=R(^Gq_n)R(^nq_b)R^b_L R(GqL)=R(Gqn)R(nqb)RLb

G p L = R ( G q b ) p L + G p B = R ( G q N ) R ( n q b ) b + G p b ^Gp_L=R(^Gq_b)p_L+^Gp_B=R(^Gq_N)R(^nq_b)^b+^Gp_b GpL=R(Gqb)pL+GpB=R(GqN)R(nqb)b+Gpb

Find partial derivatives by adding perturbation terms,for position:

G p L + δ G p L = R ( G q n ) ( I − [ δ θ ] x ) R ( n q b ) b p L + G p b + δ G p b ^Gp_L+\delta^Gp_L=R(^Gq_n)(I-[\delta\theta]_\text{x})R(^nq_b)^bp_L+^Gp_b+\delta^Gp_b GpL+δGpL=R(Gqn)(I[δθ]x)R(nqb)bpL+Gpb+δGpb

We can get the following formula:

δ G p L = − [ δ θ x ] R ( G q n ) R ( n q b ) b p L + δ G p b = R ( G q N ) [ R ( n q b ) b p L ] x δ θ + δ G p b \delta^Gp_L=-[\delta\theta_\text{x}]R(^Gq_n)R(^nq_b)^bp_L+\delta^Gp_b=R(^Gq_N)[R(^nq_b)^bp_L]_\text{x}\delta\theta+\delta^Gp_b δGpL=[δθx]R(Gqn)R(nqb)bpL+δGpb=R(GqN)[R(nqb)bpL]xδθ+δGpb

Obviously,

∂ δ G p L ∂ δ θ = R ( G q n ) [ R ( n q b ) b p L ] x \frac {\partial\delta^Gp_L}{\partial\delta\theta}=R(^Gq_n)[R(^nq_b)^bp_L]_\text{x} δθδGpL=R(Gqn)[R(nqb)bpL]x

∂ δ G p L δ G p b = I \frac {\partial\delta^Gp_L}{\delta^Gp_b}=I δGpbδGpL=I

And for attitude:

R ( G q L ) = R ( G q n ) R ( n q b ) R L b R(^Gq_L)=R(^Gq_n)R(^nq_b)R^b_L R(GqL)=R(Gqn)R(nqb)RLb

Find partial derivatives by adding perturbation terms:

R ( G q L ) ( I + [ δ G θ L ] x ) = R ( G q n ) ( I − [ δ n θ b ] x ) R ( n q b ) R L b R(^Gq_L)(I+[\delta^G\theta_L]_\text{x})=R(^Gq_n)(I-[\delta^n\theta_b]_\text{x})R(^nq_b)R^b_L R(GqL)(I+[δGθL]x)=R(Gqn)(I[δnθb]x)R(nqb)RLb

We can get the following formula:

R ( G q L ) [ δ G θ L ] x = − R ( G q n ) [ δ n θ ] x R ( n q b ) R L b R(^Gq_L)[\delta^G\theta_L]_\text{x}=-R(^Gq_n)[\delta^n\theta]_\text{x}R(^nq_b)R_L^b R(GqL)[δGθL]x=R(Gqn)[δnθ]xR(nqb)RLb

[ δ G θ L ] x = − R n L [ δ n θ ] x ( R n L ) T [\delta^G\theta_L]_x=-R_n^L[\delta^n\theta]_\text{x}(R_n^L)^T [δGθL]x=RnL[δnθ]x(RnL)T

[ δ G θ L ] x = − [ R n L δ n θ b ] x [\delta^G\theta_L]_x=-[R^L_n\delta^n\theta_b]_\text{x} [δGθL]x=[RnLδnθb]x

Obviously,

∂ δ G θ L ∂ δ θ = − R n L \frac{\partial\delta^G\theta_L}{\partial\delta\theta}=-R_n^L δθδGθL=RnL

∂ δ G θ L ∂ G p b = 0 \frac{\partial\delta^G\theta_L}{\partial^Gp_b}=0 GpbδGθL=0

To sum up, we can obtain the Jacobian matrix:

J = [ − R n L 0 0 0 0 . . . 0 R ( G q n ) [ R ( n q b ) b p L ] x 0 0 0 I . . . 0 ] J=\begin{bmatrix} -R^L_n & 0 & 0 & 0 & 0 & ... & 0 \\ R(^Gq_n)[R(^nq_b)^bp_L]_\text{x} & 0 & 0& 0& I & ... &0\end{bmatrix} J=[RnLR(Gqn)[R(nqb)bpL]x0000000I......00]

<think>嗯,用户想了解因子图优化中雅可比矩阵的作用与计算方法。我需要先回忆一下因子图相关的知识,尤其是在优化过程中雅可比矩阵的应用。记得因子图是一种概率图模型,常用于SLAM、状态估计等问题,通过分解联合概率分布为多个因子的乘积。优化时通常使用非线性最小二乘法,比如高斯-牛顿法或列文伯格-马夸尔特法,这时候雅可比矩阵就很重要了。 首先,雅可比矩阵优化中的作用应该是线性化非线性误差函数,对吧?每个因子对应一个误差项,优化过程中需要计算误差函数对状态变量的导数,也就是雅可比矩阵。这些导数帮助确定搜索方向,从而迭代更新状态变量,逐步最小化总误差。例如,在视觉SLAM中,特征点的重投影误差对相机位姿的导数就需要通过雅可比矩阵来表示。 然后,计算方法方面,可能需要分解为每个因子单独计算雅可比块,再组合成整体的稀疏矩阵。因为因子图的优势之一就是稀疏性,所以雅可比矩阵通常也是稀疏的,这样可以高效地利用稀疏求解器,比如使用Cholesky分解或QR分解。此外,自动微分或者符号微分可能在计算雅可比时用到,尤其是当手动推导复杂时,工具如Ceres Solver或GTSAM会自动处理这些导数。 不过,用户提供的引用里提到改进牛顿法中避免显式形成雅可比矩阵,而是通过前推回代来操作,这可能涉及到隐式利用雅可比的结构,比如在状态估计中,UDUT分解或者利用问题的结构来避免存储大的雅可比矩阵,从而节省内存和提高计算效率。比如,在因子图中,每个因子只涉及少量变量,因此对应的雅可比块也是稀疏的,可以分块计算,然后组合起来,而不需要显式构造整个矩阵。 另外,引用[2]提到高斯牛顿法中雅可比的计算,以及如何求等效旋转矢量的雅可比,这可能涉及到李代数上的导数,比如在SLAM中相机位姿用SE(3)表示时,需要用到扰动模型来计算导数。这部分可能需要更深入的数学知识,比如李群的局部坐标参数化,以及对应的雅可比矩阵推导。 总结来说,雅可比矩阵在因子图优化中起到线性化误差函数、指导迭代方向的作用,计算方法则涉及对每个因子的导数分块计算,利用稀疏性进行高效求解,同时可能结合自动微分或特定的参数化方法来处理复杂的变量空间,如旋转矩阵的李代数表示。此外,优化方法如高斯-牛顿或LM算法会利用雅可比矩阵的结构来高效更新状态变量,避免显式存储大矩阵,从而提高计算效率。</think>### 因子图优化中雅可比矩阵的作用与计算方法 #### 1. **雅可比矩阵的作用** 在因子图优化中,雅可比矩阵的核心作用是**线性化非线性误差函数**,从而通过迭代优化算法(如高斯-牛顿法、列文伯格-马夸尔特法)求解状态变量的最优值[^1][^2]。具体功能包括: - **误差线性化**:将非线性误差函数在当前状态点附近近似为线性模型,便于求解梯度方向。 - **构建正规方程**:通过雅可比矩阵与残差向量的乘积,生成线性方程组,计算状态变量的修正量。 - **稀疏性利用**:因子图的稀疏结构使得雅可比矩阵天然稀疏,可结合稀疏求解器(如Cholesky分解)加速计算[^1]。 #### 2. **雅可比矩阵的计算方法** 雅可比矩阵的计算需针对**每个因子**独立分析,具体步骤如下: ##### **步骤1:定义误差函数** 每个因子对应一个误差函数 $e_i(x)$,例如: - SLAM中,观测路标点的重投影误差。 - 状态估计中,传感器测量的残差。 ##### **步骤2:分块计算雅可比块** 对每个因子 $e_i(x)$,计算其误差对相关状态变量 $x_j$ 的偏导数,生成子矩阵块 $J_{ij}$: $$ J_{ij} = \frac{\partial e_i}{\partial x_j} $$ 例如,在相机位姿优化中,需计算误差对旋转和平移参数的导数,涉及李代数扰动模型。 ##### **步骤3:组合全局雅可比矩阵** 将各子矩阵块按因子和变量的关联关系拼接成全局雅可比矩阵 $J$,例如: $$ J = \begin{bmatrix} J_{11} & 0 & \dots \\ 0 & J_{22} & \dots \\ \vdots & \vdots & \ddots \end{bmatrix} $$ 稀疏结构可通过**消元排序算法**(如COLAMD)进一步优化。 ##### **步骤4:结合优化算法迭代求解** 利用雅可比矩阵构建正规方程: $$ J^T J \Delta x = -J^T e $$ 通过稀疏分解(如QR或Cholesky)求解 $\Delta x$,更新状态变量直至收敛[^1]。 #### 3. **优化中的关键技巧** - **自动微分**:借助工具(如Ceres Solver)自动计算雅可比矩阵,避免手动推导复杂模型。 - **隐式表示**:对特定结构(如UDUT分解),直接通过前推回代操作雅可比矩阵的隐含形式,避免显式存储。 - **鲁棒核函数**:对异常值引入鲁棒核(如Huber函数),需在雅可比计算中额外考虑核函数的导数。 --- ###
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

名蒸蛋柯南.

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值