基于压缩三维空间点云的平面激光SLAM与导航方法技术

本发明专利技术涉及基于压缩三维空间点云的平面激光SLAM与导航方法包括：建立坐标系并获取三维点云数据，将三维点云数据压缩至二维平面，得到平面点云数据；基于平面点云数据，进行平面激光SLAM建图及定位、路径规划与导航控制。通过三维激光雷达采集作业环境的点云信息，结合移动机器人的三维几何尺寸约束，将三维激光点云压缩至二维平面，得到包含三维空间信息的平面点云，以平面点云作为平面激光SLAM算法的原始点云数据，实现果园等户外场景的平面SLAM建图，使用该地图规划的全局路径为安全可行的路径，使用该平面点云作为导航控制算法的原始点云数据，实现移动机器人在地图中的实时定位以及在路径跟踪过程中的实时障碍探测与避障。以及在路径跟踪过程中的实时障碍探测与避障。以及在路径跟踪过程中的实时障碍探测与避障。

【技术实现步骤摘要】
基于压缩三维空间点云的平面激光SLAM与导航方法


[0001]本专利技术属于激光SLAM建图与导航
，尤其涉及一种适用于果园等户外场景的SLAM建图与导航方法。

技术介绍

[0002]同时建图与定位(Simultaneous Localization and Mapping，SLAM)是移动机器人在一个未知环境中进行定位与建图的关键技术，也为未来一切自动化技术(如智慧农业、无人农场)提供核心支持。
[0003]目前，以平面激光雷达为主的室内平面激光SLAM技术较为成熟，已广泛应用于扫地机器人、仓储机器人、服务机器人等领域。果园等户外场景具有三维结构特征明显、环境开放、障碍复杂等特点，传统的仅依靠平面激光雷达的室内平面SLAM技术显得不够适用，但平面激光SLAM具有运算量低、实时性高等特点。
[0004]如何结合三维激光雷达环境感知丰富、平面SLAM算法运算量低、实时性高等特点，实现三维激光雷达的平面SLAM建图与导航显得尤为重要。

技术实现思路

