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 ne