CN113554705A - 一种变化场景下的激光雷达鲁棒定位方法 - Google Patents

一种变化场景下的激光雷达鲁棒定位方法 Download PDF

Info

Publication number
CN113554705A
CN113554705A CN202110795249.6A CN202110795249A CN113554705A CN 113554705 A CN113554705 A CN 113554705A CN 202110795249 A CN202110795249 A CN 202110795249A CN 113554705 A CN113554705 A CN 113554705A
Authority
CN
China
Prior art keywords
point
laser radar
point cloud
points
plane
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
CN202110795249.6A
Other languages
English (en)
Other versions
CN113554705B (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202110795249.6A priority Critical patent/CN113554705B/zh
Publication of CN113554705A publication Critical patent/CN113554705A/zh
Application granted granted Critical
Publication of CN113554705B publication Critical patent/CN113554705B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/005—Tree description, e.g. octree, quadtree
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Geometry (AREA)
  • Theoretical Computer Science (AREA)
  • Software Systems (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种变化场景下的激光雷达鲁棒定位方法，具体为：步骤1：构建出预设路径周围环境的点云地图；步骤2：周期性地采集激光雷达点云数据，提取点云中的角特征点和平面特征点；步骤3：利用激光雷达里程计预测激光雷达当前位姿，将特征点投影至导航系下；步骤4：利用先验地图进行变化检测，剔除特征点中的新增点；步骤5：利用剔除后的特征点与点云地图进行匹配，得到激光在导航系下的位姿。本发明能够在变化场景中，实现基于激光雷达的高精度定位。

Description

一种变化场景下的激光雷达鲁棒定位方法
技术领域
本发明属于自主导航与制导技术领域。
背景技术
近年来，无人车的应用越来越广泛，涉及物流等各个领用。而高精度的定位是保障无人车顺利完成任务的首要前提。传统的定位方法采用GPS/惯性/里程计融合方案，当GPS受到使用环境限制，无法应用于室内等场景下。而IMU/里程计的组合导航方式虽自主性较强，当长时间下定位结果会由于误差累计而发散。激光雷达和视觉传感器的出现弥补了传统导航方案的不足，通过SLAM技术和地图匹配技术可以有效地解决室内环境的定位问题。视觉传感器体积小，价格低廉，但其工作受外界光照的影响，无法应用于黑暗环境下。激光雷达通过主动发射激光感知周围环境，受外界环境干扰小，可以全天候工作，是目前最常用的导航传感器之一。
无人车的工作通常位于固定区域，通过事先构建周围场景的地图，可以利用激光雷达实现基于地图的高精度定位。但基于地图的定位方法容易受到周围场景变化的影响，随之而来的是定位精度的下降。
发明内容
发明目的：为了解决上述现有技术存在的问题，本发明提供了一种变化场景下的激光雷达鲁棒定位方法。
技术方案：本发明提供了一种变化场景下的激光雷达鲁棒定位方法，具体包括如下步骤：
步骤1：构建预设路径周围环境的点云地图PM；
步骤2：安装有激光雷达的无人车按照预设路径运动，在第k时刻时激光雷达采集的点云数据为P(k)，在P(k)中找到属于激光雷达第n条线束的采样点，并将该些采样点构成第n个采样点集合Sn(k)，n＝1，2，…N；N为激光雷达的线束的总个数，计算Sn(k)中每个采样点的粗糙度；根据每个采样点的粗糙度，在Sn(k)中提取角特征点构成角特征点集合
Figure BDA0003162467090000021
在Sn(k)中提取平面特征点构成平面特征点集合
Figure BDA0003162467090000022
将在N个采样点集合中提取的角特征点集合组成第k时刻的角特征点集合
Figure BDA0003162467090000023
将在N个采样点集合中提取的平面特征点集合组成第k时刻的平面特征点集合
Figure BDA0003162467090000024
将
Figure BDA0003162467090000025
和
Figure BDA0003162467090000026
组成第k时刻的特征点集
Figure BDA0003162467090000027
步骤3：将
Figure BDA0003162467090000028
作为原始点云，第k-1时刻的特征点集
Figure BDA0003162467090000029
作为目标点云，计算原始点云到目标点云的位姿变换矩阵
Figure BDA00031624670900000210
根据
Figure BDA00031624670900000211
计算激光雷达位姿的估计值
Figure BDA00031624670900000212
步骤4：基于
Figure BDA00031624670900000213
将
Figure BDA00031624670900000214
中所有特征点在雷达坐标系下的坐标转换到无人车导航坐标系下的坐标，得到无人车导航坐标系下的特征点集合
Figure BDA00031624670900000215
步骤5：利用先验地图进行变化检测，从而剔除
Figure BDA00031624670900000216
中新增的特征点，得到集合
Figure BDA00031624670900000217
步骤6：将
Figure BDA00031624670900000218
作为原始点云，点云地图PM作为目标点云，计算原始点云到目标点云的位姿变换矩阵
Figure BDA00031624670900000219
将
Figure BDA00031624670900000220
作为第k时刻时激光雷达的位姿。
进一步的，所述步骤2在P(k)中找到属于激光雷达第n条线束的采样点具体为：计算P(k)中第i个采样点pi与水平方向的夹角αi：
Figure BDA00031624670900000221
其中，pi(x)、pi(y)、pi(z)分别表示pi在激光雷达坐标系下x、y、z轴的坐标值，i＝1，2，...，R，R表示P(k)中采样点的总个数；
令
Figure BDA00031624670900000222
θ为激光雷达的垂直扫描角度的最大值；若2ηi≤2mi+1，则采样点pi属于激光雷达第mi条线束，否则pi属于激光雷达第mi+1条线束。
进一步的，所述步骤2中采用如下公式计算Sn(k)中每个采样点的粗糙度：
Figure BDA00031624670900000223
其中，ci’为Sn(k)中第i’个采样点pi’的粗糙度，i’＝1，2，...，I’，I’为Sn(k)中所有采样点的个数；P为取集合Sn(k)中位于pi’左侧的d个采样点，取集合Sn(k)中位于pi’右侧的d个采样点以及采样点pi’构成的集合；
Figure BDA0003162467090000031
表示第k时刻时采样点pi’在激光雷达坐标系下的坐标，
Figure BDA0003162467090000032
表示第k时刻时采样点pj’在激光雷达坐标系下的坐标；|P|为对P求模。
进一步的，所述步骤2中根据如下规则在Sn(k)中提取角特征点构成角特征点集合
Figure BDA0003162467090000033
在Sn(k)中提取平面特征点构成平面特征点集合
Figure BDA0003162467090000034
如果ci’大于预设的角特征阈值c1，则采样点pi’为角特征点，如果ci’小于预设的平面特征阈值c2，则采样点pi’为平面特征点，将Sn(k)中所有的角特征点按照粗糙度由大到小排列，选取前V个角特征点构成角特征点集合
Figure BDA0003162467090000035
将Sn(k)中所有的平面特征点按照粗糙度由小到大排列，选取前V个平面特征点构成平面特征点集合
Figure BDA0003162467090000036
进一步的，所述步骤3或者步骤6中计算原始点云到目标点云的位姿变换矩阵的具体方法为：
令原始点云为Ps＝{Ps e，Ps p}，其中Ps e、Ps p分别为原始点云中角特征点的集合和平面特征点的集合；
令目标点云为Pt＝{Pt e，Pt p}，其中Pt e、Pt p分别为目标点云中角特征点的集合和平面特征点的集合；
针对Ps e中第r个角特征点pr，r＝1，2，..r’，r’为Ps e中角特征点的总个数，在Pt e中取与pr距离最近的角特征点pj’，在与pj’所属雷达线束相邻的线束上取与pr距离最近的角特征点pl，且pl∈Pt e；点pi’与pl构成直线，计算角特征点pr到该直线的距离
Figure BDA00031624670900000315
Figure BDA00031624670900000316
其中
Figure BDA00031624670900000317
分别表示角特征点pr、pj’、pl在激光雷达坐标系下的坐标；
针对Ps p中第q个平面特征点pq，q＝1，2，...Q，Q为Ps p中平面特征点的总个数，在Pt p中取与pq距离最近的平面特征点Pj1，在Pj1所属的激光雷达线束中选取除Pj1以外的与pq距离最近的平面特征点Pj2，在与Pj1所属的激光雷达线束相邻的线束上选取与pq距离最近的平面特征点pm，且pj2∈Pt p，pm∈Pt p，平面特征点Pj1，Pj2和pm构成一个平面，计算pq到该平面的距离：
Figure BDA0003162467090000042
其中
Figure BDA0003162467090000043
分别表示平面特征点pq、Pj1、Pj2、pm在激光雷达坐标系下的坐标；
构建最小二乘方程：
Figure BDA0003162467090000044
其中，T表示原始点云到目标点云的位姿变换矩阵：
Figure BDA0003162467090000045
其中，R3×3表示原始点云到目标点云的旋转矩阵，t3×1表示原始点云到目标点云的平移矩阵；求解最小二乘方程，得到矩阵T。
进一步的，所述步骤3中根据
Figure BDA00031624670900000413
计算激光雷达达位姿的估计值
Figure BDA0003162467090000046
具体为：
Figure BDA0003162467090000047
Figure BDA0003162467090000048
为第k-1时刻时激光雷达的位姿
Figure BDA0003162467090000049
进一步的，所述步骤5具体为：
步骤5.1：将点云地图投影至八叉树中，得到八叉树地图OM；
步骤5.2：将特征点集
Figure BDA00031624670900000410
投影至八叉树地图OM中，得到更新后的八叉树地图O′M，将OM和O′M中的体素进行对比，得到新增的体素集合I；将集合I中的体素作为新增的特征点；
步骤5.3：剔除特征点集
Figure BDA00031624670900000411
中位于集合I中的特征点，从而得到集合
Figure BDA00031624670900000412
有益效果：本发明的一种变化场景下的激光雷达鲁棒定位方法，采用基于先验点云地图的方式进行定位，首先构建出运行场景的点云地图，通过提取地图中的特征点和当前时刻点云中的特征点进行匹配，从而解算出载体当前时刻的位姿。为提高点云匹配的精度，在点云配准前，本方法采用八叉树结构对场景变化进行检测，检测出场景中新增的点云并将其进行剔除，减小了环境变化对点云匹配的干扰，从而提高了变化场景下激光雷达定位算法的鲁棒性。本发明方法适用于基于激光雷达的载体自主定位，能实现变化场景下载体的鲁棒定位。
附图说明
图1为本发明的流程图。
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
本发明实施例提供了一种变化场景下的激光雷达鲁棒定位方法，流程图如图1所示，包括步骤如下：
步骤1，构建预设路径周围环境的点云地图PM；
步骤11，携带激光雷达沿预设路径运动，激光雷达采集点云数据，要求激光雷达不受遮挡；
步骤12，对步骤11中的激光雷达点云数据提取角特征点和平面特征点；
步骤13，采集点云数据过程中，通过安装在携带有激光雷达设备的外部基准传感器(如全站仪/RTK)或SLAM技术，得到采集过程中激光雷达的位置、姿态信息；
步骤14，基于步骤13中激光雷达的位置、姿态信息，以及步骤12中采集的激光雷达点云中提取的特征点，构建周围场景的激光雷达点云地图。
步骤2，周期采集第k时刻时激光雷达点云数据P(k)，提取点云数据中的角特征点Fi e和平面特征点Fi p得到特征点集
Figure BDA0003162467090000053
包括如下步骤：
步骤21，根据激光雷达线数和垂直扫描范围对点云数据进行划分；
假设激光雷达线数为N，垂直扫描范围为[-θ，θ]，θ为激光雷达的垂直扫描角度的最大值，pi是P(k)中的第i个采样点pi，i＝1，2，...，R，R表示P(k)中采样点的总个数，计算pi和水平方向的夹角αi：
Figure BDA0003162467090000061
其中，pi(x)、pi(y)、pi(z)分别表示pi在激光雷达坐标下x、y、z轴的坐标；通过α判断pi属于激光雷达哪一线：
Figure BDA0003162467090000062
若2ηi≤2mi+1，则采样点pi属于激光雷达第mi条线束，否则pi属于激光雷达第mi+1条线束。
步骤22，计算点云数据中每个点的曲率；
将属于激光雷达第n条线束的采样点构成集合Sn(k)，设pi’是Sn(k)中第i’个采样点，i’＝1，2，...，I’，I’为Sn(k)中所有采样点的个数，令PP为取集合Sn(k)中位于pi’左侧的d个采样点，取集合Sn(k)中位于pi’右侧的d个采样点以及采样点pi’构成的集合，且pi位于P的中间。计算pi的粗糙度ci’：
Figure BDA0003162467090000063
式中，
Figure BDA0003162467090000064
表示第k时刻时采样点pi’在激光雷达坐标系下的坐标，
Figure BDA0003162467090000065
表示第k时刻时采样点pj’在激光雷达坐标系下的坐标；|P|为对P求模。
步骤23，根据点云的粗糙度筛选特征点。
使用角特征阈值c1和平面特征阈值c2来区分不同类型的特征，粗糙度大于c1的采样点为角特征点，小于c2的采样点为平面特征点。将Sn(k)所有的角特征点按照粗糙度由大到小排列，选取前V个角特征点构成角特征点集合
Figure BDA0003162467090000066
将Sn(k)所有的平面特征点按照粗糙度由小到大排列，选取前V个平面特征点构成平面特征点集合
Figure BDA0003162467090000067
步骤24，针对每一线的点云集合，重复步骤22、步骤23，从而得到第k时刻时的特征点集
Figure BDA0003162467090000071
Figure BDA0003162467090000072
分别表示第k时刻时提取得到的角特征点和平面特征点。
步骤3，利用激光雷达里程计预测第k时刻载体的导航信息得到预测值
Figure BDA0003162467090000073
将特征点集
Figure BDA0003162467090000074
投影至导航系得到导航系下的特征点集
Figure BDA0003162467090000075
步骤31，利用第k时刻提取的到的特征点集
Figure BDA0003162467090000076
作为原始点云，第k-1时刻提取得到的特征点集
Figure BDA0003162467090000077
作为目标点云，进行点云匹配：
1)假设原始点云为Ps＝{Ps e，Ps p}，其中Ps e、Ps p分别为原始点云中的角特征点和平面特征点；目标点云为Pt＝{Pt e，Pt p}，其中Pt e、Pt p分别为目标点云中的角特征点和平面特征点；
2)针对Ps e中第r个角特征点pr，r＝1，2，...r’，r’为Ps e中角特征点的总个数，在Pt e中取与pr距离最近的角特征点pj’，在与pj’所属雷达线束相邻的线束上取与pr距离最近的角特征点pl，且pl∈Pt e；计算角特征点pr到pj’与pl构成的直线的距离
Figure BDA00031624670900000716
Figure BDA00031624670900000717
其中
Figure BDA00031624670900000718
分别表示角特征点pr、pj’、pl在激光雷达坐标系下的坐标；
3)对于ps p中第q个平面特征点pq，q＝1，2，...Q，Q为Ps p中平面特征点的总个数，假设存在平面特征点pj1、pl1、pm∈Pt p，其中Pj1表示在Pt p中离pi最近的点，在Pj1所属的激光雷达线束中选取除Pj1以外的与pq距离最近的平面特征点Pj2，在与Pj1所属的激光雷达线束相邻的线束上选取与pq距离最近的平面特征点pm，平面特征点Pj1，Pj2和pm构成一个平面，计算pq到该平面的距离：
Figure BDA00031624670900000723
其中
Figure BDA0003162467090000081
分别表示平面特征点pq、Pj1、Pj2、pm在激光雷达坐标系下的坐标；
4)假设
Figure BDA0003162467090000082
表示原始点云到目标点云的位姿变换：
Figure BDA0003162467090000083
其中，R3×3表示原始点云到目标点云的旋转矩阵，t3×1表示原始点云到目标点云的平移矩阵，构建最小二乘问题如下：
Figure BDA0003162467090000084
使用高斯-牛顿法对该最小二乘问题进行迭代求解，不断变换
Figure BDA0003162467090000085
使目标函数最小，以达到最小值或者迭代次数30次，输出变化矩阵
Figure BDA0003162467090000086
Figure BDA0003162467090000087
即为k-1时刻到k时刻激光雷达的位姿变换
Figure BDA0003162467090000088
结合k-1时刻激光雷的位姿
Figure BDA0003162467090000089
计算得到第k时刻激光雷达位姿的预测值
Figure BDA00031624670900000810
Figure BDA00031624670900000811
步骤32，利用k时刻激光雷达位姿的预测值
Figure BDA00031624670900000812
将激光雷达坐标系下的特征点集
Figure BDA00031624670900000813
变换至导航坐标系下得到
Figure BDA00031624670900000814
假设点
Figure BDA00031624670900000815
Figure BDA00031624670900000816
则变换关系如下：
Figure BDA00031624670900000817
式中，
Figure BDA00031624670900000824
表示k时刻
Figure BDA00031624670900000818
在导航坐标系下坐标，
Figure BDA00031624670900000819
示k时刻
Figure BDA00031624670900000820
在激光雷达坐标系下坐标；
步骤4，利用先验地图进行变化检测，剔除特征点集
Figure BDA00031624670900000821
中新增的点云，得到剔除后的点云
Figure BDA00031624670900000822
步骤41，将步骤1中的地图点云PM投影至八叉树中，得到八叉树地图OM；
步骤42，特征点集
Figure BDA00031624670900000823
投影至八叉树地图OM中，得到变化的八叉树O′M，将OM和O′M中的体素进行对比，得到新增的体素集合{Ii”}i”＝1，2，...，z，z为新增的体素集合中体素的总个数；位于体素集合中的点即为新增加的点，得到新增加的点集
Figure BDA0003162467090000091
步骤43，从特征点集
Figure BDA0003162467090000092
中剔除点集
Figure BDA0003162467090000093
得到过滤后的特征点集
Figure BDA0003162467090000094
步骤5，将特征点集
Figure BDA0003162467090000095
作为原始点云，点云地图PM作为目标点云，采用与步骤3相同的方式进行匹配得到原始点云到目标点云的位姿变换举证
Figure BDA0003162467090000096
将求解到的位姿变换矩阵
Figure BDA0003162467090000097
作为第k时刻时激光雷达的位姿。
另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