[0005]针对上述技术问题，本专利技术的目的在于使用三维激光雷达作为环境感知传感器，将三维空间点云压缩至二维平面，为平面激光导航中的SLAM建图模块、定位模块及导航控制模块提供平面原始点云数据，实现三维激光雷达的平面SLAM建图与导航。
[0006]为了实现上述目的，本专利技术提供了如下技术方案：
[0007]基于压缩三维空间点云的平面激光SLAM与导航方法，包括如下步骤：
[0008]S1.建立坐标系并获取三维点云数据，将三维点云数据压缩至二维平面，得到平面点云数据；
[0009]S2.基于步骤S1得到的平面点云数据，进行平面激光SLAM建图；
[0010]S3.基于步骤S1得到的平面点云数据，进行定位、路径规划与导航控制。
[0011]在上述方案的基础上，步骤S1具体包括：
[0012]S1.1.建立移动机器人坐标系{v}与激光雷达坐标系{l}，获取基础数据；
[0013]S1.2.基于兴趣区域选择原则，得到三维空间点云在不同维度的取值范围；
[0014]S1.3.对三维空间点云进行压缩，得到平面点云数据。
[0015]在上述方案的基础上，步骤S1.1所述的建立移动机器人坐标系{v}与激光雷达坐标系{l}，具体包括如下步骤：
[0016]以移动机器人主体中心为原点o
v
，x
v
轴指向移动机器人正前方，y
v
轴平行于轮轴指向左侧，z
v
轴垂直指向上方，建立满足右手定则的移动机器人坐标系o
v
x
v
y
v
z
v
，记为{v}；
[0017]建立三维激光雷达所在坐标系o
l
x
l
y
l
z
l
，记为{l}，其原点o
l
位于激光雷达中心，且x
l
、y
l
、z
l
轴分别平行于x
v
、y
v
、z
v
轴；
[0018]在上述方案的基础上，步骤S1.1所述的基础数据包括：
[0019]记第k帧三维原始点云数据为P
k
，所述第k帧三维原始点云数据为通过三维激光雷达直接获取的三维点云数据；
[0020]o
l
与o
v
在x
v
轴方向的距离为Δx
v2l
m，在y
v
轴方向的距离为0，在z
v
轴方向的距离为Δz
v2l
m；
[0021]移动机器人主体的下表面至地面的垂直距离(即底盘离地间隙)为h
vb
m，原点o
l
至地面的垂直距离为h
l
m，记移动机器人的最高点至地面的垂直距离为h
v
‑
max
m。
[0022]在上述方案的基础上，S1.2具体包括：
[0023]兴趣区域(Region of Interest，ROI)的选择主要为了精确感知范围，排除地面干扰，实现原始点云的粗滤波；ROI的选择包括两个维度：水平与竖直；
[0024]兴趣区域的选择原则包括：对于水平维度，为了实现360
°
的全方位感知，水平维度的点云信息全部保留；对于竖直维度，引入选择阈值h
th
，h
th
≥0m，在激光雷达坐标系{l}中，考虑到移动机器人底盘最低处的通过能力，要求激光雷达既能感知到底盘主体最低处，又能排除地面干扰，故z
l
轴负方向取
‑
(h
l
‑
h
vb
+h
th
)m，考虑移动机器人的最高点能通过作业环境，故z
l
轴正方向取(h
v
‑
max
‑
h
l
+h
th
)m；
[0025]在上述方案的基础上，步骤S1.3具体包括：
[0026]使用开源算法pointcloud_to_laserscan实现对原始点云数据的压缩，得到平面点云数据。
[0027]在上述方案的基础上，开源算法pointcloud_to_laserscan中的主要参数包括：min_height，max_height，angle_min，angle_max，angle_increasement，range_min，range_max，cloud_in，laser；
[0028]针对截取的点云而言，min_height表示最小高度，单位m；max_height表示最大高度，单位m；angle_min表示最小角度，单位rad；angle_max表示最大角度，单位rad；angle_increasement表示角度步长，单位rad；range_min表示最小距离，单位m；range_max表示最大距离，单位m；cloud_in表示原始输入点云话题，消息格式为(sensor_msgs/PointCloud2)，laser表示压缩后的平面点云话题，消息格式为(sensor_msgs/LaserScan)；
[0029]由S1.2中的兴趣区域选择原则依次设置上面的参数为：min_height：
‑
(h
l
‑
h
vb
+h
th
)，max_height：(h
v
‑
max
‑
h
l
+h
th
)，angle_min：
‑
π，angle_max：π，angle_increasement：0.001，r本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于压缩三维空间点云的平面激光SLAM与导航方法，其特征在于，包括如下步骤：S1.建立坐标系并获取三维点云数据，将三维点云数据压缩至二维平面，得到平面点云数据；S2.基于步骤S1得到的平面点云数据，进行平面激光SLAM建图；S3.基于步骤S1得到的平面点云数据，进行定位、路径规划与导航控制。2.如权利要求1所述的基于压缩三维空间点云的平面激光SLAM与导航方法，其特征在于，步骤S1具体包括：S1.1.建立移动机器人坐标系{v}与激光雷达坐标系{l}，获取基础数据；S1.2.基于兴趣区域选择原则，得到三维空间点云在不同维度的取值范围；S1.3.对三维空间点云进行压缩，得到平面点云数据。3.如权利要求2所述的基于压缩三维空间点云的平面激光SLAM与导航方法，其特征在于，步骤S1.1所述的建立移动机器人坐标系{v}与激光雷达坐标系{l}，具体包括如下步骤：以移动机器人主体中心为原点o
v
，x
v
轴指向移动机器人正前方，y
v
轴平行于轮轴指向左侧，z
v
轴垂直指向上方，建立满足右手定则的移动机器人坐标系o
v
x
v
y
v
z
v
，记为{v}；建立三维激光雷达所在坐标系o
l
x
l
y
l
z
l
，记为{l}，其原点o
l
位于激光雷达中心，且x
l
、y
l
、z
l
轴分别平行于x
v
、y
v
、z
v
轴。4.如权利要求3所述的基于压缩三维空间点云的平面激光SLAM与导航方法，其特征在于，步骤S1.1.所述的基础数据包括：记第k帧三维原始点云数据为P
k
，所述第k帧三维原始点云数据为通过三维激光雷达直接获取的三维点云数据；o
l
与o
v
在x
v
轴方向的距离为Δx
v2l
m，在y
v
轴方向的距离为0，在z
v
轴方向的距离为Δz
v2l
m；移动机器人主体的下表面至地面的垂直距离为h
vb
m，原点o
l
至地面的垂直距离为h
l
m，记移动机器人的最高点至地面的垂直距离为h
v
‑
max
m。5.如权利要求4所述的基于压缩三维空间点云的平面激光SLAM与导航方法，其特征在于，S1.2具体包括：兴趣区域的选择包括两个维度：水平与竖直；兴趣区域的选择原则包括：对于水平维度，为了实现360
°
的全方位感知，水平维度的点云信息全部保留；对于竖直维度，引入选择阈值h
th
，h
th
≥0m，在激光雷达坐标系{l}中，考虑到移动机器人底盘最低处的通过能力，要求激光雷达既能感知到底盘主体最低处...

【专利技术属性】
技术研发人员：郑永军，刘伟洪，李文伟，许家伟，苏道毕力格，刘星星，王子蒙，
申请(专利权)人：中国农业大学，
类型：发明
国别省市：