搜索算法主要有广度优先和深度优先算法。这里主要讲广度优先算法及其工程上的实现。 主要从以下几个方面来说明：

• 图：保存待搜索数据的数据结构。
• 广度优先搜索的搜索方法。
• 广度优先搜索的实现方法。

图包括有向图和无向图，都包括节点和边组成。而搜索就是从一个节点顺着边到下一个节 点，这里的下一个对象就是节点的邻居。有向图只有箭头所指算是该节点的邻居，而无向 图则是边的两边互为邻居。

在移动机器人规划的时候，地图通常是使用二维数组表示的，每个像素的周围四个或者八 个像素都是邻居。这其实就是一个无向图了。也就是说地图导航搜索路径我们使用一个二 维数组来表示节点信息。

先比较一下广度优先搜索和深度优先搜索。

广度优先搜索是从起始节点开始，先搜索最近的节点(一级节点)，如果节点不满足要求就 把该节点的其他节点(邻居)加入待搜索的表中，最近的所有节点都没有搜索到需要的结果， 再搜索每一一级节点的下一级节点(二级节点)。以此方法直到得到需要的结果，或者搜索 完全部的节点。

而深度优先搜索则是从起始节点开始顺着一个方向一直搜索到最后，然后在退回下一级继 续往下搜索，直到得到最终结果或者搜索完所有节点。

用上面的图举个粒子：

如果是广度优先，那么搜索的顺序就是：

you -> Claire -> Alice -> Bob -> Thom -> Jonny -> Peggy -> Anuj

如果是深度优先，那么搜索的顺序就是：

you -> Claire -> Thom -> Jonny -> Alice -> Peggy -> Bob -> Anuj

对比两种方法可以发现，广度优先是起始点开始按圈搜索，深度优先则是针一样的搜索。 前者可以得到和起始节点最近的结果。而后者得到的结果可能不会是离起始节点最近的结 果。

如果要直到得到搜索结果才停止，那么两者都可以通过搜索确定是否有符合要求的结果存 在，只是前者可以得到最短，最近的结果，后者则不一定。因此广度搜索可以完成两个任 务：

• 是否存在符合结果的搜索路径；
• 如果存在符合的路径，那么广度搜索可以找到最近的结果。

在工程实现上(比如ROS导航中的costmap代价地图规划路径)，需要几个要素：

• 图(保存节点和边的信息)： costmap中实现二维数组来表示。这是搜索的原始信息。
• 队列(双向队列)：保存需要搜索的节点。每次查完一个节点之后，如果不符合要求就要把该节点的 下一级节点加入队列。队列是先入先出的，查完一个节点之后就将这个节点从队列中弹 出。
• 为了避免查找到已查过的节点，需要一个标记每个节点是否被查过的表。这样避免出现 死循环。在costmap中使用一个和地图一样大小的数组来标记，如果某个节点被查过就 在对应的index的位置做个标记。每次将一个节点加入队列的时候都先查一下这个表， 如果已经查过就不用加入队列。

当然costmap里面还涉及了权重(代价值)，那是进一步要讲的东西。只是大体上是一样的。