1.內(nèi)容簡介

• 識別不同類型的路徑規(guī)劃算法
• 理解一組算法的內(nèi)部工作原理
• 評估算法對特定應(yīng)用的適用性
• 實(shí)現(xiàn)搜索算法

2.路徑規(guī)劃示例

術(shù)語

完整性——如果一個算法能夠在起點(diǎn)和目標(biāo)之間找到一條路徑，那么這個算法就是完整的。

最優(yōu)性——如果一個算法能夠找到最佳的解決方案，那么它就是最優(yōu)的。

bug算法

下面的問題演示一個有解決方案，但bug算法無法找到它的例子：

機(jī)器人會無休止地穿越在障礙物的外墻周邊，但bug算法的一些變體可以彌補(bǔ)這個錯誤，大部分路徑規(guī)劃算法依賴于本文將介紹的其他原則。在研究新的算法時，我們將在分析算法對任務(wù)的適用性時重新考慮完整性和最優(yōu)性的概念。

3.路徑規(guī)劃方法
路徑規(guī)劃方法
在本文中，將學(xué)習(xí)三種不同的路徑規(guī)劃方法。第一種稱為離散(或組合)路徑規(guī)劃，是三種方法中最直接的。另外兩種方法，稱為基于樣本的路徑規(guī)劃和概率路徑規(guī)劃，建立在離散規(guī)劃的基礎(chǔ)上，以開發(fā)更廣泛適用的路徑規(guī)劃解決方案。

離散規(guī)劃
離散規(guī)劃是將機(jī)器人的工作空間顯式離散化為一個連通圖，并應(yīng)用graph-search（圖搜索）算法來計算最佳路徑。這個過程非常精確(實(shí)際上，可以通過改變離散空間的精細(xì)程度來顯式地調(diào)整精度)，而且非常徹底，因?yàn)樗x散了整個工作空間。因此，離散規(guī)劃的計算成本可能非常高——對于大型路徑規(guī)劃問題來說，這可能令人望而卻步。
下圖顯示了應(yīng)用于二維工作空間的離散路徑規(guī)劃的一種可能實(shí)現(xiàn)。

離散路徑規(guī)劃在其精確性上是優(yōu)雅的，但最適合于低維問題。對于高維問題，基于樣本的路徑規(guī)劃是一種更合適的方法。
基于樣本的規(guī)劃

基于樣本的路徑規(guī)劃探測工作空間以增量地構(gòu)造一個圖。與離散工作空間的每個部分不同，基于樣本的規(guī)劃采用許多樣本并使用它們來構(gòu)建工作空間的離散表示。生成的圖不如使用離散規(guī)劃創(chuàng)建的圖精確，但由于使用的樣本數(shù)量相對較少，因此構(gòu)造起來要快得多。
使用基于樣本的規(guī)劃生成的路徑可能不是最佳路徑，但在某些應(yīng)用中——快速生成可行路徑比等待數(shù)小時甚至數(shù)天生成最優(yōu)路徑要好。

下圖顯示了使用基于樣本的規(guī)劃創(chuàng)建的二維工作空間的圖形表示。

概率路徑規(guī)劃
在本模塊中學(xué)習(xí)的最后一種路徑規(guī)劃是概率路徑規(guī)劃。前兩種方法一般地考慮路徑規(guī)劃問題——不了解誰或什么可能在執(zhí)行動作——概率路徑規(guī)劃考慮了機(jī)器人運(yùn)動的不確定性。
雖然這在某些環(huán)境中可能不會提供顯著的好處，但在高度受限的環(huán)境或具有敏感或高風(fēng)險區(qū)域的環(huán)境中尤其有用。
下圖顯示了應(yīng)用于包含危險的環(huán)境(右上方的湖泊)的概率路徑規(guī)劃：

Multi-Lesson地圖
在本文中，學(xué)習(xí)幾種離散路徑規(guī)劃算法及基于樣本和概率規(guī)劃，然后應(yīng)用到路徑規(guī)劃實(shí)驗(yàn)中，并使用C++編寫搜索算法。

