机器人学习-关于经典路径规划（一）
1.内容简介
- 识别不同类型的路径规划算法
- 理解一组算法的内部工作原理
- 评估算法对特定应用的适用性
- 实现搜索算法
2.路径规划示例
术语
完整性——如果一个算法能够在起点和目标之间找到一条路径，那么这个算法就是完整的。
最优性——如果一个算法能够找到最佳的解决方案，那么它就是最优的。
bug算法
下面的问题演示一个有解决方案，但bug算法无法找到它的例子：
机器人会无休止地穿越在障碍物的外墙周边，但bug算法的一些变体可以弥补这个错误，大部分路径规划算法依赖于本文将介绍的其他原则。在研究新的算法时，我们将在分析算法对任务的适用性时重新考虑完整性和最优性的概念。
3.路径规划方法
路径规划方法
在本文中，将学习三种不同的路径规划方法。第一种称为离散(或组合)路径规划，是三种方法中最直接的。另外两种方法，称为基于样本的路径规划和概率路径规划，建立在离散规划的基础上，以开发更广泛适用的路径规划解决方案。
离散规划
离散规划是将机器人的工作空间显式离散化为一个连通图，并应用graph-search（图搜索）算法来计算最佳路径。这个过程非常精确(实际上，可以通过改变离散空间的精细程度来显式地调整精度)，而且非常彻底，因为它离散了整个工作空间。因此，离散规划的计算成本可能非常高——对于大型路径规划问题来说，这可能令人望而却步。
下图显示了应用于二维工作空间的离散路径规划的一种可能实现。
离散路径规划在其精确性上是优雅的，但最适合于低维问题。对于高维问题，基于样本的路径规划是一种更合适的方法。
基于样本的规划
基于样本的路径规划探测工作空间以增量地构造一个图。与离散工作空间的每个部分不同，基于样本的规划采用许多样本并使用它们来构建工作空间的离散表示。生成的图不如使用离散规划创建的图精确，但由于使用的样本数量相对较少，因此构造起来要快得多。
使用基于样本的规划生成的路径可能不是最佳路径，但在某些应用中——快速生成可行路径比等待数小时甚至数天生成最优路径要好。
下图显示了使用基于样本的规划创建的二维工作空间的图形表示。
概率路径规划
在本模块中学习的最后一种路径规划是概率路径规划。前两种方法一般地考虑路径规划问题——不了解谁或什么可能在执行动作——概率路径规划考虑了机器人运动的不确定性。
虽然这在某些环境中可能不会提供显著的好处，但在高度受限的环境或具有敏感或高风险区域的环境中尤其有用。
下图显示了应用于包含危险的环境(右上方的湖泊)的概率路径规划：
Multi-Lesson地图
在本文中，学习几种离散路径规划算法及基于样本和概率规划，然后应用到路径规划实验中，并使用C++编写搜索算法。
4.连续性表示
为了考虑机器人的几何形状并简化路径规划的任务，工作空间中的障碍物可以膨胀以创建一个称为配置空间(或C空间)的新空间。障碍物的边缘随着机器人的半径大小膨胀，机器人本体可以被视为一个点，使算法更容易搜索路径。C空间是所有机器人姿态的集合，可以分解为C_Free和C_Obs。
5.Minkowski求和
Minkowski求和
Minkowski之和是一种数学性质，可用于计算给定障碍物和机器人几何的配置空间。
Minkowski之和计算方法的直觉可以通过想象用一个形状像机器人的画笔绘制一个障碍物的外部来理解，机器人的原点是画笔的尖端。涂漆面积为C_obs。下图显示了这一点。
为了创建配置空间，对工作空间中的每一个障碍物都用这种方法计算Minkowski之和。下图显示了由三个不同大小的机器人从一个工作空间创建的三个配置空间。如您所见，如果机器人只是一个点，那么工作空间中的障碍物只会膨胀少量来创建C空间。随着机器人体积的增大，障碍物也会膨胀得越来越大。
对于凸多边形，计算卷积是很简单的，可以在线性时间内完成-然而对于非凸多边形(即存在间隙或空洞的多边形)，计算要昂贵得多。
如果有兴趣更详细地了解Minkowski之和，可参见以下资源:
-
A blog post on Minkowski sums and differences,
-
An interesting read on how collisions are detected in video games.
小测验:Minkowski求和
如下图所示：
以下哪个图像代表上面所示的机器人(紫色)和障碍物(白色)的配置空间? 答案：B
6.Minkowski求和的C++实现
现在已经学习了Minkowski求和，请尝试在C++中编写代码!
图示如下：
在这个例子中，可以看到两个三角形——一个蓝色的，一个红色的。假设机器人用一个蓝色三角形表示，障碍物用一个红色三角形表示。任务是用C++计算机器人A和障碍物B的配置空间C。
- 机器人:蓝色三角形，用A表示
- 障碍物:红色三角形，用B表示
下面是在C++中编写Minkowski和的步骤:
Non-shifted(未转换)图例
以上用C++编写了Minkowski求和代码，并生成了配置空间。注意到红色的障碍物并没有充分膨胀，蓝色的机器人仍然可以撞到障碍物。这是因为配置空间仍然需要转移到障碍物上。
首先将机器人转换为障碍物，计算配置空间后，再转化为机器人和障碍物。
转换后图例
上图是转换后最终的图像，其中蓝色的机器人和绿色的配置空间都发生了转化。可以看到黄色填充，它代表转换后的配置空间围绕在红色障碍物周围。蓝色的机器人不会再撞到红色的障碍物，因为它膨胀得很好。
完整代码如下：（详见-https://github.com/udacity/RoboND-MinkowskiSum）
#include
#include
#include
#include "../lib/matplotlibcpp.h"
#include
using namespace std;
namespace plt = matplotlibcpp;
// Print 2D vectors coordinate values
void print2DVector(vector > vec)
{
for (int i = 0; i < vec.size(); ++i) {
cout << "(";
for (int j = 0; j < vec[0].size(); ++j) {
if (vec[i][j] >= 0)
cout << vec[i][j] << " ";
else
cout << "\b" << vec[i][j] << " ";
}
cout << "\b\b)" << endl;
}
}
// Check for duplicate coordinates inside a 2D vector and delete them
vector > delete_duplicate(vector > C)
{
// Sort the C vector
sort(C.begin(), C.end());
// Initialize a non duplicated vector
vector > Cn;
for (int i = 0; i < C.size() - 1; i++) {
// Check if it's a duplicate coordinate
if (C[i] != C[i + 1]) {
Cn.push_back(C[i]);
}
}
Cn.push_back(C[C.size() - 1]);
return Cn;
}
// Compute the minkowski sum of two vectors
vector > minkowski_sum(vector > A, vector > B)
{
vector > C;
for (int i = 0; i < A.size(); i++) {
for (int j = 0; j < B.size(); j++) {
// Compute the current sum
vector Ci = { A[i][0] + B[j][0], A[i][1] + B[j][1] };
// Push it to the C vector
C.push_back(Ci);
}
}
C = delete_duplicate(C);
return C;
}
// Compute the centroid of a polygon
vector compute_centroid(vector > vec)
{
vector centroid(2);
double centroid_x=0, centroid_y=0;
for (int i = 0; i < vec.size(); i++) {
centroid_x += vec[i][0];
centroid_y += vec[i][1];
}
centroid[0] = centroid_x / vec.size();
centroid[1] = centroid_y / vec.size();
return centroid;
}
// Compute the angle of each point with respect to the centroid and append in a new column
// Resulting vector[xi,yi,anglei]
vector > compute_angle(vector > vec)
{
vector centroid = compute_centroid(vec);
double prec = 0.0001;
for (int i = 0; i < vec.size(); i++) {
double dy = vec[i][1] - centroid[1];
double dx = vec[i][0] - centroid[0];
// If the point is the centroid then delete it from the vector
if (abs(dx) < prec && abs(dy) < prec) {
vec.erase(vec.begin() + i);
}
else {
// compute the centroid-point angle
double theta = (atan2(dy, dx) * 180) / M_PI;
// append it to the vector in a 3rd column
vec[i].push_back(theta);
}
}
return vec;
}
// Sort the vector in increasing angle (clockwise) for plotting
vector > sort_vector(vector > vec)
{
vector > sorted_vec = compute_angle(vec);
// Change the 0 angle to 90 degrees
for (int i = 0; i < sorted_vec.size(); i++) {
if (sorted_vec[i][2] == 0)
sorted_vec[i][2] = 90.0;
}
// Sort with respect to the 3rd column(angles)
sort(sorted_vec.begin(),
sorted_vec.end(),
[](const vector& a, const vector& b) {
return a[2] < b[2];
});
return sorted_vec;
}
// Process the shapes and plot them
void plot(vector > vec, string color)
{
// Sort the vector coordinates in clockwise
vector > sorted_vec;
sorted_vec = sort_vector(vec);
// Add the first element to the end of the vector
sorted_vec.push_back(sorted_vec[0]);
// Loop through vector original size
for (int i = 0; i < sorted_vec.size() - 1; i++) {
// Connect coordinate point and plot the lines (x1,x2)(y1,y2)
plt::plot({ sorted_vec[i][0], sorted_vec[i + 1][0] }, { sorted_vec[i][1], sorted_vec[i + 1][1] }, color);
}
}
// Translate the configuration space toward the obstacle
vector > shift_space(vector > B, vector > C)
{
// Compute the obstacle and space centroids
vector centroid_obstacle = compute_centroid(B);
vector centroid_space = compute_centroid(C);
// Compute the translations deltas
double dx = centroid_space[0] - centroid_obstacle[0];
double dy = centroid_space[1] - centroid_obstacle[1];
// Translate the space
for (int i = 0; i < C.size(); i++) {
C[i][0] = C[i][0] - dx;
C[i][1] = C[i][1] - dy;
}
return C;
}
// Draw A, B and C shapes
void draw_shapes(vector > A, vector > B, vector > C)
{
//Graph Format
plt::title("Minkowski Sum");
plt::xlim(-5, 5);
plt::ylim(-5, 5);
plt::grid(true);
// Draw triangle A
plot(A, "b-");
// Draw triangle B
plot(B, "r-");
// Draw configuration space C
// Trasnlate the C space
vector > shifted_C = shift_space(B, C);
plot(shifted_C, "y-");
// Plot the original C shape
plot(C, "g-");
//Save the image and close the plot
plt::save("../images/Minkowski_Sum.png");
plt::clf();
}
int main()
{
// Define the coordinates of triangle A and B in 2D vectors
vector > A(3, vector(2));
// Robot A
A = {
{ 0, -1 }, { 0, 1 }, { 1, 0 },
};
vector > B(3, vector(2));
// Obstacle B
B = {
{ 0, 0 }, { 1, 1 }, { 1, -1 },
};
// Translating Robot toward the obstacle
A=shift_space(B,A);
// Compute the Minkowski Sum of triangle A and B
vector > C;
C = minkowski_sum(A, B);
// Print the resulting vector
print2DVector(C);
// Draw all the shapes
draw_shapes(A, B, C);
return 0;
} 简而言之，必须遵循以下步骤来生成任意多边形:
- 计算每个多边形的质心
- 计算每个点质心相对于x轴的角度
- 将点按角度升序排序(顺时针)
- 在每个连续的点之间画一条线
7.三维配置空间
机器人的配置空间取决于它的旋转，允许机器人旋转增加了一个自由度——因此，它也使配置空间复杂化了。配置空间的维数等于机器人拥有的自由度的个数。虽然二维配置空间能够表示机器人的x和y平移，但需要第三维来表示机器人的旋转。让我们看看一个机器人和它对应的两种不同旋转的配置空间。第一种是0°，第二种是18°。
三维配置空间可以通过将二维配置空间堆叠成层来生成——如下图所示：
如果要计算机器人的无限小旋转的配置空间，并将它们堆叠在一起-我们会得到如下图所示的图形： 
上图显示了一个三角形机器人的配置空间，它可以在二维空间中平移，也可以绕z轴旋转。虽然这个图像看起来很复杂，但有一些技巧可以用来生成3D配置空间并在它们周围移动。以下视频来自Freie Universität Berlin，是一个美妙的三维配置空间的可视化。该视频将显示不同类型的运动，并描述某些机器人运动如何映射到3D配置空间：Configuration Space Visualization 。