CN103454650A - 一种视觉辅助卫星完好性监测方法 - Google Patents

Abstract

本发明公开了一种视觉辅助卫星完好性监测方法，包括步骤一：通过对视觉采集的图像进行处理，采用高精度的视觉导航模型获得导航观测量，并对视觉导航观测量的误差进行估计。步骤二：采用进近阶段高空相对较好的卫星信号对视觉观测量进行标定，抑制视觉观测量误差。步骤三：将被标定后的视觉伪距信息与卫星观测信息进行组合，对RAIM可用性进行判断。步骤四：对存在故障星的情况进行故障识别，从而确定并排除故障卫星。步骤五：采用排除过故障卫星后的卫星数据与视觉观测量进行融合，获得最终的组合定位结果。本发明通过利用视觉传感器提供的观测量作为辅助，丰富导航观测量，从而提高卫星完好性检测性能，提高导航定位结果的可靠性。

Description

一种视觉辅助卫星完好性监测方法

技术领域

本发明属于卫星导航领域，具体地说，是指一种视觉辅助卫星完好性监测方法。

背景技术

接收机自主完好性监测(RAIM)是卫星导航中的一项关键技术，它通过对接收到的卫星信号的完好性进行判断，在发生故障时及时报警。因此，RAIM对保证卫星导航定位结果的可靠性具有重要作用。

当前，RAIM技术已被广泛应用于空管导航中，其采用冗余观测信息来检测卫星故障。通常，在较好的飞行环境下，可见卫星数量充足且信号质量较好，此时，RAIM技术可以得到可信的完好性告警，可靠保障飞行安全。然而，在复杂进近着陆等恶劣环境下，例如被山群环绕的中国林芝机场，脆弱的卫星信号容易因遮挡或干扰而出现遮蔽角度较大或卫星信号丢失的现象，这将导致可见卫星几何分布较差或导航信息冗余度不足，降低RAIM算法的性能。随着复杂机场、复杂低空等的不断开辟，复杂进近着陆环境下的RAIM技术将成为未来研究的重要课题。

在复杂进近着陆环境下，由于卫星信号本身质量下降，传统仅基于卫星导航信号的RAIM算法性能将无法满足导航性能需求。因此，一个很自然的想法是利用其他传感器提供的导航信息来辅助提高RAIM。目前，传感器辅助RAIM采用的辅助传感器包括气压高度表，惯导，以及其他卫星系统，该类方法采用其他传感器观测量来增加导航信息冗余度，实现完好性增强。气压高度表辅助RAIM方法往往采用气压高度表提供的垂向距离分量，将卫星几何定位方程与测高方程进行组合，丰富导航参数信息，从而提高完好性监测效果。然而，气压高度表测高的准确性受气流影响很大，而气压强剧变在复杂进近着陆环境，尤其是高原机场，很是常见，此时的气压高度表测量值很可能因精度较差而无法辅助RAIM。惯导辅助RAIM方法通常利用惯导提供的速度与姿态信息，在Kalman滤波的框架下建立惯导/卫导组合观测方程，获得动态条件下的RAIM增强算法。但是，惯导系统的误差会随时间漂移，其很可能污染RAIM结果。Stanford大学研究者采用GLONASS卫星系统与GPS卫星系统进行结合，在模式识别中经典的RANSAC算法的基础上，提出了GNSS下的RANCO算法提高了故障检测尤其是多故障检测的性能。但是，在复杂进近着陆环境下，由于信号遮挡或干扰造成较大的遮蔽角，因此很难获得如此多的可见卫星。

考虑到现有方法存在不足之处，因此有必要研究引入其他传感器来辅助RAIM。本发明提出了一种新的视觉辅助完好性监测方法来提高复杂进近着陆环境下的RAIM性能。考虑到视觉系统具有信息量大、无源性、体积小以及易装载等优点，极具潜力辅助提高RAIM性能。

然而，视觉辅助RAIM是一项非常具有挑战性的工作。用于辅助RAIM的视觉导航观测量必需是准确可靠的。然而，尽管现有的视觉信息提取技术表现得很好，但是由于背景干扰或平台抖动等不确定因素，视觉观测量将不可避免地存在误差。考虑到视觉系统本质上是测角系统，视觉误差有可能随着视线长度的增加而被放大到导航误差中。例如，视觉检测误差在图像上可能只有几个像素，但映射到三维空间中可能是几十米甚至更大的导航误差。

发明内容

本发明为了解决原有卫星完好性检测算法的用于复杂环境时，冗余信息降低而导致算法性能不足的问题，提出了一种视觉辅助卫星完好性监测方法。本方法通过利用视觉传感器提供导航冗余信息，以实现卫星完好性监测性能的提高。

本发明的一种视觉辅助卫星完好性监测方法，具体包括以下步骤：

步骤一：通过对视觉采集的图像进行处理，采用高精度的视觉导航模型获得导航观测量，并对视觉导航观测量的误差进行估计。

