關注：決策智能與機器學習，深耕AI脫水干貨

作者：矮腳獸? 來源：知乎

專欄地址：https://www.zhihu.com/column/c_1278371819016101888

如需轉載，請聯系作者

編者按：RRT是路徑規劃的重要基礎算法之一，本文介紹了基本原理和實現過程，附帶全部源碼，具有很好的參考價值。

1. 基本原理

RRT(Rapidly-Exploring Random Trees)快速隨機擴展樹,是一種單一查詢路徑規劃算法.其基本原理如下.

Rapidly-Exploring Random Trees

首先,在構型空間內隨機(一般使用均勻分布)生成一個節點??.然后在已知的路徑中找到和??距離最短的節點??,在線段??和??找一個點??,使得??和??的距離為step_size.最后檢測??是否碰到障礙物,如果碰到則舍棄這個節點.

重復上述過程,直到路徑上最后一個節點距離目標位置在一定范圍內,則找到了我們最終的路徑.

2. 代碼運行結果

RRT Algorithm

3. 關鍵C++代碼剖析

一共新建了兩個類,一個用來創建節點,一個用來運行RRT算法:

class node { private:float x, y; // 節點坐標vector<float> pathX, pathY;// 路徑node* parent; // 父節點float cost; public:node(float _x, float _y);float getX();float getY();void setParent(node*);node* getParent(); };class RRT { private:node* startNode, * goalNode; // 起始節點和目標節點vector<vector<float>> obstacleList; // 障礙物vector<node*> nodeList; // float stepSize; // 步長 public:RRT(node*, node*, const vector<vector<float>>&, float , int);node* getNearestNode(const vector<float>&);bool collisionCheck(node*);vector<node*> planning(); };

在RRT中的planning中生成隨機的節點并最終形成路徑:

while (1) {// 生成一個隨機位置vector<float> randomPosition;if (goal_dis(goal_gen) > goal_sample_rate) { // 這里可以優化成直接用節點來表示float randX = area_dis(goal_gen);float randY = area_dis(goal_gen);randomPosition.push_back(randX);randomPosition.push_back(randY);}else { // 找到了目標,將目標位置保存randomPosition.push_back(goalNode->getX());randomPosition.push_back(goalNode->getY());}// 找到和新生成隨機節點距離最近的節點node* nearestNode = getNearestNode(randomPosition);// 利用反正切計算角度,然后利用角度和步長計算新坐標float theta = atan2(randomPosition[1] - nearestNode->getY(), randomPosition[0] - nearestNode->getX());node* newNode = new node(nearestNode->getX() + stepSize * cos(theta), nearestNode->getY() + stepSize * sin(theta));newNode->setParent(nearestNode);if (!collisionCheck(newNode)) continue;nodeList.push_back(newNode);// 畫出路徑line(background,Point(static_cast<int>(newNode->getX() * imageResolution), static_cast<int>(newNode->getY() * imageResolution)),Point(static_cast<int>(nearestNode->getX() * imageResolution), static_cast<int>(nearestNode->getY() * imageResolution)),Scalar(0, 255, 0),10);count++;imshow("RRT", background);waitKey(5);if (sqrt(pow(newNode->getX() - goalNode->getX(), 2) + pow(newNode->getY() - goalNode->getY(), 2)) <= stepSize) {cout << "The path has been found!" << endl;break;} }

主函數里主要是設置障礙物、起始位置和目標位置并調用RRT規劃函數:

int main(int argc, char* argv[]) {// 障礙物,前兩個數表示圓心坐標,最后一個數表示半徑vector<vector<float>> obstacleList{{7, 5, 1},{5, 6, 2},{5, 8, 2},{5, 10, 2},{9, 5, 2},{11, 5, 2}};// 起始點和目標點node* startNode = new node(2.0, 2.0);node* goalNode = new node(14.0, 9.0);RRT rrt(startNode, goalNode, obstacleList, 0.5, 5);rrt.planning();return 0; }

代碼已上傳Github,歡迎下載。

源碼地址：https://github.com/kushuaiming/PathPalnning

歷史精華好文

• 專輯1：AI產品/工程落地

• 專輯2：AI核心算法

• 專輯3：AI課程/資源/數據

交流合作

請加微信號：yan_kylin_phenix，注明姓名+單位+從業方向+地點，非誠勿擾。

總結

以上是生活随笔為你收集整理的机器人路径规划之RRT算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。