博客转载自：http://blog.csdn.net/u012907049/article/details/74980015
前言
随着人工智能技术的日益发达，我们的生活中也出现了越来越多的智能产品。我们今天要关注的是智能家居中的一员：扫地机器人。智能扫地机器人可以在主人不在家的情况下自动检测到地面上的灰尘，并且进行清扫。有些更为对路线进行规划，找到可以清理灰尘的最短路径，达到省电的效果。当然，绕过障碍物也是必须拥有的技能。我们今天就来看一下扫地机器人自动寻路的算法的简单实现。这里我们不对机器人如何识别出灰尘进行讨论，我们只讨论发现了灰尘之后，机器人的路径规划进行一个分析。为了简单起见，我们假设机器人所处在的是一个M×N的格子的房间中，其中某些格子是灰尘，某些格子是障碍物，而另外有些格子则是单纯的空地。 我们再抽象一些，设星号(*)表示灰尘，井号(#)表示障碍物，下划线(_)表示空地，而我们的机器人所在的初始格子（当然首先要是一块空地）用at(@)表示。比如如下的图示
*#_* _*__ _#_@
表示在一个3×4的空间内（俯视图），第一行有两个灰尘和一个障碍物，第二行有一个灰尘，第三行有一个障碍物， 而我们的机器人在右下角这个位置。它的目标就是在不经过任何障碍物的格子的情况下，用尽量少的步数，把房间中的灰尘都清除掉。我们假设机器人每次行动一个格子会消耗一点的行动力，当机器人处在灰尘所在的格子上时，清除这个格子里面的灰尘也消耗一点行动力。
正文
问题分析
广度优先搜索（breadth-first search）是解决自动寻路功能的算法之一。作为一种常见的图形搜索算法，它也被广泛应用于解决其他各类算法问题。一般情况下，对于一个节点，它的邻居节点的集合被称作open list，而在这个节点被遍历之前，其他所有已经遍历过的节点存于close list中。
算法描述如下：
开始
将顶点入队列
循环
当队列为非空时，继续执行，否则算法结束
出队列取得队列头顶点V；访问并标记为已访问
查找列头顶点V的所有邻接顶点W1，W2，...Wn
对于上述的所有每一个顶点Wn
循环
若Wn在close list中， 则继续遍历下一个顶点Wn+1
否则将Wn入队列，并加入到close list中
代码
首先我们建立一个坐标点的类，用于表示一个点的坐标。
#ifndef _POINT_H_
#define _POINT_H_
class Point
{
public:
Point(int x, int y)
{
this->x = x;
this->y = y;
}
public:
int getX()
{
return x;
}
int getY()
{
return y;
}
void setX(int x)
{
this->x = x;
}
void setY(int y)
{
this->y = y;
}
static bool isSamePoint(Point p1, Point p2)
{
if (p1.x == p2.x && p1.y == p2, y)
return true;
else
return false;
}
private:
int x, y;
};
#endif //_POINT_H_
接下来就是比较重要的节点类，这里我们用state表示，state的意思为状态，也可以理解为当前屋子里的一个状态，比如当前的节点下我们的机器人的位置、房间内剩余的灰尘格子数量等等。
#ifndef _STATE_H_
#define _STATE_H_
#include"Point.h"
#include<string>
#include<list>
using namespace std;
class State
{
public:
State()
{
previousState = NULL;
}
~State() {}
private:
//机器人位置
Point robotPosition;
//操作,分为W(向上移动一格), S(向下移动一格), A(向左移动一格), D(向右移动一格)以及C(清理灰尘)
string robotOperation;
//当前节点的父节点, 用于达到目标后进行回溯
State* previousState;
//灰尘所在坐标的list
list<Point> dirtList;
public:
Point getRobotPosition()
{
return robotPosition;
}
void setRobotPosition(const Point& p)
{
robotPosition = p;
}
string getRobotOperation()
{
return robotOperation;
}
void setRobotOperation(string command)
{
robotOperation = command;
}
State* getPreviousState()
{
return previousState;
}
void setPreviousState(State* state)
{
this->previousState = state;
}
list<Point> getDirtList()
{
return this->dirtList;
}
void setDirtList(list<Point> list)
{
this->dirtList.clear();
this->dirtList = list;
}
//用于判断两个节点是否相同
static bool isSameState(State s1, State s2)
{
//若机器人位置不同,则节点不同
if (!Point::isSamePoint(s1.robotPosition, s2.robotPosition))
return false;
//若灰尘列表长度不同, 则节点不同
else if (s1.dirtList.size() != s2.dirtList.size())
return false;
//若前两者都相同, 则判断两个state中的灰尘列表中的灰尘坐标是否完全相同
else
{
for (Point p : s1.getDirtList())
{
bool same = false;
for (Point p2 : s2.getDirtList())
{
if (Point::isSamePoint(p, p2))
{
same = true;
}
}
if (!same)
{
return false;
}
}
//若满足上述所有条件, 则两节点相同。
return true;
}
}
};
#endif //_STATE_H_
为了避免机器人走不必要的回头路（比如永远在两个空格之间徘徊），我们需要一个写一个方法来判断两个state是否相同，这样我们才能判断一个state是否存在于close list中。具体的判断规则就是上面的代码中isSameState所写的那样。这样我们就可以避免机器人的徘徊的情况：当机器人经过同一个坐标时，要么比之前多清除了一个或几个灰尘，要么是两个并列的状态，两个状态中清理的灰尘数量相同，但清理的灰尘所在格子不一样（比如房间中有四个灰尘格子，第一个平行状态下是其中两个灰尘被清理，另一个状态是另外两个灰尘被清理）。如果不是上述两种情况之一，那么这个坐标就不应该再次被机器人经过。
下面是最重要的算法实现类，其中也包括了简单的对用户的输入进行整理，即用户可自定义房间地图。
#ifndef _ROBOT_H_
#define _ROBOT_H_
#include<stack>
#include<queue>
#include<iostream>
#include"State.h"
using namespace std;
class Robot
{
private:
Robot();
~Robot();
public:
static Robot* instance()
{
static Robot ins;
return &ins;
}
public:
int coloumnNum;
int rowNum;
int obstacleNum;
//用于深度优先搜索的栈
stack<State> depthSearch;
//用于广度优先搜索的队列
queue<State> breadthSearch;
//地图
char map[10][10];
//灰尘坐标列表
list<Point> dirtList;
//closeList，用于存放已经存在的state
list<State> closeList;
//遍历总耗费
int cost;
//判断地图中当前位置的灰尘在这个state中是否已经被除去。
bool isCleared(Point& p, list<Point>& pList);
//判断节点是否存在,即将state与closeList中的state相比较,若都不相同则为全新节点
bool isDuplicated(State* state);
//输出,由最后一个state一步一步回溯到起始state
void output(State* state);
//判断是否已经达到目标,即当前遍历到的state中手否已经没有灰尘需要清理
bool isCompleted(State* state);
void caculate(State* state);
};
#endif //_ROBOT_H_
Robot.cpp
#include"Robot.h"
Robot::Robot()
{
coloumnNum = 0;
rowNum = 0;
cost = 0;
}
Robot::~Robot()
{
}
bool Robot::isCleared(Point& p, list<Point>& pList)
{
for (Point pElement : pList)
{
if (Point::isSamePoint(pElement, p))
{
return false;
}
}
return true;
}
bool Robot::isDuplicated(State* state)
{
for (State state2 : closeList)
{
if (State::isSameState(*state, state2))
return true;
}
return false;
}
bool Robot::isCompleted(State* state)
{
if (state->getDirtList().empty())
return true;
return false;
}
void Robot::output(State* state)
{
string outputStr = "";
//回溯期间把每一个state的操作(由于直接输出的话是倒序)加入到output字符串之前,再输出output
while (state != NULL)
{
if (!state->getRobotOperation().empty())
outputStr = state->getRobotOperation() + "
" + outputStr;
state = state->getPreviousState();
}
//最后输出遍历过的节点(state)数量
cout << outputStr << endl;
//最后输出遍历过的节点(state)数量
cout << cost << endl;
}
void Robot::caculate(State* state)
{
//获取当前机器人的坐标
int x = state->getRobotPosition().getX();
int y = state->getRobotPosition().getY();
//如果当前的点是灰尘并且没有被清理
if (map[x][y] == '*' && !isCleared(Point(x, y), state->getDirtList()))
{
State* newState = new State();
list<Point> newdirtList;
//在新的state中,将灰尘列表更新,即去掉当前点的坐标
for (Point point : state->getDirtList())
{
if (point.getX() == x && point.getY() == y)
continue;
else
newdirtList.push_back(Point(point.getX(), point.getY()));
}
newState->setDirtList(newdirtList);
newState->setRobotPosition(Point(x, y));
//C代表Clean操作
newState->setRobotOperation("C");
newState->setPreviousState(state);
//若新产生的状态与任意一个遍历过的状态都不同,则进入队列
if (!isDuplicated(newState))
{
Robot::instance()->breadthSearch.push(*newState);
closeList.push_back(*newState);
cost++;
}
}
//若当前机器人坐标下方有格子并且不是障碍物
if (x + 1 < rowNum)
{
if (map[x + 1][y] != '#')
{
State* newState = new State();
newState->setDirtList(state->getDirtList());
newState->setRobotPosition( Point(x + 1, y));
//S代表South,即向下方移动一个格子
newState->setRobotOperation("S");
newState->setPreviousState(state);
if (!isDuplicated(newState))
{
//stack.push(newState);
Robot::instance()->breadthSearch.push(*newState);
//加入到closeList中
closeList.push_back(*newState);
cost++;
}
}
}
//若当前机器人坐标上方有格子并且不是障碍物
if (x - 1 >= 0)
{
if (map[x - 1][y] != '#')
{
State* newState = new State();
newState->setDirtList(state->getDirtList());
newState->setRobotPosition(Point(x - 1, y));
//W代表向上方移动一个格子
newState->setRobotOperation("W");
newState->setPreviousState(state);
if (!isDuplicated(newState))
{
//stack.push(newState);
Robot::instance()->breadthSearch.push(*newState);
//加入到closeList中
closeList.push_back(*newState);
cost++;
}
}
}
//若当前机器人坐标左侧有格子并且不是障碍物
if (y - 1 >= 0)
{
if (map[x][y - 1] !='#')
{
State* newState = new State();
newState->setDirtList(state->getDirtList());
newState->setRobotPosition(Point(x , y-1));
//A向左侧移动一个格子
newState->setRobotOperation("A");
newState->setPreviousState(state);
if (!isDuplicated(newState))
{
//stack.push(newState);
Robot::instance()->breadthSearch.push(*newState);
//加入到closeList中
closeList.push_back(*newState);
cost++;
}
}
}
//若当前机器人坐标右侧有格子并且不是障碍物
if (y + 1 < coloumnNum)
{
if (map[x][y + 1] != '#')
{
State* newState = new State();
newState->setDirtList(state->getDirtList());
newState->setRobotPosition(Point(x, y + 1));
//D右侧移动一个格子
newState->setRobotOperation("D");
newState->setPreviousState(state);
if (!isDuplicated(newState))
{
//stack.push(newState);
Robot::instance()->breadthSearch.push(*newState);
//加入到closeList中
closeList.push_back(*newState);
cost++;
}
}
}
}
main.cpp
#include<iostream>
#include"Robot.h"
using namespace std;
int main(int argc, char** argv)
{
int coloumnNum;
int rowNum;
State* initialState = new State();
cout << "Please enter coloumn number: " << endl;
cin >> coloumnNum;
Robot::instance()->coloumnNum = coloumnNum;
cout << "Please enter row number: " << endl;
cin >> rowNum;
Robot::instance()->rowNum = rowNum;
string str;
for (int row = 0; row < rowNum; row++)
{
cout << "Plese enter the elements in row: " << row << endl;
cin >> str;
for (int coloumn = 0; coloumn < coloumnNum; coloumn++)
{
if (str[coloumn] == '#')
{
Robot::instance()->obstacleNum++ ;
}
if (str[coloumn] == '*')
{
Robot::instance()->dirtList.push_back(Point(row, coloumn));
}
if (str[coloumn] == '@')
{
initialState->setRobotPosition(Point(row, coloumn));
}
Robot::instance()->map[row][coloumn] = str[coloumn];
}
}
initialState->setDirtList(Robot::instance()->dirtList);
//stack.push(initialState);
Robot::instance()->closeList.push_back(*initialState);
//Robot::instance()->depthSearch.push(initialState);
Robot::instance()->breadthSearch.push(*initialState);
Robot::instance()->cost++;
//遍历开始
while (!Robot::instance()->breadthSearch.empty())
{
//取出队列中第一个state
State* state = new State();
*state = Robot::instance()->breadthSearch.front();
Robot::instance()->breadthSearch.pop();
//如果达到目标,输出结果并退出
if (Robot::instance()->isCompleted(state))
{
Robot::instance()->output(state);
Robot::instance()->closeList.clear();
while (state != NULL)
{
State* temp = state->getPreviousState();
delete state;
state = temp;
}
system("pause");
return 0;
}
Robot::instance()->caculate(state);
}
system("pause");
return 0;
}
我们可以看出，对于一个state，机器人能进行的操作无非只有左移、右移、上移、下移以及清理灰尘这个五种。当一个state从队列中被poll出来之后，这五个动作进行之后产生的五个新的state就是这个state的open list，我们对于这五个state进行判断，如果动作合理（即不能移动到边界之外或障碍物之上）并且新产生的state不在close list里，那么就把新产生的state添加到队列中去，并同时加入到close list中。下面是测试的输出：
Please Enter Row Number: 3 Please Enter Colomn Number: 3 Please Enter the Elements in row 1: @#* Please Enter the Elements in row 2: *__ Please Enter the Elements in row 3: #*_ S C E S C N E N C 45
即对于一个3×3的空间
@#* *__ #*_
机器人选择的路线是：
下移(S) -> 清理(C) -> 右移(E) ->下移(S) -> 清理(C) -> 上移(N) ->右移(E) ->上移(N) -> 清理(C)。
45是程序遍历过的节点（state）数量。
下面我们再来看文章开头的例子：
*#_* _*__ _#_@
程序的输出为
Please Enter Row Number: 3 Please Enter Colomn Number: 4 Please Enter the Elements in row 1: *#_* Please Enter the Elements in row 2: _*__ Please Enter the Elements in row 3: _#_@ N N C S W W C W N C 56
机器人选择的路线是：
上移(N) -> 上移(N) -> 清理(C) ->下移(S) -> 左移(W) -> 左移(W) ->清理(C) -> 左移(W) ->上移(N) -> 清理(C)。
56是程序遍历过的节点（state）数量。
可以看出，机器人每次寻找的都是最优解。
这就是广度优先搜索算法的一个特性：只要问题有解，就一定能找到，而且找到的一定是最优解。我们可以想象一下遍历树的场景：首先遍历跟节点，然后遍历根节点的所有子节点；如果没找到解，就再依次遍历子节点的子节点。这种方式就是横向遍历，只有当前遍历的深度（depth）中没有解的时候才会遍历下一个深度。所以当在某一个depth找到解的时候，就代表了这个depth之前的所有depth都没有解。所以这个解一定是depth最少的解，也就是最优解。在我们的程序中，一个depth就可以理解为机器人的一次行动(上移、下移等)，当一个解的depth最少，就代表机器人所需要的行动最少，也就是最优解。
我们同样可以分析出，由于这种算法是沿着树的宽度遍历树的节点，所以当深度depth很大的时候，需要遍历的节点就非常多。这个算法的空间复杂度为
O(BM)，其中B是最大分支系数，而M是树的最长路径长度。O(BM)，其中B是最大分支系数，而M是树的最长路径长度。
结束语
下一篇文章人工智能: 自动寻路算法实现(二、深度优先搜索)中，我将介绍与广度优先搜索相对应的深度优先搜索算法来解决自动寻路的问题, 以及两种算法的比较。
