一种复杂环境下的多源组合导航定位方法

in urban areas.)，提高了相机重叠区域的探测性能，但由于视觉定位的缺陷，气象环境较差的地区无法使用。
4.根据现有文献分析，单纯的gnss/imu组合具有gnss不可用误差积累过快的缺点；gnss和视觉进行组合具有计算负荷较高，且气象条件较差的情况下无法使用，可靠性较差的缺点。利用obd获取速度更加准确，能够对gnss/imu在行径方向上的误差进行约束；气压高度计能够全天候使用，在垂直方向的可靠性比gnss更高。为了结合城市环境使用特征并考虑到成本因素，本专利提出了一种城市环境下的gnss/imu/obd/高度计组合导航方法。
5.和本发明比较接近的技术是利用通过视景系统结合3d城市模型对城市街区进行识别的方法进行定位检测，但这一种方法由于在城市中地面干扰较多，还有系统的可靠性不高，较容易到其他载体、行人以及其他障碍物等的影响，可操作性及实用性意义不大。


技术实现要素：

6.发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种复杂环境下的多源组合导航定位方法。
7.为了解决上述技术问题，本发明公开了一种复杂环境下的多源组合导航定位方法，包括以下步骤：
8.步骤1，全球导航卫星系统及多传感器测量观测，所述多传感器包括全球导航卫星系统接收机、惯性导航系统、在线诊断系统和气压高度计；
9.步骤2，去除卫星信号载噪比低于阈值的卫星；
10.步骤3，多普勒频移测速与气压高度解算，得到高度信息；
11.步骤4，速度卡尔曼滤波，获取在线诊断系统的速度、力学编排以及多普勒频移速度，并输出速度估计，得到姿态和速度信息；
12.步骤5，利用hatch滤波获取载波平滑伪距并进行位置解算，得到位置信息；
13.步骤6，将步骤5中得到的位置信息，步骤3中得到的高度信息，步骤4中得到的姿态和速度信息输入无迹卡尔曼滤波，对力学编排进行误差反馈，最终获取载体的位置和速度信息，完成城市环境下的组合导航定位。
14.本发明中，步骤1包括：
15.步骤1-1，在城市环境下，通过全球导航卫星系统载体接收机u与参考站接收机r获取第k颗卫星的多普勒频移测量为和获取载波相位测量为和获取伪距观测量为和获取当前的卫星集合即卫星数量，以及卫星信号载噪比；
16.步骤1-2，计算载波相位测量单差：
[0017][0018]
计算载波相位测量双差：
[0019][0020]
计算载波相位测量三差：
[0021][0022]
计算伪距测量单差：
[0023][0024]
计算伪距测量双差：
[0025][0026]
计算多普勒频移单差：
[0027][0028]
计算多普勒频移双差：
[0029][0030]
其中，δ为载体u与参考站r间的测量进行差分，为针对不同卫星k,l之间的测量进行差分；
[0031]
步骤1-3，通过气压高度计获取环境气压p(t)和温度t0(t)，通过载体在线诊断系统获取载体相对于载体坐标系的速度惯性测量元件获取载体在北、东和地方向的比力[f
n f
e fd]与角速度。
[0032]
本发明中，步骤2包括：
[0033]
根据历史观测数据确定卫星信号载噪比阈值β，将所有卫星的卫星信号载噪比与阈值β进行比较，所有卫星信号载噪比大于β的卫星保存在a1集合，剔除所有卫星信号载噪比低于β的卫星。
[0034]
本发明中，步骤3包括：
[0035]
步骤3-1，多普勒频移测速，方法如下：
[0036][0037]
本发明中，参数上方的“·”代表针对时间t求导，“^”代表后验估计，
“‑”
代表先验估计，“～”代表测量值；为t时刻载体u至第k颗卫星的矢量，为t时刻载体u至第k颗卫星以及第l颗卫星的矢量之差，为针对时间t求导，为导航坐标系至ecef的转换矩阵，为载体u在导航坐标系下的速度估计，为载体u相对参考站r的位置后验估计，为多普勒三差测量值，εd(t)为多普勒频移噪声，
▽
δεd(t)为多普勒频移噪声三差，xu(t)为载体位置；步骤3-2，利用国际标准大气模型计算气压高度，方法如下：
[0038][0039]
h(t)＝a(t) n(t) εh(t)
[0040]
其中，a(t)为气压高度，t0(t)为当地海平面的绝对温度，p0(t)为当地海平面的气压，p(t)为实测气压，γ、r和g为相应的物理常数，εa(t)为气压高度计误差，n(t)为大地水准面高，h(t)为椭球高度，εh(t)为椭球高度误差。
[0041]
本发明中，步骤4包括：
[0042]
步骤4-1，构建状态向量x如下：
[0043][0044]
其中，为相对于导航坐标系的速度误差，δvn、δve和δvd分别为北向、东向和地向的速度误差；φ＝[φ
n φ
e φd]为使用欧拉角表示的姿态误差，φn、φe和φd分别为北向、东向和地向的姿态误差；为加速度计零偏，和为载体坐标系下各方向的零偏；ξ＝[ξ
x ξ
y ξz]为陀螺仪漂移，ξ
x
、ξy和ξz为载体坐标系下各方向的陀螺仪漂移；为在线诊断系统偏移；
[0045]
步骤4-2，构建惯性测量元件力学编排，方法如下：
[0046][0047][0048][0049][0050][0051]
[0052][0053]
其中，ω为惯性测量元件测量噪声，rm和r
t
分别为地球子午线以及赤道曲率半径，h为椭球高度，l为纬度；[ω
n 0 ωd]为相对于导航坐标系北、东和地各方向的地球转动角速率；[ρ
n ρ
e ρd]分别为北、东和地方向的转移角速率，[f
n f
e fd]分别为北、东和地方向的比力，为载体坐标系至导航坐标系的转移矩阵；
[0054]
步骤4-3，测量误差模型，方法如下：
[0055]
多普勒频移测速结果为：
[0056][0057]
由obd速度测量获取的观测量z
obd
为：
[0058][0059]
其中，为载体在导航坐标系中的速度，为载体至导航坐标系的后验估计，测量更新方程为：
[0060]
z(t)＝h(t)x(t) εz(t)
[0061][0062][0063][0064]
其中，r
obd
、rd分别为在线诊断系统和多普勒频移的噪声方差，εz(t)为测量误差；
[0065]
步骤4-4，速度估计输出：
[0066]
判定伪距与载波相位测量是否可用，当可用卫星数量无法用于定位时，利用速度
卡尔曼滤波的速度估计计算的替代测量ω
kf
(t)对载波相位间接测量ω
φ
(t)进行替代更新：
[0067][0068][0069]
其中，xu为载体位置，为t时刻用户位置对时间求导，xr为参考站位置，为载波相位测量三差；通过上述替代更新，对hatch滤波的误差积累进行控制，最终输出速度估计以及替代测量ω
kf
(t)。
[0070]
本发明中，步骤5包括：
[0071]
步骤5-1，增量状态更新：
[0072]
计算伪距间接测量：
[0073][0074]
其中为载波相位三差的测量对时间t的导数，为载波相位测量噪声三差。
[0075]
速度估计的间接测量，由速度卡尔曼滤波获取：
[0076][0077][0078]
其中，为导航坐标系至地心地固坐标系的转换矩阵，l、l分别为纬度和经度；
[0079]
间接测量方程：
[0080][0081]
其中，
[0082]
为增量状态向量估计，通过进行更新，其中为状态
向量的先验估计，为状态向量的后验估计；
[0083]
步骤5-2，测量更新：
[0084]
伪距间接测量为：
[0085][0086][0087]
其中为伪距观测量的测量值，为载体相对参考站的位置先验估计，间接测量更新为：
[0088][0089]
其中，
[0090][0091][0092]
为构成的列向量。
[0093]
将先验估计更新至后验估计：
[0094][0095]
其中，z(t)为hatch滤波增益；
[0096]
步骤5-3，输出载波平滑伪距解算后得到的位置xu(t)。
[0097]
本发明中，步骤6包括：将载波平滑伪距解算的位置xu(t)，气压高度计解算的高度a(t)，力学编排获取的姿态φ、速度输入无迹卡尔曼滤波，对力学编排进行误差反馈，最终获取载体的位置和速度信息，完成城市环境下的组合导航定位。
[0098]
本发明中，步骤3-2所述物理常数为：温度直减率γ＝-0.00649k/m、摩尔气体常数r＝287.05j
·
kg
·
k-1
和重力常数g＝9.80665m/s2。
[0099]
本发明步骤2中所述卫星信号载噪比阈值β设定为40。
[0100]
本发明中，步骤6所述获取载体的位置方法为：
[0101]
xu(t 1)＝xu(t) δxu(t)
[0102]
获取速度信息方法为：
[0103]vu
(t 1)＝vu(t) δvu(t)。
[0104][0105]
有益效果：
[0106]
1、现有多传感器融合中常用的视觉定位，在气象条件较差的情况下不可用，无法实现全天候的可靠导航定位。本发明通过使用全天候传感器：气压高度计、obd、imu来提高组合导航系统的可靠性。
[0107]
2、现有的组合导航方法在城市环境下的垂直精度较低。本发明中，多个载体在同一气压基准面下利用气压高度计获取的高度能够很好的建立垂直间隔，这对城市立体环境下的载体运行，尤其是无人驾驶航空器的运行非常有效，可以显著提高垂直定位的精度和可靠性。
[0108]
3、传统组合导航方法在gnss不可用时，载体位置信息的估计精度会显著下降。本发明利用速度卡尔曼滤波获取载体速度估计，在gnss信号不可用时作为载波平滑伪距的替代测量，保证了载波平滑伪距的连续性，降低了计算复杂度，最终提高了gnss不可用时的组合导航定位的精度。
附图说明
[0109]
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
[0110]
图1为本发明流程示意图。
具体实施方式
[0111]
一种复杂环境下的多源组合导航定位方法，如图1所示，首先，获取gnss接收机以及多传感器的测量信息，包括gnss提供的多普勒频移、伪距、载波相位测量；imu获取的比力、角速度；obd系统提供的载体速度；气压高度计获取的气压高度。其次，对imu进行力学编排；对gnss测量进行故障检测提取可用信号。接着，将多普勒频移测得的速度、obd获取的载体速度、力学编排获取的imu速度、姿态、位置信息输入卡尔曼滤波，获取载体的速度估计；判断载波相位测量、伪距测量是否可用，可用时输入hatch滤波，不可用时利用卡尔曼滤波的速度估计输入hatch滤波进行更新，并输出载波平滑伪距测量；利用获取的大地基准面高度、环境温度、气压高度计的气压高度解算椭球高度。然后，利用载波平滑伪距解算载体位置，将该位置、多普勒频移测量的速度、椭球高度、力学编排获取的速度、姿态、位置信息输入无迹卡尔曼滤波，对力学编排进行反馈纠正获取该组合导航方法解算的载体速度、姿态和位置。具体如下：
[0112]
步骤1，全球导航卫星系统及多传感器测量观测，所述多传感器包括全球导航卫星系统接收机、惯性导航系统、在线诊断系统和气压高度计；
[0113]
步骤1-1，在城市环境下，通过全球导航卫星系统载体接收机u与参考站接收机r获取第k颗卫星的多普勒频移测量为和获取载波相位测量为和获取伪距观测量为和获取当前的卫星集合即卫星数量，以及卫星信号载噪比；
[0114]
步骤1-2，计算载波相位测量单差：
[0115][0116]
计算载波相位测量双差：
[0117][0118]
计算载波相位测量三差：
[0119][0120]
计算伪距测量单差：
[0121][0122]
计算伪距测量双差：
[0123][0124]
计算多普勒频移单差：
[0125][0126]
计算多普勒频移双差：
[0127][0128]
其中，δ为载体u与参考站r间的测量进行差分，为针对不同卫星k,l之间的测量进行差分；
[0129]
步骤1-3，通过气压高度计获取环境气压p(t)和温度t0(t)，通过载体在线诊断系统获取载体相对于载体坐标系的速度惯性测量元件获取载体在北、东和地方向的比力[f
n f
e fd]与角速度。
[0130]
步骤2，去除卫星信号载噪比低于阈值的卫星；根据历史观测数据确定卫星信号载噪比阈值β，将所有卫星的卫星信号载噪比与阈值β进行比较，所有卫星信号载噪比大于β的卫星保存在a1集合，剔除所有卫星信号载噪比低于β的卫星。其中所述卫星信号载噪比阈值β设定为40。
[0131]
步骤3，多普勒频移测速与气压高度解算，得到高度信息；
[0132]
步骤3-1，多普勒频移测速，方法如下：
[0133][0134]
全文中，参数上方的“·”代表针对时间t求导，“^”代表后验估计，
“‑”
代表先验估计，“～”代表测量值；为t时刻载体u至第k颗卫星的矢量，为t时刻载体u至第k颗卫星以及第l颗卫星的矢量之差，为针对时间t求导，为导航坐标系至ecef的转换矩阵，为载体u在导航坐标系下的速度估计，为载体u相对参考站r的位置后验估计，为多普勒三差测量值，εd(t)为多普勒频移噪声，为多普勒频移噪声
三差，xu(t)为载体位置；
[0135]
步骤3-2，利用国际标准大气模型计算气压高度，方法如下：
[0136][0137]
h(t)＝a(t) n(t) εh(t)
[0138]
其中，a(t)为气压高度，t0(t)为当地海平面的绝对温度，p0(t)为当地海平面的气压，p(t)为实测气压，γ、r和g为相应的物理常数，εa(t)为气压高度计误差，n(t)为大地水准面高，h(t)为椭球高度，εh(t)为椭球高度误差。
[0139]
其中，温度直减率γ＝-0.00649k/m、摩尔气体常数r＝287.05j
·
kg
·
k-1
和重力常数g＝9.80665m/s2。
[0140]
步骤4，速度卡尔曼滤波，获取在线诊断系统的速度、力学编排以及多普勒频移速度，并输出速度估计，得到姿态和速度信息；
[0141]
步骤4-1，构建状态向量x如下：
[0142][0143]
其中，为相对于导航坐标系的速度误差，δvn、δve和δvd分别为北向、东向和地向的速度误差；φ＝[φ
n φ
e φd]为使用欧拉角表示的姿态误差，φn、φe和φd分别为北向、东向和地向的姿态误差；为加速度计零偏，和为载体坐标系下各方向的零偏；ξ＝[ξ
x ξ
y ξz]为陀螺仪漂移，ξ
x
、ξy和ξz为载体坐标系下各方向的陀螺仪漂移；为在线诊断系统偏移；
[0144]
步骤4-2，构建惯性测量元件力学编排，方法如下：
[0145][0146][0147]
[0148][0149][0150][0151][0152]
其中，ω为惯性测量元件测量噪声，rm和r
t
分别为地球子午线以及赤道曲率半径，h为椭球高度，l为纬度；[ω
n 0 ωd]为相对于导航坐标系北、东和地各方向的地球转动角速率；[ρ
n ρ
e ρd]分别为北、东和地方向的转移角速率，[f
n f
e fd]分别为北、东和地方向的比力，为载体坐标系至导航坐标系的转移矩阵；
[0153]
步骤4-3，测量误差模型，方法如下：
[0154]
多普勒频移测速结果为：
[0155][0156]
obd速度测量为：
[0157][0158]
其中，为载体在导航坐标系中的速度，为载体至导航坐标系的后验估计，测量更新方程为：
[0159]
z(t)＝h(t)x(t) εz(t)
[0160]
[0161][0162][0163]
其中，r
obd
、rd分别为在线诊断系统和多普勒频移的噪声方差，εz(t)为测量误差；
[0164]
步骤4-4，速度估计输出：
[0165]
判定伪距与载波相位测量是否可用，当可用卫星数量无法用于定位时，利用速度卡尔曼滤波的速度估计计算的替代测量ω
kf
(t)对载波相位间接测量ω
φ
(t)进行替代更新：
[0166][0167][0168]
其中，xu为载体位置，为t时刻用户位置对时间求导，xr为参考站位置，为载波相位测量三差；通过上述替代更新，对hatch滤波的误差积累进行控制，最终输出速度估计以及替代测量ω
kf
(t)。
[0169]
步骤5，利用hatch滤波获取载波平滑伪距并进行位置解算，得到位置信息；
[0170]
步骤5-1，增量状态更新：
[0171]
计算伪距间接测量：
[0172][0173]
其中为载波相位三差的测量对时间t的导数，为载波相位测量噪声三差。
[0174]
速度估计的间接测量，由速度卡尔曼滤波获取：
[0175]
[0176][0177]
其中，为导航坐标系至地心地固坐标系的转换矩阵，l、l分别为纬度和经度；
[0178]
间接测量方程：
[0179][0180]
其中，
[0181]
为增量状态向量估计，通过进行更新，其中为状态向量的先验估计，为状态向量的后验估计；
[0182]
步骤5-2，测量更新：
[0183]
伪距间接测量为：
[0184][0185]
其中为伪距观测量的测量值，为载体相对参考站的位置先验估计，间接测量更新为：
[0186][0187]
其中，其中，
[0188]
为构成的列向量。
[0189]
将先验估计更新至后验估计：
[0190][0191]
其中，z(t)为hatch滤波增益；
[0192]
步骤5-3，输出载波平滑伪距解算后得到的位置xu(t)。
[0193]
步骤6，将步骤5中得到的位置信息，步骤3中得到的高度信息，步骤4中得到的姿态和速度信息输入无迹卡尔曼滤波，对力学编排进行误差反馈，最终获取载体的位置和速度信息，完成城市环境下的组合导航定位，具体包括：将载波平滑伪距解算的位置xu(t)，气压高度计解算的高度a(t)，力学编排获取的姿态φ、速度输入无迹卡尔曼滤波，对力学编排进行误差反馈，最终获取载体的位置和速度信息，完成城市环境下的组合导航定位。
[0194]
所述获取载体的位置方法为：
[0195]
xu(t 1)＝xu(t) δxu(t)
[0196]
获取速度信息方法为：
[0197]vu
(t 1)＝vu(t) δvu(t)。
[0198][0199]
本发明提供了一种复杂环境下的多源组合导航定位方法的思路及方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。