CN115493601A - 基于路网匹配的车辆自主定位方法、装置、存储介质 - Google Patents

基于路网匹配的车辆自主定位方法、装置、存储介质 Download PDF

Info

Publication number
CN115493601A
CN115493601A CN202211248836.4A CN202211248836A CN115493601A CN 115493601 A CN115493601 A CN 115493601A CN 202211248836 A CN202211248836 A CN 202211248836A CN 115493601 A CN115493601 A CN 115493601A
Authority
CN
China
Prior art keywords
road network
road
vehicle
matching
odometer
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
Application number
CN202211248836.4A
Other languages
English (en)
Inventor
李昂松
伍孟琪
王凡
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zongmu Technology Shanghai Co Ltd
Original Assignee
Zongmu Technology Shanghai Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zongmu Technology Shanghai Co Ltd filed Critical Zongmu Technology Shanghai Co Ltd
Priority to CN202211248836.4A priority Critical patent/CN115493601A/zh
Publication of CN115493601A publication Critical patent/CN115493601A/zh
Pending legal-status Critical Current

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
    • 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/20—Instruments for performing navigational calculations
    • 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/34—Route searching; Route guidance
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本申请提供一种基于路网匹配的车辆自主定位方法、装置、存储介质。基于路网匹配的车辆自主定位方法包括：加载地图路网数据并构建路网，所述路网包括若干路段，每条路段对应一条道路，所述路段包括若干节点；获取车辆在所述路网中的初始位置；基于所述初始位置以及车辆运行数据获取里程计运动轨迹；将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹；以路网匹配轨迹为基准，将里程计运动轨迹配准对齐到路网匹配轨迹上，得到车辆实时定位运动轨迹。该方法具有鲁棒性强，成本低，精度高，运算快等优点。

Description