4.連續(xù)性表示
為了考慮機(jī)器人的幾何形狀并簡化路徑規(guī)劃的任務(wù)，工作空間中的障礙物可以膨脹以創(chuàng)建一個稱為配置空間(或C空間)的新空間。障礙物的邊緣隨著機(jī)器人的半徑大小膨脹，機(jī)器人本體可以被視為一個點(diǎn)，使算法更容易搜索路徑。C空間是所有機(jī)器人姿態(tài)的集合，可以分解為C_Free和C_Obs。

5.Minkowski求和
Minkowski求和
Minkowski之和是一種數(shù)學(xué)性質(zhì)，可用于計算給定障礙物和機(jī)器人幾何的配置空間。
Minkowski之和計算方法的直覺可以通過想象用一個形狀像機(jī)器人的畫筆繪制一個障礙物的外部來理解，機(jī)器人的原點(diǎn)是畫筆的尖端。涂漆面積為C_obs。下圖顯示了這一點(diǎn)。

為了創(chuàng)建配置空間，對工作空間中的每一個障礙物都用這種方法計算Minkowski之和。下圖顯示了由三個不同大小的機(jī)器人從一個工作空間創(chuàng)建的三個配置空間。如您所見，如果機(jī)器人只是一個點(diǎn)，那么工作空間中的障礙物只會膨脹少量來創(chuàng)建C空間。隨著機(jī)器人體積的增大，障礙物也會膨脹得越來越大。

對于凸多邊形，計算卷積是很簡單的，可以在線性時間內(nèi)完成-然而對于非凸多邊形(即存在間隙或空洞的多邊形)，計算要昂貴得多。
如果有興趣更詳細(xì)地了解Minkowski之和，可參見以下資源:

• A blog post on Minkowski sums and differences,

• An interesting read on how collisions are detected in video games.

小測驗(yàn):Minkowski求和

如下圖所示：

以下哪個圖像代表上面所示的機(jī)器人(紫色)和障礙物(白色)的配置空間??答案：B

6.Minkowski求和的C++實(shí)現(xiàn)
現(xiàn)在已經(jīng)學(xué)習(xí)了Minkowski求和，請嘗試在C++中編寫代碼!
圖示如下：?

在這個例子中，可以看到兩個三角形——一個藍(lán)色的，一個紅色的。假設(shè)機(jī)器人用一個藍(lán)色三角形表示，障礙物用一個紅色三角形表示。任務(wù)是用C++計算機(jī)器人A和障礙物B的配置空間C。

• 機(jī)器人:藍(lán)色三角形，用A表示
• 障礙物:紅色三角形，用B表示

?下面是在C++中編寫Minkowski和的步驟:

???

Non-shifted(未轉(zhuǎn)換)圖例
以上用C++編寫了Minkowski求和代碼，并生成了配置空間。注意到紅色的障礙物并沒有充分膨脹，藍(lán)色的機(jī)器人仍然可以撞到障礙物。這是因?yàn)榕渲每臻g仍然需要轉(zhuǎn)移到障礙物上。
首先將機(jī)器人轉(zhuǎn)換為障礙物，計算配置空間后，再轉(zhuǎn)化為機(jī)器人和障礙物。
轉(zhuǎn)換后圖例

上圖是轉(zhuǎn)換后最終的圖像，其中藍(lán)色的機(jī)器人和綠色的配置空間都發(fā)生了轉(zhuǎn)化。可以看到黃色填充，它代表轉(zhuǎn)換后的配置空間圍繞在紅色障礙物周圍。藍(lán)色的機(jī)器人不會再撞到紅色的障礙物，因?yàn)樗蛎浀煤芎谩?/p>

完整代碼如下：（詳見-https://github.com/udacity/RoboND-MinkowskiSum）

