一种八叉树地图构建方法及系统与流程

1.本发明涉及移动机器人三维地图构建技术领域，特别是涉及一种八叉树地图构建方法及系统。


背景技术：

2.随着应用场景和任务的要求，二维地图已经不能满足任务要求，三维地图更能够详细描述三维空间环境，因此对三维地图的需求也越来越多，三维地图的应用不仅用于移动机器人的定位，还需要其他的应用需求，比如三维重建，导航，避障等。三维地图的表现形式主要有点云和八叉树两种。其中，点云地图构建方式主要有3d激光slam、视觉slam、2d激光与视觉结合的slam。其中，视觉slam对光照和环境敏感，对计算机算力要求高，且计算位姿不如激光精确。3d激光slam成本高昂，且需要对大量点云进行匹配等操作，对计算机性能要求高。相比于视觉和3d激光构建点云地图的方法，2d激光与相机结合的方式更合适，2d激光图优化slam的可靠性和精度更高，例如cartographer和karto图优化slam等算法已经成为了较为成熟的2d激光的建图算法，更适用于日常环境和大场景建图等。在2d激光图优化slam基础上，采用相机的点云信息进行点云地图构建更具有实际意义。
3.但三维点云地图是稀疏的，移动机器人并不能知道三维点云地图是否可以通过。同时点云地图还提供了不必要的细节。例如窗帘的褶皱等我们不需要关系的细节，把这种信息放在地图里浪费空间。因此需要转换为一种压缩的、可更新的和分辨率可调的八叉树地图。


技术实现要素：

