CN115015959A - 一种基于人工信标的无人车辆激光雷达建图与定位方法 - Google Patents

一种基于人工信标的无人车辆激光雷达建图与定位方法 Download PDF

Info

Publication number
CN115015959A
CN115015959A CN202210744154.6A CN202210744154A CN115015959A CN 115015959 A CN115015959 A CN 115015959A CN 202210744154 A CN202210744154 A CN 202210744154A CN 115015959 A CN115015959 A CN 115015959A
Authority
CN
China
Prior art keywords
beacon
point
points
vehicle
point cloud
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
CN202210744154.6A
Other languages
English (en)
Other versions
CN115015959B (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.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
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 Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202210744154.6A priority Critical patent/CN115015959B/zh
Publication of CN115015959A publication Critical patent/CN115015959A/zh
Application granted granted Critical
Publication of CN115015959B publication Critical patent/CN115015959B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • 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/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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/30241—Trajectory
    • 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)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computing Systems (AREA)
  • Software Systems (AREA)
  • Medical Informatics (AREA)
  • General Health & Medical Sciences (AREA)
  • Multimedia (AREA)
  • Evolutionary Computation (AREA)
  • Databases & Information Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明涉及一种基于人工信标的无人车辆激光雷达建图与定位方法，包括如下步骤：步骤一：制作和布置信标；步骤二：无人车辆通过激光雷达获取信标的位置和编号信息；步骤三：结合车载里程计信息和信标位置和编号信息构建位姿图；步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图。通过位姿图优化对车载里程计信息和信标观测结果进行融合定位，大幅修正里程计的定位误差，可实现无人车辆在隧道等卫星信号弱、环境特征少的场景中的精准定位。

Description

