基于SSD的自动路径规划算法

1、场景需求

  随着数据挖掘、机器学习和深度学习等技术的兴起，它们为家用机器人提供了更敏锐的听觉和视觉，使得它们可以完成一些更加复杂的任务，不仅仅是简单的扫地、抓取等。自动路径规划是家用机器人中的一个关键技术，它的主要作用是根据用户设定的起点和终点在尽可能短的时间内寻找到一个最短或者最优的路径，从而提高机器人的工作效率。

2、路径规划算法简介

2.1 、PRM算法简介

  PRM是一种经典的自动路径识别算法，该算法的主要特点是基于随机采样技术的，可以有效的解决高维空间个和复杂场景约束下的自动路径规划问题。该算法是一种基于图搜索的路径规划算法，主要包含两个大的步骤，即学习和查询阶段。该算法的主要思想是首先将一个连续空间转换为一个离散空间；然后使用A*等其它的搜索算法在路径图中寻找最优的路径，从而在一定程度上提升该算法的搜索效率。这种算法的主要优势是可以借助采样技术使用很少的点来找到一个满意的解，它是基于这样的一个前提的，即对于现实中的大多数问题而言，使用少量的样本其实就可以覆盖大多数的可行空间，并且随着样本数量的不断增加，找到最优路径的概率接近1。该算法的一个缺点是当数据中的采样点的个数比较少或者数据的分布情况不太合理时，该算法可能并不能找到最优的路径，在这种情况下就需要增加样本的个数。该算法的实现步骤如下所示：

• 学习阶段，即在给定图的自由空间里随机撒点（自定义个数），构建一个路径网络图。通常包括构造和扩张两个步骤。
  
• 查询阶段，即使用其它的算法来查询从某一个起点到某一个终点的路径是否存在，通常需要进行局部路径的规划、计算距离和碰撞检测等操作，最终的规划结果如下图所示。
  

2.2、RRT算法简介

  RRT算法是一个经典的路径规划算法，提出该方法的主要目的是为了克服传统路径算法中需要在一个特定的空间中对障碍物进行建模，计算复杂度比较高和不适用于解决多自由度机器人在复杂场景中的路径规划问题。RRT算法的主要思路是对状态空间中的采样点进行碰撞检测，这行可以有效的避免对空间场景的建模，从而极大的降低算法对维度空间的要求，适合于解决复杂场景中的路径规划问题。该算法的主要特点是能够在高维数据空间中进行有效、快速的搜索，利用状态空间中的随机采样点来使得搜索的方向不断的向空白区域靠近，从而寻找出一条最优的路径。该算法和PRM算法具有类似的缺点，即该算法是概率完备的但并不是最优的，有时候并不能找到最优的路径。该算法的实现流程如下所示：

• 步骤1-首先产生一个属于$X_{\text{free }}$的节点${\mathcal{X}}_{init}$，在当前的循环中产生一个随机点$x_{rand}$，这个随机点是在整个状态空间$X$内的任意点；
• 步骤2-通过上一步获得随机点之后，遍历随机树中的每一个节点，计算出每一个节点和随机点之间的距离，并寻找出距离该随机点最近的一个节点${\mathcal{X}}_{\text{near }}$；
• 步骤3-定义一个步进量EPS，当找到${\mathcal{X}}_{\text{near }}$时，${\mathcal{X}}_{\text{near }}$向${\mathcal{X}}_{\text{near }}$和${\mathcal{X}}_{\text{rand}}$连接方向扩展EPS步长，扩展后产生一个新的节点$x_{new}$；
• 步骤4-判断新的节点${\mathcal{X}}_{new}$是否满足非完整微分约束，如果不满足，则需要舍弃掉${\mathcal{X}}_{new}$，重新生成一个新的随机点；如果满足非完整微分约束条件，则需要将${\mathcal{X}}_{new}$加入其中，并在${\mathcal{X}}_{near}$和${\mathcal{X}}_{new}$之间添加上一条新的边；
• 步骤5-在插入新节点的过程中，当${\mathcal{X}}_{\text{new }}$,${\mathcal{X}}_{\text{near }}$和${\mathcal{X}}_{\text{near }}$到${\mathcal{X}}_{\text{new }}$节点之间的任意一条边位于$X_{\mathrm{obs}}$中或者和$X_{\mathrm{obs}}$相交，则跳过次数的循环，并在下一轮的循环中生成一个新的随机点${\mathcal{X}}_{new}$，并重复的执行判断操作，如果该节点属于${\mathcal{X}}_{free}$则保留这个新节点；
  
    上图展示了RRT算法的效果，图中蓝色的点表示障碍物，红色的路径表示RRT算法寻找到的最优路径，其它的表示RRT算法在搜索过程中的一些中间结果，我们可以观察到这个搜索结果比较平滑，基本上可以满足我们的要求。

