机器人路径规划之RRT算法

关注：决策智能与机器学习，深耕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，注明姓名+单位+从业方向+地点，非诚勿扰。