基于路网匹配的车辆自主定位方法、装置、存储介质
技术领域
本申请属于车辆定位技术领域，特别是涉及一种基于路网匹配的车辆自主定位方法、装置、存储介质。
背景技术
车辆自主定位是无人驾驶技术的基本功能。目前主流的车辆定位技术主要使用如下传感器：视觉相机、激光雷达、全球导航卫星***(Global Navigation SatelliteSystem，GNSS)等。其中，视觉相机定位容易受到场景光照变化的影响，且算力较高，通常需要一个较好的相机标定参数才能得到精确的结果。激光雷达定位精度高，鲁棒性较强，但算力较高，且受限于昂贵的硬件价格，在有大批量产需求的家庭乘用车上仍无法普及。GNSS在使用实时动态(Real-time kinematic，RTK)定位技术时定位精度较高，但RTK需搭建外部基站，维护成本较高，且使用场景有限，只适用于室外信号强的环境。目前普遍使用的低成本、低精度的GNSS设备，存在定位结果跳变，误差较大，在室内无法工作等问题。除此之外，车辆轮速脉冲以及惯性测量单元(Inertial Measurement Unit，IMU)积分计算能够得到二维平面的车辆位姿，不受环境因素影响，但存在长时间定位累计误差较大的问题。
发明内容
鉴于以上所述现有技术的缺点，本申请的目的在于提供一种基于路网匹配的车辆自主定位方法、装置、存储介质，用于解决现有技术中低成本GNSS定位跳动，室内无法工作，以及利用车辆轮速脉冲和惯性测量单元积分计算得到的二维平面车辆位姿定位误差较大的问题。
第一方面，本申请提供一种基于路网匹配的车辆自主定位方法，所述方法包括：加载地图路网数据并构建路网，所述路网包括若干路段，每条路段对应一条道路，所述路段包括若干节点；获取车辆在所述路网中的初始位置；基于所述初始位置以及车辆运行数据获取里程计运动轨迹；将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹；以路网匹配轨迹为基准，将里程计运动轨迹配准对齐到路网匹配轨迹上，得到车辆实时定位运动轨迹。
本申请中，通过使用地图路网数据，结合里程计数据实现车辆行驶轨迹与路网匹配的功能，实现车辆在地图中的定位。相比上述传统定位技术相比，该方法具有鲁棒性强，成本低，精度高，运算快等优点。
在第一方面的一种实现方式中，所述获取车辆在所述路网中的初始位置包括：获取车辆起步时的一段GNSS数据；将所述一段GNSS数据转换至路网坐标系中得到路网坐标系下的一段车辆粗略位置定位点；将所述一段车辆粗略位置定位点与所述路网中的路段进行匹配，，获取各个车辆粗略位置定位点对应的匹配路段；基于匹配路段获取车辆初始位置。
本实现方式中，通过低成本、低精度的GNSS数据来获取车辆的初始位置，并通过与路网匹配的方式将低精度的车辆粗略位置点定位至路网路段中，提供了相对较为精确的车辆初始位置，从而，一定程度上提高后续里程计运动轨迹推算的准确性。
在第一方面的一种实现方式中，所述基于匹配路段获取车辆初始位置包括：将车辆当前时刻最临近的粗略位置定位点垂直投影至该粗略位置定位点对应的匹配路段上得到投影点，将所述投影点作为车辆在路网坐标系下的初始位置。
在第一方面的一种实现方式中，所述基于所述初始位置以及车辆运行数据获取里程计运动轨迹包括：计算车辆的里程计定位数据，包括车辆运动的水平面位置数据以及航向角数据：
xk＝xk-1+ds×cos(θk-1+0.5×da)
yk＝yk-1+ds×sin(θk-1+0.5×da)
θk＝θk-1+da
其中，xk，yk，θk分别为k时刻车辆的位置坐标和航向角，xk-1，yk-1，θk-1为k-1时刻车辆的位置坐标和航向角，ds为车辆在两个时刻之间的位移变化值，da为车辆在两个时刻之间的航向变化值；基于车辆的里程计定位数据得到里程计运动轨迹。
在第一方面的一种实现方式中，将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹包括：截取当前时刻临近的一段连续时间内的局部里程计轨迹作为滑动窗口输入数据；将所述局部里程计轨迹均匀离散化，生成均匀间隔的离散里程计定位数据；将离散里程计定位数据中的各个定位点与所述路网中的路段进行匹配，连续的最优匹配路段构成最优匹配路径；对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹。
本实现方式中，截取当前时刻临近的一段连续时间内的局部里程计轨迹作为滑动窗口输入数据，通过离散化将连续且频率较高的车辆里程计轨迹均匀离散化，生成均匀间隔的离散里程计定位数据，进而可以将离散的里程计定位数据与路网中的路段进行匹配，从而避免了通过推算获取的车辆里程计数据在长时间运行时存在较大的累计误差而无法直接用于车辆在长距离行驶时的定位。
在第一方面的一种实现方式中，所述将车辆一段粗略位置定位点与所述路网中的路段进行匹配或将离散里程计定位数据中的各个定位点与所述路网中的路段进行匹配包括：获取待匹配的定位点；选取路网一定范围内的路段，组成第一候选路段集，所述第一候选路段集中至少包括一条第一候选路段；针对任意一条所述第一候选路段，分别计算所述待匹配的定位点与所述第一候选路段的距离，筛选处于距离阈值范围内的路段作为所述待匹配的定位点对应的第二候选路段集，所述第二候选路段集中至少包括一条第二候选路段；针对所述第二候选路段集，采用概率论模型获取所待匹配的定位点对应的最优路段。
在第一方面的一种实现方式中，所述对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹包括：对最优匹配路径中弯道处的节点进行修正，修正后的节点位于弯道位置处两条路段形成的夹角范围内；对修改后的节点以及最优匹配路径中其余节点进行拟合形成拟合曲线，将所述拟合曲线作为路网匹配轨迹。
在第一方面的一种实现方式中，所述将里程计运动轨迹配准对齐到路网匹配轨迹上包括：采用ICP算法进行配准。
第二方面，本申请提供一种电子设备，所述电子设备包括：存储器，被配置为存储计算机程序；以及处理器，与所述存储器通信相连，并且被配置为调用所述计算机程序以执行根据本申请第一方面所述的基于路网匹配的车辆自主定位方法。
第三方面，本申请提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现根据本申请第一方面所述的基于路网匹配的车辆自主定位方法。
如上所述，本申请所述的基于路网匹配的车辆自主定位方法、装置、介质，具有以下有益效果：
本申请通过使用地图路网数据，结合里程计数据实现车辆行驶轨迹与路网匹配的功能，实现车辆在地图中的定位。相比上述传统定位技术相比，该方法具有鲁棒性强，成本低，精度高，运算快等优点。
附图说明
图1显示为本申请实施例所述的无人驾驶车辆的示意图。
图2显示为本申请实施例所述的基于路网匹配的车辆自主定位方法的总体流程示意图。
图3显示为本申请实施例所述的基于路网匹配的车辆自主定位方法的一具体流程示意图。
图4显示为本申请实施例中进行路网匹配的示意图。
图5显示为本申请实施例中局部里程计轨迹均匀离散化的过程示意图。
图6显示为本申请实施例中对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹的过程示意图。
图7显示为本申请实施例中将里程计运动轨迹配准对齐到路网匹配轨迹上的过程示意图。
图8显示为本申请实施例所述的基于路网匹配的车辆自主定位方法的另一具体流程示意图。
图9显示为本申请实施例所述的电子设备的结构框图。
元件标号说明
1 车辆
2 GNSS卫星
11 GNSS设备
12 车载传感器
9 电子设备
91 存储器
92 处理器
S1～S5 步骤
S21～S24 步骤
S41～S43 步骤
S51～S52 步骤
具体实施方式
以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
需要说明的是，以下实施例中所提供的图示仅以示意方式说明本申请的基本构想，遂图式中仅显示与本申请中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
本申请以下实施例提供了一种基于路网匹配的车辆自主定位方法、装置、介质，包括但不限于用于无人驾驶车辆，以下将以无人驾驶车辆为例进行描述。
如图1所示，本实施例提供一种无人驾驶车辆，包括车辆1，车辆1中配置GNSS设备11和车载传感器12，GNSS车载定位终端11用于接收GNSS卫星2信号从而对车辆1进行定位，其中，GNSS设备11采用低成本、低精度的GNSS设备11，这类GNSS设备11存在定位结果跳变，误差较大，且室内无法工作等问题。车载传感器12包括轮速脉冲里程计以及惯性测量单元(Inertial Measurement Unit，IMU)，通过车辆轮速脉里程计以及惯性测量单元可以推算出二维平面的车辆位姿，不受环境因素影响，但存在长时间定位累计误差较大的问题。基于此，本申请提供了一种基于路网匹配的车辆自主定位方法，该方法使用地图路网数据，结合低成本、低精度的GNSS数据以及里程计数据，实现车辆行驶轨迹与路网匹配的功能，实现车辆在地图中的定位。相比上述传统定位技术相比，该方法具有鲁棒性强，成本低，精度高，运算快等优点。
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行详细描述。
如图2所示，本实施例提供一种基于路网匹配的车辆自主定位方法，所述方法包括步骤S1至步骤S5。
本申请设定如下定义：
节点：表示路网中道路的起始点，结束点，路口点，或者道路的拐点
路段：表示路网中两个节点之间的道路，包含有编号，长度，开始点，结束点等信息路网：所有节点集合和路段集合组成路网
路径：给定路网中的两个顶点，一条路径是始于某一节点止于另一节点的一组连接线段。
步骤S1：加载地图路网数据并构建路网，所述路网包括若干路段，每条路段对应一条道路，所述路段包括若干节点。
步骤S2：获取车辆在所述路网中的初始位置。
步骤S3：基于所述初始位置以及车辆运行数据获取里程计运动轨迹。
步骤S4：将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹。
步骤S5：以路网匹配轨迹为基准，将里程计运动轨迹进行配准对齐到路网匹配轨迹上，得到车辆实时定位运动轨迹。
本申请步骤S2中获取车辆在路网中的初始位置，可以根据车辆当前环境采用不同的方式获取车辆的初始位置：当车辆在室外等GNSS信号优良的环境下起步时，可通过GNSS设备获取GNSS数据并与路网匹配获取车辆在路网中的初始位置；当车辆在室内或其他GNSS信号微弱环境下起步时，采用诸如历史位置、视觉相机全局定位等方法实现初始定位。本申请将上次熄火前记录的一段车辆路网坐标系下的位置轨迹保存下来，在车辆处于室内环境上电起步时，直接读取历史位置数据，初始化路网匹配定位***，后续路网匹配流程可依照前述里程计路网匹配定位方法进行。
在本申请的一实施例中，车辆在室外环境下起步，通过GNSS设备获取车辆的初始位置。如图3所示为本实施例提供的一种基于路网匹配的车辆自主定位方法的具体流程图。
步骤S1包括：步骤S11，加载地图路网数据；以及，步骤S12，路网组网。
步骤S2，车辆在室外场景起步时，车端实时接收全球导航卫星***(GNSS)数据，然后将GNSS数据进行坐标变换并匹配至路网，具体包括步骤S21至步骤S24。
在步骤S21中，获取GNSS经纬度数据。
在步骤S22中，将GNSS经纬度数据转换投影至路网地图坐标系，得到路网坐标系下的车辆粗略位置定位点。
本实施例通过GNSS经纬度墨卡托(Mercator)投影变换，将经纬度数据转换至笛卡尔坐标系，获得米制坐标。再通过预先计算的路网地图转换矩阵，将米制单位坐标系下的平面坐标转换至路网坐标系，得到通过GNSS定位的路网坐标系下车辆粗略位置定位点。以下所述GNSS坐标点均为已经过转换投影至路网地图坐标系下的坐标点。投影关系如式(1)所示，其中Pg为经纬度墨卡托投影转换至笛卡尔坐标系下的平面坐标点，Tmg为预计算的转换矩阵，Pm为笛卡尔坐标系下的坐标点经转换得到的路网坐标系下的坐标点。
Pm＝TmgPg (1)
在步骤S23中，进行路网匹配：将所述车辆一段粗略位置定位点与所述路网中的路段进行匹配，获取车辆在路网坐标系下的初始位置。此步骤中在路网上选取范围候选点，使用概率论数学模型，求解得出车辆在路网上的最优路径，实现误差较大的一段粗略GNSS数据与路网匹配定位功能，输出路网坐标系下的精确位置。
本申请路网匹配定位方法中，假设i时刻投影至路网地图坐标系的粗略GNSS坐标点为pi，选取路网一定范围内的路段，组成候选路段。本申请计算GNSS粗略坐标点各到路段的投影点之间的直线距离，筛选处于距离阈值范围内的路段作为该点对应的候选路段
Figure BDA0003887078860000061
进一步的，在上述候选路段中，使用概率论数学模型，求解得出当前车辆GNSS位置在路网上的最优路段
Figure BDA0003887078860000062
将一段连续时间内的多个误差较大的粗略GNSS坐标离散数据与路网路段进行求解匹配，每个GNSS坐标点对应的最优路段组合成最优路径
Figure BDA0003887078860000063
实现GNSS粗略坐标点和路网的匹配。
本申请在使用概率论模型求解时，包括观测概率和转移概率。
具体的，观测概率是指给定状态导致测量的可能性。
如图4所示：
l1,l2,l3,l4分别表示局部路网中的路段；
pi,pi+1分别表示车辆在i时刻和i+1时刻的GNSS笛卡尔坐标经转换矩阵转换至路网坐标系下的坐标点；
xi,1,xi+1,2分别表示上述转换后的GNSS坐标点在候选路段上的最近点。
给定转换后的GNSS坐标点pi和其候选路段li，将GNSS坐标点在i时刻的观测概率记为P(pi|Pn)，即GNSS坐标点在路网中的路段位置为ln时，观测到pi的概率，建立以ln为中心的高斯观测概率模型，如式(2)所示。
Figure BDA0003887078860000071
其中，σz是GNSS测量数据的标准差，由GNSS设备硬件提供，值越大，数据的噪声越大，表示GNSS位置越不可信。xi,n表示i时刻路段ln上离观测点pi最近的候选点，||pi-xi,n||2表示xi,n到pi的距离平方和。
转移概率是指GNSS定位点在道路网络上从i时刻的一条候选路段ln移动到i+1时刻的一条候选路段ln+1的概率，相邻GNSS坐标点从ln移动到ln+1的转移概率为：
Figure BDA0003887078860000072
其中，xi,n表示i时刻路段ln上离观测点pi最近的候选点，xi+1,n+1表示i+1时刻路段ln+1上离观测点pi+1最近的候选点，||pi-pi+1||为pi、pi+1两点之间的距离，||xi+1,n+1-xi,n||为xi+1,n+1、xi,n两点之间的距离。
根据观测概率和转移概率，可为相邻不同时刻GNSS坐标点在不同候选路段分配一个分数：
score＝P(pi|ln)×P(ln,ln+1) (4)
选取得分最高的路段，作为该段GNSS相邻位置点在路网上的最优匹配路段。连续的最优匹配路段构成最优匹配路径。
在此基础上，将所述车辆当前时刻最临近的粗略位置定位点垂直投影至其匹配路段的点位置作为车辆在路网坐标系下的初始位置。
通过上述方法，可将粗略的GNSS定位结果匹配至路网，得到更高精度的车辆初始定位结果。
在步骤S24中，输出车辆的初始定位位姿，包括通过步骤S23获取到的车辆在路网坐标系下的平面初始位置坐标，车辆的初始姿态航向角设为0。
以上过程为GNSS数据与路网匹配实现初始定位的功能。进一步的，车辆在路网中继续行驶，为了避免持续使用误差较大的GNSS的数据，同时避免诸如室内等环境因素对GNSS数据的影响，本申请在后续定位阶段执行步骤S3：使用车辆轮速脉冲以及惯性测量单元(IMU)积分得到的里程计相对定位数据，实现与路网的匹配定位。
车辆在道路中行驶时，依靠轮速脉冲递推计算得到里程计运动轨迹，车辆的里程计定位数据包括车辆运动的水平面位置数据(x，y)以及航向角数据θ。其中，车辆的水平面位置数据(x，y)以及航向角数据θ的推算公式如下：
Figure BDA0003887078860000081
其中，xk，yk，θK分别为当前时刻车辆的位置坐标和航向角，xk-1，yk-1，θK-1为上一时刻车辆的位置坐标和航向角，ds为轮速脉冲提供的车辆两个时刻的位移变化值，da为IMU提供的车辆两个时刻之间的航向变化值。在本实施例中以GNSS数据进行初始定位时，初始时刻x0,y0为上述GNSS匹配的路网坐标系下的初始位置，初始航向角θ0为0。进而基于车辆的里程计定位数据得到里程计运动轨迹。
上述递推计算获取的车辆里程计数据在长时间运行时，会存在较大的累计误差，无法直接用于车辆在长距离行驶时的定位。因此，本申请执行步骤S4：将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹。
步骤S4具体包括：
步骤S41：截取当前时刻临近的一段连续时间内的局部里程计轨迹作为滑动窗口输入数据，将所述局部里程计轨迹均匀离散化，生成均匀间隔的离散里程计定位数据。如图5所示，黑色点为局部路网节点，节点间的连线为局部路网路段，弯曲实线为滑动窗口内的局部里程计轨迹。将连续且频率较高的车辆里程计轨迹均匀离散化，生成均匀间隔的离散里程计数据，即图5中的虚线所示。
步骤S42：将离散里程计定位数据中的各个定位点与所述路网中的路段进行匹配，连续的最优匹配路段构成最优匹配路径。此处，依照上述GNSS与路网匹配的原理，将离散里程计数据同理成GNSS数据，计算获取匹配分数最高的路网路径，即最优匹配路径。具体地：获取待匹配的定位点；选取路网一定范围内的路段，组成第一候选路段集，所述第一候选路段集中至少包括一条第一候选路段；针对任意一条所述第一候选路段，分别计算所述待匹配的定位点与所述第一候选路段的距离，筛选处于距离阈值范围内的路段作为所述待匹配的定位点对应的第二候选路段集，所述第二候选路段集中至少包括一条第二候选路段；针对所述第二候选路段集，采用概率论模型获取所待匹配的定位点对应的最优路段；连续的最优匹配路段构成最优匹配路径。其具体实施过程与上述GNSS与路网匹配的过程一致，此处不再赘述。
步骤S43：对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹。最优匹配的路网路段进行处理，具体为将路段内的节点使用B样条拟合算法，将匹配的路网节点拟合成曲线，曲线包含的点的密度相比离散里程计数据更多，以确保下文的迭代配准步骤正常运行。
在拟合曲线时，由于路网节点与路段之间连线为类似矩形的多边形，直接拟合节点后的结果和实际行车轨迹差别较大。因此，对最优匹配路径中弯道处的节点进行修正，修正后的节点位于弯道位置处两条路段形成的夹角范围内；对修改后的节点以及最优匹配路径中其余节点进行拟合形成拟合曲线，将所述拟合曲线作为路网匹配轨迹。本实施例中，在拟合路网节点时，将弯道处的两个节点向内移动一定距离，使拟合后的轨迹近似为车辆的行驶轨迹。如图6所示，P1P2为移动前的路网节点，P′1P′2为移动后的路网节点，拟合轨迹后生成的点数比离散里程计数量更多。
最后，执行步骤S5：以路网匹配轨迹为基准，将里程计运动轨迹配准对齐到路网匹配轨迹上，得到车辆实时定位运动轨迹。
为了修正里程计的累计误差，如图7所示，执行步骤S51：将拟合得到的路网匹配轨迹和车辆里程计生成的里程计运动轨迹进行最近点迭代算法(Iterative Closest Point,ICP)配准。具体如式(6)所示：
Tnew＝ΔT·Todometry (6)
其中，Todometry为里程计递推配准前的车辆定位轨迹位姿，ΔT为配准计算后需要变换的位姿量，Tnew为配准后的新的车辆位姿。
通过配准算法，可将长时间运行导致累计误差较大的里程计运动轨迹配准对齐到路网拟合的路网匹配轨迹上，实现里程计轨迹与路网拟合的近似行驶轨迹路线配准。匹配得到的数据即是修正后的车辆在路网坐标系下的位置和姿态，从而执行步骤S52：输出车辆位姿(包括位置和姿态)。由于车辆在道路中行驶，道路与路网是一一对应的，该方法可以得到相比单纯里程计定位而言，精度更高的定位结果。
上述提供的一种基于路网匹配的车辆自主定位方法首先加载定位场景的地图路网数据并构建路网，车辆在室外场景起步时，车端接收全球导航卫星***(GNSS)数据，将一段连续的GNSS经纬度数据，转换成笛卡尔平面坐标数据，再通过该场景的路网地图对应的坐标转换矩阵，将前述笛卡尔坐标系下的平面位置坐标转换至路网平面坐标系。在路网上选取范围候选点，使用概率论数学模型，求解得出车辆在路网上的最优路径，实现误差较大的粗略GNSS数据与路网匹配定位功能，输出路网坐标系下的精确位置。
以上过程为GNSS数据与路网匹配实现初始定位的功能。进一步的，车辆在路网中继续行驶，为了避免持续使用误差较大的GNSS数据，同时避免诸如室内等环境因素对GNSS数据的影响，本申请在后续定位阶段使用车辆轮速脉冲以及惯性测量单元(IMU)积分得到的里程计数据，实现里程计与路网的匹配定位。车辆在道路中行驶时，依靠轮速脉冲和惯性测量单元递推计算得到里程计运动轨迹。获取的车辆里程计轨迹数据在长时间运行时，会存在较大的累计误差，无法直接用于车辆在长距离行驶时的定位。本申请选取连续一段时间内的里程计轨迹作为滑动窗口数据输入。将连续且频率较高的里程计轨迹均匀离散化，生成均匀间隔的离散里程计数据，依照上述GNSS与路网匹配的原理，计算获取在路网上的最佳匹配路段。之后，将路段内的节点使用样条拟合算法，将路网节点拟合成连续曲线。
进一步的，上述拟合路网节点的目的是将路网构建的轨迹近似为车辆的行驶轨迹。为了修正里程计的累计误差，将拟合得到的轨迹和车辆里程计生成的离散轨迹进行位置配准，将长时间运行导致累计误差较大的里程计运动轨迹配准对齐到路网拟合的路网匹配轨迹上，实现里程计轨迹与路网拟合的近似行驶轨迹路线配准。
除此之外，车辆直接在室内上电起步时，由于GNSS设备无法工作，相比前述方法，缺少从室外到室内的GNSS初始定位流程，此种情况下，上述里程计与路网匹配的方法因缺少初始位置，会导致后续算法无法进行。可采用诸如读取历史位置、视觉相机全局定位等方法实现在室内上电起步时，路网匹配算法的初始化。车辆初始定位后，后续路网匹配定位流程可依照上述方法进行。
如图8所示为本申请一种基于路网匹配的车辆自主定位方法在另一实施例中的具体流程示意图。包括上述步骤S1至步骤S5，其中步骤S1、步骤S3、步骤S4和步骤S5的具体执行均与上文相同，此处不再赘述。
不同的是，在本实施例步骤S2中通过历史位置获取车辆在所述路网中的初始位置。本实施例中将上次车辆熄火前记录的一段车辆路网坐标系下的位置轨迹数据保存到硬盘，在车辆处于室内环境或GNSS信号丢失的环境上电起步时，直接从硬盘读取历史位置数据，初始化路网匹配定位***，后续路网匹配流程可依照前述里程计路网匹配定位方法进行。若无历史位置数据，则可以采用诸如视觉相机全局初始定位等方法。
本申请实施例所述的一种基于路网匹配的车辆自主定位方法的保护范围不限于本实施例列举的步骤执行顺序，凡是根据本申请的原理所做的现有技术的步骤增减、步骤替换所实现的方案都包括在本申请的保护范围内。
本申请还提供一种电子设备。如图9所示，本实施例提供一种电子设备9，所述电子设备9包括：存储器91，被配置为存储计算机程序；以及处理器92，与所述存储器91通信相连，并且被配置为调用所述计算机程序以执行所述的基于路网匹配的无人驾驶车辆自主定位方法。
在本申请所提供的几个实施例中，应该理解到，所揭露的方法或电子设备，可以通过其它的方式实现。本领域普通技术人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
本申请实施例还提供了一种计算机可读存储介质。本领域普通技术人员可以理解实现上述实施例的方法中的全部或部分步骤是可以通过程序来指令处理器完成，所述的程序可以存储于计算机可读存储介质中，所述存储介质是非短暂性(non-transitory)介质，例如随机存取存储器，只读存储器，快闪存储器，硬盘，固态硬盘，磁带(magnetic tape)，软盘(floppy disk)，光盘(optical disc)及其任意组合。上述存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。该可用介质可以是磁性介质(例如，软盘、硬盘、磁带)、光介质(例如数字视频光盘(digital video disc，DVD))、或者半导体介质(例如固态硬盘(solid state disk，SSD))等。
本申请实施例还可以提供一种计算机程序产品，所述计算机程序产品包括一个或多个计算机指令。在计算设备上加载和执行所述计算机指令时，全部或部分地产生按照本申请实施例所述的流程或功能。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机或数据中心进行传输。
所述计算机程序产品被计算机执行时，所述计算机执行前述方法实施例所述的方法。该计算机程序产品可以为一个软件安装包，在需要使用前述方法的情况下，可以下载该计算机程序产品并在计算机上执行该计算机程序产品。
上述各个附图对应的流程或结构的描述各有侧重，某个流程或结构中没有详述的部分，可以参见其他流程或结构的相关描述。
上述实施例仅例示性说明本申请的原理及其功效，而非用于限制本申请。任何熟悉此技术的人士皆可在不违背本申请的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本申请所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本申请的权利要求所涵盖。