步骤二：采用进近阶段高空相对较好的卫星信号对视觉观测量进行标定，从而进一步抑制视觉观测量误差。

步骤三：将被标定后的视觉伪距信息与卫星观测信息进行组合，对RAIM可用性进行判断。若不可用时，输出不可用告警，结束。否者，采用视觉辅助RAIM方法中，获得检测统计量。当检测统计量大于门限值时，认为存在故障，并进行报警，执行步骤四。否则，执行步骤五。

步骤四：对存在故障星的情况进行故障识别，从而确定并排除故障卫星，转至步骤三。

步骤五：采用排除过故障卫星后的卫星数据与视觉观测量进行融合，获得最终的组合定位结果。

本发明通过利用视觉传感器提供的观测量作为辅助，丰富导航观测量，从而提高卫星完好性检测性能，提高导航定位结果的可靠性。

为了提高视觉导航观测量的准确性，本发明提出了一个两步误差抑制方法，即误差形成抑制和误差标定。首先，为了抑制视觉导航误差的形成，本发明采用一种具有高精度的视觉导航模型获得视觉导航信息。随后，本发明利用高质量的卫星信号来对视觉导航误差进行标定，从而进一步减小视觉的误差。然而，通过以上误差抑制方法不可能完全去除视觉误差，尤其是随机误差。因此，为了有效利用视觉辅助RAIM，并抑制视觉系统误差对视觉/卫星融合的负面影响，本发明提出了一种基于鲁棒估计的加权奇偶空间法用于视觉辅助RAIM，最终实现视觉辅助完好性性能的提高。

目前，针对视觉辅助卫星完好性监测的问题，还鲜有探讨。本发明首次对该问题进行研究并提出了一种视觉辅助RAIM来提高卫星完好性监测性能。与本发明类似的工作为视觉导航，然而，该工作主要考虑的是卫星不可用时的场景。尽管视觉辅助卫星在视觉/卫星组合定位方面有一些研究，但对利用视觉辅助卫星完好性监测问题的关注相对较少。

附图说明

图1表示本发明提供的视觉辅助卫星完好性监测流程图；

图2表示实施例中的仿真场景示意图；

图3表示实施例得到的视觉伪距误差特性曲线；

图4表示实施例得到的视觉辅助RAIM的故障检测率曲线；

图5表示实施例得到的视觉定位误差分布；

图6表示实施例得到的卫导定位误差分布；

图7表示实施例得到的视觉/卫导组合定位误差分布；

图8表示实施例得到的视觉信息的引入对几何分布的改善。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的一种视觉辅助卫星完好性监测方法，方法流程图如图1所示，具体包括以下步骤：

步骤一：通过对视觉采集的图像进行处理，采用高精度的视觉导航模型获得导航观测量，并对视觉导航观测量的误差进行估计。

通过采用特征点检测匹配算法获取地标在计算机视觉设备提取的视觉图像中的二维坐标

其中下标i为第i个地标点的序号，上角标I代表图像平面直角坐标系(因为地标最后会在图像中显示，本发明获得的是它在图像中的位置)。与此同时，计算获得地标齐次矢量

其中f为相机焦距。

将所有的地标在视觉图像中的二维坐标值和地标先验参数代入到高精度视觉模型中计算视觉伪距，其中视觉模型场景示意图如图2所示，图中以三个地标进行示意，其中为相机c在地心地固坐标系(ECEF坐标系)下的坐标，为第i个地标在ECEF坐标系下的坐标，m_i是第i个地标的视觉伪距，d_ij是第i个地标和第j个地标的距离，θ_ij为第i个地标和第j个地标相对于相机的视线夹角。

采用最小二乘法，通过求解如下二次方程求解视觉伪距：

$\underset{\tilde{m}}{\min }J(m,\tilde{m})=\underset{i=1}{\overset{N_l}{\mathrm{\Σ}}}{(m_i-\widetilde{m_i})}^2---\left(2\right)$

其中视觉伪距矢量 $m={[m_1,m_2,...,m_{N_l}]}^T$ ， $\tilde{m}={[{\tilde{m}}_1,{\tilde{m}}_2,...,{\tilde{m}}_{N_l}]}^T$ 是视觉伪距矢量的估计矢量。N₁是先验地标的数量，是地标视线的所有组合

视觉伪距对矢量m_ij≡[m_i，m_j，1]^T。式(2)中的系数矩阵为：

$K_{\mathrm{ij}}=\left[\begin{array}{ccc}1& -c_{\mathrm{ij}}& 0\\ -c_{\mathrm{ij}}& 1& 0\\ 0& 0& -d_{\mathrm{ij}}^2\end{array}\right]---\left(3\right)$

其中c_ij是地标i与地标j的视线夹角的余弦值，可由地标齐次矢量求得，即：其中〈·〉表示内积，||·||为范数。

由公式(1)-(3)可以看出，视觉伪距误差由参数和c_ij的误差决定。由于地标之间的距离很可能无法精确获得，因此分别对每个地标进行定位

