CN114413885A - 基于多传感器融合定位的时间同步方法及系统 - Google Patents
基于多传感器融合定位的时间同步方法及系统 Download PDFInfo
- Publication number
- CN114413885A CN114413885A CN202111581160.6A CN202111581160A CN114413885A CN 114413885 A CN114413885 A CN 114413885A CN 202111581160 A CN202111581160 A CN 202111581160A CN 114413885 A CN114413885 A CN 114413885A
- Authority
- CN
- China
- Prior art keywords
- positioning information
- positioning
- time
- moment
- queue
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 34
- 238000000034 method Methods 0.000 title claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims abstract description 56
- 238000005259 measurement Methods 0.000 claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 37
- 230000001133 acceleration Effects 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000012937 correction Methods 0.000 claims description 11
- 238000012163 sequencing technique Methods 0.000 claims description 9
- 230000010365 information processing Effects 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 4
- 230000004807 localization Effects 0.000 claims 1
- 239000000243 solution Substances 0.000 description 7
- 230000005540 biological transmission Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000010790 dilution Methods 0.000 description 2
- 239000012895 dilution Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/166—Mechanical, construction or arrangement details of inertial navigation systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于多传感器融合定位的时间同步方法及系统,该方法包括:根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,生成递推队列;在每个第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;根据雅可比矩阵,利用卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新;本发明能有效解决融合定位时多传感器输入时间不同步的问题,提高融合定位精度。
Description
技术领域
本发明涉及无线定位技术领域,尤其涉及一种基于多传感器融合定位的时间同步方法及系统。
背景技术
智能驾驶车辆是一种通过车载传感器感知道路环境、确定自身位置,并自行规划行驶路线抵达预设目的地的智能汽车,而确定自身位置对于规划行驶路线和控制车辆的转向和速度极其重要,是智能驾驶实时定位技术的主要目的。精度是定位技术的主要指标之一,现有方案通过基于多传感器进行融合定位。但是,由于多个传感器的输出存在发送和接收的不确定时间延迟,因此在进行多个传感器的定位数据融合时,如何解决多传感器输入时间不同步成为本领域亟待解决的问题。
发明内容
本发明实施例提供一种基于多传感器融合定位的时间同步方法及系统,其能够解决融合定位时多传感器输入时间不同步的问题,提高融合定位精度。
第一方面,本发明实施例提供一种基于多传感器融合定位的时间同步方法,包括:
根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
接收所述车辆的多个传感器发送的第二定位信息;
在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
作为上述方案的改进,所述根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新,包括:
将所述雅可比矩阵对齐到当前时刻,利用所述卡尔曼滤波算法进行测量更新,得到当前时刻的定位误差;
根据所述定位误差对当前时刻的第一定位信息进行校正。
作为上述方案的改进,所述根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列,包括:
以在设定历史时刻下的第一定位信息为初始值,利用所述惯性测量单元输出的加速度和角速度进行积分运算,得到若干个连续时刻的第一定位信息;
将积分得到的第一定位信息按照时间先后顺序进行排序,得到所述递推队列。
作为上述方案的改进,所述接收所述车辆的多个传感器发送的第二定位信息之后,还包括:
按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
作为上述方案的改进,所述第二定位信息携带有接收时刻和发送时刻;
则,所述在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息,包括:
在每个接收时刻,将截取出的第二历史地位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐,得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息。
作为上述方案的改进,利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵,包括:
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到各个时刻的位置方差对时间的导数;
根据各个时刻的位置方差对时间的导数,得到从设定历史时刻到当前时刻的雅可比矩阵。
作为上述方案的改进,所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数,每一列表示同一时刻不同传感器的位置方差对时间的导数;
则,所述方法还包括:
当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时,将所述雅可比矩阵中第i个所述传感器对应的导数进行调低;
其中,j∈[t0,tk],t0表示设定历史时刻,tk表示当前时刻。
作为上述方案的改进,所述传感器包括:GNSS传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
第二方面,本发明实施例提供了一种基于多传感器融合定位的时间同步系统,包括:
惯性基高频定位模块,用于根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
传感器信息处理模块,用于接收所述车辆的多个传感器发送的第二定位信息;
定位数据校正模块,用于在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
所述定位数据校正模块,还用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
时间同步递推模块,用于根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
作为上述方案的改进,所述传感器信息处理模块包括:多传感器历史定位数据队列生成器、历史数据固定区间截取器;
多传感器历史定位数据队列生成器,用于按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
历史数据固定区间截取器,用于对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
相对于现有技术,本发明实施例的有益效果在于:根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;然后利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;并根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。本发明实施例通过在接收到每个传感器的第二定位信息的接收时刻,将其发送时刻与递推队列进行时间对齐,避免各个传感器发送和接收的不确定性时间延迟,解决融合定位时多传感器输入时间不同步的问题,然后再进行卡尔曼滤波的预测和测量更新,校正当前时刻的第一定位信息,提高融合定位精度。
附图说明
为了更清楚地说明本发明的技术方案,下面将对实施方式中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施方式,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种基于多传感器融合定位的时间同步方法的流程图;
图2是本发明实施例提供的递推队列对齐示意图;
图3是本发明实施例提供的一种基于多传感器融合定位的时间同步系统的示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例一
请参阅图1,本发明实施例提供一种基于多传感器融合定位的时间同步方法,包括:
S11:根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
进一步,所述根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列,包括:
以在设定历史时刻下的第一定位信息为初始值,利用所述惯性测量单元输出的加速度和角速度进行积分运算,得到若干个连续时刻的第一定位信息;
将积分得到的第一定位信息按照时间先后顺序进行排序,得到所述递推队列。
示例性的,惯性测量单元IMU是基于检测到的加速度和角速度按照时间顺序对车辆的位置进行递推,具有高频定位、实时性高的特点。例如根据加速度和角速度,确定位置增量、姿态增量和速度增量,并根据确定出的位置增量、姿态增量和速度增量作为高频定位增量,基于上一时刻的第一定位信息和高频定位增量,可以递推出当前时刻的第一定位信息,以此类推,可以得到设定时间区间内各个时刻的第一定位信息,并形成按照时间先后排序的递推队列。
S12:接收所述车辆的多个传感器发送的第二定位信息;
具体的,所述传感器包括:GNSS传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
由于惯性测量单元递推出的第一定位信息存在定位精度随时间累计、定位信息存在误差的问题,本发明实例将车辆内其余传感器(例如GNSS传感器、轮速脉冲传感器Wheelspeed、视觉传感器Vision、激光雷达传感器Lidar以及毫米波雷达传感器Radar)对惯性测量单元的定位数据进行校正,以获得提高定位精度。
S13:在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
进一步,所述第二定位信息携带有接收时刻和发送时刻;
其中,每个所述第二定位信息携带有对应传感器发送该第二定位数据的发送时刻和系统接收到该第二定位信息的接收时刻。一般来说,每个所述第二定位信息或所述第一定位信息包括车辆的位置、速度、姿态中至少一种,即可以将各个传感器的输出第二定位信息和IMU输出的第一定位信息转换包括位置、速度、姿态三个元素的矩阵,例如X=[u,v,s],u表示位置、v表示速度,s表示姿态,若对应传感器无法检测到位置、速度、姿态中的其中一种信息时,可将无法检测到的信息设为一定值,例如0,例如X=[u,v,0]。
则,所述在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息,包括:
在每个接收时刻,将截取出的第二历史定位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐,得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息;如图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时刻下第二定位信息和第一定位信息,实现第二定位信息与递推队列的对齐。
S14:利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
S15:根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
本发明实施例将基于惯性测量单元输出的第一定位信息的递推队列与其余传感器的第二定位信息进行时间对齐,具体在每个第二定位信息的接收时刻,将对应发送时刻的第二定位信息与递推队列的第一定位信息对齐,然后基于对齐后的第一定位信息和第二定位信息,利用卡尔曼滤波算法进行预测解算,得到雅可比矩阵,根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新,校正当前时刻的第一定位信息,从而解决了由于各个传感器存在发送和接收的不确定性时间延迟,导致卡尔曼滤波器收到的传感器的第二定位信息总是在历史时刻,融合定位时多传感器输入时间不同步,无法用于当前时刻的融合定位输出的问题,提高融合定位精度。
在一种可选的实施例中,所述利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵,包括:
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到各个时刻的位置方差对时间的导数;
根据各个时刻的位置方差对时间的导数,得到从设定历史时刻到当前时刻的位置偏差雅施可比矩阵。
进一步的,所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数,每一列表示同一时刻不同传感器的位置方差对时间的导数;
在一种可选的实例中,所述根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新,包括:
将所述雅可比矩阵对齐到当前时刻,利用所述卡尔曼滤波算法进行测量更新,得到当前时刻的定位误差;
根据所述定位误差对当前时刻的第一定位信息进行校正。
为了更清楚的说明本实施例,下面对卡尔曼滤波的工作原理进行说明,具体如下:
定位预测:X′n+1=AXn+BU;
协方差预测:P′n+1=FPnFT+Q;
卡尔曼增益计算:S=P′n+1H(HP′n+1HT+R)-1;
定位更新:Xn+1=X′n+1+S(Zk-H×X′n+1);
协方差更新:Pn+1=(1-SH)P′n+1;
其中,X′n+1表示n+1时刻卡尔曼滤波预测更新得到的预测定位信息;Xn表示n时刻卡尔曼滤波测量更新得到的校正后的第一定位信息;Xn+1表示n+1时刻卡尔曼滤波测量更新得到的校正后的第一定位信息;A表示状态转移矩阵;B表示控制矩阵;U表示控制状态量;P′n+1表示n+1时刻卡尔曼滤波预测更新得到的协方差预测值;Pn+1表示n+1时刻卡尔曼滤波测量更新得到的协方差;Pn表示n时刻卡尔曼滤波测量更新得到的协方差;Q表示过程协方差;S表示卡尔曼增益;H表示测量矩阵,HT表示H的转置;R表示测量协方差;Zk表示测量向量(可以采用IMU输出的初始定位信息作为测量向量);F表示雅可比矩阵,FT表示F的转置。示例性的,可根据所述第二定位信息,生成所述测量矩阵H,并通过计算第二定位信息的协方差,得到测量协方差R。
初始化卡尔曼滤波器:可以将设定时间区间的设定历史时刻对应的第一定位信息对卡尔曼滤波器的变量和预测更新方程和测量更新方程进行初始化;
卡尔曼滤波器的迭代:每个迭代获取校正后的第一定位信息Xn+1,和对应的误差协方差Pk+1。每个迭代周期包含预测更新和测量更新;
预测更新:根据上一时刻的校正后的第一定位信息Xn,用预测更新方程估计当前时刻的预测定位信息X′n+1,以及根据上一时刻的测量误差协方差Pk预测当前时刻的预测状态误差协方差P′n+1。
测量更新:基于车辆的当前时刻的测量位姿,确定测量矩阵H和测量协方差R,根据测量更新方程更新状态X′n+1→Xn+1和状态误差协方差P′n+1→Pn+1。
通过不断迭代预测更新和测量更新,当前时刻随着时间的流逝,变成上一时刻,重新去估计新的当前时刻。
在本发明实施例中,在第二定位信息与递推队列和对齐后,在卡尔曼滤波预测解算阶段,通过在每个发送时刻,计算第一定位信息和第二定位信息的位置方差及位置方差对时间的导数,得到雅可比矩阵。其中,雅可比矩阵表示每个传感器对待估计的位置、速度、姿态的权重关联关系,即各个传感器对惯性测量单元定位的校正影响程度。利用卡尔曼滤波器融合车辆多个传感器的定位数据,得到当前时刻的融合定位输出,可实现高容错、高稳定性的定位效果。
在一种可选的实施例中,所述接收所述车辆的多个传感器发送的第二定位信息之后,还包括:
按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
示例性的,可截取[t0,tk]时间段内的第二定位信息,得到所述第二历史定位信息,其中,t0表示设定历史时刻,tk表示当前时刻。其中,所述第二历史定位队列中的数据量不小于500,即所述第二历史定位队列至少包括500个第二定位信息。
在一种可选的实施例中,所述方法还包括:
当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时,将所述雅可比矩阵中第i个所述传感器对应的导数进行调低;
其中,j∈[t0,tk],t0表示设定历史时刻,tk表示当前时刻。
在本发明实施例中,当第二定位信息与第一定位信息的偏差大于预设偏差阈值时,可认为对应传感器出现误报情况,因此将雅可比矩阵中对应的权重值调低,即雅可比矩阵中该传感器对应的导数进行调低,从而实现对卡尔曼滤波器的错误输入进行概率稀释,降低定位被某一个传感器带偏的概率,可实现高容错、高稳定性的定位效果。
相对于现有技术,本发明实施例的有益效果在于:
1、本发明实施例通过在接收到每个传感器的第二定位信息的接收时刻,将其发送时刻与递推队列进行时间对齐,避免各个传感器发送和接收的不确定性时间延迟,解决融合定位时多传感器输入时间不同步的问题,然后再进行卡尔曼滤波的预测和测量更新,校正当前时刻的第一定位信息,提高融合定位精度;
2、当传感器出现误报情况,通过将雅可比矩阵中对应的权重值调低,即雅可比矩阵中该传感器对应的导数进行调低,从而实现对卡尔曼滤波器的错误输入进行概率稀释,降低定位被某一个传感器带偏的概率,可实现高容错、高稳定性的定位效果。
实施例二
请参阅图3,本发明实施例提供了一种基于多传感器融合定位的时间同步系统,包括:
惯性基高频定位模块1,用于根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
传感器信息处理模块2,用于接收所述车辆的多个传感器发送的第二定位信息;
定位数据校正模块3,用于在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
所述定位数据校正模块3,还用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
时间同步递推模块4,用于根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
在一种可选的实施例中,所述传感器信息处理模块2包括:多传感器历史定位数据队列生成器21、历史数据固定区间截取器22;
多传感器历史定位数据队列生成器21,用于按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
历史数据固定区间截取器22,用于对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
在一种可选的实施例中,所述定位数据校正模块3包括误差计算单元、校正单元;
所述误差计算单元,用于将所述雅可比矩阵对齐到当前时刻,利用所述卡尔曼滤波算法进行测量更新,得到当前时刻的定位误差;
所述校正单元,用于根据所述定位误差对当前时刻的第一定位信息进行校正。
在一种可选的实施例中,所述惯性基高频定位模1包括:定位信息递推单元、递推队列生成单元;
所述定位信息递推单元,用于以在设定历史时刻下的第一定位信息为初始值,利用所述惯性测量单元输出的加速度和角速度进行积分运算,得到若干个连续时刻的第一定位信息;
所述递推队列生成单元,用于将积分得到的第一定位信息按照时间先后顺序进行排序,得到所述递推队列。
在一种可选的实施例中,所述第二定位信息携带有接收时刻和发送时刻;
则,所述定位数据校正模块3,用于在每个接收时刻,将截取出的第二历史定位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐,得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息。
在一种可选的实施例中,所述定位数据校正模块3包括导数计算单元、雅可比矩阵生成单元;
导数计算单元,用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到各个时刻的位置方差对时间的导数;
雅可比矩阵生成单元,用于根据各个时刻的位置方差对时间的导数,得到从设定历史时刻到当前时刻的雅可比矩阵。
在一种可选的实施例中,所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数,每一列表示同一时刻不同传感器的位置方差对时间的导数;
则,所述系统还包括:
雅克比矩阵调整模块,用于当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时,将所述雅可比矩阵中第i个所述传感器对应的导数进行调低;
其中,j∈[t0,tk],t0表示设定历史时刻,tk表示当前时刻。
在一种可选的实施例中,所述传感器包括:GNSS传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
可以理解的是,上述的所述融合定位装置的实施例还可以对应参考上述的所述融合定位方法的相关实施例的内容,在此不再做赘述。
需说明的是,以上所描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外,本发明提供的装置实施例附图中,模块之间的连接关系表示它们之间具有通信连接,具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
以上所述是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (10)
1.一种基于多传感器融合定位的时间同步方法,其特征在于,包括:
根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
接收所述车辆的多个传感器发送的第二定位信息;
在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
2.如权利要求1所述的基于多传感器融合定位的时间同步方法,其特征在于,所述根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新,包括:
将所述雅可比矩阵对齐到当前时刻,利用所述卡尔曼滤波算法进行测量更新,得到当前时刻的定位误差;
根据所述定位误差对当前时刻的第一定位信息进行校正。
3.如权利要求1所述的基于多传感器融合定位的时间同步方法,其特征在于,所述根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列,包括:
以在设定历史时刻下的第一定位信息为初始值,利用所述惯性测量单元输出的加速度和角速度进行积分运算,得到若干个连续时刻的第一定位信息;
将积分得到的第一定位信息按照时间先后顺序进行排序,得到所述递推队列。
4.如权利要求1所述的基于多传感器融合定位的时间同步方法,其特征在于,所述接收所述车辆的多个传感器发送的第二定位信息之后,还包括:
按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
5.如权利要求4所述的基于多传感器融合定位的时间同步方法,其特征在于,所述第二定位信息携带有接收时刻和发送时刻;
则,所述在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息,包括:
在每个接收时刻,将截取出的第二历史地位队列中的各个第二定位信息按照其发送时刻与所述递推队列中的第一定位信息进行对齐,得到从所述设定历史时刻到当前时刻之间于各个时刻下的第一定位信息和第二定位信息。
6.如权利要求1所述的基于多传感器融合定位的时间同步方法,其特征在于,利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵,包括:
利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到各个时刻的位置方差对时间的导数;
根据各个时刻的位置方差对时间的导数,得到从设定历史时刻到当前时刻的雅可比矩阵。
7.如权利要求6所述的基于多传感器融合定位的时间同步方法,其特征在于,所述雅可比矩阵的每一行表示同一传感器的在不同时刻的位置方差对时间的导数,每一列表示同一时刻不同传感器的位置方差对时间的导数;
则,所述方法还包括:
当j时刻下的第i个所述传感器的第二定位信息与第一定位信息的偏差大于预设偏差阈值时,将所述雅可比矩阵中第i个所述传感器对应的导数进行调低;
其中,j∈[t0,tk],t0表示设定历史时刻,tk表示当前时刻。
8.如权利要求1所述的基于多传感器融合定位的时间同步方法,其特征在于,所述传感器包括:GNSS传感器、轮速脉冲传感器、视觉传感器、激光雷达传感器以及毫米波雷达传感器。
9.一种基于多传感器融合定位的时间同步系统,其特征在于,包括:
惯性基高频定位模块,用于根据惯性测量单元输出的加速度和角速度,递推出车辆在多个时刻的第一定位信息,并生成递推队列;
传感器信息处理模块,用于接收所述车辆的多个传感器发送的第二定位信息;
定位数据校正模块,用于在每个所述第二定位信息的接收时刻,按照其发送时刻将对应的第二定位信息与所述递推队列中的第一定位信息进行对齐,得到处于同一时刻下的第一定位信息和第二定位信息;
所述定位数据校正模块,还用于利用卡尔曼滤波算法对从设定历史时刻到当前时刻之间每个时刻下的第一定位信息和第二定位信息进行预测解算,得到雅可比矩阵;
时间同步递推模块,用于根据所述雅可比矩阵,利用所述卡尔曼滤波算法对当前时刻的第一定位信息进行测量更新。
10.如权利要求9所述的基于多传感器融合定位的时间同步系统,其特征在于,所述传感器信息处理模块包括:多传感器历史定位数据队列生成器、历史数据固定区间截取器;
多传感器历史定位数据队列生成器,用于按照所述第二定位信息对应的发送时刻的先后顺序,对所述第二定位信息进行排序,得到第一历史定位信息队列;
历史数据固定区间截取器,用于对所述第一历史定位队列进行截取,得到从所述设定历史时刻到当前时刻的第二历史定位队列。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111581160.6A CN114413885B (zh) | 2021-12-22 | 2021-12-22 | 基于多传感器融合定位的时间同步方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111581160.6A CN114413885B (zh) | 2021-12-22 | 2021-12-22 | 基于多传感器融合定位的时间同步方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114413885A true CN114413885A (zh) | 2022-04-29 |
CN114413885B CN114413885B (zh) | 2024-05-24 |
Family
ID=81266710
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111581160.6A Active CN114413885B (zh) | 2021-12-22 | 2021-12-22 | 基于多传感器融合定位的时间同步方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114413885B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116707746A (zh) * | 2023-08-07 | 2023-09-05 | 长春航盛艾思科电子有限公司 | 一种阵列传感器的测量方法 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7605747B1 (en) * | 2006-08-14 | 2009-10-20 | Lockheed Martin Corporation | Method for compensating for the positional errors of a sensor |
US7626534B1 (en) * | 2007-06-12 | 2009-12-01 | Lockheed Martin Corporation | Unified navigation and inertial target tracking estimation system |
CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
CN108535755A (zh) * | 2018-01-17 | 2018-09-14 | 南昌大学 | 基于mems的gnss/imu车载实时组合导航方法 |
CN110515381A (zh) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | 用于定位机器人的多传感器融合算法 |
CN111721299A (zh) * | 2020-06-30 | 2020-09-29 | 上海汽车集团股份有限公司 | 一种实时定位时间同步方法和装置 |
CN112097771A (zh) * | 2020-08-18 | 2020-12-18 | 青岛职业技术学院 | Gps延迟时间自适应的扩展卡尔曼滤波导航算法 |
CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
US20210041239A1 (en) * | 2018-06-01 | 2021-02-11 | Zhejiang Yat Electrical Appliance Co., Ltd | Integrated navigation method for mobile vehicle |
DE102020101837A1 (de) * | 2020-01-27 | 2021-07-29 | Bayerische Motoren Werke Aktiengesellschaft | Verfahren und Vorrichtung zur Ermittlung des Zustands eines Fahrzeugs |
CN113295161A (zh) * | 2020-02-24 | 2021-08-24 | 中国航天科工飞航技术研究院(中国航天海鹰机电技术研究院) | 一种多源信息融合的超高速磁悬浮列车车载测速定位装置 |
-
2021
- 2021-12-22 CN CN202111581160.6A patent/CN114413885B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7605747B1 (en) * | 2006-08-14 | 2009-10-20 | Lockheed Martin Corporation | Method for compensating for the positional errors of a sensor |
US7626534B1 (en) * | 2007-06-12 | 2009-12-01 | Lockheed Martin Corporation | Unified navigation and inertial target tracking estimation system |
CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
CN108535755A (zh) * | 2018-01-17 | 2018-09-14 | 南昌大学 | 基于mems的gnss/imu车载实时组合导航方法 |
US20210041239A1 (en) * | 2018-06-01 | 2021-02-11 | Zhejiang Yat Electrical Appliance Co., Ltd | Integrated navigation method for mobile vehicle |
CN110515381A (zh) * | 2019-08-22 | 2019-11-29 | 浙江迈睿机器人有限公司 | 用于定位机器人的多传感器融合算法 |
DE102020101837A1 (de) * | 2020-01-27 | 2021-07-29 | Bayerische Motoren Werke Aktiengesellschaft | Verfahren und Vorrichtung zur Ermittlung des Zustands eines Fahrzeugs |
CN113295161A (zh) * | 2020-02-24 | 2021-08-24 | 中国航天科工飞航技术研究院(中国航天海鹰机电技术研究院) | 一种多源信息融合的超高速磁悬浮列车车载测速定位装置 |
CN111721299A (zh) * | 2020-06-30 | 2020-09-29 | 上海汽车集团股份有限公司 | 一种实时定位时间同步方法和装置 |
CN112097771A (zh) * | 2020-08-18 | 2020-12-18 | 青岛职业技术学院 | Gps延迟时间自适应的扩展卡尔曼滤波导航算法 |
CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
Non-Patent Citations (4)
Title |
---|
YACHENG XIAO 等: "Nonlinear wireless time synchronization based on variational Bayesian adaptive filtering", 《2021 IEEE INTERNATIONAL CONFERENCE ON SIGNAL PROCESSING, COMMUNICATIONS AND COMPUTING (ICSPCC)》, 25 October 2021 (2021-10-25), pages 1 - 6 * |
凌祥 等: "脉冲耦合振荡器实现传感器网络时间同步", 《宇航学报》, vol. 30, no. 3, 31 May 2009 (2009-05-31), pages 1 - 6 * |
耿华;张涛;连小珉;杨殿阁;: "北斗卫星与车载传感器融合的车辆定位", 汽车工程, no. 11, 25 November 2007 (2007-11-25) * |
陈靖;王涌天;刘越;刘伟;郭俊伟;林精敦;: "适用于户外增强现实系统的混合跟踪定位算法", 计算机辅助设计与图形学学报, no. 02, 15 February 2010 (2010-02-15) * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116707746A (zh) * | 2023-08-07 | 2023-09-05 | 长春航盛艾思科电子有限公司 | 一种阵列传感器的测量方法 |
CN116707746B (zh) * | 2023-08-07 | 2024-03-22 | 长春航盛艾思科电子有限公司 | 一种阵列传感器的测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114413885B (zh) | 2024-05-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111721289B (zh) | 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆 | |
US10853669B2 (en) | Object recognition device, object recognition method and self-driving system | |
US7180443B1 (en) | Reduced state estimator for systems with physically bounded parameters | |
US10422658B2 (en) | Method, fusion filter, and system for fusing sensor signals with different temporal signal output delays into a fusion data set | |
EP3578919B1 (en) | Travel history storage method, method for producing travel path model, method for estimating local position, and travel history storage device | |
JP6677533B2 (ja) | 車載装置、及び、推定方法 | |
US20150134182A1 (en) | Position estimation and vehicle control in autonomous multi-vehicle convoys | |
NO166901B (no) | Navigasjonsfremgangsmaate for fartoey. | |
US20040176882A1 (en) | Attitude sensing apparatus for determining the attitude of a mobile unit | |
CN109059907A (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
CN114179825B (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
CN111721299B (zh) | 一种实时定位时间同步方法和装置 | |
CN114264301B (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
US20230396349A1 (en) | Time synchronization method and apparatus for vehicle-mounted device, vehicle-mounted device, and storage medium | |
CN114413885A (zh) | 基于多传感器融合定位的时间同步方法及系统 | |
KR20190001086A (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
CN116086445A (zh) | 一种基于因子图优化的多源信息时延融合导航方法 | |
JP2007333652A (ja) | 測位装置、ナビゲーションシステム | |
CN112833919B (zh) | 一种多余度的惯性测量数据的管理方法及其系统 | |
CN113532428B (zh) | 数据处理方法、装置、动中通终端和计算机可读存储介质 | |
KR101837821B1 (ko) | 다중구조 필터를 이용한 위치추정 방법 및 이의 시스템 | |
Dubois et al. | Performance evaluation of a moving horizon estimator for multi-rate sensor fusion with time-delayed measurements | |
CN111708008A (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
CN115683170A (zh) | 基于雷达点云数据融合误差的校准方法 | |
CN115327587A (zh) | 基于gnss定位信息的低轨卫星轨道误差修正方法及系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |