非结构化环境下的单目视觉可通行区域检测

郭植星，曾 碧，刘建圻，陈文轩，王俊丰

广东工业大学 计算机学院，广州 510006

自主机器人进行作业首先要具备避障导航规划的能力。目前基于不同的传感器及其组合，如激光雷达、里程计、IMU、声纳、单目相机、双目相机和RGBD相机等，已经有多种避障导航方案。但基于激光的导航避障方案存在激光雷达信息少、维度单一等问题，特别是救援场景等其他需要对语义分析的场景中，纯激光SLAM很难完成任务。多传感器组合的方案在机器人导航避障方面逐渐兴起，虽然组合不同类型的传感器[1-3]可以获得更加详尽的信息和准确的决策，但也增加了成本并提高了算法的复杂性。

相机的优点是便宜、容易安装、体积小、信息丰富。对于微型飞行器（MAVs）或者小型汽车，由于体积小，载重能力弱，电池容量有限，只能携带重量轻的传感器，如单目摄像头，这可避免对电池寿命和自身移动造成过大影响，也能大大降低移动任务难度。对于自主机器人运动，移动机器人如何判断其相对于潜在障碍的位置是至关重要的。基于单目视觉的方法不需要特殊的摄像头配置，更适合各种低成本的机器人应用。本文主要侧重于解决单目视觉下的实时导航规划问题。

1 相关工作

只利用相机作为外部感知传感器的SLAM称为视觉SLAM（VSLAM），常见的视觉SLAM算法有ORBSLAM[4]、DSO-SLAM[5]或LSD-SLAM[6]等。ORB-SLAM稳定性强、鲁棒性高，DSO-SLAM生成的特征点更加稠密，有助于计算，LSD-SLAM在纹理比较弱的地方工作性能更好。基于直接法的LSD-SLAM对光照和动态干扰比较敏感，因此精度一般会劣于ORB-SLAM。LSDSLAM对纯旋转的鲁棒性很大程度上取决于后端优化中地图的优化效率，能否获得准确的地图信息是后续帧能否稳定跟踪的关键。ORB-SLAM不需要恢复稠密深度，计算效率高，所以在近似纯旋转场景中的鲁棒性要比LSD-SLAM更好[7]。由于直接法需要比较两帧之间的图像信息，其结果容易收到外界光照干扰。DSO提出了光度标定，通过对相机曝光时间、暗角、伽马响应等参数进行标定而使直接法更加鲁棒，只要相机运动不快，模糊不明显，DSO能顺利完成工作，但如果出现明显的模糊或者失真，DSO也会丢失。DSO对场景光照要求高，要求尽可能保持曝光时间的稳定。

纯视觉避障方案最大的缺陷是缺乏深度信息，只有RGB图像很难对障碍物位置进行有效判断。近年来，深度学习技术逐渐兴起和完善，夏梦琪等[8]提出一种全卷积编码-解码网络模型，使图像高层信息更有效地在网络中传递，在保证精度的同时提升了精度。Eigen等[9-10]使用深度学习进行单目深度估计，估计障碍物位置并进行避障；Wang等[11]引入了人类视觉系统估计深度的要素之一——运动，依此提出了一种基于深度学习的多视点稠密深度图和里程计估计方法，采用递归神经网络（RNN），利用多视点图像重投影和前后向流一致性损失进行训练并得到预测深度图。Riegler等[12]利用立体成像技术来改进单目深度估计，提出了一种从单个输入图像推断深度的新型深度框架——monoResMatch，其原理是将单目深度估计转化成立体匹配问题，再用立体匹配网络进行视差估计。另一种思路是直接求解空间中的空闲区域，以此作为机器人的可通行区域进行导航规划。Liang等[13]利用倒极（reciprocal-polar，RP）图像校正和RP空间中的正弦模型拟合进行地平面分割从而得到机器人可移动的地平面区域。Zhou等[14]从图像序列中提取连续两帧进行单应性计算，并由此得到地平面区域。Jin等[15]通过固定相机高度和约束水平旋转以加强单应性约束，再使用该约束进行地平面检测。