然后采用位置差法求得地标之间的距离。

$d_{\mathrm{ij}}^2={\left|\right|p_i^E-p_j^E\left|\right|}^2---\left(4\right)$

其中：

表示第j个地标在ECEF坐标系下的坐标。

位置差法的误差可求得

的误差

为：

${\mathrm{\&delta;d}}_{\mathrm{ij}}^2=2<p_i^E-p_j^E,\mathrm{\&delta;}{p}_i^E-\mathrm{\&delta;}{p}_j^E>---\left(5\right)$

其中和

分别表示对地标i和地标j位置的估计误差。

假设地标定位误差服从独立高斯分布

其中

为地标定位误差的方差，则误差也服从高斯分布，且均值为

方差为 ${\mathrm{\&sigma;}}_{\mathrm{\&delta;}{d}_{\mathrm{ij}}^2}^2=4\underset{\mathrm{\&alpha;}=x,y,z}{\mathrm{\&Sigma;}}{({\mathrm{\&alpha;}}_i^E-{\mathrm{\&alpha;}}_j^E)}^2{\mathrm{\&sigma;}}_{{(\mathrm{\&delta;}{\mathrm{\&alpha;}}_i^E-\mathrm{\&delta;}{\mathrm{\&alpha;}}_j^E)}^2}^2=8d_{\mathrm{ij}}^2{\mathrm{\&sigma;}}_g^2.$

两个地标视线夹角的余弦值c_ij的误差可求得如下

${\mathrm{\&delta;c}}_{\mathrm{ij}}=<\frac{p_j^C}{\left|\right|p_i^C\left|\right|\left|\right|p_i^C\left|\right|}-\frac{c_{\mathrm{ij}}{p}_i^C}{{\left|\right|p_i^C\left|\right|}^2},\mathrm{\&delta;}{p}_i^C>+<\frac{p_i^C}{\left|\right|p_i^C\left|\right|\left|\right|p_j^C\left|\right|}-\frac{c_{\mathrm{ij}}{p}_i^C}{{\left|\right|p_i^C\left|\right|}^2},\mathrm{\&delta;}{p}_j^C>---\left(6\right)$

其中

和

表示地标i和地标j在相机坐标系C中的坐标，和

对应其误差变量。

假设地标检测误差服从独立高斯分布

其中

为地标检测误差的方差则误差δc_ij同样服从高斯分布，其均值

归一化方差为：

$J(p_i^C,p_j^C,c_{\mathrm{ij}})=\frac{{\mathrm{\&sigma;}}_{{\mathrm{\&delta;c}}_{\mathrm{ij}}}^2}{{\mathrm{\&sigma;}}_m^2}$

$=-(\frac{{\left|\right|p_i^C\left|\right|}^2+1}{{\left|\right|p_i^C\left|\right|}^4}+\frac{{\left|\right|p_j^C\left|\right|}^2+1}{{\left|\right|p_j^C\left|\right|}^4})c_{\mathrm{ij}}^2+\frac{2}{\left|\right|p_i^C\left|\right|\left|p_j^C\right||}(\frac{1}{{\left|\right|p_i^C\left|\right|}^2}+\frac{1}{{\left|\right|p_j^C\left|\right|}^2})c_{\mathrm{ij}}+\frac{{\left|\right|p_2^C\left|\right|}^2+{\left|\right|p_j^C\left|\right|}^2-2}{{\left|\right|p_i^C\left|\right|}^2{\left|\right|p_j^C\left|\right|}^2}---\left(7\right)$

$s.t.\cos ({\cos }^{-1}\frac{1}{\left|\right|p_i^C\left|\right|}+{\cos }^{-1}\frac{1}{\left|\right|p_j^C\left|\right|})<c_{\mathrm{ij}}<\cos ({\cos }^{-1}\frac{1}{\left|\right|p_i^C\left|\right|}-{\cos }^{-1}\frac{1}{\left|\right|p_j^C\left|\right|}),\left|\right|p_i^C\left|\right|\&GreaterEqual;1---\left(8\right)$

其中约束条件(8)保证地标i和对标j均在图像内。

视觉伪距误差δm与

和的关系可得：

$\left[\begin{array}{ccccccccccc}\frac{1}{m_2}& -& \frac{c_{12}}{m_1}& & \frac{1}{m_1}& -& \frac{c_{12}}{m_2}& & & 0& \\ & 0& & & \frac{1}{m_3}& -& \frac{c_{23}}{m_2}& & \frac{1}{m_2}& -& \frac{c_{23}}{m_3}\\ \frac{1}{m_3}& -& \frac{c_{31}}{m_1}& & & 0& & & \frac{1}{m_1}& -& \frac{c_{31}}{m_3}\end{array}\right]\mathrm{\&delta;m}=\left[\begin{array}{c}{\mathrm{\&delta;c}}_{12}\\ {\mathrm{\&delta;c}}_{23}\\ {\mathrm{\&delta;c}}_{31}\end{array}\right]+\left[\begin{array}{c}\frac{{\mathrm{\&delta;d}}_{12}^2}{2m_1{m}_2}\\ \frac{{\mathrm{\&delta;d}}_{23}^2}{2m_2{m}_3}\\ \frac{\mathrm{\&delta;}{d}_{31}^2}{2m_3{m}_1}\end{array}\right]---\left(9\right)$

