一种GNSS、INS、Vision组合导航系统完好性保护级计算方法

一种gnss、ins、vision组合导航系统完好性保护级计算方法
技术领域
1.本发明属于gnss、ins、vision组合导航数据处理技术领域，尤其涉及一种gnss、ins、vision组合导航系统完好性保护级计算方法。


背景技术：

2.随着自动驾驶、机器人和无人机等技术的兴起，城市等复杂gnss(globalnavigation satellite system)环境下的高精度定位定姿需求不断增长。gnss/ins(inertialnavigation system)组合导航能够提供连续、可靠和完备的高精度导航参数，是应用最广的组合导航技术之一。然而，在gnss复杂环境下，基于低成本mems
‑
imu(micro
‑
electro mechanical
‑
systems inertial measurement unit)的gnss/ins组合导航在gnss信号中断期间导航误差将迅速积累，这无疑降低了系统的可用性。基于相机的视觉导航具有精度高、自主无源等优点，视觉/ins组合可极大降低ins导航误差。但是，视觉/ins组合无法提供全球框架下的导航信息且误差会随距离累积。因此，将gnss、ins和视觉进行信息融合可实现优势互补，获得更优的导航性能。
3.然而多源组合导航系统的优秀性能是建立在系统的复杂性之上，而系统的复杂性势必增加系统故障发生的概率。因此，系统的完好性监测对于组合导航系统的可靠性至关重要。系统的完好性主要包括两个部分：保护级和故障检测。目前针对多源组合导航系统的故障检测主要是针对当前信息的“快照法”和基于历史信息的“连续法”，理论相对成熟，而gnss、ins、vision组合导航系统的保护级计算却缺乏具体的理论推导，尤其是基于误差状态模型的扩展卡尔曼滤波下gnss/ins松组合和vision/ins紧组合系统。


技术实现要素：

