CN116518992A - 一种退化场景下的无人车定位方法和装置 - Google Patents

一种退化场景下的无人车定位方法和装置 Download PDF

Info

Publication number
CN116518992A
CN116518992A CN202310399453.5A CN202310399453A CN116518992A CN 116518992 A CN116518992 A CN 116518992A CN 202310399453 A CN202310399453 A CN 202310399453A CN 116518992 A CN116518992 A CN 116518992A
Authority
CN
China
Prior art keywords
points
point
point cloud
laser
map
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
Application number
CN202310399453.5A
Other languages
English (en)
Other versions
CN116518992B (zh
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.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
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 Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202310399453.5A priority Critical patent/CN116518992B/zh
Publication of CN116518992A publication Critical patent/CN116518992A/zh
Application granted granted Critical
Publication of CN116518992B publication Critical patent/CN116518992B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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

Abstract

本发明公开了一种退化场景下的无人车定位方法，结合预先设计好的特征图层进行全局定位，消除定位的累积误差，以及融合imu和车辆底盘速度信息，提供较为准确的实际位姿，最后将里程计和全局匹配的观测信息融入卡尔曼滤波框架中对实际位姿进行校正，得到高频的、鲁棒的无人车定位结果。本发明还公开了一种退化场景下的无人车定位装置和存储介质。

Description

一种退化场景下的无人车定位方法和装置
技术领域
本发明属于无人车定位技术领域，具体涉及一种退化场景下的无人车定位方法和装置。
背景技术
在无人驾驶领域中，鲁棒的定位是必不可少的环节之一。目前基于点云的定位方案已经十分成熟，然而在一些退化场景中，例如下地库或者隧道中，基于点云的定位方案仍存在巨大的挑战。
在这些退化场景中，纵向上的结构特征不够丰富且大量重复，同时伴随着地面的起伏以及坡道的影响，对定位造成了严重的影响。目前主流的定位方案为点云配准算法，点云配准(Point Cloud Registration)算法指的是输入两幅点云Ps(source)和Pt(target)，输出一个变换T(即旋转R和平移t)使得T(Ps)和Pt的重合程度尽可能高。常用的点云配准算法有NDT、ICP。
例如迭代最近邻算法(ICP，Iterative Closest Point)和正态分布变换算法(NDT，Normal Distributions Transform)，迭代最近邻算法是经典的精配准算法，其以“点”为配准基元，不断迭代求得最优的位姿，最终使得目标函数最小。
正态分布变换算法(NDT，Normal Distributions Transform)，正态分布变换算法是一个一次性初始化的工作。和迭代最近邻算法相比，正态分布变换算法的配准效果同迭代最近邻算法相似，其改进实质等同于将三维点云栅格化，并统计每个栅格点云的正太分布，是另一种传统的点云配准算法。该算法实质是使用优化方法迭代计算，使得两幅点云正太分布的概率密度之和最大，从而计算得到两幅点云的位姿变换。
迭代最近邻算法和正态分布变换算法均是将整个的实时点云与场景点云进行匹配进而得到定位结果，强依赖于场景中丰富的结构特征，但是在退化场景中，这种定位方式就不太适用。
还有一些基于点云特征的定位方案，例如基于激光雷达的SLAM算法(LOAM，LidarOdometry and Mapping in Real-time)系列的算法，也应用相当广泛，在这些方案中，从点云中提取出平面点和边缘点，利用边缘点以及平面点的对应关系进行定位，基于特征的定位方式是一个很好的策略，但是由于退化场景中特征点的稀少以及重复性较高，导致定位结果存在累积误差和不稳定性。
发明内容
本发明提供一种退化场景下的无人车定位方法，利用该方法能够在退化场景准确的、稳定的获得无人车定位结果。
本发明具体实施例提供一种退化场景下的无人车定位方法，包括：
基于上一帧和当前帧的IMU信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息；
所述卡尔曼滤波模型的更新方式，包括：
获得退化场景的点云地图，对点云地图进行预处理得到特征图层，特征图层包括每个点对应的类型标签、平面方程、直线方程和占据体素信息；
获得激光点云，将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取；
对不同帧实际位姿进行时间插值得到与激光点云时间同步的同步位姿；
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
进一步的，所述卡尔曼滤波模型的更新方式，还包括：
对上一帧激光点云对应的特征点和当前帧激光点云对应的特征点进行点云配准得到相对位姿，并根据与上一帧激光点云时间同步的同步位姿和所述相对位姿得到里程计位姿，基于里程计位姿和与当前帧激光点云时间同步的同步位姿的差值对卡尔曼滤波模型进行更新。
进一步的，所述对上一帧激光点云对应的特征点和当前帧激光点云对应的特征点进行点云配准，包括：
根据与上一帧激光点云时间同步的同步位姿和与当前帧激光点云时间同步的同步位姿，将上一帧激光点云对应的特征点投影到当前帧激光点云对应的特征点坐标系中，根据特征点的类型进行最近邻搜索，采用ICP算法得到相对位姿。
进一步的，所述对点云地图进行预处理，包括：
遍历点云地图中的每个点，设定每个点的领域范围，基于每个点的领域范围得到对应的特征值，基于对应的特征值将每个点分为角点，边缘点，平面点、无效点和直线点，并标记点的类型标签，基于提取的平面点集合拟合得到多个平面方程，基于提取的直线点集合拟合得到多个直线方程；
按照设定分辨率对点云地图进行划分得到空间体素，将存在点的体素标记为占据体素，将不存在点的体素标记为非占据体素；
通过类型标签、平面方程、直线方程和占据体素信息构建特征图层。
进一步的，所述对保留的激光点云进行特征点的提取，包括：
根据同步位姿将对应的激光点云映射到点云地图坐标系中，根据特征图层的占据体素信息，过滤掉属于非占据体素中的点；
根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，然后根据激光发射装置的高度将二维矩阵分为上下两部分；
统计二维矩阵的下半部分中的每个点的横向曲率与纵向仰角信息，将小于曲率最小阈值且小于仰角最小阈值的点作为地面点；
统计二维矩阵的上半部分的每个点的横向曲率信息，将小于横向曲率最小阈值的点作为平面点，将大于横向曲率最大阈值的点作为角点；
通过获得的地面点、平面点和角点统称为特征点。
进一步的，所述根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，包括：
在xoy平面上以激光发射点为原点按照设定的角度将xoy平面分成n等份，每个等份对应一个点平均距离，将多个激光垂直方向角对应的剩余的点分别投影到xoy平面上得到每个激光垂直方向角对应的n个点平均距离，按照激光垂直方向角的角度由高到低排列得到每行由n个点平均距离组成的l×n的二维矩阵，其中，l为激光垂直方向角的数量；
点平均距离为投影到对应等份上的不同激光垂直方向角对应的剩余的点到激光发射点的距离的平均值。
进一步的，所述获得对应的激光点云的特征点与近邻地图点的多个统计误差，包括：
若对应的激光点云的特征点为地面点，则统计对应的激光点云的特征点和近邻地图点的高程误差；
若对应的激光点云的特征点为平面点，则从特征图层中获取近邻地图点的平面方程，统计对应的激光点云的特征点到平面方程的距离误差；
若对应的激光点云的特征点为角点，近邻地图点类型为直线点，则从特征图层中获取近邻地图点的直线方程，统计对应的激光点云的特征点到直线方程的距离误差；
若对应的激光点云的特征点为角点，近邻地图点为角点，则统计对应的激光点云的特征点到近邻地图点间的距离误差；
通过对应的激光点云的特征点和近邻地图点的高程误差、对应的激光点云的特征点到平面方程的距离误差、对应的激光点云的特征点到直线方程的距离误差和对应的激光点云的特征点到近邻地图点间的距离误差构建对应的激光点云的特征点与近邻地图点的多个统计误差。
本发明具体实施例还提供了一种退化场景下的无人车定位装置，包括：
卡尔曼滤波预测单元，用于基于上一帧和当前帧的IMU信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息；
卡尔曼滤波纠正单元，用于对卡尔曼滤波模型进行更新，包括：获得退化场景的点云地图，对点云地图进行预处理得到特征图层，特征图层包括多种类型的点和每个点的特征信息，特征信息包括每个点对应的类型标签、平面方程、直线方程和占据体素信息；
获得激光点云，将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取；
对不同帧实际位姿进行线性插值得到与激光点云时间同步的同步位姿；
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
本发明具体实施例还提供了一种基于退化场景下的无人车定位装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现所述的退化场景下的无人车定位方法。
本发明具体实施例还提供了一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现所述的退化场景下的无人车定位方法。
与现有技术相比，本发明的有益效果为：
本发明提出一种退化场景下的无人车定位方法，结合预先设计好的特征图层进行全局定位，消除定位的累积误差，以及将特征全局匹配的位姿输入到卡尔曼滤波模型中进行模型的更新纠正，最后融合imu(惯性测量单元(IMU，Inertial measurement unit)和车辆底盘速度信息，提供高频的无人车实时位姿，得到高频的、鲁棒的无人车定位结果。
附图说明
图1为本发明具体实施例提供的基于退化场景下的无人车定位方法得到的扫描图；
图2为本发明具体实施例提供的基于退化场景下的无人车定位方法的示意框图；
图3为本发明具体实施例提供的退化场景示意图。
图4为本发明具体实施例提供的基于退化场景下的无人车定位方法的卡尔曼滤波模型纠正流程图；
图5为本发明具体实施例提供的获得二维矩阵示意图；
图6为本发明具体实施例提供的基于退化场景下的无人车定位装置框图。
具体实施方式
以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。
结合预先设计好的特征图层进行全局定位，消除定位的累积误差，以及将特征里程计与特征全局匹配的位姿输入到卡尔曼滤波模型中进行模型的更新纠正，最后融合imu信息和车辆底盘速度信息，提供高频的无人车实时位姿，得到高频的、鲁棒的无人车定位结果，退化场景示意图，如图1所示。
本发明具体实施例方式提供一种基于退化场景下的无人车定位方法，如图2所示，包括：
S1、基于上一帧和当前帧的IMU(惯性测量单元(IMU，Inertial measurementunit)信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息，如图4所示。本发明提供的卡尔曼滤波用误差卡尔曼滤波算法，详细推导可以参考论文《Quaternion kinematics for the error-state Kalman filter》，具体步骤为：
S11、输入imu信息，根据上一帧和当前帧的时间戳间隔，对角速度进行积分，得到角度的增量，即姿态的增量。
S12、输入车辆底盘速度信息，根据上一帧和当前帧的时间戳间隔，对速度进行积分，得到位移的增量，即位置的增量。
S13、根据上一帧实际位姿，以及步骤S21和S22中得到的位姿的增量得到当前帧实际位姿。
S2、本发明具体实施例提供通过全局匹配校正或里程计校正对对卡尔曼滤波模型进行更新，如图3和图4，包括：
S21、获得退化场景的点云地图，如图1所示，对点云地图进行预处理得到特征图层，具体步骤为：获得场景的点云地图,遍历点云中的每个点，在其邻域范围内，例如0.5米范围内，统计其PCA信息，根据PCA的特征值分布获得角点，边缘点，平面点和无效点，并且根据高度信息提取出地面点。按照设定分辨率对点云地图进行划分得到空间体素，将存在点的体素标记为占据体素，将不存在点的体素标记为非占据体素。该特征图层包括每个点对应的平面方程和直线方程，以及占据体素的信息，该多种类型的点包括角点，边缘点，平面点、无效点和直线点。
本发明具体实施例将获得的角点标记为角点，将获得的地面点标记为地面点，对标记的平面点，直线点，角点，地面点以及其对应的特征信息进行人工校验，保证特征信息的完整性和正确性。
从本发明具体实施例提供的平面点集合中拟合多个平面方程，并将拟合的外点和噪声点标记为无效点，拟合的内点标记为平面点，并记录平面方程信息。
从本发明具体实施例提供的直线点中拟合多个直线方程，并将拟合的外点和噪声点标记为无效点，拟合的内点标记为直线点，并记录直线方程信息。
本发明具体实施例将点云地图按照空间分辨率，例如1m*1m*1m，进行划分，得到一系列的空间体素，若体素内存在点，则该体素标记为占据体素，否则标记为非占据体素。
S22、由于IMU和激光扫描的时间不一致，因此通过步骤S1输出的实际位姿的时间与激光点云需要进行时间同步，因此首先需要对得到的不同帧实际位姿按照对应的时间戳进行时间插值操作得到与激光点云时间同步的同步位姿。
S23、将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取。本发明具体实施例提供的对保留的激光点云进行特征点的提取的具体步骤为：
S231、接收感知模型输出的动态障碍物检测框，从激光点云中过滤掉动态障碍物检测框内的动态障碍物的点。
S232、根据同步位姿将对应的激光点云映射到点云地图坐标系中，将对应的激光点云投影到特征图层的空间体素内，若该空间体素的标记为非占据体素，则说明投影到该体素内的点为障碍物，过滤掉非占据体素内的点，保留位于占据体素的点。
S233、根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，然后根据激光发射装置的高度基于经验值将二维矩阵分为上下两部分；统计二维矩阵的下半部分中的每个点的横向曲率与纵向仰角信息，将小于曲率最小阈值且小于仰角最小阈值的点作为地面点；统计二维矩阵的上半部分的每个点的横向曲率信息，将小于横向曲率最小阈值的点作为平面点，将大于横向曲率最大阈值的点作为角点，通过获得的地面点、平面点和角点统称为特征点。
在一具体实施例中，根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵的具体步骤为：根据激光发射装置发射的激光线组成多个水平面，如图5的(a)所示，在xoy平面上以激光发射点为原点O按照设定的角度将xoy平面分成12等份，每个等份对应一个点平均距离，点平均距离为投影到对应等份上的不同激光垂直方向角对应的剩余的点到激光发射点的距离的平均值。相对于xoy平面得到激光垂直方向角(vertical angle)1，即角度POQ，保持激光垂直方向角1不变，将激光垂直方向角1对应的剩余的点投影到xoy平面上，将落入到每个等份内的投影点对应的剩余的点到激光发射点的距离的平均值作为每个等份对应的点平均距离，如图5的(b)所示，激光垂直方向角1对应的12个点平均距离，作为二维矩阵的一行数据，将激光垂直方向角的角度由高到低排列，即按照角度由高到低排列，激光垂直方向角1＞激光垂直方向角2＞激光垂直方向角3，得到每行由12个点平均距离组成的3×12的二维矩阵。
S24、本发明具体实施例提供了基于同步位姿、特征点和特征图层通过全局匹配校正对卡尔曼滤波模型进行更新，包括：
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
在一具体实施例中，获得近邻地图点的具体步骤为：从而点云地图中获得与对应的激光点云的特征点类型相匹配的地图点，采用最邻近算法从类型相匹配的地图点中筛选与对应的激光点云的特征点最近邻的地图点，将筛选的地图点作为对应的近邻地图点。
在一具体实施例中，本发明具体实施例提供的对应的激光点云的特征点包括地面点、角点和平面点；若对应的激光点云的特征点为地面点，则统计对应的激光点云的特征点和近邻地图点的高程误差dh；若对应的激光点云的特征点为平面点，则从特征图层中获取近邻地图点的平面方程，统计对应的激光点云的特征点到平面方程的距离误差dplane；若对应的激光点云的特征点为角点，近邻地图点类型为直线点，则从特征图层中获取近邻地图点的直线方程，统计对应的激光点云的特征点到直线方程的距离误差dline；若对应的激光点云的特征点为角点，近邻地图点为角点，则统计对应的激光点云的特征点到近邻地图点间的距离误差dpoint；将对应的激光点云的特征点和近邻地图点的高程误差、对应的激光点云的特征点到平面方程的距离误差、对应的激光点云的特征点到直线方程的距离误差和对应的激光点云的特征点到近邻地图点间的距离误差作为对应的激光点云的特征点与近邻地图点的多个统计误差。
根据统计的误差采用最小二乘对同步位姿进行迭代优化，基于统计的误差构建的优化方程为 其中m为对应激光点的特征点的个数，i为对应激光点的特征点的索引。
本发明具体实施例还提供了基于特征点和同步位姿通过里程计校正对卡尔曼滤波模型进行更新，包括：
基于上一帧激光点云和当前帧激光点云对应的特征点进行点云配准得到相对位姿，并根据与上一帧激光点云时间同步的同步位姿和所述相对位姿得到里程计位姿，基于里程计位姿和当前帧激光点云时间同步的同步位姿的差值对卡尔曼滤波模型进行更新。
在一具体实施例中，基于上一帧激光点云和当前帧激光点云对应的特征点进行点云配准得到相对位姿，包括：
根据与上一帧激光点云时间同步的同步位姿和与当前帧激光点云时间同步的同步位姿，将上一帧激光点云对应的特征点投影到当前帧激光点云对应的特征点坐标系中，根据特征点的类型进行最近邻搜索，采用ICP算法得到相对位姿。
本发明具体实施例还提供了一种基于退化场景下的无人车定位装置，如图6所示，包括：
卡尔曼滤波预测单元，基于上一帧和当前帧的IMU(惯性测量单元(IMU，Inertialmeasurement unit)信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息，如图4所示。本发明提供的卡尔曼滤波用误差卡尔曼滤波算法，详细推导可以参考论文《Quaternion kinematics for the error-stateKalman filter》，具体步骤为：
S11、输入imu信息，根据上一帧和当前帧的时间戳间隔，对角速度进行积分，得到角度的增量，即姿态的增量。
S12、输入车辆底盘速度信息，根据上一帧和当前帧的时间戳间隔，对速度进行积分，得到位移的增量，即位置的增量。
S13、根据上一帧实际位姿，以及步骤S21和S22中得到的位姿的增量得到当前帧实际位姿。
卡尔曼滤波纠正单元，用于对卡尔曼滤波模型进行更新，包括：
S21、获得退化场景的点云地图，对点云地图进行预处理得到特征图层，具体步骤为：获得场景的点云地图，如图3所示，遍历点云中的每个点，在其邻域范围内，例如0.5米范围内，统计其PCA信息，根据PCA的特征值分布获得角点，边缘点，平面点和无效点，并且根据高度信息提取出地面点。按照设定分辨率对点云地图进行划分得到空间体素，将存在点的体素标记为占据体素，将不存在点的体素标记为非占据体素。该特征图层包括不同类型的点和每个点的特征信息，特征信息包括每个点对应的平面方程和直线方程，以及占据体素的信息，该多种类型的点包括角点，边缘点、平面点、无效点和直线点。
从本发明具体实施例提供的平面点集合中拟合多个平面方程，并将拟合的外点和噪声点标记为无效点，拟合的内点标记为平面点，并记录平面方程信息。
从本发明具体实施例提供的直线点中拟合多个直线方程，并将拟合的外点和噪声点标记为无效点，拟合的内点标记为直线点，并记录直线方程信息。
本发明具体实施例将获得的角点标记为角点，将获得的地面点标记为地面点，对标记的平面点，直线点，角点，地面点以及其对应的特征信息进行人工校验，保证特征信息的完整性和正确性。
本发明具体实施例将点云地图按照空间分辨率，例如1m*1m*1m，进行划分，得到一系列的空间体素，若体素内存在点，则该体素标记为占据体素，否则标记为非占据体素。
S22、由于IMU和激光扫描的时间不一致，因此通过步骤S1输出的实际位姿的时间与激光点云需要进行时间同步，因此首先需要对得到的不同帧实际位姿按照对应的时间戳进行线性插值操作得到与激光点云时间同步的同步位姿。
S23、将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取。本发明具体实施例提供的对保留的激光点云进行特征点的提取的具体步骤为：
S231、接收感知模型输出的动态障碍物检测框，从激光点云中过滤掉动态障碍物检测框内的动态障碍物的点。
S232、根据同步位姿将对应的激光点云映射到点云地图坐标系中，将对应的激光点云投影到特征图层的空间体素内，若该空间体素的标记为非占据体素，则说明投影到该体素内的点为障碍物，过滤掉非占据体素内的点，保留位于占据体素的点。
S233、根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，然后根据激光发射装置的高度基于经验值将二维矩阵分为上下两部分；统计二维矩阵的下半部分中的每个点的横向曲率与纵向仰角信息，将小于曲率最小阈值且小于仰角最小阈值的点作为地面点；统计二维矩阵的上半部分的每个点的横向曲率信息，将小于横向曲率最小阈值的点作为平面点，将大于横向曲率最大阈值的点作为角点。
在一具体实施例中，根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵的具体步骤为：根据激光发射装置发射的激光线组成多个水平面，如图5的(a)所示，在xoy平面上以激光发射点为原点O按照设定的角度将xoy平面分成12等份，每个等份对应一个点平均距离，点平均距离为投影到对应等份上的不同激光垂直方向角对应的剩余的点到激光发射点的距离的平均值。相对于xoy平面得到激光垂直方向角(vertical angle)1，即角度POQ，保持激光垂直方向角1不变，将激光垂直方向角1对应的剩余的点投影到xoy平面上，将落入到每个等份内的投影点对应的剩余的点到激光发射点的距离的平均值作为每个等份对应的点平均距离，如图5的(b)所示，激光垂直方向角1对应的12个点平均距离，作为二维矩阵的一行数据，将激光垂直方向角的角度由高到低排列，即按照角度由高到低排列，激光垂直方向角1＞激光垂直方向角2＞激光垂直方向角3，得到每行由12个点平均距离组成的3×12的二维矩阵。
S24、通过全局匹配校正或里程计校正对对卡尔曼滤波模型进行更新，其中，通过全局匹配校正对卡尔曼滤波模型进行更新，包括：
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
在一具体实施例中，获得近邻地图点的具体步骤为：从而点云地图中获得与对应的激光点云的特征点类型相匹配的地图点，采用最邻近算法从类型相匹配的地图点中筛选与对应的激光点云的特征点最近邻的地图点，将筛选的地图点作为对应的近邻地图点。
在一具体实施例中，本发明具体实施例提供的对应的激光点云的特征点包括地面点、角点和平面点；若对应的激光点云的特征点为地面点，则统计对应的激光点云的特征点和近邻地图点的高程误差dh；若对应的激光点云的特征点为平面点，则从特征图层中获取近邻地图点的平面方程，统计对应的激光点云的特征点到平面方程的距离误差dplane；若对应的激光点云的特征点为角点，近邻地图点类型为直线点，则从特征图层中获取近邻地图点的直线方程，统计对应的激光点云的特征点到直线方程的距离误差dline；若对应的激光点云的特征点为角点，近邻地图点为角点，则统计对应的激光点云的特征点到近邻地图点间的距离误差dpoint；将对应的激光点云的特征点和近邻地图点的高程误差、对应的激光点云的特征点到平面方程的距离误差、对应的激光点云的特征点到直线方程的距离误差和对应的激光点云的特征点到近邻地图点间的距离误差作为对应的激光点云的特征点与近邻地图点的多个统计误差。
根据统计的误差采用最小二乘对同步位姿进行迭代优化，基于统计的误差构建的优化方程为 其中m为对应激光点的特征点的个数，i为对应激光点的特征点的索引，Poseopt为位姿优化量。
本发明还提供了一种基于退化场景下的无人车定位装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现退化场景下的无人车定位方法。
本发明还提供了一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现退化场景下的无人车定位方法。
本发明中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的实施例而已，并不用于限制本发明。对于本领域技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (10)

1.一种退化场景下的无人车定位方法，其特征在于，包括：
基于上一帧和当前帧的IMU信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息；
所述卡尔曼滤波模型的更新方式，包括：
获得退化场景的点云地图，对点云地图进行预处理得到特征图层，特征图层包括每个点对应的类型标签、平面方程、直线方程和占据体素信息；
获得激光点云，将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取；
对不同帧实际位姿进行时间插值得到与激光点云时间同步的同步位姿；
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
2.根据权利要求1所述的一种退化场景下的无人车定位方法，其特征在于，所述卡尔曼滤波模型的更新方式，还包括：
对上一帧激光点云对应的特征点和当前帧激光点云对应的特征点进行点云配准得到相对位姿，并根据与上一帧激光点云时间同步的同步位姿和所述相对位姿得到里程计位姿，基于里程计位姿和与当前帧激光点云时间同步的同步位姿的差值对卡尔曼滤波模型进行更新。
3.根据权利要求2所述的一种退化场景下的无人车定位方法，其特征在于，所述对上一帧激光点云对应的特征点和当前帧激光点云对应的特征点进行点云配准，包括：
根据与上一帧激光点云时间同步的同步位姿和与当前帧激光点云时间同步的同步位姿，将上一帧激光点云对应的特征点投影到当前帧激光点云对应的特征点坐标系中，根据特征点的类型进行最近邻搜索，采用ICP算法得到相对位姿。
4.根据权利要求1所述的一种退化场景下的无人车定位方法，其特征在于，所述对点云地图进行预处理，包括：
遍历点云地图中的每个点，设定每个点的领域范围，基于每个点的领域范围得到对应的特征值，基于对应的特征值将每个点分为角点，边缘点，平面点、无效点和直线点，并标记点的类型标签，基于提取的平面点集合拟合得到多个平面方程，基于提取的直线点集合拟合得到多个直线方程；
按照设定分辨率对点云地图进行划分得到空间体素，将存在点的体素标记为占据体素，将不存在点的体素标记为非占据体素；
通过类型标签、平面方程、直线方程和占据体素信息构建特征图层。
5.根据权利要求1所述的一种退化场景下的无人车定位方法，其特征在于，所述对保留的激光点云进行特征点的提取，包括：
根据同步位姿将对应的激光点云映射到点云地图坐标系中，根据特征图层的占据体素信息，过滤掉属于非占据体素中的点；
根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，然后根据激光发射装置的高度将二维矩阵分为上下两部分；
统计二维矩阵的下半部分中的每个点的横向曲率与纵向仰角信息，将小于曲率最小阈值且小于仰角最小阈值的点作为地面点；
统计二维矩阵的上半部分的每个点的横向曲率信息，将小于横向曲率最小阈值的点作为平面点，将大于横向曲率最大阈值的点作为角点；
通过获得的地面点、平面点和角点统称为特征点。
6.根据权利要求5所述的一种退化场景下的无人车定位方法，其特征在于，所述根据剩余的点的空间位置对剩余的点进行降维操作得到二维矩阵，包括：
在xoy平面上以激光发射点为原点按照设定的角度将xoy平面分成n等份，每个等份对应一个点平均距离，将多个激光垂直方向角对应的剩余的点分别投影到xoy平面上得到每个激光垂直方向角对应的n个点平均距离，按照激光垂直方向角的角度由高到低排列得到每行由n个点平均距离组成的l×n的二维矩阵，其中，l为激光垂直方向角的数量；
点平均距离为投影到对应等份上的不同激光垂直方向角对应的剩余的点到激光发射点的距离的平均值。
7.根据权利要求5所述的一种退化场景下的无人车定位方法，其特征在于，所述获得对应的激光点云的特征点与近邻地图点的多个统计误差，包括：
若对应的激光点云的特征点为地面点，则统计对应的激光点云的特征点和近邻地图点的高程误差；
若对应的激光点云的特征点为平面点，则从特征图层中获取近邻地图点的平面方程，统计对应的激光点云的特征点到平面方程的距离误差；
若对应的激光点云的特征点为角点，近邻地图点类型为直线点，则从特征图层中获取近邻地图点的直线方程，统计对应的激光点云的特征点到直线方程的距离误差；
若对应的激光点云的特征点为角点，近邻地图点为角点，则统计对应的激光点云的特征点到近邻地图点间的距离误差；
通过对应的激光点云的特征点和近邻地图点的高程误差、对应的激光点云的特征点到平面方程的距离误差、对应的激光点云的特征点到直线方程的距离误差和对应的激光点云的特征点到近邻地图点间的距离误差构建对应的激光点云的特征点与近邻地图点的多个统计误差。
8.一种退化场景下的无人车定位装置，其特征在于，包括：
卡尔曼滤波预测单元，用于基于上一帧和当前帧的IMU信息以及车辆底盘速度信息，根据卡尔曼滤波模型或更新后的卡尔曼滤波模型得到上一帧至当前帧的位姿增量，基于上一帧实际位姿与所述位姿增量计算得到当前帧实际位姿，即无人车定位信息；
卡尔曼滤波纠正单元，用于对卡尔曼滤波模型进行更新，包括：获得退化场景的点云地图，对点云地图进行预处理得到特征图层，特征图层包括多种类型的点和每个点的特征信息，特征信息包括每个点对应的类型标签、平面方程、直线方程和占据体素信息；
获得激光点云，将激光点云投影到特征图层的空间体素内，保留位于占据体素的激光点云，并对保留的激光点云进行特征点的提取；
对不同帧实际位姿进行线性插值得到与激光点云时间同步的同步位姿；
根据同步位姿将对应的激光点云的特征点投影到点云地图中，从点云地图中获得与对应的激光点云的特征点的空间距离最近且类型相同的近邻地图点，获得对应的激光点云的特征点与近邻地图点的多个统计误差，基于多个统计误差采用最小二乘算法对同步位姿进行迭代优化得到全局位姿，基于全局位姿和同步位姿的差值对卡尔曼滤波模型进行更新。
9.一种基于退化场景下的无人车定位装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现权利要求1-7中任一项所述的退化场景下的无人车定位方法。
10.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-7中任一项所述的退化场景下的无人车定位方法。
CN202310399453.5A 2023-04-14 2023-04-14 一种退化场景下的无人车定位方法和装置 Active CN116518992B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310399453.5A CN116518992B (zh) 2023-04-14 2023-04-14 一种退化场景下的无人车定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310399453.5A CN116518992B (zh) 2023-04-14 2023-04-14 一种退化场景下的无人车定位方法和装置

Publications (2)

Publication Number Publication Date
CN116518992A true CN116518992A (zh) 2023-08-01
CN116518992B CN116518992B (zh) 2023-09-08

Family

ID=87403919

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310399453.5A Active CN116518992B (zh) 2023-04-14 2023-04-14 一种退化场景下的无人车定位方法和装置

Country Status (1)

Country Link
CN (1) CN116518992B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115451948A (zh) * 2022-08-09 2022-12-09 中国科学院计算技术研究所 一种基于多传感器融合的农业无人车定位里程计方法及系统
WO2023050638A1 (zh) * 2021-09-29 2023-04-06 上海仙途智能科技有限公司 基于激光点云的路沿识别
CN115930977A (zh) * 2022-12-08 2023-04-07 北京踏歌智行科技有限公司 特征退化场景的定位方法、系统、电子设备和可读存介质
CN116124161A (zh) * 2022-12-22 2023-05-16 东南大学 一种基于先验地图的LiDAR/IMU融合定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023050638A1 (zh) * 2021-09-29 2023-04-06 上海仙途智能科技有限公司 基于激光点云的路沿识别
CN115451948A (zh) * 2022-08-09 2022-12-09 中国科学院计算技术研究所 一种基于多传感器融合的农业无人车定位里程计方法及系统
CN115930977A (zh) * 2022-12-08 2023-04-07 北京踏歌智行科技有限公司 特征退化场景的定位方法、系统、电子设备和可读存介质
CN116124161A (zh) * 2022-12-22 2023-05-16 东南大学 一种基于先验地图的LiDAR/IMU融合定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
YUWEI CHENG: "Relocalization based on millimeter wave radar point cloud for visually degraded environments", JOURNAL OF FIELD ROBOTICS *
刘振宇;张德喜;田大吉;: "基于xtion和惯导融合的机器人定位研究", 计算机仿真, no. 05 *
欧阳仕晗;刘振宇;赵怡巍;秦圣然;刘潇;: "移动机器人三维激光SLAM算法研究", 微处理机, no. 05 *
苏本跃;韩韦;彭玉升;盛敏;: "面向RGB-D数据的4D-ICP点云配准方法", 南京大学学报(自然科学), no. 04 *

Also Published As

Publication number Publication date
CN116518992B (zh) 2023-09-08

Similar Documents

Publication Publication Date Title
WO2021093240A1 (en) Method and system for camera-lidar calibration
CN110163930B (zh) 车道线生成方法、装置、设备、系统及可读存储介质
CN113409410B (zh) 一种基于3d激光雷达的多特征融合igv定位与建图方法
WO2020108510A1 (en) Method and system for semantic label generation using sparse 3d data
WO2018061010A1 (en) Point cloud transforming in large-scale urban modelling
JP5753422B2 (ja) ３ｄパターンマッチング方法
CN115372989A (zh) 基于激光雷达的越野自动小车长距离实时定位系统及方法
KR102233260B1 (ko) 정밀 지도 업데이트 장치 및 방법
CN116518992B (zh) 一种退化场景下的无人车定位方法和装置
US11348261B2 (en) Method for processing three-dimensional point cloud data
CN117392268A (zh) 一种基于自适应结合cpd和icp算法的激光扫描建图方法及系统
CN117392237A (zh) 一种鲁棒的激光雷达-相机自标定方法
CN114563000B (zh) 一种基于改进型激光雷达里程计的室内外slam方法
CN114280583B (zh) 无gps信号下激光雷达定位精度验证方法及系统
CN115239978A (zh) 一种基于实例分割的相机和激光雷达自动标定方法
CN111784798B (zh) 地图生成方法、装置、电子设备和存储介质
WO2022021209A9 (zh) 电子地图生成方法、装置、计算机设备和存储介质
CN114913297A (zh) 一种基于mvs稠密点云的场景正射影像生成方法
KR102616437B1 (ko) 라이다 및 관성측정장치의 캘리브레이션 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램
CN117115215A (zh) 非刚性配准方法、装置及电子设备
JP7485749B2 (ja) ビデオベースの位置決め及びマッピングの方法及びシステム
CN113280789B (zh) 一种地表起伏区域激光测高点作为影像高程控制点方法
KR102618951B1 (ko) 시각적 매핑 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램
CN116993627B (zh) 一种激光扫描图像数据校正方法
KR102626574B1 (ko) 카메라 및 라이다의 캘리브레이션 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램

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