Claims (10)

1.一种基于路网匹配的车辆自主定位方法，其特征在于，所述方法包括：
加载地图路网数据并构建路网，所述路网包括若干路段，每条路段对应一条道路，所述路段包括若干节点；
获取车辆在所述路网中的初始位置；
基于所述初始位置以及车辆运行数据获取里程计运动轨迹；
将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹；
以路网匹配轨迹为基准，将里程计运动轨迹配准对齐到路网匹配轨迹上，得到车辆实时定位运动轨迹。
2.根据权利要求1所述的一种基于路网匹配的车辆自主定位方法，其特征在于，所述获取车辆在所述路网中的初始位置包括：
获取车辆起步时的一段离散GNSS数据；
将所述一段离散GNSS数据转换至路网坐标系中得到路网坐标系下的一段车辆粗略位置定位点；
将所述一段车辆粗略位置定位点与所述路网中的路段进行匹配，获取各个车辆粗略位置定位点对应的匹配路段；
基于匹配路段获取车辆初始位置。
3.根据权利要求2的基于路网匹配的车辆自主定位方法，其特征在于，所述基于匹配路段获取车辆初始位置包括：将车辆当前时刻最临近的粗略位置定位点垂直投影至该粗略位置定位点对应的匹配路段上得到投影点，将所述投影点作为车辆在路网坐标系下的初始位置。
4.根据权利要求1所述的一种基于路网匹配的车辆自主定位方法，其特征在于，所述基于所述初始位置以及车辆运行数据获取里程计运动轨迹包括：
计算车辆的里程计定位数据，包括车辆运动的水平面位置数据以及航向角数据：
xk＝xk-1+ds×cos(θk-1+0.5×da)
yk＝yk-1+ds×sin(θk-1+0.5×da)
θk＝θk-1+da
其中，xk，yk，θk分别为k时刻车辆的位置坐标和航向角，xk-1，yk-1，θk-1为k-1时刻车辆的位置坐标和航向角，ds为车辆在两个时刻之间的位移变化值，da为车辆在两个时刻之间的航向变化值；
基于车辆的里程计定位数据得到里程计运动轨迹。
5.根据权利要求1所述的一种基于路网匹配的车辆自主定位方法，其特征在于，将所述里程计运动轨迹与所述路网进行匹配获取路网匹配轨迹包括：
截取当前时刻临近的一段连续时间内的局部里程计轨迹作为滑动窗口输入数据；
将所述局部里程计轨迹均匀离散化，生成均匀间隔的离散里程计定位数据；
将离散里程计定位数据中的各个定位点与所述路网中的路段进行匹配，连续的匹配路段构成最优匹配路径；
对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹。
6.根据权利要求2或5所述的一种基于路网匹配的车辆自主定位方法，其特征在于，所述将一段车辆粗略位置定位点与所述路网中的路段进行匹配或将离散里程计定位数据中的各个定位点与所述路网中的路段进行匹配包括：
获取待匹配的定位点；
选取路网一定范围内的路段，组成第一候选路段集，所述第一候选路段集中至少包括一条第一候选路段；
针对任意一条所述第一候选路段，分别计算所述待匹配的定位点与所述第一候选路段的距离，筛选处于距离阈值范围内的路段作为所述待匹配的定位点对应的第二候选路段集，所述第二候选路段集中至少包括一条第二候选路段；
针对所述第二候选路段集，采用概率论模型获取所待匹配的定位点对应的匹配路段。
7.根据权利要求5所述的一种基于路网匹配的车辆自主定位方法，其特征在于，所述对最优匹配路径中对应路段的节点进行拟合获取路网匹配轨迹包括：
对最优匹配路径中弯道处的节点进行修正，修正后的节点位于弯道位置处两条路段形成的夹角范围内，所述夹角小于180度；
对修改后的节点以及最优匹配路径中其余节点进行拟合形成拟合曲线，将所述拟合曲线作为路网匹配轨迹。
8.根据权利要求1所述的基于路网匹配的车辆自主定位方法，其特征在于，所述将里程计运动轨迹配准对齐到路网匹配轨迹上包括：采用ICP算法进行配准。
9.一种装置，其特征在于，所述装置包括：
存储器，被配置为存储计算机程序；以及
处理器，与所述存储器通信相连，并且被配置为调用所述计算机程序以执行根据权利要求1至8中任一项所述的基于路网匹配的车辆自主定位方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现根据权利要求1至8中任一项所述的基于路网匹配的车辆自主定位方法。
CN202211248836.4A 2022-10-12 2022-10-12 基于路网匹配的车辆自主定位方法、装置、存储介质 Pending CN115493601A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211248836.4A CN115493601A (zh) 2022-10-12 2022-10-12 基于路网匹配的车辆自主定位方法、装置、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211248836.4A CN115493601A (zh) 2022-10-12 2022-10-12 基于路网匹配的车辆自主定位方法、装置、存储介质

