0 引言

自主导航是移动机器人执行各项任务的基础和前提。移动机器人实现自主导航需要预先知道所在环境的地图信息，从而可以进行自定位和路径规划。机器人同时定位与地图创建 (Simultaneous Localization And Mapping, SLAM) 技术完成了移动机器人对未知环境的地图构建。然而，传统的SLAM方法多是使用激光或声纳进行二维地图的创建^[1-4]。随着机器人移动空间的拓展，出现了空中机器人和水下机器人，二维地图已经不能满足这类机器人导航的需要。即使是地面移动机器人，有些较为高大的机器人也需要三维地图来判断环境的空间结构，从而进行精确的导航。因此，三维地图的精确建立引起了广大学者的关注^[5-8]。

目前常用的三维地图表示方法有点云地图 (Point cloud map)^[9-10]、高程地图 (Elevation map)^[11-12]和立体占用地图 (Volumetric occupancy map)^[13-15]等。其中：点云地图存储了所有的空间点坐标，其对硬盘和内存的消耗均较大，且对于机器人来说不易区分障碍和空闲的区域，难以用于机器人导航。高程地图只存储每一栅格的表面高度，有效克服了点云地图高消耗的缺点，但其无法表示环境中的复杂结构，因此多适用于室外导航。立体占用地图多是基于八叉树构建的，其类似于二维地图中的栅格地图，使用立方体的状态 (空闲，占用，未知) 来表示该立方体中是否有障碍。其中，OctoMap^[16-17]作为一种基于八叉树的地图表示方法，建立了体素的占用概率模型，提高了地图的表示精度，且其地图压缩方法极大地减少了地图对内存和硬盘的需求，代表了当前三维地图表示方法中的较高水准。Schauwecker等^[18]在OctoMap的基础上提出了一种基于可见性模型和传感器深度误差模型的鲁棒OctoMap并应用于基于双目立体相机的SLAM中，有效克服了传感器深度误差对地图精度的影响，并通过与OctoMap比较体现了所提方法的有效性; 然而，鲁棒OctoMap的可见性模型未考虑相机和目标体素的相对位置关系，具有一定的局限性，会导致建图精度不高。

本文针对鲁棒OctoMap可见模型的局限性，提出一种基于完整可见性模型的改进鲁棒OctoMap并将其应用于基于Kinect的RGB-D SLAM中。首先，考虑相机和目标体素的相对位置关系及地图分辨率进行可连通性判断，从而获得满足可连通性的相邻体素的个数及位置；其次，针对可连通性的不同情况，分别建立相机到目标体素的可见性模型，从而构建普适性更强的完整可见性模型；再次，使用基于高斯混合模型的Kinect深度误差模型代替文献[18]中的简单深度误差模型，进一步提高地图精度；最后，结合贝叶斯公式和线性插值算法来更新八叉树中每个节点的实际占用概率，最终构建基于八叉树的立体占用地图。对比实验表明，本文方法的建图精度较鲁棒OctoMap有明显的提高。

1 鲁棒OctoMap可见性模型及其局限性

对于立体双目摄像头和Kinect等传感器，其深度值均具有较大的误差，对所建地图精度影响较大。在基于八叉树的地图中，部分区域由于遮挡造成不可见，因此这些区域的所有测量值均是有误差的。针对这两种情况，Schauwecker等^[18]提出了一种基于可见性模型和传感器深度误差模型的鲁棒OctoMap并应用于基于双目立体相机的SLAM中，有效克服了传感器深度误差对地图精度的影响，提高了地图的精度。

1.1 鲁棒OctoMap可见性模型

基于八叉树的地图表示方法就是使用小立方体的状态 (空闲、占用、未知) 来表示地图中的障碍物，其中每一个小立方体称为一个体素。假设体素v与体素a、b、c相邻，如图 1所示。如果体素a、b、c状态均为占用，则体素v可视为局部闭合，称该事件为C_v。根据文献[18]，事件C_v发生的概率为:

$P\left( {{C}_{v}} \right)=\min \left\{ P\left( {{O}_{a}} \right),P\left( {{O}_{b}} \right),P\left( {{O}_{c}} \right) \right\}$

(1)

其中:P(O_a)、P(O_b)、P(O_c) 分别表示体素a、b、c被占用的概率。

定义从传感器发出的射线R能穿过一系列体素到达体素v_i为事件V_vi，即表示体素v_i可见。根据文献[18]，事件V_vi发生的概率取决于事件C_vi和事件V_vi-1，其中v_i-1表示沿射线方向靠近传感器一侧的体素v_i的前一个体素。另外，假设体素v_i-1不可见则v_i必然不可见，于是P(V_vi) 的计算公式如下：

