1.简介
本实验任务：编写前面学过的两个路径规划算法。
首先用C++编写Breadth-first搜索算法。该算法分为不同的编码测验，最终生成机器人从起点移动到目标的最短路径。
然后，将继续进行必要的更改，以编写A*算法。在对BFS和A*算法进行编码之后，将可视化地比较生成的扩展列表。仔细检查后，判断哪种算法更有效。
在本实验的后面部分，将把A*算法应用到现实世界的问题中。实际问题只是使用占用网格映射算法生成的地图。
实验:路径规划
实验的详细步骤列表如下:
2.建模问题
本实验的目的是利用不同的路径规划算法，为机器人在5x6地图中从起始位置移动到目标位置找到最短路径。机器人只能向四个方向移动:上、左、下、右。我们将首先使用C++中的类来建模这个问题，然后用BFS和A*算法来解决它。
Given
Grid(5x6):
0 1 0 0 0 0
0 1 0 0 0 0
0 1 0 0 0 0
0 1 0 0 0 0
0 0 0 1 1 0
其中1代表障碍，0代表自由空间。
机器人起始位置:0,0
机器人目标位置:4,5
移动方向:上(-1,0)-左(0，-1)-下(1,0)-右(0,1)
移动方向矢量是四个不同2D矢量的集合，每个矢量都允许在地图中的网格单元之间移动。
移动箭头:上(^)-左(<)-下(v) -右(>)
移动箭头向量存储机器人的动作，这个向量将在本实验室稍后使用，以可视化机器人在最短路径上的每个网格单元的方向。
移动成本:1
移动成本值表示从一个单元格移动到另一个单元格的成本。在这里，对于所有可能的移动，代价都是相等的。
测试
在这个测试中，为了建模问题，有三个主要任务要完成:
注意
在整个实验过程中，将使用C++中的1D和2D向量。vector允许使用预先构建的函数轻松地管理和操作数据。例如:pop_back函数可用于删除vector中的最后一个元素。
关于向量，可参阅以下两个资源:
- 2D Vectors: 学习如何在C++中定义和使用2D向量。
- Documentation: 学习向量迭代器和修饰器（modifiers）。
参考代码如下：
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
/* TODO: Define a Map class
Inside the map class, define the mapWidth, mapHeight and grid as a 2D vector
*/
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
/* TODO: Define a Planner class
Inside the Planner class, define the start, goal, cost, movements, and movements_arrows
Note: The goal should be defined it terms of the mapWidth and mapHeight
*/
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
/* TODO: Define a print2DVector function which will print 2D vectors of any data type
Example
Input:
vector<vector<int> > a{
{ 1, 0 },{ 0, 1 }};
print2DVector(a);
vector<vector<string> > b{
{ "a", "b" },{ "c", "d" }};
print2DVector(b);
Output:
1 0
0 1
a b
c d
Hint: You need to use templates
*/
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
/*############ Don't modify the main function############*/
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Print classes variables
cout << "Map:" << endl;
print2DVector(map.grid);
cout << "Start: " << planner.start[0] << " , " << planner.start[1] << endl;
cout << "Goal: " << planner.goal[0] << " , " << planner.goal[1] << endl;
cout << "Cost: " << planner.cost << endl;
cout << "Robot Movements: " << planner.movements_arrows[0] << " , " << planner.movements_arrows[1] << " , " << planner.movements_arrows[2] << " , " << planner.movements_arrows[3] << endl;
cout << "Delta:" << endl;
print2DVector(planner.movements);
return 0;
}
3.BFS:扩展列表
现在使用C++中的Map和Planner类对问题进行建模，接下来将从BFS算法的第一部分开始。在这个测试中，编写搜索函数，以最低的代价扩展单元格，直到达到目标。
为此，需要用三元组值[g, x, y]表示每个单元格，其中g表示向该单元格扩展的总代价，x是行值，y是列值。
一旦扩展到达目标，打印目标的最终三重值。
在编写搜索函数时，记住以下事项
- 当扩展到一个新的单元格时，检查是否达到了目标;一旦到达，打印它的三重值。
- 主动检查是否遇到了障碍。如果遇到了障碍，停止扩展并打印一条消息，表明未能达到目标。
- 展开g值最低的单元格，并将展开存储在一个开放向量中。如果两个单元格的g值相等，则可以选择其中一个单元格进一步展开。
提示
下面是如何使用BFS算法扩展单元，直到达到目标:
Expansion #: 0
Open List: [0 0 0 ]
Cell Picked: [0 0 0]
Expansion #: 1
Open List: [1 1 0 ]
Cell Picked: [1 1 0]
Expansion #: 2
Open List: [2 2 0 ]
Cell Picked: [2 2 0]
Expansion #: 3
Open List: [3 3 0 ]
Cell Picked: [3 3 0]
Expansion #: 4
Open List: [4 4 0 ]
Cell Picked: [4 4 0]
Expansion #: 5
Open List: [5 4 1 ]
Cell Picked: [5 4 1]
Expansion #: 6
Open List: [6 4 2 ]
Cell Picked: [6 4 2]
Expansion #: 7
Open List: [7 3 2 ]
Cell Picked: [7 3 2]
Expansion #: 8
Open List: [8 3 3 ], [8 2 2 ]
Cell Picked: [8 2 2]
Expansion #: 9
Open List: [9 2 3 ], [9 1 2 ], [8 3 3 ]
Cell Picked: [8 3 3]
Expansion #: 10
Open List: [9 3 4 ], [9 2 3 ], [9 1 2 ]
Cell Picked: [9 1 2]
Expansion #: 11
Open List: [10 1 3 ], [10 0 2 ], [9 3 4 ], [9 2 3 ]
Cell Picked: [9 2 3]
Expansion #: 12
Open List: [10 2 4 ], [10 1 3 ], [10 0 2 ], [9 3 4 ]
Cell Picked: [9 3 4]
Expansion #: 13
Open List: [10 3 5 ], [10 2 4 ], [10 1 3 ], [10 0 2 ]
Cell Picked: [10 0 2]
Expansion #: 14
Open List: [11 0 3 ], [10 3 5 ], [10 2 4 ], [10 1 3 ]
Cell Picked: [10 1 3]
Expansion #: 15
Open List: [11 1 4 ], [11 0 3 ], [10 3 5 ], [10 2 4 ]
Cell Picked: [10 2 4]
Expansion #: 16
Open List: [11 2 5 ], [11 1 4 ], [11 0 3 ], [10 3 5 ]
Cell Picked: [10 3 5]
Expansion #: 17
Open List: [11 4 5 ], [11 2 5 ], [11 1 4 ], [11 0 3 ]
Cell Picked: [11 0 3]
Expansion #: 18
Open List: [12 0 4 ], [11 4 5 ], [11 2 5 ], [11 1 4 ]
Cell Picked: [11 1 4]
Expansion #: 19
Open List: [12 1 5 ], [12 0 4 ], [11 4 5 ], [11 2 5 ]
Cell Picked: [11 2 5]
Expansion #: 20
Open List: [12 1 5 ], [12 0 4 ], [11 4 5 ]
Cell Picked: [11 4 5]
参考代码如下：
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
/*#### TODO: Code the search function which will generate the expansion list ####*/
// You are only required to print the final triplet values
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags
bool found = false;
bool resign = false;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
}
}
}
}
}
}
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
4.BFS:扩张向量
现在已经展开了单元格，直到达到目标为止，系统要求打印每个单元格展开的顺序。为此，需要修改搜索函数并创建与地图大小相同的2D展开向量。展开向量中的每个单元格将存储展开时的顺序。有些单元格从未展开，应该显示值为-1。
提示
看一下运行代码后生成的扩展列表:
可以看到，我们从第一个单元格开始，并在经过20次迭代后扩展的目标单元格结束。所有的障碍和一些单元格从未展开，因此显示的值为-1。
现在，继续修改搜索函数以生成并打印展开的2D矢量。
参考代码如下：
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags and counters
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
5.BFS:最短路径
最后一步是打印机器人从起点到目标所需要的最短路径。需要记录机器人应该采取的每个动作(例如:左转<)，并将所有动作存储在一个策略2D向量中。
提示
下面是运行代码后生成的输出策略向量:
可以看到不同的动作(v - > - < - ^)是机器人为了达到*标记的目标所必须采取的措施。其中一些单元格将永远不会被机器人访问，并被标记为“-”。现在，继续修改搜索函数以生成策略2D Vector。
参考代码如下：
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the triplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
// Store the expansions
vector<vector<int> > open;
open.push_back({ g, x, y });
// Flags and counters
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove triplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[1];
y = next[2];
g = next[0];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
open.push_back({ g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
//print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the path with arrows
print2DVector(policy);
}
int main()
{
// Instantiate map and planner objects
Map map;
Planner planner;
// Search for the expansions
search(map, planner);
return 0;
}
6.A*:最短路径
现在将实现A*算法，并通过修改前面的代码找到最短路径。如你所知，A*是基于启发式函数的。因此，我们将实现一个基于曼哈顿的启发式向量，并计算每个单元相对于目标位置的曼哈顿距离，其中:
扩展
现在不是用最小路径代价g来扩展单元格，而是用最小的f值来扩展单元格f值是路径代价g和单元格的启发式值h的总和。
f=g+h
现在每个单元格都用四元组值[f,g,x,y]表示，而不是三元组值[g,x,y]。
注：参照以下步骤，并进行必要的更改，以使用A*算法找到最短路径:
提示
下面是如何使用A*算法扩展单元格，直到达到目标:
| Map | 0 |
1 |
2 |
3 |
4 |
5 |
|---|---|---|---|---|---|---|
0 |
0 | 1 | 0 | 0 | 0 | 0 |
1 |
0 | 1 | 0 | 0 | 0 | 0 |
2 |
0 | 1 | 0 | 0 | 0 | 0 |
3 |
0 | 1 | 0 | 0 | 0 | 0 |
4 |
0 | 0 | 0 | 1 | 1 | 0 |
Expansion #: 0
Open List: [9 0 0 0 ]
Cell Picked: [9 0 0 0]
Expansion #: 1
Open List: [9 1 1 0 ]
Cell Picked: [9 1 1 0]
Expansion #: 2
Open List: [9 2 2 0 ]
Cell Picked: [9 2 2 0]
Expansion #: 3
Open List: [9 3 3 0 ]
Cell Picked: [9 3 3 0]
Expansion #: 4
Open List: [9 4 4 0 ]
Cell Picked: [9 4 4 0]
Expansion #: 5
Open List: [9 5 4 1 ]
Cell Picked: [9 5 4 1]
Expansion #: 6
Open List: [9 6 4 2 ]
Cell Picked: [9 6 4 2]
Expansion #: 7
Open List: [11 7 3 2 ]
Cell Picked: [11 7 3 2]
Expansion #: 8
Open List: [13 8 2 2 ], [11 8 3 3 ]
Cell Picked: [11 8 3 3]
Expansion #: 9
Open List: [13 9 2 3 ], [13 8 2 2 ], [11 9 3 4 ]
Cell Picked: [11 9 3 4]
Expansion #: 10
Open List: [13 10 2 4 ], [13 9 2 3 ], [13 8 2 2 ], [11 10 3 5 ]
Cell Picked: [11 10 3 5]
Expansion #: 11
Open List: [13 11 2 5 ], [13 10 2 4 ], [13 9 2 3 ], [13 8 2 2 ], [11 11 4 5 ]
Cell Picked: [11 11 4 5]
参考代码如下：
#include <iostream>
#include <string.h>
#include <vector>
#include <algorithm>
using namespace std;
// Map class
class Map {
public:
const static int mapWidth = 6;
const static int mapHeight = 5;
vector<vector<int> > grid = {
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 1, 1, 0 }
};
vector<vector<int> > heuristic = {
{ 9, 8, 7, 6, 5, 4 },
{ 8, 7, 6, 5, 4, 3 },
{ 7, 6, 5, 4, 3, 2 },
{ 6, 5, 4, 3, 2, 1 },
{ 5, 4, 3, 2, 1, 0 }
};
};
// Planner class
class Planner : Map {
public:
int start[2] = { 0, 0 };
int goal[2] = { mapHeight - 1, mapWidth - 1 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
};
// Template function to print 2D vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
// Search function will generate the expansions
void search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
cout << endl;
print2DVector(policy);
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
search(map, planner);
return 0;
}
7.比较
现在已经编写了BFS和A*算法，让我们仔细查看它们的扩展列表并进行比较。
结论
可以清楚地看到，A*的效率更高，因为它没有像BFS那样在自由空间中膨胀。在A*中，只需完成11个扩展，而在BFS中要完成20个扩展。
8.A*:真实世界地图（Real-World Map）
以下这张地图是利用声纳和里程测量数据，使用占用网格映射算法生成的。我们现在的任务是应用A*算法，找到机器人从起点o到目标*位置的最短路径。
给定条件
地图Map(300x150):以日志的形式存储在Map .txt文件中的地图数据，应该如何理解这些数字:
-
如果单元格的log odds值等于0，则认为它是未知的。
-
如果单元格的log odds值大于0，则认为该单元格已被占用。
-
如果单元格的log odds值小于0，则认为单元格是空置的。
网格Grid(300x150): 将日志值转换为0和1，其中0表示空闲空间，1表示已占用或未知空间。
起始位置 Robot Start position: 230,145
目标位置 Robot Goal Position: 60,50
移动方向:上(-1,0)-左(0,-1)-下(1,0)-右(0,1)
运动箭头:上(^)-左(<)-下(v)-右(>)
移动成本:1
启发式向量:曼哈顿Manhattan
向Map类添加三个新函数:GetMap函数，用于读取map.txt日志（log odds）值，并将它们分配给map变量；MapToGrid函数，以便将日志（log odds）值转换为0和1。这些0和1值将被分配给网格变量；GeneratedHeuristic函数，为了生成一个基于曼哈顿的启发式向量，通过计算每个单元格相对于目标位置的曼哈顿距离。每个单元的曼哈顿距离可计算如下:
注：按照下面的说明，使用A*算法生成最短路径:
参考代码如下：
#include <iostream>
#include <math.h>
#include <vector>
#include <algorithm>
#include <fstream>
using namespace std;
// Map class
class Map {
public:
const static int mapHeight = 300;
const static int mapWidth = 150;
vector<vector<double> > map = GetMap();
vector<vector<int> > grid = MaptoGrid();
vector<vector<int> > heuristic = GenerateHeuristic();
private:
// Read the file and get the map
vector<vector<double> > GetMap()
{
vector<vector<double> > mymap(mapHeight, vector<double>(mapWidth));
ifstream myReadFile;
myReadFile.open("map.txt");
while (!myReadFile.eof()) {
for (int i = 0; i < mapHeight; i++) {
for (int j = 0; j < mapWidth; j++) {
myReadFile >> mymap[i][j];
}
}
}
return mymap;
}
//Convert the map to 1's and 0's
vector<vector<int> > MaptoGrid()
{
vector<vector<int> > grid(mapHeight, vector<int>(mapWidth));
for (int x = 0; x < mapHeight; x++) {
for (int y = 0; y < mapWidth; y++) {
if (map[x][y] == 0) //unkown state
grid[x][y] = 1;
else if (map[x][y] > 0) //Occupied state
grid[x][y] = 1;
else //Free state
grid[x][y] = 0;
}
}
return grid;
}
// Generate a Manhattan Heuristic Vector
vector<vector<int> > GenerateHeuristic()
{
vector<vector<int> > heuristic(mapHeight, vector<int>(mapWidth));
int goal[2] = { 60, 50 };
for (int i = 0; i < heuristic.size(); i++) {
for (int j = 0; j < heuristic[0].size(); j++) {
int xd = goal[0] - i;
int yd = goal[1] - j;
// Manhattan Distance
int d = abs(xd) + abs(yd);
// Euclidian Distance
// double d = sqrt(xd * xd + yd * yd);
// Chebyshev distance
// int d = max(abs(xd), abs(yd));
heuristic[i][j] = d;
}
}
return heuristic;
}
};
// Planner class
class Planner : Map {
public:
int start[2] = { 230, 145 };
int goal[2] = { 60, 50 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
vector<vector<int> > path;
};
// Printing vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
Planner search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
//print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
// Store the Path in a vector
planner.path.push_back({ x2, y2 });
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
//cout << endl;
print2DVector(policy);
return planner;
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
// Generate the shortest Path using the Astar algorithm
planner = search(map, planner);
return 0;
}
附map.txt：参见机器人学习-路径规划实验（二）
9.A*:可视化
路径规划
到目前为止，已经使用A*算法生成了最短路径，但是很难看到它。现在，编写可视化函数，以绘制最短路径。
从GitHub克隆实验
$ cd /home/workspace/
$ git clone https://github.com/udacity/RoboND-A-Visualization接下来，编辑main.cpp
使用matplotlib python库修改可视化函数并绘制起始位置、目标位置和路径。请注意，使用字母“o”(而不是数字字符“0”)和星号“*”来标记可视化中的开始和结束状态!
void visualization(Map map, Planner planner)
{
//Graph Format
plt::title("Path");
plt::xlim(0, map.mapHeight);
plt::ylim(0, map.mapWidth);
// Draw every grid of the map:
for (double x = 0; x < map.mapHeight; x++) {
cout << "Remaining Rows= " << map.mapHeight - x << endl;
for (double y = 0; y < map.mapWidth; y++) {
if (map.map[x][y] == 0) { //Green unkown state
plt::plot({ x }, { y }, "g.");
}
else if (map.map[x][y] > 0) { //Black occupied state
plt::plot({ x }, { y }, "k.");
}
else { //Red free state
plt::plot({ x }, { y }, "r.");
}
}
}
// TODO: Plot start and end states in blue colors using o and * respectively
// TODO: Plot the robot path in blue color using a .
//Save the image and close the plot
plt::save("./Images/Path.png");
plt::clf();
}下面是一些有用的命令，可以使用matplotlib库生成图形:
-
设置标题:plt:: Title(“你的标题”);
-
设置限制:plt::xlim(x轴下限，x轴上限);
-
Plot数据:plt:: Plot ({x-value}， {y-value}，“颜色和形状”);
-
保存Save Plot: plt:: Save(“文件名和目录”);
-
关闭Close Plot: plt::clf();
有关matplotlib C++库的更多信息，请查看此链接（GitHub - lava/matplotlib-cpp: Extremely simple yet powerful header-only C++ plotting library built on the popular matplotlib）。有关绘图颜色和形状的信息，请参阅MATLAB（2-D line plot - MATLAB plot）文档的LineSpec和LineColor部分。
然后，编译程序
$ cd RoboND-A-Visualization/
$ rm -rf Images/* #Delete the folder content and not the folder itself!
$ g++ main.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7最后，运行程序
$ ./app如果收到关于matplotlib库的警告，请忽略。现在，等待程序生成路径并将其存储在/home/workspace/RoboND-A-Visualization/Images目录中!
生成路径
地图图例
-
绿色:未知/未发现区域
-
红色:空置区域
-
黑色:已占用区域
-
蓝色:最短路径
参考代码如下：
#include <iostream>
#include <math.h>
#include <vector>
#include <iterator>
#include <fstream>
#include "src/matplotlibcpp.h" //Graph Library
using namespace std;
namespace plt = matplotlibcpp;
// Map class
class Map {
public:
const static int mapHeight = 300;
const static int mapWidth = 150;
vector<vector<double> > map = GetMap();
vector<vector<int> > grid = MaptoGrid();
vector<vector<int> > heuristic = GenerateHeuristic();
private:
// Read the file and get the map
vector<vector<double> > GetMap()
{
vector<vector<double> > mymap(mapHeight, vector<double>(mapWidth));
ifstream myReadFile;
myReadFile.open("map.txt");
while (!myReadFile.eof()) {
for (int i = 0; i < mapHeight; i++) {
for (int j = 0; j < mapWidth; j++) {
myReadFile >> mymap[i][j];
}
}
}
return mymap;
}
//Convert the map to 1's and 0's
vector<vector<int> > MaptoGrid()
{
vector<vector<int> > grid(mapHeight, vector<int>(mapWidth));
for (int x = 0; x < mapHeight; x++) {
for (int y = 0; y < mapWidth; y++) {
if (map[x][y] == 0) //unkown state
grid[x][y] = 1;
else if (map[x][y] > 0) //Occupied state
grid[x][y] = 1;
else //Free state
grid[x][y] = 0;
}
}
return grid;
}
// Generate a Manhattan Heuristic Vector
vector<vector<int> > GenerateHeuristic()
{
vector<vector<int> > heuristic(mapHeight, vector<int>(mapWidth));
int goal[2] = { 60, 50 };
for (int i = 0; i < heuristic.size(); i++) {
for (int j = 0; j < heuristic[0].size(); j++) {
int xd = goal[0] - i;
int yd = goal[1] - j;
// Manhattan Distance
int d = abs(xd) + abs(yd);
// Euclidian Distance
// double d = sqrt(xd * xd + yd * yd);
// Chebyshev distance
// int d = max(abs(xd), abs(yd));
heuristic[i][j] = d;
}
}
return heuristic;
}
};
// Planner class
class Planner : Map {
public:
int start[2] = { 230, 145 };
int goal[2] = { 60, 50 };
int cost = 1;
string movements_arrows[4] = { "^", "<", "v", ">" };
vector<vector<int> > movements{
{ -1, 0 },
{ 0, -1 },
{ 1, 0 },
{ 0, 1 }
};
vector<vector<int> > path;
};
// Printing vectors of any type
template <typename T>
void print2DVector(T Vec)
{
for (int i = 0; i < Vec.size(); ++i) {
for (int j = 0; j < Vec[0].size(); ++j) {
cout << Vec[i][j] << ' ';
}
cout << endl;
}
}
Planner search(Map map, Planner planner)
{
// Create a closed 2 array filled with 0s and first element 1
vector<vector<int> > closed(map.mapHeight, vector<int>(map.mapWidth));
closed[planner.start[0]][planner.start[1]] = 1;
// Create expand array filled with -1
vector<vector<int> > expand(map.mapHeight, vector<int>(map.mapWidth, -1));
// Create action array filled with -1
vector<vector<int> > action(map.mapHeight, vector<int>(map.mapWidth, -1));
// Defined the quadruplet values
int x = planner.start[0];
int y = planner.start[1];
int g = 0;
int f = g + map.heuristic[x][y];
// Store the expansions
vector<vector<int> > open;
open.push_back({ f, g, x, y });
// Flags and Counts
bool found = false;
bool resign = false;
int count = 0;
int x2;
int y2;
// While I am still searching for the goal and the problem is solvable
while (!found && !resign) {
// Resign if no values in the open list and you can't expand anymore
if (open.size() == 0) {
resign = true;
cout << "Failed to reach a goal" << endl;
}
// Keep expanding
else {
// Remove quadruplets from the open list
sort(open.begin(), open.end());
reverse(open.begin(), open.end());
vector<int> next;
// Stored the poped value into next
next = open.back();
open.pop_back();
x = next[2];
y = next[3];
g = next[1];
// Fill the expand vectors with count
expand[x][y] = count;
count += 1;
// Check if we reached the goal:
if (x == planner.goal[0] && y == planner.goal[1]) {
found = true;
//cout << "[" << g << ", " << x << ", " << y << "]" << endl;
}
//else expand new elements
else {
for (int i = 0; i < planner.movements.size(); i++) {
x2 = x + planner.movements[i][0];
y2 = y + planner.movements[i][1];
if (x2 >= 0 && x2 < map.grid.size() && y2 >= 0 && y2 < map.grid[0].size()) {
if (closed[x2][y2] == 0 and map.grid[x2][y2] == 0) {
int g2 = g + planner.cost;
f = g2 + map.heuristic[x2][y2];
open.push_back({ f, g2, x2, y2 });
closed[x2][y2] = 1;
action[x2][y2] = i;
}
}
}
}
}
}
// Print the expansion List
print2DVector(expand);
// Find the path with robot orientation
vector<vector<string> > policy(map.mapHeight, vector<string>(map.mapWidth, "-"));
// Going backward
x = planner.goal[0];
y = planner.goal[1];
policy[x][y] = '*';
while (x != planner.start[0] or y != planner.start[1]) {
x2 = x - planner.movements[action[x][y]][0];
y2 = y - planner.movements[action[x][y]][1];
// Store the Path in a vector
planner.path.push_back({ x2, y2 });
policy[x2][y2] = planner.movements_arrows[action[x][y]];
x = x2;
y = y2;
}
// Print the robot path
cout << endl;
print2DVector(policy);
return planner;
}
void visualization(Map map, Planner planner)
{
//Graph Format
plt::title("Path");
plt::xlim(0, map.mapHeight);
plt::ylim(0, map.mapWidth);
// Draw every grid of the map:
for (double x = 0; x < map.mapHeight; x++) {
cout << "Remaining Rows= " << map.mapHeight - x << endl;
for (double y = 0; y < map.mapWidth; y++) {
if (map.map[x][y] == 0) { //Green unkown state
plt::plot({ x }, { y }, "g.");
}
else if (map.map[x][y] > 0) { //Black occupied state
plt::plot({ x }, { y }, "k.");
}
else { //Red free state
plt::plot({ x }, { y }, "r.");
}
}
}
// Plot start and end states
plt::plot({ (double)planner.start[0] }, { (double)planner.start[1] }, "bo");
plt::plot({ (double)planner.goal[0] }, { (double)planner.goal[1] }, "b*");
// Plot the robot path
for (int i = 0; i < planner.path.size(); i++) {
plt::plot({ (double)planner.path[i][0] }, { (double)planner.path[i][1] }, "b.");
}
//Save the image and close the plot
plt::save("./Images/Path.png");
plt::clf();
}
int main()
{
// Instantiate a planner and map objects
Map map;
Planner planner;
// Generate the shortest Path using the Astar algorithm
planner = search(map, planner);
// Plot the Map and the path generated
visualization(map, planner);
return 0;
}