CN112762944A - 零速区间检测及零速更新方法 - Google Patents
零速区间检测及零速更新方法 Download PDFInfo
- Publication number
- CN112762944A CN112762944A CN202011563843.4A CN202011563843A CN112762944A CN 112762944 A CN112762944 A CN 112762944A CN 202011563843 A CN202011563843 A CN 202011563843A CN 112762944 A CN112762944 A CN 112762944A
- Authority
- CN
- China
- Prior art keywords
- time
- measurement unit
- zero
- vehicle
- speed
- 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
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000001514 detection method Methods 0.000 title claims abstract description 68
- 238000005259 measurement Methods 0.000 claims abstract description 196
- 230000004044 response Effects 0.000 claims abstract description 28
- 230000033001 locomotion Effects 0.000 claims abstract description 16
- 230000008859 change Effects 0.000 claims abstract description 11
- 238000001914 filtration Methods 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000003860 storage Methods 0.000 claims description 8
- 230000003068 static effect Effects 0.000 abstract description 21
- 238000010586 diagram Methods 0.000 description 12
- 230000001133 acceleration Effects 0.000 description 9
- 230000001360 synchronised effect Effects 0.000 description 9
- 238000012937 correction Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 238000005070 sampling Methods 0.000 description 7
- 230000005540 biological transmission Effects 0.000 description 6
- 238000004590 computer program Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 230000008054 signal transmission Effects 0.000 description 5
- 238000009825 accumulation Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 239000013598 vector Substances 0.000 description 4
- 238000011065 in-situ storage Methods 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000036961 partial effect Effects 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000035945 sensitivity 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
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
技术领域
本公开涉及驾驶技术领域,具体涉及一种零速区间检测及零速更新方法。
背景技术
惯性导航系统是一种自主导航系统,其原理是利用惯性元件来测量运载体本身的加速度,然后经过积分运算得到速度和位置,从而达到对运载体导航定位的目的。惯性导航系统作为如今驾驶领域最主流的定位方式,如何提高定位精度成为其重要的研究方向。
发明内容
为提高自主导航系统的定位精度,本公开实施方式提供了一种零速区间检测及零速更新方法、装置、自主导航系统、存储介质。
第一方面,本公开实施方式提供了一种零速区间检测方法,应用于自主导航系统,所述方法包括:
响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态;
响应于所述第一测量单元检测到第二信号,确定检测到所述第二信号的第二时刻;所述第二信号表示所述运载体由静止状态变化为移动状态;
确定所述第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻,包括:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
第二方面,本公开实施方式提供了一种零速更新方法,应用于自主导航系统,所述方法包括:
响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态,所述零速区间包括至少一个更新时刻;
针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,直至所述第一测量单元检测到第二信号;所述第二信号表示所述运载体由静止状态变化为移动状态。
在一些实施方式中,所述行驶数据包括观测数据和预测数据;其中,所述观测数据是预设传感器设备获取的;所述预测数据是根据所述运载体的惯性测量单元的检测数据得到。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻,包括:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,包括:
根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
根据所述状态误差对该更新时刻之前第三时间阈值的时刻的运载体的状态信息进行更新,得到更新状态信息;
根据所述更新状态信息,得到该更新时刻的运载体的状态信息。
在一些实施方式中,根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差,包括:
基于获取的行驶数据,采用卡尔曼滤波算法得到更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
根据所述更新状态信息,得到该更新时刻的运载体的状态信息,包括:
基于所述更新状态信息,积分计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述观测数据包括运载体的观测位置信息、观测速度信息以及观测航向信息,所述预测数据包括运载体的预测位置信息、预测速度信息以及预测航向信息;
根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,包括:
根据获取的观测数据和预测数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的位置信息误差、速度信息误差以及航向信息误差;
根据所述位置信息误差、速度信息误差以及航向信息误差,对该更新时刻之前第三时间阈值时刻的运载体状态进行更新,得到位置更新信息、速度更新信息以及航向更新信息;
基于所述位置更新信息、速度更新信息以及航向更新信息,计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述的方法还包括:
根据所述惯性测量单元的采样频率确定所述零速区间的至少一个更新时刻。
在一些实施方式中,所述运载体为车辆,所述第一测量单元为所述车辆的轮速计。
第三方面,本公开实施方式提供了一种零速区间检测装置,应用于自主导航系统,所述装置包括:
第一确定模块,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态;
第二确定模块,被配置为响应于所述第一测量单元检测到第二信号,确定检测到所述第二信号的第二时刻;所述第二信号表示所述运载体由静止状态变化为移动状态;
第三确定模块,被配置为确定所述第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第一确定模块具体被配置为:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
第四方面,本公开实施方式提供了一种零速更新装置,应用于自主导航系统,所述装置包括:
第四确定模块,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态,所述零速区间包括至少一个更新时刻;
零速更新模块,被配置为针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,直至所述第一测量单元检测到第二信号;所述第二信号表示所述运载体由静止状态变化为移动状态。
在一些实施方式中,所述行驶数据包括观测数据和预测数据;其中,所述观测数据是预设传感器设备获取的;所述预测数据是根据所述运载体的惯性测量单元的检测数据得到。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第四确定模块具体被配置为:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述零速更新模块包括:
第一误差确定子模块,被配置为根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
第一更新子模块,被配置为根据所述状态误差对该更新时刻之前第三时间阈值的时刻的运载体的状态信息进行更新,得到更新状态信息;
第一得到子模块,被配置为根据所述更新状态信息,得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述第一误差确定子模块具体被配置为:基于获取的行驶数据,采用卡尔曼滤波算法得到更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
所述第一得到子模块具体被配置为:基于所述更新状态信息,积分计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述观测数据包括运载体的观测位置信息、观测速度信息以及观测航向信息,所述预测数据包括运载体的预测位置信息、预测速度信息以及预测航向信息。
在一些实施方式中,所述零速更新模块包括:
第二误差确定子模块,被配置为根据获取的观测数据和预测数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的位置信息误差、速度信息误差以及航向信息误差;
第二更新子模块,被配置为根据所述位置信息误差、速度信息误差以及航向信息误差,对该更新时刻之前第三时间阈值时刻的运载体状态进行更新,得到位置更新信息、速度更新信息以及航向更新信息;
第二得到子模块,被配置为基于所述位置更新信息、速度更新信息以及航向更新信息,计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,零速更新装置还包括:
更新时刻确定模块,被配置为根据所述惯性测量单元的采样频率确定所述零速区间的至少一个更新时刻。
在一些实施方式中,所述运载体为车辆,所述第一测量单元为所述车辆的轮速计。
第五方面,本公开实施方式提供了一种自主导航系统,包括:
第一测量单元和惯性测量单元,设于运载体上,并基于所述运载体的运动状态获取信号;
处理器;
存储器,存储有能够被所述处理器读取的计算机指令,当所述计算机指令被读取时,所述处理器执行根据第一方面或者第二方面中任一实施方式的方法。
第六方面,本公开实施方式提供了一种运载体,包括本公开实施例第五方面提供的自主导航系统。
第七方面,本公开实施方式提供了一种存储介质,用于存储计算机可读指令,所述计算机可读指令用于使计算机执行根据第一方面或者第二方面中任一实施方式的方法。
本公开实施方式的零速区间检测方法,根据运载体上的第一测量单元检测到第一信号的第一时刻,确定零速区间的开始时刻,响应于第一测量单元检测到第二信号,确定检测到第二信号的时刻为第二时刻,将位于第二时刻之前第一时间阈值的时刻确定为零速区间的结束时刻,从而得到零速区间范围。通过第一测量单元检测零速区间,避免了运载体静止状态下发生抖动,导致惯性测量单元误判的问题,提高零速区间的检测精度,进而更有利于后续的零速更新,提高自主导航系统的定位精度。并且,考虑到多个传感器之间的同步误差,提前第一时间阈值结束零速区间,降低将运动状态误判为静止状态的风险,保证对零速区间的检测稳定可靠。
附图说明
为了更清楚地说明本公开具体实施方式或现有技术中的技术方案,下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图是本公开的一些实施方式,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是根据本公开一些实施方式的零速区间检测方法的流程图。
图2是根据本公开一些实施方式的零速更新方法的流程图。
图3是根据本公开一些实施方式的零速更新方法的流程图。
图4是根据本公开一些实施方式的零速更新方法的流程图。
图5是根据本公开一些实施方式的零速更新方法的原理图。
图6是根据本公开一些实施方式中零速区间检测装置的结构框图。
图7是根据本公开一些实施方式中零速更新装置的结构框图。
图8是根据本公开一些实施方式中零速更新装置的结构框图。
图9是根据本公开一些实施方式中零速更新装置的结构框图。
图10是适于实现本公开实施方式中方法的计算机系统结构框图。
具体实施方式
下面将结合附图对本公开的技术方案进行清楚、完整地描述,显然,所描述的实施方式是本公开一部分实施方式,而不是全部的实施方式。基于本公开中的实施方式,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式,都属于本公开保护的范围。此外,下面所描述的本公开不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。
惯性导航系统(INS)是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统,其工作原理是利用惯性元件来测量运载体本身的加速度,对时间进行积分得到速度和位置,从而达到对运载体导航定位的目的。由于惯导系统具有完全独立自主、不受外界干扰、全天候工作的特点,其与全球导航卫星系统(GNSS)形成优势互补的组合导航系统,已经成为目前驾驶领域最主流的定位方式。
以智能驾驶场景为例(本公开中智能驾驶可以包括全自动驾驶,也可以包括具有部分自动驾驶功能的手动驾驶),在车辆运动状态下,INS系统的惯性测量元件(IMU)具有良好的可观性,位置和姿态都能够有效收敛。但是在智能驾驶场景下,通常会有长时间的驻车,例如红绿灯路口等,车辆在静止状态下IMU的可观性变差,在经过长时间积分累积后,很容易出现航向姿态漂移的问题,而且在静止状态下容易出现定位的抖动。零速修正正是针对IMU静止场景的约束方法,可以抑制车辆静止状态的航向漂移和定位原地抖动。
零速修正主要包括零速区间检测和零速更新,相关技术中,零速区间检测通常是根据IMU的加速度或角速度输出数据来判断当前是否属于零速状态,但是这种检测方法的检测精度和稳定性较差。以自动驾驶场景为例,车辆在静止状态下通常会受干扰而发生抖动,例如驻车时引擎的震动、人员上下车的影响均会造成车辆抖动,从而影响IMU的测量值,导致IMU检测为非零速状态。而且,对于不同检测场景,IMU对零速区间的检测阈值也比较难于设定,存在需要手动调节以及参数敏感等问题。
正是基于上述相关技术中的缺陷,第一方面中,本公开实施方式提供了一种零速区间检测方法,提高零速区间的检测精度以及检测的稳定性。本公开方法可应用于运载体的自主导航系统,其执行主体可以是运载体的处理器,也可以是与运载体通过无线/有线建立通讯连接的终端或服务器,本公开对此不作限制。下面结合图1对本公开方法作详细说明。
如图1所示,在一些实施方式中,本公开的零速区间检测方法包括:
S110、响应于运载体上的第一测量单元检测到第一信号,根据检测到第一信号的第一时刻,确定零速区间的开始时刻。
具体来说,运载体指搭载有自主导航系统的移动载体,例如装配有INS系统的车辆、移动机器人、人体穿戴设备等,本公开对此不作限制。以INS系统为例,其包括搭载于运载体上的多个惯性测量单元(IMU),例如加速度计、陀螺仪等,从而系统可根据IMU输出的加速度和角速度确定运载体的状态。
在本公开实施方式中,运载体上还搭载有第一测量单元,第一测量单元指对运载体的静止状态具有良好可观性的测量元件,其可以是例如车辆自带的轮速计,也可以是例如其他增设于运载体上的传感器,本公开对此不作限制。以智能驾驶场景为例,轮速计是车辆上测量轮速的元件,其在车辆静止状态下的速度精度非常高,也即零的速度输出,而且不会受车上物体扰动的影响,因为车辆静止状态下车轮的速度输出始终为零。
可以理解,第一测量单元并不局限于轮速计,还可以是其他任何适于实施的测量元件。例如同样以智能驾驶场景为例,还可以采用静止传感器检测车轮的转动状态,从而得到车辆的运动状态。本公开对此不再赘述。
运载体在静止状态时,第一测量单元即可检测得到第一信号,也即表示运载体的速度为零。例如运载体在初开机静止时,或者运载体由运动状态变为静止状态时,第一测量单元即可检测得到第一信号。
检测到第一信号的第一时刻,表示运载体处于静止状态。在一个示例中,可以将第一时刻作为零速区间的开始时刻。在另一个示例中,也可以将第一时刻持续一定时间后的时刻作为零速区间的开始时刻。本公开下文对此进行说明,在此暂且不表。
S120、响应于第一测量单元检测到第二信号,确定检测到第二信号的第二时刻。
S130、确定第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
在利用第一测量单元得到零速区间的开始时刻之后,需要确定零速区间的结束时刻。
值得说明的是,在涉及多个传感器系统中,一个最重要的问题在于不同传感器之间的数据同步,因为各个传感器存在不同的系统延时和传输延时。在本公开实施方式中,零速修正以IMU作为估计核心,因此在利用第一测量单元进行零速区间检测的情况下,需要考虑到第一测量单元与惯性测量单元IMU之间的同步误差,从而避免把运动状态误判为静止状态。
具体来说,在本公开实施方式中,响应于第一测量单元检测到第二信号,将检测到第二信号的时刻确定为第二时刻。第二信号表示运载体由静止状态变化为移动状态,也即对于第一测量单元来说,运载体在第二时刻改变为运动状态。
然而在本公开实施方式中,并不是直接将第二时刻作为零速区间的结束时刻,而是将第二时刻之前第一时间阈值的时刻作为零速区间的结束时刻。
可以理解,设置第一时间阈值的目的是提前结束零速区间,避免第一测量单元与IMU之间的同步误差造成零速区间的误判。因此,在一些实施方式中,第一时间阈值可以基于第一测量单元与IMU之间的同步误差确定。下文中对第一时间阈值的确定作进一步说明,在此暂不详述。
在一个示例中,以智能驾驶车辆场景为例,在轮速计检测到第二信号时,表示对于轮速计信号来说,车辆在第二时刻结束静止状态,但是IMU与轮速计存在同步误差,IMU的检测信号可能超前于轮速计也可能滞后于轮速计。例如在IMU信号超前的情况下,虽然轮速计检测到车辆在第二时刻结束静止状态,但是实际上,IMU在第二时刻之前就已经检测到车辆结束静止状态。换句话说,真实的车辆在第二时刻之前就已经结束零速区间,如果将第二时刻作为零速区间的结束时刻,也就把静止刚结束一段时间内的运动状态误判为静止状态,在经过长时间的积分累积之后,将引入极大的定位误差,甚至造成系统震荡。
因此在本示例中,首先根据轮速计与IMU之间的同步误差,确定第一时间阈值。同步误差可以结合系统效率进行估算,也可以通过实验测量,本公开对此不作限制。例如,对于一个可靠系统而言,两个传感器之间的数据不同步一般都在100ms以下,因此可以假设轮速计与IMU之间的数据同步误差约为100ms,但是无需限制两者哪个滞后和哪个超前,本公开方法均可适用。
可以理解,第一时间阈值的目的是避免由于同步误差导致的,将零速区间结束时刻的车辆运动状态误判为静止状态。因此,为了尽可能的消除同步误差影响,在一些实施方式中,可设置第一时间阈值不小于同步误差。例如,在上述示例中,根据第一测量单元与IMU之间的100ms同步误差,确定第一时间阈值为200ms。
在确定第一时间阈值之后,将第二时刻之前第一时间阈值的时刻确定为同步区间的结束时刻。也即,在上述示例中,将第二时刻之前200ms的时刻作为零速区间的结束时刻,提前结束零速区间。
在确定零速区间的开始时刻和结束时刻之后,也即得到系统的零速区间。
通过上述可知,本公开的零速区间检测方法,利用第一测量单元来确定零速区间的开始和结束时刻,避免IMU在运载体静止状态下受干扰发生抖动而导致的误判问题。而且,由于第一测量单元对于静止状态的可观性更好,因此在零速检测时对于信号阈值的设定更加容易且通用,提高系统鲁棒性。同时,在进行零速检测时,充分考虑到多个传感器之间的同步误差问题,利用第一测量单元检测到第二信号之前的时刻来作为零速区间的结束时刻,消除由于不同传感器之间的同步误差,避免将运动状态误判为静止状态,在提高零速区间检测精度的情况下,同样保证对零速区间的检测稳定可靠。
在一些实施方式中,在确定零速区间的开始时刻时,一方面考虑到对于很短时间的静止状态,例如运载体静止了1秒,在这种情况下IMU的累积误差并不明显,没有必要进行零速修正,因此也就没有必要将这个很短的静止时间作为零速区间。另一方面,考虑到第一测量单元与IMU之间的同步误差,假设IMU信号传输滞后于第一测量单元,在第一测量单元检测到第一信号时,对于第一测量单元来说,运载体此时处于零速状态,但是IMU信号有可能检测到此时运载体处于移动状态,由于IMU信号滞后,若直接将此时刻确定为零速区间的开始时刻,则有可能将零速区间开始前的运动状态误判为静止状态。
因此,在一些实施方式中,在确定零速区间开始时刻时,本公开零速区间检测方法包括:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到第一信号,确定位于第一时刻之后第二时间阈值的时刻为零速区间的开始时刻。
具体来说,在第一测量单元检测到第一信号时,将检测到第一信号的时刻确定为第一时刻。但是并不是直接将第一时刻作为零速区间的开始时刻,而是将第一时刻之后第二时间阈值的时刻作为零速区间的开始时刻。在一些实施方式中,第二时间阈值基于第一测量单元和IMU之间的同步误差确定。
可以理解,设置第二时间阈值的目的是避免同步误差带来的误判问题,因此在一些实施方式中,理论上只要保证第二时间阈值不小于两者的同步误差即可。然而在一些实施方式中,进一步考虑到较短的静止时间内没有必要进行零速修正,因此第二时间阈值也可以根据系统精度设置更长的时间。
在一个示例中,以智能驾驶车辆为例,假设轮速计与IMU的同步误差为100ms,在轮速计第一时刻检测到第一信号时,将第一时刻之后3s的时段作为第二时间阈值。也即,若在第一时刻之后的3s内持续检测到第一信号,则将第一时刻之后3s的时刻确定为零速区间的开始时刻。
通过上述可知,本公开的检测方法,通过将第一时刻之后第二时间阈值的时刻确定为零速区间的开始时刻,一方面过滤了较短时间的静止情况,降低系统运算量,另一方面避免了同步误差带来的误判问题,提高检测精度和稳定性。
值得说明的是,本公开实施方式的零速区间检测方法,可以用于对运载体的离线数据进行零速区间检测,以便于后续对零速区间进行更新修正。离线数据指运载体运动的历史数据,通过上述方法,可从离线数据中检测到零速区间的开始时刻和结束时刻,在确定零速区间之后,进而即可对零速区间进行零速更新修正。可以理解,本公开零速区间检测方法在于:利用第一测量单元来确定零速区间的开始和结束时刻,避免IMU在运载体静止状态下受干扰发生抖动而导致的误判问题,同时考虑到不同传感器的同步误差,提前结束零速区间,避免将运动状态误判为静止状态保证对零速区间的检测稳定可靠。
在上述零速区间检测方法的发明构思基础上,本公开实施方式还提供了一种零速更新方法,该方法可适用于对运载体数据的在线实时零速更新。与上述离线状态不同的是:在离线状态下,可以根据历史数据确定出零速区间的开始与结束时刻;而在线状态下,系统需要实时检测运载体状态,当检测到运载体处于零速状态时,需要实时对当前时刻进行零速更新,直到检测到运载体结束零速状态,则停止零速更新。基于此,下述对本公开实施方式的零速更新方法进行说明。
第二方面,本公开实施方式还提供了一种零速更新方法,从而对零速区间的IMU数据进行零速更新。
可以理解,零速更新是指利用零速更新算法消除零速状态下的累积误差,零速更新算法通常指在零速区间内利用卡尔曼滤波器估计导航误差,卡尔曼滤波器的基本原理是:在连续的更新时刻根据惯导系统的解算结果和观测数据对导航误差进行迭代估计,从而可根据估计误差对系统进行持续的补偿更新,进而抑制运载体位置和速度的发散。对于基于卡尔曼滤波的惯导系统原理,本领域技术人员参照相关技术毫无疑问可以理解,本公开对此不再赘述。
在一些实施方式中,如图2所示,本公开的零速更新方法包括:
S210、响应于运载体上的第一测量单元检测到第一信号,根据检测到第一信号的第一时刻,确定零速区间的开始时刻。
具体来说,在零速更新时,首先要确定零速区间的开始时刻,也即确定运载体何时进入零速区间。
在一些实施方式中,可参照前述步骤S110确定零速区间的开始时刻,在此不再赘述。
S220、针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的运载体的状态信息进行更新,直至第一测量单元检测到第二信号。
具体来说,在确定零速区间的开始时刻之后,即需要实时对当前更新时刻进行零速更新。
本公开所述的“更新时刻”指零速区间中需要进行零速更新的时刻,相邻两个更新时刻通常间隔固定的时间步长,例如时间步长为10ms,则每间隔10ms即可作为一次更新时刻。
在一个示例中,零速区间的更新时刻可以根据惯性测量单元IMU的采样频率确定,例如可将IMU的每个采样时刻作为一个更新时刻,也可以将IMU的连续多个采样时刻作为一个更新时刻,本公开对此不作限制。通过将IMU的一个或多个采样时刻作为更新时刻,从而在每个更新时刻可以得到对应的IMU的检测数据,例如该更新时刻的加速度、角速度等数据信息。
行驶数据包括运载体在行驶过程中,搭载于运载体上的各个传感器所采集到的数据,具体来说,本公开所述的行驶数据可包括观测数据和预测数据。可以理解,在运载体行驶过程中,随着时间的推移,IMU的误差累积积分会导致整个系统状态的漂移,为了对系统状态进行修正需要引入通过预设传感器获取的观测量,该观测量即本公开所述的“观测数据”。以自动驾驶车辆为例,在对车辆状态修正时,通常的观测数据可以是GNSS传感器获取的数据,而对于零速区间中,观测数据中速度信息也即为零。而本公开所述的“预测数据”指,惯性导航系统基于IMU的检测数据得到的惯导解算结果,也即通过数学模型计算得到的状态预测结果。
在相关技术的零速更新中,即可根据当前时刻的观测数据和预测数据利用卡尔曼滤波算法对当前时刻的导航误差进行估计。但是,本公开零速更新方法的发明构思与之不同,本公开方法考虑到第一测量单元与IMU之间的同步误差问题,若直接采用当前最新时刻的零速观测对当前时刻进行滤波更新,则有可能引入错误的零速观测。例如,假设IMU信号传输滞后于第一测量单元,在零速区间即将结束时的某个时刻,第一测量单元检测到此时运载体处于静止状态,但是IMU检测到运载体处于移动状态,但是由于IMU信号传输滞后,若直接采用该时刻的观测进行零速更新,则错误的将零速区间结束前的运动状态误判为静止状态。
因此,在本公开实施方式中,对于每个更新时刻,并非获取该更新时刻的观测数据和预测数据,而是首先确定第三时间阈值,然后获取当前更新时刻之前第三时间阈值的时刻的观测数据和预测数据,利用第三时间阈值之前的观测数据进行滤波更新,再积分到当前时刻。也即,本公开实施方式的零速更新方法,对零速区间进行滞后更新,基于较早时刻数据对当前时刻的系统状态进行修正。
可以理解,在一些实施方式中,第三时间阈值可以基于第一测量单元与IMU之间的同步误差确定,而且第三时间阈值不小于第一测量单元与IMU之间的同步误差。
在一个示例中,以智能驾驶车辆为例,假设第一测量单元与IMU之间的同步误差为100ms,确定第三时间阈值为200ms,则对于零速区间每个更新时刻,均获取该更新时刻之前200ms时刻的观测数据和预测数据,基于200ms之前的数据进行滤波更新,然后基于更新后的状态信息积分到200ms后的当前时刻,得到当前时刻的运载体状态信息。
在零速更新过程中,还需要确定零速更新的结束时间。在本公开实施方式中,当第一测量单元检测到第二信号时,则停止零速更新,第二信号表示运载体由静止状态变化为移动状态。
具体来说,在零速区间开始之后,实时对每个当前更新时刻进行零速更新,直至第一测量单元检测到第二信号,表示运载体结束零速状态,变为运动状态,即可结束零速更新。可以理解,本公开实施方式中,基于当前更新时刻的第三时间阈值之前的观测数据和预测数据进行零速更新,因此在零速区间的结束时刻,同样是采用结束时刻的第三时间阈值之前的数据进行更新,可以有效避免多个传感器之间产生较早但是传输延时较高的数据被丢弃,避免引入错误的观测数据。
在一个示例中,以智能驾驶车辆为例,假设轮速计与IMU误差为100ms,且IMU信号传输滞后于第一测量单元,设置第三时间阈值为200ms。在运载体刚结束静止状态的某个时刻,第一测量单元刚检测到此时运载体处于移动状态,但是在此时刻之前,IMU已经检测到运载体处于移动状态,只是信号传输滞后于轮速计。也就是说,真实的车辆在此时刻之前就已经结束零速区间,若在此时刻采用当前时刻的数据进行零速更新,则错误的将零速区间结束前一段时间的运动状态误判为零速状态。
而在本公开实施方式中,则是采用200ms之前的数据对当前时刻200ms之前的时刻进行零速更新,然后再基于更新后的数据积分累积到当前时刻,得到当前时刻的状态信息。换句话说,由于第三时间阈值大于轮速计与IMU同步误差,因此当前时刻之前200ms的时刻车辆应当为静止状态,不会引入错误的零速观测。
通过上述可知,本公开实施方式的零速更新方法,通过利用当前更新的第三时间阈值之前的观测数据和预测数据,对运载体的状态信息持续更新,可以避免多个传感器之间产生较早但是传输延时较高的观测数据被丢弃,导致引入错误的零速观测的问题,提高系统精度和稳定性。
在一些实施方式中,在进行零速更新时,本公开实施方式不仅对运载体的速度和位置进行约束,而是利用全状态的位置、速度以及航向姿态对运载体进行约束更新,从而可以很好的解决零速更新中的原地位姿抖动和误差累积问题。下面结合图3进行说明。
如图3所示,在一些实施方式中,本公开零速更新方法包括:
S310、根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻自主导航系统的状态误差。
S320、根据状态误差对该更新时刻之前第三时间阈值的时刻的运载体的状态信息进行更新,得到更新状态信息。
S330、根据更新状态信息,得到该更新时刻的运载体的状态信息。
具体来说,在本公开实施方式中,行驶数据可以包括观测数据和预测数据。观测数据包括运载体的观测位置信息、观测速度信息以及观测航向信息,预测数据包括运载体的预测位置信息、预测速度信息以及预测航向信息。通过前述可知,观测数据是指预设传感器(GNNS传感器)获取的数据,也即预设传感器获取的数据中不仅包括了运载体的速度信息,还包括运载体的位置信息和航向信息。同理,预测数据是指系统基于IMU获取数据通过数学模型结算得到的数据,也即预测数据中同样包括了运载体的速度信息、位置信息以及航向信息。
针对步骤S310,可以基于获取的行驶数据,采用卡尔曼滤波算法得到更新时刻之前第三时间阈值的时刻,自主导航系统的状态误差。实施时,针对每个更新时刻而言,可以在得到该更新时刻第三时间阈值之前时刻的数据之后,利用卡尔曼滤波计算得到对应时刻的位置信息误差、速度信息误差以及航向信息误差。针对步骤S320,实施时可以根据得到的位置信息误差、速度信息误差以及航向信息误差,对第三时间阈值之前那个时刻的状态信息进行补偿更新,得到之前时刻的位置更新信息、速度更新信息以及航向更新信息。针对步骤330,可以基于更新状态信息,积分计算得到该更新时刻的运载体的状态信息。实施时可以基于位置更新信息、速度更新信息以及航向更新信息,积分累积到该更新时刻,得到该更新时刻运载体的状态信息。
在一个示例中,以智能驾驶场景为例,假设轮速计与IMU同步误差为100ms,确定第三时间阈值为200ms。对于每个更新时刻,可获取200ms之前的观测数据和预测数据,根据观测数据和预测数据确定200ms之前时刻的状态误差(包括位置、速度以及航向信息误差)。利用卡尔曼滤波根据状态误差对200ms之前时刻的运载体状态信息进行滤波更新,得到200ms之前时刻的更新状态信息(包括位置、速度以及航向更新信息)。然后再基于更新状态信息和200ms内的观测数据进行积分运算,计算得到当前更新时刻的状态信息(包括对应更新时刻的位置、速度以及航向状态信息)。
通过上述可知,本公开的零速更新方法,通过采用位置、速度以及航向信息对系统进行全状态的约束,在零速区间的更新中,基于零速区间开始时刻的运载体状态进行全状态约束,也即把后续的状态都约束到运载体刚静止的状态,较好的改善了零速更新的原地位姿抖动和误差累积的问题。而且,利用当前更新的第三时间阈值之前的观测数据和预测数据,对运载体的状态信息持续更新,可以避免多个传感器之间产生较早但是传输延时较高的观测数据被丢弃,导致引入错误的零速观测的问题,提高系统精度和稳定性。
上述对本公开实施方式的零速区间检测方法以及零速更新方法进行了说明,下面结合图4、图5实施方式进一步说明。在本实施方式中,运载体以智能驾驶车辆为例,第一测量单元以车辆上的轮速计为例。当然,本领域技术人员应当理解,本公开方法并不局限本实施方式示例,对此不再赘述。
如图4、图5所示,在本实施方式中,对车辆进行零速修正的过程包括:
S410、响应于轮速计在第一时刻之后的第二时间阈值内持续检测到第一信号,确定第一时刻之后第二时间阈值的时刻为零速区间的开始时刻。
在本实施方式中,假设轮速计与IMU的同步误差为100ms,确定第二时间阈值为3s。若轮速计在第一时刻t0检测到第一信号,表示车辆在t0时刻进入静止状态,若车辆持续静止状态3s,则将第一时刻t0之后3s的t1时刻确定为零速区间的开始时刻。
S420、针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据行驶数据对该更新时刻的运载体的状态信息进行更新。
在本实施方式中,假设相邻更新时刻之间的时间间隔为10ms,也即每隔10ms的更新时刻进行一次零速更新,假设第三时间阈值200ms。如图5所示,以当前更新时刻t2为例,对于更新时刻t2,获取t2之前200ms的t3时刻的行驶数据。在本实施方式中,行驶数据包括前述的观测数据和预测数据。
具体来说,本实施方式中选择IMU作为卡尔曼滤波估计的核心,要估计的系统状态总共为22维向量,也即,在本实施方式中,车辆的状态信息总共表示为如下22为向量:
式(1)中,X表示车辆的系统状态,r表示车辆的三维位置,vn表示车辆的三维速度,表示车辆的四位姿态四元数,ba表示车辆的三维加速度零偏,bg表示车辆的三维角速度零偏,Sa表示车辆的三维加速度尺度因子,Sg表示车辆的三维角速度尺度因子,T表示转置。
在本实施方式中,采用基于误差状态卡尔曼滤波,也即非直接滤波。如图5所示,对于更新时刻t2,在获取t2时刻之前200ms的t3时刻的观测数据和预测数据之后,利用卡尔曼滤波算法即可估计得到t3时刻的系统状态的误差,可表示为:
δX=[δrδvnεδbaδbgδSaδSg]T(2)
式(2)中,δX车辆的系统状态的误差,δr表示车辆的三维位置误差,δvn表示车辆的三维速度误差,ε表示车辆的三维姿态误差(最小参数化为旋转矢量),δba表示车辆的三维加速度零偏误差,δbg表示车辆的三维角速度零偏误差,δSa表示车辆的三维加速度尺度因子误差,δSg表示车辆的三维角速度尺度因子误差。
在得到系统状态误差之后,即可根据系统状态误差对t3时刻的车辆状态进行补偿更新,得到更新状态信息。具体来说,对于多个更新时刻的系统误差状态连续动态过程,可表示为:
式(3)中,F表示系统转移矩阵,G表示扰动矩阵,W表示系统噪声向量。对于系统状态误差的补偿计算,在相关领域已形成固定模式,本领域技术人员可以理解,本公开对此不再赘述。
可以理解,在本实施方式中,采用全状态约束的零速更新方法,也即包括三维位置、三维速度和一维航向姿态,因为对于车辆来说,两个水平姿态本身由于重力约束不会发散,因此本实施方式的全状态不需要再额外约束水平姿态。即,车辆的零速观测数据可表示为:
式(5)中,Zz残差,h为对应时刻IMU姿态四元数解析出的航向预测值。对应的观测矩阵H表示为:
式(6)中,Cij表示表示方向余弦矩阵C(从姿态四元数转换得到)第i行和第j列。
在得到t3时刻的更新状态信息之后,基于t3时刻的更新状态信息以及t3至t2之间200ms内的IMU数据,基于上述数学模型积分计算即可得到t2时刻的车辆的状态信息。
随着时间的积累,对于每一个更新时刻,重复执行上述S420~S430即可,直至零速区间的结束时刻。在本实施方式中,全状态包括位置信息、速度信息和航向信息,这些信息来自于零速区间的第一个时刻对应的IMU系统状态,在后续的零速区间内,将把系统的后续状态约束更新到与第一个时刻相同,从而达到抑制状态漂移的目的。
S430、响应于轮速计在第二时刻检测到第二信号,结束零速更新。
在本实施方式中,假设第一时间阈值与第三时间阈值相同,均为200ms,当然本领域技术人员可以理解,第一时间阈值与第三时间阈值也可以不同,本公开对此不作限制。
具体来说,轮速计在第二时刻t4检测到第二信号,表示车辆在t4时刻由静止状态变为移动状态,将第二时刻t4之前200ms的t5时刻作为零速区间的结束时刻,从而结束零速更新。也即,零速区间为t1至t5,通过t4与t5之间预留的200ms,可以有效避免多个传感器之间的同步误差导致的错误的将零速区间结束时的运动状态误判为静止状态的问题,提高系统精度和稳定性。
通过上述可知,本公开的零速区间检测方法和零速更新方法,利用第一测量单元来确定零速区间的开始和结束时刻,避免IMU在运载体静止状态下受干扰发生抖动而导致的误判问题。而且,由于第一测量单元对于静止状态的可观性更好,因此在零速检测时对于信号阈值的设定更加容易且通用,提高系统鲁棒性。同时,在进行零速检测时,充分考虑到多个传感器之间的同步误差问题,利用第一测量单元检测到第二信号之前的预设时间来作为零速区间的结束时刻,消除由于不同传感器之间的同步误差,避免将运动状态误判为静止状态,在提高零速区间检测精度的情况下,同样保证对零速区间的检测稳定可靠。而且,通过利用当前更新的第三时间阈值之前的观测数据和预测数据对当前更新时刻的状态进行零速更新,避免多个传感器之间产生较早但是传输延时较高的观测数据被丢弃,导致引入错误的零速观测的问题,提高系统精度和稳定性。
第三方面,本公开实施方式提供了一种零速区间检测装置,以提高零速区间的检测精度以及检测的稳定性。本公开装置可应用于自主导航系统。
如图6所示,在一些实施方式中,本公开的零速区间检测装置包括:
第一确定模块101,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态;
第二确定模块102,被配置为响应于所述第一测量单元检测到第二信号,确定检测到所述第二信号的第二时刻;所述第二信号表示所述运载体由静止状态变化为移动状态;
第三确定模块103,被配置为确定所述第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
基于前述可知,本公开的零速区间检测装置,利用第一测量单元来确定零速区间的开始和结束时刻,避免IMU在运载体静止状态下受干扰发生抖动而导致的误判问题。而且,由于第一测量单元对于静止状态的可观性更好,因此在零速检测时对于信号阈值的设定更加容易且通用,提高系统鲁棒性。同时,在进行零速检测时,充分考虑到多个传感器之间的同步误差问题,利用第一测量单元检测到第二信号之前的时刻来作为零速区间的结束时刻,消除由于不同传感器之间的同步误差,避免将运动状态误判为静止状态,在提高零速区间检测精度的情况下,同样保证对零速区间的检测稳定可靠。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第一确定模块101具体被配置为:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
第四方面,本公开实施方式提供了一种零速更新装置,从而对零速区间的IMU数据进行零速更新。本公开装置可应用于自主导航系统,以提高零速区间的检测精度以及零速更新的效果。
如图7所示,在一些实施方式中,本公开零速更新装置包括:
第四确定模块104,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态,所述零速区间包括至少一个更新时刻;
零速更新模块105,被配置为针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,直至所述第一测量单元检测到第二信号;所述第二信号表示所述运载体由静止状态变化为移动状态。
通过上述可知,本公开实施方式的零速更新方法,通过利用当前更新的第三时间阈值之前的观测数据和预测数据,对运载体的状态信息持续更新,可以避免多个传感器之间产生较早但是传输延时较高的观测数据被丢弃,导致引入错误的零速观测的问题,提高系统精度和稳定性。
在一些实施方式中,所述行驶数据包括观测数据和预测数据;其中,所述观测数据是预设传感器设备获取的;所述预测数据是根据所述运载体的惯性测量单元的检测数据得到。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
在一些实施方式中,所述第四确定模块104具体被配置为:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定。
在一些实施方式中,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
在一些实施方式中,所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定,并且,所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
如图8所示,在一些实施方式中,所述零速更新模块105包括:
第一误差确定子模块111,被配置为根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
第一更新子模块112,被配置为根据所述状态误差对该更新时刻之前第三时间阈值的时刻的运载体的状态信息进行更新,得到更新状态信息;
第一得到子模块113,被配置为根据所述更新状态信息,得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述第一误差确定子模块111具体被配置为:基于获取的行驶数据,采用卡尔曼滤波算法得到更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
所述第一得到子模块113具体被配置为:基于所述更新状态信息,积分计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,所述观测数据包括运载体的观测位置信息、观测速度信息以及观测航向信息,所述预测数据包括运载体的预测位置信息、预测速度信息以及预测航向信息。
如图9所示,在一些实施方式中,所述零速更新模块105包括:
第二误差确定子模块114,被配置为根据获取的观测数据和预测数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的位置信息误差、速度信息误差以及航向信息误差;
第二更新子模块115,被配置为根据所述位置信息误差、速度信息误差以及航向信息误差,对该更新时刻之前第三时间阈值时刻的运载体状态进行更新,得到位置更新信息、速度更新信息以及航向更新信息;
第二得到子模块116,被配置为基于所述位置更新信息、速度更新信息以及航向更新信息,计算得到该更新时刻的运载体的状态信息。
在一些实施方式中,零速更新装置还包括:
更新时刻确定模块,被配置为根据所述惯性测量单元的采样频率确定所述零速区间的至少一个更新时刻。
在一些实施方式中,所述运载体为车辆,所述第一测量单元为所述车辆的轮速计。
第五方面,本公开实施方式提供了一种自主导航系统,包括:
第一测量单元和惯性测量单元,设于运载体上,并基于所述运载体的运动状态获取信号;
处理器;
存储器,存储有能够被所述处理器读取的计算机指令,当所述计算机指令被读取时,所述处理器执行根据第一方面或者第二方面中任一实施方式的方法。
第六方面,本公开实施方式提供了一种运载体,包括本公开实施方式第五方面提供的自主导航系统。
本公开提供的运载体可以包括智能车辆,该智能车辆可以包括全自动驾驶车辆,也可以包括具有部分自动驾驶功能的手动驾驶车辆,部分自动驾驶功能可以执行本公开提供的任何零速区间检测方法、零速更新方法等。
本公开实施例提供的运载体可以包括车辆,车辆可以为自动驾驶车辆,也可以为带有部分智能驾驶功能的手动驾驶车辆。其中部分智能驾驶功能可以用于执行本公开任意实施例提供的零速区间检测方法和/或零速更新方法。
第七方面,本公开实施方式提供了一种存储介质,用于存储计算机可读指令,所述计算机可读指令用于使计算机执行根据第一方面或者第二方面中任一实施方式的方法。
具体而言,图10示出了适于用来实现本公开方法的计算机系统600的结构示意图,通过图10所示系统,可实现上述处理器及存储介质相应功能。
如图10所示,计算机系统600包括处理器601,其可以根据存储在存储器602中的程序或者从存储部分608加载到存储器602中的程序而执行各种适当的动作和处理。在存储器602中,还存储有系统600操作所需的各种程序和数据。处理器601和存储器602通过总线604彼此相连。输入/输出(I/O)接口605也连接至总线604。
以下部件连接至I/O接口605:包括键盘、鼠标等的输入部分606;包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分607;包括硬盘等的存储部分608;以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分609。通信部分609经由诸如因特网的网络执行通信处理。驱动器610也根据需要连接至I/O接口605。可拆卸介质611,诸如磁盘、光盘、磁光盘、半导体存储器等等,根据需要安装在驱动器610上,以便于从其上读出的计算机程序根据需要被安装入存储部分608。
特别地,根据本公开的实施方式,上文方法过程可以被实现为计算机软件程序。例如,本公开的实施方式包括一种计算机程序产品,其包括有形地包含在机器可读介质上的计算机程序,计算机程序包含用于执行上述方法的程序代码。在这样的实施方式中,该计算机程序可以通过通信部分609从网络上被下载和安装,和/或从可拆卸介质611被安装。
附图中的流程图和框图,图示了按照本公开各种实施方式的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分,模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个接连地表示的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或操作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
显然,上述实施方式仅仅是为清楚地说明所作的举例,而并非对实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本公开创造的保护范围之中。
Claims (17)
1.一种零速区间检测方法,其特征在于,应用于自主导航系统,所述方法包括:
响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态;
响应于所述第一测量单元检测到第二信号,确定检测到所述第二信号的第二时刻;所述第二信号表示所述运载体由静止状态变化为移动状态;
确定所述第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
2.根据权利要求1所述的方法,其特征在于,
所述第一时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定;和/或
所述第一时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
3.根据权利要求1所述的方法,其特征在于,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻,包括:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
4.根据权利要求3所述的方法,其特征在于,
所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定;和/或
所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
5.一种零速更新方法,其特征在于,应用于自主导航系统,所述方法包括:
响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态,所述零速区间包括至少一个更新时刻;
针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,直至所述第一测量单元检测到第二信号;所述第二信号表示所述运载体由静止状态变化为移动状态。
6.根据权利要求5所述的方法,其特征在于,所述行驶数据包括观测数据和预测数据;其中,所述观测数据是预设传感器设备获取的;所述预测数据是根据所述运载体的惯性测量单元的检测数据得到;
所述第三时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定;和/或
所述第三时间阈值不小于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差。
7.根据权利要求5所述的方法,其特征在于,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻,包括:
响应于所述第一测量单元在所述第一时刻之后的第二时间阈值内持续检测到所述第一信号,确定位于所述第一时刻之后第二时间阈值的时刻为所述开始时刻。
8.根据权利要求7所述的方法,其特征在于,
所述第二时间阈值是基于所述第一测量单元与所述自主导航系统的惯性测量单元之间的同步误差确定;和/或
所述第二时间阈值不小于所述第一测量单元与所述惯性测量单元之间的同步误差。
9.根据权利要求5所述的方法,其特征在于,根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,包括:
根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
根据所述状态误差对该更新时刻之前第三时间阈值的时刻的运载体的状态信息进行更新,得到更新状态信息;
根据所述更新状态信息,得到该更新时刻的运载体的状态信息。
10.根据权利要求9所述的方法,其特征在于,
根据获取的行驶数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差,包括:
基于获取的行驶数据,采用卡尔曼滤波算法得到更新时刻之前第三时间阈值的时刻,所述自主导航系统的状态误差;
根据所述更新状态信息,得到该更新时刻的运载体的状态信息,包括:
基于所述更新状态信息,积分计算得到该更新时刻的运载体的状态信息。
11.根据权利要求6所述的方法,其特征在于,所述观测数据包括运载体的观测位置信息、观测速度信息以及观测航向信息,所述预测数据包括运载体的预测位置信息、预测速度信息以及预测航向信息;
根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,包括:
根据获取的观测数据和预测数据,确定该更新时刻之前第三时间阈值的时刻,所述自主导航系统的位置信息误差、速度信息误差以及航向信息误差;
根据所述位置信息误差、速度信息误差以及航向信息误差,对该更新时刻之前第三时间阈值时刻的运载体状态进行更新,得到位置更新信息、速度更新信息以及航向更新信息;
基于所述位置更新信息、速度更新信息以及航向更新信息,计算得到该更新时刻的运载体的状态信息。
12.根据权利要求1或5所述的方法,其特征在于,
所述运载体为车辆,所述第一测量单元为所述车辆的轮速计。
13.一种零速区间检测装置,其特征在于,应用于自主导航系统,所述装置包括:
第一确定模块,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态;
第二确定模块,被配置为响应于所述第一测量单元检测到第二信号,确定检测到所述第二信号的第二时刻;所述第二信号表示所述运载体由静止状态变化为移动状态;
第三确定模块,被配置为确定所述第二时刻之前第一时间阈值的时刻为零速区间的结束时刻。
14.一种零速更新装置,其特征在于,应用于自主导航系统,所述装置包括:
第四确定模块,被配置为响应于运载体上的第一测量单元检测到第一信号,根据检测到所述第一信号的第一时刻,确定零速区间的开始时刻;所述第一信号表示所述运载体处于静止状态,所述零速区间包括至少一个更新时刻;
零速更新模块,被配置为针对每个更新时刻,获取该更新时刻之前第三时间阈值的时刻的行驶数据,并根据获取的行驶数据,对该更新时刻的所述运载体的状态信息进行更新,直至所述第一测量单元检测到第二信号;所述第二信号表示所述运载体由静止状态变化为移动状态。
15.一种自主导航系统,其特征在于,包括:
第一测量单元和惯性测量单元,设于运载体上,并基于所述运载体的运动状态获取信号;
处理器;
存储器,存储有能够被所述处理器读取的计算机指令,当所述计算机指令被读取时,所述处理器执行根据权利要求1至4任一项所述的方法,或者执行根据权利要求5至12任一项所述的方法。
16.一种运载体,其特征在于,包括如权利要求15所述的自主导航系统。
17.一种存储介质,其特征在于,用于存储计算机可读指令,所述计算机可读指令用于使计算机执行根据权利要求1至4任一项所述的方法,或者执行根据权利要求5至12任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011563843.4A CN112762944B (zh) | 2020-12-25 | 2020-12-25 | 零速区间检测及零速更新方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011563843.4A CN112762944B (zh) | 2020-12-25 | 2020-12-25 | 零速区间检测及零速更新方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112762944A true CN112762944A (zh) | 2021-05-07 |
CN112762944B CN112762944B (zh) | 2024-03-22 |
Family
ID=75695663
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011563843.4A Active CN112762944B (zh) | 2020-12-25 | 2020-12-25 | 零速区间检测及零速更新方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112762944B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114120252A (zh) * | 2021-10-21 | 2022-03-01 | 阿波罗智能技术(北京)有限公司 | 自动驾驶车辆状态的识别方法、装置、电子设备及车辆 |
CN114279441A (zh) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN117705094A (zh) * | 2023-05-16 | 2024-03-15 | 荣耀终端有限公司 | 导航定位方法及终端设备 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101218136A (zh) * | 2005-05-31 | 2008-07-09 | 西门子威迪欧汽车电子股份公司 | 陆上汽车的当前横摆角及当前侧滑角的测定 |
CN103616030A (zh) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | 基于捷联惯导解算和零速校正的自主导航系统定位方法 |
CN108007477A (zh) * | 2017-11-29 | 2018-05-08 | 哈尔滨工程大学 | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 |
CN108362282A (zh) * | 2018-01-29 | 2018-08-03 | 哈尔滨工程大学 | 一种基于自适应零速区间调整的惯性行人定位方法 |
CN110274592A (zh) * | 2019-07-18 | 2019-09-24 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
CN110715659A (zh) * | 2019-10-25 | 2020-01-21 | 高新兴物联科技有限公司 | 零速检测方法、行人惯性导航方法、装置及存储介质 |
CN111707294A (zh) * | 2020-08-20 | 2020-09-25 | 中国人民解放军国防科技大学 | 基于最优区间估计的行人导航零速区间检测方法和装置 |
-
2020
- 2020-12-25 CN CN202011563843.4A patent/CN112762944B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101218136A (zh) * | 2005-05-31 | 2008-07-09 | 西门子威迪欧汽车电子股份公司 | 陆上汽车的当前横摆角及当前侧滑角的测定 |
CN103616030A (zh) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | 基于捷联惯导解算和零速校正的自主导航系统定位方法 |
CN108007477A (zh) * | 2017-11-29 | 2018-05-08 | 哈尔滨工程大学 | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 |
CN108362282A (zh) * | 2018-01-29 | 2018-08-03 | 哈尔滨工程大学 | 一种基于自适应零速区间调整的惯性行人定位方法 |
CN110274592A (zh) * | 2019-07-18 | 2019-09-24 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
CN110715659A (zh) * | 2019-10-25 | 2020-01-21 | 高新兴物联科技有限公司 | 零速检测方法、行人惯性导航方法、装置及存储介质 |
CN111707294A (zh) * | 2020-08-20 | 2020-09-25 | 中国人民解放军国防科技大学 | 基于最优区间估计的行人导航零速区间检测方法和装置 |
Non-Patent Citations (1)
Title |
---|
常乐;章红平;高周正;丁昱心;张全;: "零速修正在GNSS/INS紧组合导航中的应用研究", 大地测量与地球动力学 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114120252A (zh) * | 2021-10-21 | 2022-03-01 | 阿波罗智能技术(北京)有限公司 | 自动驾驶车辆状态的识别方法、装置、电子设备及车辆 |
CN114120252B (zh) * | 2021-10-21 | 2023-09-01 | 阿波罗智能技术(北京)有限公司 | 自动驾驶车辆状态的识别方法、装置、电子设备及车辆 |
CN114279441A (zh) * | 2021-12-15 | 2022-04-05 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN114279441B (zh) * | 2021-12-15 | 2023-12-29 | 中国科学院深圳先进技术研究院 | 一种零速区间检测方法、行人导航系统及存储介质 |
CN117705094A (zh) * | 2023-05-16 | 2024-03-15 | 荣耀终端有限公司 | 导航定位方法及终端设备 |
Also Published As
Publication number | Publication date |
---|---|
CN112762944B (zh) | 2024-03-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112762944B (zh) | 零速区间检测及零速更新方法 | |
US10082583B2 (en) | Method and apparatus for real-time positioning and navigation of a moving platform | |
EP3701286B1 (en) | A method and system for combining sensor data | |
CN110702104B (zh) | 一种基于车辆零速检测的惯性导航误差修正方法 | |
US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
JP5036462B2 (ja) | ナビゲーション・システムおよびナビゲーション方法 | |
JP5736106B2 (ja) | 移動状態検出装置 | |
JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
US20100007550A1 (en) | Positioning apparatus for a mobile object | |
US20160016590A1 (en) | Method for detecting driving events of a vehicle based on a smartphone | |
US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
CN113405545B (zh) | 定位方法、装置、电子设备及计算机存储介质 | |
CN111854740B (zh) | 能够在交通工具中进行航位推算的惯性导航系统 | |
CN110715659A (zh) | 零速检测方法、行人惯性导航方法、装置及存储介质 | |
CN111026081B (zh) | 一种误差计算方法、装置、设备及存储介质 | |
CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
JP6488860B2 (ja) | 勾配推定装置及びプログラム | |
CN115060257B (zh) | 一种基于民用级惯性测量单元的车辆变道检测方法 | |
WO2016089263A1 (en) | Method and system for estimating relative angle between headings | |
KR20110108208A (ko) | 주행 모드 판별법, 상기 주행 모드 판별을 이용한 항법 시스템 및 그 제공방법 | |
CN115112119A (zh) | 一种基于lstm神经网络辅助的车载导航方法 | |
US20240319736A1 (en) | Position estimation device, automated driving system, position estimation method, and storage medium storing program | |
CN118112623A (zh) | 一种间断gnss信号下的车辆融合定位方法及相关设备 | |
CN118670409A (zh) | 一种车辆定位方法、装置、存储介质及电子设备 | |
CN118067118A (zh) | 定位方法、定位系统、机器人和计算机可读存储介质 |
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 |