一种基于人工信标的无人车辆激光雷达建图与定位方法
技术领域
本发明涉及无人车辆定位领域，更具体地，涉及一种基于人工信标的无人车辆激光雷达建图与定位方法。
背景技术
目前无人驾驶车辆主要依靠卫星导航系统和环境特征进行定位，因此在隧道等卫星信号弱、环境特征少的场景下难以实现精准定位，存在较大的安全隐患。针对该问题，现有方案主要是在场景中加入具有高反射率的反光物(如反光柱、反光板等)作为人工信标，车辆通过激光雷达传感器实现对反光物的检测，进一步根据车辆相对反光物的位移实现车辆定位。
如现有的一种基于视觉标记的自动建图机器人建图修整方法，通过检测环境中的视觉标记，基于所述视觉标记的角点信息，计算此时自动建图机器人相对于视觉标记点的相对位置与航向，最后由视觉标记点的定位信息与自动建图机器人相对于所述视觉标记点的相对位置得到自动建图机器人的坐标与航向，对视觉标记点之间出现偏移、误差的建图数据进行修正与优化。
在上述的技术方案中，根据标记读取到的信息，在已采集到的地图上放置精确定位点，根据新添加的精确定位点对采集得到的地图信息进行修整与局部优化，最终得到所需的高精度建图结果。但是缺乏对车辆位姿和行驶状态的考虑，使得即使建图后的车辆定位的精准度还是有所欠缺。
发明内容
本发明为克服上述现有技术中现有的基于标记的无人车辆定位的精准度有所欠缺，提供一种基于人工信标的无人车辆激光雷达建图与定位方法，通过位姿图优化算法对车载里程计信息和信标观测结果进行融合定位，实现无人车辆的精准定位。
为解决上述技术问题，本发明采用的技术方案是：一种基于人工信标的无人车辆激光雷达建图与定位方法，包括如下步骤：
步骤一：制作和布置信标；
步骤二：无人车辆通过激光雷达获取信标的位置和编号信息；
步骤三：结合车载里程计信息和信标位置和编号信息构建位姿图；
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图。
优选的，信标为根据编码长度l、编码最小汉明距离h和编码复杂度c生成的二进制编码图案；编码长度l对应编码图案部分的方块个数，编码图案由d×d个黑白方块组成，所以l必须为完全平方数，每个方块代表一个码字，黑色方块代表0，白色方块代表1，根据编码按照从上到下、从左到右的顺序生成编码图案。信标具有编码图案，具备可区分性，可提升定位精度和鲁棒性。编码最小汉明距离h表示将一个编码转换为编码库中的另一个编码至少需要改变的码字个数，h越大，编码库中不同编码的区分度越大，越不容易出现解码错误；编码复杂度c表示生成编码图案所需的最少矩形个数，例如，生成一个全黑的编码图案只需一个黑色矩形，而生成一个“黑-白-黑”条纹的编码图案则需要两个矩形，即一个黑色矩形加上中间的白色矩形，编码复杂度c越大，则编码图案越复杂，越不容易与自然环境混淆，可以降低信标误检率。
优选的，在步骤二中，激光雷达获取信标的原始点云数据，通过信标感知算法对激光雷达获取的原始点云数据进行处理，实时获取环境中信标的位置和编号信息。
优选的，通过信标感知算法对激光雷达获取的原始点云数据进行处理的具体流程如下：
S2.1：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
Figure BDA0003719020150000021
其中，pi,m为第m条激光线束中的第i个点，pi-1,m和pi+1,m分别为pi,m在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值ζ，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵
Figure BDA0003719020150000031
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为(d+4)2个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第i个方块计算反射强度均值
Figure BDA0003719020150000032
将其与整个信标点云的反射强度均值
Figure BDA0003719020150000033
对比，若
Figure BDA0003719020150000034
则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于设定的阈值，则将该编码对应的编号作为编码信息。
优选的，在步骤S2.2中，类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数N应满足
Figure BDA0003719020150000035
其中，η＝4表示激光扫描到每个方块的最少点数，(d+4)2表示构成信标图案的方块个数，M为扫描到信标的激光线束，D为信标与激光雷达之间的距离，θ＝0.1°为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
Figure BDA0003719020150000036
其中，
Figure BDA0003719020150000037
为第m条激光线束中的第i个点的强度值，
Figure BDA0003719020150000038
和
Figure BDA0003719020150000039
分别为pi,m在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于设定的阈值τ，则认为该点为强度边界点，强度边界点个数Q应大于等于2(d+2)；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的4-5％。
优选的，在步骤S2.3中，得到位姿矩阵的具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为s、厚度为∈＝0.01m的投影目标，通过刚体变换矩阵
Figure BDA00037190201500000310
将信标点云投影到投影目标处，定义投影损失如下：
Figure BDA0003719020150000041
其中，
Figure BDA0003719020150000042
为信标点云，
Figure BDA0003719020150000043
和
Figure BDA0003719020150000044
分别为
Figure BDA0003719020150000045
中第i个点经过
Figure BDA0003719020150000046
投影后的坐标，c(·)为损失计算函数；
S2.3.2：定义损伤计算函数c(·)为:
Figure BDA0003719020150000047
其中，
Figure BDA0003719020150000048
λ≥0
S2.3.3:通过非线性优化库NLopt对优化问题
Figure BDA0003719020150000049
进行求解，得到最优的刚体变换矩阵
Figure BDA00037190201500000410
使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵
Figure BDA00037190201500000411
优选的，在步骤三中，里程计信息记录的是两个时间步之间的车辆位姿变化，记为ot；设定步骤二中车辆运行的步长为T，则车辆的运动轨迹可以表示为X＝{x0,…,xT}，其中xt∈SE(3)，表示车辆在第t个时间步的位姿，ot为里程计测得的xt与xt-1之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为L＝{l1,l2,…,lN}，其中，
Figure BDA00037190201500000412
表示编号为i的信标的三维坐标；车辆在第t个时间步观测到Kt个信标，信标相对于车辆的位置记为
Figure BDA00037190201500000413
其中
Figure BDA00037190201500000414
信标的编号记为
Figure BDA00037190201500000415
其中
Figure BDA00037190201500000416
根据里程计信息ot和信标相对于车辆的位置构建位姿图。
优选的，在步骤四中，对位姿图进行优化的流程为：
第t个时间步的里程计信息可以表示为
Figure BDA00037190201500000417
其中，
Figure BDA00037190201500000418
为相对位姿运算符，wt表示里程计测量噪声，设定该噪声服从均值为0、方差为Q的高斯分布，给定xt和xt-1，ot的似然估计为
Figure BDA00037190201500000419
第t个时间步观测到的第k个信标的位置可以表示为
Figure BDA00037190201500000420
其中，vt表示信标的位置测量误差，设定该误差服从均值为0、方差为R的高斯分布，给定车辆位姿xt和信标位置
Figure BDA00037190201500000421
的似然估计为
Figure BDA00037190201500000422
Figure BDA00037190201500000423
根据上述ot和
Figure BDA0003719020150000051
的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
Figure BDA0003719020150000052
由于噪声符合高斯分布，可以将上式中的log(f(ot；xt-1,xt))和
Figure BDA0003719020150000053
进一步化简为二次型：
Figure BDA0003719020150000054
Figure BDA0003719020150000055
对位姿图的优化等价于求解极大似然估计问题：
Figure BDA0003719020150000056
求解结果X*即为优化后的车辆定位轨迹，L*即为优化后的信标地图。
优选的，信标的编码图案的外层添加一层黑色边框和一层白色边框。
优选的，在布置信标的时候将信标顺时针旋转45°。以使更多的激光线束扫描到信标。增加信标的辨识度。
与现有技术相比，本发明的有益效果是：通过位姿图优化对车载里程计信息和信标观测结果进行融合定位，大幅修正里程计的定位误差，可实现无人车辆在隧道等卫星信号弱、环境特征少的场景中的精准定位。
附图说明
图1是本发明的一种基于人工信标的无人车辆激光雷达建图与定位方法的流程图；
图2是本发明的信标的编码图案的结构示意图；
图3是本发明的信标感知算法对激光雷达获取的原始点云数据进行处理的流程图；
图4是本发明的位姿图优化的示意图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。
下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：
实施例1
如图1所示为一种基于人工信标的无人车辆激光雷达建图与定位方法的实施例1，包括如下步骤：
步骤一：制作和布置信标；
步骤二：无人车辆通过激光雷达获取信标的位置和编号信息；
步骤三：结合车载里程计信息和信标位置和编号信息构建位姿图；
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图。
本实施例的有益效果：通过位姿图优化对车载里程计信息和信标观测结果进行融合定位，大幅修正里程计的定位误差，可实现无人车辆在隧道等卫星信号弱、环境特征少的场景中的精准定位。
实施例2
如图1所示，一种基于人工信标的无人车辆激光雷达建图与定位方法的实施例2，包括如下步骤：
步骤一：制作和布置信标；信标布置隧道中。信标为根据编码长度l、编码最小汉明距离h和编码复杂度c生成的二进制编码图案；编码长度l对应编码图案部分的方块个数，编码图案由d×d个黑白方块组成，所以l必须为完全平方数，每个方块代表一个码字，黑色方块代表0，白色方块代表1，根据编码按照从上到下、从左到右的顺序生成编码图案。信标具有编码图案，具备可区分性，可提升定位精度和鲁棒性。本实施例的信标与墙壁保持一定间距b＝0.2m，确保信标具有显著的边缘特征，方便后续检测。本实施例采用l＝16、h＝6、c＝5和s＝1.22m作为参数制作了一系列信标，图2展示的是编码值为“1010010101110100”的信标。
步骤二：激光雷达获取信标的原始点云数据，通过信标感知算法对激光雷达获取的原始点云数据进行处理，实时获取环境中信标的位置和编号信息；如图3所示，具体流程为：
S2.1：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
Figure BDA0003719020150000071
其中，pi,m为第m条激光线束中的第i个点，pi-1,m和pi+1,m分别为pi,m在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值ζ，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数N应满足
Figure BDA0003719020150000072
其中，η＝4表示激光扫描到每个方块的最少点数，(d+4)2表示构成信标图案的方块个数，M为扫描到信标的激光线束，D为信标与激光雷达之间的距离，θ＝0.1°为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
Figure BDA0003719020150000073
其中，
Figure BDA0003719020150000074
为第m条激光线束中的第i个点的强度值，
Figure BDA0003719020150000075
和
Figure BDA0003719020150000076
分别为pi,m在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于一定阈值τ＝10，则认为该点为强度边界点，强度边界点个数Q应大于等于2(d+2)；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的5％。
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵
Figure BDA0003719020150000081
具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为s、厚度为∈＝0.01m的投影目标，通过刚体变换矩阵
Figure BDA0003719020150000082
将信标点云投影到投影目标处，定义投影损失如下：
Figure BDA0003719020150000083
其中，
Figure BDA0003719020150000084
为信标点云，
Figure BDA0003719020150000085
和
Figure BDA0003719020150000086
分别为
Figure BDA0003719020150000087
中第i个点经过
Figure BDA0003719020150000088
投影后的坐标，c(·)为损失计算函数；
S2.3.2：定义损伤计算函数c(·)为:
Figure BDA00037190201500000817
其中，
Figure BDA0003719020150000089
λ≥0
S2.3.3:通过非线性优化库NLopt对优化问题
Figure BDA00037190201500000810
进行求解，得到最优的刚体变换矩阵
Figure BDA00037190201500000811
使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵
Figure BDA00037190201500000812
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为(d+4)2个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第i个方块计算反射强度均值
Figure BDA00037190201500000813
将其与整个信标点云的反射强度均值
Figure BDA00037190201500000814
对比，若
Figure BDA00037190201500000815
则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于2，则将该编码对应的编号作为编码信息。
步骤三：里程计信息记录的是两个时间步之间的车辆位姿变化，记为ot；设定步骤二中车辆运行的步长为T，则车辆的运动轨迹可以表示为X＝{x0,…,xT}，其中xt∈SE(3)，表示车辆在第t个时间步的位姿，ot为里程计测得的xt与xt-1之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为L＝{l1,l2,…,lN}，其中，
Figure BDA00037190201500000816
表示编号为i的信标的三维坐标；车辆在第t个时间步观测到Kt个信标，信标相对于车辆的位置记为
Figure BDA0003719020150000091
其中
Figure BDA0003719020150000092
信标的编号记为
Figure BDA0003719020150000093
其中
Figure BDA0003719020150000094
根据里程计信息ot和信标相对于车辆的位置构建位姿图，如图4所示。
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图，具体流程为：第t个时间步的里程计信息可以表示为
Figure BDA0003719020150000095
其中，
Figure BDA0003719020150000096
为相对位姿运算符，wt表示里程计测量噪声，设定该噪声服从均值为0、方差为Q的高斯分布，给定xt和xt-1，ot的似然估计为
Figure BDA0003719020150000097
第t个时间步观测到的第k个信标的位置可以表示为
Figure BDA0003719020150000098
其中，vt表示信标的位置测量误差，设定该误差服从均值为0、方差为R的高斯分布，给定车辆位姿xt和信标位置
Figure BDA0003719020150000099
的似然估计为
Figure BDA00037190201500000910
Figure BDA00037190201500000911
根据上述ot和
Figure BDA00037190201500000912
的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
Figure BDA00037190201500000913
由于噪声符合高斯分布，可以将上式中的log(f(ot；xt-1,xt))和
Figure BDA00037190201500000914
进一步化简为二次型：
Figure BDA00037190201500000915
Figure BDA00037190201500000916
对位姿图的优化等价于求解极大似然估计问题：
Figure BDA00037190201500000917
求解结果X*即为优化后的车辆定位轨迹，L*即为优化后的信标地图。
实施例3
一种基于人工信标的无人车辆激光雷达建图与定位方法的实施例3，在实施例1和实施例2的基础上，对信标进一步限定。信标的编码图案的外层添加一层黑色边框和一层白色边框。在布置信标的时候将信标顺时针旋转45°
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，包括如下步骤：
步骤一：制作和布置信标；
步骤二：无人车辆通过激光雷达获取信标的位置和编号信息；
步骤三：结合车载里程计信息和信标位置和编号信息构建位姿图；
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图。
2.根据权利要求1所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，信标为根据编码长度l、编码最小汉明距离h和编码复杂度c生成的二进制编码图案；编码长度l对应编码图案部分的方块个数，编码图案由d×d个黑白方块组成，所以l必须为完全平方数，每个方块代表一个码字，黑色方块代表0，白色方块代表1，根据编码按照从上到下、从左到右的顺序生成编码图案。
3.根据权利要求2所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤二中，激光雷达获取信标的原始点云数据，通过信标感知算法对激光雷达获取的原始点云数据进行处理，实时获取环境中信标的位置和编号信息。
4.根据权利要求3所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，通过信标感知算法对激光雷达获取的原始点云数据进行处理的具体流程如下：
S2.1：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
Figure FDA0003719020140000011
其中，pi,m为第m条激光线束中的第i个点，pi-1,m和pi+1,m分别为pi,m在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值ζ，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵
Figure FDA0003719020140000021
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为(d+4)2个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第i个方块计算反射强度均值
Figure FDA0003719020140000022
将其与整个信标点云的反射强度均值
Figure FDA0003719020140000023
对比，若
Figure FDA0003719020140000024
则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于设定的阈值，则将该编码对应的编号作为编码信息。
5.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤S2.2中，类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数N应满足
Figure FDA0003719020140000025
其中，η＝4表示激光扫描到每个方块的最少点数，(d+4)2表示构成信标图案的方块个数，M为扫描到信标的激光线束，D为信标与激光雷达之间的距离，θ＝0.1°为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
Figure FDA0003719020140000026
其中，
Figure FDA0003719020140000027
为第m条激光线束中的第i个点的强度值，
Figure FDA0003719020140000028
和
Figure FDA0003719020140000029
分别为pi,m在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于设定的阈值τ，则认为该点为强度边界点，强度边界点个数Q应大于等于2(d+2)；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的4％-6％。
6.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤S2.3中，得到位姿矩阵的具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为s、厚度为∈＝0.01m的投影目标，通过刚体变换矩阵
Figure FDA0003719020140000031
将信标点云投影到投影目标处，定义投影损失如下：
Figure FDA0003719020140000032
其中，
Figure FDA0003719020140000033
为信标点云，
Figure FDA0003719020140000034
和
Figure FDA0003719020140000035
分别为
Figure FDA0003719020140000036
中第i个点经过
Figure FDA0003719020140000037
投影后的坐标，c(·)为损失计算函数；
S2.3.2：定义损伤计算函数c(·)为:
Figure FDA0003719020140000038
其中，
Figure FDA0003719020140000039
λ≥0
S2.3.3:通过非线性优化库NLopt对优化问题
Figure FDA00037190201400000310
进行求解，得到最优的刚体变换矩阵
Figure FDA00037190201400000311
使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵
Figure FDA00037190201400000312
7.根据权利要求4所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤三中，里程计信息记录的是两个时间步之间的车辆位姿变化，记为ot；设定步骤二中车辆运行的步长为T，则车辆的运动轨迹可以表示为X＝{x0,…,xT}，其中xt∈SE(3)，表示车辆在第t个时间步的位姿，ot为里程计测得的xt与xt-1之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为L＝{l1,l2,…,lN}，其中，
Figure FDA00037190201400000313
表示编号为i的信标的三维坐标；车辆在第t个时间步观测到Kt个信标，信标相对于车辆的位置记为
Figure FDA00037190201400000314
其中
Figure FDA00037190201400000315
信标的编号记为
Figure FDA00037190201400000316
其中
Figure FDA00037190201400000317
根据里程计信息ot和信标相对于车辆的位置构建位姿图。
8.根据权利要求7所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤四中，对位姿图进行优化的流程为：
第t个时间步的里程计信息可以表示为
Figure FDA00037190201400000318
其中，
Figure FDA00037190201400000319
为相对位姿运算符，wt表示里程计测量噪声，设定该噪声服从均值为0、方差为Q的高斯分布，给定xt和xt-1，ot的似然估计为
Figure FDA0003719020140000041
第t个时间步观测到的第k个信标的位置可以表示为
Figure FDA0003719020140000042
其中，vt表示信标的位置测量误差，设定该误差服从均值为0、方差为R的高斯分布，给定车辆位姿xt和信标位置
Figure FDA0003719020140000043
Figure FDA0003719020140000044
的似然估计为
Figure FDA0003719020140000045
Figure FDA0003719020140000046
根据上述ot和
Figure FDA0003719020140000047
的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
Figure FDA0003719020140000048
由于噪声符合高斯分布，可以将上式中的log(f(ot；xt-1,xt))和
Figure FDA0003719020140000049
进一步化简为二次型：
Figure FDA00037190201400000410
对位姿图的优化等价于求解极大似然估计问题：
Figure FDA00037190201400000411
求解结果X*即为优化后的车辆定位轨迹，L*即为优化后的信标地图。
9.根据权利要求2-8任一所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，信标的编码图案的外层添加一层黑色边框和一层白色边框。
10.根据权利要求2-8任一所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在布置信标的时候将信标顺时针旋转45°。
CN202210744154.6A 2022-06-28 2022-06-28 一种基于人工信标的无人车辆激光雷达建图与定位方法 Active CN115015959B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210744154.6A CN115015959B (zh) 2022-06-28 2022-06-28 一种基于人工信标的无人车辆激光雷达建图与定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210744154.6A CN115015959B (zh) 2022-06-28 2022-06-28 一种基于人工信标的无人车辆激光雷达建图与定位方法