Claims (7)

1.一种变化场景下的激光雷达鲁棒定位方法，其特征在于，具体包括如下步骤：
步骤1：构建预设路径周围环境的点云地图PM；
步骤2：安装有激光雷达的无人车按照预设路径运动，在第k时刻时激光雷达采集的点云数据为P(k)，在P(k)中找到属于激光雷达第n条线束的采样点，并将该些采样点构成第n个采样点集合Sn(k)，n＝1,2,…N；N为激光雷达的线束的总个数，计算Sn(k)中每个采样点的粗糙度；根据每个采样点的粗糙度，在Sn(k)中提取角特征点构成角特征点集合Fn e，在Sn(k)中提取平面特征点构成平面特征点集合Fn p，将在N个采样点集合中提取的角特征点集合组成第k时刻的角特征点集合
Figure FDA0003162467080000011
将在N个采样点集合中提取的平面特征点集合组成第k时刻的平面特征点集合
Figure FDA0003162467080000012
将
Figure FDA0003162467080000013
和
Figure FDA0003162467080000014
组成第k时刻的特征点集
Figure FDA0003162467080000015
步骤3：将
Figure FDA0003162467080000016
作为原始点云，第k-1时刻的特征点集
Figure FDA0003162467080000017
作为目标点云，计算原始点云到目标点云的位姿变换矩阵Ts k，根据Ts k计算激光雷达位姿的估计值
Figure FDA0003162467080000018
步骤4：基于
Figure FDA0003162467080000019
将
Figure FDA00031624670800000110
中所有特征点在雷达坐标系下的坐标转换到无人车导航坐标系下的坐标，得到无人车导航坐标系下的特征点集合
Figure FDA00031624670800000111
步骤5：利用先验地图进行变化检测，从而剔除
Figure FDA00031624670800000112
中新增的特征点，得到集合
Figure FDA00031624670800000113
步骤6：将
Figure FDA00031624670800000114
作为原始点云，点云地图PM作为目标点云，计算原始点云到目标点云的位姿变换矩阵
Figure FDA00031624670800000115
将
Figure FDA00031624670800000116
作为第k时刻时激光雷达的位姿。
2.根据权利要求1所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤2在P(k)中找到属于激光雷达第n条线束的采样点具体为：计算P(k)中第i个采样点pi与水平方向的夹角αi：
Figure FDA00031624670800000117
其中，pi(x)、pi(y)、pi(z)分别表示pi在激光雷达坐标系下x、y、z轴的坐标值，i＝1,2，…,R，R表示P(k)中采样点的总个数；
令
Figure FDA0003162467080000021
mi＝[ηi]，θ为激光雷达的垂直扫描角度的最大值，[ηi]表示取不超过ηi的最大整数；若2ηi≤2mi+1，则采样点pi属于激光雷达第mi条线束，否则pi属于激光雷达第mi+1条线束。
3.根据权利要求1所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤2中采用如下公式计算Sn(k)中每个采样点的粗糙度：
Figure FDA0003162467080000022
其中，ci’为Sn(k)中第i’个采样点pi，的粗糙度，i’＝1，2，…，I’,I’为Sn(k)中所有采样点的个数；P为取集合Sn(k)中位于pi’左侧的d个采样点，取集合Sn(k)中位于pi’右侧的d个采样点以及采样点pi’构成的集合；
Figure FDA0003162467080000023
表示第k时刻时采样点pi’在激光雷达坐标系下的坐标，
Figure FDA0003162467080000024
表示第k时刻时采样点pj’在激光雷达坐标系下的坐标；|P|为对P求模。
4.根据权利要求3所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤2中根据如下规则在Sn(k)中提取角特征点构成角特征点集合Fn e，在Sn(k)中提取平面特征点构成平面特征点集合Fn p：
如果ci’大于预设的角特征阈值c1，则采样点pi’为角特征点，如果ci’小于预设的平面特征阈值c2，则采样点pi’为平面特征点，将Sn(k)中所有的角特征点按照粗糙度由大到小排列，选取前V个角特征点构成角特征点集合Fn e；将Sn(k)中所有的平面特征点按照粗糙度由小到大排列，选取前V个平面特征点构成平面特征点集合Fn p。
5.根据权利要求1所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤3或者步骤6中计算原始点云到目标点云的位姿变换矩阵的具体方法为：
令原始点云为Ps＝{Ps e,Ps p}，其中Ps e、Ps p分别为原始点云中角特征点的集合和平面特征点的集合；
令目标点云为Pt＝{Pt e,Pt p}，其中Pt e、Pt p分别为目标点云中角特征点的集合和平面特征点的集合；
针对Ps e中第r个角特征点pr，r＝1，2，…r’,r’为Ps e中角特征点的总个数，在Pt e中取与pr距离最近的角特征点pj’，在与pj’所属雷达线束相邻的线束上取与pr距离最近的角特征点pl，且pl∈Pt e；点pj’与pl构成直线，计算角特征点pr到该直线的距离
Figure FDA0003162467080000031
Figure FDA0003162467080000032
其中
Figure FDA0003162467080000033
分别表示角特征点pr、pj’、pl在激光雷达坐标系下的坐标；
针对Ps p中第q个平面特征点pq，q＝1,2,…Q,Q为Ps p中平面特征点的总个数,在Pt p中取与pq距离最近的平面特征点Pj1，在Pj1所属的激光雷达线束中选取除Pj1以外的与pq距离最近的平面特征点Pj2，在与Pj1所属的激光雷达线束相邻的线束上选取与pq距离最近的平面特征点pm，且pj2∈Pt p，pm∈Pt p，平面特征点Pj1，Pj2和pm构成一个平面，计算pq到该平面的距离：
Figure FDA0003162467080000034
其中
Figure FDA0003162467080000035
分别表示平面特征点pq、Pj1、Pj2、pm在激光雷达坐标系下的坐标；
构建最小二乘方程：
Figure FDA0003162467080000036
其中，T表示原始点云到目标点云的位姿变换矩阵：
Figure FDA0003162467080000037
其中，R3×3表示原始点云到目标点云的旋转矩阵，t3×1表示原始点云到目标点云的平移矩阵；求解最小二乘方程，得到矩阵T。
6.根据权利要求1所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤3中根据Ts k计算激光雷达达位姿的估计值
Figure FDA0003162467080000041
具体为：
Figure FDA0003162467080000042
Figure FDA0003162467080000043
为第k-1时刻时激光雷达的位姿
Figure FDA0003162467080000044
7.根据权利要求1所述的一种变化场景下的激光雷达鲁棒定位方法，其特征在于，所述步骤5具体为：
步骤5.1：将点云地图投影至八叉树中，得到八叉树地图OM；
步骤5.2：将特征点集
Figure FDA0003162467080000045
投影至八叉树地图OM中，得到更新后的八叉树地图O′M，将OM和O′M中的体素进行对比，得到新增的体素集合I；将集合I中的体素作为新增的特征点；
步骤5.3：剔除特征点集
Figure FDA0003162467080000046
中位于集合I中的特征点，从而得到集合
Figure FDA0003162467080000047
CN202110795249.6A 2021-07-14 2021-07-14 一种变化场景下的激光雷达鲁棒定位方法 Active CN113554705B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110795249.6A CN113554705B (zh) 2021-07-14 2021-07-14 一种变化场景下的激光雷达鲁棒定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110795249.6A CN113554705B (zh) 2021-07-14 2021-07-14 一种变化场景下的激光雷达鲁棒定位方法