上述方程可记为Tδm＝δc+δd通过求解上述方程，可通过最小二乘法求得视觉伪距误差

由于在给定地标估计位置时，误差项δd是确定值，因此视觉伪距误差的互相关矩阵

(E[·]为均值算子)，从而获得视觉伪距误差估计。

步骤二：采用进近阶段高空相对较好的卫星信号对视觉观测量进行标定，从而进一步抑制视觉观测量误差。

以卫星接收天线获得的卫星数据作为输入，通过采用进近阶段收星状况较好条件下的卫星信号作为基准，纠正视觉系统的固定偏差。

a)采用传统RAIM算法对卫星信号数据进行验证，对卫星信号质量进行判断，如果RAIM可用且卫星完好时，则进行视觉误差标定，进入步骤b)，否则，说明卫星信号出现完好性问题，因此，需要视觉辅助进行完好性检测，转到步骤三。

b)根据完好的卫星信号计算接收机准确的位置，利用地标先验信息计算接收机与地标之间的距离，从而获得准确的视觉伪距及余弦值，并与利用式(1)、式(2)中的视觉伪距矢量m和余弦值c_ij作差得到视觉伪距误差δm和余弦值误差δc_ij，将这些参数带入到式(9)中。以为未知参数求解方程(9)，即可求得距离平方偏差然后用纠正步骤一中地标距离平方参数

c)在进近着陆阶段，采用纠正后的地标距离平方参数

代入(3)中，通过求解式(1)、式(2)来获得标定后的视觉伪距，即获得高精度的视觉伪距信息。

由图3所示，左图和右图分别为视觉伪距误差均值和标准差在ECEF坐标系三个方向的分量，单位为米；其横坐标为视觉作用距离，单位为百米。由左图可以看出，通过步骤一和步骤二后，视觉伪距误差的均值接近于0，从而验证误差抑制方法有效地去除了视觉伪距固定偏差。同时，由右图可以看出，尽管视觉伪距误差的方差受误差放大效应的影响，随着视线距离的增加而增大，但是增大趋势区域平缓，从而验证误差抑制方法的对误差放大效应的抑制作用。实验证明，通过本文发明误差抑制方法，能够获得相对精确的视觉伪距观测量。

步骤三：将被标定后的视觉伪距与卫星观测信息进行组合，对RAIM可用性进行判断。若不可用时，输出不可用告警，结束。否者，采用视觉辅助RAIM方法，获得检测统计量。当检测统计量大于门限值时，认为存在故障，并进行报警，执行步骤四。否则，执行步骤五。

具体为：

a)建立卫星非线性观测方程：

$G_{\mathrm{\&pi;}}={\mathrm{\&rho;}}_{\mathrm{\&pi;}}-\left|\right|p_{\mathrm{\&pi;}}^E-p_c^E\left|\right|-C_b=0---\left(10\right)$

其中：ρ_π是第π颗可见卫星伪距，

是该卫星在ECEF坐标系下的位置，C_b是接收机钟差。

b)通过对(10)线性化，可获得N_s颗卫星线性观测模型方程：

Z＝Hx+ε+b_j                 (11)

其中Z是N_s×1伪距残差矢量，H是N_s×4观测矩阵，x是状态误差矢量，包括三维位置[Δ_x，Δ_y，Δ_z]，和接收机钟差ΔC_b，ε是不确定独立高斯分布，方差矩阵为

为观测伪距方差。但卫星故障模型b_j＝be_j，其中j是故障卫星，e_j是的第j个标准基底矢量，b为故障大小。

c)对权重矩阵进行奇异值分解，得到：

其中是视觉伪距误差互相关矩阵，由于是实对称矩阵，因此可奇异值分解为

其中Λ为对角矩阵，其对角元素为

的奇异值的开方值，U为对应奇异值分解的酉矩阵。

d)将被标定后的视觉伪距与卫星观测信息进行组合，即将(1)-(2)和(11)-(12)进行组合，可获得加权视觉/卫星组合扩展方程：

其中： $\frac{{\&PartialD;F}_{\mathrm{ij}}}{\&PartialD;x}=2[(1-\frac{m_j}{m_i})c_{\mathrm{ij}}(x-x_i)+(1-\frac{m_i}{m_j})c_{\mathrm{ij}}(x-x_j)],\frac{\&PartialD;G_{\mathrm{\&pi;}}}{\&PartialD;x}=\frac{x_{\mathrm{\&pi;}}-x}{{\mathrm{\&rho;}}_{\mathrm{\&pi;}}-C_b}.$ 其中为变量在卫星方程上的扩展形式，