Publications (1)

Publication Number Publication Date
CN115493601A true CN115493601A (zh) 2022-12-20

Family

ID=84474309

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211248836.4A Pending CN115493601A (zh) 2022-10-12 2022-10-12 基于路网匹配的车辆自主定位方法、装置、存储介质

Country Status (1)

Country Link
CN (1) CN115493601A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115824234A (zh) * 2023-02-23 2023-03-21 智道网联科技(北京)有限公司 地图匹配方法、装置及电子设备
CN116383675A (zh) * 2023-05-26 2023-07-04 交通运输部规划研究院 车辆位置数据的路网匹配方法及装置
CN116539036A (zh) * 2023-03-13 2023-08-04 苏州科技大学 车辆位置确定方法、车辆轨迹补偿方法、电子设备

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115824234A (zh) * 2023-02-23 2023-03-21 智道网联科技(北京)有限公司 地图匹配方法、装置及电子设备
CN115824234B (zh) * 2023-02-23 2023-06-02 智道网联科技(北京)有限公司 地图匹配方法、装置及电子设备
CN116539036A (zh) * 2023-03-13 2023-08-04 苏州科技大学 车辆位置确定方法、车辆轨迹补偿方法、电子设备
CN116539036B (zh) * 2023-03-13 2024-02-02 苏州科技大学 车辆位置确定方法、车辆轨迹补偿方法、电子设备
CN116383675A (zh) * 2023-05-26 2023-07-04 交通运输部规划研究院 车辆位置数据的路网匹配方法及装置
CN116383675B (zh) * 2023-05-26 2023-08-11 交通运输部规划研究院 车辆位置数据的路网匹配方法及装置

