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
- 初始化阶段, 我们用PVT和通过GPS receiver algorithm计算的卫星来初始化3D地图. 我们把所有的GPS测量误差状态设为0.5表示neutrality(中性). 在视觉方面, 我们用初始标定来估计尺度.
- 我们用hybrid sky detection算法来预处理图像, 来区分sky-pixel和non-sky pixel. 检测出来的sky-pixel是用来区分LOS和NLOS的卫星的.
- 我们考虑non-sky pixels和GPS伪距和carrier-to-noise density (C/N_0), 接收器的运动模型和卫星轨迹模型是我们算法的估计输入. 我们把这些观测结合在extended图优化里来估计总体的状态向量. 状态向量包括: 车辆, GPS卫星, 图像像素.
- 我们单独分析了观测残差against empirical distribution(经验分布)来检测和区分GPS faults. 我们用super-pixel RANSAC来做spatial voting来检测和孤立视觉错误.
- 最后, 利用估计的fault mode和总体的状态向量, 我们formulate failure slope for Graph-SLAM.
2.1 观测预处理
- GPS伪距和$C/N_0$ (从GPS receiver拿到).
- 像素internsity(从鱼眼图拿到)
GPS Module
我们描述GPS观测如下: $$ \rho^{k}=\left|\mathbf{y}^{k}-\mathbf{x}\right|+\left(c \delta t-c \delta t^{k}\right)+\eta^{k} $$ 这里$x$表示车辆的3D位置, $y$表示第k个卫星.
$c\delta t$和 $c\delta t^k$表示接收器的始终bias和第k个卫星始终的bias.
$\mu ^k$ 表示根据第k个卫星的测量噪声.
第k个卫星的估计协方差用$C/N_0$来表示. 注意LOS和NLOS卫星的分类是用未知的状态向量给的. 那么第k个卫星的测量协方差就是: $$ \left(\sigma^{k}\left(\boldsymbol{x}{t}, \boldsymbol{y}{t}^{k}\right)\right)^{2}=\sqrt{b^{k}+a^{k} \frac{1}{\left(C / N_{0}\right)^{k}}} \text { 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
- 接收器运动模型
- 卫星轨迹模型
$$ \begin{aligned} e_{t}\left(\boldsymbol{\theta}{t}\right)=&\left.\sum{k=1}^{N} \Lambda\left(\left(\left(\vec{r}{t}^{k}+1\right) \sigma{t}^{k}\right)^{-1} | \rho_{t}^{k}-h\left(\boldsymbol{x}{t}, y{t}^{k}\right) |\right)+\sum_{k=1}^{N} \Lambda\left(\hat{\Omega}{t}^{k}\right)^{-1}\left|\boldsymbol{y}{t}^{i}-f\left(u_{t}^{k}, \bar{y}{t-1}^{k}\right)\right|\right) \ &+\Lambda\left(\left(\bar{x}{t} \boldsymbol{I}+\hat{\Sigma}{t}\right)^{-1}\left|x{t}-g\left(u_{E, t}, \bar{x}{t-1}\right)\right|\right)+\sum{y \in \mathbb{H}} \Lambda\left(\left(\left(\bar{s}{t-1}(u)+1\right) \omega{t}(u)\right)^{-1}\left|I_{k f}(\boldsymbol{u})-I_{t}\left(\pi\left(w\left(\Delta \mu_{t}, u\right)\right)\right)\right|\right) \end{aligned} $$
- $h$是GPS观测模型, $g$是receiver的运动模型, $f$是卫星轨迹模型.
- $\overline{\boldsymbol{x}}{t-1}$是车辆的状态向量的估计, $\overline{\boldsymbol{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的个数.