V-SLAM综述：VINS-MONO(可观性退化)

为什么不能用2dof的姿态估计：因为平面假设只是近似满足，实际还是有很多不平整、颠簸、低高度从而可驶过的地板等

为什么不能直接用3dof的姿态估计：因为运动受限（近似平面运动），从而导致额外的不可观的自由度

一个单目系统，6自由度位姿和1自由度尺度不可观

一个单目IMU系统，3自由度平移（trans）和1自由度旋转（yaw）不可观，但是这是在IMU有足够的3D运动的时候情况

在近似平面假设的自动驾驶车辆上，额外的一个自由度变得不可观了，这就是尺度

这样的情况例如：车辆在恒定加速度下运动，特别的，在完全已知陀螺仪偏差bg的简化假设下，对于某些运动，VINS的初始状态不能唯一的确定

我们来看一看，在更一般的情况下：未知的陀螺仪偏差bg，来确定不可观的自由度

单目VINS系统，状态向量：

还要注意的是，q是G（大地坐标）在I（IMU坐标，或者叫机体坐标的位置），而p和v是更好理解的I在G中的位置

为什么q要这么写？继续往下看

而且这里为什么状态只有一帧的，如果这是状态的一部分，为什么特征点深度也在这里？

根据上面这个，推导连续系统微分方程：

然后是图像特征：

zj可以理解为归一化平面的像点，z=1所以省略，fj本来是在g系的图像坐标，减去自车位置P，再进行自车旋转，并用外参C变换到相机坐标系

那么，在恒定加速度情况下：

这个不可观的方向就是尺度

我们来检验线性化VINS模型的能观性矩阵的右零空间，为了表示清楚，我们只在状态向量中包含一个特征点(扩展到多个特征点很简单)

能观性矩阵的任意块行是这样的结构：

下面的 x k+1|k 的这种表示 应该是条件概率 

另外，下面这两个传播和测量方程写得可能有些迷惑人，但实际上 第一个矩阵就是误差状态转移矩阵雅克比，第二个矩阵也就是H就是观测矩阵雅克比

而能观性矩阵的零空间为：（以下的O我确认了就是上面的M），注意M有m行块，这个m是啥？就是观测方程的个数，也就是特征点的数量

以EKF为例

回到刚才的Mk，其中Ti矩阵的定义就不写在这里了，从这个Mk中可以得到：想要尺度维不可观，当且仅当对于任意的K，都有MkNs=0

而Ns是这样的

这里的NS构建就是根据状态向量构建的，第一个0对应q旋转 第二个0对应bg陀螺仪偏置，第三个对应速度，第四个对应ba加速度偏置，这里应该是认为在恒定加速度假设下，加速度偏置就等于恒定的加速度，这个似乎是很自然的

然后我们就看看，MKNS是否真的等于0

32等式成立是由于相机投影矩阵（观测矩阵）的雅克比Hck的右零空间是imu坐标下的特征点位置，完全看不懂在说什么，但是在公式上我们注意到，如果我们有定理“Hck的右零空间是imu坐标系下的特征点位置”，从32看出，特征点f在g中的位置-当前imu坐标系在g中的位置就等于特征点f在i中的位置，那就必有Hck f=0

而且，这个新的N的零空间的向量Ns是和之前的4个不可观维度N1不一样的，因为N1的第四个块元是0，而Ns中不是，所以这时一个新的不可观维度

首先我们在时刻L有一个m行的能观性矩阵M或者叫O，对于VINS系统，我们已经知道零空间有四个维度，就是N1，然后现在我们再构建一个N向量叫做Ns，这个向量是根据我们目前的假设（系统在恒定加速度下运动）而构建的，那么接下来我们就要证明MkNs总是等于0（对于M的任意一行），最终，通过一系列计算，和定理“Hck的右零空间是imu坐标系下的特征点位置”，我们证明了这一点，而且我们确认Ns并不在N1撑出的空间之中，因为Ns第四维不等于0，而N1所有向量第四维都是0，所以我们找到了一个新的不可观向量

