CN114659513A - 一种面向非结构化道路的点云地图构建与维护方法 - Google Patents

Abstract

本发明公开了一种面向非结构化道路的点云地图构建与维护方法，包括以下步骤：将激光雷达实时输出的点云数据分割为动态目标点和静态目标点，将静态目标点的点云从激光雷达坐标系变换到点云地图坐标系，得到构建点云地图；车辆实时运行，从已构建点云地图提取局部点云地图，将其变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图，实时点云与局部点云地图通过匹配算法匹配后投影到格栅地图，对比栅格地图内的局部点云地图和实时点云，实现车端点云地图更新维护；将已构建点云地图和车端维护的地图，投影到栅格地图，判断每个栅格内地图是否更新，实现服务器端点云地图维护。该方法能够实现点云地图的构建与实时更新，简单高效且成本低。

Description

一种面向非结构化道路的点云地图构建与维护方法

技术领域

本发明属于自动驾驶车辆感知与定位领域，尤其涉及一种面向非结构化道路的点云地图构建与维护方法。

背景技术

随着自动驾驶技术的发展及应用，感知和定位等相关技术也有了很大的发展。基于点云的高精地图，可以为感知及定位提供先验信息，在自动驾驶领域具有重要作用。高精度地图不但包含道路级信息，而且包含车道级信息，如道路边界、车道线、道路中心线、道路曲率、路口模型、红绿灯标识等，可以为自动驾驶车辆车道级路径规划提供稳定可靠输入。高精地图的道路边界和车道线信息，可以作为感知可行驶区域的边界，过滤可行驶区域外点云，减小感知算法处理数据量，提高感知算法效率和准确性。

结构化道路点云高精地图一般通过专业的地图采集车或者激光SLAM技术构建，需要具有明显的几何特征，如路面平坦坡度小，道路边界与路面成90度，并且道路边界与路面高度差稳定等，地图标注的信息丰富，包括道路边界、车道线、交通标识等，建图成本高耗时长。对于矿山等非结构化道路，路面颠簸不平坡度大，道路边界和路面高度差和角度都不固定，道路几何特征不明显，道路更新频繁，结构化道路建图方案难以适用。同时矿山等封闭非结构化道路区域相对较小，道路拓扑相对简单，针对矿山等封闭非结构化道路的点云地图构建与维护具有重要实用价值。

专利《用于矿区的高精地图构建方法及装置》文献公开号为CN113008247A，公开了一种矿区环境的高精地图构建方法及装置，根据激光点云扫描线在道路边缘上表现出的几何特征提取道路边缘候选点，利用深度学习方法从图像中提取可行驶区域，结合点云道路边缘点和图像可行驶区域，实现矿山环境高精地图构建。该方法结合点云和图像提取的道路特征，实现矿区环境高精地图构建，提高建图稳定性。但是不同矿山或同一个矿山不同路段的道路边缘几何特征差异较大，需要根据实际场景调节较多参数，导致鲁棒性较差。

专利《基于低成本设备的高精度三维点云地图构建方法》文献公开号为CN109934920A，采用激光雷达、低成本GPS和IMU，通过帧间点云特征匹配估计相对位姿，再融合GPS和IMU数据，得到优化的绝对位姿，实现点云地图的构建。该方法所需硬件设备成本低，软件算法简单，便于实施。但是该方法未考虑场景中动态目标滤除，构建的点云地图中可能会有动态目标，导致地图的可用性降低。

专利《一种3D点云地图更新方法和系统》文献公开号为CN112762923A，当检测到地图发生变化时，采用由粗到细的匹配算法，增量式优化算法，构建变化区域局部地图，再将局部地图与全局地图通过点云配准匹配更新，实现地图更新维护，不影响原地图其他区域应用，避免重复工作量。但是该方法未考虑单次检测波动导致的误检测问题，当一次检测某个区域地图发生变化时，可能是由于车辆颠簸或前方出现可移动的车辆等目标干扰导致的误检测，重新构建该区域地图会将移动目标加在地图中。

因此，现有技术中点云地图的构建与维护至少存在如下问题：(1)专业地图采集车采集地图成本高并且周期长，不适用于道路频繁更新的矿山等非结构化道路；(2)激光SLAM对道路周围特征不明显的环境，具有较大建图误差或者建图失败；(3)不能有效检测出动态障碍物，导致动态障碍物出现在构建的地图中；(4)不能根据已有地图有效检测到道路变化，导致地图更新滞后或误更新。

发明内容

有鉴于此，本发明提供一种面向非结构化道路的点云地图构建与维护方法，该方法具有简单、高效的特征。本发明采用以下技术方案：

一种面向非结构化道路的点云地图构建与维护方法，包括以下步骤：

