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