3、基于SSD的自动路径规划算法简介

  对于家居机器人的自动路径识别问题而言，我们的主要任务是对家居环境中的场景进行机器人的自动路径规划，但是这里不仅仅涉及到自动路径规划算法的实现，我们首先需要实时的获取到家居环境中的图片或者视频，这就要求我们整个系统中需要配备一个外置摄像头来进行家居环境的图片或者视频的捕获；在获取到我们需要的图片或者视频之后，我们的下一个关键问题是如何对这些图片或者视频中的障碍物进行准确的识别和定位，为了解决这个问题，首先我们需要知道什么是障碍物；然后我们通过哪种方式来快速的捕获这些障碍物。对于家居环境下的障碍物而言，主要包括桌子，椅子，瓶子和人等；为了获取到这些物体在图片或者视频中的具体位置，我们尝试着使用目标检测算法来获取图像或者视频中的目标，为了获得实时的检测速度，本文使用了单阶段的SSD目标检测算法，我们将目标检测算法输出的结果作为家居环境下的障碍物，目标检测算法最终的输出是目标的类别和相应的位置信息，这刚好满足我们这个任务的需要；在获取到图片或者视频中的障碍物的具体位置信息之后，我们需要去根据这些障碍物生成一张二维映射平面图，即整个图片中仅仅包含障碍物。为了解决这个问题，本文将这些检测的BB绘制在一张1000x1000大小的图上面，将这个边界框所在的区域绘制成全黑色，其它的区域用白色表示，这样我们就可以获得一张简单的二维平面映射图，即将真实环境中的图片和视频转化成对应的障碍物平面图;；在获得了平面图之后，我们要做的就是实现经典的自动路径规划算法并使用这些算法在获取到的映射图上面执行路径规划操作并展示最终的结果。整个系统的实现流程如下所示：

  上图展示了基于家居机器人的自动路径规划系统设计流程，整个系统可以总结为以下4个主要的步骤，即利用外置摄像头获取图像或者视频、利用目标检测算法进行障碍物的识别和定位、根据目标检测结果制作二维平面映射图和利用自动路径算法进行路径规划和结果可视化。下面将对这些步骤的实现细节和实现效果进行详细的展示。

4、基于SSD的自动路径规划算法详解

4.1、利用外置摄像头获取图像或者视频

  整个系统中的第一步就是图像或者视频的获取，为了获取到图像或者视频，我们可以采用的方案比较多，具体包括利用普通摄像头获取视频、利用深度摄像头获取视频、利用双目摄像头获取视频、利用三目摄像头获取视频的等。总而言之，使用越复杂的摄像头能够获得更加丰富的纹理信息，而这些信息可以给后续的处理算法提供一些辅助信息，考虑到系统的成本和通用性等多个因素，本系统最后选择使用普通的摄像头来完成家居环境下视频的实时采集任务。主要的实现方式是通过Matlab中提供的图像获取工具箱中的接口来实现视频的捕获和保存工作，即vid = videoinput (‘winvideo’, 1, ‘YUY2_640x480’)，其中videoinput是Matlab中的捕获视频的接口，1表示当前摄像头的ID号，每一个摄像头都会有特定的ID号，YUY2_640x480表示视频的具体格式，vid表示获取到的视频对象；然后使用preview函数来对获取到的视频进行预览，即preview(vid,hImage)，其中preview是相应的预览函数，hlmage是提前定义好的一个图像，vid就是前面的视频对象；最后我们可以使用VideoWriter函数来是对捕获到视频进行保存，即writerObj = VideoWriter( [filename ‘.avi’] )，其中filename 表示保存视频的具体名字，.avi表示保存的视频格式，writerObj 是输出的写对象。使用外置摄像头获取都的室内场景的图片如下所示：

4.2、利用目标检测算法进行障碍物的识别和定位

  在获取到室内场景的图片或者视频之后，对于我们的任务而言，我们需要检测出图片中的障碍物的信息，具体包括目标的类别和位置信息。为了获取到这些信息，本文尝试着使用目标检测器来快速的获得我们需要的障碍物的信息，尽管当前已经有很多经典的目标检测算法，但是由于我们使用的环境是Matlab，使用的深度学习库是Matconvnet，支持它们的目标检测算法十分有限，尽管Matconvnet提供了一个预训练好的fast r-cnn预训练模型，但是经过实验之后我们发现这个模型并不能满足我们的实际要求，具体的原因包括：该模型的运行速度比较慢，不能满足实时场景的需要；该模型的检测精度比较低，并不能准确的获取到图像中的障碍物；该模型能够识别的目标类别比较少，并不能我们的需求。针对以上的这些原因，本文最终实现了一个基于单阶段检测SSD目标算法，该算法不仅具有较快的检测速度，同时可以提供准确的检测结果，基本上可以满足整个系统的需要。
  为了实现SSD目标检测器，我们首先收集了一个含有标签的目标检测数据集；然后通过Matconvnet深度学习开源库进行模型的训练，最终保存最好的一个SSD模型；接着我们直接使用预训练好的SSD模型来进行简单的前向推理操作就可以获得我们需要的目标检测结果。下面对该模型的检测结果进行展示。
  上图展示了本文实现的SSD目标检测算法的检测效果。第1行第1列的图片是一个室外的场景图片，该图片中包含的目标是一个人和一辆摩托车，SSD准确的检测出了这两个目标，以99%的自信度确定这个目标，并输出准确的边界框信息；对于第1行第2列的室内场景图片而言，图片中的目标包括4把凳子和1张桌子，SSD算法都准确的检测出了这些目标，由于受到部分的遮挡，仅以24%的概率确定某一个目标是一把椅子；对于第2行第1列的家居场景图片而言，图片中包含着3把椅子和1张桌子，有两把椅子受到了桌子的严重的遮挡，而本文实现的SSD算法仍然可以准确的检测出这些障碍物，并且输出了较大的自信度；对于第2行第2列的家居场景图片而言，图片中包含了4把椅子和1张桌子，本文实现的SSD可以准确的检测到这些障碍物，并输出准确的边界框。通过以上的观察和分析，我们可以得出一个初步的结论，即本文实现的SSD目标检测算法可以准确的检测到家居环境中的障碍物，并输出一个准确的边界框，这个边界框可以有助后续的二维平面映射图的生成。

4.3、根据目标检测结果制作二维平面映射图

  通过SSD目标检测器我们可以准确的获取到图片中障碍物的类别和位置信息，而对于基于家居机器人的自动路径规划问题而言，我们需要的是在某一个平面映射图中的执行自动路径规划的任务，即在选择出起始位置和终止位置之后，算法通过大量的查找最终会输出一个最优的路径。即我们需要将获取的图像转换为一个仅仅包含障碍物的二维平面映射图。本文的主要思路是将目标检测的结果看做家居场景中的障碍物，然后在一个大小为1000X1000图像中将检测到的边界框所在的位置填充上黑色，其它的位置填充为白色，通过以上的这个操作，我们就可以将原始的输入图片转化为相应的二维平面映射图。下面对本文实现的映射图进行效果展示和分析。

  上图对视频中捕获到的连续3帧的效果进行了展示，第1列表示的是外置摄像头捕获到的真实场景图片；第2列表示的是使用SSD目标检测算法检测的结果，包括障碍物的类别和边界框信息；第3类表示使用本文的转换方法获得的二维映射平面，该平面中仅仅包含障碍物，由于本文的摄像头的分辨率最大支持640x480，而生成的映射图的大小为1000x1000，因而在转换的过程中需要使用到线性插值操作。通过下面的结果我们可以观察到SSD算法在真实场景中的图片中能够获得比较好的检测结果，准确的检测出图片中的障碍物，通过简单的映射转换我们就可以获得我们需要的二维平面映射图，图中黑色部分表示障碍物，白色的部分表示可行区域，后续我们将在该图的基础上进行自动路径规划和效果展示。

4.4、利用GUIDE制作的自动路径规划GUI效果展示和分析

  通过以上的步骤我们可以获得路径规划中需要的二维平面映射图，接下来我们的主要工作就是集中在如何实现、使用和展示经典的自动路径规划算法。为了方便用户的使用，本文将自动路径规划的功能整合到了一个GUI界面上面，整个GUI界面是使用Matlab的GUIDE工具实现的，该界面的主要功能包含原始二维映射平面的选择、起始点和终止点的设置或选择、自动路径规划算法的选择和自动路径规划结果的展示。下面对本文实现的GUI界面进行简单的介绍和效果展示。