为微分算子，x，y，z分别代表接收机在ECEF坐标系下的三个方向，x_i代表地标i在ECEF坐标系下一个方向的坐标，

为大小为4×N_s的全1矩阵。

e)采用加权最小二乘估计用户位置信息：

$\tilde{x}={\left({\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\left(\stackrel{\&OverBar;}{H}\right)\right)}^{-1}{\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\stackrel{\&OverBar;}{z}---\left(14\right)$

其中鲁棒估计加权矩阵有：

$f_i=\left\{\begin{array}{cc}1& \left|{\tilde{w}}_i\right|\&le;k_0\\ \frac{k_0}{|{\tilde{w}}_i|}\left(\frac{k_1-|{\tilde{w}}_i|}{k_2-k_0}\right)& k_0<\left|\widetilde{w_i}\right|\&le;k_1\\ 0& \left|{\tilde{w}}_i\right|>k_1\end{array}\right.---\left(15\right)$

其中，

为第i个视觉/卫星组合导航残差，其中k₀＝2σ_p和k₁＝6σ_p。

f)计算残差矢量：

$\widetilde{\mathrm{\&epsiv;}}=[I-\stackrel{\&OverBar;}{H}{\left({\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\stackrel{\&OverBar;}{H}\right)}^{-1}{\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}]\stackrel{\&OverBar;}{z}=S\stackrel{\&OverBar;}{z}---\left(16\right)$

g)通过计算HPL值进行可用性判断：

$\mathrm{HPL}=\underset{i}{\max }\{{\mathrm{Slope}}_i\&times;\sqrt{D_T}\}---\left(17\right)$

其中：Slope_i为第i颗卫星的特征斜率，D_T为该航路对应的RAIM阈值。若HPL大于门限时，即判断为不可用时，输出不可用报警，结束，否者，转至h)

h)利用残差矢量计算检测统计量，获得基于鲁棒估计的加权RAIM的检测统计量：

$D_S={\left|\right|\widetilde{\mathrm{\&epsiv;}}/{\mathrm{\&sigma;}}_p\left|\right|}^2---\left(18\right)$

i)通过给定虚警率p_FA，由下公式计算RAIM阈值：

$D_T=\underset{D_T}{\arg }{p}_{\mathrm{FA}}=\frac{1}{2^{(N_S-1)/2}{\mathrm{\&sigma;}}^{N_S-1}\mathrm{\&Gamma;}((N_S-1)/2)}{\&Integral;}_{D_T}^{\&infin;}{x}^{\frac{N_S-3}{2}}{e}^{\frac{x}{2{\mathrm{\&sigma;}}^2}}\mathrm{dx}---\left(19\right)$

其中：e自然常数，

为伽马分布，e为自然常数。

j)将检测统计量D_S与RAIM阈值D_T进行比较，如果D_S≥D_T，即视觉辅助RAIM告警，检测到故障，转至步骤四，进行故障检测。否则，说明无卫星故障，不进行告警，转至步骤五。

步骤四：对存在故障星的情况进行故障识别，从而确定并排除故障卫星。

对步骤三中，检测到的故障，通过采用极大似然法进行故障识别与排除，故障排除后，返回步骤三，继续进行故障检测，直到不再出现故障卫星。

由图4卫星故障检测曲线可以看出，本发明提出的视觉辅助RAIM方法相比于传统RAIM方法获得更高的故障检测率和更小的最小可检测偏差。视觉辅助RAIM方法的最小可检测偏差为8.8σ_n，比不含鲁棒估计的视觉RAIM的15.8σ_n小44.3％，同时，比传统RAIM方法的26.1σ_n好66.3％。同时，不含鲁棒估计的视觉RAIM含有1％的虚警，而加入鲁棒估计之后，视觉辅助RAIM的虚警减少为0。同时，由于视觉观测的加入，观测信息冗余度增加爱，RAIM的可用性也得到了提高，从而提高了RAIM的可用性。由此可见，应用本发明提供的视觉辅助RAIM方法可以提高复杂进近着陆环境下的RAIM性能。

步骤五：采用排除过故障卫星后的卫星数据与视觉观测量进行融合，获得最终的组合定位结果。

通过完好性检测后，排除故障卫星对定位结果的影响，并由式(14)估计得到的用户位置信息作为最终的组合定位结果。图5-7给出了视觉、卫导、视觉/卫导组合三种方式定位误差在ECEF坐标系三个方向上的分布直方图。由于受误差放大效应的影响，视觉定位结果误差较大，ECEF坐标下三维标准差为[32.9 17.9 41.1]^T。卫星由于存在伪距误差，受几何分布的影响，也存在较大的误差，ECEF坐标下三维标准差为[25.9 45.6 19.9]^T。视觉/卫导组合定位结果误差最小，ECEF坐标下三维标准差为[12.0 7.5 8.9]^T。如图8所示，视觉与卫导定位的GDOP值较大，几何分布较差，将视觉信息与卫导信息进行组合后，GDOP值减小，这是因为视觉的引入增加了信息冗余，从而提高了几何分布。

