车辆导航系统的多传感器融合定位方法与流程

1.本发明涉及车辆导航技术领域，特别涉及一种车辆导航系统的多传感器融合定位方法。


背景技术：

2.ar汽车实景导航技术在最近几年受到学术界和工业界的广泛关注，ar导航利用虚拟增强技术帮助驾驶员及时、正确地做出导航判断决策，其前提是车辆的位置与姿态信息能够及时地反馈给导航引擎。然而目前市场上对普通驾驶车辆的位姿分析都是依靠gnss卫星信号，精度误差在10米左右，无法满足路况复杂情况下(高架桥、多岔路口等)导航产品的精度要求。尤其在涵洞与隧道场景下，丢失卫星信号仍然是ar导航产品失效的根本原因。市场上对特殊功能车辆的位姿分析可依靠融合rtk差分信号增强技术与专业的惯导组件实现厘米级的精度误差，然而同时也大幅提高了硬件成本，限制了该技术在普通驾驶车辆上的应用。
3.因此，现有技术中车辆的导航系统存在精度不高，且利用差分信号增强技术进行导航成本较高的问题。


技术实现要素：

4.本发明的目的在于解决现有技术中车辆的导航系统存在精度不高，且利用差分信号增强技术进行导航成本较高的问题。
5.为解决上述问题，本发明的实施方式公开了一种车辆导航系统的多传感器融合定位方法，车辆导航系统包括捷联惯导系统和全球导航卫星定位系统；车辆导航系统的多传感器融合定位方法包括以下步骤：
6.s0：判断全球导航卫星定位系统的信号是否丢失；其中
7.若全球导航卫星定位系统的信号丢失，则将捷联惯导系统的输出信息作为车辆的导航信息；其中，车辆的导航信息包括车辆位置信息和车辆速度信息；或
8.若全球导航卫星定位系统的信号未丢失，则利用粒子群优化与粒子滤波算法对捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行多传感器信息融合，并将融合后的信息作为车辆的导航信息。
9.采用上述方案，将全球导航卫星定位系统和低成本的捷联惯导系统进行结合，共同对车辆进行定位，保证了系统在进入涵洞、隧道等卫星信号丢失情况下的定位功能，提高了车辆的导航系统的精度，也降低了车辆导航系统的成本。
10.根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，利用粒子群优化与粒子滤波算法对捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行多传感器信息融合，并将融合后的信息作为车辆的导航信息包括：
11.s1：对捷联惯导系统和全球导航卫星定位系统建立初始粒子群，并确定初始粒子
群的粒子群参数；
12.s2：根据捷联惯导系统的输出信息更新初始粒子群，以形成更新粒子群，并获取更新粒子群对应的捷联惯导系统的输出信息；
13.s3：利用粒子群优化算法驱动更新粒子群中的粒子向高似然区域运动；其中，高似然区域为更新粒子群的迭代群体最优值和更新粒子群中的各粒子的迭代粒子最优值；
14.s4：利用粒子滤波算法计算更新粒子群中的各粒子的粒子权重，并对粒子权重进行归一化处理；
15.s5：对更新粒子群中的各粒子进行重采样以形成重采样粒子群；
16.s6：根据重采样粒子群的中心粒子的维度确定捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息，并将捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息带入更新后的捷联惯导系统的姿态转换矩阵，以求出中心粒子的更新姿态，并将更新姿态作为车辆的导航信息。
17.采用上述方案，结合普通的全球导航卫星定位系统和低成本捷联惯导系统进行组合定位，在卫星信号丢失时保障了定位系统功能，利用粒子滤波算法计算更新粒子群中的各粒子的粒子权重，并对粒子权重进行归一化处理，满足车辆导航对车辆定位精度系统的需求，同时降低了车辆定位系统的硬件成本。相对于传统的基于卡尔曼滤波的多传感器融合组合定位算法，本发明考虑联惯导系统模型非线性特点，采用粒子滤波算法，采用粒子群优化算法使得粒子的分布向高似然概率密度分布区域运动，最终在提高精度的基础上减少滤波粒子数，解决粒子贫乏问题，提高定位系统的实时性，保证其鲁棒性和稳定性。同时基于少量粒子进行的每一次滤波时间，即实时输出定位结果的周期可以满足目前低成本的全球导航卫星定位系统和捷联惯导系统信息融合对输出定位结果的频率要求，减小计算资源且提高计算精度。
18.根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，步骤s1中，初始粒子群的粒子群参数包括各粒子的初始维度、粒子群的初始均值、粒子群的初始分布方差；并且，确定初始粒子群的粒子群参数包括：
19.s11：根据以下公式对初始粒子群的各粒子进行定义：
20.x＝(vn,ve,vd,lat,lon,h)
21.其中，x为各粒子的维度；vn为地理北方向的车辆速度；ve为地理东方向的车辆速度；vd为垂直于地表的地方向的车辆速度；lat为经度；lon为纬度；h为高度；
22.s12：利用惯导递推算法获取前一时刻北方向的车辆速度、东方向的车辆速度、地方向的车辆速度、经度、纬度、高度，并将其作为粒子群的初始均值；
23.s13：获取全球导航卫星定位系统的观测精度分布方差，并将其作为粒子群的初始分布方差；
24.s14：根据粒子群的初始均值和粒子群的初始分布方差确定各粒子的维度的具体数值。
25.根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，捷联惯导系统的输出信息包括捷联惯导系统的角速度、捷联惯导系统的加速度；并且，步骤s2包括：
26.s21：根据当前时刻的捷联惯导系统的角速度和捷联惯导系统的加速度对初始粒
子群中的各粒子进行捷联惯导解算，以得到更新后的捷联惯导系统的角速度和加速度；
27.s22：根据捷联惯导系统的加速度计的随机游走误差确定滤波过程状态噪声；
28.s23：根据滤波过程状态噪声、更新后的捷联惯导系统的角速度和加速度，并利用高斯模型预测捷联惯导系统的角速度和加速度；
29.s24：将预测得到的捷联惯导系统的角速度和加速度作为更新粒子群对应的捷联惯导系统的输出信息。
30.采用上述方案，根据滤波过程状态噪声、更新后的捷联惯导系统的角速度和加速度，并利用高斯模型预测捷联惯导系统的角速度和加速度，能够使得预测得到的捷联惯导系统的角速度和加速度更接近真实值。
31.根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，初始粒子群的粒子群参数还包括预设迭代步数、各粒子的初始运动速度；并且，
32.步骤s3包括：
33.s31：构建粒子群优化算法的适应度函数表达式；
34.s32：对初始粒子群进行迭代处理，并将各粒子的初始运动速度设定为各粒子的初始值与全球导航卫星定位系统的观测量距离的千分之一至千分之十；
35.s33：根据粒子群优化算法的适应度函数表达式计算并记录各初始粒子群中各粒子的初始粒子最优值、各个初始粒子群的初始群体最优值；
36.s34：根据自学习率、群体学习率、速度及速度惯性权重系数使得更新粒子群中的各粒子向初始粒子最优值和初始群体最优值移动；
37.s35：根据粒子群优化算法的适应度函数表达式计算并记录所有更新粒子群的中各粒子的粒子历史最优值、粒子历史最优值对应的粒子，以及所有更新粒子群的全局群体最优值、全局群体最优值对应的粒子；
38.s36：判断当前迭代步数是否等于预设迭代步数；
39.若是，则结束迭代，并输出优化后的优化粒子群；
40.若否，则进入下一轮迭代。
41.采用上述方案，能够使得粒子快速向最优值移动，保证了系统的实时性。
42.根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，粒子群优化算法的适应度函数表达式为：
[0043][0044]
其中，fitness为适应度函数表达式；r
t
为全球导航卫星定位系统的量测噪声协方差矩阵；并且
[0045]
根据如下公式使得更新粒子群中的各粒子向初始粒子最优值和初始群体最优值移动：
[0046]vk 1
＝c0*vk c1*rand*(y
k-xk) c2*rand*(pg
best-xk)
[0047]
x
k 1
＝xk v
k 1
[0048]
其中，下标k表示当前迭代步数；v为粒子的速度向量；x为当前粒子的值；y为各粒子的粒子历史最优值；pg
best
是粒子历史最优值对应的粒子；c0为速度惯性权重系数；c1为粒
子的自学习率；c2为粒子的群体学习率。
[0049]
采用上述方案，在粒子移动时考虑了粒子的自学习率和粒子的群体学习率，提高了粒子移动的准确率和移动效率。
[0050]
根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，步骤s4包括：
[0051]
s41：利用高斯分布对优化粒子群进行计算；其中，高斯分布的均值为全球导航卫星定位系统的观测值，高斯分布的方差为全球导航卫星定位系统的量测噪声协方差；
[0052]
s42：根据s41的计算结果更新更新粒子群中的各粒子的粒子权重；并且，根据以下公式更新更新粒子群中的各粒子的粒子权重：
[0053][0054]
其中，i为第i个粒子；p为粒子群中各粒子的总数；为高斯分布；
[0055]
s43：根据更新后的粒子权重对粒子权重进行归一化处理；并且，根据以下公式对粒子权重进行归一化处理：
[0056][0057]
其中，为粒子权重；i为第i个粒子。
[0058]
采用上述方案，进行粒子群优化的粒子滤波处理，相比于纯粒子滤波算法，本方案采用粒子群优化使得粒子在采样时趋向高似然概率分布，不仅可以抑制发散，还可以提高滤波定位精度。
[0059]
根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，步骤s5包括：
[0060]
采用轮盘赌方法对更新粒子群中的各粒子进行重采样，以使重采样后的粒子向后验概率密度分布取值大于预设密度分布取值阈值的区域运动。
[0061]
根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，初始粒子群的粒子群参数还包括预设滤波次数，且预设滤波次数至少为一次；
[0062]
当预设滤波次数大于一次时，步骤s5之后还包括：
[0063]
s5’：重复执行步骤s4和步骤s5，并判断滤波次数是否等于预设滤波次数；
[0064]
当滤波次数等于预设滤波次数时，结束滤波并执行步骤s6；
[0065]
当滤波次数不等于预设滤波次数时，继续重复执行步骤s4和步骤s5；并且，
[0066]
重复执行步骤s5’之前，还包括建立初始化粒子群；
[0067]
当预设滤波次数大于等与两次时，更新粒子群的分布方差根据上一次滤波后对应的粒子群的分布方差确定。
[0068]
根据本发明的另一具体实施方式，本发明实施方式公开的车辆导航系统的多传感器融合定位方法，初始粒子群的粒子群参数还包括初始粒子群的粒子数量；并且，
[0069]
初始粒子群的粒子数量为80个至120个。
[0070]
采用上述方案，通过将初始粒子群的数量设置为80至120个，满足了定位系统实时
性的要求。
[0071]
本发明的有益效果是：
[0072]
本方案通过设定较小的粒子群数量，满足了车辆定位系统的实时性需求，同时采用的粒子群优化算法有效地解决了滤波过程中的粒子贫乏问题，提高了定位系统的精度和稳定性。并且，粒子群优化使得粒子在采样时趋向高似然概率分布，不仅可以抑制发散，还可以提高滤波定位精度。
[0073]
进一步的，本方案采用的采集捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息的传感器均采用车载通信设备中已有的硬件，且不考虑差分定位等服务费昂贵的定位方式，能够最大程度降低硬件成本。
[0074]
更进一步地，本方案结合捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行组合定位算法研究，在减小粒子数，即节约计算资源的基础上，保证车辆导航对车辆定位精度、实时性和稳定性的要求。
附图说明
[0075]
图1是本发明实施例提供的车辆导航系统的多传感器融合定位方法的流程示意图；
[0076]
图2是本发明实施例提供的车辆导航系统的多传感器融合定位方法的另一流程示意图；
[0077]
图3是本发明实施例提供的车辆导航系统的多传感器融合定位方法的另一流程示意图；
[0078]
图4是本发明实施例提供的车辆导航系统的多传感器融合定位方法的另一流程示意图；
[0079]
图5是本发明实施例提供的车辆导航系统的多传感器融合定位方法的另一流程示意图；
[0080]
图6是本发明实施例提供的车辆导航系统的多传感器融合定位方法的另一流程示意图；
[0081]
图7是本发明实施例提供的车辆导航系统的多传感器融合定位方法的定位结果；
[0082]
图8a、图8b、图8c是本发明实施例提供的车辆导航系统的多传感器融合定位方法中全球导航卫星定位系统的轨迹误差和经粒子群优化粒子滤波融合后的轨迹误差对比图；
[0083]
图9a、图9b、图9c是本发明实施例提供的车辆导航系统的多传感器融合定位方法中粒子群优化粒子滤波和纯粒子滤波算法轨迹对比图。
具体实施方式
[0084]
以下由特定的具体实施例说明本发明的实施方式，本领域技术人员可由本说明书所揭示的内容轻易地了解本发明的其他优点及功效。虽然本发明的描述将结合较佳实施例一起介绍，但这并不代表此发明的特征仅限于该实施方式。恰恰相反，结合实施方式作发明介绍的目的是为了覆盖基于本发明的权利要求而有可能延伸出的其它选择或改造。为了提供对本发明的深度了解，以下描述中将包含许多具体的细节。本发明也可以不使用这些细节实施。此外，为了避免混乱或模糊本发明的重点，有些具体细节将在描述中被省略。需要
说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
[0085]
应注意的是，在本说明书中，相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。
[0086]
在本实施例的描述中，需要说明的是，术语“上”、“下”、“内”、“底”等指示的方位或位置关系为基于附图所示的方位或位置关系，或者是该发明产品使用时惯常摆放的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0087]
术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0088]
在本实施例的描述中，还需要说明的是，除非另有明确的规定和限定，术语“设置”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本实施例中的具体含义。
[0089]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步地详细描述。
[0090]
为解决现有技术中车辆的导航系统存在精度不高，且利用差分信号增强技术进行导航成本较高的问题，本发明的实施方式公开了一种车辆导航系统的多传感器融合定位方法。具体地，本发明实施方式提供的车辆导航系统的多传感器融合定位方法中，车辆导航系统包括捷联惯导系统和全球导航卫星定位系统。参考图1示出的本发明实施例提供的车辆导航系统的多传感器融合定位方法的流程示意图，车辆导航系统的多传感器融合定位方法包括以下步骤：
[0091]
s0：判断全球导航卫星定位系统的信号是否丢失。
[0092]
若全球导航卫星定位系统的信号丢失，则将捷联惯导系统的输出信息作为车辆的导航信息；其中，车辆的导航信息包括车辆位置信息和车辆速度信息；或
[0093]
若全球导航卫星定位系统的信号未丢失，则利用粒子群优化与粒子滤波算法对捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行多传感器信息融合，并将融合后的信息作为车辆的导航信息。
[0094]
采用上述方案，将全球导航卫星定位系统和低成本的捷联惯导系统进行结合，共同对车辆进行定位，保证了系统在进入涵洞、隧道等卫星信号丢失情况下的定位功能，提高了车辆的导航系统的精度，也降低了车辆导航系统的成本。
[0095]
下面结合图1-9具体描述本发明实施例提供的车辆导航系统的多传感器融合定位方法，其中，图1-6是本发明实施例提供的车辆导航系统的多传感器融合定位方法的流程示意图；图7是本发明实施例提供的车辆导航系统的多传感器融合定位方法的定位结果；图8是本发明实施例提供的车辆导航系统的多传感器融合定位方法中全球导航卫星定位系统的轨迹误差和经粒子群优化粒子滤波融合后的轨迹误差对比图；图9是本发明实施例提供的车辆导航系统的多传感器融合定位方法中粒子群优化粒子滤波和纯粒子滤波算法对比图。
[0096]
参考图1，本实施例提供的车辆导航系统的多传感器融合定位方法，首先执行步骤
s0，判断全球导航卫星定位系统的信号是否丢失。
[0097]
而判断全球导航卫星定位系统的信号是否丢失，可以根据车况间接判定，即当车辆驶入隧道、涵洞时，判定为全球导航卫星定位系统的信号丢失，或者可以通过车载通信设备的接收信号直接判定，即当车载通信设备接收不到全球导航卫星定位系统的信号时，则判定为全球导航卫星定位系统的信号丢失。
[0098]
如果全球导航卫星定位系统的信号丢失，则将捷联惯导系统的输出信息作为车辆的导航信息。也就是说，在涵洞与隧道场景下，如果全球导航卫星定位系统的信号丢失，就仅仅将捷联惯导系统的输出信息作为车辆的导航信息，并以该导航信息为车辆导航。
[0099]
需要说明的是，本实施例中，捷联惯导系统是一种不依赖于任何外部信息、也不向外部辐射能量的自主式导航系统。捷联惯导系统由三个速率陀螺、三个线加速度计和微型计算机组成，且陀螺和加速度计分别用来测量运载体的角运动信息和线运动信息。捷联惯导系统的输出信息包括捷联惯导系统的角速度、捷联惯导系统的加速度。其中，捷联惯导系统的角速度也即为捷联惯导系统的速率陀螺的角速度，捷联惯导系统的加速度也即为捷联惯导系统的加速度计采集的加速度信息。微型计算机根据这些测量信息解算出运载体即车辆的航向、姿态、速度和位置，就可以对车辆进行导航。
[0100]
如果全球导航卫星定位系统的信号未丢失，则利用粒子群优化与粒子滤波算法对捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行多传感器信息融合，并将融合后的信息作为车辆的导航信息。
[0101]
也就是说，当车辆能接收到全球导航卫星定位系统的信号以及捷联惯导系统的信号，则将二者的输出信息进行融合，并以融合后的信息为车辆导航。
[0102]
以下，对利用粒子群优化与粒子滤波算法对捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息进行多传感器信息融合，并将融合后的信息作为车辆的导航信息进行详细说明。
[0103]
参考图2，本实施例中，如果全球导航卫星定位系统的信号未丢失，则首先执行步骤s1，对捷联惯导系统和全球导航卫星定位系统建立初始粒子群，并确定初始粒子群的粒子群参数。
[0104]
具体地，本实施例中，初始粒子群的粒子群参数包括各粒子的初始维度、粒子群的初始均值、粒子群的初始分布方差。
[0105]
需要说明是，本实施例中，初始粒子群的粒子群参数还包括初始粒子群的粒子数量。而为了满足定位系统实时性的要求，需要设定尽可能少的初始粒子群的数量。本实施例中，初始粒子群的粒子数量为80个至120个，具体可以是80个、90个、100个、110个、120个还可以是该范围内的其他数值，本实施例对此不做限制。
[0106]
更具体地，参考图3，本实施例中，确定初始粒子群的粒子群参数包括以下步骤：
[0107]
s11：根据以下公式对初始粒子群的各粒子进行定义：
[0108]
x＝(vn,ve,vd,lat,lon,h)
[0109]
其中，x为各粒子的维度；vn为地理北方向的车辆速度；ve为地理东方向的车辆速度；vd为垂直于地表的地方向的车辆速度；lat为经度；lon为纬度；h为高度。
[0110]
s12：利用惯导递推算法获取前一时刻地理北方向的车辆速度、地理东方向的车辆速度、垂直于地表的地方向的车辆速度、经度、纬度、高度，并将其作为粒子群的初始均值。
[0111]
需要说明的是，惯导递推算法具体为基于递推最小二乘法的惯导姿态误差动态标定方法，其具体可以参考现有技术，本实施在此不再赘述。
[0112]
s13：获取全球导航卫星定位系统的观测精度分布方差，并将其作为粒子群的初始分布方差。
[0113]
s14：根据粒子群的初始均值和粒子群的初始分布方差确定各粒子的维度的具体数值。
[0114]
上述即为对初始粒子群进行定义，以求解出初始粒子群中各粒子的维度的具体数值的过程。
[0115]
在完成建立初始粒子群并设定初始粒子群的粒子群参数之后，执行步骤s2：根据捷联惯导系统的输出信息更新初始粒子群，以形成更新粒子群，并获取更新粒子群对应的捷联惯导系统的输出信息。
[0116]
也就是说，此步骤需要根据捷联惯导系统的角速度、捷联惯导系统的加速度对初始粒子群进行更新，以得到更新后的更新粒子群，然后进一步获取更新粒子群所对应的捷联惯导系统的角速度、捷联惯导系统的加速度。
[0117]
更具体地，参考图3，步骤s2包括以下步骤：
[0118]
s21：根据当前时刻的捷联惯导系统的角速度和捷联惯导系统的加速度对初始粒子群中的各粒子进行捷联惯导解算，以得到更新后的捷联惯导系统的角速度和加速度。
[0119]
此步骤对初始粒子群中的各粒子进行捷联惯导解算，是为了得到更新后的位置和速度理论值。
[0120]
s22：根据捷联惯导系统的加速度计的随机游走误差确定滤波过程状态噪声。
[0121]
s23：根据滤波过程状态噪声、更新后的捷联惯导系统的角速度和加速度，并利用高斯模型预测捷联惯导系统的角速度和加速度。
[0122]
s24：将预测得到的捷联惯导系统的角速度和加速度作为更新粒子群对应的捷联惯导系统的输出信息。
[0123]
此步骤是为了将高斯模型叠加至上述更新后的位置和速度理论值，以得到预测的真实值。
[0124]
在得到预测的更新后的捷联惯导系统的输出信息之后，执行步骤s3：利用粒子群优化算法驱动更新粒子群中的粒子向高似然区域运动。
[0125]
需要说明的是，本实施例中，高似然区域为更新粒子群的迭代群体最优值和更新粒子群中的各粒子的迭代粒子最优值。
[0126]
需要解释的是，迭代群体最优值为粒子群中最优的粒子。其具体的确定方式为，确定出初始粒子群的最优粒子，然后更新初始粒子群，得到更新后的更新粒子群，确定出更新粒子群的最优粒子，并与初始粒子群的最优粒子进行比较，选出二者中的最优粒子。在后续进行更新的过程中，每更新一次就进行一次选择，以得到迭代群体最优值。迭代粒子最优值则是各代粒子群中，各粒子的历史最优值。
[0127]
具体地，本实施例中，初始粒子群的粒子群参数还包括预设迭代步数、各粒子的初始运动速度。
[0128]
更具体地，参考图4，本实施例中，步骤s3包括以下步骤：
[0129]
s31：构建粒子群优化算法的适应度函数表达式。
[0130]
需要说明的是，本实施例中，将当前时刻的全球导航卫星定位系统的观测量引入适应度函数的求解。具体体现为各粒子和观测量的距离。也就是说，如果该粒子群的分布趋向于真实状态，即真实车辆速度和车辆位置，则计算出的每个粒子的适应度函数值较高，反之粒子群中各粒子的个体最优值和全局最优值都较低。
[0131]
还需要说明的是，本实施例中，粒子群优化算法的适应度函数表达式为：
[0132][0133]
其中，fitness为适应度函数表达式；r
t
为全球导航卫星定位系统的量测噪声协方差矩阵。
[0134]
s32：对初始粒子群进行迭代处理，并将各粒子的初始运动速度设定为各粒子的初始值与全球导航卫星定位系统的观测量距离的千分之一至千分之十。
[0135]
本实施例中，对初始粒子群进行迭代处理时，需要设定预设迭代步数。本实施例中，为了满足定位系统实时性的要求，不能设置较多的迭代步数。具体地，本实施例中预设迭代步数可以是3至5代，即可以是3代、4代或者5代。
[0136]
s33：根据粒子群优化算法的适应度函数表达式计算并记录各初始粒子群中各粒子的初始粒子最优值、各个初始粒子群的初始群体最优值。
[0137]
s34：根据自学习率、群体学习率、速度及速度惯性权重系数使得更新粒子群中的各粒子向初始粒子最优值和初始群体最优值移动。
[0138]
需要说明的是，本实施例中，根据如下公式使得更新粒子群中的各粒子向初始粒子最优值和初始群体最优值移动：
[0139]vk 1
＝c0*vk c1*rand*(y
k-xk) c2*rand*(pg
best-xk)
[0140]
x
k 1
＝xk v
k 1
[0141]
其中，下标k表示当前迭代步数；v为粒子的速度向量；x为当前粒子的值；y为各粒子的粒子历史最优值；pg
best
是粒子历史最优值对应的粒子；c0为速度惯性权重系数；c1为粒子的自学习率；c2为粒子的群体学习率。
[0142]
需要说明的是，在本发明的一具体实施例中，使得更新粒子群中的各粒子按照速度惯性权重系数c0＝0.5、自学习率c1＝1.5、群体学习率c2＝1.5，以及0到1之间的随机率向初始粒子最优值和初始群体最优值移动。
[0143]
s35：根据粒子群优化算法的适应度函数表达式计算并记录所有更新粒子群的中各粒子的粒子历史最优值、粒子历史最优值对应的粒子，以及所有更新粒子群的全局群体最优值、全局群体最优值对应的粒子。
[0144]
s36：判断当前迭代步数是否等于预设迭代步数；
[0145]
若是，则结束迭代，并输出优化后的优化粒子群；
[0146]
若否，则进入下一轮迭代。
[0147]
需要说明的是，本实施例中，迭代完成之后，输出优化后的优化粒子群，此时的粒子群分布趋向于高似然概率密度分布，从而解决了粒子群中粒子数量较少的问题。也就是说，通过上述方法，即使是设置较少的初始粒子群的粒子数量和较少的迭代次数，也不会使得系统最终输出的结果误差较大。
[0148]
上述过程即为使得更新粒子群接近真实后验状态的过程。之后，执行步骤s4：利用
粒子滤波算法计算更新粒子群中的各粒子的粒子权重，并对粒子权重进行归一化处理。此步骤即为对粒子进行滤波处理的过程。
[0149]
具体地，参考图5，本实施例中，步骤s4具体包括以下步骤：
[0150]
s41：利用高斯分布对优化粒子群进行计算；其中，高斯分布的均值为全球导航卫星定位系统的观测值，高斯分布的方差为全球导航卫星定位系统的量测噪声协方差。
[0151]
需要说明的是，利用高斯分布对优化粒子群进行计算也就是进行权重采样的过程，权重采样过程即为求粒子位于x状态时观测值y出现的概率，具体实现方式为将优化后的粒子群放到以观测值为均值，量测噪声为方差的高斯分布中进行计算。
[0152]
s42：根据s41的计算结果更新更新粒子群中的各粒子的粒子权重；并且，根据以下公式更新更新粒子群中的各粒子的粒子权重：
[0153][0154]
其中，i为第i个粒子；p为粒子群中各粒子的总数；为高斯分布。
[0155]
需要说明的是，此步骤即为利用最新观测值对粒子群进行权重更新的步骤。
[0156]
s43：根据更新后的粒子权重对粒子权重进行归一化处理；并且，根据以下公式对粒子权重进行归一化处理：
[0157][0158]
其中，为粒子权重；i为第i个粒子。
[0159]
在本发明的一具体实施例中，初始权重
[0160]
在对粒子进行滤波处理之后，执行步骤s5：对更新粒子群中的各粒子进行重采样以形成重采样粒子群。
[0161]
具体地，本实施例中，步骤s5具体包括：采用轮盘赌方法对更新粒子群中的各粒子进行重采样，以使重采样后的粒子向后验概率密度分布取值大于预设密度分布取值阈值的区域运动。
[0162]
也就是说，对粒子进行重采样，目的是为了将接近真实状态的粒子被最大概率地选择出来。
[0163]
在本发明的一具体实施例中，重采样后每个粒子的权重为
[0164]
更具体地，参考图6，初始粒子群的粒子群参数还包括预设滤波次数，且预设滤波次数至少为一次。而当预设滤波次数大于一次时，步骤s5之后还包括：
[0165]
s5’：重复执行步骤s4和步骤s5，并判断滤波次数是否等于预设滤波次数。
[0166]
当滤波次数等于预设滤波次数时，结束滤波并执行步骤s6。
[0167]
当滤波次数不等于预设滤波次数时，继续重复执行步骤s4和步骤s5。
[0168]
并且，重复执行步骤s5’之前，还包括建立初始化粒子群。也就是说，每一次粒子滤波开始时，都要进行一次粒子群初始化。
[0169]
当预设滤波次数大于等与两次时，更新粒子群的分布方差根据上一次滤波后对应
的粒子群的分布方差确定。
[0170]
也就是说，在第一次滤波时，以全球导航卫星定位系统给出的平均方差为粒子群的分布方差，在第n次滤波时，该方差由第n-1次滤波后的粒子群分布方差确定。此外，在第一次滤波时，以v0作为粒子群的分布标准差；v0为初始时刻地理北方向的车辆速度。
[0171]
在将最接近真实状态的粒子被选择出来之后，执行步骤s6：根据重采样粒子群的中心粒子的维度确定捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息，并将捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息带入更新后的捷联惯导系统的姿态转换矩阵，以求出中心粒子的更新姿态，并将更新姿态作为车辆的导航信息。
[0172]
本实施例中，重采样后组成的粒子群中心即为捷联惯导系统的输出信息和全球导航卫星定位系统的输出信息的融合结果，该中心粒子的六个纬度分别描述融合后的车辆位置和速度信息，并更新姿态转换矩阵，求解出更新后的姿态信息。
[0173]
本实施例中，最终基于多传感器融合定位方法的定位结果如图7所示，图7中，具有较多波动的轨迹即为基于全球导航卫星定位系统的轨迹，较平缓、没有凸出的轨迹即为基于粒子群优化与粒子滤波算法的轨迹。可以看出，融合后的基于粒子群优化与粒子滤波算法的轨迹优于基于全球导航卫星定位系统的轨迹，且基于粒子群优化与粒子滤波算法的轨迹趋近于真实轨迹。
[0174]
参考图8a、图8b、图8c和图9a、图9b、图9c，当检测到全球导航卫星定位系统的输出信号时，全球导航卫星定位系统的观测数据与捷联惯导系统融合，输出逼近于真实值的轨迹。
[0175]
具体地，参考图8a、图8b、图8c，当采样时间为t＝80s至120s时，全球导航卫星定位系统出现间歇性失效情况，检测不到全球导航卫星定位系统的输出信号时，定位系统依赖捷联惯导系统进行状态递推，间歇性检测到全球导航卫星定位系统的输出信号后将继续进行信息融合，融合轨迹没有出现较大偏差，保证定位系统如常运行，实现了轨迹的平滑。
[0176]
进一步地，参考图9a、图9b、图9c，当采样时间为90s至150s时，全球导航卫星定位系统的轨迹会偏离预先获得的真实轨迹，而粒子群优化与粒子滤波算法的轨迹则始终保持平稳，且与预先获得的真实轨迹没有较大差异。
[0177]
虽然通过参照本发明的某些优选实施方式，已经对本发明进行了图示和描述，但本领域的普通技术人员应该明白，以上内容是结合具体的实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。本领域技术人员可以在形式上和细节上对其作各种改变，包括做出若干简单推演或替换，而不偏离本发明的精神和范围。