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

基于压缩三维空间点云的平面激光slam与导航方法
技术领域
1.本发明属于激光slam建图与导航技术领域，尤其涉及一种适用于果园等户外场景的slam建图与导航方法。


背景技术：

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


技术实现要素：

5.针对上述技术问题，本发明的目的在于使用三维激光雷达作为环境感知传感器，将三维空间点云压缩至二维平面，为平面激光导航中的slam建图模块、定位模块及导航控制模块提供平面原始点云数据，实现三维激光雷达的平面slam建图与导航。
6.为了实现上述目的，本发明提供了如下技术方案：
7.基于压缩三维空间点云的平面激光slam与导航方法，包括如下步骤：
8.s1.建立坐标系并获取三维点云数据，将三维点云数据压缩至二维平面，得到平面点云数据；
9.s2.基于步骤s1得到的平面点云数据，进行平面激光slam建图；
10.s3.基于步骤s1得到的平面点云数据，进行定位、路径规划与导航控制。
11.在上述方案的基础上，步骤s1具体包括：
12.s1.1.建立移动机器人坐标系{v}与激光雷达坐标系{l}，获取基础数据；
13.s1.2.基于兴趣区域选择原则，得到三维空间点云在不同维度的取值范围；
14.s1.3.对三维空间点云进行压缩，得到平面点云数据。
15.在上述方案的基础上，步骤s1.1所述的建立移动机器人坐标系{v}与激光雷达坐标系{l}，具体包括如下步骤：
16.以移动机器人主体中心为原点ov，xv轴指向移动机器人正前方，yv轴平行于轮轴指向左侧，zv轴垂直指向上方，建立满足右手定则的移动机器人坐标系ovx
vyvzv
，记为{v}；
17.建立三维激光雷达所在坐标系o
l
x
lylzl
，记为{l}，其原点o
l
位于激光雷达中心，且x
l
、y
l
、z
l
轴分别平行于xv、yv、zv轴；
18.在上述方案的基础上，步骤s1.1所述的基础数据包括：
19.记第k帧三维原始点云数据为pk，所述第k帧三维原始点云数据为通过三维激光雷达直接获取的三维点云数据；
20.o
l
与ov在xv轴方向的距离为δx
v2l
m，在yv轴方向的距离为0，在zv轴方向的距离为δz
v2l
m；
21.移动机器人主体的下表面至地面的垂直距离(即底盘离地间隙)为h
vb
m，原点o
l
至地面的垂直距离为h
l
m，记移动机器人的最高点至地面的垂直距离为h
v-max
m。
22.在上述方案的基础上，s1.2具体包括：
23.兴趣区域(region of interest，roi)的选择主要为了精确感知范围，排除地面干扰，实现原始点云的粗滤波；roi的选择包括两个维度：水平与竖直；
24.兴趣区域的选择原则包括：对于水平维度，为了实现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；
25.在上述方案的基础上，步骤s1.3具体包括：
26.使用开源算法pointcloud_to_laserscan实现对原始点云数据的压缩，得到平面点云数据。
27.在上述方案的基础上，开源算法pointcloud_to_laserscan中的主要参数包括：min_height，max_height，angle_min，angle_max，angle_increasement，range_min，range_max，cloud_in，laser；
28.针对截取的点云而言，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)；
29.由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，range_min：0，range_max：10.0，cloud_in，laser；其中，cloud_in对应的三维点云为s1.1中定义的第k帧三维原始点云数据pk，将laser对应的平面点云数据记为qk；
30.在上述方案的基础上，步骤s2具体包括：
31.根据s1.3中得到的平面点云数据qk，结合当前开源的平面激光slam建图算法进行建图，所述平面激光slam建图算法包括但不限于：cartographer；
32.在上述方案的基础上，步骤s3具体包括：
33.根据s1.3中得到的平面点云数据qk，结合当前开源的平面定位与导航框架move_base，进行定位、路径规划与导航控制。
34.与现有技术相比，本发明的有益效果在于：
35.本发明通过将三维空间点云压缩至二维平面，提供给平面激光导航中的slam建图模块、定位模块以及导航控制模块调用，实现三维激光雷达的平面slam建图与导航。与单纯的平面激光雷达提供的点云数据相比，压缩后的平面点云数据qk，在定位中具有更为丰富
的环境特征信息，大大提升定位的准确性；在路径规划中也参考了移动机器人在环境中竖直方向的障碍信息，规划的全局路径更为准确、安全、可靠；在导航控制中提供了环境竖直方向的障碍信息，可提升本地规划器的障碍识别能力，进而提升任务执行中的安全性。
附图说明
36.本发明有如下附图：
37.图1为本发明的基于压缩三维空间点云的平面激光slam与导航方法的流程图；
38.图2为本发明的硬件平台示意图；
39.图3为本发明的slam建图户外场景林间实景图；
40.图4为本发明的基于压缩三维空间点云的林间平面激光slam样图；
41.图5为本发明的基于单线激光雷达的林间平面激光slam样图。
具体实施方式
42.下面结合附图和实施例对本发明进行进一步说明。
43.如图1所示，本发明的一种基于压缩三维空间点云的平面激光slam与导航方法，包括如下步骤：
44.s1.建立坐标系并获取三维点云数据，将三维点云数据压缩至二维平面，得到平面点云数据；
45.s1.1建立移动机器人坐标系{v}与激光雷达坐标系{l}，获取基础数据；
46.如图2所示，以移动机器人主体中心为原点ov，xv轴指向移动机器人正前方，yv轴平行于轮轴指向左侧，zv轴竖直指向上方，建立满足右手定则的移动机器人坐标系ovx
vyvzv
，记为{v}。建立三维激光雷达所在坐标系为o
l
x
lylzl
，记为{l}，其原点o
l
位于激光雷达中心，且x
l
、y
l
、z
l
轴分别平行于xv、yv、zv轴；
47.所述的基础数据包括：记第k帧三维原始点云数据为pk。o
l
与ov在xv轴方向的距离为δx
v2l
m，在yv轴方向的距离为0，在zv轴方向的距离为δz
v2l
m，移动机器人主体的下表面至地面(图2中车轮下面的实线为地面)的垂直距离(即离地间隙)为h
vb
m，o
l
至地面的垂直距离为h
l
m，记移动机器人的最高点至地面的垂直距离为h
v-max
m。
48.s1.2.基于兴趣区域选择原则，得到三维空间点云在不同维度的取值范围；
49.兴趣区域(region of interest，roi)的选择主要为了精确感知范围，排除地面干扰，实现原始点云的粗滤波。roi的选择主要有两个维度：水平与竖直。对于水平维度，为了实现360
°
的全方位感知，该维度的点云信息全部保留。对于竖直维度，引入选择阈值h
th
(h
th
≥0)m，在{l}系中，考虑到移动机器人底盘最低处的通过能力，要求激光雷达能感知到底盘主体最低处，同时又能排除地面干扰，故z
l
轴负方向取-(h
l-h
vb
h
th
)m，考虑移动机器人的最高点能通过作业环境，故z
l
轴正方向取(h
v-max-h
l
h
th
)m，如图2所示，上下两条虚线分别为竖直维度的上下边界。
50.s1.3.对三维空间点云进行压缩，得到平面点云数据；
51.使用开源算法pointcloud_to_laserscan实现对原始点云数据的压缩，该算法主要参数如下：min_height，max_height，angle_min，angle_max，angle_increasement，range_min，range_max，cloud_in，laser；
52.针对截取的点云而言，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)。由s1.2中的roi选择原则依次设置上面的参数为：-(h
l-h
vb
h
th
)，(h
v-max-h
l
h
th
)，-π，π，0.001，0，10.0，cloud_in，laser。易知cloud_in对应的三维点云为s1.1中定义的第k帧原始点云pk，将laser对应的平面点云记为qk。
53.具体的，这里结合本案例所使用的移动机器人平台的参数，h
l
取0.66m，h
vb
取0.08m，h
v-max
取0.97m，h
th
取0.03m，则参数min_height为-0.61m，max_height为0.34m。
54.s2.基于步骤s1得到的平面点云数据，进行平面激光slam建图；
55.根据s1.3中得到的平面点云数据qk，结合当前开源的平面激光slam建图算法，进行建图。
56.这里slam建图的户外场景以图3所示的林间场景为例；slam建图算法以cartographer为例；使用本文所述的点云压缩方法对16线的三维激光雷达的原始点云数据pk进行压缩得到平面点云数据qk，cartographer以qk作为平面激光点云输入源，实现对图3所示的林间场景的slam建图，结果如图4所示；为了论证本文所述方法同单线激光雷达建图的区别，分别设置s1.3中的参数min_height、max_height为-0.1m、0.1m，其他参数保持不变，即仅保留{l}系垂直区间[-0.1,0.1]内的点云数据pk，实现三维激光雷达到单线激光雷达的点云模拟，使用pk作为cartographer的平面激光点云输入源，实现对图3所示的林间场景的slam建图，结果如图5所示；通过图4、图5的对比，可以看出，结合三维空间点云信息的建图结果(图4)包含更为丰富的环境信息，如图4中的果树保留了枝干的投影信息，而模拟的单线激光雷达的建图结果(图5)仅包含树干的信息，这对后面移动机器人的导航极为不利。
[0057]
s3.基于步骤s1得到的平面点云数据，进行定位、路径规划与导航控制。
[0058]
根据s1.3中得到的平面点云数据qk，结合当前开源的平面定位与导航框架move_base，进行定位、路径规划与导航控制。在move_base导航框架下的定位环节，蒙特卡洛算法(amcl)在特征点明显的地图中能够提升定位的精度，根据s2中所提供的两幅地图结果，显然基于压缩三维空间点云的平面激光slam建图(图4)提供的环境特征相较于基于单线激光雷达的平面激光slam建图(图5)更为丰富，定位精度、鲁棒性会更好；在move_base导航框架下的路径规划环节，图4的环境信息中考虑了移动机器人的几何尺寸的空间约束，使用该地图做全局路径规划时的路径为可行路径，而图5的环境信息仅包含竖直方向单个平面的障碍信息，使用该地图做全局路径规划时的路径具有一定的碰撞风险；在move_base导航框架下的导航控制环节，使用基于压缩三维空间点云的平面点云qk作为局部路径规划的实时点云输入，动态窗口(dwa)算法规划的局部路径将结合多层空间障碍信息生成局部路径，提升了移动机器人对障碍的感知能力，而仅使用单线激光雷达的点云信息作为局部路径规划的实时点云输入，由于该点云仅包含空间竖直方向给定平面的障碍信息，导致移动机器人对空间中竖直维度的障碍感知不足，必将增加移动机器人在导航控制中的碰撞风险。
[0059]
以上实施方式仅用于说明本发明，而并非对本发明的限制。尽管参照实例对本发
明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行各种组合、修改或者等同替换，都无法脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。