基于多传感器融合定位的时间同步方法及系统与流程

1.本发明涉及无线定位技术领域，尤其涉及一种基于多传感器融合定位的时间同步方法及系统。


背景技术：

2.智能驾驶车辆是一种通过车载传感器感知道路环境、确定自身位置，并自行规划行驶路线抵达预设目的地的智能汽车，而确定自身位置对于规划行驶路线和控制车辆的转向和速度极其重要，是智能驾驶实时定位技术的主要目的。精度是定位技术的主要指标之一，现有方案通过基于多传感器进行融合定位。但是，由于多个传感器的输出存在发送和接收的不确定时间延迟，因此在进行多个传感器的定位数据融合时，如何解决多传感器输入时间不同步成为本领域亟待解决的问题。


技术实现要素：

3.本发明实施例提供一种基于多传感器融合定位的时间同步方法及系统，其能够解决融合定位时多传感器输入时间不同步的问题，提高融合定位精度。
4.第一方面，本发明实施例提供一种基于多传感器融合定位的时间同步方法，包括：
5.根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列；
6.接收所述车辆的多个传感器发送的第二定位信息；
7.在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息；
8.利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵；
9.根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
10.作为上述方案的改进，所述根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新，包括：
11.将所述雅可比矩阵对齐到当前时刻，利用所述卡尔曼滤波算法进行测量更新，得到当前时刻的定位误差；
12.根据所述定位误差对当前时刻的第一定位信息进行校正。
13.作为上述方案的改进，所述根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列，包括：
14.以在设定历史时刻下的第一定位信息为初始值，利用所述惯性测量单元输出的加速度和角速度进行积分运算，得到若干个连续时刻的第一定位信息；
15.将积分得到的第一定位信息按照时间先后顺序进行排序，得到所述递推队列。
16.作为上述方案的改进，所述接收所述车辆的多个传感器发送的第二定位信息之后，还包括：
17.按照所述第二定位信息对应的发送时刻的先后顺序，对所述第二定位信息进行排序，得到第一历史定位信息队列；
18.对所述第一历史定位队列进行截取，得到从所述设定历史时刻到当前时刻的第二历史定位队列。
19.作为上述方案的改进，所述第二定位信息携带有接收时刻和发送时刻；
20.则，所述在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息，包括：
21.在每个接收时刻，将截取出的第二历史地位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐，得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息。
22.作为上述方案的改进，利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵，包括：
23.利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到各个时刻的位置方差对时间的导数；
24.根据各个时刻的位置方差对时间的导数，得到从设定历史时刻到当前时刻的雅可比矩阵。
25.作为上述方案的改进，所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数，每一列表示同一时刻不同传感器的位置方差对时间的导数；
26.则，所述方法还包括：
27.当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时，将所述雅可比矩阵中第i个所述传感器对应的导数进行调低；
28.其中，j∈[t0，tk]，t0表示设定历史时刻，tk表示当前时刻。
[0029]
作为上述方案的改进，所述传感器包括：gnss传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
[0030]
第二方面，本发明实施例提供了一种基于多传感器融合定位的时间同步系统，包括：
[0031]
惯性基高频定位模块，用于根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列；
[0032]
传感器信息处理模块，用于接收所述车辆的多个传感器发送的第二定位信息；
[0033]
定位数据校正模块，用于在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息；
[0034]
所述定位数据校正模块，还用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵；
[0035]
时间同步递推模块，用于根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
[0036]
作为上述方案的改进，所述传感器信息处理模块包括：多传感器历史定位数据队列生成器、历史数据固定区间截取器；
[0037]
多传感器历史定位数据队列生成器，用于按照所述第二定位信息对应的发送时刻的先后顺序，对所述第二定位信息进行排序，得到第一历史定位信息队列；
[0038]
历史数据固定区间截取器，用于对所述第一历史定位队列进行截取，得到从所述设定历史时刻到当前时刻的第二历史定位队列。
[0039]
相对于现有技术，本发明实施例的有益效果在于：根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列；在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息；然后利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵；并根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。本发明实施例通过在接收到每个传感器的第二定位信息的接收时刻，将其发送时刻与递推队列进行时间对齐，避免各个传感器发送和接收的不确定性时间延迟，解决融合定位时多传感器输入时间不同步的问题，然后再进行卡尔曼滤波的预测和测量更新，校正当前时刻的第一定位信息，提高融合定位精度。
附图说明
[0040]
为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0041]
图1是本发明实施例提供的一种基于多传感器融合定位的时间同步方法的流程图；
[0042]
图2是本发明实施例提供的递推队列对齐示意图；
[0043]
图3是本发明实施例提供的一种基于多传感器融合定位的时间同步系统的示意图。
具体实施方式
[0044]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0045]
实施例一
[0046]
请参阅图1，本发明实施例提供一种基于多传感器融合定位的时间同步方法，包括：
[0047]
s11：根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列；
[0048]
进一步，所述根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列，包括：
[0049]
以在设定历史时刻下的第一定位信息为初始值，利用所述惯性测量单元输出的加速度和角速度进行积分运算，得到若干个连续时刻的第一定位信息；
[0050]
将积分得到的第一定位信息按照时间先后顺序进行排序，得到所述递推队列。
[0051]
示例性的，惯性测量单元imu是基于检测到的加速度和角速度按照时间顺序对车辆的位置进行递推，具有高频定位、实时性高的特点。例如根据加速度和角速度，确定位置增量、姿态增量和速度增量，并根据确定出的位置增量、姿态增量和速度增量作为高频定位增量，基于上一时刻的第一定位信息和高频定位增量，可以递推出当前时刻的第一定位信息，以此类推，可以得到设定时间区间内各个时刻的第一定位信息，并形成按照时间先后排序的递推队列。
[0052]
s12：接收所述车辆的多个传感器发送的第二定位信息；
[0053]
具体的，所述传感器包括：gnss传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
[0054]
由于惯性测量单元递推出的第一定位信息存在定位精度随时间累计、定位信息存在误差的问题，本发明实例将车辆内其余传感器(例如gnss传感器、轮速脉冲传感器wheel speed、视觉传感器vision、激光雷达传感器lidar以及毫米波雷达传感器radar)对惯性测量单元的定位数据进行校正，以获得提高定位精度。
[0055]
s13：在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息；
[0056]
进一步，所述第二定位信息携带有接收时刻和发送时刻；
[0057]
其中，每个所述第二定位信息携带有对应传感器发送该第二定位数据的发送时刻和系统接收到该第二定位信息的接收时刻。一般来说，每个所述第二定位信息或所述第一定位信息包括车辆的位置、速度、姿态中至少一种，即可以将各个传感器的输出第二定位信息和imu输出的第一定位信息转换包括位置、速度、姿态三个元素的矩阵，例如x＝[u，v，s]，u表示位置、v表示速度，s表示姿态，若对应传感器无法检测到位置、速度、姿态中的其中一种信息时，可将无法检测到的信息设为一定值，例如0，例如x＝[u，v，0]。
[0058]
则，所述在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息，包括：
[0059]
在每个接收时刻，将截取出的第二历史定位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐，得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息；如图2所示。以发送时刻为基准，将所述第一定位信息和所述定位信息进行时间对齐，例如，接收到第二定位信息x1、x2、x3、x4、x5、x6，对应的传感器的发送时刻为t1、t2、t3、t4、t5、t6；则搜索出t1时刻对应的第二定位信息和对应时刻的第一定位信息，t2、t3、t4、t5、t6时刻同理，由此，可以得到t1、t2、t3、t4、t5、t6时刻下第二定位信息和第一定位信息，实现第二定位信息与递推队列的对齐。
[0060]
s14：利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵；
[0061]
s15：根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息
进行测量更新。
[0062]
本发明实施例将基于惯性测量单元输出的第一定位信息的递推队列与其余传感器的第二定位信息进行时间对齐，具体在每个第二定位信息的接收时刻，将对应发送时刻的第二定位信息与递推队列的第一定位信息对齐，然后基于对齐后的第一定位信息和第二定位信息，利用卡尔曼滤波算法进行预测解算，得到雅可比矩阵，根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新，校正当前时刻的第一定位信息，从而解决了由于各个传感器存在发送和接收的不确定性时间延迟，导致卡尔曼滤波器收到的传感器的第二定位信息总是在历史时刻，融合定位时多传感器输入时间不同步，无法用于当前时刻的融合定位输出的问题，提高融合定位精度。
[0063]
在一种可选的实施例中，所述利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵，包括：
[0064]
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到各个时刻的位置方差对时间的导数；
[0065]
根据各个时刻的位置方差对时间的导数，得到从设定历史时刻到当前时刻的位置偏差雅施可比矩阵。
[0066]
进一步的，所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数，每一列表示同一时刻不同传感器的位置方差对时间的导数；
[0067]
在一种可选的实例中，所述根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新，包括：
[0068]
将所述雅可比矩阵对齐到当前时刻，利用所述卡尔曼滤波算法进行测量更新，得到当前时刻的定位误差；
[0069]
根据所述定位误差对当前时刻的第一定位信息进行校正。
[0070]
为了更清楚的说明本实施例，下面对卡尔曼滤波的工作原理进行说明，具体如下：
[0071]
定位预测：x
′
n 1
＝axn bu；
[0072]
协方差预测：p
′
n 1
＝fp
nft
q；
[0073]
卡尔曼增益计算：s＝p
′
n 1
h(hp
′
n 1ht
r)-1
；
[0074]
定位更新：x
n 1
＝x
′
n 1
s(z
k-h
×
x
′
n 1
)；
[0075]
协方差更新：p
n 1
＝(1-sh)p
′
n 1
；
[0076]
其中，x
′
n 1
表示n 1时刻卡尔曼滤波预测更新得到的预测定位信息；xn表示n时刻卡尔曼滤波测量更新得到的校正后的第一定位信息；x
n 1
表示n 1时刻卡尔曼滤波测量更新得到的校正后的第一定位信息；a表示状态转移矩阵；b表示控制矩阵；u表示控制状态量；p
′
n 1
表示n 1时刻卡尔曼滤波预测更新得到的协方差预测值；p
n 1
表示n 1时刻卡尔曼滤波测量更新得到的协方差；pn表示n时刻卡尔曼滤波测量更新得到的协方差；q表示过程协方差；s表示卡尔曼增益；h表示测量矩阵，h
t
表示h的转置；r表示测量协方差；zk表示测量向量(可以采用imu输出的初始定位信息作为测量向量)；f表示雅可比矩阵，f
t
表示f的转置。示例性的，可根据所述第二定位信息，生成所述测量矩阵h，并通过计算第二定位信息的协方差，得到测量协方差r。
[0077]
初始化卡尔曼滤波器：可以将设定时间区间的设定历史时刻对应的第一定位信息对卡尔曼滤波器的变量和预测更新方程和测量更新方程进行初始化；
[0078]
卡尔曼滤波器的迭代：每个迭代获取校正后的第一定位信息x
n 1
，和对应的误差协方差p
k 1
。每个迭代周期包含预测更新和测量更新；
[0079]
预测更新：根据上一时刻的校正后的第一定位信息xn，用预测更新方程估计当前时刻的预测定位信息x
′
n 1
，以及根据上一时刻的测量误差协方差pk预测当前时刻的预测状态误差协方差p
′
n 1
。
[0080]
测量更新：基于车辆的当前时刻的测量位姿，确定测量矩阵h和测量协方差r，根据测量更新方程更新状态x
′
n 1
→
x
n 1
和状态误差协方差p
′
n 1
→
p
n 1
。
[0081]
通过不断迭代预测更新和测量更新，当前时刻随着时间的流逝，变成上一时刻，重新去估计新的当前时刻。
[0082]
在本发明实施例中，在第二定位信息与递推队列和对齐后，在卡尔曼滤波预测解算阶段，通过在每个发送时刻，计算第一定位信息和第二定位信息的位置方差及位置方差对时间的导数，得到雅可比矩阵。其中，雅可比矩阵表示每个传感器对待估计的位置、速度、姿态的权重关联关系，即各个传感器对惯性测量单元定位的校正影响程度。利用卡尔曼滤波器融合车辆多个传感器的定位数据，得到当前时刻的融合定位输出，可实现高容错、高稳定性的定位效果。
[0083]
在一种可选的实施例中，所述接收所述车辆的多个传感器发送的第二定位信息之后，还包括：
[0084]
按照所述第二定位信息对应的发送时刻的先后顺序，对所述第二定位信息进行排序，得到第一历史定位信息队列；
[0085]
对所述第一历史定位队列进行截取，得到从所述设定历史时刻到当前时刻的第二历史定位队列。
[0086]
示例性的，可截取[t0，tk]时间段内的第二定位信息，得到所述第二历史定位信息，其中，t0表示设定历史时刻，tk表示当前时刻。其中，所述第二历史定位队列中的数据量不小于500，即所述第二历史定位队列至少包括500个第二定位信息。
[0087]
在一种可选的实施例中，所述方法还包括：
[0088]
当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时，将所述雅可比矩阵中第i个所述传感器对应的导数进行调低；
[0089]
其中，j∈[t0，tk]，t0表示设定历史时刻，tk表示当前时刻。
[0090]
在本发明实施例中，当第二定位信息与第一定位信息的偏差大于预设偏差阈值时，可认为对应传感器出现误报情况，因此将雅可比矩阵中对应的权重值调低，即雅可比矩阵中该传感器对应的导数进行调低，从而实现对卡尔曼滤波器的错误输入进行概率稀释，降低定位被某一个传感器带偏的概率，可实现高容错、高稳定性的定位效果。
[0091]
相对于现有技术，本发明实施例的有益效果在于：
[0092]
1、本发明实施例通过在接收到每个传感器的第二定位信息的接收时刻，将其发送时刻与递推队列进行时间对齐，避免各个传感器发送和接收的不确定性时间延迟，解决融合定位时多传感器输入时间不同步的问题，然后再进行卡尔曼滤波的预测和测量更新，校正当前时刻的第一定位信息，提高融合定位精度；
[0093]
2、当传感器出现误报情况，通过将雅可比矩阵中对应的权重值调低，即雅可比矩阵中该传感器对应的导数进行调低，从而实现对卡尔曼滤波器的错误输入进行概率稀释，
降低定位被某一个传感器带偏的概率，可实现高容错、高稳定性的定位效果。
[0094]
实施例二
[0095]
请参阅图3，本发明实施例提供了一种基于多传感器融合定位的时间同步系统，包括：
[0096]
惯性基高频定位模块1，用于根据惯性测量单元输出的加速度和角速度，递推出车辆在多个时刻的第一定位信息，并生成递推队列；
[0097]
传感器信息处理模块2，用于接收所述车辆的多个传感器发送的第二定位信息；
[0098]
定位数据校正模块3，用于在每个所述第二定位信息的接收时刻，按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐，得到处于同一时刻下的第一定位信息和第二定位信息；
[0099]
所述定位数据校正模块3，还用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到雅可比矩阵；
[0100]
时间同步递推模块4，用于根据所述雅可比矩阵，利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
[0101]
在一种可选的实施例中，所述传感器信息处理模块2包括：多传感器历史定位数据队列生成器21、历史数据固定区间截取器22；
[0102]
多传感器历史定位数据队列生成器21，用于按照所述第二定位信息对应的发送时刻的先后顺序，对所述第二定位信息进行排序，得到第一历史定位信息队列；
[0103]
历史数据固定区间截取器22，用于对所述第一历史定位队列进行截取，得到从所述设定历史时刻到当前时刻的第二历史定位队列。
[0104]
在一种可选的实施例中，所述定位数据校正模块3包括误差计算单元、校正单元；
[0105]
所述误差计算单元，用于将所述雅可比矩阵对齐到当前时刻，利用所述卡尔曼滤波算法进行测量更新，得到当前时刻的定位误差；
[0106]
所述校正单元，用于根据所述定位误差对当前时刻的第一定位信息进行校正。
[0107]
在一种可选的实施例中，所述惯性基高频定位模1包括：定位信息递推单元、递推队列生成单元；
[0108]
所述定位信息递推单元，用于以在设定历史时刻下的第一定位信息为初始值，利用所述惯性测量单元输出的加速度和角速度进行积分运算，得到若干个连续时刻的第一定位信息；
[0109]
所述递推队列生成单元，用于将积分得到的第一定位信息按照时间先后顺序进行排序，得到所述递推队列。
[0110]
在一种可选的实施例中，所述第二定位信息携带有接收时刻和发送时刻；
[0111]
则，所述定位数据校正模块3，用于在每个接收时刻，将截取出的第二历史定位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐，得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息。
[0112]
在一种可选的实施例中，所述定位数据校正模块3包括导数计算单元、雅可比矩阵生成单元；
[0113]
导数计算单元，用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算，得到各个时刻的位置方差对时间的
导数；
[0114]
雅可比矩阵生成单元，用于根据各个时刻的位置方差对时间的导数，得到从设定历史时刻到当前时刻的雅可比矩阵。
[0115]
在一种可选的实施例中，所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数，每一列表示同一时刻不同传感器的位置方差对时间的导数；
[0116]
则，所述系统还包括：
[0117]
雅克比矩阵调整模块，用于当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时，将所述雅可比矩阵中第i个所述传感器对应的导数进行调低；
[0118]
其中，j∈[t0，tk]，t0表示设定历史时刻，tk表示当前时刻。
[0119]
在一种可选的实施例中，所述传感器包括：gnss传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
[0120]
可以理解的是，上述的所述融合定位装置的实施例还可以对应参考上述的所述融合定位方法的相关实施例的内容，在此不再做赘述。
[0121]
需说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0122]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。