#include <iostream>
#include <vector>
#include <algorithm>
#include "../lib/matplotlibcpp.h"
#include <math.h>

using namespace std;
namespace plt = matplotlibcpp;

// Print 2D vectors coordinate values
void print2DVector(vector<vector<double> > 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<vector<double> > delete_duplicate(vector<vector<double> > C)
{
    // Sort the C vector
    sort(C.begin(), C.end());
    // Initialize a non duplicated vector
    vector<vector<double> > 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<vector<double> > minkowski_sum(vector<vector<double> > A, vector<vector<double> > B)
{
    vector<vector<double> > C;
    for (int i = 0; i < A.size(); i++) {
        for (int j = 0; j < B.size(); j++) {
            // Compute the current sum
            vector<double> 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<double> compute_centroid(vector<vector<double> > vec)
{
    vector<double> 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<vector<double> > compute_angle(vector<vector<double> > vec)
{
    vector<double> 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<vector<double> > sort_vector(vector<vector<double> > vec)
{
    vector<vector<double> > 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<double>& a, const vector<double>& b) {
            return a[2] < b[2];
        });
    return sorted_vec;
}

// Process the shapes and plot them
void plot(vector<vector<double> > vec, string color)
{
    // Sort the vector coordinates in clockwise
    vector<vector<double> > 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<vector<double> > shift_space(vector<vector<double> > B, vector<vector<double> > C)
{
    // Compute the obstacle and space centroids
    vector<double> centroid_obstacle = compute_centroid(B);
    vector<double> 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<vector<double> > A, vector<vector<double> > B, vector<vector<double> > 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<vector<double> > 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<vector<double> > A(3, vector<double>(2));
    // Robot A
    A = {
        { 0, -1 }, { 0, 1 }, { 1, 0 },
    };
    vector<vector<double> > B(3, vector<double>(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<vector<double> > C;
    C = minkowski_sum(A, B);

    // Print the resulting vector
    print2DVector(C);

    // Draw all the shapes
    draw_shapes(A, B, C);
    
    return 0;
}

簡而言之，必須遵循以下步驟來生成任意多邊形:

1. 計算每個多邊形的質(zhì)心
2. 計算每個點(diǎn)質(zhì)心相對于x軸的角度
3. 將點(diǎn)按角度升序排序(順時針)
4. 在每個連續(xù)的點(diǎn)之間畫一條線

7.三維配置空間

機(jī)器人的配置空間取決于它的旋轉(zhuǎn)，允許機(jī)器人旋轉(zhuǎn)增加了一個自由度——因此，它也使配置空間復(fù)雜化了。配置空間的維數(shù)等于機(jī)器人擁有的自由度的個數(shù)。雖然二維配置空間能夠表示機(jī)器人的x和y平移，但需要第三維來表示機(jī)器人的旋轉(zhuǎn)。讓我們看看一個機(jī)器人和它對應(yīng)的兩種不同旋轉(zhuǎn)的配置空間。第一種是0°，第二種是18°。

三維配置空間可以通過將二維配置空間堆疊成層來生成——如下圖所示：

如果要計算機(jī)器人的無限小旋轉(zhuǎn)的配置空間，并將它們堆疊在一起-我們會得到如下圖所示的圖形：?

上圖顯示了一個三角形機(jī)器人的配置空間，它可以在二維空間中平移，也可以繞z軸旋轉(zhuǎn)。雖然這個圖像看起來很復(fù)雜，但有一些技巧可以用來生成3D配置空間并在它們周圍移動。以下視頻來自Freie Universit?t Berlin，是一個美妙的三維配置空間的可視化。該視頻將顯示不同類型的運(yùn)動，并描述某些機(jī)器人運(yùn)動如何映射到3D配置空間：Configuration Space Visualization?。文章來源地址http://www.zghlxwxcb.cn/news/detail-459873.html

到了這里，關(guān)于機(jī)器人學(xué)習(xí)-關(guān)于經(jīng)典路徑規(guī)劃（一）的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！