$\begin{align} & P\left( {{V}_{{{v}_{i}}}} \right)=P\left( {{V}_{{{v}_{i-1}}}} \right)[P\left( {{V}_{{{v}_{i}}}}|{{C}_{{{v}_{i}}}},{{V}_{{{v}_{i-1}}}} \right)P\left( {{C}_{{{v}_{i}}}} \right) \\ & \ \ \ \ \ \ \ \ \ \ \ \ \ +P\left( {{V}_{{{v}_{i}}}}|{{{\bar{C}}}_{{{v}_{i}}}},{{V}_{{{v}_{i-1}}}} \right)P\left( {{{\bar{C}}}_{{{v}_{i}}}} \right)] \\ \end{align}$

(2)

其中:事件C_vi表示事件C_vi的互补事件，P(V_vi|C_vi, V_vi-1) 和P(V_vi|C_vi, V_vi-1) 为先验概率。

1.2 可见性模型的局限性分析

Schauwecker等^[18]虽然考虑了体素的可见性对地图精度的影响，但其可见性模型并未考虑相机与体素v的相对位置关系，导致计算出来的P(V_vi) 是不准确的。下面举例说明该可见性模型的局限性。

如图 2，假设相机位置为点o，且P(O_a)=1，P(O_b)=0，P(O_c)=0，即体素a完全占用，体素b和c完全空闲, 则根据式 (2) 可以得出P(V_vi)>0，即表示体素v部分可见。而实际情况如图 2所示，体素a完全占用，因此射线无法穿过体素a到达体素v。而射线从点o发出，沿直线经过体素b或c根本无法到达体素v。因此，实际情况表示体素v不可见，即P(V_v)=0。

通过分析可以看出，文献[18]中的可见性模型没有考虑相机与体素v的相对位置，不能准确表达体素v的可见性，因此该模型具有一定的局限性，会对地图精度造成较大的影响。

2 改进鲁棒OctoMap

本文针对Schauwecker等^[18]的可见性模型的局限性，考虑相机与体素v的相对位置关系及地图分辨率，提出了一种完整可见性模型，可以准确描述体素v的可见性。此外，针对Kinect传感器的特点，采用基于高斯混合模型的深度误差模型代替文献[18]中的深度误差模型，更准确地表达Kinect传感器的深度值。基于以上两点，本文提出了一种基于完整可见模型和Kinect深度误差模型的改进鲁棒OctoMap，以克服传感器深度误差对地图精度的影响，进一步提高建图的精度，算法流程如图 3所示。

2.1 可连通性判断

完整可见性模型充分考虑了相机和目标体素的相对位置，有效克服了鲁棒OctoMap中可见性模型的局限性，其关键点就是在建立可见性模型之前先对目标体素、相邻体素以及相机三者进行可连通性判断。若从相机发出的射线能够穿过目标体素的相邻体素而到达目标体素，则称目标体素、相邻体素和相机三者可连通；反之，则称三者不可连通。

根据传感器和目标体素的相对位置并考虑地图分辨率，可以确定满足可连通性的相邻体素的个数及其位置，总体上分为三种情况，如图 4所示, 其对应的满足可连通性的相邻体素的个数分别为1，2，3。

假设目标体素的中心坐标为 (x_v, y_v, z_v)，相机的坐标为 (x_o, y_o, z_o)，地图分辨率为r，则可通过判断三者之间的关系确定相机和目标体素之间的可连通性情况，从而确定满足可连通性的相邻体素的个数及其位置。

情况1 若 (x_v, y_v, z_v)、(x_o, y_o, z_o) 和r满足式 (3) ~(5) 其中之一，则可以判定相机和目标体素之间的可连通性为图 4(a)。

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|＜r/2 \\ & \left| {{y}_{o}}-{{y}_{v}} \right|＜r/2 \\ \end{align} \right.$

(3)

