混合视觉系统的运动物体检测和静态地图重建

胡誉生，何炳蔚，邓清康

（福州大学机械工程及自动化学院，福州 350108）

0 引言

近年来机器人技术蓬勃发展，自主机器人［1］是目前最热门的研究领域之一，机器人可以完成人类不能够完成的任务，如危险性较高的工作。想要在特定环境下自由移动，机器人必须具备定位［2］和导航［3］等功能，而这些功能依赖于环境地图，因此环境地图的构建至关重要。目前，通常使用相机或激光雷达等视觉传感器来获取室内或室外环境下的三维地图［4-5］，但当环境中存在运动物体时，三维地图重建仍然是一个难点，运动物体会在地图上留下一系列“痕迹”，将会形成错误的特征从而影响机器人对自身位置的判断，同时增加导航的难度。

动态背景下进行运动物体检测时，传感器和目标都处于运动状态，难以区分出背景和前景的运动，因此背景运动补偿估计是动态背景下运动物体检测的主要方法。基于图像的常见背景运动补偿方法包括特征点匹配法和块匹配法，文献［6］提出了一种基于改进尺度不变特征变换（Scale Invariant Feature Transform，SIFT）特征匹配算法估计背景运动，并利用图像差分法来检测出运动目标。文献［7］提出了一种基于子块运动补偿背景估计方法和图像差分法相结合来实现运动物体检测，避免了错误的特征点匹配结果对背景运动补偿的影响。上述两种方法均未解决运动检测不完整问题。基于点云的运动物体检测方法主要借助于栅格思想，通过点云配准算法［8-9］将当前帧点云投到上一帧的坐标系下，然后分析栅格状态前后变化来提取运动物体，但是该方法对小位移运动物体检测效果不佳。文献［10］提出了一种基于图像与点云数据融合的运动物体检测方法，从点云数据中获得小车自我运动参数来进行背景运动补偿，并结合光流算法完成运动特征的检测，但是该方法在非刚性物体提取完整度上仍效果不佳。

针对以上问题，同时考虑图像可以提供丰富的色彩纹理信息，点云可以提供高精度的距离信息，二者融合可以重建出信息更加丰富的三维彩色地图，因此在文献［10］研究的基础上，本文提出了一种点云分割辅助下运动物体提取的方法，解决了检测不完整问题，能较好地处理非刚性物体运动，同时基于八叉树地图（Octomap）算法［11］提高了静态栅格地图重建的质量和效率。

1 本文方法及其原理

本章将详细介绍点云分割辅助下的运动物体检测和静态地图重建的方法。该方法的主要框架流程如图1 所示，具体步骤如下：

图1 本文方法主要框架流程Fig.1 Flowchart of main framework of proposed method

1）输入数据为相机所获得的图像数据和激光雷达所获得的点云数据。首先对混合视觉系统进行外参标定，获取相机与激光雷达两传感器之间的坐标变换参数，为后续数据融合提供基础。

2）数据作地面移除预处理后，将其作为特征点投影到对应的图像上，利用光流算法获取特征点的像素运动向量，并估算特征点因小车自我运动而引起的人工运动向量来进行背景运动补偿，之后通过分析比较两运动向量的差异来获得点云数据中的动态点。

3）完整地提取出运动物体，利用改进的欧氏聚类算法对当前点云数据进行簇分割；之后利用点云数据索引值的唯一特性来融合动态点检测结果与点云聚类分割结果，从而实现运动物体完整的提取。

4）通过Octomap 工具与当前点云数据所对应的激光雷达里程计，实现静态地图的重建。

1.1 全向相机与三维激光雷达的坐标变换

相机和激光雷达标定的目的在于获取从激光雷达坐标系到相机坐标系的投影变换，其变换公式为：

其中：表示t时刻激光扫描下的第i个点表示在对应图像相机坐标系下的值；K表示相机的内参数；R和T分别表示投影变换阵中的旋转矩阵和平移向量。

本文中使用的相机为全向相机，该相机系统通过每个相机的内参数将图像投影到统一的特定虚拟相机系统坐标系中并以球坐标系方式展示，因此可以忽略内参数K。令，该点对应的像素坐标设为(u，v)T，则从激光雷达坐标系转换到像素坐标系的计算式为：

其 中：int(⋅) 表示取整函数；arctan2(⋅) 表示值域为[-π，+π]的反正切函数；H和W分别表示图像的高度与宽度。本文采用了文献［12］中基于点云强度信息辅助下棋盘格角点自动提取的高精度标定方法进行系统传感器间外参数的标定。