本实例表明，通过采用本发明提出的视觉辅助卫星完好性监测方法，能够有效地排除故障卫星，并提高最终导航定位结果的精度和可靠性。

Claims (1)

1.一种视觉辅助卫星完好性监测方法，具体包括以下步骤：

步骤一：对视觉采集的图像进行处理，采用高精度的视觉导航模型获得导航观测量，对视觉导航观测量的误差进行估计；

通过采用特征点检测匹配算法获取地标在计算机视觉设备提取的视觉图像中的二维坐标

其中下标i为第i个地标点的序号，上角标I代表图像平面直角坐标系；与此同时，计算获得地标齐次矢量

其中f为相机焦距；

将所有的地标在视觉图像中的二维坐标值和地标先验参数代入到高精度视觉模型中计算视觉伪距；

采用最小二乘法，通过求解如下二次方程求解视觉伪距：

$\underset{\tilde{m}}{\min }J(m,\tilde{m})=\underset{i=1}{\overset{N_l}{\mathrm{\&Sigma;}}}{(m_i-{\tilde{m}}_i)}^2---\left(1\right)$

其中视觉伪距矢量 $m={[m_1,m_2,...,m_{N_l}]}^T,$ $\tilde{m}={[{\tilde{m}}_1,{\tilde{m}}_2,...,{\tilde{m}}_{N_l}\text{]}}^{\text{T}}$ 是视觉伪距矢量的估计矢量；N_l是先验地标的数量，

是地标视线的所有组合

视觉伪距对矢量m_ij≡[m_i，m_j，1]^T；式(2)中的系数矩阵为：

$K_{\mathrm{ij}}=\left[\begin{array}{ccc}1& -c_{\mathrm{ij}}& 0\\ -c_{\mathrm{ij}}& 1& 0\\ 0& 0& -d_{\mathrm{ij}}^2\end{array}\right]---\left(3\right)$

其中c_ij是地标i与地标j的视线夹角的余弦值，由地标齐次矢量求得，即：

其中<·>表示内积，||·||为范数；

视觉伪距误差由参数

和

的误差决定，分别对每个地标进行定位，然后采用位置差法求得地标之间的距离；

$d_{\mathrm{ij}}^2={\left|\right|p_i^E-p_j^E\left|\right|}^2---\left(4\right)$

其中：

表示第j个地标在ECEF坐标系下的坐标；

根据位置差法的误差，获得

的误差为：

${\mathrm{\&delta;d}}_{\mathrm{ij}}^2=2<p_i^E-p_j^E,{\mathrm{\&delta;p}}_i^E-{\mathrm{\&delta;p}}_j^E>---\left(5\right)$

其中

和

分别表示对地标i和地标j位置的估计误差；

假设地标定位误差服从独立高斯分布

其中为地标定位误差的方差，则误差

也服从高斯分布，且均值为

方差为 ${\mathrm{\&sigma;}}_{{\mathrm{\&delta;d}}_{\mathrm{ij}}^2}^2=4\underset{\mathrm{\&alpha;}=x,y,z}{\mathrm{\&Sigma;}}{({\mathrm{\&alpha;}}_i^E-{\mathrm{\&alpha;}}_j^E)}^2{\mathrm{\&sigma;}}_{{({\mathrm{\&delta;\&alpha;}}_i^E-{\mathrm{\&delta;\&alpha;}}_j^E)}^2}^2={8d}_{\mathrm{ij}}^2{\mathrm{\&sigma;}}_g^2;$

两个地标视线夹角的余弦值c_ij的误差为：

${\mathrm{\&delta;c}}_{\mathrm{ij}}=<\frac{p_j^C}{\left|\right|p_i^C\left|\right|\left|\right|p_j^C\left|\right|}-\frac{c_{\mathrm{ij}}{p}_i^C}{{\left|\right|p_i^C\left|\right|}^2},{\mathrm{\&delta;p}}_i^C>+<\frac{p_i^C}{\left|\right|p_i^C\left|\right|\left|\right|p_j^C\left|\right|}-\frac{c_{\mathrm{ij}}{p}_j^C}{{\left|\right|p_j^C\left|\right|}^2},{\mathrm{\&delta;p}}_j^C>---\left(6\right)$

其中

和表示地标i和地标j在相机坐标系C中的坐标，

和

对应其误差变量；

假设地标检测误差服从独立高斯分布其中为地标检测误差的方差，则误差δc_ij同样服从高斯分布，其均值

归一化方差为：

$J(p_i^C,p_j^C,c_{\mathrm{ij}})=\frac{{\mathrm{\&sigma;}}_{{\mathrm{\&delta;c}}_{\mathrm{ij}}}^2}{{\mathrm{\&sigma;}}_m^2}$

                                                                (7)