Similar Documents

Publication Publication Date Title
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN115493601A (zh) 基于路网匹配的车辆自主定位方法、装置、存储介质
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN110703268B (zh) 一种自主定位导航的航线规划方法和装置
CN112037260B (zh) 一种跟踪目标的位置估计方法、装置及无人飞行器
CN102506868B (zh) 基于联邦滤波的sins/smans/trns组合导航方法及***
CN109059907B (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN110887493B (zh) 基于局部地图匹配的轨迹推算方法、介质、终端和装置
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
KR20200043006A (ko) 위치 추정 장치 및 방법
CN110388939A (zh) 一种基于航拍图像匹配的车载惯导定位误差修正方法
CN111552757B (zh) 生成电子地图的方法、装置、设备及存储介质
CN115236714A (zh) 多源数据融合定位方法、装置、设备及计算机存储介质
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
CN117824667A (zh) 一种基于二维码和激光的融合定位方法及介质
CN114252897A (zh) 定位方法、装置、电子设备及计算机存储介质
CN110411449B (zh) 一种航空侦察载荷目标定位方法、***及终端设备
CN114358038B (zh) 一种基于车辆高精度定位的二维码坐标标定方法及装置
KR101502071B1 (ko) 랜드마크 기반 비전항법시스템을 위한 카메라 데이터 생성기와 그것을 실행시키기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 매체
CN115060268A (zh) 一种机房融合定位方法、***、设备及存储介质
CN116295508A (zh) 一种基于高精地图的路侧传感器标定方法、设备和***
CN113034538B (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