1.2 点云数据中动态点的检测

光流算法是运动检测领域中常用的算法之一，在静态背景环境下通过光流算法对图像中像素点的运动估计，可以实现运动物体的检测；但在建图过程中由于相机跟随小车一起移动，因此在图像背景区域也会出现相应的像素运动，同时在全景图像中每个相机视角有着各自的运动方向，仅靠光流法不能很好地区分静态背景和动态前景。

通过分析光流法原理，可以知道由光流法所估计的像素运动向量在一定程度上反映了三维世界中物体的运动场，因此只要获知小车的自我运动位姿，就可反映背景运动模型，从而通过背景运动补偿来实现前景运动物体的检测。借鉴文献［10］提出的背景运动补偿方法，本文的运动检测方法在此基础上进一步提升，以完整地提取出运动物体。

本文的动态点检测方法可以分为三部分，分别是点云地面识别，特征点光流运动向量，以及人工运动向量的估计、点云数据动态点的提取。

1.2.1 点云地面识别

点云数据中包括了大量的地面点，其不仅增大了计算的复杂度，而且对后续动态点的检测带来干扰，因此点云地面识别是重要的。本文中使用了基于随机采样一致性（RANdom SAmple Consensus，RANSAC）算法来识别点云地面，其过程［13］如下：

1）从点云数据中任意选取三个不共线的点，计算其相应的平面方程ax+by+cz=d。

2）之后计算点云中每个点到该平面的距离（di=axi+byi+czi-d），并与设定的阈值t比较，若di≤t，则将该点视为局部内点并统计有效点数n，反之将其剔除。

3）重复1）～2）步骤，迭代m次之后选取有效点数n最大的平面并采用最小二乘法估算平面参数。

通过上述过程可以知道随机采样一致性算法可以提取到局部内点数量最多的平面，但是当点云中地面点数量不是最大时将会出现错误的结果，如图2 所示，可以发现墙壁被误作为最终的识别结果。

图2 基于RANSAC地面识别错误例子Fig.2 Example of ground recognition error based on RANSAC

为了解决这个问题，本文考虑到较平坦的地面环境与传感器的安装位置，先对点云数据进行一次直通滤波（PassThrough）来过滤掉大部分位于墙体上的点云，保证在点云平面集合中位于地面的数据点最多；之后再通过随机一致性算法对剩下的点云数据进行平面检测以完成地面点云的提取。具体过程如图3所示。

图3 基于PassThrough+RANSAC 地面识别例子Fig.3 Example of ground recognition based on PassThrough+RANSAC

1.2.2 特征点运动向量的估计

经过1.2.1 节的点云地面点识别之后，通过激光雷达坐标系与相机像素坐标系的变换，将非地面点数据作为特征点投影到对应的图像上，通过光流法估计特征点的像素运动，本文使用文献［14］中提出的一种由粗到精的光流估计方法，可以在一定程度上减小光度变化所引起的光流估计误差，特征点的光流运动向量vo为：

其中：pt表示t时间下点云数据中非地面点集合；siftflow(⋅)表示光流估计函数。

获取特征点的光流运动向量之后，进行特征点的人工运动向量估计，该值反映了特征点因相机跟随小车自我运动而引起的背景运动，其完全忽略了图像中任何可能的运动对象。为了获得人工运动向量，首先通过LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry And Mapping）算 法［5］从点云数据中估计出小车的移动位姿，并利用式（6）转换后得到相邻间里程计Ot：t+1；再由式（7）将点云数据pt通过Ot：t+1转换，获得其虚拟下一时刻出现的位置集合；最后将pt与分别投影到图像上得到相应的像素坐标值，两者相减的结果作为人工运动向量va的值，如式（8）所示。

其中：Ot：1和Ot+1：1分别表示第t时刻到第1时刻的里程计和第t+1 时刻到第1 时刻的里程计；Tl2c(⋅)函数表示激光雷达坐标系到像素坐标系的映射关系。

1.2.3 点云数据动态点的提取

特征点的光流运动向量表示三维世界物体真实运动场在图像上的投影，其人工运动向量则表示仅因传感器跟随小车自我运动而引起的运动场在图像上的投影，如图4 所示，其中绿色箭头（浅灰）和红色箭头（深灰）分别表示了特征点的光流运动向量vo和人工运动向量va，可以明显地观察到两者在图像背景上运动状态相似，但是其在前景上具有较大的差异。其中点云为无序点，并以150 个点为间隔投影到图像上便于观察。

