一种基于因子图及场景约束的多径环境组合导航方法

1.本发明涉及一种多径环境组合导航方法，特别是一种基于因子图及场景约束的多径环境组合导航方法。


背景技术：

2.全球导航卫星系统(global navigation satellite system，gnss)由于具有全球覆盖、定位精度较高、自动化程度高、实时服务能力强等优点，现已广泛应用于交通、军事、农业等领域。在近年来迅速发展的无人机和自动驾驶汽车领域，gnss以及其组合导航系统也已成为了用于提供载具位置的重要手段。考虑到成本、可靠性以及应用的推广，目前大部分车载gnss接收机采用伪距观测进行定位。然而，城市高大建筑物和复杂楼宇环境会造成gnss产生多径干扰及非视距接收，两者都会导致gnss以及其组合导航定位精度下降。其中非视距接收更是会导致伪距定位产生近百米的误差。
3.gnss多源信息融合可以提高载体的导航性能，例如将gnss和惯性导航系统、激光雷达、相机以及3d地图等信息源进行组合。在传统的gnss多传感器数据融合中，通常采用最小二乘法和卡尔曼滤波法。近年来，基于因子图的多源数据融合方法也逐步成为研究热点。因子图通过使用多次迭代来解决所有的输入状态，实现观测量过去时间节点与当前时间节点之间的时间相关性挖掘，提高滤波的鲁棒性。
4.sunderhauf等提出了一种仅使用gps伪距观测值的基于因子图优化的稳健优化方法(参考：n.s
ü
nderhauf,m.obst,g.wanielik,and p.protzel,"multipath mitigation in gnss-based localization using robust optimization,"in 2012ieee intelligent vehicles symposium,2012,pp.784-789:ieee.)，通过实验证明该方法的定位精度较传统最小二乘定位方法有提高。w.li等提出了基于因子图优化方法的gnss/ins组合导航定位方法(参考：w.li,x.cui,and m.lu,"a robust graph optimization realization of tightly coupled gnss/ins integrated navigation system for urban vehicles,"tsinghua science and technology,vol.23,no.6,pp.724-732,2018.)，验证了因子图优化方法在多传感器融合定位方面具有更优的性能。suzuki提出了一种基于因子图优化的多径环境载具定位方法(参考：suzuki,taro,"robust vehicle positioning in multipath environments based on graph optimization,"proceedings of the 34th international technical meeting of the satellite division of the institute of navigation(ion gnss+2021),st.louis,missouri,september 2021,pp.4223-4233.)，通过将gnss伪距观测值以及多普勒观测值作为因子图的输入节点并通过运动约束对节点进行控制和优化，同时使用载波相位观测值对远距节点间进行精确约束，输出最终的最优定位结果。基于因子图优化的融合定位方法在提升组合导航定位精度方面具有很好的优势，但在城市复杂环境下仍存在主要以下2个问题：
5.a)现有基于因子图的gnss组合导航方法只对gnss的时序观测量上进行了一定的约束，在高架桥、隧道等遮蔽环境下由于卫星信号中断或长时间失锁环境下，依然无法提供
准确可靠的定位；
6.b)现有的基于因子图的gnss组合导航定位算法在使用前需要进行较为复杂的初始化处理、同时在使用过程中需要对算法进行实时匹配，显著地降低了定位解算的实时性，无法实现城市环境中的快速精确定位。
7.和本发明最为接近的技术是日本千叶大学学者taro suzuki提出的基于因子图优化的多径环境载具定位方法，该方法将gnss伪距观测值以及多普勒观测值作为因子图的输入节点，通过运动约束对节点控制和优化，同时使用载波相位观测值对远距节点间进行约束，最终输出最优定位解。但这种通过运动模型和载波相位观测值构建约束的方法存在过于粗略的问题：一是由于直接输入未进行质量控制的gnss观测值，定位精度可能会因受到故障观测值的影响而降低；二是此方法使用定位精度更高的载波相位观测值对远距节点状态之间进行粗略约束，例如在载具驶入以及驶出高架两个状态节点位置进行约束，但这种约束在载具处于高架桥路段行驶的整个过程中无法进行，因此该方法本质上无法完全满足城市复杂观测环境中的精确定位问题。
8.其他能够实现本发明目的更加通用的替代方案是使用最小二乘法进行gnss定位。但是这种基于最小二乘的定位算法在城市环境中存在较大的限制：一是由于接收机所接收到的卫星信号会受到严重的多路径效应的影响，使用这类观测值进行最小二乘解算所得出的定位结果必然会产生相当大的误差；二是最小二乘定位方法在多传感器融合时的性能不及图优化方法。