4.本发明提供一种gnss、ins、vision组合导航系统完好性保护级计算方法，旨在解决目前针对多源组合导航系统的故障检测主要是针对当前信息的“快照法”和基于历史信息的“连续法”，理论相对成熟，而多源组合导航系统的保护级计算却缺乏具体的理论推导问题。
5.本发明是这样实现的，一种gnss、ins、vision组合导航系统完好性保护级计算方法，包括如下步骤：
6.步骤s100：建立gnss、ins、vision组合导航系统的误差状态模型和imu的零偏模型；
7.步骤s200：建立gnss和视觉的系统观测模型，并分别计算相应的观测矩阵h；
8.步骤s300：构建不同观测模型下的扩展卡尔曼方程；
9.步骤s400：引入gnss、ins和视觉故障偏差，并计算不同传感器故障模式下的保护级大小。
10.优选的，所述步骤s100中，将位置、速度以及姿态变量的误差量作为组合导航系统的状态量，把imu的零偏建模为一阶马尔可夫模型。
11.优选的，所述步骤s200中，gnss、ins、vision组合导航系统可分为两个子系统，分别为gnss/ins的松组合导航系统和vision/ins紧组合导航系统。
12.优选的，所述gnss/ins松组合导航系统其观测值是gnss的位置信息，观测模型是ins的位置预测值减去gnss的定位结果。
13.优选的，所述vision/ins紧组合导航系统观测值是特征点在当前相机的位置，观测模型是预测的路标点在当前相机的位置减去当前相机实际观测到的特长点的位置。
14.优选的，所述步骤s300中，需要构建不同的卡尔曼方程用于更新不同观测值到达时系统的状态量。
15.与现有技术相比，本发明的有益效果是：本发明的一种gnss、ins、vision组合导航系统完好性保护级计算方法，推导出误差状态模型卡尔曼滤波情况下的组合导航系统保护级公式，进一步考虑到gnss和视觉观测值的故障偏差在实际情况下是未知的，参照lsr算法经典保护级的取值方法，将其放大为最大特征斜率和最小可检测偏差的乘积，便可以得到不同故障模式下系统的保护级。
附图说明
16.图1为本发明的方法步骤示意图；
17.图2为本发明中的gnss、ins、vision组合导航系统完好性监测的整体框架图；
18.图中：gnss:全球导航卫星系统；ins:惯性导航系统；imu:惯性测量单元；pva:位置、速度和姿态；mems:微机电系统；rctm:残差卡方检验法；aime:自主完整性外推法；ekf:扩展卡尔曼滤波；hpl:水平保护级；hal:水平报警限。
具体实施方式
19.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
20.请参阅图1
‑
2，本发明提供一种技术方案：一种gnss、ins、vision组合导航系统完好性保护级计算方法，包括如下步骤：
21.步骤s100：建立gnss、ins、vision组合导航系统的误差状态模型和imu的零偏模型，把位置，速度，姿态等变量的误差量作为组合导航系统的状态量，考虑到imu的漂移，把imu的零偏建模为一阶马尔可夫模型；在确定系统模型的基础上，推导出系统的状态方程，为卡尔曼滤波做准备；
22.步骤s200：建立gnss和视觉的系统观测模型，并分别计算相应的观测矩阵h，对于gnss、ins、vision组合导航系统，可以分为两个子系统，分别是gnss/ins的松组合导航系统和vision/ins紧组合导航系统，对于gnss/ins松组合导航系统，其观测值是gnss的位置信息，观测模型是ins的位置预测值减去gnss的定位结果；对于vision/ins紧组合导航系统，观测值是特征点在当前相机的位置，观测模型是预测的路标点在当前相机的位置减去当前相机实际观测到的特长点的位置，然后，根据前面的系统状态误差方程和系统的观测模型推算出相应的观测矩阵h；
23.步骤s300：构建不同观测模型下的扩展卡尔曼方程，因为gnss/ins的松组合系统
和vision/ins紧组合系统的状态量不同，而且两者的观测模型不同，故相关的参数比如观测矩阵，协方差矩阵等都不一样，为此，需要构建不同的卡尔曼方程用于更新不同观测值到达时系统的状态量；
24.步骤s400：引入gnss、ins和视觉故障偏差，并计算不同传感器故障模式下的保护级大小，无论是gnss/ins松组合系统还是vision/ins紧组合系统，当观测值到达时，扩展卡尔曼的状态更新方程都不是标准的卡尔曼方程。
25.本实施方式中，惯性导航机械编排(ins mechanization)是通过连续的ins导航微分方程推导而得到，地心地固坐标系(e)下的导航微分方程可以表示为：
[0026][0027]
其中，δr
e
,δv
e
,φ分别为载体坐标系原点(imu测量中心)相对于e坐标系且投影到e坐标系下的位置，速度和姿态矢量；为地球自转角速度矢量在e系下的投影；δg
e
当地重力加速度矢量在e系下的投影；是b系相对于i系的旋转角速度在b系得投影；δf
b
是b系下的比力矢量；是b系到e系的旋转矩阵。
[0028]
组合导航算法中常将imu的零偏建模为一阶高斯
‑
马尔科夫过程，模型如下：
[0029][0030]
其中，b
g
,b
a
分别是角速度和加速度的零偏误差；w
g
,w
a
是两者对应的高斯白噪声；
[0031]
则gnss、ins、vision组合导航系统的误差状态方程表达如下：
[0032][0033]
其中，是第i个相机的姿态误差和状态误差；且有：
[0034]
δx
imu
＝[(δr
e
)
t (δv
e
)
t (φ)
t (b
g
)
t (b
a
)
t
]
t
ꢀꢀꢀꢀꢀꢀ
(4)
[0035]
对于gnss、ins、vision组合结构中的gnss/ins松组合导航系统，观测方程是ins的预测位置减去实际的gnss解算位置，其表达如下：
[0036][0037]
系统对应的状态转换矩阵h
k
表达如下：
[0038][0039]
其中，是b坐标系下的杆臂向量。
[0040]
对于gnss、ins、vision组合结构中的vision/ins紧组合导航系统，当一个特征点f
j
被相机c
i
观测到时，f
j
的观测残差在c
i
的投影表达如下：
[0041]
[0042]
其中，是相机的实际观测值；是相机的预测观测值，且的表达如下：
[0043][0044]
其中，是相机的观测噪声向量；and是预测的特征点在相机坐标系下的位置。
[0045]
进一步可得观测残差近似表达如下：
[0046][0047]
其中，和分别是相对于δx
k
和特征点误差求的偏导；和表达如下：
[0048][0049][0050]
有
[0051][0052]
通常，一个静态特征会被多个连续的像机观测到，因此，可以通过将所有的残差叠加在一起来获得该静态特征的完整残差矢量。其表达如下：
[0053][0054]
对于ekf，残差线性化需要满足如下形式，即残差与误差项成线性化关系，且噪声项为与状态向量无关的零均值的高斯分布：
[0055][0056]
当多个特征点被多个相机观测到时，式(14)可以重新表达如下:
[0057]
r
o
＝a
t
h
x
δx
k
+a
t
n＝h
o
δx
k
+n
o
ꢀꢀꢀꢀꢀꢀꢀ
(15)
[0058]
其中，a是半正交矩阵(即a
t
a＝i)，a的列组成了h
f
的左零空间。
[0059]
对于gnss、ins、vision组合结构中的gnss/ins松组合导航系统，假设k历元前imu发生故障，当有gnss观测值时，得到的ekf优化误差状态是：
[0060][0061]
其中，(')表示受故障干扰的变量；k'
k
是受故障影响后的增益矩阵r
k
表达如下：
[0062]
r
k
＝h
k
·
δx
imu
+μ
k
ꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0063]
其中，μ
k
是gnss观测值的高斯白噪声。
[0064]
则imu故障模式下，gnss/ins松组合导航系统的保护级推导步骤如下：
[0065]
步骤一、公式(16)两边同时减去误差状态量δx
imu
，得到(18)：
[0066][0067]
步骤二、公式(18)两边同时乘以h
k
，得到(19)：
[0068][0069]
步骤三、把公式(17)带入公式(19),可得公式(20)
[0070][0071]
步骤四、公式(20)两边同时乘以l
k
，
[0072][0073]
其中，l
k
的表达如下：
[0074][0075]
则公式(21)可以重新写成如下的形式：
[0076][0077]
则imu故障模式的保护级为：
[0078]
pl
q,imu
＝[(k’k
‑
l
k
)r
k
]
q
±
k
md,imu
*σ
q
ꢀꢀꢀꢀꢀ
(24)
[0079]
其中，下标’q’表示的第q个分量，σ
q
表示l
k
u
k
的协方差矩阵第q行对角线元素的平方根，k
md,imu
与gnss/ins松组合系统imu故障情况下的完整性风险要求有关。
[0080]
对于gnss、ins、vision组合结构中的gnss/ins松组合导航系统，假设k历元前gnss发生故障，则r
k
可以重新表达如下：
[0081]
r
k
＝h
k
·
δx
imu
+μ
k
+f
k,gnss
ꢀꢀꢀꢀꢀꢀꢀ
(25)
[0082]
其中，f
k,gnss
是gnss观测值的偏差向量。
[0083]
可以得到：
[0084][0085]
则gnss故障模式的保护级为：
[0086]
pl
q,gnss
＝[(k’k
‑
l
k
)r
k
]
q
+k
md,gnss
*σ
q
+[l
k
f
k,gnss
]
q
ꢀꢀ
(27)
[0087]
其中，k
md,gnss
与gnss/ins松组合系统gnss故障情况下的完整性风险要求有关。
[0088]
考虑到式(27)中包含f
k,gnss
且未知，此处参考lsr算法经典保护级的取值方法，将l
k
f
k,gnss
放大为最大特征斜率max(slope
mq
)与最小可检测偏差的乘积。其中特征斜率
slope
mq
根据f
k,gnss
对gnss故障检测检验统计量t
s
和滤波估计的影响进行定义；对于给定的漏检概率，可以从测试统计信息的阈值计算
[0089]
f
k,gnss
在滤波估计第q维引起的误差为：
[0090]
μ
q
＝(l
k
)
qm
ξ
b
＝l
qm
ξ
b
ꢀꢀꢀꢀꢀꢀꢀꢀ
(28)
[0091]
其中，l
qm
代表l
k
第q行第m列的元素；且有f
k,gnss
＝[ξ
b ξ
b ξ
b
]
t
.
[0092]
而gnss故障检测检验统计量t
s
表达如下：
[0093]
t
s
＝r
kt
·
s
k
‑1·
r
k
ꢀꢀꢀꢀꢀꢀ
(29)
[0094]
f
k,gnss
对检测统计量t
s
的影响表现在非中心化参数λ上面，λ的表达如下：
[0095]
λ＝ξ
b2
s
_mm
ꢀꢀꢀꢀꢀ
(30)
[0096]
其中，s
_mm
是矩阵s
k
‑1＝(h
k
·
p
k,k
‑1·
h
kt
+r
k
)
‑1的第m行对角线元素，则slope
mq
的定义如下：
[0097][0098]
则gnss故障模式的保护级重新表达为：
[0099][0100]
对于gnss、ins、vision组合结构中的vision/ins紧组合导航系统，假设k历元前相机发生故障偏差f
x,visual
，当有视觉观测值时，得到的ekf优化误差状态是：
[0101][0102]
则r
o
可以重新表达如下：
[0103]
r
o
＝h
o
δx
k
+n
o
+f
o,visual
ꢀꢀꢀꢀꢀꢀ
(33)
[0104]
参考gnss故障，可以得到：
[0105][0106]
其中，f
o,visual
＝a
t
f
x,visual
,且l
k,o
的表达如下：
[0107][0108]
考虑到求逆时可能会出现问题，且根据式(15)可得：
[0109][0110]
因此，l
k,o
可以重新表达如下：
[0111][0112]
式(34)重新写成：
[0113][0114]
其中
[0115]
[0116]
则视觉故障模式下，vision/ins紧组合系统的保护级表达为：
[0117][0118]
σ
q,o
表示b
k,o
n的协方差矩阵第q行对角线元素的平方根；k
md,cam
与vision/ins紧组合系统相机故障情况下的完整性风险要求有关；对于给定的漏检概率，可以从测试统计信息的阈值计算slope
mq,o
的计算如下：
[0119][0120]
其中，b
qm,o
表示矩阵b
k,o
的q行，m列对应的元素；是第m个路标点观测噪声的标准差。
[0121]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。