CN113238251A - 一种基于车载激光雷达的目标级语义定位方法 - Google Patents

一种基于车载激光雷达的目标级语义定位方法 Download PDF

Info

Publication number
CN113238251A
CN113238251A CN202110363754.3A CN202110363754A CN113238251A CN 113238251 A CN113238251 A CN 113238251A CN 202110363754 A CN202110363754 A CN 202110363754A CN 113238251 A CN113238251 A CN 113238251A
Authority
CN
China
Prior art keywords
vehicle
target
point cloud
laser radar
interested
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
CN202110363754.3A
Other languages
English (en)
Other versions
CN113238251B (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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202110363754.3A priority Critical patent/CN113238251B/zh
Publication of CN113238251A publication Critical patent/CN113238251A/zh
Application granted granted Critical
Publication of CN113238251B publication Critical patent/CN113238251B/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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种基于车载激光雷达的目标级语义定位方法。所述方法包括以下步骤：利用车载激光雷达扫描周围环境，采集点云数据以及预处理；利用随机一致性采样和欧氏聚类处理点云，检测静态感兴趣目标中心位置；利用车载激光雷达安装参数和GPS定位结果，通过横轴墨卡托投影解算感兴趣目标的世界位姿；利用扩展卡尔曼滤波算法，构建语义地图或更新语义地图中感兴趣目标的语义信息；利用粒子滤波算法，将感兴趣目标与自车的相对位姿作为观测量，实时修正自车的定位飘移。本发明可有效修正惯性导航单元在高楼、隧道场景下的定位误差，解决车辆静止状态下的定位飘移问题，相较于仅使用惯导的定位模块，适用范围更广。

Description

一种基于车载激光雷达的目标级语义定位方法
技术领域
本发明涉及高等级自动驾驶定位领域，尤其涉及一种基于车载激光雷达的目标级语义定位方法。
背景技术
随着我国智能汽车行业的蓬勃发展，高等级自动驾驶需求渐长。L4级以上的自动驾驶全程由系统接管，无需人工干预，能够有效减少危险驾驶、疲劳驾驶、酒后驾驶等由于人类驾驶员不规范的驾驶行为导致的安全问题。而在实现L4级以上的自动驾驶技术过程中，定位模块受到各国研究者和企业的广泛关注。该模块为下游提供车辆的世界坐标，以便规划、控制模块根据进一步决定自车的行驶路线，因此，定位的精度直接决定车辆在动态交互环境中，做出正确的避让行为、跟随中心参考线安全地行驶，一个稳定而精确的定位系统尤为重要。
国际上应用较为广泛的定位方法是惯性导航单元。这是一种由GPS和IMU组成的定位装置，工作原理是首先从牛顿力学出发，结合车辆的运动模型对时间进行积分；然后根据GPS的RTK差分定位结果，将位置变换到导航坐标系中，从而得到速度、偏航角、位置等信息。这种导航的方式属于航迹推算导航，其主要缺点包括：
1、由于导航信息是对时间积分得到的，而积分过程会累积定位误差，因此作为一种长期定位算法，它的精度会逐渐降低；
2、GPS和IMU在惯导开机之后、使用之前，需要较长的初始对准时间，有时甚至需要通过绕八字等特殊的驾驶轨迹才能精确对准，否则直接定位误差很大乃至不可用(代凯,申棋仁,吴官朴,赵阳,马芳武.基于激光雷达的SLAM和融合定位方法综述[J].汽车文摘,2021(02):1-8.)；
3、即使车辆不动，惯导也会有静态误差，经过积分后表现为车辆的位置随时间往某个方向飘移(赵一兵,刘昌华,郑震,郭烈,马振强,韩治中.基于多传感信息融合的智能车辆定位方法[J].汽车工程学报,2021,11(01):1-10.)。
发明内容
本发明的目的是以自动驾驶车辆的精准定位作为研究目标，综合考虑准确性与实时性，结合实际应用场景，提出一种基于车载激光雷达的目标级语义定位方法。本发明首先利用体素滤波方法对点云数据进行预处理，然后加入点的分布特征进行筛选，计算筛选出来的点云簇中心位置，计算目标的朝向角，作为语义地图的基本元素。进一步降低由惯导装置导致的误差，同时保证方法的实时性，达到同时兼顾准确性和实时性的目的。
本发明的目的至少通过如下技术方案之一实现。
一种基于车载激光雷达的目标级语义定位方法，包括以下步骤：
S1、利用车载激光雷达扫描周围环境，采集点云数据以及预处理；
S2、利用随机一致性采样和欧氏聚类处理点云，检测静态感兴趣目标中心位置；
S3、利用车载激光雷达安装参数和GPS定位结果，通过横轴墨卡托投影解算感兴趣目标的世界位姿；
S4、利用扩展卡尔曼滤波算法，构建语义地图或更新语义地图中感兴趣目标的语义信息；
S5、利用粒子滤波算法，将感兴趣目标与自车的相对位姿作为观测量，实时修正自车的定位飘移。
进一步地，步骤S1具体包括以下步骤：
S1.1、将车载激光雷达安装于车辆顶部，并记录安装位置与车辆后轴中心的三维距离，用作标定参数和后续全局坐标的计算依据；
S1.2、准备一台计算平台，在计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
S1.3、启动激光雷达驱动节点，传感器设置为默认参数，记录点云数据；
S1.4、根据车辆感知范围的要求，划定感兴趣区域；
S1.5、遍历点云中的每个点，滤除高度和坐标位置在划定的感兴趣区域范围外的点云；
S1.6、为了提高目标检测速度，在点云所在三维空间创建一个体素栅格，对于体素栅格内的每一个体素，以体素内所有点的重心近似代替体素中的点，然后，所有体素中的重心点组成滤波后的点云，处理完毕后，相当于对整个点云降采样，但保持了点云的结构便于检测感兴趣目标；
S1.7、存储滤波后的点云的集合，作为步骤S2中感兴趣目标检测的输入。
进一步地，激光雷达为64线激光雷达；ROS通讯包的版本号为Kinetic。
进一步地，步骤S1.4中，划定感兴趣区域，具体设置的参数包括水平角α、高度角
Figure BDA0003006571080000021
长宽阈值X和高度阈值Y；以车头方向为正向，则从车头看去，感兴趣区域具体包括：
车身左外侧X/2至车身右外侧X/2的范围内，车前方Y/2至车后方Y/2的范围内，车载激光雷达正面左右α/2的范围内，以及车载激光雷达正面竖直上下φ/2的范围内。
进一步地，步骤S2中，检测感兴趣目标中心位置具体包括以下步骤：
S2.1、设置平面滤波阈值H，在同一个方向上，计算在法线方向激光点与随机一致性采样法拟合出的平面之间的高度偏差，高度偏差不大于H时，将范围内的点云认为是同一个平面；
S2.2、通过数据集统计得到感兴趣目标的实际高度L，设置平面滤波阈值H＝L/5，保留高度偏差大于L/5的点云，其他点云作为地面部分滤除；
S2.3、统计感兴趣目标的点云簇特征，测定每个感兴趣目标的点云簇中，两点之间的最大距离D以及同一类感兴趣目标中最大点云簇的点云数N；
S2.4、调用点云库的KD树算法搜索两点之间的最大距离为D、同一类感兴趣目标中最大点云簇的点云数为N的点云簇，作为下一步的输入；
S2.5、对步骤S2.4中的各个点云簇建立点云簇坐标系，点云簇坐标系的原点为各点云簇的几何重心，向上为Z正轴，X、Y正轴任意选取，根据人为确定的感兴趣目标的实际尺寸，设定X、Y方向宽度的最大标准差阈值S_xy和Z轴高度的最大标准差阈值S_z；
S2.6、对于步骤S2.4中得到的点云簇，滤除X坐标或Y坐标的标准差大于S_xy或Z轴标准差大于S_z的点云簇，保留剩余点云簇作为检测结果；
S2.7、遍历步骤S2.6中检测结果中的点云簇，计算每个点云簇的X坐标、Y坐标、Z坐标的平均值，即为感兴趣目标的重心点，将每个点云簇的重心点作为感兴趣目标的中心坐标。
进一步地，步骤S3中，所述车载激光雷达安装参数包括车载激光雷达与后轴中心的高度差、车载激光雷达与车辆轴线方向的距离和雷达相位角。
进一步地，步骤S3中，所述解算目标世界位姿具体包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置惯性导航单元驱动节点；
S3.2、启动惯性导航单元驱动节点，实时读取、解析差分定位服务设备输出的定位数据流，获得自车所在位置经纬度；
S3.3、利用横轴墨卡托投影公式，将经纬度WGS84坐标转换成UTM世界坐标；
S3.4、读取车载激光雷达安装参数，累加到自车位姿上，获得车载激光雷达的世界坐标；
S3.5、设置激光雷达坐标系，其中，以车载激光雷达的正前方为X轴正向，左侧为Y轴正向，向上为Z轴正向，根据感兴趣目标在激光雷达坐标系的检测结果，换算得到感兴趣目标的世界位姿，包括横坐标、纵坐标和朝向角。
进一步地，步骤S4中，所述构建语义地图具体包括以下步骤：
S4.1、初始化地图为空，假设语义地图的元素满足高斯分布；
S4.2、输入任意一个感兴趣目标的世界位姿作为语义地图的第一个元素，初始化该元素的高斯分布的均值为对应的感兴趣目标的世界位姿，方差为一单位矩阵；
S4.3、遍历剩余感兴趣目标，计算当前感兴趣目标与语义地图中所有元素之间的最小马氏距离；
S4.4、统计同一类的各个感兴趣目标在实际世界坐标系下的距离L同类，设置同类距离阈值THS＝L同类-0.55m；
S4.5、如果语义地图中所有元素与当前感兴趣目标的最小马氏距离大于同类距离阈值THS，则将当前感兴趣目标作为语义地图的新元素，否则将与当前感兴趣目标的马氏距离最小的元素所对应的感兴趣目标作为被关联目标，利用扩展卡尔曼滤波更新语义地图中被关联目标的语义信息；
S4.6、返回步骤S4.3，直到语义地图建立完毕或不再有新元素加入为止。
进一步地，所述更新地图中被关联目标的语义信息具体包括以下步骤：
S4.5.1、初始化扩展卡尔曼滤波器，设置系统误差为Q、观测误差为R；
S4.5.2、计算预测值，由于感兴趣目标为静态目标，取状态转移矩阵为一单位矩阵，具体如下：
Figure BDA0003006571080000045
式中，x′为预测状态向量，x为上一时刻最优估计值，A为状态转移矩阵，P′为预测协方差矩阵，Q为系统误差矩阵；
S4.5.3、计算观测值，观测值由二维距离向量组成，即马氏距离的向量表示，具体如下：
Figure BDA0003006571080000041
式中，DM(p)为马氏距离，p为二维距离向量即观测值，
Figure BDA0003006571080000042
μ为语义地图中的感兴趣目标当前分布的均值；xego为自车世界位姿的横坐标；yego为自车世界位姿的纵坐标；xobs为感兴趣目标世界位姿的横坐标；yobs为感兴趣目标世界位姿的纵坐标；
S4.5.4、计算卡尔曼增益，用增益与观测残差的乘积修正预测值，得到新的语义信息，更新语义地图中被关联目标的分布，达到更新语义地图的目的，具体如下：
Figure BDA0003006571080000043
式中，
Figure BDA0003006571080000044
为观测转移矩阵，R为观测误差矩阵。
进一步地，所述用相对位姿实时修正定位飘移包括以下步骤：
S5.1、初始化粒子群，设定粒子群的数量为S则每个粒子的权重初始化为1/S，每个粒子都携带一份由感兴趣目标组成的语义地图，粒子代表着车辆可能的位姿分布，当粒子的数量合适，随着迭代次数的增加，粒子群的平均加权位姿将接近于车辆的真实位姿，初始化自车位置为当前定位结果；
S5.2、取粒子群中，粒子的总个数为N；
S5.3、计算粒子的位姿预测值，粒子的状态转移矩阵定义为车辆运动模型，在简单的场景(如低速、缓弯等)中可以用自行车模型代替，或是在高速场景中考虑轮胎的侧向力，建立更贴近实际的动力学模型来预测粒子的估计位姿，利用航迹推算公式演化粒子；
S5.4、计算观测值残差：首先计算观测的感兴趣目标与语义地图中所有的感兴趣目标的最小马氏距离，取对应的感兴趣目标的状态向量与自车的位姿的相对距离，两者相减得到观测残差；
S5.5、计算卡尔曼增益，更新粒子的状态向量；
S5.6、遍历语义地图中的所有粒子，返回步骤S5.3，得到每个粒子更新后的状态向量；
S5.7、计算每个粒子的采样权重weight，粒子的采样权重由观测与语义地图的偏差决定，偏差越大则权重越低，具体如下：
Figure BDA0003006571080000051
S5.8、取所有粒子位姿的加权平均值，作为自车新的位姿估计结果；
S5.9、遍历粒子群，将粒子的权重组成一个权值向量，取权值向量叉积的倒数作为有效粒子数量E，如果有效粒子的数量低于总数的某个比例，采用轮盘赌选择法对粒子群进行重要性重采样，采样后新粒子群的权重初始化为1/N；
Figure BDA0003006571080000052
式中，w1 w2 wn...代表n个粒子的权重；
S5.10、返回步骤S5.1，实时修正定位误差。
与现有技术相比，本发明的有益效果是：
1、本发明与传统的基于点云结构的匹配定位方法相比，不易受低纹理环境的影响，可持续稳定地输出定位结果且实时性强。
2、本发明可有效修正惯性导航单元在高楼、隧道场景下的定位误差，解决车辆静止状态下的定位飘移问题，相较于仅使用惯导的定位模块，适用范围更广。
附图说明
图1本发明实施例中一种基于车载激光雷达的目标级语义定位方法的流程示意图；
图2本发明实施例中的可视化结果。
具体实施方式
下面通过具体实施例对本发明的目的作进一步详细的描述。
实施例：
本实施例中，为降低成本及方便实验测试，本发明应用线控底盘模拟车辆的应用场景。其中，用锥桶模拟静态感兴趣目标。采集数据时，摇控底盘在锥桶摆成的赛道中行驶，模拟车辆在城市中跟随结构化道路中心线行驶的场景。该实验设置是为了方便阐述本发明细节，本发明的实际实施方式并不因此限定于以下实施例。
一种基于车载激光雷达的目标级语义定位方法，如图1所示，包括以下步骤：
S1、利用车载激光雷达扫描周围环境，采集点云数据以及预处理，具体包括以下步骤：
S1.1、将车载激光雷达安装于车辆顶部，并记录安装位置与车辆后轴中心的三维距离，用作标定参数和后续全局坐标的计算依据；
S1.2、准备一台计算平台，在计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
本实施例中，将线控底盘作为等比例缩小的乘用车进行模拟仿真实验。锥桶是仿真中的感兴趣目标，由于锥桶高度限制，若将激光雷达架起，大部分扫描点云无法打在锥桶上，将失去目标检测能力。
因此，实验时将激光雷达设置在底盘，这里的布置原则和“乘用车布置在车顶”并不矛盾，都是出于“最大程度地保证对感兴趣目标的检测能力”原则。
本实施例中，使用NvidiaTX2作为计算平台，配置好激光雷达的驱动包和ROS机器人操作系统，并将该计算平台固定在线控底盘上，使用移动电源为其供电。在车辆行驶过程中，启动节点即可实时采集点云数据。激光雷达在底盘上的离地间隙为10cm、目标锥桶的尺寸为20*20*30cm。
S1.3、启动激光雷达驱动节点，传感器设置为默认参数，记录点云数据；
S1.4、根据车辆感知范围的要求，划定感兴趣区域，具体设置的参数包括水平角α、高度角
Figure BDA0003006571080000061
长宽阈值X和高度阈值Y；以车头方向为正向，则从车头看去，感兴趣区域具体包括：
车身左外侧X/2至车身右外侧X/2的范围内，车前方Y/2至车后方Y/2的范围内，车载激光雷达正面左右α/2的范围内，以及车载激光雷达正面竖直上下φ/2的范围内。
本实施例中，根据锥桶的实际尺寸，确定Z方向取0.6m以下，高度角取30度，划分感兴趣区域；
S1.5、遍历点云中的每个点，滤除高度和坐标位置在划定的感兴趣区域范围外的点云；
S1.6、为了提高目标检测速度，在点云所在三维空间创建一个体素栅格，对于体素栅格内的每一个体素，以体素内所有点的重心近似代替体素中的点，然后，所有体素中的重心点组成滤波后的点云，处理完毕后，相当于对整个点云降采样，但保持了点云的结构便于检测感兴趣目标；
S1.7、存储滤波后的点云的集合，作为步骤S2中感兴趣目标检测的输入。
本实施例中，根据激光雷达的安装情况测量到感知范围为X方向80m、Y方向左右5m，将这些数据作为直通滤波阈值。由于实施例中锥桶的尺寸相对较小，点云已经足够稀疏，因此跳过体素滤波步骤。
S2、利用随机一致性采样和欧氏聚类处理点云，检测静态感兴趣目标中心位置，具体包括以下步骤：
S2.1、设置平面滤波阈值H，在同一个方向上，计算在法线方向激光点与随机一致性采样法拟合出的平面之间的高度偏差，高度偏差不大于H时，将范围内的点云认为是同一个平面；
S2.2、通过数据集统计得到感兴趣目标的实际高度L，设置平面滤波阈值H＝L/5，保留高度偏差大于L/5的点云，其他点云作为地面部分滤除；
本实施例中，实验测定最佳的平面滤波阈值H为0.05m；
S2.3、统计感兴趣目标的点云簇特征，测定每个感兴趣目标的点云簇中，两点之间的最大距离D以及同一类感兴趣目标中最大点云簇的点云数N；
本实施例中，设置点云簇之间的最大距离D为0.3m，最大点云数N为120；
S2.4、调用点云库的KD树算法搜索两点之间的最大距离为D、同一类感兴趣目标中最大点云簇的点云数为N的点云簇，作为下一步的输入；
S2.5、对步骤S2.4中的各个点云簇建立点云簇坐标系，点云簇坐标系的原点为各点云簇的几何重心，向上为Z正轴，X、Y正轴任意选取，根据人为确定的感兴趣目标的实际尺寸，设定X、Y方向宽度的最大标准差阈值S_xy和Z轴高度的最大标准差阈值S_z；
本实施例中，由统计规律，符合条件的锥桶目标点云簇的最大标准差在X、Y方向小于0.1m，Z轴上小于0.15m；
S2.6、对于步骤S2.4中得到的点云簇，滤除X坐标或Y坐标的标准差大于S_xy或Z轴标准差大于S_z的点云簇，保留剩余点云簇作为检测结果；
S2.7、遍历步骤S2.6中检测结果中的点云簇，计算每个点云簇的X坐标、Y坐标、Z坐标的平均值，即为感兴趣目标的重心点，将每个点云簇的重心点作为感兴趣目标的中心坐标。
S3、利用车载激光雷达安装参数和GPS定位结果，通过横轴墨卡托投影解算感兴趣目标的世界位姿；
所述车载激光雷达安装参数包括车载激光雷达与后轴中心的高度差、车载激光雷达与车辆轴线方向的距离和雷达相位角。
本实施例中，购买了厘米级差分定位服务，惯导单元自带航迹推算功能，轮速信息由线控底盘提供，在上述计算平台安装好驱动程序，启动节点即可接收到在线GPS定位数据，同时将激光雷达的安装参数写入配置文件。
所述解算目标世界位姿具体包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置惯性导航单元驱动节点；
S3.2、启动惯性导航单元驱动节点，实时读取、解析差分定位服务设备输出的定位数据流，获得自车所在位置经纬度；
S3.3、利用横轴墨卡托投影公式，将经纬度WGS84坐标转换成UTM世界坐标；
S3.4、读取车载激光雷达安装参数，累加到自车位姿上，获得车载激光雷达的世界坐标；
S3.5、设置激光雷达坐标系，其中，以车载激光雷达的正前方为X轴正向，左侧为Y轴正向，向上为Z轴正向，根据感兴趣目标在激光雷达坐标系的检测结果，换算得到感兴趣目标的世界位姿，包括横坐标、纵坐标和朝向角。
S4、利用扩展卡尔曼滤波算法，构建语义地图，具体包括以下步骤：
S4.1、初始化地图为空，假设语义地图的元素满足高斯分布；
S4.2、输入任意一个感兴趣目标的世界位姿作为语义地图的第一个元素，初始化该元素的高斯分布的均值为对应的感兴趣目标的世界位姿，方差为一单位矩阵；
S4.3、遍历剩余感兴趣目标，计算当前感兴趣目标与语义地图中所有元素之间的最小马氏距离；
S4.4、统计同一类的各个感兴趣目标在实际世界坐标系下的距离L同类，设置同类距离阈值THS＝L同类-0.55m；
本实施例中，THS设置为2m；
S4.5、如果语义地图中所有元素与当前感兴趣目标的最小马氏距离大于同类距离阈值THS，则将当前感兴趣目标作为语义地图的新元素，否则将与当前感兴趣目标的马氏距离最小的元素所对应的感兴趣目标作为被关联目标，利用扩展卡尔曼滤波更新语义地图中被关联目标的语义信息，具体包括以下步骤：
S4.5.1、初始化扩展卡尔曼滤波器，设置系统误差为Q、观测误差为R；
S4.5.2、计算预测值，由于感兴趣目标为静态目标，取状态转移矩阵为一单位矩阵，具体如下：
Figure BDA0003006571080000081
式中，x′为预测状态向量，x为上一时刻最优估计值，A为状态转移矩阵(单位阵)，P′为预测协方差矩阵，Q为系统误差矩阵；
S4.5.3、计算观测值，观测值由二维距离向量组成，即马氏距离的向量表示，具体如下：
Figure BDA0003006571080000091
式中，DM(p)为马氏距离，p为二维距离向量即观测值，
Figure BDA0003006571080000092
μ为语义地图中的感兴趣目标当前分布的均值；xego为自车世界位姿的横坐标；yego为自车世界位姿的纵坐标；xobs为感兴趣目标世界位姿的横坐标；yobs为感兴趣目标世界位姿的纵坐标；
S4.5.4、计算卡尔曼增益，用增益与观测残差的乘积修正预测值，得到新的语义信息，更新语义地图中被关联目标的分布，达到更新语义地图的目的，具体如下：
Figure BDA0003006571080000093
式中，
Figure BDA0003006571080000094
为观测转移矩阵，R为观测误差矩阵。
S4.6、返回步骤S4.3，直到语义地图建立完毕或不再有新元素加入为止。
本实施例中，设置扩展卡尔曼滤波算法的系统误差协方差矩阵为斜对角0.3的三维单位阵、观测误差协方差矩阵为斜对角0.2的二维单位阵，点云簇的状态向量由横坐标X均值、纵坐标Y均值、朝向角α均值组成，观测向量由相对距离D、相对朝向角φ组成，即可由观测转移矩阵得到卡尔曼增益公式，在本实施例中，由于锥桶是静态目标，因此将锥桶的状态转移矩阵设置为单位矩阵。
S5、利用粒子滤波算法，将感兴趣目标与自车的相对位姿作为观测量，实时修正自车的定位飘移，包括以下步骤：
S5.1、初始化粒子群，设定粒子群的数量为S则每个粒子的权重初始化为1/S，每个粒子都携带一份由感兴趣目标组成的语义地图，粒子代表着车辆可能的位姿分布，当粒子的数量合适，随着迭代次数的增加，粒子群的平均加权位姿将接近于车辆的真实位姿，初始化自车位置为当前定位结果；
本实施例中，设定粒子群的数量为100，因此初始化粒子的权重为0.01。每个粒子是对自车状态向量的一次估计，因此其状态向量由自车的横坐标X、纵坐标Y、朝向角α组成。
S5.2、取粒子群中，粒子的总个数为N；
S5.3、计算粒子的位姿预测值，粒子的状态转移矩阵定义为车辆运动模型，在简单的场景(如低速、缓弯等)中可以用自行车模型代替，或是在高速场景中考虑轮胎的侧向力，建立更贴近实际的动力学模型来预测粒子的估计位姿，利用航迹推算公式演化粒子；
本实施例中，使用自行车模型推算粒子的预测位姿。
S5.4、计算观测值残差：首先计算观测的感兴趣目标与语义地图中所有的感兴趣目标的最小马氏距离，取对应的感兴趣目标的状态向量与自车的位姿的相对距离，两者相减得到观测残差；
S5.5、计算卡尔曼增益，更新粒子的状态向量；
S5.6、遍历语义地图中的所有粒子，返回步骤S5.3，得到每个粒子更新后的状态向量；
S5.7、计算每个粒子的采样权重weight，粒子的采样权重由观测与语义地图的偏差决定，偏差越大则权重越低，具体如下：
Figure BDA0003006571080000101
S5.8、取所有粒子位姿的加权平均值，作为自车新的位姿估计结果，如图2所示；
S5.9、遍历粒子群，将粒子的权重组成一个权值向量，取权值向量叉积的倒数作为有效粒子数量E，如果有效粒子的数量低于总数的80％，采用轮盘赌选择法对粒子群进行重要性重采样，采样后新粒子群的权重初始化为1/N；
Figure BDA0003006571080000102
式中，w1 w2 wn...代表n个粒子的权重；
S5.10、返回步骤S5.1，实时修正定位误差。
以上所述，仅为本发明选择的实施例，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都属于本发明的保护范围。

Claims (10)

1.一种基于车载激光雷达的目标级语义定位方法，其特征在于，包括以下步骤：
S1、利用车载激光雷达扫描周围环境，采集点云数据以及预处理；
S2、利用随机一致性采样和欧氏聚类处理点云，检测静态感兴趣目标中心位置；
S3、利用车载激光雷达安装参数和GPS定位结果，通过横轴墨卡托投影解算感兴趣目标的世界位姿；
S4、利用扩展卡尔曼滤波算法，构建语义地图或更新语义地图中感兴趣目标的语义信息；
S5、利用粒子滤波算法，将感兴趣目标与自车的相对位姿作为观测量，实时修正自车的定位飘移。
2.根据权利要求1所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S1具体包括以下步骤：
S1.1、将车载激光雷达安装于车辆顶部，并记录安装位置与车辆后轴中心的三维距离；
S1.2、准备一台计算平台，在计算平台中安装Ubuntu操作系统和ROS通讯包，配置激光雷达驱动节点；
S1.3、启动激光雷达驱动节点，传感器设置为默认参数，记录点云数据；
S1.4、根据车辆感知范围的要求，划定感兴趣区域；
S1.5、遍历点云中的每个点，滤除高度和坐标位置在划定的感兴趣区域范围外的点云；
S1.6、在点云所在三维空间创建一个体素栅格，对于体素栅格内的每一个体素，以体素内所有点的重心近似代替体素中的点，然后，所有体素中的重心点组成滤波后的点云；
S1.7、存储滤波后的点云的集合，作为步骤S2中感兴趣目标检测的输入。
3.根据权利要求2所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，激光雷达为64线激光雷达；ROS通讯包的版本号为Kinetic。
4.根据权利要求2所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S1.4中，划定感兴趣区域，具体设置的参数包括水平角α、高度角
Figure FDA0003006571070000011
长宽阈值X和高度阈值Y；以车头方向为正向，则从车头看去，感兴趣区域具体包括：
车身左外侧X/2至车身右外侧X/2的范围内，车前方Y/2至车后方Y/2的范围内，车载激光雷达正面左右α/2的范围内，以及车载激光雷达正面竖直上下φ/2的范围内。
5.根据权利要求2所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S2中，检测感兴趣目标中心位置具体包括以下步骤：
S2.1、设置平面滤波阈值H，在同一个方向上，计算在法线方向激光点与随机一致性采样法拟合出的平面之间的高度偏差，高度偏差不大于H时，将范围内的点云认为是同一个平面；
S2.2、通过数据集统计得到感兴趣目标的实际高度L，设置平面滤波阈值H＝L/5，保留高度偏差大于L/5的点云，其他点云作为地面部分滤除；
S2.3、统计感兴趣目标的点云簇特征，测定每个感兴趣目标的点云簇中，两点之间的最大距离D以及同一类感兴趣目标中最大点云簇的点云数N；
S2.4、调用点云库的KD树算法搜索两点之间的最大距离为D、同一类感兴趣目标中最大点云簇的点云数为N的点云簇，作为下一步的输入；
S2.5、对步骤S2.4中的各个点云簇建立点云簇坐标系，点云簇坐标系的原点为各点云簇的几何重心，向上为Z正轴，X、Y正轴任意选取，根据人为确定的感兴趣目标的实际尺寸，设定X、Y方向宽度的最大标准差阈值S_xy和Z轴高度的最大标准差阈值S_z；
S2.6、对于步骤S2.4中得到的点云簇，滤除X坐标或Y坐标的标准差大于S_xy或Z轴标准差大于S_z的点云簇，保留剩余点云簇作为检测结果；
S2.7、遍历步骤S2.6中检测结果中的点云簇，计算每个点云簇的X坐标、Y坐标、Z坐标的平均值，即为感兴趣目标的重心点，将每个点云簇的重心点作为感兴趣目标的中心坐标。
6.根据权利要求5所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S3中，所述车载激光雷达安装参数包括车载激光雷达与后轴中心的高度差、车载激光雷达与车辆轴线方向的距离和雷达相位角。
7.根据权利要求6所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S3中，所述解算目标世界位姿具体包括以下步骤：
S3.1、在计算平台的ROS通讯包中，配置惯性导航单元驱动节点；
S3.2、启动惯性导航单元驱动节点，实时读取、解析差分定位服务设备输出的定位数据流，获得自车所在位置经纬度；
S3.3、利用横轴墨卡托投影公式，将经纬度WGS84坐标转换成UTM世界坐标；
S3.4、读取车载激光雷达安装参数，累加到自车位姿上，获得车载激光雷达的世界坐标；
S3.5、设置激光雷达坐标系，其中，以车载激光雷达的正前方为X轴正向，左侧为Y轴正向，向上为Z轴正向，根据感兴趣目标在激光雷达坐标系的检测结果，换算得到感兴趣目标的世界位姿，包括横坐标、纵坐标和朝向角。
8.根据权利要求7所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，步骤S4中，所述构建语义地图具体包括以下步骤：
S4.1、初始化地图为空，假设语义地图的元素满足高斯分布；
S4.2、输入任意一个感兴趣目标的世界位姿作为语义地图的第一个元素，初始化该元素的高斯分布的均值为对应的感兴趣目标的世界位姿，方差为一单位矩阵；
S4.3、遍历剩余感兴趣目标，计算当前感兴趣目标与语义地图中所有元素之间的最小马氏距离；
S4.4、统计同一类的各个感兴趣目标在实际世界坐标系下的距离L同类，设置同类距离阈值THS＝L同类-0.55m；
S4.5、如果语义地图中所有元素与当前感兴趣目标的最小马氏距离大于同类距离阈值THS，则将当前感兴趣目标作为语义地图的新元素，否则将与当前感兴趣目标的马氏距离最小的元素所对应的感兴趣目标作为被关联目标，利用扩展卡尔曼滤波更新语义地图中被关联目标的语义信息；
S4.6、返回步骤S4.3，直到语义地图建立完毕或不再有新元素加入为止。
9.根据权利要求8所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，所述更新地图中被关联目标的语义信息具体包括以下步骤：
S4.5.1、初始化扩展卡尔曼滤波器，设置系统误差为Q、观测误差为R；
S4.5.2、计算预测值，由于感兴趣目标为静态目标，取状态转移矩阵为一单位矩阵，具体如下：
Figure FDA0003006571070000031
式中，x′为预测状态向量，x为上一时刻最优估计值，A为状态转移矩阵，P′为预测协方差矩阵，Q为系统误差矩阵；
S4.5.3、计算观测值，观测值由二维距离向量组成，即马氏距离的向量表示，具体如下：
Figure FDA0003006571070000032
式中，DM(p)为马氏距离，p为二维距离向量即观测值，
Figure FDA0003006571070000033
μ为语义地图中的感兴趣目标当前分布的均值；xego为自车世界位姿的横坐标；yego为自车世界位姿的纵坐标；xobs为感兴趣目标世界位姿的横坐标；yobs为感兴趣目标世界位姿的纵坐标；
S4.5.4、计算卡尔曼增益，用增益与观测残差的乘积修正预测值，得到新的语义信息，更新语义地图中被关联目标的分布，达到更新语义地图的目的，具体如下：
Figure FDA0003006571070000041
式中，
Figure FDA0003006571070000042
为观测转移矩阵，R为观测误差矩阵。
10.根据权利要求9所述的一种基于车载激光雷达的目标级语义定位方法，其特征在于，所述用相对位姿实时修正定位飘移包括以下步骤：
S5.1、初始化粒子群，设定粒子群的数量为S则每个粒子的权重初始化为1/S，每个粒子都携带一份由感兴趣目标组成的语义地图，粒子代表着车辆可能的位姿分布，当粒子的数量合适，随着迭代次数的增加，粒子群的平均加权位姿将接近于车辆的真实位姿，初始化自车位置为当前定位结果；
S5.2、取粒子群中，粒子的总个数为N；
S5.3、计算粒子的位姿预测值，粒子的状态转移矩阵定义为车辆运动模型利用航迹推算公式演化粒子；
S5.4、计算观测值残差：首先计算观测的感兴趣目标与语义地图中所有的感兴趣目标的最小马氏距离，取对应的感兴趣目标的状态向量与自车的位姿的相对距离，两者相减得到观测残差；
S5.5、计算卡尔曼增益，更新粒子的状态向量；
S5.6、遍历语义地图中的所有粒子，返回步骤S5.3，得到每个粒子更新后的状态向量；
S5.7、计算每个粒子的采样权重weight，粒子的采样权重由观测与语义地图的偏差决定，偏差越大则权重越低，具体如下：
Figure FDA0003006571070000043
S5.8、取所有粒子位姿的加权平均值，作为自车新的位姿估计结果；
S5.9、遍历粒子群，将粒子的权重组成一个权值向量，取权值向量叉积的倒数作为有效粒子数量E，如果有效粒子的数量低于总数的某个比例，采用轮盘赌选择法对粒子群进行重要性重采样，采样后新粒子群的权重初始化为1/N；
Figure FDA0003006571070000051
式中，w1 w2 wn...代表n个粒子的权重；
S5.10、返回步骤S5.1，实时修正定位误差。
CN202110363754.3A 2021-04-02 2021-04-02 一种基于车载激光雷达的目标级语义定位方法 Active CN113238251B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110363754.3A CN113238251B (zh) 2021-04-02 2021-04-02 一种基于车载激光雷达的目标级语义定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110363754.3A CN113238251B (zh) 2021-04-02 2021-04-02 一种基于车载激光雷达的目标级语义定位方法

Publications (2)

Publication Number Publication Date
CN113238251A true CN113238251A (zh) 2021-08-10
CN113238251B CN113238251B (zh) 2023-09-29

Family

ID=77131011

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110363754.3A Active CN113238251B (zh) 2021-04-02 2021-04-02 一种基于车载激光雷达的目标级语义定位方法

Country Status (1)

Country Link
CN (1) CN113238251B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113781639A (zh) * 2021-09-22 2021-12-10 交通运输部公路科学研究所 一种大场景道路基础设施数字化模型快速构建方法
CN114216470A (zh) * 2021-11-30 2022-03-22 广州景骐科技有限公司 一种语义地图加载方法、装置、交通工具及存储介质
CN114216465A (zh) * 2021-12-08 2022-03-22 哈尔滨工程大学 一种海底地形导航并行匹配方法
CN114419571A (zh) * 2022-03-30 2022-04-29 北京理工大学 一种面向无人驾驶车辆的目标检测定位方法及系统
CN118226401A (zh) * 2024-05-22 2024-06-21 南京航空航天大学 基于激光雷达三维点云的固定翼飞机位姿测量方法及装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110705543A (zh) * 2019-08-23 2020-01-17 芜湖酷哇机器人产业技术研究院有限公司 基于激光点云进行车道线识别的方法和系统
US20200025931A1 (en) * 2018-03-14 2020-01-23 Uber Technologies, Inc. Three-Dimensional Object Detection
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111539994A (zh) * 2020-04-28 2020-08-14 武汉科技大学 一种基于语义似然估计的粒子滤波重定位方法
CN111912419A (zh) * 2020-08-28 2020-11-10 清华大学苏州汽车研究院（吴江） 基于激光雷达的高精度语义导航地图构建方法和装置
US20200408535A1 (en) * 2019-06-28 2020-12-31 Gm Cruise Holdings Llc Map change detection

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200025931A1 (en) * 2018-03-14 2020-01-23 Uber Technologies, Inc. Three-Dimensional Object Detection
US20200408535A1 (en) * 2019-06-28 2020-12-31 Gm Cruise Holdings Llc Map change detection
CN110705543A (zh) * 2019-08-23 2020-01-17 芜湖酷哇机器人产业技术研究院有限公司 基于激光点云进行车道线识别的方法和系统
CN111539994A (zh) * 2020-04-28 2020-08-14 武汉科技大学 一种基于语义似然估计的粒子滤波重定位方法
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111912419A (zh) * 2020-08-28 2020-11-10 清华大学苏州汽车研究院（吴江） 基于激光雷达的高精度语义导航地图构建方法和装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
RUI JIAN等: "A Semantic Segmentation Based Lidar SLAM System Towards Dynamic Environments", 《INTELLIGENT ROBOTICS AND APPLICATIONS》, pages 582 - 590 *
牛靖博: "车辆姿态检测及其语义地图应用", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑(月刊)》, no. 11, pages 035 - 51 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113781639A (zh) * 2021-09-22 2021-12-10 交通运输部公路科学研究所 一种大场景道路基础设施数字化模型快速构建方法
CN113781639B (zh) * 2021-09-22 2023-11-28 交通运输部公路科学研究所 一种大场景道路基础设施数字化模型快速构建方法
CN114216470A (zh) * 2021-11-30 2022-03-22 广州景骐科技有限公司 一种语义地图加载方法、装置、交通工具及存储介质
CN114216465A (zh) * 2021-12-08 2022-03-22 哈尔滨工程大学 一种海底地形导航并行匹配方法
CN114216465B (zh) * 2021-12-08 2022-11-01 哈尔滨工程大学 一种海底地形导航并行匹配方法
CN114419571A (zh) * 2022-03-30 2022-04-29 北京理工大学 一种面向无人驾驶车辆的目标检测定位方法及系统
CN114419571B (zh) * 2022-03-30 2022-06-17 北京理工大学 一种面向无人驾驶车辆的目标检测定位方法及系统
CN118226401A (zh) * 2024-05-22 2024-06-21 南京航空航天大学 基于激光雷达三维点云的固定翼飞机位姿测量方法及装置

Also Published As

Publication number Publication date
CN113238251B (zh) 2023-09-29

Similar Documents

Publication Publication Date Title
CN113238251B (zh) 一种基于车载激光雷达的目标级语义定位方法
CN109061703B (zh) 用于定位的方法、装置、设备和计算机可读存储介质
CN111551958B (zh) 一种面向矿区无人驾驶的高精地图制作方法
JP7086111B2 (ja) 自動運転車のｌｉｄａｒ測位に用いられるディープラーニングに基づく特徴抽出方法
US11250576B2 (en) Systems and methods for estimating dynamics of objects using temporal changes encoded in a difference map
US11164369B2 (en) Methods and systems for constructing map data using poisson surface reconstruction
CN107246876B (zh) 一种无人驾驶汽车自主定位与地图构建的方法及系统
CN104764457B (zh) 一种用于无人车的城市环境构图方法
KR101711964B1 (ko) 자유 공간 지도 생성 방법, 자유 공간 지도 생성 시스템, 자유 공간 지도를 이용한 전경/배경 추출 방법 및 전경/배경 추출 시스템
CN111771135A (zh) 自动驾驶车辆中使用rnn和lstm进行时间平滑的lidar定位
US20220197301A1 (en) Vehicle Localization Based on Radar Detections
CN110298311B (zh) 路面积水检测方法及装置
CN114485698B (zh) 一种交叉路口引导线生成方法及系统
CN113515128B (zh) 一种无人车实时路径规划方法及存储介质
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
US20230056589A1 (en) Systems and methods for generating multilevel occupancy and occlusion grids for controlling navigation of vehicles
US12062135B2 (en) Three-dimensional motion grid system for autonomous vehicle perception
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
CN114419573A (zh) 动态占据栅格估计方法及装置
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
EP4278320A1 (en) Systems and methods for monitoring lidar sensor health
WO2021112078A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
CN117516560A (zh) 一种基于语义信息的非结构化环境地图构建方法及系统
CN112985417B (zh) 移动机器人粒子滤波定位的位姿校正方法及移动机器人
EP4330726A1 (en) Systems and methods for producing amodal cuboids

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