CN116659491A - 一种借助道路边界线约束车辆初始化定位方法及其系统 - Google Patents

一种借助道路边界线约束车辆初始化定位方法及其系统 Download PDF

Info

Publication number
CN116659491A
CN116659491A CN202310660170.1A CN202310660170A CN116659491A CN 116659491 A CN116659491 A CN 116659491A CN 202310660170 A CN202310660170 A CN 202310660170A CN 116659491 A CN116659491 A CN 116659491A
Authority
CN
China
Prior art keywords
vehicle
data
initialization
pose
positioning
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
CN202310660170.1A
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.)
Chongqing Changan Automobile Co Ltd
Original Assignee
Chongqing Changan Automobile 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 Chongqing Changan Automobile Co Ltd filed Critical Chongqing Changan Automobile Co Ltd
Priority to CN202310660170.1A priority Critical patent/CN116659491A/zh
Publication of CN116659491A publication Critical patent/CN116659491A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明公开了一种借助道路边界线约束车辆初始化定位方法及其系统，方法包括：S1：获取前视图像数据、INS数据、动态地图数据；S2：根据三次曲线方程进行拟合，得到车道线点云、道路边界线点云；S3：查找INS数据，得到车辆所有可能的初始化位姿；S4：根据INS数据做航迹推算得到车辆前视帧初始化位姿，将车道线点云投影到动态地图数据中，进行ICP匹配，得到匹配结果；S5：筛选符合匹配条件的初始化位姿并进行边界约束验证，若符合匹配条件且通过边界约束验证，执行S6；若都不符合，重复S1～S5；S6：将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位；本发明避免初始化定位错车道。

Description