$=-(\frac{{\left|\right|p_i^C\left|\right|}^2+1}{{\left|\right|p_i^C\left|\right|}^4}+\frac{{\left|\right|p_j^C\left|\right|}^2+1}{{\left|\right|p_j^C\left|\right|}^4})c_{\mathrm{ij}}^2+\frac{2}{\left|\right|p_i^C\left|\right|\left|\right|p_j^C\left|\right|}(\frac{1}{{\left|\right|p_i^C\left|\right|}^2}+\frac{1}{{\left|\right|p_j^C\left|\right|}^2})c_{\mathrm{ij}}+\frac{{\left|\right|p_2^C\left|\right|}^2+{\left|\right|p_j^C\left|\right|}^2-2}{{\left|\right|p_i^C\left|\right|}^2{\left|\right|p_j^C\left|\right|}^2}$

$s.t.\cos ({\cos }^{-1}\frac{1}{\left|\right|p_i^C\left|\right|}+{\cos }^{-1}\frac{1}{\left|\right|p_j^C\left|\right|})<c_{\mathrm{ij}}<\cos ({\cos }^{-1}\frac{1}{\left|\right|p_i^C\left|\right|}-{\cos }^{-1}\frac{1}{\left|\right|p_j^C\left|\right|}),\left|\right|p_i^C\left|\right|\&GreaterEqual;1---\left(8\right)$

其中约束条件(8)保证地标i和对标j均在图像内；

视觉伪距误差δm与c_ij和

的关系可得：

$\left[\begin{array}{ccc}\frac{1}{m_2}-\frac{c_{12}}{m_1}& \frac{1}{m_1}-\frac{c_{12}}{m_2}& 0\\ 0& \frac{1}{m_3}-\frac{c_{23}}{m_2}& \frac{1}{m_2}-\frac{c_{23}}{m_3}\\ \frac{1}{m_3}-\frac{c_{31}}{m_1}& 0& \frac{1}{m_1}-\frac{c_{31}}{m_3}\end{array}\right]\mathrm{\&delta;m}=\left[\begin{array}{c}{\mathrm{\&delta;c}}_{12}\\ {\mathrm{\&delta;c}}_{23}\\ {\mathrm{\&delta;c}}_{31}\end{array}\right]+\left[\begin{array}{c}\frac{{\mathrm{\&delta;d}}_{12}^2}{{2m}_1{m}_2}\\ \frac{{\mathrm{\&delta;d}}_{23}^2}{{2m}_2{m}_3}\\ \frac{{\mathrm{\&delta;d}}_{31}^2}{{2m}_3{m}_1}\end{array}\right]---\left(9\right)$

上述方程可记为Tδm＝δc+δd通过求解上述方程，可通过最小二乘法求得视觉伪距误差δm＝(T^TT)^-1T^T(δc+δd)；由于在给定地标估计位置时，误差项δd是确定值，因此视觉伪距误差的互相关矩阵

E[·]为均值算子，从而获得视觉伪距误差估计；

步骤二：采用进近阶段高空相对较好的卫星信号对视觉观测量进行标定，进一步抑制视觉观测量误差；

具体为：

a)采用RAIM算法对卫星信号数据进行验证，对卫星信号质量进行判断，如果RAIM可用且卫星完好时，则进行视觉误差标定，进入步骤b)，否则，说明卫星信号出现完好性问题，因此，需要视觉辅助进行完好性检测，转到步骤三；

b)根据完好的卫星信号计算接收机准确的位置，利用地标先验信息计算接收机与地标之间的距离，从而获得准确的视觉伪距及余弦值，并与利用式(1)、式(2)中的视觉伪距矢量m和余弦值c_ij作差得到视觉伪距误差δm和余弦值误差δc_ij，将上述参数带入到式(9)中；以

为未知参数求解方程(9)，得到距离平方偏差

然后用

纠正步骤一中地标距离平方参数

c)在进近着陆阶段，采用纠正后的地标距离平方参数代入(3)中，通过求解式(1)、式(2)来获得标定后的视觉伪距；

步骤三：将被标定后的视觉伪距与卫星观测信息进行组合，对RAIM可用性进行判断；若不可用时，输出不可用告警，结束；否者，采用视觉辅助RAIM方法，获得检测统计量；当检测统计量大于门限值时，认为存在故障，并进行报警，执行步骤四；否则，执行步骤五；

具体为：

a)建立卫星非线性观测方程：

$G_{\mathrm{\&pi;}}={\mathrm{\&rho;}}_{\mathrm{\&pi;}}-\left|\right|p_{\mathrm{\&pi;}}^E-p_c^E\left|\right|-C_b=0---\left(10\right)$

其中：ρ_π是第π颗可见卫星伪距，

是该卫星在ECEF坐标系下的位置，C_b是接收机钟差；

b)通过对(10)线性化，获得N_s颗卫星线性观测模型方程：