5、基于SSD的自动路径规划算法效果展示与分析

5.1、RRT算法在复杂场景下的路径规划结果分析

  上图展示了RRT算法在一种比较复杂的场景下面的路径规划效果展示。当前的平面图中包含有圆形障碍物、矩形障碍物和三角形的障碍物，选择的起始坐标位置是（74.24,750），选择的终止坐标为（377.2, 143.9），对于这种场景而言，RRT输出的路径中虽然成功的躲开了所有的障碍物，但是该算法获得最优路径并不是最优的，当然也并不是最短的，主要的原因是因为该算法是基于随机采样的思路的，它在很大程度上可以保证获得最终的路径，但是该路径其实并不一定就是最优的。

  上图展示了RRT算法在一种复杂场景下面的路径规划效果展示。当前的平面图中障碍物是自己绘制的一个比较复杂的障碍物，选择的起始坐标位置是（0,100），选择的终止坐标为（400,0），对于这种场景而言，RRT输出的路径中虽然成功的躲开了所有的障碍物，并获得一个比较好的路径规划结果，但是在拐弯的地方RRT算法输出的结果并不是最优的，获得的曲线不是很平滑，具有一定的改进空间。

  上图展示了RRT算法在一种复杂场景下面的路径规划效果展示。当前的平面图中障碍物是自己绘制的一个比较复杂的障碍物，选择的起始坐标位置是（0,0），选择的终止坐标为（128.7,37.87），对于这种场景而言，RRT输出的路径中虽然成功的躲开了所有的障碍物，并获得一个比较好的路径规划结果，但是在拐弯的地方RRT算法输出的结果并不是最优的，获得的曲线不是很平滑，具有一定的改进空间。

  上图展示了RRT算法在一种复杂场景下面的路径规划效果展示。当前的平面图是通过目标检测的结果绘制的，选择的起始坐标位置是（25.75,13.63），选择的终止坐标为（495.4,343.9），对于这种场景而言，RRT输出的路径中虽然成功的躲开了所有的障碍物，并获得一个比较好的路径规划结果，但是算法在寻找终点的时候花费了较长的时间，走了很多不必要的路，浪费了很长的时间，因而该算法的时间效率不是特别高，具有一定的改进空间。

5.2、双向RRT算法在复杂场景下的路径规划结果分析

  上图展示了双向RRT算法在一种比较复杂场景下面的路径规划效果展示。当前的平面图是通过目标检测的结果绘制的，选择的起始坐标位置是（56.06,13.63），选择的终止坐标为（853.0,13.63），对于这种场景而言，双向RRT很快就输出了最优的路径路径，图中展示的路径看起来比较平滑；对于同样的场景而言，RRT算法则需要较长的搜索时间，搜索的结果并不平滑，这在一定程度上展示了双向RRT算法的优势。

  上图展示了双向RRT算法在一种复杂场景下面的路径规划效果展示。当前的平面图是通过绘制得到的，整个平面中的障碍物比较复杂，具有多个峰值和峰谷，这些峰值和峰谷的存在会对RTT算法的查找造成一定的困难。选择的起始坐标位置是（22.72,59.09），选择的终止坐标为（534.8,7.575），对于这种场景而言，双向RRT很快就输出了最优的路径路径，图中展示的路径看起来比较平滑；对于同样的场景而言，RRT算法则需要较长的搜索时间，搜索的结果并不平滑，这在一定程度上展示了双向RRT算法的优势。

  上图展示了双向RRT算法在一种复杂场景下面的路径规划效果展示。当前的平面图是通过目标检测的结果绘制的，整个平面中的障碍物比较复杂，具有多个不规则的障碍物，为了增加复杂度，选择的起始坐标位置是（16.66,59.09），选择的终止坐标为（495.4,346.9），对于这种场景而言，双向RRT很快就输出了最优的路径路径，图中展示的路径看起来比较平滑；对于同样的场景而言，RRT算法则需要较长的搜索时间，搜索的结果并不平滑，这在一定程度上展示了双向RRT算法的优势。