图4 特征点光流运动向量和人工运动向量例子Fig.4 Example of optical flow motion vectors and artificial motion vectors of feature points

一般来说，通过特征点的光流划分为移动的或者静止的，可以转换为每对运动向量都要考虑它们的角度和模长的过程：

1）对运动向量在像素坐标系下的值，之后利用平面向量内积公式计算出运动向量间的夹角θ。

2）向量间夹角θ与设定的临界阈值Thθ（取20°）比较，如果θ≥Thθ，则对应的特征点为动态点。

3）比较两运动向量差的模长，利用高斯核函数来表示一点的运动状态Pm，如式（9）所示：

其中，σ2表示θ

经过上述处理之后获取到一组散乱的动态点，其中可能出现误检和检测不完整问题，需要进一步处理以完整地提取出运动物体；同时，考虑到出现运动物体与小车移动状态完全一致的情况可能性很低，因此在动态点检测过程中忽略了光流运动向量为0的特征点，以减少干扰因素。

1.3 基于点云分割辅助的运动物体提取

点云数据动态点检测过程中，不可避免地出现检测不完整问题，可以将其分为以下两种类型：

1）位于运动前景的特征点未被检测出来；

2）当非刚性物体的部分位置发生运动时，位于其静态部分的特征点无法被检测出来。

为了解决上述两种检测不完整类型，本文提出了一种基于点云分割辅助的运动物体提取方法，对点云数据进行聚类分割后，通过每个点索引值的唯一特性来融合点云聚类分割结果与动态点检测结果，以提取出完整的运动物体。

1.3.1 划区域点云距离聚类分割

由于点云数据量的庞大，综合考虑时间复杂度和分割效果两个因素，本文提出了一种划区域欧氏聚类分割方法，防止在单一距离阈值条件下出现的分割不足和过度分割现象，并保证最终的分割效果。

传统的欧氏距离分割算法［15］的主要思想是将距离在设定阈值内的点归为同一类，其过程如下：

1）为输入的点云数据pt建立一个KD-Tree 数据结构，并创建一个空队列q；

2）从pt中选取一个未处理点放入q中；

3）利用KD-Tree 数据结构获得该点的邻近点，计算其到邻近点的距离，并将距离小于设定阈值r的点索引加入到队列q中；

4）在队列q中选择一个未处理的点，重复步骤3）；

5）当队列q不再有新点加入时则完成一个实例分割，备份该实例索引集合，并清空队列q；

6）重复步骤2）～5），直到点云数据中所有点都被处理过。

从上述过程可以知道，传统的欧氏距离分割算法其关键在于对距离阈值r的选取，当r值过小时会出现过度分割现象，反之则会出现分割不足现象。本文使用的激光雷达设备为Velodyne HDL-64E S3，其最大角分辨率∂（垂直角分辨率）约为0.5°，根据式（10）可知随着目标点距离激光雷达中心的位置越远，其反射回来点与点之间距离越大。为了达到更好的聚类效果，本文采用动态阈值的方法，即在不同距离区域内使用不同的距离阈值r策略，如图5所示，r的计算式如下：

图5 不同区域对应不同距离阈值示意图Fig.5 Schematic diagram of different distance thresholds corresponding to different areas

其中，d表示激光雷达在XY平面上的扫描线距离。

以激光雷达为中心划分其射线距离为：D1=15 m，D2=30 m，D3=45 m，D4=60 m，其对应的距离阈值通过将该区域内最大射线距离值代入式（10）求得，分别为r1=0.13 m，r2=0.26 m，r3=0.39 m，r4=0.52 m，之后点云将在各自的区域内使用相应的距离阈值进行聚类分割，当点的距离大于80 m 时则忽略该点。

1.3.2 运动物体的提取

完成点云数据动态点检测和聚类分割后，描述如何融合两者以提取出更完整的运动物体。如图6 所示，左边圆形区域表示点云数据中动态点集合，右边方形区域中C表示点云聚类分割结果，根据点云数据结构中索引值的唯一特性对动态点进行归簇，即寻找每个动态点所位于的点云簇位置；之后对每个簇中动静态点的比值进行判断，如式（11）所示，当该值超过给定的阈值ThR（取0.4）时则断定其为运动物体。

图6 动态点归簇示意图Fig.6 Schematic diagram of finding clusters of dynamic points

