CN115326084A - 车辆定位方法、装置、计算机设备、存储介质 - Google Patents
车辆定位方法、装置、计算机设备、存储介质 Download PDFInfo
- Publication number
- CN115326084A CN115326084A CN202210929163.2A CN202210929163A CN115326084A CN 115326084 A CN115326084 A CN 115326084A CN 202210929163 A CN202210929163 A CN 202210929163A CN 115326084 A CN115326084 A CN 115326084A
- Authority
- CN
- China
- Prior art keywords
- target
- information
- predicted
- filter
- positioning information
- 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.)
- Pending
Links
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
- G01C21/30—Map- or contour-matching
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为一个实施例中里程计坐标系和ENU坐标系之间的坐标变换示意图;
图9为一个实施例中车道级变道指引的示意图;
图10为一个实施例中车辆定位装置的结构框图;
图11为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
本申请实施例提供的车辆定位方法,可以应用于如图1所示的应用环境中。其中,终端102通过网络与服务器104进行通信。数据存储***可以存储服务器104需要处理的数据。数据存储***可以集成在服务器104上,也可以放在云上或其他服务器上。终端102可以从本地数据库中获取目标车辆在行驶过程中的里程计数据,终端102也可以从服务器104获取目标车辆在行驶过程中的里程计数据,终端102根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态;终端102根据预测状态确定目标车辆在第二时刻的预测定位信息;终端102在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息;终端102根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息;定位信息与滤波器状态定义在相同坐标系。
其中,终端102可以但不限于是各种台式计算机、笔记本电脑、智能手机、平板电脑、物联网设备和便携式可穿戴设备,物联网设备可为智能音箱、智能电视、智能空调、智能车载设备等。便携式可穿戴设备可为智能手表、智能手环、头戴设备等。服务器104可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
可以理解,本申请实施例提供的服务器104也可以是区块链***中的服务节点,该区块链***中的各服务节点之间形成组成点对点(P2P,Peer To Peer)网络,P2P协议是一个运行在传输控制协议(TCP,Transmission Control Protocol)协议之上的应用层协议。
智能交通***(Intelligent Traffic System,ITS)又称智能运输***(Intelligent Transportation System),是将先进的科学技术(信息技术、计算机技术、数据通信技术、传感器技术、电子控制技术、自动控制理论、运筹学、人工智能等)有效地综合运用于交通运输、服务控制和车辆制造,加强车辆、道路、使用者三者之间的联系,从而形成一种保障安全、提高效率、改善环境、节约能源的综合运输***。或者;
智能车路协同***(Intelligent Vehicle Infrastructure CooperativeSystems,IVICS),简称车路协同***,是智能交通***(ITS)的一个发展方向。车路协同***是采用先进的无线通信和新一代互联网等技术,全方位实施车车、车路动态实时信息交互,并在全时空动态交通信息采集与融合的基础上开展车辆主动安全控制和道路协同管理,充分实现人车路的有效协同,保证交通安全,提高通行效率,从而形成的安全、高效和环保的道路交通***。
计算机视觉技术(Computer Vision,CV)计算机视觉是一门研究如何使机器“看”的科学,更进一步的说,就是指用摄影机和电脑代替人眼对目标进行识别、记录和测量等机器视觉,并进一步做图形处理,使电脑处理成为更适合人眼观察或传送给仪器检测的图像。作为一个科学学科,计算机视觉研究相关的理论和技术,试图建立能够从图像或者多维数据中获取信息的人工智能***。计算机视觉技术通常包括图像处理、图像识别、图像语义理解、图像检索、OCR、视频处理、视频语义理解、视频内容/行为识别、三维物体重建、3D技术、虚拟现实、增强现实、同步定位与地图构建、自动驾驶、智慧交通等技术,还包括常见的人脸识别、指纹识别等生物特征识别技术。
随着人工智能技术研究和进步,人工智能技术在多个领域展开研究和应用,例如常见的智能家居、智能穿戴设备、虚拟助理、智能音箱、智能营销、无人驾驶、自动驾驶、无人机、机器人、智能医疗、智能客服、车联网、自动驾驶、智慧交通等,相信随着技术的发展,人工智能技术将在更多的领域得到应用,并发挥越来越重要的价值。
在一个实施例中,如图2所示,提供了一种车辆定位方法,以该方法应用于图1中的终端为例进行说明,包括以下步骤:
步骤202,获取目标车辆在行驶过程中的里程计数据。
其中,目标车辆是指行驶在道路中的某个车辆,例如,行驶在道路A中的车辆1、车辆2以及车辆3均可作为目标车辆。车辆中安装有车载终端、摄像头和传感器,车辆中安装的车载终端可以实时获取该车辆行驶在道路A中时所对应的里程计数据。
里程计,是指测量行程的装置,比如,安装在车辆上的用于测量行程的各种类型的传感器。里程计数据是指从测量行程的装置中获取到的数据,例如,从惯性测量单元(Inertial measurement unit,简称IMU)获取当前车辆在行驶过程中的里程计数据。惯性测量单元是测量物体三轴姿态角以及加速度的装置。本申请中的里程计数据可以包括位置数据、航向数据以及时间戳信息。例如,某个时刻目标车辆的里程计数据可以记为O1=(x,y,yaw),其中x表示目标车辆在里程计坐标系下X轴的坐标值,y表示目标车辆在里程计坐标系下Y轴的坐标值,yaw表示目标车辆在里程计坐标系下的航向角。
具体地,目标车辆中安装有终端设备,或者目标车辆与某个终端设备具有关联关系,则终端可以获取目标车辆在行驶过程中的里程计数据。其中,里程计数据中包括目标车辆在不同时刻所对应的位置数据和航向数据。目标车辆中安装的终端设备包括但不限于是车载终端设备、移动终端设备以及便携终端设备等。
举个例子,目标车辆A与终端A具有关联关系,则终端A可以获取目标车辆A当前时刻在行车道路A中行驶时的里程计数据A,记为OA=(x,y,yaw),其中,x表示目标车辆在里程计坐标系下X轴的坐标值,y表示目标车辆在里程计坐标系下Y轴的坐标值,yaw表示目标车辆在里程计坐标系下的航向角。
步骤204,根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态。
其中,目标滤波器是指从不同类型的候选滤波器中选取的滤波器,例如,本申请中的目标滤波器包括但不限于是卡尔曼滤波器。
第一时刻和第二时刻用于区分不同的时刻,例如,第一时刻为前一个时刻,第二时刻为后一个时刻。
滤波器状态是指滤波器在某个时刻的状态,滤波器的状态包括目标车辆的航向角、经度、纬度,以及里程计坐标系和东北天坐标系之间的变换关系或者是等价的数据。其中,里程计坐标系和东北天坐标系均为笛卡尔坐标系,东北天坐标系(local Cartesiancoordinates coordinate system,ENU)也叫做站心坐标系、站点坐标系,主要是用于需了解以观察者为中心的其他物体运动规律,如接收机可见GPS卫星的视角、方位角及距离等,需要用到东北天坐标系。东北天坐标系是以测站为原点的坐标系,即用准备好的基座来定点并进行观察和测量。
预测状态是指滤波器基于当前时刻的状态,对下一时刻的状态进行状态递推,估算出的下一时刻的状态,即为预测状态。例如,滤波器基于第一时刻的状态Xk,对第二时刻的状态进行状态递推,估算出的第二时刻的状态Xk+1|k即为预测状态。
具体的,终端获取目标车辆在行驶过程中的里程计数据之后,终端可以根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态。其中,第一时刻可以为初始时刻,即终端的导航***首次进行车辆定位时,若导航***没有初始化,则终端需要对滤波器进行初始化。例如,初始化过程包括确定里程计坐标系和ENU坐标系之间的变换关系T、确定里程计数据的误差值、确定滤波器状态的初始值以及初始协方差矩阵。
举个例子,以目标滤波器为卡尔曼滤波器为例进行说明。假设终端A在首次进行车辆定位时,若导航***没有初始化,则终端首先对卡尔曼滤波器进行初始化,以确定初始滤波器状态X0=(C0,lon0,lat0,T),即包括车辆的航向角、经度、纬度,以及里程计坐标系和ENU坐标系之间的变换关系T;进一步的,终端获取目标车辆A在行驶过程中的里程计数据之后,终端可以根据里程计数据和卡尔曼滤波器在第一时刻的滤波器状态即初始滤波器状态为X0=(C0,lon0,lat0,T),确定卡尔曼滤波器在下一时刻的预测状态为X0+1|0=(C1,lon1,lat1,T)。
步骤206,根据预测状态确定目标车辆在第二时刻的预测定位信息。
具体的,终端根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态之后,终端可以根据预测状态确定目标车辆在第二时刻的预测定位信息,由于滤波器输出的预测状态中包括目标车辆的航向角、经度、纬度,以及里程计坐标系和ENU坐标系之间的变换关系,因此,终端可以将预测状态中的航向角、经度、纬度作为目标车辆在第二时刻的预测定位信息。
举个例子,假设终端根据目标车辆A在行驶过程中的里程计数据和目标滤波器的初始滤波器状态,确定目标滤波器在下一时刻的预测状态为X0+1|0=(C1,lon1,lat1,T),则终端可以将预测状态X0+1|0=(C1,lon1,lat1,T)中的航向角C1、经度值lon1、纬度值lat1作为目标车辆A在下一时刻的预测定位信息。
步骤208,在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息。
其中,电子地图即数字地图(Electronic map),是利用计算机技术,以数字方式存储和查阅的地图。电子地图可以包括精度不同的电子地图,例如,本申请中的电子地图包括但不限于是HD地图和SD地图。
HD地图(High Definition)是指一种能提供车道级别信息的地图数据,主要面向自动驾驶和辅助驾驶,精度可达厘米级别。HD地图数据能够提供道路形状、道路标志物位置、车道线方向和位置、交通信号灯位置等信息。
SD地图(Street Directory Map)是一种道路级别的导航地图,主要应用于导航、路径规划等场景,主要提供的信息包括道路形状、长度、坡度等信息。
候选路段是指电子地图中的多个路段,例如,将SD地图中相邻的两个路段L12和L23作为候选路段。
目标路段是指从候选路段中选取的某个路段,例如,从SD地图中的候选路段L12和L23中,选取路段L12作为目标路段。
匹配点信息是指将预测定位信息与电子地图中的信息进行匹配后,匹配得到的某个定位点信息。
具体的,终端根据预测状态确定目标车辆在第二时刻的预测定位信息之后,终端可以在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息。目标滤波器会输出下一时刻的预测定位信息,比如,输出的预测定位信息为P,P包括经纬度和航向信息。终端利用该定位信息P,选择SD地图中的一条Link进行匹配,即终端根据预设匹配算法,将定位信息P绑定到一条可行使的道路上,并返回一个匹配的定位点PL,匹配的定位点PL包括经纬度和航向信息。其中,SD地图中的一条Link,一般是指一条单行路,在SD地图上显示为一条线段。Link上的任何一点都对应一个经纬度,表示其定位信息。Link同时又携带其他附属信息,比如,道路类型、道路宽度、航向信息等。
举个例子,终端根据预测状态确定目标车辆A在第二时刻的预测定位信息为P之后,终端可以根据预设的匹配算法,在SD地图中的多条Link中选取与预测定位信息P匹配的目标Link,并在目标Link中确定与预测定位信息P对应的匹配点信息PL,即终端将预测定位信息P与SD地图进行匹配时,终端可以将定位点P向目标link上投影,得到的投影点投即为匹配点PL。可以理解,终端选择哪条link进行投影,是依据SD地图匹配模块所设置的算法来选取目标link的。
步骤210,根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息;定位信息与滤波器状态定义在相同坐标系。
其中,定位信息是指校正后的第二时刻目标车辆所对应的定位信息,例如,目标车辆A在第二时刻的预测定位信息为Xk+1|k=(C,lon,lat),校正后的目标车辆A在第二时刻的所对应的定位信息为Xk+1=(C,lon1,lat),其中,C表示目标车辆A的航向角;lon表示目标车辆A的经度信息;lat表示目标车辆A的纬度信息。
定位信息与滤波器状态定义在相同坐标系,是指定位信息所定义的坐标系与滤波器状态所定义的坐标系为相同的坐标系,例如,定位信息定义在WGS84坐标系下,滤波器状态也定义在WGS84坐标系下,即滤波器的递推过程和校正过程均在WGS84坐标系下进行,从而有效避免了坐标变换时引入的误差。
具体的,终端在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息之后,终端可以根据匹配点信息对预测定位信息进行校正,得到校正后的目标车辆在第二时刻的定位信息,即在终端得到反馈的匹配点信息之后,终端可以利用匹配点信息对滤波器状态进行校正,以使滤波器输出更加准确的第二时刻的定位信息。其中,本申请中的定位信息与滤波器状态定义在相同坐标系下,从而有效避免了坐标变换时的误差。
例如,以目标滤波器为卡尔曼滤波器为例进行说明。终端在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息之后,终端可以将匹配点信息作为观测信息,对滤波器在第二时刻的预测状态进行校正,以得到校正后的目标车辆在第二时刻的定位信息。
此外,终端除了将匹配点信息作为观测信息,对滤波器在第二时刻的预测状态进行校正之外,终端还可以将其他信息独立的作为观测信息,对滤波器在第二时刻的预测状态进行校正。例如,终端可以获取导航定位数据即GNSS数据,并将GNSS数据作为观测信息,对滤波器在第二时刻的预测状态进行校正。再比如,若终端确定当前反馈的匹配点信息可信,则可以直接利用该匹配点信息作为观测信息,对滤波器在第二时刻的预测状态进行卡尔曼校正;若终端确定当前反馈的匹配点信息不可信,但目标车辆距左右两边车道线的距离可信,则终端可以利用目标车辆与左右两边车道线的距离、以及左右两边车道线所对应的车道宽度,计算出一个新的观测信息,终端再基于新的观测信息,对滤波器在第二时刻的预测状态进行卡尔曼校正。
本实施例中,通过获取目标车辆在行驶过程中的里程计数据,根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态;根据预测状态确定目标车辆在第二时刻的预测定位信息;在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息;根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息;定位信息与滤波器状态定义在相同坐标系。由于可以将预测定位信息与不同的电子地图进行匹配,以得到不同的匹配点信息,因此,无需额外增加车辆的硬件设备,即可实现灵活配置匹配点信息,以实现自适应不同场合、不同精度的车辆定位方案,同时,将定位信息与滤波器状态定义在相同坐标系下,能够有效避免传统方式中坐标变换带来的误差,从而有效提升了车辆定位信息准确性,实现了在降低硬件设备成本的同时又能够有效提升车辆定位信息准确性的技术效果。
在一个实施例中,所述获取目标车辆在行驶过程中的里程计数据之前,所述方法还包括:
获取目标车辆在行驶过程中的历史里程计数据和导航定位数据;
基于历史里程计数据和导航定位数据,对滤波器进行初始化;
在初始化过程中,从导航定位数据中选取目标位置作为起始定位点;
基于起始定位点的航向信息和位置信息,确定初始滤波器状态。
其中,历史里程计数据是指从测量行程的装置中获取到的历史数据,例如,当前时刻为9:00,则终端可以获取到8:55这个时刻所对应的历史里程计数据,历史里程计数据包括位置数据、航向数据、以及时间戳。
导航定位数据是指全球导航定位数据,全球卫星导航***(the GlobalNavigation Satellite System,GNSS),也称为全球导航卫星***,是能在地球表面或近地空间的任何地点为用户提供全天候的3维坐标和速度以及时间信息的空基无线电导航定位***。例如,导航定位数据可以是从GPS设备获取到的,导航定位数据包括时间戳、有效的经度信息、纬度信息以及航向角信息。可以理解,本申请中的导航定位数据和里程计数据不需要同频率,比如,里程计数据是10Hz,而导航定位数据是1Hz,即本申请中对量获取到的里程计数据和导航定位数据的具体频率没有具体限制。
具体的,终端获取目标车辆在行驶过程中的里程计数据之前,终端可以获取目标车辆在行驶过程中的历史里程计数据和导航定位数据,并基于历史里程计数据和导航定位数据,对滤波器进行初始化,即在终端初次进行车辆定位时,首先需要对导航***进行初始化,在对导航***进行初始化的过程中,终端也会对滤波器进行初始化,即在初始化过程中,终端可以从导航定位数据中选取目标位置作为起始定位点,并基于起始定位点的航向信息和经纬度信息,确定初始滤波器状态。
举个例子,以目标滤波器为卡尔曼滤波器为例进行说明。终端获取目标车辆A在行驶过程中的里程计数据之前,终端可以获取目标车辆A在行驶过程中的历史里程计数据和GNSS数据,为了方便描述,终端可以将里程计数据记为O=(x,y,yaw),其中x表示目标车辆在里程计坐标系下X轴的坐标值,y表示目标车辆在里程计坐标系下Y轴的坐标值,yaw表示目标车辆在里程计坐标系下的航向角。同时,终端可以将GNSS数据记为G=(lonG,latG,CG),其中lonG表示GNSS的经度值,latG表示GNSS的纬度值,CG表示GNSS的航向值。
进一步的,终端可以基于历史里程计数据和GNSS数据,对卡尔曼滤波器进行初始化。在初始化过程中,终端可以从GNSS数据中确定一个质量较好的GNSS点作为起始点S,并将起始点S的航向信息和位置信息,作为卡尔曼滤波器的初始状态X1中的初始值;同时,终端可以从历史里程计数据中选取参考的里程计点Or=(xr,yr,yawr),并基于参考的里程计点Or=(xr,yr,yawr)和起始点S,确定里程计坐标系和ENU坐标系之间的变换关系T,即可得到滤波器的初始状态X1=(C,lon,lat,T),即初始状态包括目标车辆A的航向角、经度值、纬度值,以及里程计坐标系和ENU坐标系之间的变换关系T。此外,初始化过程中还包括终端确定里程计数据的误差值,以及确定卡尔曼滤波器状态的初始协方差矩阵的步骤。
可以理解,本申请实施例中对具体的初始化方法不做限定,同时,本申请实施例中确定GNSS质量的方法包括但不限于是利用GPS轨迹的平滑性、GSV中的信噪比信息,考察两者轨迹形状是否吻合,从而确定一个质量较好的GNSS点,或者也可以利用独立的其他信息,比如里程计数据。
本实施例中,通过从GNSS数据中确定一个质量较好的GNSS点作为起始点,并将起始点的航向信息和位置信息,作为滤波器的初始状态中的初始值,同时将里程计坐标系和ENU坐标系之间的变换关系T也座位滤波器初始状态中的一个初始值,为后续进行状态递推和校正提供了更加准确的初始数据,以使滤波器输出更加精确的定位信息。
在一个实施例中,里程计数据包括不同时刻的里程计数据;根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态的步骤,包括:
基于不同时刻的里程计数据,确定里程计增量;
根据里程计增量和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态。
里程计增量是指里程计的改变量,指的是不同时刻的里程值之差。例如,终端可以将9:00这个时刻所对应的里程计数据与8:55这个时刻所对应的里程计数据进行差值运算,得到的差值即为8:55~9:00这个时间段内的里程计增量。
具体的,终端获取目标车辆在行驶过程中的里程计数据之后,终端可以基于不同时刻的里程计数据,确定里程计增量,并根据里程计增量和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态,即终端可以根据上一时刻输入的传感器信息和动力学公式进行状态推算,推算出下一时刻滤波器的状态。
举个例子,以目标滤波器为卡尔曼滤波器为例进行说明。在终端首次进行车辆定位时,终端需要对卡尔曼滤波器进行初始化,当初始化完成后,终端获取到传感器采集到新里程计数据后,终端可以基于新里程计数据进行卡尔曼滤波器递推。
在一个实施例中,卡尔曼状态递推可以采用如下公式(1)所示的函数进行递推:
其中,Xk和Xk+1|k分别表示k时刻的定位状态和k+1时刻的预测状态,f(Xk,ΔO)表示k到k+1时刻的状态变换。Pk表示k时刻的协方差矩阵,Pk+1|k表示预测协方差矩阵,Φk表示k时刻的状态转移矩阵,Qk表示与里程计有关的线性量,R表示里程计的测量噪声,ΔO表示里程计增量。函数f表示由里程计数据变换为定位数据的函数,可以理解,本申请实施例中对函数f的具体实现形式没有限制。
本实施例中,通过使用卡尔曼滤波方法作为定位框架,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失。在本申请提供的方法框架中,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。
在其中一个实施例中,根据里程计增量和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态的步骤,包括:
根据坐标变换关系对里程计增量进行坐标变换,得到在目标坐标系下的里程计增量;坐标变换关系是里程计坐标系和目标坐标系之间的变换关系;
根据目标坐标系下的里程计增量和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态。
其中,目标坐标系是指从多个坐标系中选取的一个坐标系,例如,目标坐标系为ENU坐标系。
具体的,终端确定里程计增量之后,终端可以根据坐标变换关系对里程计增量进行坐标变换,得到在目标坐标系下的里程计增量,并根据目标坐标系下的里程计增量和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态。其中,坐标变换关系是里程计坐标系和目标坐标系之间的变换关系,例如,当目标坐标系为ENU坐标系时,坐标变换关系是里程计坐标系和ENU坐标系之间的变换关系T。
举个例子,以目标滤波器为卡尔曼滤波器、目标坐标系为ENU坐标系为例进行说明。终端确定里程计增量(Δx,Δy)之后,终端可以根据坐标变换关系T对里程计增量(Δx,Δy)进行坐标变换,得到在ENU坐标系下的里程计增量(Δx′,Δy′),并根据ENU坐标系下的里程计增量(Δx′,Δy′)和卡尔曼滤波器在第一时刻的滤波器状态Xk,确定目标滤波器在第二时刻的预测状态Xk+1|k。
其中,将里程计增量从里程计坐标系变换到ENU坐标系下的方式如下公式(2)所示:
(Δx′,Δy′)T=T·(Δx,Δy)T (2)
其中,(Δx,Δy)表示里程计坐标系下的里程计增量,(Δx′,Δy′)表示ENU坐标系下的里程计增量,T表示里程计坐标系和ENU坐标系之间的变换关系。
由于终端从GPS设备得到的经纬度信息,都定义在大地坐标系下,最常用的大地坐标系就是WGS坐标系,WGS坐标系不是笛卡尔坐标系,在传统方式中进行GPS-IMU惯导融合时,选择的坐标系是ENU坐标系,滤波器状态定义在ENU坐标系下,将得到的定位信息从WGS坐标系变换到ENU坐标系下的变换是非线性变换,本质上是在球面上选择一点,进行切平面展开。
例如,WGS坐标系与ENU坐标系之间的变换一般形式举例如下:
从一个原点出发,原点的经纬度为S1,经过2秒后,到达A点。从IMU进行积分后,得到在ENU坐标系下,车辆坐标变化为ΔX。这是一个原始估计,可能有误差,需要对ΔX进行校正。在这段时间内,经纬度变化为ΔS。ΔS从WGS坐标系变换到ENU坐标系下,形式为ΔX’=f(S1,ΔS)。可以看出ΔX’=f(S1,ΔS)中需要上个时刻的状态S1的参与,由于S1不是滤波器的状态量,因为它不在ENU坐标系下,一般S1是有误差的,因此导致计算的ΔX’与实际理想值有误差。ΔX’对ΔX进行融合校正后,再通过f的反向计算为WGS坐标系下的变化,这样反向计算变换坐标也有可能引入误差。
本实施例中,相较于传统方式中将滤波器的状态定义在ENU坐标系下,由于将从WGS坐标系到ENU坐标系的变换是非线性变换,因此,传统方式中的坐标变换会产生误差,而本申请实施例中的滤波器状态直接定义在大地坐标系下,采集到的定位信息也定义在大地坐标系下,因此,可以直接把状态引入,参与计算,即参与计算的都是状态估计的一部分,变换过程中,不会有非状态量参与,因此,有效避免了非状态量参与带来的误差,使得最终输出的定位信息更加准确。
在一个实施例中,假设里程计坐标系的Z轴与ENU坐标系的UP轴重合,那么里程计坐标系和ENU坐标系之间的变换关系T为一个二阶旋转矩阵,且与一个角度有关系,记为θ,θ表示ENU坐标系的E轴与里程计坐标系的X轴的夹角,此时卡尔曼滤波器的状态记为X=(C,lon,lat,θ),且满足下述所示的公式(3):
结合前述公式(1)卡尔曼滤波的状态递推公式中的Xk+1=f(Xk,ΔO),函数f的一种可行实现方式如下公式(4)所示:
其中,RE和F为与地球相关的常数。函数regulate用于把角度按照北零航向角输出,输出范围为0~360°;北零,表示航向角的参考0点,以正北方向为0°,正东方向为90°;同样的,东零,以正东方向为0°。
从公式(4)看,状态转移是非线性关系,那么卡尔曼滤波器变为扩展卡尔曼滤波器,相应的状态转移矩阵和噪声系数矩阵如下公式(5)所示:
在一个实施例中,在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与所述预测定位信息对应的匹配点信息的步骤,包括:
当电子地图为第一精度地图时,在第一精度地图的候选路段中选取与预测定位信息匹配的目标路段;
确定预测定位信息在目标路段中的投影点信息;
将投影点信息作为与预测定位信息对应的匹配点信息。
其中,本申请中的第一精度地图可以为SD地图。
具体的,当第一精度地图为SD地图时,终端可以通过SD地图匹配模块在第一精度地图的候选路段中选取与预测定位信息匹配的目标路段,并确定预测定位信息在目标路段中的投影点信息,将投影点信息作为与预测定位信息对应的匹配点信息。可以理解,在通过SD地图匹配模块将预测定位信息与SD地图进行匹配时,SD地图匹配模块中有预设的算法在运行,比如,基于贝叶斯滤波的方法进行匹配,SD地图匹配模块自身也会维护一些状态,也会把目标滤波器输出的预测定位信息作为观测信息,利用该观测信息对自身维护的状态进行校正。
举个例子,当第一精度地图为SD地图时,终端可以通过SD地图匹配模块在SD地图中的多条候选Link中选取与预测定位信息P匹配的目标Link,并在目标Link中确定与预测定位信息P对应的匹配点信息PL,即SD地图匹配模块将可以将定位点P向目标link上投影,类似于直线外一点向直线投影,得到的投影点投即为匹配点PL。可以理解,终端选择哪条link作为目标link进行投影,是依据SD地图匹配模块所设置的算法来选取目标link的。
本实施例中,通过使用卡尔曼滤波方法作为定位框架,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。比如,当存在SD地图时,可以自适应的实现道路级别的定位和导航。
在一个实施例中,在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息的步骤,包括:
当电子地图为第二精度地图时,获取携带车道信息的视觉信息;
将视觉信息与预测定位信息进行融合,得到融合后的定位信息;
在第二精度地图的候选车道中,选取与融合后的定位信息匹配的目标车道;
在目标车道中确定与融合后的定位信息对应的匹配点信息。
其中,本申请中的第二精度地图可以为HD地图。
视觉信息是指通过摄像头拍摄的包含车道信息的图像信息,例如,目标车辆中安装有摄像头,目标车辆中的车载终端可以通过调用摄像头,对该目标车辆在行驶过程中的数据进行采集,以得到包含车道信息的视觉信息,例如,视觉信息为实时采集的行车图片数据、行车视频数据等。本申请实施例中的视觉信息可以包括车道数、车道宽度、车道方程、车辆距离该车道两边车道线的垂直距离等信息。
具体的,当第二精度地图为HD地图时,终端可以获取携带车道信息的视觉信息,并利用贝叶斯滤波算法,对携带车道信息的视觉信息、滤波器输出的预测定位信息进行融合,得到融合后的定位信息后,终端在HD地图中,选择一条车道作为目标车道,并将融合后的定位信息与目标车道进行匹配,即可得到匹配点,该匹配点的信息包括经纬度信息和航向信息。
举个例子,终端中的视觉HD地图匹配模块可以用于对视觉感知模块感知的车道线信息进行处理,并与HD地图进行匹配,以得到高精度的定位信息,并将定位信息反馈给滤波器进行校正处理。即终端中的视觉HD地图匹配模块可以利用贝叶斯框架,维护一个状态向量,该状态向量中保存了车辆位于各个车道的概率。在车辆行驶过程中,状态本身进行状态转移,得到状态的先验信息,终端还可以把视觉信息作为观测信息,利用该观测信息进行贝叶斯后验估计,以得到贝叶斯后验估计的高精度的定位信息,可以理解,以上只是本申请方案中的一种可能的实现方式。
本实施例中,通过使用卡尔曼滤波方法作为定位框架,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。比如,当存在视觉感知和HD地图时,可以自适应的实现车道级定位和导航。
在一个实施例中,在目标车道中确定与融合后的定位信息对应的匹配点信息之后,所述方法还包括:
当确定匹配点信息的可信度大于可信度阈值时,执行根据匹配点信息对预测定位信息进行校正的步骤;
当确定匹配点信息的可信度小于或等于可信度阈值时,确定目标车辆与第一车道线之间的第一距离值,以及确定目标车辆与第二车道线之间的第二距离值;
当第一距离值和第二距离值满足可信距离条件时,基于第一距离值、第二距离值和车道宽度,确定目标观测信息;车道宽度是第一车道线和第二车道线之间的距离;
根据目标观测信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息。
其中,目标观测信息是指从多个观测信息中选取的一个信息作为观测信息,比如,观测信息可以包括视觉信息、SD地图匹配信息以及GNSS定位信息等。
具体的,终端在目标车道中确定与融合后的定位信息对应的匹配点信息之后,终端可以判断匹配点信息的可信度是否大于可信度阈值,若大于可信度阈值,则表示匹配点信息可信;若不大于可信度阈值,则表示匹配点信息不可信。当终端确定匹配点信息的可信度大于可信度阈值时,终端可以直接执行根据匹配点信息对预测定位信息进行校正的步骤;当终端确定匹配点信息的可信度小于或等于可信度阈值时,终端可以确定目标车辆与第一车道线之间的第一距离值,以及确定目标车辆与第二车道线之间的第二距离值;进一步的,终端可以判断第一距离值和第二距离值是否满足可信距离条件,若满足可信距离条件,则表示第一距离值和第二距离值可信;若不满足,则表示第一距离值和第二距离值不可信。
当终端确定第一距离值和第二距离值满足可信距离条件时,终端可以基于第一距离值、第二距离值和车道宽度,重新确定一个新的目标观测信息,以使得后续可以基于新的目标观测信息对滤波器的预测状态进行校正。
例如,如图3所示,为对预测定位信息进行校正处理的示意图。图3中的目标车道为车道1,车道1的左边车道线记为L1,车道1的右边车道线记为L2,左边车道线L1和右边车道线L2之间的距离为D,即车道宽度为D。终端在目标车道1中确定与预测定位信息P对应的匹配点为G1之后,若终端确定该匹配点G1的可信度大于可信度阈值,则终端可以直接执行根据匹配点G1对预测定位信息P进行校正的步骤;若终端确定匹配点G1的可信度小于或等于可信度阈值,终端确定目标车辆A与第一车道线即左边车道线L1之间的第一距离值为D1,以及确定目标车辆A与第二车道线即Y右边车道线L2之间的第二距离值为D2。
进一步的,终端可以判断第一距离值D1和第二距离值D2是否满足可信距离条件,当第一距离值D1和第二距离值D2满足可信距离条件时,终端可以基于第一距离值D1、第二距离值D2和车道宽度D,重新确定目标定位点S。终端确定目标定位点S之后,终端可以在HD地图的候选车道中,选取与该目标定位点S匹配的目标车道1,并在目标车道1中确定与该目标定位点S对应的匹配点信息为SL,终端可以将匹配点信息为SL作为新的目标观测信息,并根据该新的目标观测信息对预测定位信息进行校正,以得到目标车辆在第二时刻更加准确的定位信息。由此使得,可以实现动态的选取可信度最高的观测信息作为独立的观测信息,对滤波器状态进行校正,即利用多种独立的观测信息对状态进行校正,使得估计的状态会比较精确,同时,多种观测的参与,可以使得状态估计不单独依赖某种观测,可以有效避免错误观测对状态的影响。
在一个实施例中,在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与所述预测定位信息对应的匹配点信息的步骤,包括:
当电子地图为第二精度地图时,在第二电子地图的候选车道中选取与预测定位信息匹配的目标车道;
在目标车道中确定与预测定位信息对应的匹配点信息。
具体的,当第二精度地图为HD地图时,终端可以在HD地图中,选择一条车道作为目标车道,并将预测定位信息与目标车道进行匹配,即可得到匹配点,该匹配点的信息包括经纬度信息和航向信息。例如,终端中的HD地图匹配模块可以基于滤波器输出的预测定位信息和HD地图,计算出目标车辆可能位于的车道,并对滤波器状态进行后验估计,以得到更加精确的目标车辆的定位信息,可以理解,以上只是本申请方案中的一种可能的实现方式。由此使得,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。比如,当存在HD地图时,可以自适应的实现车道级定位和导航。
在一个实施例中,根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息的步骤,包括:
确定观测矩阵和观测噪声;
根据匹配点信息、目标滤波器在第一时刻的预测协方差矩阵、观测矩阵和观测噪声,确定滤波器状态的校正误差;
根据校正误差对预测状态进行校正,得到校正后预测状态;
根据校正后预测状态,确定目标车辆在第二时刻的定位信息。
其中,观测矩阵和观测噪声具体取值可以是自定义设置的,例如,当基于GNSS对卡尔曼滤波器进行观测校正时,观测噪声可以根据GNSS的质量进行调整,即低质量GNSS对应较大的观测噪声,高质量GNSS对应较小的观测噪声。
具体的,终端在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息之后,终端可以判断该匹配点信息是否可信,当终端判断该匹配点信息可信时,终端可以确定观测矩阵和观测噪声,并根据匹配点信息、目标滤波器在第一时刻的预测协方差矩阵、观测矩阵和观测噪声,确定滤波器状态的校正误差;进一步的,终端可以根据校正误差对预测状态进行校正,得到校正后预测状态,并根据校正后预测状态,确定目标车辆在第二时刻的定位信息。
其中,卡尔曼观测校正的方程如下公式(6)所示:
Z=H*△X+w (6)
其中,Z为GNSS的观测,H为观测矩阵,w为观测噪声,△X为状态校正误差。
本实施例中,状态校正需要独立的观测信息参与,比如独立的GPS接收机、提供的SD地图匹配、视觉信息等,这些信息经过处理,都可以作为独立的观测信息,对波器状态进行校正。由于GPS的数据采集、SD地图的匹配、视觉信息的采集之间是互相独立的,所以相应的校正都可以是独立的。即本申请实施例中利用多种独立的观测信息对状态进行校正,使得估计的状态会比较精确。其次,多种观测的参与,可以使得状态估计不单独依赖某种观测,可以有效避免错误观测对状态的影响,使得估计的状态更加精确。
在一个实施例中,进行GNSS观测校正时,终端可以根据如下所示的公式(7)计算出状态校正误差△X,对滤波器的状态进行校正,并对协方差进行校正。同时,对里程计坐标系和东北天坐标之间的旋转变换关系T进行校正,具体参考如下所示的公式(7):
其中,Xk+1|k表示k+1时刻的预测状态,Xk+1表示k+1时刻校正后的预测状态,△X为状态校正误差;Tk+1表示k+1时刻的里程计坐标系和东北天坐标之间的旋转变换关系,Tk表示k时刻的里程计坐标系和东北天坐标之间的旋转变换关系,T(Δθ)表示与Δθ相关的旋转变换关系的增量。
相较于传统方式中只利用GPS对状态进行估计,并不对一些隐变量进行校正,而本实施例中,可以利用各个观测信息,通过相似的方法对滤波器状态进行校正,包括经纬度、航向,还有一些隐变量,例如,隐变量为坐标系之间的变换关系,多种观测的参与,可以使得状态估计不单独依赖某种观测,可以有效避免错误观测对状态的影响,使得估计的状态更加精确。
在一个实施例中,所述方法还包括:
获取初始协方差矩阵和初始状态转移矩阵;
基于初始协方差矩阵、初始状态转移矩阵、里程计的线性量、里程计的测量噪声,确定目标滤波器在第一时刻的预测协方差矩阵。
具体的,终端对滤波器进行初始化,初始化完成后,当终端获取到新里程计数据时,终端可以基于初始时刻的滤波器状态,进行卡尔曼滤波器递推,以推算出下一时刻的预测状态,在终端进行卡尔曼滤波器递推时,终端还可以获取初始协方差矩阵和初始状态转移矩阵,并基于初始协方差矩阵、初始状态转移矩阵、里程计的线性量、里程计的测量噪声,确定目标滤波器在第一时刻的预测协方差矩阵。其中,卡尔曼状态递推可以采用如前述公式(1)所示的函数进行递推。由此使得,通过使用卡尔曼滤波方法作为定位框架,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失。在本申请提供的方法框架中,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。
在一个实施例中,如图4所示,根据校正误差对预测状态进行校正,得到校正后预测状态的步骤,包括:
步骤402,当匹配点信息和预测定位信息之间的横向距离小于或者等于预设阈值时,根据校正误差对预测状态进行纵向矫正,得到校正后预测状态;
步骤404,当匹配点信息和预测定位信息之间的横向距离大于预设阈值时,根据校正误差对预测状态分别进行纵向矫正和横向矫正,得到校正后预测状态。
其中,纵向矫正是指是在行驶方向的位置进行校正,横向校正是指在横向方向的位置进行校正,横向是指与车辆行驶方向垂直的方向,纵向是指与行驶方向平行的方向。
具体的,终端在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息之后,终端可以判断该匹配点信息是否可信,当终端判断该匹配点信息可信时,终端可以进一步判断匹配点信息和预测定位信息之间的横向距离是否小于预设阈值,当终端确定匹配点信息和预测定位信息之间的横向距离小于或者等于预设阈值时,终端可以根据校正误差对预测状态进行纵向矫正,得到校正后预测状态;当终端确定匹配点信息和预测定位信息之间的横向距离大于预设阈值时,终端可以根据校正误差对预测状态分别进行纵向矫正和横向矫正,得到校正后预测状态。
举个例子,如图3所示,为对预测定位信息进行校正处理的示意图。图3中的目标车道为车道1,车道1的左边车道线记为L1,车道1的右边车道线记为L2,左边车道线L1和右边车道线L2之间的距离为D,即车道宽度为D。当G1与G2之间的距离d>td时,G1点为GNSS观测点(实际观测点),当d<=td时,G2为GNSS观测点(虚拟GNSS观测点)。G1G2连线与航向方向垂直。
本实施例中,通过判断匹配点信息和预测定位信息之间的横向距离是否小于预设阈值,可以实现动态的采用不同策略对预测定位信息进行校正,使得最终输出的车辆定位信息更加准确。
本申请还提供一种应用场景,该应用场景应用上述的车辆定位方法。具体地,该车辆定位方法在该应用场景的应用如下:
当驾驶者需要对车辆进行定位或者实时路线导航时,可以采用上述的车辆定位方法,即驾驶员登录车载终端中的导航***之后,车载终端可以自动获取目标车辆在行驶过程中的里程计数据和GNSS数据,并根据获取得到的里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态,车载终端可以根据预测状态确定目标车辆在第二时刻的预测定位信息,并在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息,并根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息;或者车载终端可以根据获取到的GNSS数据对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息。其中,车载终端中的导航***可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
本申请实施例提供的方法,可以应用于各种定位和导航场景中,也可以应用于室外无人车定位和导航、机器人定位导航等应用中。以下以AR导航场景下基于电子地图数据的自适应定位和导航场景为例,对本申请实施例提供的车辆定位方法进行说明。
传统方式中,通常需要利用多个卡尔曼滤波器进行多传感器融合定位,并对多个卡尔曼滤波器的输出进行加权,得到稳定的定位信息,相当于进行了多重保证。但传统方式中的每个滤波器都会要求有速度、角速度等参数输入,且独立连接各自的IMU,相当于要求车辆拥有多套导航硬件,这对车辆的硬件有一定要求。此外,在传统方式中利用卡尔曼滤波进行多传感器融合定位时,把定位的状态量定义在了类似东北天坐标系的笛卡尔坐标系中,由于采集到的定位信息定义在WGS84坐标系中,因此,从WGS坐标系变换到ENU坐标系或者进行相反方向变换,都需要估计的定位信息参与,这样可能会带来误差。
因此,基于上述方式进行车辆定位和导航时,容易导致得到的车辆定位信息的准确性较低,并且需要较高的硬件设备成本。为了解决上述问题,本申请提供了一种基于卡尔曼(EKF)的定位方法,该方法对里程计信号、GNSS定位信号、SD地图信号、视觉感知信号、HD高精地图等信号的一种或几种进行融合,能够实现道路级的定位和车道级的定位,即本申请提供的方案,把视觉信息、SD地图匹配信息、GNSS定位信息都当作观测信息,对滤波器状态进行校正,这些校正互相独立不受影响。当存在GNSS观测、视觉感知和HD地图匹配观测信息时,本申请中的方法可以用于实现车道级定位;当存在GNSS观测或者同时存在GNSS观测和SD地图匹配观测信息时,本申请中的方法可以实现道路级定位和导航,可以在地库、隧道等无GNSS信号的场景下进行定位和导航,上述的这些观测可以灵活配置,实现不同功能。并且,本申请提供的方法中的定位状态直接定义在WGS84坐标系下,卡尔曼滤波器的递推和校正过程均在该坐标系下进行,避免了坐标变换时的误差。其次,从SD地图或者利用视觉信息与HD地图得到的定位信息也均在WGS84坐标下,可以直接参与卡尔曼校正,有效避免了坐标变换时的误差,使得估计的状态会更加精确,即本申请中利用多种独立的观测信息对状态进行校正,使得估计的状态会更加精确。其次,多种观测的参与,可以使得状态估计不单独依赖某种观测,可以有效避免错误观测对状态的影响,能够实现在降低硬件设备成本的同时又有效提升了车辆定位信息准确性。
在产品侧,本申请提供的技术方案可以广泛应用于各种定位和导航中。结合里程计数据、GNSS数据、SD地图数据,本申请提供的技术方案可以应用于车载实时定位和导航中,可以在GNSS质量比较差的场景,比如隧道、地库等场景中提供实时定位和导航的服务。如果结合视觉感知车道线以及与HD地图的匹配,再结合里程计数据、GNSS数据,本申请提供的方法可以应用于车道级定位和导航产品中,如图5所示,为车道级定位和导航的示意图。另外,本申请提供的方案是一种通用的定位框架方案,也可以应用于室外无人车定位和导航、机器人定位导航等应用中。
在技术侧,如图6所示,为应用于道路级定位的方法框架示意图。图6中的数据采集模块:用于采集里程计数据和全球导航定位数据即GNSS数据。SD地图匹配模块:用于把卡尔曼滤波器输出的定位位置与SD地图的link进行匹配,并反馈给滤波器进行校正处理。卡尔曼滤波器模块:用于实现多传感器融合、定位输出。初始化模块:用于初始化卡尔曼滤波器。
滤波器会输出定位信息,比如P,P包括经纬度和航向信息。SD地图匹配模块利用该定位信息P,在SD地图上选择一条link进行匹配,相当于给定位信息绑到一条可行使的道路上,并返回一个匹配的定位点,匹配的定位点包括经纬度和航向等信息,后续滤波器可以利用反馈的定位点信息,对滤波器状态进行校正。
可以理解,在进行SD地图匹配时,把定位点向link上投影,类似直线外一点向直线投影,投影点就是匹配点,只是选择哪条link进行投影,需要SD地图匹配模块根据自身算法来确定,当滤波器得到SD地图匹配模块反馈的匹配点数据时,进行标准的EKF观测校正。
如图7所示,为应用于车道级定位的方法框架示意图。图7中的视觉HD地图匹配模块:用于对视觉感知模块感知的车道线信息进行处理,并与HD地图进行匹配,以得到高精度的定位信息,并反馈给滤波器进行校正处理。
视觉HD地图匹配模块与SD地图匹配模块的功能作用类似,本发明对具体实现方式不做限定。
例如,在具体实现时,视觉HD地图匹配模块可以利用贝叶斯滤波算法,对输入的视觉信息、滤波器输出的定位信息进行融合,并与HD地图进行匹配,即视觉HD地图匹配模块在HD地图中选择一条车道进行匹配,得到匹配点,匹配点的信息包括经纬度、航向等信息。视觉HD地图匹配模块得到匹配点信息后,可以反馈给滤波器,以使得滤波器利用标准EKF校正算法,对滤波器的状态进行校正处理。
其中,输入的视觉信息包括车道数、车道宽度、车道方程、车辆距离该车道两边车道线的垂直距离等信息。视觉HD地图匹配模块利用贝叶斯框架,维护一个状态向量,该状态向量中保存了车辆位于各个车道的概率。在车辆行驶过程中,状态本身进行状态转移,得到状态的先验信息。该模块可以把视觉信息作为观测信息,利用该信息进行贝叶斯后验估计。同样的,该模块也可以把滤波器输出的定位信息与HD地图进行匹配,计算出车辆可能的车道,并同样对状态进行后验估计,以得到贝叶斯后验估计的高精度的定位信息,可以理解,以上只是本申请方案中的一种可能的实现方式。
本申请提供的一种基于卡尔曼(EKF)的定位方法,具体包括以下步骤:
第一步,采集GNSS数据和里程计数据。
里程计数据包括位置数据、航向数据、时间戳,GNSS数据包括时间戳、有效的经度、纬度信息,以及航向角信息。为了方便描述,终端可以将里程计数据记为O=(x,y,yaw),其中x表示目标车辆在里程计坐标系下X轴的坐标值,y表示目标车辆在里程计坐标系下Y轴的坐标值,yaw表示目标车辆在里程计坐标系下的航向角。同时,终端可以将GNSS数据记为G=(lonG,latG,CG),其中lonG表示GNSS的经度值,latG表示GNSS的纬度值,CG表示GNSS的航向值。
上述的GNSS数据和里程计数据不需要同频率,比如里程计数据是10Hz,而GNSS数据可以是1Hz。可以理解,在大部分情况下,传感器的频率是不同的。比如,在VIO***中,用于状态传播的传感器IMU可以到达100HZ,而GPS一般只有10HZ或者更低的1Hz。由于本申请实施例中,可以独立的使用多种信息进行状态校正,使得本实施例中一般情况下估计的状态是比较准确的,所以不用对每个观测量要求高频率,故本申请实施例中对量数据的具体频率无要求。
第二步,如果***没有初始化,终端需要对滤波器进行初始化。
初始化过程包括:终端确定一个质量较好的GNSS点作为起始点,并将起始点的航向信息和位置信息作为卡尔曼滤波器的初始状态X中的初始值;同时,终端确定里程计坐标系和东北天(ENU)坐标系之间的变换关系T、确定里程计数据的误差值,以及确定卡尔曼滤波器状态的初始值以及初始协方差矩阵。即本申请实施例中的卡尔曼滤波器状态定义为X=(C,lon,lat,T),即状态中包括车辆航向角、经度、纬度,以及里程计坐标系和东北天(ENU)坐标系之间的变换关系T或者是等价的数据。在对滤波器初始化时,终端会确定状态初始值和初始协方差矩阵。比如,终端选择一个质量较好的GNSS定位点P,将定位点P的经纬度、航向作为滤波器状态的初始值,坐标系之间的变换也是滤波器状态的一部分。终端在确定初始协方差矩阵时,基于GNSS的误差,选择一个初始值作为初始协方差矩阵。
其中,本申请实施例中确定GNSS质量的方法包括利用GPS轨迹的平滑性、GSV中的信噪比信息,或者利用独立的其他信息(比如里程计数据),考察两者轨迹形状是否吻合,从而确定一个质量较好的GNSS点。里程计坐标系与ENU一样,都是笛卡尔坐标系,二者之间的转换符合刚体变换。
例如,如图8所示,为里程计坐标系和ENU坐标系之间的坐标变换示意图。比如,某个O1点,里程计坐标系是X-O-Y坐标系,ENU坐标系是E-O-N坐标系,里程计坐标系的Z轴和ENU坐标系的U轴重合,垂直于纸面,里程计里包括本坐标系下的航向和位置信息,ENU坐标系里有航向信息,可以通过这两个值确定两个坐标系之间的变换关系T。
第三步,初始化完成后,采集到新里程计数据,进行卡尔曼滤波器递推。
其中卡尔曼状态递推可以用如前述公式(1)所示的方式进行递推。其中,公式(1)中的第二个公式是标准的卡尔曼协方差传播公式,第一个公式是一个概述表示,具体形式与里程计数据的具体意义以及选择的变换公式有关。函数f是一个符号,表示由当前状态,结合里程计数据,得到下一时刻估计的状态。本申请实施例中,f表示一个非线性的变换过程。
可以理解,本实施例中选择的EKF算法进行定位估计,类似的实现,还有多状态的优化算法等,但相应的计算量会比较大,在某些情况下,也可以使用其他的算法进行定位估计。
在其中一个实施例中,选择了函数f的一种具体实现方式,如下所示:
假设里程计坐标系的Z轴与ENU坐标系的UP轴重合,那么里程计坐标系和ENU坐标系之间的变换关系T为一个二阶旋转矩阵,且与一个角度有关系,记为θ,θ表示ENU坐标系的E轴与里程计坐标系的X轴的夹角,此时卡尔曼滤波器的状态记为X=(C,lon,lat,θ),且满足前述所示的公式(3)。
函数f的一种可行实现方式如下:
终端可以根据前述公式(2)所示变换方式,把里程计增量从里程计坐标系变换到ENU坐标系下。
进一步的,终端将变换后的ENU坐标系下里程计增量输入前述公式(1)卡尔曼滤波的状态递推公式中Xk+1=f(Xk,ΔO),即可得到函数f的一种可行实现方式如前述公式(4)所示。从公式(4)看,状态转移是非线性关系,那么卡尔曼滤波器变为扩展卡尔曼滤波器。相应的状态转移矩阵和噪声系数矩阵如前述公式(5)所示。
第四步,当新的GNSS观测到来时,进行卡尔曼观测校正。
卡尔曼观测校正方程如前述公式(6)所示,可以理解,具体计算量测误差时,每种方案会有自己的思考。比如,本申请的技术方案在计算有关GNSS的量测误差时,考虑了GNSS的固有特点,当GNSS定位点与卡尔曼滤波器定位点之间的横向距离小于一定值时,不进行横向校正,即Z中不包括横向信息。这是本申请中的一个优化点。
当终端基于GNSS进行卡尔曼观测校正时,观测噪声根据GNSS的质量进行调整,即低质量GNSS对应较大的观测噪声,高质量GNSS对应较小的观测噪声。本实施例中对GNSS的质量判断方法不做具体限定。
在终端进行GNSS观测校正时,考虑GNSS定位的误差,当终端确定GNSS定位点和卡尔曼状态定位点之间的横向距离小于阈值td时,GNSS观测不进行横向校正,只进行纵向校正。反之,则既进行横向校正,又进行纵向校正。其中,td的取值与GNSS经度有关,本发明不做具体的取值限定。横向是指与车辆行驶方向垂直的方向,纵向是指与行驶方向平行的方向。横向校正是指,在横向方向的位置进行校正,纵向校正是指在行驶方向的位置进行校正。
此外,在终端进行GNSS观测校正时,计算出状态校正误差,对状态进行校正,并对协方差按照标准卡尔曼校正形式进行校正。同时,终端也会对里程计坐标系和东北天坐标之间的旋转变换关系T进行校正,具体参考下前述公式(7)所示的校正方式。
第五步,可选地,当存在SD地图时,基于SD地图的匹配信息,进行SD地图匹配校正。
SD地图匹配指的是根据卡尔曼滤波器输出的定位信息,与SD地图的link进行匹配,选择最合适的link,计算相应的吸附点的定位信息。吸附点定位信息包括路网方向、吸附点的经度和纬度信息。
SD地图匹配校正,是根据SD地图匹配输出的吸附点的经度、纬度、道路link的航向角,按照前述公式(6)对滤波器的状态进行校正。
SD地图匹配校正时,若终端确定当前状态的航向角与道路link的航向角的差,大于一个选定的阈值,则认为SD地图匹配输出的航向角无效,对应的航向角量测误差为0。
第六步,可选地,当存在视觉感知时,终端将视觉感知与HD地图进行匹配,使用匹配结果对卡尔曼滤波器状态进行校正。
视觉感知是指对道路信息的感知,包括车道线、车道数、当前车道宽度、车辆距离左右车道线的距离、当前所在车道号等要素。终端把视觉感知结果与HD地图进行匹配,可以计算出当前车辆所在的位置。
视觉感知与HD地图匹配模块的输出包括定位点经度、纬度、车道宽度、车道号、车辆距离左右车道线的距离、车道航向角以及定位可信概率。例如,如果当前反馈的定位点可信,则终端可以直接利用公式(6)进行卡尔曼校正。如果当前定位点不可信,但车辆距左右车道的距离可信,则终端可以用左右距离和车道宽度计算出一个观测信息,然后基于观测信息,按照公式(6)进行卡尔曼校正。此外,如果当前状态的航向角与反馈的车道航向角差距大于一定值,则可以确定车道航向角不可靠,相应的航向角量测误差为0。例如,当车辆进行变道时,即是上述这种情况,如图9所示,为车道级变道指引的示意图,例如,目标车辆的前方500米有交叉路,为了提醒出行对象提前变道,则终端可以在目标车辆前方200米的位置处显示车道级变道指引,可参考图9的(a)图。
在终端进行视觉感知观测校正时,终端计算量测误差矩阵时,可以按照反馈的概率进行调整,较大概率对应较小误差,较小概率对应较大误差。
本实施例中,通过使用卡尔曼滤波方法作为定位框架,实现了对多种信号和数据的融合,综合了多种信号的优势,可以避免单一信号的缺失。在本申请提供的方法框架中,可以灵活配置观测信号,实现自适应不同场合、不同精度的定位方案。比如,当存在视觉感知和HD地图时,本申请可以应用于车道级定位和导航产品中。同时,本申请中的状态直接定义在WGS坐标系下,避免了传统方法中坐标变换带来的误差。最后,由于本申请实施例中使用的卡尔曼滤波方案,状态量少,维度小,计算量小,属于一种轻量级、应用广泛的定位方案。
应该理解的是,虽然如上所述的各实施例所涉及的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,如上所述的各实施例所涉及的流程图中的至少一部分步骤可以包括多个步骤或者多个阶段,这些步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
基于同样的发明构思,本申请实施例还提供了一种用于实现上述所涉及的车辆定位方法的车辆定位装置。该装置所提供的解决问题的实现方案与上述方法中所记载的实现方案相似,故下面所提供的一个或多个车辆定位装置实施例中的具体限定可以参见上文中对于车辆定位方法的限定,在此不再赘述。
在一个实施例中,如图10所示,提供了一种车辆定位装置,包括:获取模块1002、确定模块1004和校正模块1006,其中:
获取模块1002,用于获取目标车辆在行驶过程中的里程计数据。
确定模块1004,用于根据里程计数据和目标滤波器在第一时刻的滤波器状态,确定目标滤波器在第二时刻的预测状态;根据预测状态确定目标车辆在第二时刻的预测定位信息;在电子地图的候选路段中选取与预测定位信息匹配的目标路段,并在目标路段中确定与预测定位信息对应的匹配点信息。
校正模块1006,用于根据匹配点信息对预测定位信息进行校正,得到目标车辆在第二时刻的定位信息;定位信息与滤波器状态定义在相同坐标系。
在一个实施例中,所述装置还包括:初始化模块。获取模块还用于获取目标车辆在行驶过程中的历史里程计数据和导航定位数据;初始化模块,用于基于所述历史里程计数据和所述导航定位数据,对所述滤波器进行初始化;在初始化过程中,从所述导航定位数据中选取目标位置作为起始定位点;基于所述起始定位点的航向信息和位置信息,确定初始滤波器状态。
在一个实施例中,确定模块还用于基于不同时刻的所述里程计数据,确定里程计增量;根据所述里程计增量和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态。
在一个实施例中,所述装置还包括:坐标变换模块,用于根据坐标变换关系对所述里程计增量进行坐标变换,得到在所述目标坐标系下的里程计增量;所述坐标变换关系是里程计坐标系和目标坐标系之间的变换关系;确定模块还用于根据所述目标坐标系下的里程计增量和所述目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态。
在一个实施例中,所述装置还包括:选取模块,用于当所述电子地图为第一精度地图时,在所述第一精度地图的候选路段中选取与所述预测定位信息匹配的目标路段;确定模块还用于确定所述预测定位信息在所述目标路段中的投影点信息;将所述投影点信息作为与所述预测定位信息对应的匹配点信息。
在一个实施例中,所述装置还包括:融合模块。获取模块还用于当所述电子地图为第二精度地图时,获取携带车道信息的视觉信息;融合模块,用于将所述视觉信息与所述预测定位信息进行融合,得到融合后的定位信息;选取模块还用于在所述第二精度地图的候选车道中,选取与融合后的所述定位信息匹配的目标车道;确定模块还用于在所述目标车道中确定与融合后的所述定位信息对应的匹配点信息。
在一个实施例中,所述装置还包括:执行模块,用于当确定所述匹配点信息的可信度大于可信度阈值时,执行根据所述匹配点信息对所述预测定位信息进行校正的步骤;确定模块还用于当确定所述匹配点信息的可信度小于或等于可信度阈值时,确定所述目标车辆与第一车道线之间的第一距离值,以及确定所述目标车辆与第二车道线之间的第二距离值;当所述第一距离值和所述第二距离值满足可信距离条件时,基于所述第一距离值、所述第二距离值和车道宽度,确定目标观测信息;所述车道宽度是第一车道线和所述第二车道线之间的距离;校正模块还用于根据所述目标观测信息对所述预测定位信息进行校正,得到所述目标车辆在所述第二时刻的定位信息。
在一个实施例中,选取模块还用于当所述电子地图为第二精度地图时,在所述第二电子地图的候选车道中选取与所述预测定位信息匹配的目标车道;确定模块还用于在所述目标车道中确定与所述预测定位信息对应的匹配点信息。
在一个实施例中,确定模块还用于确定观测矩阵和观测噪声;根据所述匹配点信息、所述目标滤波器在第一时刻的预测协方差矩阵、所述观测矩阵和所述观测噪声,确定所述滤波器状态的校正误差;校正模块还用于根据所述校正误差对所述预测状态进行校正,得到校正后预测状态;确定模块还用于根据所述校正后预测状态,确定所述目标车辆在所述第二时刻的定位信息。
在一个实施例中,获取模块还用于获取初始协方差矩阵和初始状态转移矩阵;确定模块还用于基于所述初始协方差矩阵、所述初始状态转移矩阵、所述里程计的线性量、所述里程计的测量噪声,确定所述目标滤波器在第一时刻的预测协方差矩阵。
在一个实施例中,矫正模块还用于当所述匹配点信息和所述预测定位信息之间的横向距离小于或者等于预设阈值时,根据所述校正误差对所述预测状态进行纵向矫正,得到校正后预测状态;当所述匹配点信息和所述预测定位信息之间的横向距离大于预设阈值时,根据所述校正误差对所述预测状态分别进行纵向矫正和横向矫正,得到校正后预测状态。
上述车辆定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是终端,其内部结构图可以如图11所示。该计算机设备包括处理器、存储器、输入/输出接口、通信接口、显示单元和输入装置。其中,处理器、存储器和输入/输出接口通过***总线连接,通信接口、显示单元和输入装置通过输入/输出接口连接到***总线。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***和计算机程序。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的输入/输出接口用于处理器与外部设备之间交换信息。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信,无线方式可通过WIFI、移动蜂窝网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种车辆定位方法。该计算机设备的显示单元用于形成视觉可见的画面,可以是显示屏、投影装置或虚拟现实成像装置,显示屏可以是液晶显示屏或电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图11中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,还提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中,提供了一种计算机可读存储介质,存储有计算机程序,该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
在一个实施例中,提供了一种计算机程序产品或计算机程序,该计算机程序产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令,处理器执行该计算机指令,使得该计算机设备执行上述各方法实施例中的步骤。
需要说明的是,本申请所涉及的用户信息(包括但不限于用户设备信息、用户个人信息等)和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等),均为经用户授权或者经过各方充分授权的信息和数据,且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用,均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-OnlyMemory,ROM)、磁带、软盘、闪存、光存储器、高密度嵌入式非易失性存储器、阻变存储器(ReRAM)、磁变存储器(Magnetoresistive Random Access Memory,MRAM)、铁电存储器(Ferroelectric Random Access Memory,FRAM)、相变存储器(Phase Change Memory,PCM)、石墨烯存储器等。易失性存储器可包括随机存取存储器(Random Access Memory,RAM)或外部高速缓冲存储器等。作为说明而非局限,RAM可以是多种形式,比如静态随机存取存储器(Static Random Access Memory,SRAM)或动态随机存取存储器(Dynamic RandomAccess Memory,DRAM)等。本申请所提供的各实施例中所涉及的数据库可包括关系型数据库和非关系型数据库中至少一种。非关系型数据库可包括基于区块链的分布式数据库等,不限于此。本申请所提供的各实施例中所涉及的处理器可为通用处理器、中央处理器、图形处理器、数字信号处理器、可编程逻辑器、基于量子计算的数据处理逻辑器等,不限于此。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请的保护范围应以所附权利要求为准。
Claims (15)
1.一种车辆定位方法,其特征在于,所述方法包括:
获取目标车辆在行驶过程中的里程计数据;
根据所述里程计数据和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态;
根据所述预测状态确定所述目标车辆在所述第二时刻的预测定位信息;
在电子地图的候选路段中选取与所述预测定位信息匹配的目标路段,并在所述目标路段中确定与所述预测定位信息对应的匹配点信息;
根据所述匹配点信息对所述预测定位信息进行校正,得到所述目标车辆在所述第二时刻的定位信息;所述定位信息与所述滤波器状态定义在相同坐标系。
2.根据权利要求1所述的方法,其特征在于,所述获取目标车辆在行驶过程中的里程计数据之前,所述方法还包括:
获取目标车辆在行驶过程中的历史里程计数据和导航定位数据;
基于所述历史里程计数据和所述导航定位数据,对所述滤波器进行初始化;
在初始化过程中,从所述导航定位数据中选取目标位置作为起始定位点;
基于所述起始定位点的航向信息和位置信息,确定初始滤波器状态。
3.根据权利要求1所述的方法,其特征在于,所述里程计数据包括不同时刻的里程计数据;
所述根据所述里程计数据和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态,包括:
基于不同时刻的所述里程计数据,确定里程计增量;
根据所述里程计增量和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态。
4.根据权利要求3所述的方法,其特征在于,所述根据所述里程计增量和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态,包括:
根据坐标变换关系对所述里程计增量进行坐标变换,得到在所述目标坐标系下的里程计增量;所述坐标变换关系是里程计坐标系和目标坐标系之间的变换关系;
根据所述目标坐标系下的里程计增量和所述目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态。
5.根据权利要求1所述的方法,其特征在于,所述在电子地图的候选路段中选取与所述预测定位信息匹配的目标路段,并在所述目标路段中确定与所述预测定位信息对应的匹配点信息,包括:
当所述电子地图为第一精度地图时,在所述第一精度地图的候选路段中选取与所述预测定位信息匹配的目标路段;
确定所述预测定位信息在所述目标路段中的投影点信息;
将所述投影点信息作为与所述预测定位信息对应的匹配点信息。
6.根据权利要求1所述的方法,其特征在于,所述在电子地图的候选路段中选取与所述预测定位信息匹配的目标路段,并在所述目标路段中确定与所述预测定位信息对应的匹配点信息,包括:
当所述电子地图为第二精度地图时,获取携带车道信息的视觉信息;
将所述视觉信息与所述预测定位信息进行融合,得到融合后的定位信息;
在所述第二精度地图的候选车道中,选取与融合后的所述定位信息匹配的目标车道;
在所述目标车道中确定与融合后的所述定位信息对应的匹配点信息。
7.根据权利要求6所述的方法,其特征在于,所述在所述目标车道中确定与融合后的所述定位信息对应的匹配点信息之后,所述方法还包括:
当确定所述匹配点信息的可信度大于可信度阈值时,执行根据所述匹配点信息对所述预测定位信息进行校正的步骤;
当确定所述匹配点信息的可信度小于或等于可信度阈值时,确定所述目标车辆与第一车道线之间的第一距离值,以及确定所述目标车辆与第二车道线之间的第二距离值;
当所述第一距离值和所述第二距离值满足可信距离条件时,基于所述第一距离值、所述第二距离值和车道宽度,确定目标观测信息;所述车道宽度是第一车道线和所述第二车道线之间的距离;
根据所述目标观测信息对所述预测定位信息进行校正,得到所述目标车辆在所述第二时刻的定位信息。
8.根据权利要求1所述的方法,其特征在于,所述在电子地图的候选路段中选取与所述预测定位信息匹配的目标路段,并在所述目标路段中确定与所述预测定位信息对应的匹配点信息,包括:
当所述电子地图为第二精度地图时,在所述第二电子地图的候选车道中选取与所述预测定位信息匹配的目标车道;
在所述目标车道中确定与所述预测定位信息对应的匹配点信息。
9.根据权利要求1所述的方法,其特征在于,所述根据所述匹配点信息对所述预测定位信息进行校正,得到所述目标车辆在所述第二时刻的定位信息,包括:
确定观测矩阵和观测噪声;
根据所述匹配点信息、所述目标滤波器在第一时刻的预测协方差矩阵、所述观测矩阵和所述观测噪声,确定所述滤波器状态的校正误差;
根据所述校正误差对所述预测状态进行校正,得到校正后预测状态;
根据所述校正后预测状态,确定所述目标车辆在所述第二时刻的定位信息。
10.根据权利要求9所述的方法,其特征在于,所述方法还包括:
获取初始协方差矩阵和初始状态转移矩阵;
基于所述初始协方差矩阵、所述初始状态转移矩阵、所述里程计的线性量、所述里程计的测量噪声,确定所述目标滤波器在第一时刻的预测协方差矩阵。
11.根据权利要求10所述的方法,其特征在于,所述根据所述校正误差对所述预测状态进行校正,得到校正后预测状态包括:
当所述匹配点信息和所述预测定位信息之间的横向距离小于或者等于预设阈值时,根据所述校正误差对所述预测状态进行纵向矫正,得到校正后预测状态;
当所述匹配点信息和所述预测定位信息之间的横向距离大于预设阈值时,根据所述校正误差对所述预测状态分别进行纵向矫正和横向矫正,得到校正后预测状态。
12.一种车辆定位装置,其特征在于,所述装置包括:
获取模块,用于获取目标车辆在行驶过程中的里程计数据;
确定模块,用于根据所述里程计数据和目标滤波器在第一时刻的滤波器状态,确定所述目标滤波器在第二时刻的预测状态;根据所述预测状态确定所述目标车辆在所述第二时刻的预测定位信息;在电子地图的候选路段中选取与所述预测定位信息匹配的目标路段,并在所述目标路段中确定与所述预测定位信息对应的匹配点信息;
校正模块,用于根据所述匹配点信息对所述预测定位信息进行校正,得到所述目标车辆在所述第二时刻的定位信息;所述定位信息与所述滤波器状态定义在相同坐标系。
13.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至11中任一项所述的方法的步骤。
14.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至11中任一项所述的方法的步骤。
15.一种计算机程序产品,包括计算机程序,其特征在于,该计算机程序被处理器执行时实现权利要求1至11中任一项所述的方法的步骤。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210929163.2A CN115326084A (zh) | 2022-08-03 | 2022-08-03 | 车辆定位方法、装置、计算机设备、存储介质 |
PCT/CN2023/100444 WO2024027350A1 (zh) | 2022-08-03 | 2023-06-15 | 车辆定位方法、装置、计算机设备、存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210929163.2A CN115326084A (zh) | 2022-08-03 | 2022-08-03 | 车辆定位方法、装置、计算机设备、存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115326084A true CN115326084A (zh) | 2022-11-11 |
Family
ID=83922159
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210929163.2A Pending CN115326084A (zh) | 2022-08-03 | 2022-08-03 | 车辆定位方法、装置、计算机设备、存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN115326084A (zh) |
WO (1) | WO2024027350A1 (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116222545A (zh) * | 2023-05-10 | 2023-06-06 | 北京白水科技有限公司 | 用于群组导航定位的智能地标设备 |
CN116380088A (zh) * | 2023-06-05 | 2023-07-04 | 小米汽车科技有限公司 | 车辆定位方法、装置、车辆及存储介质 |
WO2024027350A1 (zh) * | 2022-08-03 | 2024-02-08 | 腾讯科技(深圳)有限公司 | 车辆定位方法、装置、计算机设备、存储介质 |
CN118067148A (zh) * | 2024-04-17 | 2024-05-24 | 深圳市科乐达电子科技有限公司 | 一种基于大数据分析的车载导航*** |
CN118168566A (zh) * | 2024-05-14 | 2024-06-11 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、装置、电子设备及存储介质 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102682590B (zh) * | 2011-03-16 | 2014-04-30 | 高德软件有限公司 | 实时交通信息处理方法和装置 |
KR102565482B1 (ko) * | 2018-03-06 | 2023-08-11 | 현대모비스 주식회사 | 차량의 측위 장치 및 그 방법 |
CN108827292A (zh) * | 2018-06-27 | 2018-11-16 | 四川大学 | 基于gnss和地面基站的组合式导航精确测速定位方法及*** |
CN109581449B (zh) * | 2018-12-14 | 2021-09-14 | 安徽江淮汽车集团股份有限公司 | 一种自动驾驶汽车的定位方法及*** |
CN110986930B (zh) * | 2019-11-29 | 2022-07-15 | 北京三快在线科技有限公司 | 设备定位方法、装置、电子设备及存储介质 |
CN110865405A (zh) * | 2019-11-29 | 2020-03-06 | 浙江商汤科技开发有限公司 | 融合定位方法及装置、移动设备控制方法及电子设备 |
CN113551680A (zh) * | 2020-04-23 | 2021-10-26 | 上汽通用汽车有限公司 | 一种车道级定位***和方法 |
CN111596329A (zh) * | 2020-06-10 | 2020-08-28 | 中国第一汽车股份有限公司 | 车辆定位方法、装置、设备及车辆 |
CN115326084A (zh) * | 2022-08-03 | 2022-11-11 | 腾讯科技(深圳)有限公司 | 车辆定位方法、装置、计算机设备、存储介质 |
-
2022
- 2022-08-03 CN CN202210929163.2A patent/CN115326084A/zh active Pending
-
2023
- 2023-06-15 WO PCT/CN2023/100444 patent/WO2024027350A1/zh unknown
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024027350A1 (zh) * | 2022-08-03 | 2024-02-08 | 腾讯科技(深圳)有限公司 | 车辆定位方法、装置、计算机设备、存储介质 |
CN116222545A (zh) * | 2023-05-10 | 2023-06-06 | 北京白水科技有限公司 | 用于群组导航定位的智能地标设备 |
CN116222545B (zh) * | 2023-05-10 | 2023-07-25 | 北京白水科技有限公司 | 用于群组导航定位的智能地标设备 |
CN116380088A (zh) * | 2023-06-05 | 2023-07-04 | 小米汽车科技有限公司 | 车辆定位方法、装置、车辆及存储介质 |
CN116380088B (zh) * | 2023-06-05 | 2023-08-29 | 小米汽车科技有限公司 | 车辆定位方法、装置、车辆及存储介质 |
CN118067148A (zh) * | 2024-04-17 | 2024-05-24 | 深圳市科乐达电子科技有限公司 | 一种基于大数据分析的车载导航*** |
CN118168566A (zh) * | 2024-05-14 | 2024-06-11 | 合众新能源汽车股份有限公司 | 一种车辆定位方法、装置、电子设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
WO2024027350A1 (zh) | 2024-02-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Jeong et al. | Complex urban dataset with multi-level sensors from highly diverse urban environments | |
CN115326084A (zh) | 车辆定位方法、装置、计算机设备、存储介质 | |
US11940277B2 (en) | Vision-aided inertial navigation system for ground vehicle localization | |
CN109059906B (zh) | 车辆定位方法、装置、电子设备、存储介质 | |
Alonso et al. | Accurate global localization using visual odometry and digital maps on urban environments | |
JP4897542B2 (ja) | 自己位置標定装置、自己位置標定方法および自己位置標定プログラム | |
CN113406682B (zh) | 一种定位方法、装置、电子设备及存储介质 | |
EP3667236B1 (en) | A method of determining position data | |
CN113063425B (zh) | 车辆定位方法、装置、电子设备及存储介质 | |
CN112985391B (zh) | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 | |
CN114061611A (zh) | 目标对象定位方法、装置、存储介质和计算机程序产品 | |
CN114248778B (zh) | 移动设备的定位方法和定位装置 | |
Bai et al. | A sensor fusion framework using multiple particle filters for video-based navigation | |
de Paula Veronese et al. | Evaluating the limits of a LiDAR for an autonomous driving localization | |
Hsu et al. | Hong Kong UrbanNav: An open-source multisensory dataset for benchmarking urban navigation algorithms | |
US20220057517A1 (en) | Method for constructing point cloud map, computer device, and storage medium | |
Chiang et al. | Multifusion schemes of INS/GNSS/GCPs/V-SLAM applied using data from smartphone sensors for land vehicular navigation applications | |
Forno et al. | Techniques for improving localization applications running on low-cost IoT devices | |
Schleicher et al. | Low-cost GPS sensor improvement using stereovision fusion | |
CN116124129A (zh) | 定位信息处理方法、装置、设备和介质 | |
CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
CN114897942A (zh) | 点云地图的生成方法、设备及相关存储介质 | |
Song et al. | Enhanced Map‐Aided GPS/3D RISS Combined Positioning Strategy in Urban Canyons | |
CN114993317A (zh) | 一种基于多源融合的室内外无缝定位方法 | |
US20240255305A1 (en) | Vehicle positioning method and apparatus, computer device, and storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
REG | Reference to a national code |
Ref country code: HK Ref legal event code: DE Ref document number: 40075665 Country of ref document: HK |
|
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |