C++实现双向RRT算法

C++实现双向RRT算法

背景介绍

RRT（Rapidly-exploring Random Trees）是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过所及构建空间搜索树实现对非凸高维空间快速搜索算法。该算法可以很容易的处理包含障碍和差分运动约束的场景，因此被广泛应用在各种机器人、无人车的运动规划场景中。

双向RRT算法

为了加快随机搜索树规划路径的速度，因此提出了一种新的搜索思路，即从起点和终点同时开始构建随机搜索树，并每次进行判断产生的节点是否满足连接的条件。并在连接条件上添加了转角约束和动态步长策略。

转角约束是用来限制路线的转折角度，避免超过无人车的最大转弯角度。动态步长策略是在产生新节点时用于判断距离障碍物的趋势，动态的调整步长，能够使规划出的路径更加平滑，同时也可加快收敛速度。

C++代码实现如下：

具体代码

头文件

//
// Created by cntia on 2022-10-01.
//

#ifndef RRT_C_RRT_H
#define RRT_C_RRT_H
#include <cmath>
#include <iostream>

using namespace std;
const int RAND_X = 21;
const int RAND_Y = 89;
const double EPS = 1e-6;

struct ListPoint{
    double x;
    double y;
    ListPoint *parent;
    ListPoint *next;
    ListPoint(): x(0), y(0), parent(nullptr),next(nullptr){}
    ListPoint(double x, double y): x(x), y(y), parent(nullptr), next(nullptr){}
};
struct ListObstacle {
    double x;
    double y;
    double r;
    ListObstacle *next;
    ListObstacle():x(),y(), r(), next(nullptr){}
    ListObstacle(double x, double y, double r):x(x),y(y), r(r), next(nullptr){}
};
struct Vector {
    double x;
    double y;
};
class RT {
private:
    ListPoint *one;
    ListPoint *two;
    ListObstacle *obstacle;

    ListPoint *start;
    ListPoint *goal;
    ListPoint *safe;
    ListPoint *recover;

    double angle;
    double step;
    double dist;
    /**
 * 生成随机点
 * @return
 */
    static ListPoint* randomPoint();
    /**
     * 计算向量夹角
     * @param vector1
     * @param vector2
     * @return
     */
    double getAngle(Vector vector1, Vector vector2);
    /**
     * 向搜索树插入实际新节点
     * @param t
     * @param point
     */
    void pushBack(int t,ListPoint *point);
    /**
     * 查询最近节点
     * @param list
     * @param point
     * @return
     */
    ListPoint *getNearestIndexPoint(ListPoint *list, ListPoint *point);
    /**
     * 查询最近障碍物
     * @param point
     * @return
     */
    ListObstacle *getNearestIndexObstacle(ListPoint *point);
    /**
     * 计算动态步长
     * @param n_point
     * @param a_point
     * @return
     */
    double dynamicStep(ListPoint *n_point, ListPoint * a_point);
    /**
     * 碰撞检测
     * @param t
     * @param newPoint
     * @return
     */
    bool collisionCheck(int t,const ListPoint *newPoint);
    /**
     * 转角约束检测
     * @param newPoint
     * @param parentPoint
     * @param ancestorPoint
     * @return
     */
    bool angleCheck(const ListPoint *newPoint, const ListPoint *parentPoint, const ListPoint *ancestorPoint);
    /**
     * 节点检测
     * @param t
     * @param newPoint
     * @return
     */
    bool conditionCheck(int t,const ListPoint *newPoint);
    /**
     * 平滑连接判断
     * @param onePoint
     * @param twoPoint
     * @return
     */
    bool perfectConnect(const ListPoint *onePoint, const ListPoint *twoPoint);
    /**
     * 实际坐标计算
     * @param t
     * @param rnd
     * @return
     */
    ListPoint *coordinate(int t, ListPoint *rnd);
public:
    RT(ListPoint *start,ListPoint *goal,ListPoint *safe,ListPoint *recover, double angle,
       double step, double dist, ListObstacle *obstacle) : start(start), goal(goal), safe(safe), recover(recover),
       angle(angle), step(step),dist(dist),obstacle(obstacle) {
        ListPoint *headOne = start;
        headOne->next = safe;
        safe->parent = headOne;
        this->one = headOne;

        ListPoint *headTwo = goal;
        headTwo->next = recover;
        recover->parent = headTwo;
        this->two = headTwo;
    };
    /**
     * 路径规划
     */
    void planning();
};
#endif //RRT_C_RRT_H

源文件

//
// Created by cntia on 2022-10-01.
//

#include "../headers/RRT.h"

ListPoint *RT::randomPoint() {
    double x = (rand() % (RAND_Y - RAND_X + 1)) + RAND_X;
    double y = (rand() % (RAND_Y - RAND_X + 1)) + RAND_X;
    auto *point = new ListPoint(x, y);
    return point;
}

double RT::getAngle(const Vector vector1, const Vector vector2) {
    double PI = 3.141592653;
    double t = (vector1.x * vector2.x + vector1.y * vector2.y) / (sqrt(pow(vector1.x, 2) + pow(vector1.y, 2)) * sqrt(pow(vector2.x, 2) + pow(vector2.y, 2)));
    double angle = acos(t) * (180 / PI);
    return angle;
}

void RT::pushBack(int t, ListPoint *point) {
    ListPoint *last;
    if(t == 1){
        last = this->one;
    } else {
        last = this->two;
    }
    point->next = nullptr;
    while(last->next != nullptr){
        last = last->next;
    }
    last->next = point;
    point->parent = last;
}

ListPoint *RT::getNearestIndexPoint(ListPoint *list, ListPoint *point) {
    auto *minIndex = new ListPoint;
    auto *head = list;
    double minD = 1.79769e+308;
    double d = 0;
    while(head){
        d = pow((point->x - head->x), 2) + pow((point->y - head->y), 2);
        if(d + EPS < minD){
            minD = d;
            minIndex = head;
        }
        head = head->next;
    }
    return minIndex;
}

ListObstacle *RT::getNearestIndexObstacle(ListPoint *point) {
    auto *minIndex = new ListObstacle;
    auto *head = this->obstacle;
    double minD = 1.79769e+308;
    double d = 0;
    while(head){
        d = sqrt(pow(head->x - point->x, 2) + pow((head->y - point->y), 2)) - head->r;
        if(d+EPS<minD){
            minD = d;
            minIndex = head;
        }
        head = head->next;
    }
    return minIndex;
}

double RT::dynamicStep(ListPoint *n_point, ListPoint *a_point) {
    double theta = atan2(a_point->y - n_point->y, a_point->x - n_point->x);
    a_point->x += cos(theta) * (this->dist + this->step) / 2;
    a_point->y += sin(theta) * (this->dist + this->step) / 2;

    auto * obstacle = getNearestIndexObstacle(a_point);

    double l_n = sqrt(pow(n_point->x-obstacle->x, 2)+pow(n_point->y - obstacle->y, 2)) - obstacle->r;
    double dynamic = this->step / (1 + (this->step / this->dist - 1) * exp( -3 * l_n / this->step));
    return dynamic;
}

bool RT::collisionCheck(int t,const ListPoint *newPoint) {
    bool flag = true;
    ListObstacle *head = this->obstacle;
    while(head != nullptr){
        double dx = head->x - newPoint->x;
        double dy = head->y - newPoint->y;
        double d = sqrt(pow(dx, 2) + pow(dy, 2));

        ListPoint *parentPoint = newPoint->parent;

        Vector vector_p_n = {newPoint->x - parentPoint->x, newPoint->y - parentPoint->y};
        Vector vector_p_o = {head->x - parentPoint->x, head->y - parentPoint->y};
        double d_p_n = abs(sqrt(pow(vector_p_o.x, 2) + pow(vector_p_o.y, 2)) * sin(getAngle(vector_p_n, vector_p_o)));
        if(d + EPS < head->r || d_p_n + EPS < head ->r){
            flag = false;
            break;
        }
        head = head->next;
    }
    return flag;
}

bool RT::angleCheck(const ListPoint *newPoint, const ListPoint *parentPoint, const ListPoint *ancestorPoint) {
    Vector vector_p_n = {newPoint->x - parentPoint->x, newPoint->y - parentPoint->y};

    Vector vector_a_p = {parentPoint->x - ancestorPoint->x, parentPoint->y - ancestorPoint->y};

    double angle = getAngle(vector_a_p, vector_p_n);
    if(angle+EPS <= this->angle){
        return true;
    } else{
        return false;
    }
}