其中：numd表示点云簇的动态点数量；numa表示点云簇的总点数量。

1.4 基于Octomap的静态地图重建

点云数据中运动物体提取之后，通过与之对应的里程计（1.2.2 节）将其转换到世界坐标系下以完成点云地图的重建，如式（12）所示，通常默认将地图的第1时刻激光雷达所在的位置设为世界坐标系。

随着建图的进行，点云地图的容量将会越来越大，并且会产生很多冗余数据，不仅提高了对计算机性能的要求，而且影响了点云地图的精度，同时无法直接运用于路径规划等机器人任务。为了解决这个问题，本文使用了基于八叉树数据结构的Octomap 地图表示方法，其将三维空间划分为树状空间网格，利用概率模型方式对每个叶子的状态（空闲、占据、位置）进行刻画，设t=1，2，…，T时刻，对应观测到的数据为z1，z2，…，zT，则第n个叶子点到T时刻记录的全部信息P(n|z1：T)为：

其中：P(n|z1：T-1)表示第n个叶子点到T-1 时刻记录的全部信息，P(n|zT)表示第n个叶子在T时刻记录的信息；P(n)表示一个固定的先验概率，通常取0.5。

2 实验与结果分析

本文所使用的混合视觉系统由全向相机Ladybug5 和三维激光雷达Velodyne HDL-64E S3组成，如图7（a）所示将其搭载在Guardian 移动小车上。其中，全向相机系统所采集图像最高分辨率为8192 × 4096，本实验中所用的图像像素大小为2048× 1024；三维激光雷达由64 个激光发射器和64 个激光接收器组成，其垂直方向视野为+2°到-24.8°；在数据采集过程中全向相机和三维激光雷达的频率均为10 Hz，图7（b）为实验环境示意图。

图7 实验平台和实验环境示意图Fig.7 Schematic diagram of experimental platform and experimental environment

2.1 运动物体提取结果

为了更好地展示所提出的基于点云分割辅助下运动目标检测算法在解决完整性问题方面上的提升，本文对运动物体常规移动状态进行分类分析，具体如图8 所示，其中，图（a）表示实验环境的实际场景图，图（b）表示对应的三维点云数据，图（c）表示对应的无点云分割辅助下运动点检测结果，图（d）表示对应的有点云分割辅助下的运动物体完整提取结果。相较于改进前和改进后的检测效果，如图（c）、图（d）中圆圈标记部分所示，运动物体检测不完整问题得到了有效的改善，特别地，第六行中物体在上下运动过程中膝盖以下部位几乎处于静止状态，本文方法也可以完整地将其提取出来；同时，部分检测错误动态点因无法满足运动物体提取条件（1.3.2 节）而被过滤掉，在一定程度上抑制了错误检测的发生，最终准确地提取出运动物体。

图8 运动物体提取结果Fig.8 Result of moving object extraction

2.2 静态地图重建结果

点云运动物体提取后即可进行静态地图的创建，根据相应的里程计信息将其转换到世界坐标系下之后，经过八叉树数据结构存储后得到Octomap 三维栅格地图，使用octovis 工具（http：//wiki.ros.org/octovis）可以查看最终的地图结果。本文实验中栅格地图的最高分辨率为0.05，其结果如图9所示，图9（a）表示未经运动物体提取的重建结果，图9（b）表示经运动物体提取之后的重建结果，图9（c）为地图所对应的部分实景图。从图9 可以发现，静态地图更好地反映了实际环境，实验中存在30个运动物体，检测到28个。

图9 基于Octomap的三维重建结果Fig.9 3D reconstruction result based on Octomap

本文分别对点云地图和Octomap 地图从点数与存储大小两个方面进行统计，其结果如表1 所示。可以发现，较于点云地图，Octomap 地图在点数与存储空间上分别降低了92%与95%，其可以构建一个更高效、紧凑的环境地图。

表1 地图的点数和存储大小Tab.1 Number of points and storage of maps

3 结语

为了重建静态地图，本文首先运用了一种基于相机与激光雷达相结合的方法，将点云数据投影到图像后通过估计其两运动向量来区分背景和前景；同时为了改善检测不完整问题，提出了一种点云分割辅助下的运动物体提取方法，实验结果表明该方法不仅可以提升检测完整度，而且在一定程度上可以去除误检点；最后，基于Octomap 算法在低内存损耗下完成静态地图的重建，为后续移动小车的自主导航提供了基础，具有较好的实用价值。