Z＝Hx+ε+b_j         (11)

其中Z是N_s×1伪距残差矢量，H是N_s×4观测矩阵，x是状态误差矢量，包括三维位置[Δx，Δy，Δz]，和接收机钟差ΔC_b，ε是不确定独立高斯分布，方差矩阵为

为观测伪距方差；卫星故障模型b_j＝be_j，其中j是故障卫星，e_j是

的第j个标准基底矢量，b为故障大小；

c)对权重矩阵进行奇异值分解，得到：

其中

是视觉伪距误差互相关矩阵，奇异值分解为

其中Λ为对角矩阵，其对角元素为

的奇异值的开方值，U为对应奇异值分解的酉矩阵；

d)将被标定后的视觉伪距与卫星观测信息进行组合，即将(1)-(2)和(11)-(12)进行组合，获得加权视觉/卫星组合扩展方程：

其中： $\frac{\&PartialD;F_{\mathrm{ij}}}{\&PartialD;x}=2[(1-\frac{m_j}{m_i})c_{\mathrm{ij}}(x-x_i)+(1-\frac{m_i}{m_j})c_{\mathrm{ij}}(x-x_j)],$ $\frac{\&PartialD;G_{\mathrm{\&pi;}}}{\&PartialD;x}=\frac{x_{\mathrm{\&pi;}}-x}{{\mathrm{\&rho;}}_{\mathrm{\&pi;}}-C_b};$ 其中

为变量在卫星方程上的扩展形式，为微分算子，x，y，z分别代表接收机在ECEF坐标系下的三个方向，x_i代表地标i在ECEF坐标系下一个方向的坐标，

为大小为4×N_s的全1矩阵；

e)采用加权最小二乘估计用户位置信息：

$\tilde{x}{\left({\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\left(\stackrel{\&OverBar;}{H}\right)\right)}^{-1}{\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\stackrel{\&OverBar;}{z}---\left(14\right)$

其中鲁棒估计加权矩阵

有：

$f_i=\left\{\begin{array}{cc}1& \left|{\tilde{w}}_i\right|\&le;k_0\\ \frac{k_0}{\left|{\tilde{w}}_i\right|}\left(\frac{k_1-\left|{\tilde{w}}_i\right|}{k_2-k_0}\right)& k_0<\left|{\tilde{w}}_i\right|\&le;k_1\\ 0& \left|{\tilde{w}}_i\right|>k_1\end{array}\right.---\left(15\right)$

其中，

为第i个视觉/卫星组合导航残差，其中k₀＝2σ_p和k₁＝6σ_p；

f)计算残差矢量：

$\widetilde{\mathrm{\&epsiv;}}=[I-\stackrel{\&OverBar;}{H}{\left({\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}\stackrel{\&OverBar;}{H}\right)}^{-1}{\left(\stackrel{\&OverBar;}{H}\right)}^T\stackrel{\&OverBar;}{W}]\stackrel{\&OverBar;}{z}=S\stackrel{\&OverBar;}{z}---\left(16\right)$

g)通过计算HPL值进行可用性判断：

$\mathrm{HPL}=\underset{i}{\max }\{{\mathrm{Slope}}_i\&times;\sqrt{D_T}\}---\left(17\right)$

其中：Slope_i为第i颗卫星的特征斜率，D_T为该航路对应的RAIM阈值；若HPL大于门限时，即判断为不可用时，输出不可用报警，结束，否者，转至h)

h)利用残差矢量计算检测统计量，获得基于鲁棒估计的加权RAIM的检测统计量：

$D_S={\left|\right|\widetilde{\mathrm{\&epsiv;}}/{\mathrm{\&sigma;}}_p\left|\right|}^2---\left(18\right)$

i)通过给定虚警率p_FA，由下公式计算RAIM阈值：

$D_T=\underset{D_T}{\arg }{p}_{\mathrm{FA}}=\frac{1}{2^{(N_S-1)/2}{\mathrm{\&sigma;}}^{N_S-1}\mathrm{\&Gamma;}((N_S-1)/2)}{\&Integral;}_{D_T}^{\&infin;}{x}^{\frac{N_S-3}{2}}{e}^{-\frac{x}{{2\mathrm{\&sigma;}}^2}}\mathrm{dx}---\left(19\right)$

其中：e自然常数，

为伽马分布，e为自然常数；

j)将检测统计量D_S与RAIM阈值D_T进行比较，如果D_S≥D_T，即视觉辅助RAIM告警，检测到故障，转至步骤四，进行故障检测；否则，说明无卫星故障，不进行告警，转至步骤五；

步骤四：对存在故障星的情况进行故障识别，从而确定并排除故障卫星；

对步骤三中，检测到的故障，通过采用极大似然法进行故障识别与排除，故障排除后，返回步骤三，继续进行故障检测，直到不再出现故障卫星；

步骤五：采用排除过故障卫星后的卫星数据与视觉观测量进行融合，获得最终的组合定位结果。

Images