$\left\{ \begin{align} & \left| {{y}_{o}}-{{y}_{v}} \right|＜r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|＜r/2 \\ \end{align} \right.$

(4)

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|＜r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|＜r/2 \\ \end{align} \right.$

(5)

情况2 若 (x_v, y_v, z_v)、(x_o, y_o, z_o) 和r满足式 (6) ~(8) 其中之一，则可以判定相机和目标体素之间的可连通性为图 4(b)。

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|>r/2 \\ & \left| {{y}_{o}}-{{y}_{v}} \right|>r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|＜r/2 \\ \end{align} \right.$

(6)

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|>r/2 \\ & \left| {{y}_{o}}-{{y}_{v}} \right|＜r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|>r/2 \\ \end{align} \right.$

(7)

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|＜r/2 \\ & \left| {{y}_{o}}-{{y}_{v}} \right|>r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|>r/2 \\ \end{align} \right.$

(8)

情况3 若 (x_v, y_v, z_v)、(x_o, y_o, z_o) 和r满足式 (9)，则可以判定相机和目标体素之间的可连通性为图 4(c)。

$\left\{ \begin{align} & \left| {{x}_{o}}-{{x}_{v}} \right|>r/2 \\ & \left| {{y}_{o}}-{{y}_{v}} \right|>r/2 \\ & \left| {{z}_{o}}-{{z}_{v}} \right|>r/2 \\ \end{align} \right.$

(9)

判断完相机和目标体素的可连通性情况之后，其对应的满足可连通性的相邻体素的个数也就随之确定了。然后根据 (x_v, y_v, z_v)、(x_o, y_o, z_o) 和r的具体关系可以判断目标体素的位置。

2.2 完整可见性模型

通过判断目标体素的可连通性可知，目标体素的可见性取决于满足可连通性的相邻体素的可见性和占用状态。当满足可连通性的相邻体素可见且状态为空闲时，目标体素才可见。因此本文根据可连通性的情况分类，建立了完整可见性模型。

情况1 当可连通性满足图 4(a)时，只有一个相邻体素满足可连通性条件，体素v可见就意味着体素a可见且状态为空闲，因为体素a的可见性与其状态是相互独立的，所以体素v的可见性概率P(V_v) 可通过式 (10) 计算获得：

$P\left( {{V}_{v}} \right)=P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)=P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)$

(10)

情况2 当可连通性满足图 4(b)时，有两个相邻体素满足可连通性条件，体素v可见就意味着体素a、b中至少有一个可见且状态为空闲，因为体素a、b的可见性与其状态是相互独立的，所以体素v的可见性概率P(V_v) 可通过式 (11) 计算获得：

$\begin{align} & P\left( {{V}_{v}} \right)=P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)+P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)-P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)= \\ & \quad \quad \quad P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)+P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right)- \\ & \quad \quad \quad P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right) \\ \end{align}$

(11)

情况3 当可连通性满足图 4(c)时，有三个相邻体素满足可连通性条件，体素v可见就意味着体素a、b、c中至少有一个可见且状态为空闲，因为体素a、b、c的可见性与其状态是相互独立的，所以体素v的可见性概率P(V_v) 可通过式 (12) 计算获得：

$\begin{align} & P\left( {{V}_{v}} \right)=P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)-P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)P\left( {{V}_{c}}{{{\bar{O}}}_{c}} \right)+ \\ & \quad \ \quad \ P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)-P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)P\left( {{V}_{c}}{{{\bar{O}}}_{c}} \right)+ \\ & \quad \ \quad \ P\left( {{V}_{c}}{{{\bar{O}}}_{c}} \right)-P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)+ \\ & \quad \ \quad \ P\left( {{V}_{a}}{{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}}{{{\bar{O}}}_{b}} \right)P\left( {{V}_{c}}{{{\bar{O}}}_{c}} \right)= \\ & \quad \ \quad \ P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)-P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right)P\left( {{V}_{c}} \right)P\left( {{{\bar{O}}}_{c}} \right)+ \\ & \ \ \ \quad \ P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right)-P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)P\left( {{V}_{c}} \right)P\left( {{{\bar{O}}}_{c}} \right)+ \\ & \ \ \ \quad \ P\left( {{V}_{c}} \right)P\left( {{{\bar{O}}}_{c}} \right)-P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right)+ \\ & \ \ \ \quad \ P\left( {{V}_{a}} \right)P\left( {{{\bar{O}}}_{a}} \right)P\left( {{V}_{b}} \right)P\left( {{{\bar{O}}}_{b}} \right)P\left( {{V}_{c}} \right)P\left( {{{\bar{O}}}_{c}} \right) \\ \end{align}$

(12)

2.3 基于高斯混合模型的Kinect传感器深度误差模型

Schauwecker等^[18]建立双目立体相机的深度误差模型，即深度误差跟深度值的平方成正比，该模型同样适用于Kinect传感器。根据文献[19]的研究，Kinect深度值的标准差与其期望存在下列关系：假设各像素点的深度值z相互独立，则z的不确定性为:

${{\hat{\sigma }}_{z}}=1.45\times {{10}^{-3}}\hat{\mu }_{z}^{2}$

(13)