一种借助道路边界线约束车辆初始化定位方法及其系统
技术领域
本发明涉及自动驾驶定位技术领域，特别涉及一种借助道路边界线约束车辆初始化定位方法及其系统。
背景技术
在自动驾驶定位领域中，大多数算法通过搭载GPS、组合惯导以及相机或者激光雷达传感器实现车辆定位。在车辆定位时，需要对车辆位置进行初始化，获得当前车辆的定位结果，基于该定位结果进行车辆在后续行驶过程的定位。因此，车辆初始化位置的确定对后续车辆定位的准确度至关重要。
在自动驾驶技术中，车辆初始化位置的确定意味着需要提高车辆初始化定位的精度；目前的初始化定位方法包括基于相机的初始化定位方法和基于激光雷达的初始化定位方法两种。
在自动驾驶领域，GNSS、IMU定位和地图匹配定位技术应用十分广泛。然而，车辆在行驶过程中，GNSS、IMU设备极其容易受到影响，在较为空旷或者有遮挡的地方定位不准，甚至会错车道，导致车辆定位初始化准确度降低，影响车辆决策规划，威胁驾驶安全。地图匹配技术是在已知汽车位姿信息的条件下进行高精度地图局部搜索的过程，需要首先利用车载GNSS、IMU做出初始位置的判断，确定高精度地图的局部搜索范围，然后将相机传感器或激光雷达采集到的数据与高精度地图数据变换到同一坐标系内进行匹配。
当GNSS、IMU数据存在较大误差时，地图匹配得到的位姿会有较大误差，影响后续进一步定位。基于地图的数据融合定位技术融合了以上两种算法，将地图匹配得到的定位结果与GNSS/IMU数据进行数据融合，进一步提高定位的精度和可靠性。
在地图匹配定位时，需要对车辆位姿进行初始化，获得当前车辆的定位结果，基于该定位结果进行车辆后续行驶过程中的定位。因此，车辆初始化位姿的确定对后续地图匹配定位及融合定位的准确度至关重要。
现有的车辆定位初始化方法，在遇到周围环境纹理不强、可参考的障碍物或其他特征较少时，很容易导致在定位过程中跟随错车道的GPS或者组合惯导的定位偏移出车道，从而影响后面的定位精度，影响后续驾驶的安全性。
发明内容
为了解决现有技术中存在当初始INS数据不准时车辆初始化定位错车道的问题，提出了一种借助道路边界线约束车辆初始化定位方法及其系统，对车辆初始化位置进行约束，避免初始化定位错车道。
本发明的技术方案如下：
本发明一方面公开了一种借助道路边界线约束车辆初始化定位方法，包括以下步骤：
S1：获取车辆前视图像数据、INS数据、动态地图数据；
S2：根据三次曲线方程对车辆前视图像数据进行拟合，得到车道线点云、道路边界线点云；
S3：查找INS数据中不小于当前车辆前视图像数据时间戳的一帧INS数据及其上一帧INS数据，经过判断得到车辆所有可能的初始化位姿；
S4：对于所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
S5：从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，则执行步骤S6；若不符合匹配条件或不通过边界约束验证，则重复步骤S1～S5；
S6：将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
优选地，S1中，所述的车辆前视图像数据为经过语义分割后的车道线、道路边界线信息。
所述的INS数据和车辆前视图像数据基于同一时钟源进行授时，INS数据与车辆前视图像数据时间同步。
优选地，S2中的三次曲线方程表达式如下：
y＝a0+a1x+a2x2+a3x3 (1)
式(1)中，a0、a1、a2、a3分别为三次曲线方程0次项、1次项、2次项和3次项的系数，x为车辆的纵向坐标；a0表示道路边界线与车辆的横向距离，a0为正，表示左侧道路边界线与车辆的横向距离，a0为负，表示右侧道路边界线与车辆的横向距离。
更进一步地，得到车道线点云的方法如下：
三次曲线方程拟合出单帧车道线、道路边界线，然后从拟合出的单帧车道线中取出其中的5～10米的一段，每间隔预设的一段距离取一个点，离散成点云。
优选地，S3中判断得到车辆初始化位姿的所有可能位姿的方法如下：
若上一帧INS数据的位姿为固定解，该位姿为上一帧车辆可能的位姿。
若上一帧INS数据的位姿为浮点解，则将该位姿投影到地图的中心线上，投影得到的多个位姿为上一帧车辆可能的位姿。
优选地，S4中的航迹推算过程如下：
pn＝pn-1+Rn-1·vn-1·Δt (2)
式中，pn-1表示上一时刻的车辆位置，Rn-1表示车辆上一时刻的旋转矩阵，vn-1表示车辆在上一时刻的速度，表示车辆在上一时刻的角加速度，pn表示当前时刻的车辆位置，Rn表示当前时刻车辆的旋转矩阵，Δt为当前时刻与上一时刻的时间差。
通过式(2)和式(3)将INS数据从上一帧开始进行航迹推算，利用上一时刻车辆的轮速和角加速度计算出两帧间的车辆平移变化量和旋转变化量，推算得到当前时刻的车辆前视帧初始化位姿：
优选地，S5中的匹配条件如下：
匹配点数占前视感知点云总数的第一阈值以上，误差的中位数小于第二阈值，且匹配点数大于第三阈值。
更进一步地，S5中的边界约束验证如下：
查找左侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d1，计算三次曲线与车体的距离|a0|，d1与|a0|的差与第四阈值比较。
查找右侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d2，计算三次曲线与车体的距离d2与/>的差与第四阈值比较；/>为右侧道路边界线与车辆的横向距离。
若d1与|a0|的差、d2与的差均小于第四阈值，则认为此时的初始化位姿为最优位姿。
本发明另一方面公开了一种借助道路边界线约束车辆初始化定位系统，采用如上所述的借助道路边界线约束车辆初始化定位方法，包括：车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块、初始化模块；
所述的车载前视相机用于获取车辆前视图像数据；
所述的车载组合惯导模块用于获取INS数据、动态地图数据；
所述的数据拟合模块用于根据三次曲线方程进行拟合，得到车道线点云、道路边界线点云；
所述的数据查找模块根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿；
所述的数据处理模块用于对所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
所述的数据筛选模块从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，进入初始化模块；若不符合匹配条件或不通过边界约束验证，则重复车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块的工作过程；
所述的初始化模块用于将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
更具体的，借助道路边界线约束车辆初始化定位系统还包括数据容器；所述的数据容器用于对获取的INS数据，按照时间戳的顺序进行保存。
本发明的有益效果：
1.通过获取车辆前视图像数据、INS数据、动态地图数据得到当前车辆的定位，为车辆初始化提供准确数据。
2.通过三次曲线方程对车辆前视图像数据进行拟合，得到车道线与道路边界线点云，提高获取的数据的准确性。
3.根据查找INS数据，得到车辆所有可能的初始化位姿；再对所有初始化位姿进行筛选，通过判断初始化位姿是否为最优初始化位姿对车辆初始化位置进行约束，避免初始化定位错车道导致影响后续的定位与驾驶。
4.当无法得到最优初始化位姿时，重复寻找最优初始化位姿的过程，避免得到错误的车辆初始化位置，提高了车辆初始化的定位精度和准确度。
附图说明
图1为实施例提供的借助道路边界线约束车辆初始化定位方法的流程图。
图2为实施例提供的车体坐标系的示意图。
图3为实施例提供的三次曲线道路边界线与车体之间距离的示意图。
图4为实施例提供的INS的位姿为浮点解时的车辆可能的位姿的示意图。
图5为实施例提供的车辆与左侧道路边界线的距离的示意图。
图6为实施例提供的有无边界约束的条件下的初始化位姿对比的示意图。
具体实施方式
下面结合附图和具体实施方式对本发明做详细描述。
在周围环境特征较少的情况下，比如高速公路、城市快速路上，使用车道线点云和高精度地图的车道线点云匹配进行地图匹配定位时，由于各车道的车道线点云极为相似，因此地图匹配定位精度十分依赖初始化位置。在实际应用中发现，定位程序启动后，当初始INS数据不准时，例如INS定位错车道，车辆初始化定位会跟随INS数据偏离当前真实位置，导致后续车辆定位一直不准，影响驾驶功能。
针对上述问题，考虑利用除地面车道线以外的信息对车辆初始化位置进行约束。前视相机在分割车道线信息的同时，也能将道路两边的护栏、边界、边石分割出来，这些特征具有独特性，且稳定存在于道路两边。根据车辆当前位置与这些特征的横向距离，可以有效地对车辆初始化位置进行约束，避免初始化定位错车道。
本实施例提供了一种借助道路边界线约束车辆初始化定位方法，如图1所示，包括以下步骤：
S1：获取车辆前视图像数据、INS数据、动态地图数据；
S2：根据三次曲线方程对车辆前视图像数据进行拟合，得到车道线点云、道路边界线点云；
S3：查找INS数据中不小于当前车辆前视图像数据时间戳的一帧INS数据及其上一帧INS数据，经过判断得到车辆所有可能的初始化位姿；
S4：对于所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
S5：从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，则执行步骤S6；若不符合匹配条件或不通过边界约束验证，则重复步骤S1～S5；
S6：将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
在本实施例中，S1中，所述的车辆前视图像数据为经过语义分割后的车道线、道路边界线信息。
所述的INS数据和车辆前视图像数据基于同一时钟源进行授时，INS数据与车辆前视图像数据时间同步；避免因为时间不同步导致数据出现偏差，导致定位不精准的问题。
在本实施例中，如图2所示，原点位于车辆后轴中心处，x指向车头，y垂直于x指向车身左侧，遵循右手定则。
在本实施例中，S2中的三次曲线方程表达式如下：
y＝a0+a1x+a2x2+a3x3 (1)
式(1)中，a0、a1、a2、a3分别为三次曲线方程0次项、1次项、2次项和3次项的系数，x为车辆的纵向坐标；a0表示道路边界线与车辆的横向距离，a0为正，表示左侧道路边界线与车辆的横向距离，a0为负，表示右侧道路边界线与车辆的横向距离；通过三次曲线方程得到车道线点云、道路边界线点云的方法简单快捷。
更具体的，如图3所示，两条道路边界线分别位于车辆的左侧与车辆的右侧；
三次曲线方程分别表示为：
位于车辆的左侧的道路边界线：
y1＝a0+a1x+a2x2+a3x3
式中，a0为正，表示左侧道路边界线与车辆的横向距离。
位于车辆的右侧的道路边界线：
式中，为负，表示右侧道路边界线与车辆的横向距离；/>分别与a0、a1、a2、a3对应。
更具体的，得到车道线点云的方法如下：
三次曲线方程拟合出单帧车道线、道路边界线，然后从拟合出的单帧车道线中取出其中的5～10米的一段，每间隔预设的一段距离取一个点，离散成点云；预设的距离可以为0.5m；以此距离离散的点云可以保证较高的精度。
在本实施例中，S3中判断得到车辆初始化位姿的所有可能位姿的方法如下：
若上一帧INS数据的位姿为固定解，该位姿为上一帧车辆可能的位姿；
若上一帧INS数据的位姿为浮点解，则将该位姿投影到地图的中心线上，投影得到的多个位姿为上一帧车辆可能的位姿；如图4所示，假定汽车行驶在同向拥有4条车道的路上，当前时刻的INS浮点解位于3车道，将此浮点解投影到4个车道的中心线上，得到4个可能的初始化位姿。
在本实施例中，S4中的航迹推算过程如下：
pn＝pn-1+Rn-1·vn-1·Δt (2)
式中，pn-1表示上一时刻的车辆位置，Rn-1表示车辆上一时刻的旋转矩阵，vn-1表示车辆在上一时刻的速度，表示车辆在上一时刻的角加速度，pn表示当前时刻的车辆位置，Rn表示当前时刻车辆的旋转矩阵，Δt为当前时刻与上一时刻的时间差。
通过式(2)和式(3)将INS数据从上一帧开始进行航迹推算，利用上一时刻车辆的轮速和角加速度计算出两帧间的车辆平移变化量和旋转变化量，推算得到当前时刻的车辆前视帧初始化位姿：
在本实施例中，S5中的匹配条件如下：
匹配点数占前视感知点云总数的第一阈值以上，误差的中位数小于第二阈值，且匹配点数大于第三阈值；所述的第一阈值可以为50％，所述的第二阈值可以为0.8m，所述的第三阈值可以为50；以此排除部分不符合条件的初始化位姿。
更具体的，S5中的边界约束验证如下：
查找左侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d1，计算三次曲线与车体的距离|a0|，d1与|a0|的差与第四阈值比较；为右侧道路边界线与车辆的横向距离。
查找右侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d2，计算三次曲线与车体的距离d2与/>的差与第四阈值比较。
若d1与|a0|的差、d2与的差均小于阈值，则认为此时的初始化位姿为最优位姿。以此排除所有不符合条件的初始化位姿，提高初始化定位的精度和准确度。
在本实施例中，如图5所示，在左边界线点云kdtree中选取2个离车辆最近的边界线点，假设过这两点的直线为l，判断车辆在l上的投影是否在这2点之间，如果在，则计算车辆到l的垂直距离d1，作为车辆与左边界线的距离；右边界线d2的计算方法同理。
在实施例中，如图6所示，假设INS定位错车道，由于车道线点云较为相似，在错误的车道上ICP匹配的结果也可能满足要求，导致车辆初始化位姿跟随INS错车道；加入边界约束后，错车道的初始化位姿到地图边界线的距离与前视相机拟合的边界线距离相差较远，因此被过滤掉。
在另一个具体的实施例中公开了一种借助道路边界线约束车辆初始化定位系统，采用如实施例1所述的借助道路边界线约束车辆初始化定位方法，包括：车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块、初始化模块；
所述的车载前视相机用于获取车辆前视图像数据；
所述的车载组合惯导模块用于获取INS数据、动态地图数据；
所述的数据拟合模块用于根据三次曲线方程进行拟合，得到车道线点云、道路边界线点云；
所述的数据查找模块根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿；
所述的数据处理模块用于对所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
所述的数据筛选模块从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，进入初始化模块；若不符合匹配条件或不通过边界约束验证，则重复车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块的工作过程；
所述的初始化模块用于将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
在另一个具体的实施例中公开了一种借助道路边界线约束车辆初始化定位方法，包括以下步骤：
S1：通过车载前视相机获取车辆前视图像数据；通过车载组合惯导模块获取INS数据、动态地图数据；将INS数据按照时间戳顺序保存在数据容器中；
S2：通过数据处理模块将得到的车辆前视图像数据根据三次曲线方程进行拟合，得到车道线点云、道路边界线点云；
S3：通过数据处理模块查找数据容器中的INS数据中不小于当前车辆前视图像数据时间戳的一帧INS数据及其上一帧INS数据，经过判断得到车辆初始化位姿的所有可能位姿；
S4：车载组合惯导模块根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，数据处理模块将前视帧中的车道线点云投影到动态地图数据中，与地图中的车道线点云进行ICP匹配，得到匹配结果；
S5：数据处理模块从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，则执行步骤S6；若不符合匹配条件或不通过边界约束验证，则重复步骤S1～S5；
S6：将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
在本实施例中，车载组合惯导模块中的惯性测量单元可以测量出车辆的加速度和角速度，配合上车辆的轮速脉冲信号，可以实时获取车辆的准确速度，从而通过航迹推算得到每一帧的车体位姿。
获取的动态地图数据可以表示为高精度地图数据。
经过多次在高速公路上的行车测试表明，利用道路边界线对车辆初始化位姿进行约束能够有效避免组合惯导定位不准导致车辆初始化位姿错车道的情况。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (11)