Publications (2)

Publication Number Publication Date
CN115015959A true CN115015959A (zh) 2022-09-06
CN115015959B CN115015959B (zh) 2024-08-02

Family

ID=83077204

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210744154.6A Active CN115015959B (zh) 2022-06-28 2022-06-28 一种基于人工信标的无人车辆激光雷达建图与定位方法

Country Status (1)

Country Link
CN (1) CN115015959B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110595459A (zh) * 2019-09-18 2019-12-20 百度在线网络技术（北京）有限公司 车辆定位方法、装置、设备和介质
CN113128569A (zh) * 2021-03-26 2021-07-16 浙江理工大学 基于词袋模型及度量学习的车辆轨迹聚类方法
CN113191459A (zh) * 2021-05-27 2021-07-30 山东高速建设管理集团有限公司 一种基于路侧激光雷达的在途目标分类方法
CN113674399A (zh) * 2021-08-16 2021-11-19 杭州图灵视频科技有限公司 一种移动机器人室内三维点云地图构建方法及系统
WO2022041706A1 (zh) * 2020-08-28 2022-03-03 华为技术有限公司 一种定位方法、定位系统和车辆

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110595459A (zh) * 2019-09-18 2019-12-20 百度在线网络技术（北京）有限公司 车辆定位方法、装置、设备和介质
WO2022041706A1 (zh) * 2020-08-28 2022-03-03 华为技术有限公司 一种定位方法、定位系统和车辆
CN113128569A (zh) * 2021-03-26 2021-07-16 浙江理工大学 基于词袋模型及度量学习的车辆轨迹聚类方法
CN113191459A (zh) * 2021-05-27 2021-07-30 山东高速建设管理集团有限公司 一种基于路侧激光雷达的在途目标分类方法
CN113674399A (zh) * 2021-08-16 2021-11-19 杭州图灵视频科技有限公司 一种移动机器人室内三维点云地图构建方法及系统

