关注:决策智能与机器学习,深耕AI脱水干货
编者按:RRT是路径规划的重要基础算法之一,本文介绍了基本原理和实现过程,附带全部源码,具有很好的参考价值。
1. 基本原理
RRT(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,注明姓名+单位+从业方向+地点,非诚勿扰。