5.3、PRM算法在复杂场景下的路径规划结果分析

  上图展示了PRM算法在一种复杂场景下面的路径规划效果展示。当前的平面图是通过目标检测的结果绘制的，整个平面中的障碍物比较复杂，具有多个不规则的障碍物，为了增加复杂度，选择的起始坐标位置是（22.72,383.3），选择的终止坐标为（265.1,219.6），对于这种场景而言，PRM很快就输出了最优的路径路径，即图中最粗的一条线段，图中展示的中间结果包括该算法搜索的一些路径。我们可以观察到该算法最终获得的路径还是比较平滑的，基本上是一条最优的路径。和RRT算法，该算法的运行效率会比较高，可以在很短的时间内获得一个最优的路径。

  上图展示了PRM算法在一种复杂场景下面的路径规划效果展示。当前的平面图是通过手工绘制得到的，整个平面中的障碍物比较复杂，具有多个不规则的障碍物，为了增加复杂度，选择的起始坐标位置是（10.60,62.12），选择的终止坐标为（768.1,46.96），对于这种场景而言，PRM并没有找到最优的路径，主要的原因可能是障碍物太过复杂，这在一定的程度上显示了该算法的缺陷，即当场景中的障碍物比较复杂时，PRM算法可能并不能找到最优的路径。

  上图展示了PRM算法在一种复杂场景下面的路径规划效果展示。当前的平面图是调用平面模板1获得的，整个平面中的障碍物比较复杂，为了增加复杂度，选择的起始坐标位置是（0,0），选择的终止坐标为（1000,1000），对于这种场景而言，PRM很快就输出了最优的路径路径，即图中最粗的一条线段，图中展示的中间结果包括该算法搜索的一些路径。我们可以观察到该算法最终获得的路径还是比较平滑的，基本上是一条最优的路径。和RRT算法，该算法的运行效率会比较高，可以在很短的时间内获得一个最优的路径。

6、总结与分析

  本文实现的家居机器人自动路径规划系统巧妙的将视频捕获、障碍物检测、制作平面映射和路径规划算法结合起来，能够很好的处理简单的场景和稍微复杂的平面映射图，同时具有实时的处理速度，可以应用到多个现实场景中，具有较广的实际应用价值。
  本文实现的基于家居机器人的自动路径规划系统的应用场景主要是针对室内的，而现实场景中的需求不仅包括室内环境，同时也会包含大量的室外场景，因而将本文提出的系统扩展到室外场景中是一个比较好的研究方向；
  本文实现的基于家居机器人的自动路径规划系统仅仅依赖于单个普通的摄像头来获取家庭的环境信息，而单个摄像头能够获取到的信息往往比较有限，而多个摄像头的协作可以捕获到更多有用的信息，另外室内场景的深度和红外等其它信息有助于场景中障碍物的识别，因而加入多目相机或深度相机等其它的信息可以进一步提升该系统的性能；
  本文实现的基于家居机器人的自动路径规划系统中仅仅使用简单的目标检测算法来进行障碍物的检测，而本系统中使用到的目标检测算法仅仅能够识别出有限种的障碍物，而现实场景中通常会存在很多种障碍物，检测这些障碍物是进行自动路径规划的关键一步。因此尝试着使用其它的障碍物识别算法可以进一步提升该系统的性能；
  本文实现的基于家居机器人的自动路径规划系统的整个过程都是在二维坐标系中执行的，而我们生活在一个三维的环境中；除此之外，自动路径规划的应用场景大多都是三维场景中，因而实现一个3D的自动路径规划系统具有更大的实用性；
  本文实现的基于家居机器人的自动路径规划系统中仅仅包含了几种经典的自动路径规划算法，这些算法的整体性能比较一般，作为一个系统应当包含足够多的算法，这样就可以根据真实场景的需要使用合适的算法，从而规划出一个更好的路径，因而加入更多的自动路径规划算法也是一件值得去做的事情。

注意事项

[1] 如果您对AI、自动驾驶、AR、ChatGPT等技术感兴趣，欢迎关注我的微信公众号“AI产品汇”，有问题可以在公众号中私聊我！
[2] 该博客是本人原创博客，如果您对该博客感兴趣，想要转载该博客，请与我联系（qq邮箱：1575262785@qq.com）,我会在第一时间回复大家，谢谢大家的关注.
[3] 由于个人能力有限，该博客可能存在很多的问题，希望大家能够提出改进意见。
[4] 如果您在阅读本博客时遇到不理解的地方，希望您可以联系我，我会及时的回复您，和您交流想法和意见，谢谢。
[5] 本文测试的图片可以通过关注微信公众号AI产品汇之后向我索取！
[6] 本人业余时间承接各种本科毕设设计和各种小项目，包括图像处理（数据挖掘、机器学习、深度学习等）、matlab仿真、python算法及仿真等，有需要的请加QQ：1575262785详聊，备注“项目”！！！