写在前面

这篇文章主要介绍C++基于vector实现

效果展示:

黑色:障碍物(可由鼠标绘制);
绿色线段:探索路径;
绿色方块:终点
红色方块:起点
棕色线段:最终路径
点划线:路径探索下边界
绿色圆点:代表算法开始运行

实现

因为C++语言的特性以及方便实现,算法实现可能与伪代码有些许区别,但是总体逻辑上是一样的。

首先是rrt算法的主体程序了:

void RRT::rrt_algorithm()
{
    srand((unsigned)time(NULL));
    setlinecolor(GREEN);
    setlinestyle(PS_SOLID, rrt_user_para.rrt_line_width);
//
    int min_dist_random_idx = 0;
    Point* search_p = new Point(start->x,start->y);
    Point* search_next_p = nullptr;
    rrt_path.push_back(search_p);

    //testAtan2Radian();


    while (1) {
        //showParaBelow(end);
        delayMs(1);
        if (getRandomFloat(0, 1) > rrt_user_para.beta) {
            search_p = findMinDistPoint(end);
            search_next_p = getRRTNode(search_p);
        }
        else {
            search_next_p = getRandomRRTNode(min_dist_random_idx);
            search_p = rrt_path[min_dist_random_idx];
        }


        if (!isObstacle(search_next_p) && isLegalPoint(search_next_p)) {
            search_next_p->pre_point = search_p;

            rrt_path.push_back(search_next_p);

            line(search_p->x, search_p->y, search_next_p->x, search_next_p->y);
            solidcircle(search_next_p->x, search_next_p->y, rrt_user_para.rrt_line_width);

        }
        else{
            delete search_next_p;
        }

        search_p = findMinDistPoint(end);
        if (calcuDist(search_p, end) < rrt_user_para.rrt_edge_len) { break; }
    }

    getPath(search_p);

}

开始的3行是设置随机数的种子和绘图相关初始化,暂时不用讲解。

跟之前A*算法实现的方式相同,我都是在堆里申请内存,然后获取返回的地址并将地址存入vector,以便索引。

所以在刚开始的时候是先将start的父节点置nullptr,然后将start添加进rrt_path,从而保证搜索的时候包含起点。

while循环里面就是算法的主体,首先就是生成随机数并且与特定阈值进行比较,判断是向随机点延伸还是向终点延伸。

如果是向随机点延伸的话算法实现如下:

Point* RRT::getRandomRRTNode(int &idx)
{

    Point* p_new = new Point();
    Point p(RANDOM_RANGE(0,rrt_user_para.rrt_graph_width-1),
        RANDOM_RANGE(0,rrt_user_para.rrt_graph_height-1));

    int temp_dist = 0;
    int min_dist = INT_MAX;
    for (int i = 0; i < rrt_path.size(); i++) {
        temp_dist = calcuDist(rrt_path[i], &p);
        if (temp_dist < min_dist) {
            min_dist = temp_dist;
            idx = i;
        }
    }

    Point* p_node = rrt_path[idx];

    if (p.x == p_node->x) {
        p_new->x = p.x;
        int symbol = (p.y - p_node->y) > 0 ? 1 : -1;
        int p_y = p_node->y;
        p_new->y = p_y + symbol * rrt_user_para.rrt_edge_len;
    }
    else {
        float theta = atan2((p.x - p_node->x), (p.y - p_node->y));
        getAtan2Node(p_new, p_node, theta);
    }

    return p_new;
}

该部分的算法实现逻辑是首先生成一个随机点,然后在rrt_path里面找离随机点最近的点找到之后将其做为父节点,然后算法可以通过atan2()函数获取父节点到随机点的朝向角(弧度值),然后通过父节点向该方向延长特定的距离获取新节点的坐标,新节点的坐标是通过sin和cos函数获得。

这里要注意一个点,就是要判断随机点和父节点的x值关系,如果两者的x数值相等,此时的tan数值无穷大,因此需要特定处理,也就是直接将父节点的y方向进行延伸即可。

如果是以终点为目标点进行延伸的话,算法如下图所示:

Point* RRT::findMinDistPoint(const Point* end)
{
    int min_dist = INT_MAX;
    int temp_dist = 0;
    int min_dist_idx = 0;

    for (int i = 0; i < rrt_path.size(); i++) {
        temp_dist = calcuDist(rrt_path[i], end);
        if (temp_dist < min_dist) {
            min_dist = temp_dist;
            min_dist_idx = i;
        }
    }

    return rrt_path[min_dist_idx];
}

Point* RRT::getRRTNode(const Point* node)
{
    Point* p = new Point();

    if (node->x != end->x) {
        float theta = atan2((end->x - node->x), (end->y - node->y));
        getAtan2Node(p, node, theta);
    }
    else {
        p->x = node->x;
        p->y = node->y + rrt_user_para.rrt_edge_len;
    }

    return p;
}

首先是获取rrt_path里面离终点最近的节点,然后按照前述方法进行延伸获取新节点即可。

生成节点之后要判断是否触碰到了障碍物,该部分的算法如下所示:

bool RRT::isObstacle(const Point* p)
{
    for (int i = -2; i < rrt_user_para.avoid_obstacles_cnt; i++) {
        for (int j = -2; j < rrt_user_para.avoid_obstacles_cnt; j++) {
            Point tmp_p(p->x + i, p->y + j);
            if (isLegalPoint(&tmp_p)) {
                if (getpixel(tmp_p.x, tmp_p.y) == BLACK) {
                    return true;
                }
            }
        }
    }
    return false;
}

算法思想是查看以待核查节点为中心的边长为5个单位的正方体里是否黑像素(障碍物),如果有的话就取消该生成的节点,并且delete掉该节点的内存。

还要判断一下生成的节点是否超出了地图范围:

bool RRT::isLegalPoint(const Point* p)
{
    if (p->x >= 0 && p->x < rrt_user_para.rrt_graph_width && 
        p->y >= 0 && p->y < rrt_user_para.rrt_graph_height) {
        return true;
    }
    else {
        return false;
    }

    return false;
}

最后如果判断离终点最近的节点到终点的距离小于特定数值之后就可以判定找到路径,然后break掉循环,返回路径即可。