郑争光等[16]通过对图像进行自适应阈值分割，获取障碍物和地面的边界轮廓后进行障碍物判断。Lenser等[17]采用低分辨率颜色分割和目标检测（训练为8个目标类）进行单目避障。针对室内环境，Ohya等[18]中提出了一个导航框架，使用单帧图像检测静止目标和超声波传感检测运动目标，使用当前和期望图像之间的差异来区分动静态障碍物。Lin等[19]结合特征匹配和自适应颜色分割算法进行平面估计，经过IPT和地面区域分割获得可通行的栅格地图。Souhila等[20]从图像序列中提取光流，光流提供了机器人环境的相关信息，如障碍物布置、机器人航向、碰撞时间和深度等，该策略通过平衡左右侧的光流量来避开障碍物。Cherubini等[21]用一系列有序图像帧表示待前进路径，将导航任务切割为多个前往下一个关键帧的子任务。此类方法确实能实现视觉导航的任务[22]，但却忽略了避障功能的实现，Lamiraux等[23]对此部分进行了补充和完善。但这类方法都需要预先构建一部分环境模型，如门、窗或者墙等。以上方法通过划分可通行区域和障碍物区域以使机器人在不同的空间区域完成实时导航规划任务。

针对机器人导航任务，另一种解决方案是利用场景信息分析得到运动决策。文献[24-25]引入深度强化学习方法，Zhu等[24]采用端到端的方式解决导航问题，通过对同一场景下的两幅输入图像的比较，得出当前state想要到达target需要执行什么动作。Yang等[25]将先验场景信息进行编码，加入到端到端的视觉导航中，可以完成去找杯子、去厨房等的语义导航能力。基于先验信息的导航不仅可以在已知场景中找到目标，还可以在未知场景中进行导航规划。李鹏等[26]利用分布式环境的不同特征，对区域导航子模块进行强化学习训练，实现针对不同区域的独立控制策略。

上述导航避障方案中，每种传感器都有其优点和不足：超声波传感器抗环境干扰能力强，但会受环境温度影响，小物体难以反射足够的声波；激光雷达实时性强、信息准确，但价格昂贵、信息维度单一；视觉相机价格低廉，信息丰富，但缺乏直接的距离信息。基于深度学习的视觉深度估计算法或者基于强化学习的导航避障算法在指定场景能出色地完成任务，但在随机场景中鲁棒性较低。为了实现三维空间下低成本且鲁棒性高的机器人导航系统，受文献[14-15，19]的启发，本文提出了一种基于单目相机的可通行区域检测方法（accessible space detection method based on monocularcamera，ASDM-Mono），仅通过单目视觉特征点地图信息即可计算空间中各点的可通行代价。首先利用单目SLAM特征点法计算得到3D特征点地图；然后在特征点地图的基础上，提出了一种能精确计算前方空间与其邻近特征点簇最小距离的障碍物距离求解器；为了得到机器人导航所需的视觉代价地图和可通行区域，设计了一个代价求解器；固定相机高度结合可通行区域的方法可以求解得到单目视觉中的尺度因子以解决单目SLAM中尺度不确定的问题。最后经过反投影变换获得可通行方向。该方法仅需低成本的单目相机传感器便可实现可通行区域的检测任务。此外，该方法计算得到了视觉代价地图和可通行方向，有利于后续小车的路径规划模块，更易落地，实用价值高。

2 基于单目视觉的可通行区域检测

2.1 单目可通行区域检测框架及原理

本文提出的ASDM-Mono方法旨在实现非结构化环境下鲁棒性高的单目视觉导航方案，提供一种基于单目视觉特征点地图求解可通行区域的方法。为了追求旋转场景的鲁棒性及降低室内场景的光照依赖性，特征点法选取ORB-SLAM。

假定相机高度固定并且约束相机只在水平面上旋转，ASDM-Mono算法实现步骤如下：

步骤1通过特征提取和匹配、PnP算法计算特征点集

步骤2分割前方区域空间S为m个细小子区域利用KD-Tree查找每个子区域sj附近k个邻居特征点N=( )P1,P2,…,Pk,N⊆D，使用障碍物距离求解器求解得到子区域sj的最近障碍物距离dsj。

步骤3将障碍物距离信息dsj,sj∈S代入代价求解器求得视觉代价地图Mcost，并结合预先设定好的代价阈值σ对Mcost进行划分得到可通行区域。

步骤4利用可通行区域和随机采样一致算法（random sample consensus，RANSAC）过滤得到地面点集Dplane⊆D并进行尺度因子ρ求解。

步骤5根据尺度因子ρ更正可通行区域尺度，通过投影变换获得可通行方向并结合机器人运动规划器进行导航规划。

