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

Posted 2023-04-12

参考技术A 为什么不能用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列，现在最后一列不再满足Ni*M=0了）不再满足，在这里就是绕重力的旋转从不可观变成可观，因为它不在零空间中了

回到正文：

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

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

来看第二种情况，零旋转

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

这个的证明暂时不写了

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

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

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

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

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

安装VINS-Mono

安装VINS-Mono

 

cd ~/catkin_ws/src
git clone https://gitee.com/kay2020/VINS-Mono.git
cd ..
catkin_make

　　

 

修改rs_camera.launch

 

 

修改realsense_color_config.yaml

 

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

　　

 

作者：kay 出处：https://www.cnblogs.com/kay2018/ 本文版权归作者和博客园共有，欢迎转载，但未经作者同意必须保留此段声明，且在文章页面明显位置给出原文连接，否则保留追究法律责任的权利。 如果文中有什么错误，欢迎指出。以免更多的人被误导。