S1，点云地图构建：将激光雷达实时输出的点云数据分割为动态目标点和静态目标点，过滤动态目标点的点云，将静态目标点的点云从激光雷达坐标系变换到点云地图坐标系，得到点云地图，上传至点云地图服务器；

S2，车端点云地图维护：车辆实时运行，从点云地图服务器下载点云地图，提取局部点云地图，获取实时点云，将局部点云地图变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图，将匹配后的实时点云与局部点云地图投影到格栅地图，对比栅格地图内的局部点云地图和实时点云，实现新路径点云地图更新维护和/或已有地图场景变化更新维护；

S3，服务器端点云地图维护：将点云地图服务器中的已有点云地图和步骤S2得到的车端点云地图维护的地图，投影到栅格地图，根据栅格内地图更新次数和高度差，判断每个栅格内地图是否更新，实现服务器端点云地图维护。

进一步，所述步骤S1包括以下子步骤：

(1)自动驾驶车辆的激光雷达实时输出点云数据，通过点云深度学习语义分割，将点云分割为动态目标点或静态目标点；

(2)组合导航系统输出车辆坐标系定位位姿P_B，根据车辆坐标系和激光雷达坐标系的标定参数旋转矩阵R^L _B和平移矩阵T^L _B，将车辆坐标系定位位姿P_B转换到激光雷达坐标系下，得到激光雷达坐标原点定位位姿P_L，如公式(1)所示：

P_L＝R^L _BP_B+T^L _B (1)

(3)过滤动态目标点的点云，以激光雷达坐标原点定位位姿为初始值，以匹配算法将过滤动态目标点的点云与构建中的点云地图(第一帧点云不用匹配)匹配，得到匹配矩阵，将过滤动态目标点的点云通过匹配矩阵变换到点云地图坐标系下，并叠加到点云地图坐标系中，实现点云地图构建；

(4)完成所有行驶路径点云地图构建后，人工校验构建的点云地图准确性，得到点云地图，上传至点云地图服务器。

进一步，所述步骤S2包括以下子步骤：

(1)自动驾驶车辆启动时，从点云地图服务器下载点云地图；

(2)自动驾驶车辆实时运行，获取过滤动态目标点后的实时点云；

(3)自动驾驶车辆实时运行，获取激光雷达坐标原点定位位姿；

(4)根据激光雷达坐标原点定位位姿和设置的检测半径R，从已构建的点云地图中提取局部点云地图；

(5)根据车辆坐标系和激光雷达坐标系的标定参数，将局部点云地图变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图；

(6)将过滤动态目标点后的实时点云和激光雷达坐标系下局部点云地图，经过匹配算法匹配；

(7)将匹配后的实时点云与局部点云地图，投影到栅格地图中，栅格大小和范围根据需求设定；

(8)遍历每个栅格，如果栅格内有实时点云、无局部地图点云，则该区域内点云为新路径对应点云，如果栅格内实时点云与局部地图点云高度差大于设定阈值，则该区域内点云为点云地图变化点云；将新路径对应点云或点云地图变化点云，根据激光雷达原点的全局位姿，变换到点云地图坐标系下，并上传至点云地图服务器。

进一步，所述步骤S3包括以下子步骤：

(1)将点云地图服务器中的已有点云地图和步骤S2的更新的点云数据投影到栅格地图中，栅格范围根据已构建点云地图和更新的点云数据范围确定，栅格分辨率根据需求设定；

(2)遍历(1)中每个栅格，删除更新的点云数据时间早于设定阈值的数据，保留剩余更新数据，确保更新数据的实时性；

(3)遍历(2)中每个栅格，计算栅格内已构建点云地图点和所有更新点云数据点的高度差，如果栅格内无已构建点云地图数据，但是有更新点云数据，并且更新次数超过设定阈值N_thr-1，则该栅格内更新点云数据确定为新路径对应的地图点，将该栅格内点云数据叠加到已有点云地图中，实现新路径点云地图维护；如果栅格内已构建点云地图数据与更新数据的最大高度差绝对值ΔH大于设定阈值H_thr的次数大于设定阈值N_thr-2，则该栅格内点云地图数据用满足条件的更新数据计算平均值替代，实现点云地图的更新维护。

本发明与现有技术相比所具有的有益效果：

(1)本发明实施例通过点云深度学习语义分割算法，实现了建图场景中动态目标滤除，确保构建的地图场景为静态场景，保证构建地图的准确性；

(2)本发明实施例实现了点云地图的维护功能，在自动驾驶车辆行驶到无地图场景或已有地图场景发生变化时，会自动新构建局部地图，并将局部地图上传服务器，服务器会根据变化区域点云更新次数判断该区域地图是否需要更新，实现点云地图及时更新；

(3)本发明实施例以自动驾驶车辆采集点云地图数据，无需特殊地图采集车辆，点云地图构建成本低。

附图说明