Also Published As

Publication number Publication date
CN115015959B (zh) 2024-08-02

Similar Documents

Publication Publication Date Title
Wang et al. Intensity scan context: Coding intensity and geometry relations for loop closure detection
CN111882612B (zh) 一种基于三维激光检测车道线的车辆多尺度定位方法
CN111583369B (zh) 一种基于面线角点特征提取的激光slam方法
CN112767490B (zh) 一种基于激光雷达的室外三维同步定位与建图方法
Lenac et al. Fast planar surface 3D SLAM using LIDAR
US8180100B2 (en) Plane detector and detecting method
Steinhauser et al. Motion segmentation and scene classification from 3D LIDAR data
US12033400B2 (en) Overhead-view image generation device, overhead-view image generation system, and automatic parking device
Habib et al. A new approach for matching surfaces from laser scanners and optical scanners
JP2001331787A (ja) 道路形状推定装置
Fujii et al. Urban object reconstruction using airborne laser elevation image and aerial image
CN112862858A (zh) 一种基于场景运动信息的多目标跟踪方法
CN113743171A (zh) 目标检测方法及装置
CN112455502B (zh) 基于激光雷达的列车定位方法及装置
CN114283394A (zh) 一种车载传感器融合的交通目标检测系统
Wang et al. Autonomous landing of multi-rotors UAV with monocular gimbaled camera on moving vehicle
CN111612818A (zh) 新型双目视觉多目标跟踪方法及系统
García et al. Fusion procedure for pedestrian detection based on laser scanner and computer vision
Del Pizzo et al. Reliable vessel attitude estimation by wide angle camera
US20210304518A1 (en) Method and system for generating an environment model for positioning
CN115015959A (zh) 一种基于人工信标的无人车辆激光雷达建图与定位方法
CN117058231A (zh) 一种基于视觉深度信息的分体式飞行汽车定位对接方法
Yanase et al. LIDAR Based Altitude Estimation for Autonomous Vehicles using Elevation Maps
Pyo et al. Development of radial layout underwater acoustic marker using forward scan sonar for AUV
Odaka et al. Localization in tunnels using feature-based scan matching

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