机器人路径规划之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，注明姓名+单位+从业方向+地点，非诚勿扰。