证明结束

注意，这个不可观的维度对应的就是尺度

再额外放一点，

下面这是，滑动窗口中，或者说理想状态是在真实值附近展开，而现在只能在当前估计下展开，从而会导致能观性变化

要记住的就只是，在这种真实情况下，比起理想，能观性矩阵多了一下Δ，叫做扰动项，所以O的零空间最后一列（也就是零空间最后一列的Ni，在理想状态下零空间一共有4列，现在最后一列不再满足NiM=0了）不再满足，在这里就是绕重力的旋转从不可观变成可观，因为它不在零空间中了

回到正文：

这个结论的物理解释是，当局部加速度不变是，人们无法区分真实机体加速度的大小和加速度偏置的大小，因为他们暂时都是不变的，从而真实加速度大小可以是任意的，所以导致了尺度的模糊

我们应该要意识到，在实践中，自动驾驶车辆都是恒定加速度的，比如沿着直线 恒速行驶（这时，恒定加速度就是重力加速度） 或者恒加速度行驶，或者沿着恒定速度圆弧转弯，这导致了VINS系统尺度的模糊（或者叫不可观）

来看第二种情况，零旋转

例如，在一段时间上，旋转保持不动

这个的证明暂时不写了

注意，这时的不可观测方向是所有三个自由度的全局旋转方向，而不仅仅是yaw

物理解释：当没有旋转时，无法从恒定的旋转中区分局部重力加速度的方向和加速度偏置的方向，从而roll和pitch都变得不可观（本来yaw就不可观了，所以所有三个自由度旋转都不可观）

也就是当自动驾驶车辆纯直线运动的时候，由于能观性的缺失，旋转估计变得不准确

结论：当车辆恒定加速度或者纯直线运动的时候，会有额外的不可观方向。我们要注意到，在实践中并不会有绝对的恒加速度或者绝对的直线运动，但当汽车近似于这种运动的时候，其在不可观方向得到的信息很少，从而信息矩阵是病态的，甚至缺秩，从而影响定位性能

解决方式，允许车辆偏离直线路径可以解决全局定向问题，但是尺度问题不太好解决，需要车辆不断的改变加速度，这不现实也会造成机构磨损，所以我们选择使用轮速计来解决这个问题

一般slam当中做pose graph optimization的话，有三个关键的变换，1)当前帧到上一帧;  2)当前帧到世界坐标系;  3)上一帧到世界坐标系 其实就是一个闭环，然后优化每一帧到世界坐标系的变换。

object slam相对来说就是多了一个物体坐标系，它是 1)当前帧到世界坐标系 2）物体坐标系世界坐标系 3)物体坐标系到当前帧 。 而pose graph主要是为了分散累计误差， 当前帧到世界坐标系和物体坐标系世界坐标系都涉及累积误差，所以他们都是优化的对象。

具体公式可以参考 slam++ , 而另一个tutorial给了非常详细的具体步骤和解法 >

在构建地图的过程中, 可能会存在先验的姿态或者先验的位置, 这些先验信息可能来自不同的传感器, 也可能来自同一传感器的不同算法 比如使用激光雷达建图, 提供先验位置给视觉建图; 或者使用线特征估计Manhattan World进而提供先验姿态 本文先后求解对于姿态和位置约束的雅克比矩阵

已知当前估计位置为 为 的矩阵, 表示平移, 表示world, 表示robot 此时有先验姿态 那么构造误差方程为:

本文中 为反对成运算, 为其反运算

下面求解误差 对于局部扰动 的导数

这里认为 , 进而求得雅克比矩阵

下面求解误差 关于全局扰动 的求导

参考 State Estimation for Robotics 表格7-2中的公式 另外近似

定位当前估计位姿为 , 为4x4矩阵, 表示旋转和平移, 表示world, 表示robot 有先验位姿 那么构建误差方程:

本文中 为反对称, 与反对称运算相反

下面对于求解误差 对于局部扰动 的导数

这里认为 的误差很小, 得到雅克比矩阵

下面对于求解误差 对于全部扰动 的导数

其中 是 的伴随阵

注意以上伴随阵的表达方式和 对应, 其中 为6维向量, 为三维, 表示旋转 为两维, 表示平移 论文Local Accuracy and Global Consistency for Efficient Visual SLAM提供了一种更复杂的求解方式, 具体内容查看器附录B

最小二乘最终求得的对应关系为

本问题中 , J为2x6的矩阵 为权重, 为2x2的对角阵

对于局部扰动和全局扰动最终的迭代略有不同, 另优化后局部扰动计算的步长为 , 全局扰动为 , 那么:

姿态同理

单目系统6自由度位姿和1尺度共计7自由度不可观

单目+imu系统 3自由度位移和yaw共计4自由度不可观

边缘化的问题在于，线性点不一致，会导致零空间发生变化，从而使本来不可观的可观了，可观并不是好事

在理想的线性点，就是真值上，能观性矩阵的零空间是3维的，正等于不可观的维度   （3自由度系统，也就是二维机器人）

而在实际，一般是在最新的状态上泰勒展开，从而零空间维度为2，把原来不可观的维度变得可观了，也就是yaw的维度

从而，滤波器获得了关于yaw（全局方向）的本质上不存在的信息，会导致方向不确定性的不合理的降低了（也就是，实际上方向不确定性很高，但是现在非常不合理的这个不确定性降低了），从而进一步降低了所有状态变量的不确定性（不合理的），从而导致不一致

注意，使用FEJ策略，就是在第一次估计值附近展开，这确实会导致线性化点和真值的误差相对较大（因为线性化点不会迭代更新了）但是能保证能观性矩阵和理想系统一样

VIO系统4自由度不可观，如果不使用FEJ策略，最后会变成3自由度不可观，变得可观的自由度也是yaw

OKVIS和DSO都采用了FEJ策略，VINS-MONO没有采用这种策略

在滑动窗口方法时，当有一个新的帧进来，我们边缘化掉一个帧，肯定不能直接丢，得把先验信息加入

解释一下：xm是要边缘化的变量，xb是剩下的变量中与xm有关的（先验信息就加在xb上），xr是和xm完全无关的

激光SLAM（Simultaneous Localization and Mapping）是一种基于激光雷达的实现机器人自主导航的技术。在使用激光SLAM技术进行机器人自主导航时，会生成机器人的轨迹信息，可以通过以下几种方式来查看：

1 通过机器人自身的显示屏或者外部设备显示屏来查看机器人的轨迹信息。一些机器人自身就配备有显示屏，可以直接显示机器人的轨迹信息。另外，还可以使用外部设备显示屏，通过网络连接和软件控制，实现对机器人轨迹信息的查看。

2 通过激光雷达的输出信息来查看机器人的轨迹信息。激光雷达在扫描周围环境时，会输出每个扫描点的坐标信息，通过对这些坐标点的分析可以得到机器人的轨迹信息。

3 通过SLAM算法的输出信息来查看机器人的轨迹信息。SLAM算法可以实现对机器人在环境中的自主定位和建图，在算法运行过程中，会输出机器人的轨迹信息，可以通过相关软件来查看。

需要注意的是，查看机器人的轨迹信息需要使用专业的软件和设备，并需要对相关技术有一定的了解和掌握。如果您对此不太熟悉，建议寻求相关技术人员的帮助和指导。

以上就是关于V-SLAM综述：VINS-MONO(可观性退化)全部的内容，包括:V-SLAM综述：VINS-MONO(可观性退化)、Object SLAM -- Pose Graph Optimization、旋转约束与姿态约束的误差方程及其相关求导等相关内容解答，如果想了解更多相关内容，可以关注我们，你们的支持是我们更新的动力！