4.为了解决上述问题，本发明提供了一种八叉树地图构建方法及系统。
5.为实现上述目的，本发明提供了如下方案：
6.一种八叉树地图构建方法，包括：
7.根据激光雷达和里程计数据，采用激光图优化slam算法确定关键帧以及关键帧下的移动机器人位姿；
8.采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
9.将坐标转换后的点云信息进行拼接，得到三维点云地图；
10.基于三维点云地图，对激光图优化slam算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
11.对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
12.将滤波后的三维点云地图转化为八叉树地图。
13.可选地，采用激光图优化slam算法确定关键帧，具体包括：
14.若当前帧为第一帧，则不进行扫描匹配；
15.若当前帧不是第一帧，则判断当前帧与上一关键帧的采集时间间隔、移动距离或航向角的差值是否超过设定的阈值；
16.若是，则判断当前帧为关键帧；
17.若否，则舍弃当前帧。
18.可选地，所述将点云数据在相机坐标系下的坐转换到世界坐标系下，具体包括：
19.获取相机坐标系与移动机器人坐标系的位姿转换关系；
20.根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
21.可选地，所述点云坐标转换公式为：
[0022][0023]
其中，[x
w
,y
w
,z
w
]表示转换后点云坐标，表示移动机器人位姿，表示相机坐标系与移动机器人坐标系的位姿转换关系，[x
c
,y
c
,z
c
]表示点云数据在相机坐标系下的坐标，w表示世界坐标系，r表示机器人坐标系，c表示相机坐标系。
[0024]
可选地，所述后端优化的过程如下：
[0025]
设c＝(c1,c2,...,c
n
)是需要优化的节点的位姿，因移动机器人在二维平面内运动，所以有：
[0026][0027]
其中，表示第i节点处机器人在地图中的位置，θ
i
表示第i节点处机器人在地图中的方向角；t表示矩阵的转置；
[0028]
第i个节点和第j个节点之间建立的边约束所对应的相对变换关系为：
[0029][0030]
r
i
为与θ
i
相关的余弦公式矩阵：
[0031][0032]
当前边的误差函数e
ij
表示为：
[0033][0034]
其中，为观测量，h
ij
为期望值；
[0035]
总误差函数f(c)表示为：
[0036][0037]
式中，ω
ij
为表示边误差所占权重的信息矩阵；f
ij
表示第i节点和第j节点之间的误差；
[0038]
极小化总误差函数消除累积误差，得到更新后的机器人位姿c*，从而构建最优的地图：
[0039][0040]
本发明还提供了一种八叉树地图构建系统，包括：
[0041]
关键帧以及关键帧下的移动机器人位姿确定模块，用于根据激光雷达和里程计数据，采用激光图优化slam算法确定关键帧以及关键帧下的移动机器人位姿；
[0042]
坐标转换模块，用于采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
[0043]
拼接模块，用于将坐标转换后的点云信息进行拼接，得到三维点云地图；
[0044]
回环检测和后端优化模块，用于基于三维点云地图，对激光图优化slam算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
[0045]
滤波模块，用于对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
[0046]
转化模块，用于将滤波后的三维点云地图转化为八叉树地图。
[0047]
可选地，所述坐标转换模块具体包括：
[0048]
位姿转换关系获取单元，用于获取相机坐标系与移动机器人坐标系的位姿转换关系；
[0049]
坐标转换单元，用于根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
[0050]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0051]
1)本发明基于激光图优化slam算法获取关键帧位姿，相对于视觉图优化slam，精度更高，对计算资源要求更低，且理论充分，实际可行；
[0052]
2)拼接形成的点云地图，不需要复杂的匹配计算，通过坐标转换即可实现拼接得到点云地图；
[0053]
3)最终的八叉树地图易于存储，占用内存空间更小，灵活方便，可用于后续的移动机器人导航等。
附图说明
[0054]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0055]
图1为本发明实施例八叉树地图构建的流程图；
[0056]
图2为本发明实施例仿真实验平台；
[0057]
图3为本发明实施例仿真实验环境；
[0058]
图4为本发明实施例所用2d激光图优化slam算法流程图；
[0059]
图5为本发明实施例所用2d激光图优化slam算法中回环检测示意图；
[0060]
图6为本发明实施例处理点云地图离群点所用半径滤波；
[0061]
图7为本发明实施例构建的无颜色八叉树地图；
[0062]
图8为本发明实施例构建的有颜色八叉树地图。
具体实施方式
[0063]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0064]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0065]
如图1所示，本发明提供的一种八叉树地图构建方法，包括以下步骤：
[0066]
步骤1：根据激光雷达和里程计数据，采用激光图优化slam算法确定关键帧以及关键帧下的移动机器人位姿。
[0067]
控制移动机器人运动并根据传感器的输入运行激光图优化slam算法，过程中，不断根据激光图优化slam算法关键帧选取规则筛选关键帧，计算并保存slam计算出的此刻移动机器人世界坐标系下位姿索引。
[0068]
本实施例中，使用安装hokuyo二维激光雷达和kinect相机的两轮驱动移动机器人turtlebot作为实验平台，如图2所示。采用gazebo仿真环境进行仿真建模，如图3所示。基于2d激光图优化slam算法karto图优化slam，输入传感器原始数据，运行得到karto图优化slam算法的激光关键帧信息和移动机器人的位姿信息，并同时触发kinect相机采集点云数据。
[0069]
数据的采集与2d图优化slam算法处理：通过键盘控制turtlebot在仿真环境中移动，运动时同时采集imu、里程计和激光雷达原始数据并传给2d激光karto图优化slam算法，算法的流程如图4所示，此时根据激光图优化slam的关键帧筛选要求：第一，若当前帧为第一帧，则不用进行扫描匹配；第二，判断当前帧数据采集时间与上一关键帧采集时间间隔、移动距离、航向角的差值是否超过设定的阈值，若满足其中条件之一，则判断为关键帧，若不满足，则舍弃此帧，继续判断下一帧是否为关键帧。
[0070]
在判断为关键帧，得到关键帧下的移动机器人位姿，保存此刻的位姿索引值便于后续的更新。
[0071]
步骤2：采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下。
[0072]
触发kinect相机采集此时的环境点云信息并进行实时的坐标转换，过程如下：
[0073]
根据在图优化slam算法中保存的turtlebot机器人在世界坐标系的位姿信息、相机坐标系下点云坐标，以及相机在机器人坐标系下的坐标，可以实现将点云坐标转换到世界坐标系下，进行点云拼接，获得三维点云地图。
[0074]
kinect相机安装于移动机器人上方固定位置处，可以通过测量直接得出相机坐标系与移动机器人坐标系的位姿转换关系。kinect相机可以直接采集到点云数据在相机坐标系下的坐标([x
c
,y
c
,z
c
])，根据步骤1的中得到的激光关键帧下的移动机器人在世界坐标系下的位姿以及kinect相机与移动机器人的坐标转换关系经过一系列的坐标转换可以得到三维点云在世界坐标系下的坐标([x
w
,y
w
,z
w
])。由于经过筛选后的激光关键帧可以代表某一时间段某一区域的环境特征，因此相对应的，当前关键帧的移动机器人位姿下采集的kinect点云数据可以实现对当前区域的完全表示而没有过多的重叠。具体坐标
变换的公式为：
[0075][0076]
其中，其中，和是3
×
3的旋转矩阵，和是3
×
1的位移矩阵。
[0077]
步骤3：将坐标转换后的点云信息进行拼接，得到三维点云地图。对采集的所有帧点云实时的进行上述转换，可逐步完成所有点云在世界坐标系下的拼接，构建实时点云地图。
[0078]
步骤4：基于三维点云地图，对激光图优化slam算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图。
[0079]
通过观测实时拼接的点云地图，直到机器人在环境中运动扫描形成闭环。此时，激光图优化slam回环检测和后端优化结束，更新对应索引的机器人位姿，从而更新转换的点云地图。
[0080]
实时的拼接查看点云地图便于控制移动机器人的运动轨迹和方向，构建一个充分完整的点云地图。同时，由于误差的累积，激光图优化slam前端匹配之后的位姿与真实的位姿存在误差，因此需要回环检测和后端优化来消除误差，所以，可以控制turtlebot在仿真环境中来回移动后回到出发位置形成闭环约束。体现在图4算法中为：若机器人回到原来的位置，则会回环检测成功，经过图4所示的回环检测和后端优化流程，最终输出得到用于构建地图的当前关键帧下的优化后的移动机器人位姿。其中，回环检测示意图如图5所示，以半径r为搜索半径范围，并判断搜索到的节点链中的位姿节点数量是否满足阈值要求，若满足则回环检测成功，继续进行后端优化，修正位姿。
[0081]
设c＝(c1,c2,...,c
n
)是需要优化的节点的位姿，因移动机器人在二维平面内运动，所以有
[0082][0083]
其中，表示第i节点处机器人在地图中的位置，θ
i
表示第i节点处机器人在地图中的方向角；t表示矩阵的转置。第i个节点和第j个节点之间建立的边约束所对应的相对变换关系为：
[0084][0085]
r
i
为与θ
i
相关的余弦公式矩阵：
[0086][0087]
这条边的误差函数可表示为：
[0088][0089]
其中，为观测量，h
ij
为期望值。所有边约束之和，即总误差函数可表示为：
[0090][0091]
式中，ω
ij
为表示边误差所占权重的信息矩阵，即c
i
和c
j
之间协方差矩阵的逆，信息权重越大，表示测量越准确。f
ij
表示第i节点和第j节点之间的误差。极小化总误差函数可消除累积误差，得到最可能的机器人位姿c*，从而构建最优的地图：
[0092][0093]
图优化问题转变为了非线性最小二乘问题，对于目标函数的求解，目前常用的方法是高斯牛顿、levenberg
‑
marquart(lm)等算法。优化后根据步骤1中保存的所有的位姿索引的更新位姿对之前的点云坐标转换进行更新，从而更新点云地图。
[0094]
步骤5：对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点。
[0095]
滤波方法可以采用的方法有统计滤波器或半径滤波器等。本例中使用半径滤波器，通过设定滤波半径d，计算每个点在其半径范围内的其他点的个数。半径范围内少于某一设定阈值a的点将被滤除。如图6所示，半径d内少于2个的点将被删除。针对本例，经过试验设定d＝18cm，a＝3时效果最佳。
[0096]
步骤6：将滤波后的三维点云地图转化为八叉树地图。
[0097]
利用八叉树地图构建库，将滤波后的三维点云地图转换为八叉树地图。优选的，如果需要将三维点的rgb颜色信息在八叉树地图中显示，给八叉树地图添加颜色信息，可进行进一步的处理。
[0098]
安装编译octomap库，将保存的pcd点云文件转换为octomap文件。此时octomap里只存储了点的空间信息，没有颜色信息，如图7所示，在rviz软件中进行显示。由于pcd点云文件中存储有点的rgb颜色信息，因此，如果需要给octomap添加颜色，则可以利用octomap库中提供的coloroctree类中的integratenodecolor函数，将转换后的八叉树地图赋予彩色信息。然后再将带有颜色信息的八叉树地图保存为.ot文件。显示结果如图8所示。此时，转换完成的八叉树地图可用于导航等任务中。
[0099]
本发明提供的方法主要包括五个部分：基于激光图优化slam算法的关键帧提取与相机点云数据采集；点云在相机坐标系与世界坐标系的位姿转换与拼接实时获取三维点云地图；激光图优化slam回环检测与后端优化，更新移动机器人位姿；点云地图的离群点滤波处理；点云地图转换为八叉树地图。第一部分，关键帧提取通过设置时间、距离、角度阈值来选取，关键帧选取成功后采集该时刻相机的点云数据。第二部分，由于相机与移动机器人安装位置固定，且机器人在世界坐标系下的位姿通过激光图优化slam算法可得，因此可将点云在相机坐标系的坐标转换为在世界坐标系的坐标。通过控制移动机器人在环境中运动，上述算法可持续运行，从而完成多个关键帧的点云拼接，实时构建点云地图。第三部分，通过后续的匹配和后端优化等算法得到优化后的关键帧对应的机器人位姿以及更新后的点云地图并保存。第四部分，鉴于相机点云数据离群点数据相对不准确，影响地图精度和美观性，因此，采用了滤波算法对离群点进行去除。第五部分，相对于八叉树地图，点云地图提供
了大量不需要的细节，因此占据大量的存储空间，显示时对造成地图的明显重叠，难以用于导航等。因此需要将点云地图转化为八叉树地图。
[0100]
本发明基于激光图优化slam筛选关键帧并计算和优化位姿，方法简单，地图构建快速，在现实中机器人平台中验证了该方法的可靠性，仅依靠激光图优化slam和相机采集到点云信息拼接得到的三维点云地图是稀疏的，机器人不能根据点云地图知道哪里可以到达，哪里不能到达。同时点云地图中还包含很多不必要的信息，比如窗帘的褶皱等，点云地图的保存占有了大量的内存，将点云保存成八叉树地图能够节省内存，并且八叉树地图可以描述机器人在地图中的可达性。
[0101]
本发明还提供了一种八叉树地图构建系统，包括：
[0102]
关键帧以及关键帧下的移动机器人位姿确定模块，用于根据激光雷达和里程计数据，采用激光图优化slam算法确定关键帧以及关键帧下的移动机器人位姿；
[0103]
坐标转换模块，用于采集环境点云信息，并将点云数据在相机坐标系下的坐转换到世界坐标系下；
[0104]
拼接模块，用于将坐标转换后的点云信息进行拼接，得到三维点云地图；
[0105]
回环检测和后端优化模块，用于基于三维点云地图，对激光图优化slam算法进行回环检测和后端优化，得到更新后的移动机器人位姿和更新后的三维点云地图；
[0106]
滤波模块，用于对更新后的三维点云地图进行滤波，去除更新后的三维点云地图的离群点；
[0107]
转化模块，用于将滤波后的三维点云地图转化为八叉树地图。
[0108]
其中，所述坐标转换模块具体包括：
[0109]
位姿转换关系获取单元，用于获取相机坐标系与移动机器人坐标系的位姿转换关系；
[0110]
坐标转换单元，用于根据所述移动机器人位姿和所述位姿转换关系，将点云数据在相机坐标系下的坐转换到世界坐标系下。
[0111]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0112]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。