图1展示了ASDM-Mono算法的整体框架，主要包括特征提取、可通行区域检测、尺度因子求解及障碍物检测等四大部分。下面将对其中的具体细节进行详细的阐述。

图1 ASDM-Mono算法整体流程框图Fig.1 Overall flow diagram of ASDM-Mono

2.2 障碍物距离求解器

ASDM-Mono使用特征点标识障碍物，利用KD-Tree收集已划分区域sj的邻近特征点集N，通过统计所有子区域sj到临近点的距离来获取其对应最近障碍物距离dsj。直接使用平均距离进行距离求解并不能很好表示不同位置之间的差异及准确表达最近障碍物距离，而采用高斯核密度估计获取特征点的分布情况能更好地辨识最近障碍物。高斯核密度估计的分布密度函数fh(x)计算如公式（1）所示：

其中，f(x)表示概率密度函数；K(x)表示核函数，这里选取高斯核函数；h表示带宽，本文取0.5；k表示收集到的临近点数量；待计算区域sj的最近障碍物距离dsj计算公式如式（5）所示：

其中，peaki(f)表示获取f(x)密度分布中第i个波峰值；di表示待计算区域sj与第i个临近点Pi的距离；dkde表示利用高斯核密度估计估算的距离；dave表示特征点集平均距离；α表示权重因子。

图2中（a）展示了待计算区域sj附近特征点的距离信息，（b）表示使用高斯核密度估计得到的分布情况，横坐标表示临近点的距离x，纵坐标表示该临近点根据高斯核密度估计计算得到的概率密度f()x。由图2（b）可以看出，该区域附近有两个特征点簇x1、x2，分别表示两个障碍物O1、O2，希望得到sj最近障碍物的距离信息，故取第一个波峰f(x1)的距离作为的最近障碍物距离dsj，此做法可以实现简单的聚类功能，实现了对不同方位障碍物的区分处理。图2中选取x1=0.3作为当前场景下的最近障碍物距离值。

图2 障碍物距离计算示意图Fig.2 Obstacles distance calculation

以下通过一个简单的模拟实验进行障碍物求解器测试。

图3中颜色深浅表示特征点分布密度，颜色越深表示特征点数量越密集。实验场景是模拟最常见的直线道路区域，道路总宽度为2 m，实验目的是求得红色块区域（小车）离最近障碍物的距离。

图3道路模拟实验示意图Fig.3 Road simulation experiment

图4中（a）表示使用公式（3）计算得到的障碍物距离值，（b）表示使用公式（5）计算得到的障碍物距离值，（c）表示临近特征点平均距离，（d）为临近特征点距离标准差。

图4 道路模拟实验数据分析Fig.4 Analysis of road simulation experiment

如图4（a）和（d）所示，道路中间（即水平偏移量为0处）数据方差小，数值较接近，取首波峰数据作为距离量会有较大波动和误差，直接使用首波峰距离dkde或平均距离dave均不能很好地满足道路中间小车距障碍物距离大，道路两侧距障碍物距离小的原则，该方法将dkde和dave相结合，当方差较小时增大对应权重，最终求得的结果如图4（b）所示，能很好地解决了这个问题。

2.3 代价求解器

以上步骤已经得到了前方区域的最近障碍物距离信息，接下来需要对前方区域进行视觉代价地图求解，为了降低机器人撞击障碍物的概率，方便后续机器人进行路径规划，设计了一个代价求解器，根据最近障碍物距离信息进行代价分配，且距障碍物较近区域代价值应远高于较远区域代价值。

基于特征点进行代价求解存在两个问题，一是由于障碍物背部的特征点信息缺失，如果直接进行代价计算，该方法会将障碍物背部视为空闲区域，针对该问题，代价求解器中引入了代价传播机制；二是空闲区域中心位置附近特征点数量较少，此时计算得到的距离信息可能会有较大误差，如果不加以修正，会出现中心空闲区域代价值更高的情况，这是不合逻辑的，尽管该问题可以通过增大KD-Tree搜索范围解决，但此举会增加时间复杂度，降低实时性。为保证实时性和避免临近特征点数量过少带来过大误差，代价求解中引入一个临近特征点数量临界值n*，若临近点数量n过小，即n

经过测试，代价求解器设计如公式（6）所示：