1.一种借助道路边界线约束车辆初始化定位方法，其特征在于，包括以下步骤：
S1：获取车辆前视图像数据、INS数据、动态地图数据；
S2：根据三次曲线方程对车辆前视图像数据进行拟合，得到车道线点云、道路边界线点云；
S3：查找INS数据中不小于当前车辆前视图像数据时间戳的一帧INS数据及其上一帧INS数据，经过判断得到车辆所有可能的初始化位姿；
S4：对于所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
S5：从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，则执行步骤S6；若不符合匹配条件或不通过边界约束验证，则重复步骤S1～S5；
S6：将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
2.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S1中，所述的车辆前视图像数据为经过语义分割后的车道线、道路边界线信息；
所述的INS数据和车辆前视图像数据基于同一时钟源进行授时，INS数据与车辆前视图像数据时间同步。
3.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S1中，对于获取的INS数据，按照时间戳的顺序进行保存。
4.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S2中的三次曲线方程表达式如下：
y＝a0+a1x+a2x2+a3x3 (1)
式(1)中，a0、a1、a2、a3分别为三次曲线方程0次项、1次项、2次项和3次项的系数，x为车辆的纵向坐标；a0表示道路边界线与车辆的横向距离，a0为正，表示左侧道路边界线与车辆的横向距离，a0为负，表示右侧道路边界线与车辆的横向距离。
5.根据权利要求4所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，得到车道线点云的方法如下：
三次曲线方程拟合出单帧车道线、道路边界线，然后从拟合出的单帧车道线中取出其中的5～10米的一段，每间隔预设的一段距离取一个点，离散成点云。
6.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S3中判断得到车辆初始化位姿的所有可能位姿的方法如下：
若上一帧INS数据的位姿为固定解，该位姿为上一帧车辆可能的位姿；
若上一帧INS数据的位姿为浮点解，则将该位姿投影到地图的中心线上，投影得到的多个位姿为上一帧车辆可能的位姿。
7.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S4中的航迹推算过程如下：
pn＝pn-1+Rn-1·vn-1·Δt (2)
式中，pn-1表示上一时刻的车辆位置，Rn-1表示车辆上一时刻的旋转矩阵，vn-1表示车辆在上一时刻的速度，表示车辆在上一时刻的角加速度，pn表示当前时刻的车辆位置，Rn表示当前时刻车辆的旋转矩阵，Δt为当前时刻与上一时刻的时间差；
通过式(2)和式(3)将INS数据从上一帧开始进行航迹推算，利用上一时刻车辆的轮速和角加速度计算出两帧间的车辆平移变化量和旋转变化量，推算得到当前时刻的车辆前视帧初始化位姿：
8.根据权利要求1所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S5中的匹配条件如下：
匹配点数占前视感知点云总数的第一阈值以上，误差的中位数小于第二阈值，且匹配点数大于第三阈值。
9.根据权利要求4所述的一种借助道路边界线约束车辆初始化定位方法，其特征在于，S5中的边界约束验证如下：
查找左侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d1，计算三次曲线与车体的距离|0|，d1与|0|的差与第四阈值比较；
查找右侧道路边界线点云与符合匹配条件的初始化位姿最近的两个点，计算车辆与这两点构成的线段的距离d2，计算三次曲线与车体的距离d2与/>的差与第四阈值比较；为右侧道路边界线与车辆的横向距离；
若d1与|0|的差、d2与的差均小于第四阈值，则认为此时的初始化位姿为最优位姿。
10.一种借助道路边界线约束车辆初始化定位系统，采用如权利要求1～9任一项所述的借助道路边界线约束车辆初始化定位方法，其特征在于，包括：车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块、初始化模块；
所述的车载前视相机用于获取车辆前视图像数据；
所述的车载组合惯导模块用于获取INS数据、动态地图数据；
所述的数据拟合模块用于根据三次曲线方程进行拟合，得到车道线点云、道路边界线点云；
所述的数据查找模块根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿；
所述的数据处理模块用于对所有可能位姿，根据上一帧INS数据做航迹推算得到当前的车辆前视帧初始化位姿，将前视帧中的车道线点云投影到动态地图数据中，与动态地图数据中的车道线点云进行ICP匹配，得到匹配结果；
所述的数据筛选模块从匹配结果中筛选符合匹配条件的初始化位姿，对符合匹配条件的初始化位姿进行边界约束验证，若符合匹配条件且通过边界约束验证，进入初始化模块；若不符合匹配条件或不通过边界约束验证，则重复车载前视相机、车载组合惯导模块、数据拟合模块、数据查找模块、数据处理模块、数据筛选模块的工作过程；
所述的初始化模块用于将通过边界约束验证的初始化位姿作为最优初始化位姿，完成车辆初始化定位。
11.根据权利要求10所述的一种借助道路边界线约束车辆初始化定位系统，其特征在于，还包括数据容器；
所述的数据容器用于对获取的INS数据，按照时间戳的顺序进行保存。
CN202310660170.1A 2023-06-05 2023-06-05 一种借助道路边界线约束车辆初始化定位方法及其系统 Pending CN116659491A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310660170.1A CN116659491A (zh) 2023-06-05 2023-06-05 一种借助道路边界线约束车辆初始化定位方法及其系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310660170.1A CN116659491A (zh) 2023-06-05 2023-06-05 一种借助道路边界线约束车辆初始化定位方法及其系统