其中:${{\hat{\sigma }}_{z}}$为z的标准差、${{\hat{\mu }}_{z}}$为z的均值。

实际上，各像素的深度值不一定相互独立，因此式 (13) 具有一定的不合理性。Dryanovski等^[20]提出一种基于高斯混合模型的Kinect传感器深度误差模型，且在文献[20]中验证了该模型相对于式 (13) 的简单深度误差模型具有更高的可靠性; 尤其是在物体边缘，基于高斯混合模型的深度误差模型表现出突出的优势。

因此，本文采用基于高斯混合模型的Kinect传感器深度误差模型来代替文献[18]中的简单深度误差模型。

假设1 Kinect深度值z满足下列关系：

1) 假设Kinect深度值z服从正态分布，即$z\tilde{\ }\text{N}({{\hat{\mu }}_{z}},{{\hat{\sigma }}_{z}})$。

2) 假设各像素点深度值与其相邻像素点深度值有关。

当系统满足假设1时，z的不确定性可以根据高斯混合模型估计得到。本文采用如下高斯核

$G=\frac{1}{16}\left[ \begin{matrix} 1 & 2 & 1 \\ 2 & 4 & 2 \\ 1 & 2 & 1 \\ \end{matrix} \right]$

(14)

可以得出Kinect深度图中像素 (m, n) 处z的均值和方差为：