其中，C表示待计算区域代价值；d*表示影响半径，本文设置为1.0；n*表示临近点数量临界值，本文设置为100；η*表示代价尺度因子，本文设置为0.1。

为了避免因障碍物背面检测不到特征点而导致可通行区域计算出现错误的情况，代价求解过程中引入代价传播机制，原理如算法1所示：

算法1 Cost propagate mechanism

Input c：cost to be calculated，c*：cost threshold，

Ocam：camera position and s：subspace ahead

Output Optimized cost c'

1.for each ci(1≤i≤m)do

2. if ci>c*then

3. P←Bresenham(Ocam,si)；

4. for each p in P do

5. Propagate cito cp；

6. end for

7. end if

8.end for

从相机位置（原点）向障碍物潜在区域（代价值较大区域）发射射线，将障碍物代价值向后方区域传播，效果如图5所示。

图5 代价传播示意图Fig.5 Cost propagation

2.4 尺度因子求解器

通过对极几何求解相机运动时，由于本质矩阵具有尺度等价性，导致分解得到的平移矩阵具有一个未知的尺度，常见做法是对该平移矩阵进行归一化处理，但归一化后的尺度导致了单目视觉的尺度不确定性。

尺度因子求解的前提是相机高度固定并且只进行水平面旋转。首先使用前面得到的可通行区域和RANSAC对特征点进行过滤，得到地面点并算出归一化尺度下的地面高度，即图6中的h'，然后利用预设好的相机高度h计算尺度因子ρ=h/h'，最后利用ρ进行障碍物深度估计和特征点真实尺寸恢复。

图6 尺度因子求解示意图Fig.6 Calculation of scale factor

3 实验

为了验证ASDM-Mono的可行性与实用性，在KITTI数据集和现实场景中分别进行了实验测试，KITTI数据集图像分辨率为1 241×376，实验采用ORB-SLAM3进行特征点数据采集。实验使用搭载Ubuntu 18.04和ROS系统的电脑，处理器（CPU）为Intel®CoreTMi7-6700 CPU@3.40 GHz 3.41 GHz，内存为8 GB。显卡（GPU）为AMD Radeon R7 350 Series，显存为2 GB。

3.1 障碍物距离求解器实验

下面将对障碍物距离求解器进行实验，主要目的是对比引入障碍物距离求解器前后的差异。图7中（a）是实验场景原图，（b）是使用平均距离即公式（4）计算得到的视野范围内视觉代价地图，（c）是使用该方法距离求解器即公式（5）计算得到的视觉代价地图。图7（b）视觉代价地图中部出现空闲区域中心代价值更小的情况，这是因为中心区域临近特征点数量少、距离方差小，该情况会对可通行方向的判断和可通行区域的计算带来较大干扰，为了解决该问题，引入临近点数量临界值，同时结合平均距离和首波峰距离进行优化计算。优化后的效果如图7（c）所示，可以看到，空闲区域中心同附近障碍物之间的过渡符合预期，更能满足机器人在实际场景中进行导航规划的需求。

图7 障碍物距离实验效果图Fig.7 Experiment of obstacles distance calculation

3.2 代价传播机制实验

本节实验主要测试代价求解器中的代价传播机制。图8中（a）是实验场景原图，（b）是使用该方法障碍物距离求解器但未引入代价传播机制的视觉代价地图，（c）是使用该方法障碍物距离求解器且引入代价传播机制优化后的视觉代价地图。图8（b）中可以看出，未引入代价传播机制的视觉代价地图中，由于无法获取障碍物背部信息，基于特征点标识障碍物的方法会将特征点背部视为空闲空间，这不利于机器人的路径规划。而代价传播机制能适当增加未知区域的代价值而不是单纯地将其视为空闲区域，效果如图8（c）所示。

图8 代价传播实验效果图Fig.8 Experiment of cost propagation

3.3 尺度因子求解器实验

利用可通行区域过滤得到的地面点结合相机的固定高度可进行单目SLAM的尺度因子求解。本节实验将对尺度因子求解器进行障碍物距离测试。实验选用Kinect-v1.0相机进行静态障碍物距离估计实验，Kinect相机采集得到的深度图像同实际测量距离作为实验GroundTruth，Kinect相机RGB图像作为实验的单目图像输入。实验结果如表1所示，相机被控制在距离障碍物0.5 m至1.5 m之间，障碍物距离估计受地面点单应性和数量影响，误差范围在2.5%至4.9%之间。

