VIO优化中不客观自由度 (gauge freedom) 的处理 (gauge handle)
创始人
2024-06-01 15:58:07
0次

文章目录

  • 1. 不可观的解释
  • 2. 几种不同的gauge handle处理方式
    • 2.1. free gauge方式
    • 2.2. fix gauge方式
    • 2.3. prior gauge方式
    • 2.4. g2o tutorial方式
  • 3.不同方式的协方差矩阵

1. 不可观的解释

这篇论文 中对VIO的4-DOF不可观的定义如下,可以看到这种不可观就是如果对最后求得的解进行一个四自由度的变换(类似单应变换),那么损失函数J(θ)J(\theta)J(θ)并不会改变,所以这就是4-DOF的不可观。

另外从公式这个角度出发也是理解不可观的自由度的一种方式,例如VIO的yaw轴不可观,实际上就是把最后估计的位姿的yaw角再加上一个角度,不会改变损失函数。而这个加上yaw角,利用公式来理解就是把最后的位姿 绕着垂直于重力的zzz再进行旋转。

在这里插入图片描述

2. 几种不同的gauge handle处理方式

仿真模型如下,第0次滑窗优化的时候状态变量是 P0,P1,P2,LP_0, P_1, P_2, LP0​,P1​,P2​,L。显然这个系统是1自由度不可观的,所以下面要通过不同的方式来处理这个不可观的自由度。

在这里插入图片描述
先给出此时系统的残差、雅克比矩阵以及H矩阵:
r=[6.0−(L−P0)1.1−(P1−P0)0.95−(P2−P1)5.05−(L−P1)3.8−(L−P2)]J=[100−11−10001−10010−1001−1]H=J′J=[2−10−1−13−1−10−12−1−1−1−13]\begin{aligned} r &= \begin{bmatrix} 6.0 - (L - P_0) \\ 1.1 - (P_1 - P_0) \\ 0.95 - (P_2 - P_1) \\ 5.05 - (L - P_1) \\ 3.8 - (L - P_2) \end{bmatrix} \\\\ J &= \begin{bmatrix} 1 & 0 & 0 & -1 \\ 1 & -1 & 0 & 0 \\ 0 & 1 & -1 & 0 \\ 0 & 1 & 0 & -1 \\ 0 & 0 & 1 & -1 \end{bmatrix} \\\\ H = J'J &= \begin{bmatrix} 2 & -1 & 0 & -1 \\ -1 & 3 & -1 & -1 \\ 0 & -1 & 2 & -1 \\ -1 & -1 & -1 & 3 \end{bmatrix} \end{aligned} rJH=J′J​=​6.0−(L−P0​)1.1−(P1​−P0​)0.95−(P2​−P1​)5.05−(L−P1​)3.8−(L−P2​)​​=​11000​0−1110​00−101​−100−1−1​​=​2−10−1​−13−1−1​0−12−1​−1−1−13​​​
容易发现,HHH 矩阵的每一行都加起来结果为0,所以 HHH 矩阵是不满秩的,rank(H)=3rank(H) = 3rank(H)=3,所以需要进行一些特殊的处理才能求解。

这种存在自由度不可观的情况在论文中称为 gauge freedom,处理 gauge freedom(称为gauge handle)有多种方式,这篇论文 总结为下面三种方式(g2o中还有一种方式,这里定义为第4种)。实际上在VIO的课程中,总结的应该是四种,恰好对应下面自己写的这四种。

在这里插入图片描述

2.1. free gauge方式

不管这个自由度,而 HΔx=−bH\Delta x=-bHΔx=−b 的求解 HHH 不满秩的问题靠 L-M算法 中给 HHH 增加一个λI\lambda IλI 来间接解决(因为本来 λI\lambda IλI 是为了限制高斯牛顿法接近二次拟合的)。但是这样的结果最后 仍然会在零空间漂移,所以在VINS中用的做法就是使用free gauge求解 Δx\Delta xΔx,但是求解之后给变量赋值的时候,是让第滑窗中第0帧的4-DOF保持不变,也就是认为第0帧的状态不发生改变。

MATLAB 代码如下:

%% 本代码测试不同的零空间处理方法gauge handle
clear all; clc;%% 0.正常状态,滑窗中维护3个位置+1个路标点
% 真实状态
x_t = [0; 1; 2; 6];  % 滑窗长度为3, 变量分别为P0, P1, P2, L
% 码盘测量值, e=encoder, l=lidar
l0 = 6.0; e1 = 1.1; e2 = 0.95; l1 = 5.05; l2 = 3.8;
e0 = 0.0;  % 为了解决漂移的问题,给一个超强先验
% 状态初值, 由码盘测量值来得到
P0 = e0; P1 = P0 + e1; P2 = P1 + e2; L = P0 + l0; 
% 状态初始估计值
x = [P0; P1; P2; L]; %% gauge handle 1: free gauge
disp("****** gauge handle 1: free gauge ******");
J = [1, 0, 0, -1;1, -1, 0, 0; 0, 1, -1, 0;0, 1, 0, -1;0, 0, 1, -1];
H = J' * J
fprintf("rank(H) = %d\n", rank(H));  % rank(H) = 3
miu = 0.1;
H = H + miu * eye(4);
% 其实由于是线性模型,所以没必要迭代,一步就是最速下降法得到最优解
for i = 1 : 5   fprintf("第 %d 次迭代\n", i); % 残差r = [l0 - (x(4) - x(1));e1 - (x(2) - x(1)); e2 - (x(3) - x(2));l1 - (x(4) - x(2));l2 - (x(4) - x(3))];b = -J' * r;delta_x = H \ b;x = x + delta_x;disp("delta_x = "); disp(delta_x');disp("x = "); disp(x');
end
x = x + 0 - x(1);  % 手动对齐第0帧的位置到想要的位置上
disp("x = "); disp(x');
fprintf("error = %f\n", (x_t-x)' * (x_t-x));

代码输出如下:
在这里插入图片描述

2.2. fix gauge方式

固定滑窗中的第0帧,具体操作就是和滑窗中的 第0帧有关的雅克比矩阵全都置为0,这就意味第0帧的变化对所有的残差都不会有贡献,所以第0帧在优化中就不会改变了。

从数学上来看,也就是上面仿真的例子中雅克比矩阵 JJJ 的第1列(对应状态P0P_0P0​)全为0,即:

J=[000−10−10001−10010−1001−1]H=J′J=[000003−1−10−12−10−1−13]b=−J′e=[0.........]\begin{aligned} J &= \begin{bmatrix} 0 & 0 & 0 & -1 \\ 0 & -1 & 0 & 0 \\ 0 & 1 & -1 & 0 \\ 0 & 1 & 0 & -1 \\ 0 & 0 & 1 & -1 \end{bmatrix} \\\\ H = J'J &= \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 3 & -1 & -1 \\ 0 & -1 & 2 & -1 \\ 0 & -1 & -1 & 3 \end{bmatrix} \\\\ b = -J'e &= \begin{bmatrix} 0 \\ ... \\ ... \\ ... \end{bmatrix} \end{aligned} JH=J′Jb=−J′e​=​00000​0−1110​00−101​−100−1−1​​=​0000​03−1−1​0−12−1​0−1−13​​=​0.........​​​

由于最后我们求解还是使用 L-M算法 求解,即会在 HHH 矩阵对角线上加一个数 μ\muμ,所以最后求解的方程变成

[μ00003+μ−1−10−12+μ−10−1−13+μ][ΔP0ΔP1ΔP2ΔL]=[0.........]\begin{bmatrix} \mu & 0 & 0 & 0 \\ 0 & 3+\mu & -1 & -1 \\ 0 & -1 & 2+\mu & -1 \\ 0 & -1 & -1 & 3+\mu \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \\ \end{bmatrix} = \begin{bmatrix} 0 \\ ... \\ ... \\ ... \end{bmatrix} ​μ000​03+μ−1−1​0−12+μ−1​0−1−13+μ​​​ΔP0​ΔP1​ΔP2​ΔL​​=​0.........​​

这时候关于 ΔP0\Delta P_0ΔP0​ 这一项的解就变成了 (0+μ)ΔP0=0(0+\mu)\Delta P_0 = 0(0+μ)ΔP0​=0,结果只能是 ΔP0=0\Delta P_0 = 0ΔP0​=0,从而起到了固定第0帧的位置的作用。我感觉这个本质上属于 一种数学技巧,并不是很有物理意义。

代码实现如下:

%% gauge handle 2: fix gauge
fprintf("\n");
disp("****** gauge handle 2: fix gauge ******"); 
x = [P0; P1; P2; L]; 
% 雅克比第1列是关于状态P0的,全部置为0
J = [0, 0, 0, -1;0, -1, 0, 0; 0, 1, -1, 0;0, 1, 0, -1;0, 0, 1, -1];
H = J' * J
fprintf("rank(H) = %d\n", rank(H));  % rank(H) = 3
miu = 0.1;
H = H + miu * eye(4);for i = 1 : 5 fprintf("第 %d 次迭代\n", i); % 残差r = [l0 - (x(4) - x(1));e1 - (x(2) - x(1)); e2 - (x(3) - x(2));l1 - (x(4) - x(2));l2 - (x(4) - x(3))];b = -J' * r;delta_x = H \ b;x = x + delta_x;disp("delta_x = "); disp(delta_x');disp("x = "); disp(x');
end
fprintf("error = %f\n", (x_t-x)' * (x_t-x));

代码输出如下,可见迭代速度不快,这个迭代速度可以通过调节μ\muμ来改变。

2.3. prior gauge方式

给滑窗中的第0帧添加先验,这种方式实际上是 最正规的方式,因为它有明确的物理意义,而其他方式更像是一种数学上的技巧。这种添加先验的方式就是从存在不可观的原因出发,即由于测量全都是相对测量而没有绝对测量,导致优化结果可以漂移。所以这种方式就是增加一个绝对位置的先验测量,比如给滑窗中第0帧添加一个先验观测的位置是0,这样优化的时候最终结果就不会离0太远。

(1) 如果想固定第0帧就是在先验观测0的位置,那么只需要把这个先验观测的协方差矩阵设置的非常小,也就是信息矩阵(权重)设置的非常大,这样优化状态离先验观测稍微偏离一点就会造成非常大的误差项,从而最后会把第0帧的优化状态固定在先验的位置。

(2) 如果就是有一个实际观测到的先验,比如GPS观测,有明确的协方差矩阵,那么就可以把它作为先验融合到滑窗优化中,这样滑窗中第0帧位置可能会发生变化,但是此时就是需要让它有变化,因为我们已经有先验了,需要来修正这个状态。

代码如下:

%% gauge handle 3: prior gauge
fprintf("\n");
disp("****** gauge handle 3: prior gauge ******"); 
x = [P0; P1; P2; L];
w = 30;   % 超强先验的权重,注意一定是正数。而且这个数值还不能太大,如果太大会导致H矩阵直接退化成rank=1
J = [-w, 0, 0, 0;   % 超强先验, 残差是 w(e0 - P0), 所以雅克比是-w1, 0, 0, -1;1, -1, 0, 0; 0, 1, -1, 0;0, 1, 0, -1;0, 0, 1, -1];
% 开始构造高斯牛顿的优化问题,认为信息权重矩阵都是I
H = J' * J
fprintf("rank(H) = %d\n", rank(H));  % rank(H) = 4% 其实由于是线性模型,所以没必要迭代,一步就是最速下降法得到最优解
for i = 1 : 2   fprintf("第 %d 次迭代\n", i); % 残差r = [ w * (e0 - x(1));l0 - (x(4) - x(1));e1 - (x(2) - x(1)); e2 - (x(3) - x(2));l1 - (x(4) - x(2));l2 - (x(4) - x(3))];b = -J' * r;delta_x = H \ b;x = x + delta_x;disp("delta_x = "); disp(delta_x');disp("x = "); disp(x');
end
fprintf("error = %f\n", (x_t-x)' * (x_t-x));

代码输出:

注意:

  • 如果是想固定第0帧的话,给的先验权重大小要合适。首先不能太小,否则起不到固定的效果;其次也不能太大,否则此时的 HHH 矩阵中数值差异过大,会直接导致 HHH 矩阵的秩变成1,相当于整个 HHH 矩阵中只有 wwwwww 构成的这一项,因为其他项相比它都太小了,这样反而会直接导致系统更加不可观。
  • 可以发现这种方式系统一次就迭代到了最小值,到底这几种方法为什么会有这种迭代速度的区别还需要后面仔细思考。但是根据论文里说的,这几种方法实际上速度差别不大。

2.4. g2o tutorial方式

这种方式也是属于 固定第0帧,具体叫什么名字不知道,在手写VIO的课程中有讲解,因为g2o就是使用的这种方式来固定某些帧,所以这里就称之为g2o tutorial的方式了。

其操作就是在最后的 HHH 矩阵要固定的状态变量的位置强行加一个 III,但是对应的 bbb 的位置并不加任何东西。这样其实本质上就是更改了原来的 HΔx=bH\Delta x = bHΔx=b 的方程组,导致求解的结果中 Δx0\Delta x_0Δx0​ 只能为0。所以这个本质上也属于一种 数学技巧,并没有物理意义。

比如上面的例子,假设 HΔx=bH\Delta x = bHΔx=b 的形式如下:

[h11h12h13h14h21h22h23h24h31h32h33h34h41h42h43h44][ΔP0ΔP1ΔP2ΔL]=[b1b2b3b4]\begin{bmatrix} h_{11} & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ h_{41} & h_{42} & h_{43} & h_{44} \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \\ \end{bmatrix} = \begin{bmatrix} b_1 \\ b_2 \\ b_3 \\ b_4 \end{bmatrix} \\ ​h11​h21​h31​h41​​h12​h22​h32​h42​​h13​h23​h33​h43​​h14​h24​h34​h44​​​​ΔP0​ΔP1​ΔP2​ΔL​​=​b1​b2​b3​b4​​​
现在想要固定 P0P_0P0​ 的位置,那么就给 HHH 矩阵对应 P0P_0P0​ 的位置增加一个单位阵 III,这样上面的方程变为:

[h11+1h12h13h14h21h22h23h24h31h32h33h34h41h42h43h44][ΔP0ΔP1ΔP2ΔL]=[b1b2b3b4]\begin{bmatrix} h_{11} + 1 & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ h_{41} & h_{42} & h_{43} & h_{44} \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \\ \end{bmatrix} = \begin{bmatrix} b_1 \\ b_2 \\ b_3 \\ b_4 \end{bmatrix} \\ ​h11​+1h21​h31​h41​​h12​h22​h32​h42​​h13​h23​h33​h43​​h14​h24​h34​h44​​​​ΔP0​ΔP1​ΔP2​ΔL​​=​b1​b2​b3​b4​​​

利用矩阵的乘法分配律,可以把上述方程拆解为两个方程:

[h11h12h13h14h21h22h23h24h31h32h33h34h41h42h43h44][ΔP0ΔP1ΔP2ΔL]=[b1b2b3b4][1000000000000000][ΔP0ΔP1ΔP2ΔL]=[0000]\begin{aligned} \begin{bmatrix} h_{11} & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ h_{41} & h_{42} & h_{43} & h_{44} \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \\ \end{bmatrix} &= \begin{bmatrix} b_1 \\ b_2 \\ b_3 \\ b_4 \end{bmatrix} \\\\ \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \Delta P_0 \\ \Delta P_1 \\ \Delta P_2 \\ \Delta L \\ \end{bmatrix} &= \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} \end{aligned} ​h11​h21​h31​h41​​h12​h22​h32​h42​​h13​h23​h33​h43​​h14​h24​h34​h44​​​​ΔP0​ΔP1​ΔP2​ΔL​​​1000​0000​0000​0000​​​ΔP0​ΔP1​ΔP2​ΔL​​​=​b1​b2​b3​b4​​​=​0000​​​

可见我们可以利用第2个方程先求写出 ΔP0=0\Delta P_0 = 0ΔP0​=0,然后带入到第1个方程中再求解处剩下的变量。也就是说,实际上这种方式就是在原来的4个方程的基础上又增加了一个附加方程为 1×ΔP0=01 \times \Delta P_0 = 01×ΔP0​=0,从而强行改变了原来方程组的解。所以这个本质上也属于一种 数学技巧,并没有物理意义。

代码如下:

%% gauge handle 4: g2o tutorial
fprintf("\n");
disp("****** gauge handle 4: g2o tutorial ******"); 
x = [P0; P1; P2; L]; 
J = [1, 0, 0, -1;1, -1, 0, 0;  0, 1, -1, 0;0, 1, 0, -1;0, 0, 1, -1];
H = J' * J
fprintf("rank(H) = %d\n", rank(H));  % rank(H) = 3
H(1, 1) = H(1, 1) + 1;% 其实由于是线性模型,所以没必要迭代,一步就是最速下降法得到最优解
for i = 1 : 2   fprintf("第 %d 次迭代\n", i); % 残差r = [l0 - (x(4) - x(1));e1 - (x(2) - x(1)); e2 - (x(3) - x(2));l1 - (x(4) - x(2));l2 - (x(4) - x(3))];b = -J' * r;delta_x = H \ b;x = x + delta_x;disp("delta_x = "); disp(delta_x');disp("x = "); disp(x');
end
fprintf("error = %f\n", (x_t-x)' * (x_t-x));

代码输出如下,可见这种方式的迭代速度也非常快。

3.不同方式的协方差矩阵

待总结,参见论文。

这个地方还是很重要的,如果能够求解出协方差矩阵的话,那么可以利用prior gauge的方式来对下一次滑窗优化添加先验,而不用像VINS一样每次滑窗优化都是把滑窗中第0帧当做不变的帧。

相关内容

热门资讯

存储芯片巨头的好日子何时结束 ... 摩根士丹利最新报告指出,旧存储芯片行业的前景被低估。曾经因供需失衡而获得高额利润的旧存储芯片现在正处...
国足开场2分钟落后 邵佳一眉头... 点击加载图片直播吧11月17日讯 国足开场2分钟丢球,0-1落后巴林,镜头给到眉头紧锁的邵佳一。国字...
小米:相关内容纯属虚构,警惕虚... 小米旗下微博账号“小米辟谣”于11月17日发布声明,指出近期有不法分子冒充“小米汽车”发送诈骗短信,...
最差密码榜单公布 123456... 网络安全公司 NordPass 发布了《2026 年最常用密码 Top 200》报告,指出“1234...
2岁孩子生命最后11天依然努力... 25岁的妈妈将患有罕见病的儿子兜兜送进了长沙的“雏菊之家”安宁病房。她陪伴儿子度过了最后11天,并每...
13岁少年被控杀害5岁男童案 ... 一名13岁少年因妹妹被邻居5岁男童撒沙而心生怨恨,将男童带至桥上推下水致其溺亡。此案引起了媒体的广泛...
全球顶尖科学家加速回流中国 风... 全球顶尖科学家纷纷选择离开美国,转向中国任职。2026年初,莱斯大学90后斯隆奖得主汪淏田全职加盟同...
APEC工商领导人峰会今天聚焦... 11月17日,来自亚太经合组织21个经济体及英国、法国、德国、瑞士等13个域外经济体的2000多位政...
雷军APEC发言 具身智能潜力... 11月17日,小米CEO雷军在2026年APEC工商领导人峰会上表示,具身智能机器人的潜力超过汽车,...
俄罗斯记者惊呼在深圳谁还用现金... 11月17日,今日俄罗斯国际通讯社记者德米特里·加夫里洛夫在接受采访时分享了他的深圳体验。他提到,刚...
雷军:智能机器人产业潜力超汽车... 11月17日,在2026年亚太经合组织工商领导人峰会上,小米CEO雷军介绍了公司在AI、智能制造和机...
保研举报成风,这不只是“坏人变... 保研举报成风,这不只是“坏人变小了” 制度留操作空间。高校保研季举报成风:这不是学生坏,是制度留了太...
广东01年医护女孩为00年闺蜜... 据河南广播电视台小莉帮忙,广东一位01年的女孩,亲自为自己00年的闺蜜接生。她们的缘分有多深?女孩称...
广东网友为迎接APEC会议周有... 11月14日,2026年APEC领导人会议周新闻中心举办了广东省、深圳市专场中外媒体吹风会。这是20...
动车插座在头顶被吐槽像在打吊瓶... 近日,有网友在社交平台发帖分享自己的乘车经历:乘坐D2928次动车组时,发现车厢内的充电插座安装在顶...
万斯称与特朗普无分歧 一致反对... JD·万斯周一表示,他与总统唐纳德·特朗普在伊朗战争问题上完全一致。此前,他曾将共和党在中期选举中的...
痛风人群这4种饮品要少喝 咖啡... 很多尿酸高、痛风人群都知道啤酒、果汁会升高尿酸,那么有没有什么饮料对尿酸“很友好”,甚至还能降低痛风...
医生辟谣左侧睡根本不伤心:正常... 11月17日,经常朝左睡对心脏的影响引发关注。从解剖学角度看,心脏虽然位于胸腔偏左,但它并非孤零零地...
96岁奶奶扒着车窗含泪送孙女返... 近日,湖南,假期快要结束,孙女准备返程。96岁的奶奶扒着车窗,眼里满是不舍。一句有空常回家看看,戳中...
人民日报:从《持械入宋》火爆看... 有能够让行业内外普遍认可的作品出现,是一种新文艺类型成熟的重要标志。正是因为“家里家外”系列、《盛夏...