机器人路径规划之RRT算法

九三智能控v

2902人浏览 · 2020-10-21 08:05:26

九三智能控v · 2020-10-21 08:05:26 发布

关注：决策智能与机器学习，深耕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

历史精华好文

交流合作

请加微信号：yan_kylin_phenix，注明姓名+单位+从业方向+地点，非诚勿扰。