技术实现要素：

9.发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于因子图及场景约束的多径环境组合导航方法。
10.为了解决上述技术问题，本发明公开了一种基于因子图及场景约束的多径环境组合导航方法，包括以下步骤：
11.离线处理部分：
12.步骤1，构建地图影像样本训练集：选取地图影像进行等大切分，初始选取b张训练样本图像，构建地图影像样本训练集q；
13.步骤2，利用步骤1中所述地图影像样本训练集进行卷积神经网络训练，构建道路图像提取模型；
14.步骤3，根据初始位置获取本地城市道路区域图；
15.在线计算部分：
16.步骤4，载具获取全球导航卫星系统伪距与多普勒观测值；
17.步骤5，排除卫星信号信噪比低于阈值的卫星：根据历史数据经验设定卫星信号信噪比的阈值为snr
thre
，对步骤4中获得的每一颗卫星的信噪比进行条件判断，当其信噪比大于阈值时，保留卫星的相关观测数据，否则删除卫星的相关观测数据；
18.步骤6，判断剩余卫星是否满足定位条件，若不满足则采用历史数据估计方法推算当前位置，若满足则对原始伪距观测值数据进行误差修正处理；
19.步骤7，判断剩余卫星是否有冗余，若存在冗余卫星，则对冗余卫星观测值采用欧式距离矩阵法进行卫星故障的快速检测，否则保留当前卫星观测值；
20.步骤8，根据当前位置基准点提取当前区域道路形状范围：将上一历元的定位结果作为本历元道路形状提取的中心点，根据步骤3中获得的城市道路区域图在x轴方向和y轴方向分别截取一定范围，得到当前区域的道路区域图；
21.步骤9，计算伪距因子和多普勒因子，并构造接收机位置坐标与道路区域之间的差值参数；
22.步骤10，根据场景约束进行因子图优化定位；
23.步骤11，输出定位结果。
24.本发明步骤2包括：
25.步骤2-1，划分测试集：
26.使用自主采样法划分训练集中的部分数据作为测试集，用于在卷积神经网络训练时实时验证网络的学习性能；
27.所述测试集构建方法如下：
28.每次从样本集q中随机挑选一个样本拷贝至数据集k中，然后放回至原本的样本集q以供下次采样；
29.重复上述过程b次后得到包含b个样本的数据集k，其中包含重复的样本，而q中的一部分样本未被采集到；
30.得到的未被采集样本比率为：
[0031][0032]
其中，e为自然常数；选取样本集q和数据集k中的差异部分作为训练时的实时测试集；
[0033]
步骤2-2，网络训练：
[0034]
将地图中的路网矢量数据与样本图像进行匹配，作为卷积神经网络网络训练的输入二值图像，进行前向传播和反向传播调整卷积核权重值，最终生成道路提取结果，训练结束。
[0035]
本发明步骤3包括：
[0036]
步骤3-1，网络输出：
[0037]
对于给定的初始化位置，选定该位置所处的城市地图进行等大划分，得到h张本地地图图像，构建本地样本集l；输入本地样本进行卷积神经网络输出，得到h张初始道路提取图像；
[0038]
步骤3-2，图像处理：
[0039]
初始提取的道路图像存在道路边缘波折与非道路图斑，根据形态学运算滤除非道路区域图斑，使用面积指数对图像进行面积筛选，根据经验得到面积指数阈值为a
thre
，若面积指数a＜a
thre
，则去除该图斑；
[0040]
利用道路狭长指数对噪声进行进一步的去除，道路狭长指数为：
[0041][0042]
其中，l
p
为道路图块外接矩形的对角线长度，w
p
为宽度，s
p
为道路面积；设定狭长指数阈值为r
thre
＝2，狭长指数低于阈值时判定图像块为非道路区域，去除此部分噪声区域；
[0043]
根据道格拉斯-普克算法对道路边缘进行直线逼近，得到直线化处理后的道路图块，该算法实现过程如下：
[0044]
对曲线的首末点连一条直线，求出曲线上所有的点与直线的垂直距离di，其中i＝1,2,
…
,μ，μ为曲线上点的个数，并找出最大值将d
max
与给定的阈值d
thre
相比，若d
max
＜d
thre
，则将这条曲线上的中间点全部舍去，以初始划定的直线作为曲线的近似；若d
max
≥d
thre
，则保留d
max
对应的点，并以该点为界将曲线分为两部分，对每一段曲线都重复使用上述逼近方法进行曲线逼近，直到所有的d
max
均小于d
thre
，完成对波折道路边缘的直线化处理；
[0045]
步骤3-3，图像合成：
[0046]
将生成的道路提取图像按原始切割方法拼接，得到完整的基于深度学习构建的城市道路区域图，并根据原始地图影像的经纬度坐标对生成的城市道路区域图进行坐标匹配，给道路区域图进行矢量赋值；将经纬度坐标转换为地心地固坐标系下的坐标，设经纬度坐标向量为：
[0047]
p
llh
＝[lat lon alt]
t
[0048]
其中，lat为纬度，lon表示经度，alt为高度；在地心地固坐标系中的坐标向量为：
[0049]
p
ecef
＝[x y z]
t
[0050]
其中，x、y、z分别为地心地固坐标系中的x轴、y轴、z轴坐标；坐标转换公式为：
[0051][0052]
其中，地球的极扁率和分别为地球的长轴和短轴，n的计算方法为：
[0053][0054]
步骤3-4，样本训练集更新：
[0055]
将本地样本集l加入原始地图影像样本训练集q，更新得到新的训练集q，其中包含b+h张样本图像，用于卷积神经网络的下一次定期优化更新；得到基于深度学习构建的城市道路区域图；
[0056]
设定定期更新模式，设置更新周期为7天，每7天利用更新后的训练集重新进行卷积神经网络训练，更新周期内使用无需进行网络训练，直接根据输入的初始位置得出网络输出的图像结果。
[0057]
本发明步骤4包括：
[0058]
在城市多径环境下，载具获得当前历元k的伪距观测数据以及多普勒观测数据，其中包括：可见星数量可见星在地心地固坐标系中的坐标卫星信
号信噪比每一颗可见星所对应的原始伪距观测值以及原始多普勒观测值其中，自然数
[0059]
本发明步骤6包括：
[0060]
步骤6-1，根据步骤5所述，根据信噪比低于阈值排除观测质量不佳的卫星后，剩余卫星的数量为所属星座数为对剩余卫星数量进行判断，若则不满足全球导航卫星系统的定位条件，采用历史数据估计方法推算当前位置；通过历史历元数据推算当前历元位置变化量；读取k-1历元和k-2历元的位置信息，k-1历元的位置为x
k-1
，k-2历元的位置为x
k-2
，历元间位置变化量δx＝x
k-1-x
k-2
，推算得出当前历元的位置xk＝2x
k-1-x
k-2
，当前历元定位结束，直接输出定位结果；
[0061]
步骤6-2，若则满足卫星定位条件，对原始伪距观测值数据进行误差修正处理，电离层误差修正量为对流层误差修正量为得到进行了电离层延迟和对流层延迟改正后的伪距观测值
[0062]
本发明步骤7包括：
[0063]
步骤7-1，若则不存在冗余卫星观测值，无法进行卫星定位故障检测与排除，卫星数保留当前全球导航卫星系统观测值，设定
[0064]
步骤7-2，若则存在冗余卫星观测值，采用欧式距离矩阵法进行卫星故障的快速检测；利用已知的卫星位置与载具获得的伪距观测值构建欧氏距离矩阵其中矩阵的第一行和第一列由测量伪距构成，对角线元素为0，其他元素由已知的卫星位置填充；
[0065]
欧式距离矩阵的构成如下：
[0066]
三颗卫星与接收机的相对位置关系如下：对角线上的矩阵元素a
11
、a
22
、a
33
、a
44
均为0，第一行的矩阵元素a
1p
与第一列的矩阵元素a
q1
为卫星与接收机间的测量伪距平方和其余位置a
pq
为两颗卫星之间的距离根据欧氏距离矩阵求得对应的gram矩阵
[0067][0068]
其中，矩阵j为几何中心矩阵，i为单位矩阵，1表示1的列向量；gram矩阵的秩等于状态空间的维数n，即利用该式对故障卫星进行检测；
[0069]
对gram矩阵进行奇异值分解得：
[0070]
[0071]
其中u为左侧奇异向量，v为右侧奇异向量，σ为对角矩阵，包含的奇异值σ1,σ2,
…
,σ
1+m
，根据矩阵的奇异值判断检验统计量是否超过检测门限：
[0072][0073]
若检验统计量超过检测门限，则表明存在故障卫星，需要进行故障卫星的检测和排除；三维空间中状态空间n＝3，令u矩阵的第n+1列为u4，v矩阵的第n+1列为v4，按绝对值大小对u4和v4中的元素进行排序，最大的绝对值对应故障卫星；排除故障卫星后重新计算检验统计量并判断是否超过检测门限，若超过检测门限，重新进行排序和故障卫星排除，直到低于检测门限，则认为不存在故障卫星；
[0074]
进行故障排除后剩余可用卫星数量为对应的卫星在地心地固坐标系中的坐标为伪距观测值以及多普勒观测值
[0075]
本发明步骤8包括：
[0076]
将上一历元的定位结果x
k-1
＝[x
k-1
,y
k-1
,z
k-1
]
t
作为本历元道路形状提取的中心点，根据步骤3中获得的城市道路区域图以x
k-1
为中心，在x轴方向和y轴方向分别截取
±
10m的范围，得到当前区域的道路区域图；
[0077]
使用滑动图窗裁取本历元所需参考的道路区域图，设置图窗范围为：m＝{x,y|x
k-1-10m＜x＜x
k-1
+10m,y
k-1-10m＜y＜y
k-1
+10m}其中道路区域i∈m。
[0078]
本发明步骤9包括：
[0079]
步骤9-1，计算伪距因子：
[0080]
根据进行了误差修正处理后的得到误差修正后的伪距测量方程：
[0081][0082]
其中表示在k历元卫星与接收机之间的几何距离，c为光速，δk和分别表示接收机钟差和卫星钟差，表示多路径效应等其他测量噪声所造成的误差；将伪距观测方程写成参数方程形式：
[0083][0084][0085]
其中，xk＝[xk,yk,zk]
t
为k历元接收机的位置；根据上述方程得到卫星j在k历元的伪距误差函数，定义为伪距因子：
[0086][0087]
其中为由卫星高度角与信噪比构成的协方差矩阵；
[0088]
步骤9-2，计算多普勒因子：
[0089]
多普勒测量方程表示为：
[0090][0091]
其中λ为卫星信号的载波波长，和分别为接收机和卫星钟的钟漂，为电离层误差变化率，为对流层误差变化率，为其它误差变化率，伪距率为：
[0092][0093]
为卫星指向接收机的单位向量，为已知的卫星j在k历元的速度，为接收机的运动速度，根据最小二乘法得到当前历元的速度状态方程：
[0094][0095][0096]
其中为多普勒求解中k历元的其他噪声，为k历元接收机的位置，x
k-1
＝[x
k-1
,y
k-1
,z
k-1
]
t
为k-1历元的接收机位置，δt为采样间隔；
[0097]
根据上述方程得到k历元的速度误差函数，定义为多普勒因子：
[0098][0099]
其中为协方差矩阵；
[0100]
步骤9-3，计算道路形状因子：
[0101]
步骤8中得到的道路区域矢量集为i，接收机位置坐标与道路区域之间的差值参数为：
[0102][0103]
其中xk＝[xk,yk,zk]
t
为k历元接收机的位置，为k历元道路图像图窗中的道路区域；道路形状因子表示为：
[0104][0105]
其中为协方差矩阵。
[0106]
本发明步骤10包括：
[0107]
接收机状态量表示为：
[0108]
χ
*
＝[χ1,χ2,
…
,χk]
[0109]
χ
t
＝(x
t
,v
t
,δ
t
)
t
,1≤t≤k
[0110]
χ
*
表示从最初历元到当前历元k的接收机状态量集合，χ
t
表示在t历元的接收机状态量，其中包括位置x
t
、速度v
t
以及接收机钟差δ
t
；
[0111]
用表示k历元状态的最优估计，表示为：
[0112][0113]
当前k历元状态最优估计结果中的最优位置为将最优位置估计作为当前历元的定位结果xk＝[xk,yk,zk]
t
。
[0114]
本发明步骤11包括：
[0115]
输出定位结果：根据卫星数不同，由步骤6或步骤10中得出的最终的定位结果xk＝[xk,yk,zk]
t
，作为当前历元的定位结果，将该定位结果保存后用于历元k+1的定位解算，并输出当前历元的定位结果。
[0116]
有益效果：
[0117]
本发明提出的一种基于因子图及场景约束的多径环境组合导航方法，解决了如下2种技术缺陷：
[0118]
1)针对目前已有的基于因子图的gnss组合导航算法基于gnss时间序列观测量的节点约束并不能很好的解决高架桥、隧道等遮蔽环境下由于卫星信号中断或长时间失锁环境下的载具准确可靠的定位的问题，本发明在此基础上加入了基于场景约束，通过根据正常情况下车辆行驶区域应当处于道路范围内的物理特性，采用多普勒观测值所构建的运动约束以及道路坐标区域所构建的道路形状约束共同构成状态节点之间的约束，对节点进行控制，得出最优定位解，实现城市建筑物遮挡环境下的连续可靠定位。
[0119]
2)针对目前已有的基于因子图优化的gnss多传感器融合算法中定位效率不足，本发明结合离线处理和在线匹配相结合的模式，通过使用深度学习对地图信息进行预处理提取平面的坐标区而非需全程在线进行加载、处理及匹配的地图图像模型，实现离线使用和快速匹配，避免了复杂的初始化，提高了定位效率，实现城市复杂环境下的快速定位。
附图说明
[0120]
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
[0121]
图1为本发明总体流程图。
[0122]
图2为欧氏距离矩阵示意图。
[0123]
图3为本发明的因子图图形结构示意图。
具体实施方式
[0124]
本发明的核心内容为一种基于因子图及场景约束的多径环境组合导航方法，如图1所示：
[0125]
一、离线处理部分：
[0126]
步骤1：构建地图影像样本训练集：选取地图影像进行等大切分，初始选取b张训练样本图像，构建地图影像样本训练集q。
[0127]
步骤2：利用步骤1构建的训练集进行卷积神经网络训练，构建道路图像提取模型，
其步骤包括：
[0128]
(1)划分测试集。使用自主采样法划分训练集中的部分数据作为测试集，用于在卷积神经网络训练时实时验证网络的学习性能。测试集构建方法为：每次从样本集q中随机挑选一个样本拷贝至数据集k中，然后放回至原本的样本集q以供下次采样。重复上述过程b次后得到包含b个样本的数据集k，其中包含重复的样本，而q中的一部分样本未被采集到。这种方法得到的未被采集样本比率为：
[0129][0130]
其中，e为自然常数。因此差异样本占比约36.8％，选取q集和k集中的差异部分作为训练时的实时测试集。
[0131]
(2)网络训练。将地图中的路网矢量数据与样本图像进行匹配，作为卷积神经网络网络训练的输入二值图像，进行前向传播和反向传播调整卷积核权重值，最终生成符合预期的道路提取结果，训练结束。
[0132]
步骤3：根据初始位置获取本地城市道路区域图，其步骤包括：
[0133]
(1)网络输出。对于给定的初始化位置(手动标定或上一次使用本定位方法所储存的最后位置)，选定该位置所处的城市地图进行等大划分，得到h张本地地图图像，构建本地样本集l。输入本地样本进行卷积神经网络输出，得到h张初始道路提取图像。
[0134]
(2)图像处理。初始提取的道路图像除预期得到的道路图块外，还存在道路边缘波折与非道路图斑，根据形态学运算滤除非道路区域图斑，首先使用面积指数对图像进行面积筛选，根据经验得到的面积指数阈值为a
thre
若面积指数a＜a
thre
，则去除该图斑。
[0135]
利用道路狭长指数对噪声进行进一步的去除，道路狭长指数为：
[0136][0137]
其中，l
p
为道路图块外接矩形的对角线长度，w
p
为宽度，s
p
为道路面积。设定狭长指数阈值为r
thre
＝2，狭长指数低于阈值时判定图像块为非道路区域，此部分噪声区域应被去除。
[0138]
根据道格拉斯-普克算法进行对道路边缘进行直线逼近，得到更加接近于实际物理形状的道路图块。算法实现过程为：对曲线的首末点连一条直线，求出曲线上所有的点与直线的垂直距离di(i＝1,2,
…
,μ，μ为曲线上点的个数)，找出最大值将d
max
与给定的阈值d
thre
相比。若d
max
＜d
thre
，则将这条曲线上的中间点全部舍去，以初始划定的直线作为曲线的近似；若d
max
≥d
thre
，则保留d
max
对应的点，并以该点为界将曲线分为两部分，对每一段曲线都重复使用上述逼近方法进行曲线逼近，直到所有的d
max
均小于d
thre
，完成对波折道路边缘的直线化处理。
[0139]
(3)图像合成。将生成的道路提取图像按原始切割方法拼接，得到完整的基于深度学习构建的城市道路区域图，并根据原始地图影像的经纬度坐标对生成的城市道路区域图进行坐标匹配，给道路区域图进行矢量赋值。将经纬度坐标转换为地心地固坐标系下的坐标，设经纬度坐标向量为p
llh
＝[lat lon alt]
t
，其中，lat为纬度，lon表示经度，alt为高度；在地心地固坐标系中的坐标向量为p
ecef
＝[x y z]
t
，其中，x、y、z分别为地心地固坐标系
中的x轴、y轴、z轴坐标，则坐标转换公式为：
[0140][0141]
其中地球的极扁率和分别为地球的长轴和短轴，n的计算方法为：
[0142][0143]
(4)样本训练集更新。将本地样本集l加入原始地图影像样本训练集q，更新得到新的训练集q，其中包含b+h张样本图像，用于卷积神经网络的下一次定期优化更新。
[0144]
至此，离线的地图信息预处理模块加载完成，得到基于深度学习构建的城市道路区域图，在本次使用期间无需重复进行卷积神经网络训练。设定定期更新模式，设置更新周期为7天，每7天利用更新后的训练集重新进行卷积神经网络训练，而在更新周期内的每次使用无需进行网络训练，直接根据输入的初始位置得出网络输出的图像结果。
[0145]
二、在线计算部分
[0146]
步骤4：gnss伪距与多普勒观测值的获取。在城市环境下，载具使用车载gnss接收机进行gnss观测，获得当前历元(历元k)的伪距观测数据以及多普勒观测数据。其中包括：可见星数量可见星在地心地固坐标系中的坐标卫星信号信噪比每一颗可见星所对应的原始伪距观测值以及原始多普勒观测值其中，
[0147]
步骤5：排除卫星信号信噪比低于阈值的卫星。信噪比是接收机的载波信号强度与噪声强度的比值，单位为db-hz，能够反映出卫星信号质量，当信噪比低于一定值时，认为噪声对卫星信号的影响不可忽视。根据历史数据经验确定的卫星信号信噪比的阈值为snr
thre
，对上一步所获得的每一颗卫星的信噪比进行条件判断，当时，认为卫星的信号质量良好，保留卫星的相关观测数据，否则认为卫星的信号质量较差，删除卫星的相关观测数据。
[0148]
步骤6：判断剩余卫星是否满足定位条件，并实现不同决策方案。
[0149]
(1)根据信噪比低于阈值排除观测质量不佳的卫星后，剩余卫星的数量为所属星座数为对剩余卫星数量进行判断，若认为不满足gnss定位条件，采用历史数据估计方法推算当前位置。由于通常接收机采样频率设置较高，相邻历元的状态变化率可近似一致，因此可以通过历史历元数据大致推算当前历元位置变化量。读取k-1历元和k-2历元的位置信息，k-1历元的位置为x
k-1
，k-2历元的位置为x
k-2
，因此历元间位置变化量δx＝x
k-1-x
k-2
，推算得出当前历元的位置xk＝2x
k-1-x
k-2
，当前历元定位结束，直接输出定位结果。
[0150]
(2)若认为满足卫星定位条件，对原始伪距观测值数据进行误差修正处理，电离层误差修正量为对流层误差修正量为因此可得到进行了电离层延迟和对流层延迟改正后的伪距观测值进入下一步。
[0151]
步骤7：判断剩余卫星是否有冗余，并用不同策略进行质量控制。
[0152]
(1)若认为不存在冗余卫星观测值，无法进行卫星定位故障检测与排除。故可用卫星数保留当前gnss观测值，保留当前gnss观测值，直接进入下一步。
[0153]
(2)若认为存在冗余卫星观测值，采用欧式距离矩阵法进行卫星故障的快速检测。利用已知的卫星位置与接收机获得的伪距观测值构建欧氏距离矩阵其中矩阵的第一行和第一列由测量伪距构成，对角线元素为0，其他元素由已知的卫星位置填充。欧式距离矩阵的构成如图2所示：
[0154]
以三颗卫星与接收机的相对位置关系为例：对角线上的矩阵元素a
11
、a
22
、a
33
、a
44
均为0，第一行的矩阵元素a
1p
与第一列的矩阵元素a
q1
为卫星与接收机间的测量伪距平方和其余位置a
pq
为两颗卫星之间的距离根据欧氏距离矩阵求得对应的gram矩阵
[0155][0156]
此处的矩阵j为几何中心矩阵，i为单位矩阵，1表示1的列向量。gram矩阵的秩等于状态空间的维数n，即该式对故障卫星进行检测。
[0157]
对gram矩阵进行奇异值分解得：
[0158][0159]
其中u为左侧奇异向量，v为右侧奇异向量，σ为对角矩阵，包含的奇异值σ1,σ2,
…
,σ
1+m
，根据矩阵的奇异值判断检验统计量是否超过检测门限：
[0160][0161]
若检验统计量超过检测门限，则表明存在故障卫星，需要进行故障卫星的检测和排除。三维空间中状态空间n＝3，令u矩阵的第n+1列为u4，v矩阵的第n+1列为v4，按绝对值大小对u4和v4中的元素进行排序，最大的绝对值对应故障卫星。排除故障卫星后重新计算检验统计量并判断是否超过检测门限，若超过检测门限，重新进行排序和故障卫星排除，直到低于检测门限，则认为不存在故障卫星。
[0162]
进行故障排除后剩余可用卫星数量为对应的卫星在地心地固坐标系中的坐
标为伪距观测值以及多普勒观测值
[0163]
步骤8：根据当前位置基准点提取当前区域道路形状范围：将上一历元的定位结果x
k-1
＝[x
k-1
,y
k-1
,z
k-1
]
t
作为本历元道路形状提取的中心点，根据第三步所获得的城市道路区域图以x
k-1
为中心，在x轴方向和y轴方向分别截取
±
10m的范围，得到当前区域的道路区域图。
[0164]
使用滑动图窗裁取本历元所需参考的道路区域图，图窗范围为：m＝{x,y|x
k-1-10m＜x＜x
k-1
+10m,y
k-1-10m＜y＜y
k-1
+10m}其中道路区域i∈m。
[0165]
步骤9：计算伪距因子和多普勒因子，并构造接收机位置坐标与道路区域之间的差值参数，其步骤包括：
[0166]
(1)计算伪距因子。根据进行了误差修正处理后的得到误差修正后的伪距测量方程：
[0167][0168]
其中表示在k历元卫星与接收机之间的几何距离，c为光速，δk和分别表示接收机钟差和卫星钟差，表示多路径效应等其他测量噪声所造成的误差。将伪距观测方程写成参数方程形式：
[0169][0170]
因此根据上述方程得到卫星j在k历元的伪距误差函数，定义为伪距因子：
[0171][0172]
其中为由卫星高度角与信噪比构成的协方差矩阵。
[0173]
(2)计算多普勒因子。多普勒测量方程为：
[0174][0175]
其中λ为卫星信号的载波波长，和分别为接收机和卫星钟的钟漂，为电离层误差变化率，为对流层误差变化率，为其它误差变化率，伪距率为：
[0176][0177]
为卫星指向接收机的单位向量，为已知的卫星j在k历元的速度，为接收机的运动速度，因此根据最小二乘方法得到当前历元的速度状态方程：
[0178]
[0179][0180]
其中为多普勒求解中k历元的其他噪声，xk＝[xk,yk,zk]
t
为k历元接收机的位置，x
k-1
＝[x
k-1
,y
k-1
,z
k-1
]
t
为k-1历元的接收机位置，δt为采样间隔。
[0181]
因此根据上述方程得到k历元的速度误差函数，定义为多普勒因子：
[0182][0183]
其中也为协方差矩阵。
[0184]
(3)计算道路形状因子。第八步中得到的道路区域矢量集为t，接收机位置坐标与道路区域之间的差值参数为：
[0185][0186]
其中xk＝[xk,yk,zk]
t
为k历元接收机的位置，为k历元道路图像图窗中的道路区域。因此道路形状因子表示为：
[0187][0188]
其中也为协方差矩阵。
[0189]
步骤10：根据场景约束进行因子图优化定位：接收机状态量表示为：
[0190]
χ
*
＝[χ1,χ2,
…
,χk]
[0191]
χ
t
＝(x
t
,v
t
,δ
t
)
t
,1≤t≤k
[0192]
χ
*
表示从最初历元到当前历元k的接收机状态量集合，χ
t
表示在t历元的接收机状态量，其中包括位置x
t
、速度v
t
以及接收机钟差δ
t
。所述因子图优化定位方法的因子图图形结构如图3所示。
[0193]
用表示k历元状态的最优估计，表示为：
[0194][0195]
当前k历元状态最优估计结果中的最优位置为将最优位置估计作为当前历元的定位结果xk＝[xk,yk,zk]
t
。
[0196]
步骤11：输出定位结果：根据卫星数不同，由步骤6或步骤10得出的最终的定位结果为xk＝[xk,yk,zk]
t
，将定位结果储存在储存器中用于历元k+1的定位解算，并输出当前历元的定位结果。
[0197]
本发明提供了一种基于因子图及场景约束的多径环境组合导航方法的思路及方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部
分均可用现有技术加以实现。