图1为本发明所述的点云地图构建流程图；

图2为本发明所述的点云地图车端维护流程图；

图3为本发明所述的点云地图服务器端维护流程。

具体实施方式

下面结合附图和具体实施例，对本发明进行详细描述。

本发明提供了一种面向非结构化道路的点云地图构建与维护方法，包括以下步骤：

S1：点云地图构建，图1所示为本发明所述的点云地图构建流程，该流程具体包括如下子步骤：

(1)部署RTK GPS/IMU的组合导航系统、多线激光雷达的自动驾驶车辆，按照行驶路径，同时采集GPS、IMU和激光雷达点云数据。激光雷达实时输出的点云数据，通过点云深度学习语义分割，将点云分割为不同类别点，类别在语义分割模型训练时设定，如包括卡车、轿车、行人、地面、其他类别等。根据语义分割的类别，滤除卡车、轿车、行人等动态目标点，剩余地面、其他类别等静态目标点；

(2)组合导航系统输出车辆坐标系定位位姿P_B，根据车辆坐标系和激光雷达坐标系的标定参数(根据其他已有标定算法标定)旋转矩阵R^L _B和平移矩阵T^L _B，将车辆坐标系定位位姿P_B转换到激光雷达坐标系下，得到激光雷达坐标原点定位位姿P_L，如公式(1)所示：

P_L＝R^L _BP_B+T^L _B (1)

(3)过滤动态目标的点云，以激光雷达坐标原点定位位姿为初始值，以匹配算法(NDT算或ICP算法)将过滤动态目标点云与构建中的点云地图匹配，得到匹配矩阵，将过滤动态目标的点云通过匹配矩阵变换到全局点云地图坐标系下，并叠加到点云地图坐标系中，实现点云地图构建；

(4)完成所有行驶路径点云地图构建后，人工校验构建的点云地图准确性，得到点云地图，上传至点云地图服务器。

S2：车端点云地图维护，图2所示为本发明所述的自动驾驶车辆实时运行时点云地图维护流程，该流程具体包括如下子步骤：

(1)自动驾驶车辆启动时，从点云地图服务器下载点云地图；

(2)采用图1中流程(1)相同流程，获取过滤动态目标后的实时点云；

(3)采用图1中流程(2)相同流程，获取激光雷达坐标原点定位位姿；

(4)根据激光雷达坐标原点定位位姿和设置的检测半径R，从已构建的点云地图中提取局部点云地图；

(5)根据车辆坐标系和激光雷达坐标系的标定参数，将局部点云地图变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图；

(6)将过滤动态目标后的实时点云和激光雷达坐标系下局部点云地图，经过匹配算法匹配(如NDT或ICP匹配算法)；

(7)将匹配后的实时点云与局部点云地图，投影到栅格地图中，栅格大小和范围根据需求设定(如果栅格大小为0.5m，纵向栅格个数为100，横向栅格个数为100，对应纵向检测范围为0～50m，横向检测范围为-25～25m)；

(8)遍历每个栅格，如果栅格内有实时点云、无局部地图点云，则该区域内点云为新路径对应点云，如果栅格内实时点云与局部地图点云高度差大于设定阈值(如高度差阈值设置为0.3m)，则该区域内点云为点云地图变化点云。将新路径对应点云或点云地图变化点云，根据激光雷达原点的全局位姿，变换到点云地图坐标系下，并上传至点云地图服务器。

S3：服务器端点云地图维护，图3所示为本发明所述的点云地图在服务器端的维护流程，该流程具体包括如下子步骤：

(1)将点云地图服务器中的已有点云地图和步骤S2的更新的点云数据投影到栅格地图中，栅格范围根据已构建点云地图和更新的点云数据范围确定，栅格分辨率根据需求设定(如设置为0.1m)；

(2)遍历(1)中每个栅格，删除更新的点云数据时间早于设定阈值的数据(如时间阈值为1天)，保留剩余更新数据，确保更新数据的实时性；

(3)遍历(2)中每个栅格，计算栅格内已构建点云地图点和所有更新点云数据点的高度差(更新数据点，包括多台自动驾驶车、多次经过相同位置上传的更新数据点)，如果栅格内无已构建点云地图数据，但是有更新点云数据，并且更新次数超过设定阈值N_thr-1(如N_thr-1＝100),则该栅格内更新点云数据确定为新路径对应的地图点，将该栅格内点云数据叠加到已有点云地图中，实现新路径点云地图维护。如果栅格内已构建点云地图数据与更新数据的最大高度差绝对值ΔH大于设定阈值H_thr(如0.3m)的次数大于设定阈值N_thr-2(如N_thr-2＝100)，则该栅格内点云地图数据用满足条件的更新数据计算平均值替代，实现点云地图变换的更新维护。

