關(guān)注：決策智能與機器學(xué)習(xí)，深耕AI脫水干貨

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

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

如需轉(zhuǎn)載，請聯(lián)系作者

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

1. 基本原理

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

Rapidly-Exploring Random Trees

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

重復(fù)上述過程,直到路徑上最后一個節(jié)點距離目標(biāo)位置在一定范圍內(nèi),則找到了我們最終的路徑.

2. 代碼運行結(jié)果

RRT Algorithm

3. 關(guān)鍵C++代碼剖析

一共新建了兩個類,一個用來創(chuàng)建節(jié)點,一個用來運行RRT算法:

class node { private:float x, y; // 節(jié)點坐標(biāo)vector<float> pathX, pathY;// 路徑node* parent; // 父節(jié)點float cost; public:node(float _x, float _y);float getX();float getY();void setParent(node*);node* getParent(); };class RRT { private:node* startNode, * goalNode; // 起始節(jié)點和目標(biāo)節(jié)點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中生成隨機的節(jié)點并最終形成路徑:

while (1) {// 生成一個隨機位置vector<float> randomPosition;if (goal_dis(goal_gen) > goal_sample_rate) { // 這里可以優(yōu)化成直接用節(jié)點來表示float randX = area_dis(goal_gen);float randY = area_dis(goal_gen);randomPosition.push_back(randX);randomPosition.push_back(randY);}else { // 找到了目標(biāo),將目標(biāo)位置保存randomPosition.push_back(goalNode->getX());randomPosition.push_back(goalNode->getY());}// 找到和新生成隨機節(jié)點距離最近的節(jié)點node* nearestNode = getNearestNode(randomPosition);// 利用反正切計算角度,然后利用角度和步長計算新坐標(biāo)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;} }

主函數(shù)里主要是設(shè)置障礙物、起始位置和目標(biāo)位置并調(diào)用RRT規(guī)劃函數(shù):

int main(int argc, char* argv[]) {// 障礙物,前兩個數(shù)表示圓心坐標(biāo),最后一個數(shù)表示半徑vector<vector<float>> obstacleList{{7, 5, 1},{5, 6, 2},{5, 8, 2},{5, 10, 2},{9, 5, 2},{11, 5, 2}};// 起始點和目標(biāo)點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產(chǎn)品/工程落地

• 專輯2：AI核心算法

• 專輯3：AI課程/資源/數(shù)據(jù)

交流合作

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。