Publications (1)

Publication Number Publication Date
CN116659491A true CN116659491A (zh) 2023-08-29

Family

ID=87713435

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310660170.1A Pending CN116659491A (zh) 2023-06-05 2023-06-05 一种借助道路边界线约束车辆初始化定位方法及其系统

Country Status (1)

Country Link
CN (1) CN116659491A (zh)

Similar Documents

Publication Publication Date Title
CN111380539B (zh) 车辆定位、导航方法和装置及相关系统
Rose et al. An integrated vehicle navigation system utilizing lane-detection and lateral position estimation systems in difficult environments for GPS
EP3492871B1 (en) Self-position estimation method and self-position estimation apparatus
KR102441073B1 (ko) 자이로 센싱값 보상 장치, 그를 포함한 시스템 및 그 방법
CN112904395B (zh) 一种矿用车辆定位系统及方法
Shunsuke et al. GNSS/INS/on-board camera integration for vehicle self-localization in urban canyon
JP4596566B2 (ja) 自車情報認識装置及び自車情報認識方法
CN109154505B (zh) 用于确定车辆相对于马路的车道的横向位置的方法、设备和具有指令的计算机可读存储介质
WO2021046578A1 (en) Vehicular sensor system calibration
US11928871B2 (en) Vehicle position estimation device and traveling position estimation method
US20190266419A1 (en) Method, Device And Computer-Readable Storage Medium With Instructions For Determining The Lateral Position Of A Vehicle Relative To The Lanes Of A Road
CN110018503B (zh) 车辆的定位方法及定位系统
JP2013032953A (ja) 位置判定装置およびナビゲーション装置並びに位置判定方法，プログラム
CN114088114A (zh) 车辆位姿校准方法、装置和电子设备
CN114889606A (zh) 一种基于多传感融合的低成本高精定位方法
US11908206B2 (en) Compensation for vertical road curvature in road geometry estimation
KR20200002257A (ko) 꼭지점 검출 기반의 도로 표지판 검출 방법 및 장치
CN110094498B (zh) 一种轮速比的获取方法及装置
JP2021120683A (ja) 出力装置、制御方法、プログラム及び記憶媒体
JP6790951B2 (ja) 地図情報学習方法及び地図情報学習装置
CN113566817A (zh) 一种车辆定位方法及装置
JP7025293B2 (ja) 自車位置推定装置
CN116524454A (zh) 物体追踪装置、物体追踪方法及存储介质
CN116659491A (zh) 一种借助道路边界线约束车辆初始化定位方法及其系统
CN115790613A (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