Publications (2)

Publication Number Publication Date
CN113554705A true CN113554705A (zh) 2021-10-26
CN113554705B CN113554705B (zh) 2024-03-19

Family

ID=78131762

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110795249.6A Active CN113554705B (zh) 2021-07-14 2021-07-14 一种变化场景下的激光雷达鲁棒定位方法

Country Status (1)

Country Link
CN (1) CN113554705B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563795A (zh) * 2022-02-25 2022-05-31 湖南大学无锡智能控制研究院 基于激光里程计和标签融合算法的定位追踪方法及系统
CN115480235A (zh) * 2022-08-30 2022-12-16 中汽创智科技有限公司 一种路端激光雷达标定方法、装置及电子设备

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN112330661A (zh) * 2020-11-24 2021-02-05 交通运输部公路科学研究所 一种多期车载激光点云道路变化监测方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN112330661A (zh) * 2020-11-24 2021-02-05 交通运输部公路科学研究所 一种多期车载激光点云道路变化监测方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563795A (zh) * 2022-02-25 2022-05-31 湖南大学无锡智能控制研究院 基于激光里程计和标签融合算法的定位追踪方法及系统
CN114563795B (zh) * 2022-02-25 2023-01-17 湖南大学无锡智能控制研究院 基于激光里程计和标签融合算法的定位追踪方法及系统
CN115480235A (zh) * 2022-08-30 2022-12-16 中汽创智科技有限公司 一种路端激光雷达标定方法、装置及电子设备