$\left\{ \begin{align} & {{\mu }_{z}}=\sum\limits_{i,j}{{{G}_{ij}}{{{\hat{\mu }}}_{{{z}_{i,j}}}}} \\ & \sigma _{z}^{2}=\sum\limits_{i,j}{{{G}_{ij}}\left( \hat{\sigma }_{{{z}_{i,j}}}^{2}+\hat{\mu }_{{{z}_{i,j}}}^{2} \right)}-\mu _{z}^{2} \\ \end{align} \right.$

(15)

$i\in \left[ m-1,m+1 \right],j\in \left[ n-1,n+1 \right]$

其中:(m, n) 表示像素坐标，μ表示均值，σ²表示方差。

2.4 实际占用概率计算

要构建基于八叉树的三维地图，需要计算每个体素的实际占用概率P(O_v)。文献[16-17]提出的OctoMap是基于概率表示的八叉树，具有可更新性。本文在完整可见性模型和Kinect深度误差模型的基础上，结合贝叶斯公式和线性插值算法来更新八叉树中每个节点的实际占用概率。

文献[18]定义了一个事件H，即相机的射线从体素v中反射。对于不可见体素v，事件H发生的概率只与体素v的可见性有关，与体素v的状态无关，因此只将P(H|_v) 和P(|_v) 定义为一个先验概率。对于可见体素v，则事件H发生的概率与体素v的可见性和状态均有关，因此根据传感器属性将P(H|O_v, V_v)、P(H|O_v, V_v)、P(H|O_v, V_v) 和P(H|_v, V_v) 定义为先验概率。

对于Kinect传感器的一条射线R的反射点q=(x, y, z)，则射线R路过的体素的实际占用概率都要更新。对于远离q点的体素和靠近q点的体素分别采用贝叶斯公式和线性插值的方法进行更新。

对于远离q点的体素，实际占用概率更新方法采用贝叶斯公式，即:

$P\left( {{O}_{v}}|M \right)=\frac{P\left( M|{{O}_{v}} \right)P\left( {{O}_{v}} \right)}{P\left( M \right)}\ $

(16)

其中：M∈{H, H}，P(O_v) 取上一次更新的结果P(O_v|M)，P(M|O_v) 和P(M) 均通过全概率公式求，即：

$P\left( M|{{O}_{v}} \right)=P\left( M|{{{\bar{V}}}_{v}} \right)P\left( {{{\bar{V}}}_{v}} \right)+P\left( M|{{O}_{v}},{{V}_{v}} \right)P\left( {{V}_{v}} \right)$

(17)

$\begin{align} & P\left( M \right)=P\left( M|{{{\bar{V}}}_{v}} \right)P\left( {{{\bar{V}}}_{v}} \right)\ +P\left( M|{{O}_{v}},{{V}_{v}} \right)P\left( {{V}_{v}} \right)P\left( {{O}_{v}} \right)+ \\ & \ \ \ \ \ \ \ \ \ \ \ \ \ P\left( M|{{{\bar{O}}}_{v}},{{V}_{v}} \right)P\left( {{V}_{v}} \right)P\left( {{{\bar{O}}}_{v}} \right) \\ \end{align}$

(18)

由于远离q点的体素可看作射线从其中穿过，因此事件H没有发生，式 (16) 中取M=H。

对于靠近q点的体素，其实际占用概率应当介于P(O_v|H) 和P(O_v|H) 之间，因此采用线性插值的方法进行计算，即：

$P\left( {{O}_{v}}|M \right)=P\left( {{I}_{v}} \right)P\left( {{O}_{v}}|H \right)+P\left( {{{\bar{I}}}_{v}} \right)P\left( {{O}_{v}}|\bar{H} \right)$

(19)

其中，P(I_v) 表示点q在障碍物内部的概率，由文献[18]知，根据q点的深度值及其不确定性可预先制作一个P(I_v) 的离散表，从而将其作为一个先验概率。

当SLAM过程完成时，将通过式 (18) 和式 (19) 计算的八叉树每一节点的实际占用概率作为最终的占用概率，从而构建立体占用地图。

3 实验与分析

本文以一个模拟室内场景为实际场景，如图 5所示。使用现有RGB-D SLAM算法进行建图，分别使用点云地图 (图 6)、鲁棒OctoMap (图 7) 和改进鲁棒OctoMap (图 8) 来表示地图。其中，本文实验中所用参数和鲁棒OctoMap所用参数如表 1所示。

表 1中鲁棒OctoMap的参数选取参考文献[18]，而改进鲁棒OctoMap的实验参数是根据经验调试获得的。

图 7和图 8中，蓝色的点表示占用概率P(O_v) 接近0.5，即在该体素地图的不确定性较大，深色的点表示确定的已占用体素，即P(O_v) 接近1。通过对比图 7和图 8(a)可以看出，改进鲁棒OctoMap较鲁棒OctoMap浅色的点明显减少，极大地降低了地图的不确定性，提高了地图的精度，验证了高斯混合误差模型的有效性。其中，浅色的点在物体边缘的对比效果更加突出，这与文献[20]的结论是一致的。

目前针对地图的确定性问题，还没有公认的定量描述形式。为了更精确地描述本文对地图确定性的提高，本文提出一个定量描述地图确定性的公式：

$Q=\left( 2\sum\limits_{i=1}^{N}{\left| P\left( {{O}_{{{v}_{i}}}} \right)-0.5 \right|} \right)/N$

(20)

其中:P(O_vi) 表示体素v_i的占用概率，N表示地图中的体素个数，Q表示评价结果，其取值范围为0~1。Q越接近1，表示地图的确定性越高。

使用上述评价方法对鲁棒OctoMap和图 8(a)中的改进鲁棒OctoMap的评价结果分别为0.72和0.86，可以看出改进鲁棒OctoMap较鲁棒OctoMap确定性提高了19.4%。

从实物图中可以看出，建图过程中部分区域被遮挡 (黑色方框中标定板背面区域)，标定板后方区域为不可见，而图 7和图 8(a)中显示背后仍有部分区域可见，足以说明该可见性模型的局限性，而图 8(b)中使用完整可见性模型的改进鲁棒OctoMap标定板背后几乎没有可见区域，表明本文的完整可见性模型更加精确，可适用于任意遮挡情况。

图 8(c)是同时使用完整可见性模型和高斯混合误差模型的最终建图效果，可以看出地图不确定性较低，且其遮挡问题也得到了较好的解决，地图精度较高，验证了本文方法的可行性和有效性。

本文增加了可连通性判断，且在建立可见性模型中情况3的计算量也较改进前有所增加。为检验本文算法的实时性，对鲁棒OctoMap和改进鲁棒OctoMap进行耗时比较。经过测量，该场景体积为74.8 m³，本文采用地图分辨率为1 cm³，则共有体素7.48×10⁷个。本文采用笔记本配置为i3处理器，4 GB内存。经测试，改进前鲁棒OctoMap耗时52.8 s，改进鲁棒OctoMap耗时57.6 s，耗时稍有增加但对建图的实时性影响不大。

4 结语

本文从移动机器人自主导航中对3D地图精度的需求出发，在Schauwecker等^[18]鲁棒OctoMap的基础上，提出了一种基于完整可见性模型的改进鲁棒OctoMap并应用于基于Kinect的RGB-D SLAM中。一方面，本文通过考虑相机和目标体素的相对位置关系及地图分辨率，从而分情况建立了完整可见性模型，克服了鲁棒OctoMap中可见性模型的局限性。另一方面，本文采用基于高斯混合模型的Kinect深度误差模型代替文献[18]中的简单深度误差模型，从而有效降低了地图的不确定性。实验表明，本文的改进鲁棒OctoMap所建地图较原有方法具有更高的精度。