通过本实施例所述方法，实现了点云地图的构建与维护，在构建和维护点云地图时可以滤除动态目标，点云地图维护方法，实现点云地图自动、高效维护。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种面向非结构化道路的点云地图构建与维护方法，其特征在于，包括以下步骤：

S1，点云地图构建：将激光雷达实时输出的点云数据分割为动态目标点和静态目标点，过滤动态目标点的点云，将静态目标点的点云从激光雷达坐标系变换到点云地图坐标系，得到点云地图，上传至点云地图服务器；

S2，车端点云地图维护：车辆实时运行，从点云地图服务器下载点云地图，提取局部点云地图，获取实时点云，将局部点云地图变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图，将匹配后的实时点云与局部点云地图投影到格栅地图，对比栅格地图内的局部点云地图和实时点云，实现新路径点云地图更新维护和/或已有地图场景变化更新维护；

S3，服务器端点云地图维护：将点云地图服务器中的已有点云地图和步骤S2得到的车端维护的地图，投影到栅格地图，根据栅格内地图更新次数和高度差，判断每个栅格内地图是否更新，实现服务器端点云地图维护。

2.根据权利要求1所述的面向非结构化道路的点云地图构建与维护方法，其特征在于，所述步骤S1包括以下子步骤：

(1)自动驾驶车辆的激光雷达实时输出点云数据，通过点云深度学习语义分割，将点云分割为动态目标点或静态目标点；

(2)组合导航系统输出车辆坐标系定位位姿P_B，根据车辆坐标系和激光雷达坐标系的标定参数旋转矩阵R^L _B和平移矩阵T^L _B，将车辆坐标系定位位姿P_B转换到激光雷达坐标系下，得到激光雷达坐标原点定位位姿P_L，如公式(1)所示：

P_L＝R^L _BP_B+T^L _B (1)

(3)过滤动态目标点的点云，以激光雷达坐标原点定位位姿为初始值，以匹配算法将过滤动态目标点的点云与构建中的点云地图匹配，其中第一帧点云不用匹配，得到匹配矩阵，将过滤动态目标点的点云通过匹配矩阵变换到点云地图坐标系下，并叠加到点云地图坐标系中，实现点云地图构建；

(4)完成所有行驶路径点云地图构建后，人工校验构建的点云地图准确性，得到点云地图，上传至点云地图服务器。

3.根据权利要求2所述的面向非结构化道路的点云地图构建与维护方法，其特征在于，所述步骤S2包括以下子步骤：

(1)自动驾驶车辆启动时，从点云地图服务器下载点云地图；

(2)自动驾驶车辆实时运行，获取过滤动态目标点后的实时点云；

(3)自动驾驶车辆实时运行，获取激光雷达坐标原点定位位姿；

(4)根据激光雷达坐标原点定位位姿和设置的检测半径R，从已构建的点云地图中提取局部点云地图；

(5)根据车辆坐标系和激光雷达坐标系的标定参数，将局部点云地图变换到激光雷达坐标系下，得到激光雷达坐标系下局部点云地图；

(6)将过滤动态目标点后的实时点云和激光雷达坐标系下局部点云地图，经过匹配算法匹配；

(7)将匹配后的实时点云与局部点云地图，投影到栅格地图中，栅格大小和范围根据需求设定；

(8)遍历每个栅格，如果栅格内有实时点云、无局部地图点云，则该区域内点云为新路径对应点云，如果栅格内实时点云与局部地图点云高度差大于设定阈值，则该区域内点云为点云地图变化点云；将新路径对应点云或点云地图变化点云，根据激光雷达原点的全局位姿，变换到点云地图坐标系下，并上传至点云地图服务器。

4.根据权利要求3所述的面向非结构化道路的点云地图构建与维护方法，其特征在于，所述步骤S3包括以下子步骤：

(1)将点云地图服务器中的已有点云地图和步骤S2的更新的点云数据投影到栅格地图中，栅格范围根据已构建点云地图和更新的点云数据范围确定，栅格分辨率根据需求设定；

(2)遍历(1)中每个栅格，删除更新的点云数据时间早于设定阈值的数据，保留剩余更新数据，确保更新数据的实时性；

(3)遍历(2)中每个栅格，计算栅格内已构建点云地图点和所有更新点云数据点的高度差，如果栅格内无已构建点云地图数据，但是有更新点云数据，并且更新次数超过设定阈值N_thr-1，则该栅格内更新点云数据确定为新路径对应的地图点，将该栅格内点云数据叠加到已有点云地图中，实现新路径点云地图维护；如果栅格内已构建点云地图数据与更新数据的最大高度差绝对值ΔH大于设定阈值H_thr的次数大于设定阈值N_thr-2，则该栅格内点云地图数据用满足条件的更新数据计算平均值替代，实现点云地图的更新维护。

Images