Also Published As

Publication number Publication date
CN113554705B (zh) 2024-03-19

Similar Documents

Publication Publication Date Title
CN112014857B (zh) 用于智能巡检的三维激光雷达定位导航方法及巡检机器人
CN111337941B (zh) 一种基于稀疏激光雷达数据的动态障碍物追踪方法
CN108647646B (zh) 基于低线束雷达的低矮障碍物的优化检测方法及装置
CN109945858B (zh) 用于低速泊车驾驶场景的多传感融合定位方法
CN113865580B (zh) 构建地图的方法、装置、电子设备及计算机可读存储介质
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
CN113345008B (zh) 考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法
CN112965063B (zh) 一种机器人建图定位方法
CN113985429B (zh) 基于三维激光雷达的无人机环境扫描与重构方法
Jang et al. Road lane semantic segmentation for high definition map
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN114998276B (zh) 一种基于三维点云的机器人动态障碍物实时检测方法
CN114777775B (zh) 一种多传感器融合的定位方法及系统
CN115908539A (zh) 一种目标体积自动测量方法和装置、存储介质
CN114114367A (zh) Agv室外定位切换方法、计算机装置及程序产品
CN113971697B (zh) 一种空地协同车辆定位定向方法
CN113741523B (zh) 一种基于边界和采样的混合无人机自主探测方法
CN117075158A (zh) 基于激光雷达的无人变形运动平台的位姿估计方法及系统
CN117029870A (zh) 一种基于路面点云的激光里程计
CN114323038B (zh) 融合双目视觉和2d激光雷达的室外定位方法
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
CN114462545A (zh) 一种基于语义slam的地图构建方法及装置
CN112747752A (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Drulea et al. An omnidirectional stereo system for logistic plants. Part 2: stereo reconstruction and obstacle detection using digital elevation maps

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