转载自：https://mp.weixin.qq.com/s/jlkXvygaTsql1nk76HD94g
原创 泡泡机器人 泡泡机器人SLAM 今天
泡泡图灵智库，带你精读机器人顶级会议文章
标题:Optimization-Based Visual-Inertial SLAM Tightly Coupled with Raw GNSS Measurements
作者：Jinxu Liu, Wei Gao* and Zhanyi Hu
来源：ICRA 2021
编译: 冯凯
审核: wyc
这是泡泡图灵智库推送的第661篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
摘要
大家好，今天为大家带来的文章是 Optimization-Based Visual-InertialSLAM Tightly Coupled with Raw GNSS Measurements
与文献中松耦合方法和基于ekf的方法不同，本文提出了一种基于优化的视觉惯性SLAM与全球导航卫星系统(GNSS)测量数据紧密耦合的方法，据我们所知，这是文献中此类方法的首次尝试。更具体地说，在滑动窗口内将重投影误差、IMU预积分误差和GNSS原始测量误差共同最小化，其中考虑了图像和GNSS数据之间的不同步。此外，如边缘化，去噪声，以及弱信号的问题也得到了解决。在复杂城市场景下的公共数据集上的实验结果表明，无论是在以低层建筑为主的场景还是在以城市为主的场景中，我们的方法都优于目前最先进的视觉惯性SLAM、GNSS单一定位方法以及松耦合方法。
主要工作与贡献
提出了一种基于优化的状态估计方法，联合最小化重投影误差、IMU误差、伪距误差、多普勒偏移误差和边缘误差，其中考虑图像和原始GNSS测量之间的不同步。
设计了滑动窗口的方案，其中考虑GNSS测量的附加约束和状态。
提出了两种消除GNSS测量噪声的方法，并比较了它们的性能和时间消耗。
算法流程
1.PRELIMINARIES
本文的坐标系包括地心地球固定坐标系(ECEF)、地面东北天坐标系(ENU)和局部世界坐标系(SLAM)。传感器帧包括摄像机帧、IMU帧和GNSS接收帧。使用WE、WG、WL分别代表ECEF、东北天、局部坐标系。R^B_A代表R旋转矩阵使得从{A}帧转换到{B}帧，p^B_A代表{A}帧原点在{B}帧坐标系下的位置。
图1：世界坐标系和传感器坐标系，图中左上角的地球仪是地球仪。ECEF坐标系{W E}的原点在地球中心。ECEF坐标系的X轴指向本初子午线与赤道的交点，Z轴指向北极。SLAM的地面ENU框架{W G}和局部世界框架{W L}都位于地面上，其Z轴向上。
2.具体实现
A.初始化
系统启动后，执行VI-SLAM的初始化，并且执行VI-SLAM的非线性优化。同时，执行GNSS单点定位，由此分别得到了VI-SLAM轨迹和GNSS单点定位轨迹。在GNSS-SLAM初始化阶段，我们将上述两个轨迹对齐，以计算R^WE_WG,P^WE_WG和R^WG_WL,P^WG_WL的初始值。
首先，我们选择一个GNSS单点定位计算的位置作为参考点P^WE_ref，并作为ENU帧的原点，在实现中，选择启动系统后的第三个GNSS定位结果作为参考点。给定参考点，R^WE_WG 和P^WE_WG可以被直接计算出，有了P^WE_WG=P^WE_ref，和
很容易转换为p^WE_ref.然后GNSS单点位置轨迹从ECEF坐标系转到ENU坐标系为
通过最小化损失方程来求解初始值R^WG_WL和p^WG_WL
由于在退化情况中，VI-SLAM对尺度漂移很敏感，因此对尺度参数s进行了估计。注意R^WG_WL只包含一个自由度，即绕Z轴旋转，因为{WG}和{WL}的Z轴都是向上的。P^WG_WL的初始值由两组位置中心求解，R^WG_WL的初始值由中心后每1度搜索得到，然后迭代求解最小化问题。在最小化后，由于在接下来的阶段没有应用比例因子s，所以根据估计的s对p^WG_ WL进行调整。
B.紧耦合优化
在GNSS- slam初始化后，将GNSS原始测量值，即伪距和多普勒频移加入优化中。预计的状态包括：
K是滑动窗口内图像数量，M是标志数量，E是GNSS在滑动窗口中的数量，f是标志逆深度。
误差函数定义为：
对于GNSS因子，伪距残差和多普勒频移残差记为：
C.滑动窗口和边缘化
在滑动窗口中，与GNSS相关的参数，即GNSS接收时钟偏差和时钟漂移，加到它前面的图像帧上。在某一图像帧上可能存在多个GNSS参数节点。
如果次新帧是一个关键帧，我们将其保持在滑动窗口中，并边缘化最老的关键帧及其相关的GNSS参数和特征点，以及与它们相关的重投影因子、IMU因子和GNSS因子。如果次新帧不是一个关键帧，简单地删除帧和它所有相应的视觉测量。但次新帧相关GNSS参数不会被移除，而是附加在倒数第三的最新帧上。最初附加到次新帧的GNSS因子将被更改，原因是上述GNSS因子中的IMU预积分需要重新计算，因为它们的起始时刻将从第二最新帧切换到第三最新帧。上述GNSS因子中的GNSS测量值保持不变。
我们使用舒尔补的方法进行边缘化，值得注意的是，由于应用了边缘化，已滑出滑动窗口的GNSS测量值仍然对当前滑动窗口中的状态估计产生影响。因此，即使当前滑动窗口中没有GNSS测量值，R^WG_WL和p^WG_WL仍然是可观测的。
图2：滑动窗口中的边缘化，空心圆表示参数(节点)，实心矩形表示因子(边)。为了解释清楚，这里没有显示摄像机-imu的外部参数和边缘因子。
D. 去除GNSS噪声
在城市中，原始的GNSS测量可能会非常嘈杂。因此，在GNSS测量值进入滑动窗口进行优化之前，通过以下两种可能的方法之一进行滤波。第一种方法是进行单点定位和速度测量。对于第二种方法，如果在过去5秒内排除所有GNSS测量值，则使用原始多普勒移测量值进行单点定位和速度确定。否则在滑动窗口内进行联合优化，优化只涉及当前时刻的GNSS测量值。对于这两种方法，我们都计算了优化后的伪距和多普勒频移测量的残差。当伪距和多普勒偏移的残差分别超过阈值T^P和T^D时，将被滤除。如果在某个GNSS时段中滤除噪声测量值后，剩下的GNSS测量值太少，我们会将该时段的所有GNSS测量值一起删除。第一种方法在速度上有优势，第二种方法在精度上有优势。对于第二种方法，如果在过去5秒内所有GNSS测量都被排除在外，那么执行GNSS单点定位可以防止良好的GNSS测量因为错误的当前位置估计而被排除在外。
实验结果
这里GNSS SPP指的是GNSS单点定位。Proposed指本文提出的方法。符号*表示结果不可用。GNSS单点定位仅提供定位结果，无法获得旋转精度。
比较东、北方向的轨迹。在图(b)中，省略了一些位于图外的非常嘈杂的GNSS SPP结果。所有的轨迹都与ground true一致。
高度方向的轨迹比较。在图(a)和(b)中，忽略了图外的一些非常嘈杂的GNSS SPP结果。所有的轨迹都与地面真相一致。
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。
点击阅读原文， 即可获取本文下载链接。