bool RT::conditionCheck(int t,const ListPoint *newPoint) {
    if(collisionCheck(t, newPoint)){
        ListPoint *parentPoint = newPoint->parent;
        if(parentPoint->parent == nullptr){
            return false;
        }
        ListPoint *ancestorPoint = parentPoint->parent;
        if(angleCheck(newPoint, parentPoint, ancestorPoint)){
            return true;
        } else {
            return false;
        }
    } else {
        return false;
    }
}

bool RT::perfectConnect(const ListPoint *onePoint, const ListPoint *twoPoint) {
    ListPoint *oneParent = onePoint->parent;
    ListPoint *twoParent = twoPoint->parent;

    Vector vector_n_w = {onePoint->x - oneParent->x, onePoint->y - oneParent->y};

    Vector vector_w_x = {twoPoint->x - onePoint->x, twoPoint->y - onePoint->y};

    Vector vector_x_j = {twoParent->x - twoPoint->x, twoParent->y - twoPoint->x};

    double angle_one = getAngle(vector_n_w, vector_w_x);
    double angle_two = getAngle(vector_w_x, vector_x_j);
    if(angle_one <= this->angle - EPS){
        if(fabs(angle_two - 180.0) < EPS || fabs(angle_one - 0.0) < EPS){
            return false;
        }else{
            return true;
        }
    }else{
        return false;
    }
}

ListPoint *RT::coordinate(int t, ListPoint *rnd) {
    // 寻找最近节点
    auto *nearestPoint = new ListPoint;
    if(t==1) {
        nearestPoint = getNearestIndexPoint(this->one, rnd);
    } else {
        nearestPoint = getNearestIndexPoint(this->two, rnd);
    }
    // 按照原始步长计算虚坐标
    double theta = atan2(rnd->y - nearestPoint->y, rnd->x - nearestPoint->x);
    auto *newPoint = new ListPoint(nearestPoint->x + cos(theta) * this->step, nearestPoint->y + sin(theta) * this->step);
    // 使用动态步长计算实际坐标
    double actualStep = dynamicStep(nearestPoint, newPoint);
    newPoint->x = nearestPoint->x + cos(theta) * actualStep;
    newPoint->y = nearestPoint->y + sin(theta) * actualStep;
    newPoint->parent = nearestPoint;
    return newPoint;
}

void RT::planning() {
    while(true){
        ListPoint *rnd = randomPoint();
        ListPoint *newPoint=coordinate(1, rnd);

        if(!conditionCheck(1, newPoint)){
            continue;
        }
        pushBack(1, newPoint);

        ListPoint *newPointTwo = coordinate(2, newPoint);
        if(!conditionCheck(2, newPointTwo)){
            continue;
        }
        pushBack(2, newPointTwo);

        double dx = newPoint->x - newPointTwo->x;
        double dy = newPoint->y - newPointTwo->y;
        double d = sqrt(pow(dx, 2)+ pow(dy, 2));

        if(this-> dist+ EPS < d && d + EPS <this->step){
            if(perfectConnect(newPoint, newPointTwo)){
                break;
            }
            else{
                continue;
            }
        }else{
            continue;
        }
    }
    ListPoint *tempOne = this->one;
    while(tempOne!= nullptr){
        cout<<tempOne->x<<"  "<<tempOne->y<<endl;
        tempOne = tempOne->next;
    }
    ListPoint *tempTwo = this->two;
    while(tempTwo != nullptr){
        cout<<tempTwo->x<<"  "<<tempTwo->y<<endl;
        tempTwo = tempTwo->next;
    }
}

主程序

#include "headers//RRT.h"
using namespace std;

const double ANGLE = 60.0;
const double STEP = 10.0;
const double DISTANCE = 5.0;

int main() {
    double obstacle_x[]= {50, 50, 50};
    double obstacle_y[]= {50, 13, 87};
    double obstacle_r[]= {15, 12, 11};
    auto * obstacle = new ListObstacle(50, 50, 15);
    for(int i = 1; i<3; i++){
        auto *node = new ListObstacle;
        node->x = obstacle_x[i];
        node->y = obstacle_y[i];
        node->r = obstacle_r[i];

        node->next = obstacle->next;
        obstacle->next = node;
    }
    auto *start = new ListPoint(0, 0);
    auto *goal = new ListPoint(100, 100);
    auto *safe = new ListPoint(20, 20);
    auto *recover = new ListPoint(90, 90);
    RT rrt = RT(start, goal, safe, recover, ANGLE, STEP, DISTANCE, obstacle);
    rrt.planning();
}