转载自:https://mp.weixin.qq.com/s/jlkXvygaTsql1nk76HD94g
【泡泡图灵智库】基于优化的视觉惯性SLAM与GNSS紧耦合
原创 泡泡机器人 泡泡机器人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】公众号。
点击阅读原文, 即可获取本文下载链接。