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

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

Info

Publication number
CN115015959B
CN115015959B CN202210744154.6A CN202210744154A CN115015959B CN 115015959 B CN115015959 B CN 115015959B CN 202210744154 A CN202210744154 A CN 202210744154A CN 115015959 B CN115015959 B CN 115015959B
Authority
CN
China
Prior art keywords
beacon
points
vehicle
point
pose
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.)
Active
Application number
CN202210744154.6A
Other languages
English (en)
Other versions
CN115015959A (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

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：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
其中，pi,m为第m条激光线束中的第i个点，pi-1,m和pi+1,m分别为pi,m在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值ζ，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为(d+4)2个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第i个方块计算反射强度均值将其与整个信标点云的反射强度均值对比，若则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于设定的阈值，则将该编码对应的编号作为编码信息。
优选的，在步骤S2.2中，类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数N应满足其中，η＝4表示激光扫描到每个方块的最少点数，(d+4)2表示构成信标图案的方块个数，M为扫描到信标的激光线束，D为信标与激光雷达之间的距离，θ＝0.1°为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
其中，为第m条激光线束中的第i个点的强度值，和分别为pi,m在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于设定的阈值τ，则认为该点为强度边界点，强度边界点个数Q应大于等于2(d+2)；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的4-5％。
优选的，在步骤S2.3中，得到位姿矩阵的具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为s、厚度为∈＝0.01m的投影目标，通过刚体变换矩阵将信标点云投影到投影目标处，定义投影损失如下：
其中，为信标点云，和分别为中第i个点经过投影后的坐标，c(·)为损失计算函数；
S2.3.2：定义损伤计算函数c(·)为:
其中，λ≥0
S2.3.3:通过非线性优化库NLopt对优化问题进行求解，得到最优的刚体变换矩阵使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵
优选的，在步骤三中，里程计信息记录的是两个时间步之间的车辆位姿变化，记为ot；设定步骤二中车辆运行的步长为T，则车辆的运动轨迹可以表示为X＝{x0,…,xT}，其中xt∈SE(3)，表示车辆在第t个时间步的位姿，ot为里程计测得的xt与xt-1之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为L＝{l1,l2,…,lN}，其中，表示编号为i的信标的三维坐标；车辆在第t个时间步观测到Kt个信标，信标相对于车辆的位置记为其中信标的编号记为其中
根据里程计信息ot和信标相对于车辆的位置构建位姿图。
优选的，在步骤四中，对位姿图进行优化的流程为：
第t个时间步的里程计信息可以表示为其中，为相对位姿运算符，wt表示里程计测量噪声，设定该噪声服从均值为0、方差为Q的高斯分布，给定xt和xt-1，ot的似然估计为
第t个时间步观测到的第k个信标的位置可以表示为其中，vt表示信标的位置测量误差，设定该误差服从均值为0、方差为R的高斯分布，给定车辆位姿xt和信标位置的似然估计为
根据上述ot和的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
由于噪声符合高斯分布，可以将上式中的log(f(ot；xt-1,xt))和进一步化简为二次型：
对位姿图的优化等价于求解极大似然估计问题：
求解结果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：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
其中，pi,m为第m条激光线束中的第i个点，pi-1,m和pi+1,m分别为pi,m在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值ζ，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数N应满足其中，η＝4表示激光扫描到每个方块的最少点数，(d+4)2表示构成信标图案的方块个数，M为扫描到信标的激光线束，D为信标与激光雷达之间的距离，θ＝0.1°为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
其中，为第m条激光线束中的第i个点的强度值，和分别为pi,m在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于一定阈值τ＝10，则认为该点为强度边界点，强度边界点个数Q应大于等于2(d+2)；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的5％。
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为s、厚度为∈＝0.01m的投影目标，通过刚体变换矩阵将信标点云投影到投影目标处，定义投影损失如下：
其中，为信标点云，和分别为中第i个点经过投影后的坐标，c(·)为损失计算函数；
S2.3.2：定义损伤计算函数c(·)为:
其中，λ≥0
S2.3.3:通过非线性优化库NLopt对优化问题进行求解，得到最优的刚体变换矩阵使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为(d+4)2个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第i个方块计算反射强度均值将其与整个信标点云的反射强度均值对比，若则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于2，则将该编码对应的编号作为编码信息。
步骤三：里程计信息记录的是两个时间步之间的车辆位姿变化，记为ot；设定步骤二中车辆运行的步长为T，则车辆的运动轨迹可以表示为X＝{x0,…,xT}，其中xt∈SE(3)，表示车辆在第t个时间步的位姿，ot为里程计测得的xt与xt-1之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为L＝{l1,l2,…,lN}，其中，表示编号为i的信标的三维坐标；车辆在第t个时间步观测到Kt个信标，信标相对于车辆的位置记为其中信标的编号记为其中
根据里程计信息ot和信标相对于车辆的位置构建位姿图，如图4所示。
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图，具体流程为：第t个时间步的里程计信息可以表示为其中，为相对位姿运算符，wt表示里程计测量噪声，设定该噪声服从均值为0、方差为Q的高斯分布，给定xt和xt-1，ot的似然估计为
第t个时间步观测到的第k个信标的位置可以表示为其中，vt表示信标的位置测量误差，设定该误差服从均值为0、方差为R的高斯分布，给定车辆位姿xt和信标位置的似然估计为
根据上述ot和的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
由于噪声符合高斯分布，可以将上式中的log(f(ot；xt-1,xt))和进一步化简为二次型：
对位姿图的优化等价于求解极大似然估计问题：
求解结果X*即为优化后的车辆定位轨迹，L*即为优化后的信标地图。
实施例3
一种基于人工信标的无人车辆激光雷达建图与定位方法的实施例3，在实施例1和实施例2的基础上，对信标进一步限定。信标的编码图案的外层添加一层黑色边框和一层白色边框。在布置信标的时候将信标顺时针旋转45°
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (6)

1.一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，包括如下步骤：
步骤一：制作和布置信标；
步骤二：无人车辆通过激光雷达获取信标的位置和编号信息；在步骤二中，激光雷达获取信标的原始点云数据，通过信标感知算法对激光雷达获取的原始点云数据进行处理，实时获取环境中信标的位置和编号信息；
步骤三：结合车载里程计信息和信标位置和编号信息构建位姿图；
步骤四：对位姿图进行优化得到车辆定位轨迹和信标地图；
信标为根据编码长度、编码最小汉明距离和编码复杂度生成的二进制编码图案；编码长度对应编码图案部分的方块个数，编码图案由个黑白方块组成，所以必须为完全平方数，每个方块代表一个码字，黑色方块代表0，白色方块代表1，根据编码按照从上到下、从左到右的顺序生成编码图案；
通过信标感知算法对激光雷达获取的原始点云数据进行处理的具体流程如下：
S2.1：对原始点云进行特征点提取，对点云中的每个点计算距离梯度：
其中，为第条激光线束中的第个点，和分别为在水平方向上的左右邻域点；
若某个点的距离梯度大于设定的阈值，则认为该点为边缘点；若某边缘点的左右领域点均不是边缘点，则认为该点为特征点；
S2.2：通过层次聚类算法对特征点进行聚类，将每个特征点作为一个类簇，然后不断找到距离最近的两个类簇并合并，直到任意两个类簇之间的距离大于设定的阈值，对每个类簇，分别从类簇内点数、强度边界点个数和平面度三个验证条件进行验证，若该类簇同时符合上述的三个验证条件，则认为该类簇中的点为信标点云；
S2.3:从原始点云中提取出信标点云后，通过位姿估计算法对信标相对于激光雷达的位置和姿态进行求解，得到信标的位姿矩阵；得到位姿矩阵的具体流程为：
S2.3.1：在激光雷达坐标系原点处设置一个边长为、厚度为的投影目标，通过刚体变换矩阵将信标点云投影到投影目标处，定义投影损失如下：
其中，为信标点云，、和分别为中第个点经过投影后的坐标，为损失计算函数；
S2.3.2：定义损伤计算函数为:
其中，，
S2.3.3:通过非线性优化库NLopt对优化问题进行求解，得到最优的刚体变换矩阵使得投影损失最小，对该矩阵求逆即得到信标的位姿矩阵；
S2.4:根据步骤S2.3中的位姿矩阵将信标点云变换到坐标原点处，对信标点云进行分割等分为个方块；根据白色物体的激光反射强度比黑色物体高的特性，对编码部分的第个方块计算反射强度均值，将其与整个信标点云的反射强度均值对比，若则认为该方块对应的码字为1，否则为0；把每个方块对应的码字依次串联起来即得到该信标点云的编码值，计算该编码值与编码库中的所有编码之间的汉明距离，若汉明距离最小值不大于设定的阈值，则将该编码对应的编号作为编码信息。
2.根据权利要求1所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤S2.2中，类簇内点数、强度边界点个数和平面度的验证方法具体为：
类簇内点数：将属于该类簇内的原始点云填充回去，类簇内点数应满足，其中，表示激光扫描到每个方块的最少点数，表示构成信标图案的方块个数，为扫描到信标的激光线束，为信标与激光雷达之间的距离，为激光雷达的水平分辨率；
强度边界点个数：对类簇中的每个点计算强度梯度
其中，为第条激光线束中的第个点的强度值，和分别为在水平方向上的左右邻域点的强度值；
若某个点的强度梯度大于设定的阈值，则认为该点为强度边界点，强度边界点个数应大于等于；
平面度：对类簇内的所有点用随机采样一致算法进行平面拟合，离群点个数应小于类簇内点数的4%-6%。
3.根据权利要求1所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤三中，里程计信息记录的是两个时间步之间的车辆位姿变化，记为；设定步骤二中车辆运行的步长为，则车辆的运动轨迹可以表示为，其中，表示车辆在第个时间步的位姿，为里程计测得的与之间的相对位姿；
信标设定有N个，每个信标的编号各不相同，记为，其中，，表示编号为的信标的三维坐标；车辆在第个时间步观测到个信标，信标相对于车辆的位置记为，表示该信标相对于车辆的三维坐标，信标的编号记为，其中；
根据里程计信息和信标相对于车辆的位置构建位姿图。
4.根据权利要求3所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在步骤四中，对位姿图进行优化的流程为：
第个时间步的里程计信息可以表示为，其中，为相对位姿运算符，表示里程计测量噪声，设定该噪声服从均值为0、方差为的高斯分布，给定和，的似然估计为；
第个时间步观测到的第个信标的位置可以表示为，其中，表示信标的位置测量误差，设定该误差服从均值为0、方差为的高斯分布，给定车辆位姿和信标位置，的似然估计为；
根据上述和的似然估计，可以得到所有里程计信息和信标感知结果的联合对数似然估计：
由于噪声符合高斯分布，可以将上式中的 和 进一步化简为二次型：
对位姿图的优化等价于求解极大似然估计问题：
求解结果即为优化后的车辆定位轨迹，即为优化后的信标地图。
5.根据权利要求1-4任一所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，信标的编码图案的外层添加一层黑色边框和一层白色边框。
6.根据权利要求1-4任一所述的一种基于人工信标的无人车辆激光雷达建图与定位方法，其特征在于，在布置信标的时候将信标顺时针旋转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 CN115015959A (zh) 2022-09-06
CN115015959B true 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 (2)

* 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 浙江理工大学 基于词袋模型及度量学习的车辆轨迹聚类方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114200481A (zh) * 2020-08-28 2022-03-18 华为技术有限公司 一种定位方法、定位系统和车辆
CN113191459B (zh) * 2021-05-27 2022-09-09 山东高速建设管理集团有限公司 一种基于路侧激光雷达的在途目标分类方法
CN113674399A (zh) * 2021-08-16 2021-11-19 杭州图灵视频科技有限公司 一种移动机器人室内三维点云地图构建方法及系统

Patent Citations (2)

* 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 浙江理工大学 基于词袋模型及度量学习的车辆轨迹聚类方法

Also Published As

Publication number Publication date
CN115015959A (zh) 2022-09-06

Similar Documents

Publication Publication Date Title
Wang et al. Intensity scan context: Coding intensity and geometry relations for loop closure detection
EP3349041B1 (en) Object detection system
CN111882612B (zh) 一种基于三维激光检测车道线的车辆多尺度定位方法
Habib et al. A new approach for matching surfaces from laser scanners and optical scanners
US5136297A (en) Method for navigation and updating of navigation for aircraft
CN112346463B (zh) 一种基于速度采样的无人车路径规划方法
Fujii et al. Urban object reconstruction using airborne laser elevation image and aerial image
CN112455502B (zh) 基于激光雷达的列车定位方法及装置
CN112379393B (zh) 一种列车碰撞预警方法及其装置
Burger et al. Fast multi-pass 3D point segmentation based on a structured mesh graph for ground vehicles
CN114137560A (zh) 基于改进激光描述符的车辆重定位方法、装置及电子设备
CN113743171A (zh) 目标检测方法及装置
Wang et al. Autonomous landing of multi-rotors UAV with monocular gimbaled camera on moving vehicle
US11982752B2 (en) GPS error correction method through comparison of three-dimensional HD-maps in duplicate area
CN112255641A (zh) 一种汽车轴距的测量方法
EP3940670B1 (en) Information processing device and mobile robot
CN115930946A (zh) 室内外交变环境下动态障碍物多特征描述的方法
CN110550067B (zh) 列车车轮的测量方法及相关系统
CN113759928B (zh) 用于复杂大尺度室内场景的移动机器人高精度定位方法
US20210304518A1 (en) Method and system for generating an environment model for positioning
CN115015959B (zh) 一种基于人工信标的无人车辆激光雷达建图与定位方法
CN114119957A (zh) 高速铁路钢轨廓形检测方法及装置
Xu et al. A method of 3d building boundary extraction from airborne lidar points cloud
CN117029870A (zh) 一种基于路面点云的激光里程计
Huang et al. A hybrid approach to extraction and refinement of building footprints from airborne LIDAR data

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