表1 障碍物深度实验数据Table 1 Analysis of ablation experiments

图9、图10中，（a）是实验环境图像，（b）、（c）、（d）分别表示相机坐标下特征点全局图、俯视图和侧视图。图中绿色部分是Kinect相机得到的深度图像。实验环境中相机距离障碍物距离为0.8 m，矫正前检测深度为1.03 m，矫正后检测深度为0.82 m。可以看出，如果不经过任何处理，单目SLAM得到的特征点具有尺度不确定性的问题，此时所测得的深度信息具有较大的误差，而经过该方法尺度因子求解优化后误差由28.75%降至2.5%，具有较大提升。

图9 尺度因子求解器矫正前示意图Fig.9 Experiment of scale-factor solver（before correction）

图10 尺度因子求解器矫正后示意图Fig.10 Experiment of scale-factor solver（after correction）

3.4 ASDM-Mono实时性实验

可通行区域检测实时性实验中，主要测试首波峰距离和代价传播机制对实时性的影响。该方法所需时间如表2所示，当采用平均距离和代价传播模块时，每帧图像所需时间为0.050 s，视觉代价地图生成频率为20 Hz。当继续引入首波峰距离进行距离优化时，时间有所增加，为每帧0.073 s，视觉代价地图生成频率为14 Hz。如果希望实现视觉实时避障功能时，采取前者的方案，而如果希望构建一张精准的代价地图以供以后导航使用时，则选取后者，两套方案在不同的需求下都满足了SLAM中实时性要求。

表2 ASDM-Mono算法所需时间Table 2 Time required for ASDM-Mono algorithm

3.5 非结构化场景可通行区域检测实验

相比于结构化环境，非结构化环境增加了更多的复杂性和多样性，并且道路特征不固定，较难直接从周围环境中获取信息。为了检测文章方法在非结构场景下的实验效果，本节进行了可通行区域的覆盖率和错检率实验。实验中，激光雷达获取到的边界信息加上膨胀层作为可通行区域GroundTruth，比较该方法得到的可通行区域和激光栅格地图空闲区域占比获得覆盖率数据，超出激光边界的可通行区域视为错检，实验对比示意图如图11所示，图11（d）中红色框表示错检区域，蓝色框为激光雷达误检区域，将进行手动纠正。

图11 非结构化场景实验对比图Fig.11 Unstructured environment experimental comparison

实验过程中，小车间隔1 m提取一次可通行区域进行比对，统计得到的可通行区域覆盖率和错检率分别如图12、13所示。覆盖率实验数据表明，覆盖率在70%~80%、80%~90%、90%~100%内的样本占采样总数的比例分别为12.4%、32.4%、40.9%，平均覆盖率为88.39%。错检率实验数据表明，错检率在区间0%~10%、10%~20%、20%~30%内的比例分别为70.1%、17.7%、7%，平均错检率为5.03%。

图12 可通行区域覆盖率实验示意图Fig.12 Experiment of accessible space coverage

3.6 可通行区域检测综合实验

综合以上障碍物距离求解器、代价传播机制和尺度因子求解器，该算法使用特征点标识障碍物并将代价值较低区域标记为可通行区域，最后将计算得到的代价地图进行投影变换便可得到二维图像中的可通行方向，最终选取平均距离+首波峰距离+代价传播的方案进行综合实验测试，KITTI数据集上测试的实验效果如图14所示，现实场景中实验效果如图15所示。

图13 可通行区域错检率实验示意图Fig.13 Experiment of accessible space error detection

图14 KITTI数据集可通行区域检测效果图Fig.14 Results of accessible space detection with KITTI dataset

图15 现实场景可通行区域检测效果图Fig.15 Results of accessible space detection in real scene

4 总结

通过将单目视觉图像中提取的特征点作为障碍物标记，研发了一套基于单目特征点法的纯视觉导航算法——ASDM-Mono，只需单目相机便可构建视觉代价地图并获得可通行区域和可通行方向，可用于实现三维避障功能，弥补了激光雷达维度信息缺失、价格昂贵以及深度强化学习方案训练成本高的缺点。该方法可准确计算得到机器人前方区域各个位置的通行代价，最终得到的可通行区域可用于障碍物判别，视觉代价地图可用于路径规划。在未来的实验中，可考虑将该方法移植到移动端和无人机上，扩展为未知环境中的定点探索任务，并且添加动态障碍物的避障功能。