论文阅读: 基于SLAM的使用GPS和鱼眼相机第Integrity Monitoring

Abstract

在urban场景用GPS和鱼眼有multipath effect. 我们提出了一种SLAM-Based IM(Integrity Monitoring)算法来计算位置保护等级. 我们用GPS pseudoranges的连续数据, 像素光度, 车辆动态和satellite ephemeris (卫星星历表)来同时计算车辆位置和landmarks: GPS卫星和图像像素.

我们估计了fault mode vector通过GPS measurement residuals来分析temporal correlation和通过视觉光度残差来计算spatial correlation.

仔细说, 为了检测和孤立视觉错误, 我们研发了基于superpixel的RANSAC算法来做spatial voting.

对于一个estimated fault mode, 我们通过使用线性的Graph-SLAM framework的worst-case failure slope analysis计算protection level.

我们在semi-urban场景, 展现了成功的错误的检测和孤立.

1. Introduction

IM是导航方案估计中很重要的东西.

因为好多种的障碍, GPS系统会接收到更少的观测, 因为urban环境下degraded satellite visibility. 他们也suffer from multipath和由传输过程引起的异常现象.

为了解决上述的挑战, 一种可能的方案是多传感器融合. 视觉传感器在urban场景还不错因为丰富的纹理.

我们考虑了global landmarks-GPS和局部landmarks-像素keypoint.

这里世界系被表示为Earth-Centered Earth-Fixed (ECEF) 坐标系.

2. SLAM-based IM using GPS and Fish-eye Camera

1. 初始化阶段, 我们用PVT和通过GPS receiver algorithm计算的卫星来初始化3D地图. 我们把所有的GPS测量误差状态设为0.5表示neutrality(中性). 在视觉方面, 我们用初始标定来估计尺度.
2. 我们用hybrid sky detection算法来预处理图像, 来区分sky-pixel和non-sky pixel. 检测出来的sky-pixel是用来区分LOS和NLOS的卫星的.
3. 我们考虑non-sky pixels和GPS伪距和carrier-to-noise density (C/N_0), 接收器的运动模型和卫星轨迹模型是我们算法的估计输入. 我们把这些观测结合在extended图优化里来估计总体的状态向量. 状态向量包括: 车辆, GPS卫星, 图像像素.
4. 我们单独分析了观测残差against empirical distribution(经验分布)来检测和区分GPS faults. 我们用super-pixel RANSAC来做spatial voting来检测和孤立视觉错误.
5. 最后, 利用估计的fault mode和总体的状态向量, 我们formulate failure slope for Graph-SLAM.

2.1 观测预处理

• GPS伪距和(C/N_0) (从GPS receiver拿到).
• 像素internsity(从鱼眼图拿到)

GPS Module

我们描述GPS观测如下:

[ ho^{k}=left|mathbf{y}^{k}-mathbf{x} ight|+left(c delta t-c delta t^{k} ight)+eta^{k} ]

这里(x)表示车辆的3D位置, (y)表示第k个卫星.

(cdelta t)和 (cdelta t^k)表示接收器的始终bias和第k个卫星始终的bias.

(mu ^k) 表示根据第k个卫星的测量噪声.

---

第k个卫星的估计协方差用(C/N_0)来表示. 注意LOS和NLOS卫星的分类是用未知的状态向量给的. 那么第k个卫星的测量协方差就是:

[left(sigma^{k}left(oldsymbol{x}_{t}, oldsymbol{y}_{t}^{k} ight) ight)^{2}=sqrt{b^{k}+a^{k} frac{1}{left(C / N_{0} ight)^{k}}} ext { from }[17] ]

• (x_t)表示车辆的状态向量, 在t时刻的, 包括3D位置, 速度, 时钟bias, 时钟drift和3D attitude.
• (y_t^i)表示第i个卫星的状态向量, 包括位置, 速度, 时钟bias, 时钟drift corrections.
• (b^k)和(a^k)是视觉系数, .....

2.2 Extended graph optimization

我们的扩展图优化有四个误差项:

• GPS伪距
• non-sky intensity
• 接收器运动模型
• 卫星轨迹模型

[egin{aligned} e_{t}left(oldsymbol{ heta}_{t} ight)=&left.sum_{k=1}^{N} Lambdaleft(left(left(vec{r}_{t}^{k}+1 ight) sigma_{t}^{k} ight)^{-1} | ho_{t}^{k}-hleft(oldsymbol{x}_{t}, y_{t}^{k} ight) | ight)+sum_{k=1}^{N} Lambdaleft(hat{Omega}_{t}^{k} ight)^{-1}left|oldsymbol{y}_{t}^{i}-fleft(u_{t}^{k}, ar{y}_{t-1}^{k} ight) ight| ight) \ &+Lambdaleft(left(ar{x}_{t} oldsymbol{I}+hat{Sigma}_{t} ight)^{-1}left|x_{t}-gleft(u_{E, t}, ar{x}_{t-1} ight) ight| ight)+sum_{y in mathbb{H}} Lambdaleft(left(left(ar{s}_{t-1}(u)+1 ight) omega_{t}(u) ight)^{-1}left|I_{k f}(oldsymbol{u})-I_{t}left(pileft(wleft(Delta mu_{t}, u ight) ight) ight) ight| ight) end{aligned} ]

• (h)是GPS观测模型, (g)是receiver的运动模型, (f)是卫星轨迹模型.
• (overline{oldsymbol{x}}_{t-1})是车辆的状态向量的估计, (overline{oldsymbol{y}}_{t-1}^{k}) 是第k个卫星的状态向量估计. ----前一时刻的
• (u_{R, t})是车辆的运动控制输入, (u_t^k)是卫星的运动控制输入

上面式子的前三项是GSP伪距, 卫星星历表, 车辆状态向量. 最后一项是non-sky像素.

2.3 IM for Graph-SLAM framework

1) Multiple FDI module

GPS faults: 为了在伪距中检测和估计GPS faults, 我们把评估每一个残差against 经验高斯分布----这玩意儿标识在没有错误的情况下观测的误差分布.

这个已经是正当的因为我们发现GPS观测在没有错误的情况下就是遵循高斯分布的[11].

Vision faults: 与GPS faults不同, 因为数据关联错误引起的vision faults会展现出high spatial correlation across image pixels and low temporal correlation. 这个也是正当的因为视觉faults are localizaed to a group of neighboring pixels and are not isolated to a standalone pixel. 我们用superpixle-based RANSAC来spatial voting.

我们先把图像分成clusters, 也就是superpixels.

(Gamma): 是non-sky pixels的superpixel的个数.

2) Protection level computation

3. Experiment Results

4. Conclusions