手撕Dijkstra、RRT、RRT*算法（Python、C++版本）

已于 2024-02-01 15:23:06 修改
阅读量730 收藏 13
点赞数 8
文章标签： c++ 开发语言
于 2024-01-19 09:18:28 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/Legendyyy/article/details/135688679
版权

路径规划设计选择无人机飞行的轨迹和航线，以确保它达到既定的目标，并在飞行过程中避免障碍物和冲突。这里我对传统的路径规划代码Python和C++版本进行了学习。

一、Dijkstra(Python)

        Dijkstra算法通过代价函数来计算每个节点的代价，从而选择代价最小的那一个节点进行移动。

import math
import matplotlib.pyplot as plt

show_animation = True

class Dijkstra:
    def __init__(self, ox, oy, grid_size, robot_radius):
        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        self.resolution = grid_size
        self.robot_radius = robot_radius
        self.x_width = round((self.max_x-self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        self.obstacle_map = [[False for _ in range(self.y_width)]
                             for _ in range(self.x_width)]

        self.create_obstacle_map(ox, oy)
        self.motion = self.get_motion_model()

    def create_obstacle_map(self, ox, oy):
        for ix in range(self.x_width):
            x = self.calc_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(x-iox, y-ioy)
                    if d <= self.robot_radius:
                        self.obstacle_map[ix][iy] = True

    class Node:
        def __init__(self, x, y, cost, parent_index):
            self.x = x
            self.y = y
            self.cost = cost
            self.parent_index = parent_index

    def planning(self, sx, sy, gx, gy):
        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                               self.calc_xy_index(gy, self.min_y), 0.0, -1)

        open_set, closed_set = dict(), dict()

        open_set[self.calc_index(start_node)] = start_node

        while 1:
            n_id = min(open_set, key=lambda o: open_set[o].cost)
            current = open_set[n_id]

            del open_set[n_id]
            closed_set[n_id] = current

            if show_animation:
                plt.plot(self.calc_position(current.x, self.min_x),
                         self.calc_position(current.y, self.min_y), 'xc')
                plt.gcf().canvas.mpl_connect(
                    'key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else None])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)

            if current.x == goal_node.x and current.y == goal_node.y:
                goal_node.cost = current.cost
                goal_node.parent_index = current.parent_index
                break

            for move_x, move_y, move_cost in self.motion:
                x = move_x + current.x
                y = move_y + current.y
                cost = move_cost + current.cost
                new_node = self.Node(x, y, cost, n_id)
                c_id = self.calc_index(new_node)

                if c_id in closed_set:
                    continue

                if not self.verify_node(new_node):
                    continue

                if c_id not in open_set:
                    open_set[c_id] = new_node
                else:
                    if open_set[c_id].cost > new_node.cost:
                        open_set[c_id] = new_node

        rx, ry = [], []
        while goal_node.parent_index != -1:
            rx.append(self.calc_position(goal_node.x, self.min_x))
            ry.append(self.calc_position(goal_node.y, self.min_y))
            goal_node = closed_set[goal_node.parent_index]

        return rx, ry

    def get_motion_model(self):
        motion = [
            [1, 0, 1],
            [0, 1, 1],
            [-1, 0, 1],
            [0, -1, 1],
            [1, 1, math.sqrt(2)],
            [1, -1, math.sqrt(2)],
            [-1, 1, math.sqrt(2)],
            [-1, -1, math.sqrt(2)],
        ]
        return motion

    def calc_position(self, index, minp):
        return index * self.resolution + minp

    def calc_xy_index(self, position, minp):
        return round((position-minp) / self.resolution)

    def calc_index(self, node):
        return node.y * self.x_width + node.x

    def verify_node(self, node):
        if self.calc_position(node.x, self.min_x) < self.min_x:
            return False
        if self.calc_position(node.x, self.min_x) >= self.max_x:
            return False
        if self.calc_position(node.y, self.min_y) < self.min_y:
            return False
        if self.calc_position(node.y, self.min_y) >= self.max_y:
            return False
        if self.obstacle_map[node.x][node.y]:
            return False

        return True


def main():
    sx = -5.0
    sy = -5.0
    gx = 50.0
    gy = 50.0
    grid_size = 2.0
    robot_radius = 1.0

    ox, oy = [],[]

    for i in range(-10,60):
        ox.append(i)
        oy.append(-10)
    for i in range(-10,60):
        ox.append(-10)
        oy.append(i)
    for i in range(-10,61):
        ox.append(60)
        oy.append(i)
    for i in range(-10,61):
        ox.append(i)
        oy.append(60)
    for i in range(-10,40):
        ox.append(10)
        oy.append(i)
    for i in range(0,30):
        ox.append(40)
        oy.append(60-i)

    if show_animation:
        plt.plot(sx, sy, 'og')
        plt.plot(gx, gy, 'xb')
        plt.plot(ox, oy, '.k')
        plt.grid(True)
        plt.axis('equal')

    dijkstra = Dijkstra(ox, oy, grid_size, robot_radius)
    rx, ry = dijkstra.planning(sx, sy, gx, gy)

    if show_animation:
        plt.plot(rx, ry, 'r-')
        plt.pause(0.01)
        plt.show()


if __name__ == "__main__":
    main()

二、Dijkstra(C++)

#include "Dijkstra.h"

Dijkstra::Node::Node(double x, double y, float cost, double parentIndex) : x(x), y(y), cost(cost), parent_index(parentIndex) {}

Dijkstra::Dijkstra( double resolution, double robotRadius) :  resolution(resolution), robot_radius(robotRadius) {}

void Dijkstra::calObstacleMap(const vector<double> &ox, const vector<double> &oy){
    min_x = round(*min_element(ox.begin(),ox.end()));
    min_y = round(*min_element(oy.begin(),oy.end()));
    max_x = round(*max_element(ox.begin(),ox.end()));
    max_y = round(*max_element(oy.begin(),oy.end()));
    //确定栅格大小
    x_width = round((max_x-min_x)/resolution);
    y_width = round((max_y-min_y)/resolution);
    //确定栅格障碍物
    obstacle_map=vector<vector<bool>>(x_width,vector<bool>(y_width,false));

    for(double i=0;i<x_width;i++){
        double x = calPosition(i,min_x);
        for(double j=0;j<y_width;j++){
            double y = calPosition(j,min_y);
            for(double k=0;k<ox.size();k++)
            {
                double d = sqrt(pow(ox[k]-x,2)+pow(oy[k]-y,2));
                if(d<=robot_radius){
                    obstacle_map[i][j] = true;
                    break;
                }
            }
        }
    }

}

/**
*计算栅格在地图中的位置
*@param index
*@param minp
*@return
*/

double Dijkstra::calPosition(double index, double minp){
    double pos = index * resolution + minp;
    return pos;
}
/**
 * 标记移动代价
 * @return
*/
vector<vector<double>> Dijkstra::getMotionModel(){
    motion = {{1, 0, 1},
              {0, 1, 1},
              {-1,0, 1},
              {0,-1, 1},
              {1, 1, sqrt(2)},
              {1,-1, sqrt(2)},
              {-1,-1,sqrt(2)},
              {-1,1,sqrt(2)}
              };
    return motion;
}
/**
 * 计算地图在栅格的位置
 * @param position
 * @param minp
 * @return
*/
double Dijkstra::calXyIndex(double position,double minp){
    return round((position - minp)/resolution);
}
/**
 * 计算栅格索引
 * @param node
 * @return
*/
double Dijkstra::calIndex(Dijkstra::Node *node) {
    return (node->y - min_y) * x_width + (node->x - min_x);
}
/**
 * 判断节点是否有效，即是否超出边界或碰到障碍物
 * @param node
 * @return
*/
bool Dijkstra::verifyNode(Dijkstra::Node *node){
    double px = calPosition(node->x,min_x);
    double py = calPosition(node->y,min_y);
    if(px<min_x) return false;
    if(px>=max_x) return false;
    if(py<min_y) return false;
    if(py>=max_y) return false;
    if(obstacle_map[node->x][node->y]) return false;
    return true;
}

/**
 * 计算路径，方便画图
 * @param goal_node
 * @param closed_set
 * @return
*/
pair<vector<double>,vector<double>> Dijkstra::calFinalPath(Dijkstra::Node *goal_node,map<double, Node *> closed_set){
    vector<double> rx,ry;
    rx.push_back(calPosition(goal_node->x,min_x));
    ry.push_back(calPosition(goal_node->y,min_y));

    double parent_index = goal_node->parent_index;

    while(parent_index!=-1){
        Node *node = closed_set[parent_index];
        rx.push_back(calPosition(node->x,min_x));
        ry.push_back(calPosition(node->y,min_y));
        parent_index = node->parent_index;
    }
    return {rx,ry};
}

/**
 * 规划
 * @param start 起点
 * @param goal 终点
 * @return 规划后的路径
*/
pair<vector<double>,vector<double>> Dijkstra::planning(vector<double> start,vector<double> goal){
    double sx = start[0],sy = start[1];
    double gx = goal[0],gy = goal[1];
    Node *start_node = new Node(calXyIndex(sx,min_x),calXyIndex(sy,min_y),0.0,-1);
    Node *goal_node = new Node(calXyIndex(gx,min_x),calXyIndex(gy,min_y),0.0,-1);
    //定义映射
    map<double,Node *> open_set,closed_set;
    open_set[calIndex(start_node)] = start_node;
    Node *current;
    while(true){
        //返回该数据类型的可以表示的最大值
        double c_id = numeric_limits<double>::max();
        double cost = numeric_limits<double>::max();
        //计算代价最小的节点
        for(auto it=open_set.begin();it!=open_set.end();it++){
            //first会得到Map中key的有效值，second会得到Map中value的有效值。
            if(it->second->cost < cost){
                cost = it->second->cost;
                c_id = it->first;
            }
        }
        current = open_set[c_id];
        plotGraph(current);

        if(abs(current->x - goal_node->x)<EPS && abs(current->y - goal_node->y)<EPS){
            cout << "Find goal" <<endl;
            goal_node->parent_index = current->parent_index;
            goal_node->cost = current->cost;
            break;
        }

        //从open set中移除
        auto iter = open_set.find(c_id);
        open_set.erase(iter);
        //将其加入到closed set
        closed_set[c_id] = current;

        for(vector<double>move:motion){
            Node *node = new Node(current->x+move[0],current->y+move[1],current->cost+move[2],c_id);
            double n_id = calIndex(node);

            if(closed_set.find(n_id) != closed_set.end()) continue;

            if(!verifyNode(node)) continue;
            if(open_set.find(n_id) == open_set.end()){
                open_set[n_id] = node;
            }else {
                if(open_set[n_id]->cost>node->cost){
                    open_set[n_id]=node;
                }
            }
        }
    }
    return calFinalPath(goal_node,closed_set);
}
/**
 * @param current
*/
void Dijkstra::plotGraph(Dijkstra::Node *current){
    plt::plot(ox,oy,".k");
    plt::plot(vector<double>{st[0]}, vector<double>{st[1]},"og");
    plt::plot(vector<double>{go[0]}, vector<double>{go[1]},"xb");
    plt::grid(true);
    plt::plot(vector<double>{calPosition(current->x,min_x)},vector<double>{calPosition(current->y,min_y)},"xc");
    plt::pause(0.001);
}

void Dijkstra::setSt(const vector<double> &st){
    Dijkstra::st = st;
}

void Dijkstra::setGo(const vector<double> &go){
    Dijkstra::go = go;
}

void Dijkstra::setOx(const vector<double> &ox){
    Dijkstra::ox = ox;
}

void Dijkstra::setOy(const vector<double> &oy){
    Dijkstra::oy = oy;
}
#ifndef CPP_DIJKSTRA_H
#define CPP_DIJKSTRA_H

#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include <cmath>
#include <algorithm>
#include <stdlib.h>
#include <time.h>
#include <map>
#include "../../matplotlibcpp.h"
namespace plt = matplotlibcpp;

using namespace std;
using namespace Eigen;

#define EPS 1e-4
#define PI 3.14159265354

class Dijkstra {
public:
        struct Node{
            double x;
            double y;
            float cost;
            double parent_index;

            Node(double x,double y,float cost,double parent_index);
        };
    
private:
        double resolution;
        double robot_radius;
        double min_x,min_y,max_x,max_y;
        double x_width,y_width;

        vector<vector<bool>> obstacle_map;
        vector<vector<double>> motion;
        vector<double>st,go;
        vector<double>ox,oy;

public:
    Dijkstra(double resolution,double robotRadius);

    void calObstacleMap(const vector<double> &ox, const vector<double> &oy);

    double calPosition(double index,double minp);

    vector<vector<double>>getMotionModel();

    double calXyIndex(double position, double minp);

    double calIndex(Node*node);

    bool verifyNode(Node*node);

    pair<vector<double>,vector<double>>calFinalPath(Node*goal_node, map<double,Node*>closed_set);

    pair<vector<double>,vector<double>>planning(vector<double>start,vector<double>goal);

    void plotGraph(Node*current);

    void setSt(const vector<double> &st);

    void setGo(const vector<double> &go);

    void setOx(const vector<double> &Ox);

    void setOy(const vector<double> &Oy);
};


#endif
#include "Dijkstra.h"

int main(){
    vector<double> start{-5,-5},goal{50,50};
    double grid_size=2.0;
    double robot_radius = 1.0;

    vector<double> ox;
    vector<double> oy;

    for(double i=-10;i<60;i++){
        ox.push_back(i);
        oy.push_back(-10.0);
    }
    for(double i=-10;i<60;i++){
        ox.push_back(-10);
        oy.push_back(i);
    }
    for(double i=-10;i<61;i++){
        ox.push_back(i);
        oy.push_back(60);
    }
    for(double i=-10;i<61;i++){
        ox.push_back(60);
        oy.push_back(i);
    }
    for(double i=-10;i<30;i++){
        ox.push_back(10);
        oy.push_back(i);
    }
    for(double i=30;i<60;i++){
        ox.push_back(40);
        oy.push_back(i);
    }
    Dijkstra dijkstra(grid_size,robot_radius);
    dijkstra.setGo(goal);
    dijkstra.setSt(start);
    dijkstra.setOx(ox);
    dijkstra.setOy(oy);

    dijkstra.calObstacleMap(ox,oy);
    dijkstra.getMotionModel();

    pair<vector<double>,vector<double>> xy = dijkstra.planning(start,goal);

    plt::plot(xy.first,xy.second,"-r");
    plt::show();
    return 0;
}

三、RRT(Python)

        RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。

import random
import time
import copy
import matplotlib.pyplot as plt
import math
import numpy as np

show_animation = True

class Node:
    def __init__(self, x, y, cost=0.0, parent=None):
        self.x = x
        self.y = y
        self.cost = cost
        self.parent = parent

class RRT:
    def __init__(self, randArea, obstacleList,
                 expandDis=2.0, goalSampleRate=10, maxIter=200):
        self.start = None
        self.goal = None
        self.min_rand = randArea[0]
        self.max_rand = randArea[1]
        self.obstacle_list = obstacleList
        self.expand_dis = expandDis
        self.goal_sample_rate = goalSampleRate
        self.max_iter = maxIter
        self.node_list = None

    def rrt_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None

        for i in range(self.max_iter):
            rnd = self.sample()
            n_id = self.find_nearest_node(rnd)
            nearestNode = self.node_list[n_id]

            theta = math.atan2(rnd[1]-nearestNode.y, rnd[0]-nearestNode.x)
            newNode = self.create_newNode(theta, n_id, nearestNode)

            nocollision = self.check_collision(newNode, nearestNode)

            if nocollision:
                self.node_list.append(newNode)

                if animation:
                    self.draw_graph(newNode, path)

                if self.point_cost(newNode, self.goal) <= self.expand_dis:
                    if self.check_collision(newNode, self.goal):
                        lastIndex = len(self.node_list) - 1
                        path = self.find_final(lastIndex)
                        pathLen = self.calc_len(path)
                        print("The total length is {},it takes {} seconds",pathLen,time.time()-start_time)

                        if animation:
                            self.draw_graph(newNode, path)
                        return path

    def sample(self):
        if random.randint(0, 100) > self.goal_sample_rate:
            return [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:
            return [self.goal.x, self.goal.y]

    def find_nearest_node(self, rnd):
        d_List = [(node.x-rnd[0])**2
                  + (node.y-rnd[1])**2 for node in self.node_list]
        n_id = d_List.index(min(d_List))
        return n_id

    def create_newNode(self, theta, n_id, nearestNode):
        newNode = copy.deepcopy(nearestNode)
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)
        newNode.cost += self.expand_dis
        newNode.parent = n_id
        return newNode

    def check_collision(self, node1, node2):
        for [ox, oy, size] in self.obstacle_list:
            d = self.point_to_line(np.array([node1.x, node1.y]),
                                   np.array([node2.x, node2.y]),
                                   np.array([ox, oy]))
            if d <= size**2:
                return False
        return True

    def point_to_line(self, v, w, p):
        if np.array_equal(v, w):
            return (p-v).dot(p-v)
        t = (p-v).dot(w-v) / (w-v).dot(w-v)
        projection = v + max(0, min(1, t)) * (w - v)
        return (projection - p).dot(projection - p)

    def point_cost(self, node1, node2):
        return math.sqrt((node1.x - node2.x)**2 + (node1.y-node2.y)**2)

    def point_cost_array(self, array1, array2):
        return math.sqrt((array1[0] - array2[0])**2 + (array1[1]-array2[1])**2)

    def draw_graph(self, rnd=None, path=None):
        plt.clf()
        # for stopping simulation with the esc key.
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        #黑色三角表示新探索的节点
        if rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
        for node in self.node_list:
            if node.parent is not None:
                plt.plot([node.x, self.node_list[node.parent].x],
                         [node.y, self.node_list[node.parent].y],"-g")

        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], "-r")

        for [ox, oy, size] in self.obstacle_list:
            plt.plot(ox, oy, "ok", ms=size*30)

        plt.plot(self.start.x, self.start.y, 'xb')
        plt.plot(self.goal.x, self.goal.y, 'xb')

        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

    def find_final(self, lastIndex):
        path = [[self.goal.x, self.goal.y]]
        while self.node_list[lastIndex].parent is not None:
            node = self.node_list[lastIndex]
            path.append([node.x, node.y])
            lastIndex = self.node_list[lastIndex].parent
        path.append([self.start.x, self.start.y])
        return path

    def calc_len(self, path):
        totalLen = 0.0
        for i in range(1, len(path)):
            totalLen += self.point_cost_array(path[i], path[i-1])

        return totalLen


def main():
    print("rrt start!")
    obstacleList = [
        [3,  3,  1.5],
        [12, 2,  3],
        [3,  9,  2],
        [8,  11, 2]
    ]

    rrt = RRT(randArea=[-2, 18], obstacleList=obstacleList, maxIter=200)
    path = rrt.rrt_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print("Done!!")

    if show_animation and path:
        plt.show()

if __name__ == '__main__':
    main()

四、RRT(C++)

#include "RRT.h"

RRT::Node::Node(double x,double y): x(x),y(y),parent(NULL),cost(0) {}
RRT::RRT(const vector<vector<double>> &obstacleList,const vector<double> &randArea,const vector<double> &playArea,
double robotRadius,double expandDis,double goalSampleRate,int maxIter) : obstacle_list(obstacleList),rand_area(randArea),play_area(playArea),
robot_radius(robotRadius),expand_dis(expandDis),goal_sample_rate(goalSampleRate),max_iter(maxIter) {}

double RRT::calDisToGoal(RRT::Node* node){
    double dx = node->x - end->x;
    double dy = node->y - end->y;
    double dist = sqrt(pow(dx,2) + pow(dy,2));
    return dist;
}

double RRT::calNodeDist(RRT::Node* n1,RRT::Node* n2){
    double dx = n1->x - n2->x;
    double dy = n1->y - n2->y;
    double dist = sqrt(pow(dx,2) + pow(dy,2));
    return dist;
}

double RRT::calPointDist(vector<double> p1,vector<double> p2){
    double dx = p1[0] - p2[0];
    double dy = p1[1] - p2[1];
    double dist = sqrt(pow(dx,2) + pow(dy,2));
    return dist;
}

vector<double> RRT::calDistAngle(RRT::Node* from_node,RRT::Node* to_node){
    double dx = to_node->x - from_node->x;
    double dy = to_node->y - from_node->y;
    double dist = sqrt(pow(dx,2) + pow(dy,2));
    double angle = atan2(dy,dx);
    return {dist,angle};
}

int RRT::findNearestNodeIndex(RRT::Node* rnd_node){
    double d = numeric_limits<double>::max();
    int index = -1;
    for(int i=0;i<node_list.size();i++){
        double dist = calNodeDist(rnd_node,node_list[i]);
        if(dist < d){
            index = i;
            d = dist;
        }
    }
    return index;
}

RRT::Node* RRT::sampleFree(){
    double new_x,new_y;
    if(rand() % (100) > goal_sample_rate){
        new_x = rand()/double(RAND_MAX)*(rand_area[1]-rand_area[0]) + rand_area[0];
        new_y = rand()/double(RAND_MAX)*(rand_area[1]-rand_area[0]) + rand_area[0];
    }else{
        new_x = end->x;
        new_y = end->y;
    }
    Node* new_node = new Node(new_x,new_y);
    return new_node;
}

RRT::Node* RRT::steer(RRT::Node* from_node,RRT::Node* to_node,double expand_length){
    vector<double> dist_angle = calDistAngle(from_node,to_node);
    double new_x,new_y;
    if(dist_angle[0] <= expand_length){
        new_x = to_node->x;
        new_y = to_node->y;
    }else{
        new_x = from_node->x + expand_length * cos(dist_angle[1]);
        new_y = from_node->y + expand_length * sin(dist_angle[1]);
    }
    Node* new_node = new Node(new_x,new_y);
    new_node->parent = from_node;
    new_node->path_x.push_back(from_node->x);
    new_node->path_x.push_back(new_node->x);
    new_node->path_y.push_back(from_node->y);
    new_node->path_y.push_back(new_node->y);
    return new_node;
}

bool RRT::isInsidePlayArea(RRT::Node* node){
    if(node->x < play_area[0] || node->x >= play_area[1] || node->y < play_area[2] || node->y >= play_area[3])
        return false;
    else
        return true;
}

bool RRT::obstacleFree(RRT::Node* node){
    vector<double> p1{node->x,node->y};
    vector<double> p2{node->parent->x,node->parent->y};
    for(vector<double> ob:obstacle_list){
        vector<double> center{ob[0],ob[1]};
        double t = ((center[1]-p1[1])*(p2[1]-p1[1])+(center[0]-p1[0])*(p2[0]-p1[0]))/(pow(p2[1]-p1[1],2)+pow(p2[0]-p1[0],2));
        double t_min_1 = (t>1)? 1:t;
        double t_max_0 = (t_min_1>0)? t_min_1:0;
        vector<double> projection{p1[0]+t_max_0*(p2[0]-p1[0]),p1[1]+t_max_0*(p2[1]-p1[1])};
        double dist = calPointDist(projection,center);
        if(dist <= (ob[2] + robot_radius))
            return false;
    }
    return true;
}

pair<vector<double>,vector<double>> RRT::getFinalCourse(int final_index){
    vector<double> x_t,y_t;
    Node* node = node_list[final_index];
    while(node->parent){
        x_t.push_back(node->x);
        y_t.push_back(node->y);
        node = node->parent;
    }
    x_t.push_back(node->x);
    y_t.push_back(node->y);
    return {x_t,y_t};
}

pair<vector<double>,vector<double>> RRT::planning(){
    node_list.push_back(begin);
    for(int i=0;i<max_iter;i++){
        Node* rnd_node = sampleFree();
        int index = findNearestNodeIndex(rnd_node);
        Node* from_node = node_list[index];
        Node* new_node = steer(from_node,rnd_node,expand_dis);
        if(!isInsidePlayArea(new_node))
            continue;
        if(!obstacleFree(new_node))
            continue;
        node_list.push_back(new_node);
        draw(new_node);
        if(calNodeDist(new_node,end)<=expand_dis){
            Node* final_node = steer(new_node,end,expand_dis);
            if(!obstacleFree(final_node))
                continue;
            node_list.push_back(final_node);
            cout<< "Found Goal" << endl;
            return getFinalCourse(node_list.size()-1);
        }
    }
    cout<< "Cannot Found Goal" << endl;
    return {};
}

void RRT::draw(RRT::Node* node){
    plt::clf();

    if(node){
        plt::plot(vector<double>{node->x},vector<double>{node->y},"^k");
        plotCircle(node->x,node->y,robot_radius,"-r");
    }

    for(vector<double> ob:obstacle_list){
        plotCircle(ob[0],ob[1],ob[2]);
    }

    for(Node* node1: node_list){
        if(node1->parent)
            plt::plot(node1->path_x,node1->path_y,"-g");
    }

    plt::plot(vector<double>{begin->x},vector<double>{begin->y},"xr");
    plt::plot(vector<double>{end->x},vector<double>{end->y},"xr");

    plt::plot(vector<double>{play_area[0],play_area[0],play_area[1],play_area[1],play_area[0]},
            vector<double>{play_area[2],play_area[3],play_area[3],play_area[2],play_area[2]},"k-");

    plt::grid(true);
    plt::axis("equal");
    plt::xlim(play_area[0]-1,play_area[1]+1);
    plt::ylim(play_area[2]-1,play_area[3]+1);
    plt::pause(0.01);
}

void RRT::setBegin(RRT::Node* begin){
    RRT::begin = begin;
}

void RRT::setAnEnd(RRT::Node* anEnd){
    RRT::end = anEnd;
}

void RRT::plotCircle(double x,double y,double size,string color){
    vector<double> x_,y_;
    for(double i=0;i<2*PI;i+=0.01){
        x_.push_back(x+size*cos(i));
        y_.push_back(y+size*sin(i));
    }
    plt::plot(x_,y_,color);
}
#ifndef CPP_RRT_H
#define CPP_RRT_H

#include <iostream>
#include <Eigen/Dense>
#include <stdlib.h>
#include <time.h>
#include <vector>
#include <algorithm>
#include <cmath>
#include "../../matplotlibcpp.h"

namespace plt = matplotlibcpp;
using namespace std;
using namespace Eigen;

#define PI 3.141592653

class RRT {
public:
    struct Node{
        double x,y;
        vector<double> path_x={},path_y={};
        Node(double x,double y);
        Node*parent;
        double cost;
    };

public:
    vector<vector<double>> obstacle_list;
    vector<double> rand_area,play_area;
    vector<Node*> node_list;
    Node* begin;
    Node* end;
    double expand_dis;
    double goal_sample_rate;
    double robot_radius;

    int max_iter;

public:
    RRT(const vector<vector<double>> &obstacleList,const vector<double> &randArea,const vector<double> &playArea,
    double robotRadius,double expandDis,double goalSampleRate,int maxIter);
    double calDisToGoal(Node* node);
    double calNodeDist(Node* n1,Node* n2);
    double calPointDist(vector<double> p1,vector<double> p2);
    vector<double> calDistAngle(Node* from_node,Node* to_node);
    int findNearestNodeIndex(Node* rnd_node);
    Node* sampleFree();
    Node* steer(Node* from_node,Node* to_node,double expand_length = numeric_limits<double>::max());
    bool isInsidePlayArea(Node* node);
    bool obstacleFree(Node* node);
    pair<vector<double>,vector<double>> getFinalCourse(int final_index);
    pair<vector<double>,vector<double>> planning();
    void draw(Node* node);
    void setBegin(Node* begin);
    void setAnEnd(Node* anEnd);
    void plotCircle(double x,double y,double size,string color="b");
};

#endif
#include "RRT.h"

int main(){
    RRT::Node* begin = new RRT::Node(0,0);
    RRT::Node* end = new RRT::Node(6,10);
    srand((unsigned)time(NULL));

    vector<vector<double>> obstacleList = {
        {5, 5, 1},
        {3, 6, 2},
        {3, 8, 2},
        {3, 10, 2},
        {7, 5, 2},
        {9, 5, 2},
        {8,10,1},
        {6,12,1}
    };

    double robotRadius = 0.8;
    vector<double> randArea = {-2,15};
    vector<double> playArea = {-2,12,0,14};
    double expandDis = 3.0;
    double goalSampleRate = 8.0;
    int maxIter = 1000;

    RRT rrt(obstacleList,randArea,playArea,
    robotRadius,expandDis,goalSampleRate,maxIter);

    rrt.setBegin(begin);
    rrt.setAnEnd(end);

    pair<vector<double>,vector<double>> traj =  rrt.planning();

    plt::plot(traj.first,traj.second,"-r");
    plt::show();

    return 0;
}

五、RRT*(Python)

import math
import time
import copy
import random

import numpy as np
import matplotlib.pyplot as plt

show_animation = True

class RRT:
    def __init__(self, obstacleList, randArea,
                 expandDis=2.0, goalSampleRate=10.0, maxIter=200):
        self.obstacle_list = obstacleList
        self.rand_area = randArea
        self.min_rand = randArea[0]
        self.max_rand = randArea[1]
        self.expand_dis = expandDis
        self.goal_sample_rate = goalSampleRate
        self.max_iter = maxIter
        self.start = None
        self.goal = None
        self.node_list = None

    def rrt_star_planning(self, start, goal, animation=True):
        start_time = time.time()
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.node_list = [self.start]
        path = None
        lastPathLength = float('inf')

        for i in range(self.max_iter):
            rnd = self.sample()
            n_ind = self.find_nearest_index(self.node_list, rnd)
            nearestNode = self.node_list[n_ind]
            theta = math.atan2(rnd[1]-nearestNode.y, rnd[0]-nearestNode.x)
            newNode = self.get_new_node(theta, n_ind, nearestNode)
            noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
            if noCollision:
                nearInd = self.find_near_node(newNode)
                newNode = self.choose_parent(newNode, nearInd)
                self.node_list.append(newNode)
                self.rewire(newNode, nearInd)

                if self.node_distance(newNode, self.goal) <= self.expand_dis:
                    if self.check_segment_collision(newNode.x, newNode.y, self.goal.x, self.goal.y):
                        pathTmp = self.get_final_course()
                        finalLength = self.get_final_length(pathTmp)
                        if finalLength < lastPathLength:
                            lastPathLength = finalLength
                            path = pathTmp
                            print("current path length: {}, It costs {} s".format(lastPathLength, time.time() - start_time))
                if animation:
                    self.drawGraph(newNode, path)




    def sample(self):
        if(random.uniform(0,100) > self.goal_sample_rate):
            return [random.uniform(self.min_rand, self.max_rand), random.uniform(self.min_rand, self.max_rand)]
        else:
            return [self.goal.x, self.goal.y]

    def find_nearest_index(self, node_list, rnd):
        dList = [(node.x-rnd[0])**2 + (node.y-rnd[1])**2 for node in node_list]
        return dList.index(min(dList))

    def get_new_node(self, theta, n_ind, nearestNode):
        newNode = copy.deepcopy(nearestNode)
        newNode.x += self.expand_dis * math.cos(theta)
        newNode.y += self.expand_dis * math.sin(theta)
        newNode.parent = n_ind
        newNode.cost = nearestNode.cost + self.expand_dis
        return newNode

    def check_segment_collision(self, x1, y1, x2, y2):
        for (ox, oy, size) in self.obstacle_list:
            d = self.line_to_circle(np.array([x1, y1]),
                                    np.array([x2, y2]),
                                    np.array([ox, oy]))
            if d <= size**2:
                return False
        return True

    def line_to_circle(self, v, w, p):
        if np.array_equal(v, w):
            return (p-v).dot(p-v)
        l2 = (w-v).dot(w-v)
        t = max(0, min(1, (p-v).dot(w-v)/l2))
        projection = t * (w-v) + v
        return (projection-p).dot(projection-p)

    def find_near_node(self, rnd):
        n_node = len(self.node_list)
        r = 50 * math.sqrt(math.log(n_node)/n_node)
        dList = [self.node_distance(node, rnd) for node in self.node_list]
        nearInd = [dList.index(i) for i in dList if i < r]
        return nearInd

    def node_distance(self, node1, node2):
        return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)

    def choose_parent(self, newNode, nearInd):
        if len(nearInd) == 0:
            return newNode
        dList = []
        for index in nearInd:
            nearNode = self.node_list[index]
            d = self.node_distance(newNode, nearNode)
            theta = math.atan2(newNode.y-nearNode.y, newNode.x - nearNode.x)
            if self.check_collision(theta, d, nearNode):
                dList.append(d + nearNode.cost)
            else:
                dList.append(float('inf'))
        # print(dList)
        minCost = min(dList)
        if minCost == float('inf'):
            print("min cost is inf")
            return newNode

        if minCost < newNode.cost:
            newNode.cost = minCost
            newNode.parent = nearInd[dList.index(minCost)]
        return newNode

    def check_collision(self, theta, d, nearNode):
        tmpNode = copy.deepcopy(nearNode)
        tmpNode.x += d * math.cos(theta)
        tmpNode.y += d * math.sin(theta)
        if self.check_segment_collision(tmpNode.x, tmpNode.y, nearNode.x, nearNode.y):
            return True
        return False

    def rewire(self, newNode, nearInd):
        n_node = len(self.node_list) - 1
        for index in nearInd:
            node = self.node_list[index]
            d = self.node_distance(newNode, node)
            theta = math.atan2(node.y - newNode.y, node.x - newNode.x)
            if self.check_collision(theta, d, newNode):
                if d + newNode.cost < self.node_list[index].cost:
                    self.node_list[index].cost = d + newNode.cost
                    self.node_list[index].parent = n_node

    def get_final_course(self):
        path = [[self.goal.x, self.goal.y]]
        n_node = len(self.node_list) - 1

        while self.node_list[n_node].parent is not None:
            path.append([self.node_list[n_node].x, self.node_list[n_node].y])
            n_node = self.node_list[n_node].parent
        path.append([self.start.x, self.start.y])
        return path

    def get_final_length(self, path):
        length = 0
        for i in range(1, len(path)):
            length += math.sqrt((path[i][0] - path[i-1][0])**2 + (path[i][1] - path[i-1][1])**2)
        return length

    def drawGraph(self, newNode, path):
        plt.clf()
        plt.gcf().canvas.mpl_connect(
            'key_release_event',
            lambda event: [exit(0) if event.key == 'escape' else None])
        if newNode is not None:
            plt.plot(newNode.x, newNode.y, "^k")
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.goal.x, self.goal.y, "xr")

        for node in self.node_list:
            if node.parent is not None:
                plt.plot([node.x, self.node_list[node.parent].x], [node.y, self.node_list[node.parent].y], "-g")

        for (ox, oy, size) in self.obstacle_list:
            plt.plot(ox, oy, "ok", ms=30 * size)

        if path is not None:
            plt.plot([x for (x, y) in path], [y for (x, y) in path], "-r")

        plt.axis([-2, 18, -2, 15])
        plt.grid(True)
        plt.pause(0.01)

class Node:
    def __init__(self, x, y, cost=0, parent=None):
        self.x = x
        self.y = y
        self.cost = cost
        self.parent = parent

def main():
    print("start rrt star planning")
    obstacleList = [
        (3, 3, 1.5),
        (12, 2, 3),
        (3, 9, 3),
        (9, 11, 2)
    ]

    rrt = RRT(obstacleList=obstacleList, randArea=[-2, 18])
    rrt.rrt_star_planning(start=[0, 0], goal=[15, 12], animation=show_animation)
    print("Done")

    if show_animation:
        plt.show()

if __name__ == "__main__":
    main()

六、RRT*(C++)

#include "RRT_Star.h"

RRT_Star::RRT_Star(const vector<vector<double>> &obstacleList,const vector<double> &randArea,const vector<double> &playArea,
    double robotRadius,double expandDis,double goalSampleRate,int maxIter,double connectCircleDist):RRT(obstacleList,randArea,playArea,robotRadius,expandDis,goalSampleRate,maxIter),connect_circle_dist(connectCircleDist) {}

pair<vector<double>,vector<double>> RRT_Star::planningSimplify(){
    node_list.push_back(begin);
    double len_min = numeric_limits<double>::max();
    bool found = false;
    for(int i=0;i<max_iter;i++){
        Node* rnd_node = sampleFree();
        int nearest_ind = findNearestNodeIndex(rnd_node);
        Node* nearest_node = node_list[nearest_ind];
        Node* new_node = steer(nearest_node,rnd_node,expand_dis);
        new_node->cost = nearest_node->cost + calNodeDist(nearest_node,new_node);
        if(obstacleFree(new_node) && isInsidePlayArea(new_node)){
            vector<int> near_inds = findNearInds(new_node);
            Node* update_node = chooseParent(new_node,near_inds);
            rewire(update_node,near_inds);
            node_list.push_back(update_node);
            if(calDisToGoal(update_node) <= expand_dis){
                Node* final_node = steerSimplify(update_node,end);
                if(obstacleFree(final_node)){
                    pair<vector<double>,vector<double>> pathTmp = getFinalCourseTmp();
                    double len = getFinalLength(pathTmp);
                    if(len < len_min){
                        len_min = len;
                        path = pathTmp;
                        found = true;
                        cout << "current path length" << len_min << endl;
                    }
                }
            }
        }
        draw(new_node);
        if(found){
            plt::plot(path.first,path.second,"-r");
        }
        plt::pause(0.01);
    }
    if(found){
        return path;
    }else{
        cout<< "Cannot Found Goal" << endl;
        return {};
    }
}

pair<vector<double>,vector<double>> RRT_Star::getFinalCourseTmp(){
    int nnode = node_list.size() - 1;
    vector<double> x_,y_;
    x_.push_back(end->x);
    y_.push_back(end->y);
    Node* node = node_list[nnode];
    while(node->parent){
        x_.push_back(node->x);
        y_.push_back(node->y);
        node = node->parent;
    }
    x_.push_back(node->x);
    y_.push_back(node->y);
    return {x_,y_};
}

double RRT_Star::getFinalLength(pair<vector<double>,vector<double>> pathTmp){
    double len;
    for(int i=1;i<pathTmp.first.size();i++){
        len += sqrt(pow(pathTmp.first[i] - pathTmp.first[i-1],2) + pow(pathTmp.second[i] - pathTmp.second[i-1],2));
    }
    return len;
}

vector<int> RRT_Star::findNearInds(RRT::Node* new_node){
    vector<int> inds;
    int nnode = node_list.size() + 1;
    double r = connect_circle_dist * sqrt(log(nnode)/nnode);
    for(int i=0;i<node_list.size();i++){
        if(calNodeDist(node_list[i],new_node) < r){
            inds.push_back(i);
        }
    }
    return inds;
}

RRT::Node* RRT_Star::chooseParent(RRT::Node* new_node,vector<int> near_inds){
    if(near_inds.empty()) return new_node;
    vector<double> costs;
    
    for(int i:near_inds){
        Node* node = steerSimplify(node_list[i],new_node);
        if(obstacleFree(node)){
            costs.push_back(node_list[i]->cost+calNodeDist(node_list[i],new_node));
        }else{
            costs.push_back(numeric_limits<double>::max());
        }
    }
    double cost_min = *min_element(costs.begin(),costs.end());
    if(cost_min == numeric_limits<double>::max()){
        cout << "Cannot find good parent" << endl;
        return new_node;
    }

    Node* parent_node = node_list[near_inds[min_element(costs.begin(),costs.end()) - costs.begin()]];
    Node* update_node = steerSimplify(parent_node,new_node);
    update_node->cost = parent_node->cost + calNodeDist(update_node,parent_node);
    return update_node;
}

RRT::Node* RRT_Star::steerSimplify(RRT::Node* from_node,RRT::Node* to_node){
    Node* node = new Node(to_node->x,to_node->y);
    node->parent = from_node;
    node->path_x.push_back(from_node->x);
    node->path_x.push_back(to_node->x);
    node->path_y.push_back(from_node->y);
    node->path_y.push_back(to_node->y);
    return node;
}

void RRT_Star::rewire(RRT::Node* update_node,vector<int> near_inds){
    for(int i :near_inds){
        Node* near_node = node_list[i];
        Node* edge_node = steerSimplify(update_node,near_node);
        edge_node->cost = update_node->cost + calNodeDist(update_node,near_node);
        if((edge_node->cost < near_node->cost) && obstacleFree(edge_node)){
            node_list[i] = edge_node;
        }
    }
}
#ifndef CPP_RRT_STAR_H
#define CPP_RRT_STAR_H

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

class RRT_Star : public RRT{
public:
    double connect_circle_dist;
    pair<vector<double>,vector<double>> path;

    RRT_Star(const vector<vector<double>> &obstacleList,const vector<double> &randArea,const vector<double> &playArea,
    double robotRadius,double expandDis,double goalSampleRate,int maxIter,double connectCircleDist);

    pair<vector<double>,vector<double>> planningSimplify();
    pair<vector<double>,vector<double>> getFinalCourseTmp();
    double getFinalLength(pair<vector<double>,vector<double>> pathTmp);
    vector<int> findNearInds(Node* new_node);
    Node* chooseParent(Node* new_node,vector<int> near_inds);
    Node* steerSimplify(Node* from_node,Node* to_node);
    void rewire(Node* update_node,vector<int> near_inds);
};

#endif
#include "RRT_Star.h"

int main(){
    RRT::Node* begin = new RRT::Node(0,0);
    RRT::Node* end = new RRT::Node(6,10);
    srand((unsigned)time(NULL));

    vector<vector<double>> obstacleList = {
        {5, 5, 1},
        {3, 6, 2},
        {3, 8, 2},
        {9, 5, 2},
        {8,10,1},
        {6,12,1}
    };

    double robotRadius = 0.8;
    vector<double> randArea = {-2,15};
    vector<double> playArea = {-2,12,0,14};
    double expandDis = 3.0;
    double goalSampleRate = 10.0;
    int maxIter = 800;
    double connectCircleDist = 25.0;

    RRT_Star rrt(obstacleList,randArea,playArea,
    robotRadius,expandDis,goalSampleRate,maxIter,connectCircleDist);

    rrt.setBegin(begin);
    rrt.setAnEnd(end);

    pair<vector<double>,vector<double>> traj = rrt.planningSimplify();
    plt::plot(traj.first,traj.second,"-r");
    plt::show();

    return 0;
}

基尼台妹
关注
路径规划 | 详解混合A*算法Hybrid A*(附ROS C++/Python/Matlab仿真)
FRIGIDWINTER的博客
04-29 1万+
Hybrid A*算法通过结合连续状态空间搜索、运动约束考虑和轨迹优化等技术，弥补了普通A*算法在高维连续空间和的不足，提高了路径规划的效率和鲁棒性，广泛应用于车规级路径规划。本文详细图解Hybrid A*算法的原理——启发式、维诺耗散项、节点扩展法则等，并给出ROS C++、python、matlab三种语言的仿真实现加深理解
RRT*（Rapidly-exploring Random Trees Star）算法 定义+特性+原理+公式+Python示例代码（带详细注释）
qq_51929160的博客
04-11 5678
RRT*算法是一种基于树结构的路径搜索算法，用于在连续空间中快速搜索最优路径。通过随机地扩展搜索树并在节点间进行连线，RRT* 能够在给定的搜索次数内发现一条连接起点和终点的路径。其基本原理是以随机策略生成节点并通过连接最近邻节点的方式逐步扩展搜索树，同时利用启发式搜索策略来优化已有路径，从而有效地探索空间并获得最优路径。RRT* 算法在无人车导航、机器人路径规划等领域有着广泛的应用。
手撕迪杰斯特拉算法（Dijkstra）及代码（附图解）
qq_63786973的博客
06-10 4087
引言：我们常常纠结一个对路径选择的决策问题，假设我们要从北京到上海，那么如何才能走 花最少的钱，又最节省时间的线路呢？​ 这时候，我们可以把从 北京 到 上海 间的路线站标记，那么 北京 到 各路线站 都会有相应的金钱和时间花费，我们只需要找出一条从北京到上海所经过的路线站的时间和金钱总值消耗最少的即可。​ 显而易见，对应到图中，就是一张带权的图，即 一张 网。我们只需要找出 起点 到 终点 权值之和最少的路径即可。即target=Min(∑beginendweight)i target=Min{ (\
Lectcode手撕算法（C++）
zhangqin__的博客
08-25 465
Lectcode手撕算法 两数之和 删除数组中的重复项 移除元素 整数反转 回文数 最长公共前缀 合并两个有序链表 有效括号 lectcode-1:两数之和： 题目：https://leetcode-cn.com/problems/two-sum/ 解答： class Solution { public: vector<int> twoSum(vector<int&g...
手撕Dijkstra算法
weixin_42393947的博客
09-06 176
c++ RRT 算法实现
小硕算法工程师
01-15 181
【代码】c++ RRT 算法实现。
手撕最短路径——Dijkstra算法模板(c++最简单）
NZR的blog
04-20 266
最短路径——Dijkstra算法模板 #include <iostream> using namespace std; int Graph[100][100]; int ver_num,edge_num; int dist[100]; void CreateGraph() { for(int i=0;i<edge_num;i++) { int ver1,ver2,weight; cin>>ver1>>ver2>>weight; Graph
基于matlab实现的RRT算法、双向RRT算法、A*算法、PRM、模糊路径规划算法、遗传算法路径规划
12-31
4. **A*算法**：A*是一种启发式搜索算法，它结合了Dijkstra算法和优先级队列。A*使用一个估价函数（通常是欧几里得距离加上一个障碍物权重）来指导搜索，确保找到从起点到终点的最短路径。A*算法非常有效，但需要...
Dijkstra和A*算法
qq_45063488的博客
06-25 1012
（2）当弹出的节点为n时，在拓展时，对于n节点所有的邻居节点m，如果m节点未被拓展，则检查m节点的cost g(m)，g(m)可不可以通过n进行下降，可以理解为m自身的cost为g(m)，如果设定为n走向m时可以将m整体的cost降低，则把cost更新。对于一个高度结构化的地图，有结构化移动的规则，则最短路径是可以计算的，可以理解为一个蚂蚁爬行最短路线问题，所以在结构化的地图中，可以直接计算出h*(n)作为启发式函数。上图中，从整体g+h角度s直接到g的代价为5，上边代价为10，结果与真实路径代价相反。
A*算法学习（python代码实现）
05-17
A*算法是一种在图形搜索中用于寻找从起点到终点最短路径的有效算法，它结合了Dijkstra算法的全局最优性和Greedy最佳优先搜索算法的效率。A*算法的核心在于引入了启发式函数，使得搜索更加高效，能够在大量可能的路径...
"基于A*算法与Dijkstra算法的路径规划仿真对比：MATLAB代码实现及运行结果分析",基于Dijkstra算法与A*算法的路径规划仿真：算法性能对比与MATLAB代码实现,基于A*算法 、 D
最新发布
02-13
"基于A*算法与Dijkstra算法的路径规划仿真对比：MATLAB代码实现及运行结果分析",基于Dijkstra算法与A*算法的路径规划仿真：算法性能对比与MATLAB代码实现,基于A*算法 、 Dijkstr算法对比的路径规划仿真 路径规划算法...
RRT.Python-master(2).zip
05-19
python 版本的路径规划算法仿真，rrt算法
RRT*算法介绍
OLAFM的博客
08-30 2486
https://blog.youkuaiyun.com/weixin_43795921/article/details/88557317 https://blog.youkuaiyun.com/yuxuan20062007/article/details/88843690 https://zhuanlan.zhihu.com/p/51087819（最详细的）
基于采样的RRT/RRT*/RRT_connect算法笔记及C++实现
weixin_37705221的博客
10-14 8372
本文记录常见的基于采样的RRT算法及相关改进算法（RRT*，RRT_connect）的原理和代码实现效果。与上一章介绍A*算法的文章不同，本文会先给出几种算法之间的效果对比，先有个直观的感受，再介绍其内在算法差异。 1. RRT/RRT*/RRT_connect算法效果对比 表1.1 简单地图路径搜索对比 算法 采样次数 实现效果 RRT 3198 RRT* 3570 RRT_connect 214 RRT*_connect 195 表1.2 复杂地图路径搜索对比（最
RRT与RRT*算法具体步骤与程序详解（python）
热门推荐
weixin_42875283的博客
04-29 4万+
提示：前面写了A*、D 文章目录前言一、RRT的原理与步骤二、RRT算法编写的步骤2.读入数据总结 前言 RRT和RRT*的区别： RRT的中文名为快速随机探索树，它的原理很简单，实际上就是维护一棵路径树：从起点开始，在空间中随机采样，并找到路径树上与采样点最接近且能与它无障碍地连接的点，连接这个点与采样点，将采样点加入路径树，直至终点附近区域被探索到。这种方式无法保证得到的路径是最优的。 RRT* 在RRT基础上做了改进，主要是进行了重新选择父节点和重布线的操作。试想在RRT中，我们的采样点最终与整.
RRT* 算法研究（附 MATLAB 和 Python 实现）
Solititude的博客
06-30 1万+
简要介绍 RRT* 算法的原理，并给出 MATLAB 和 Python 两种语言的实现
RRT*算法的原理简介以及Python实现代码
Jerry_hello的博客
04-20 8741
![RRT算法原理图](https://img-blog.csdnimg.cn/20210420101155956.png?x-oss-p RRT大致流程 1.初始化随机树tree，以空的随机树开始添加节点，最开始只有Qinit。 2.执行sample函数，在地图中获得一个随机点Qrand。 3.遍历tree中所有节点，找出与Qrand之间代价最小的点Qnearest。 4.执行extend函数，获得Qnearest向Qrand方向上的指定长度的扩展点Qnew。并对Qnew进行碰撞检测，若碰撞检测为真，则结
【基础学习】Dijkstra算法
weixin_45058446的博客
04-24 259
【基础学习】Dijkstra算法
Python实现-RRT-Rapidly-exploring Random Tree-快速搜索随机树
海晨威
08-23 5633
RRT是Rapidly-exploring Random Tree的简写，是基于随机采样的一种路径规划方法，它能够快速地搜索整个状态空间，并偏向于还未探索的空间。 主体部分是节点扩展过程： 前提：已知起点、终点和地图。 步骤：首先，在空间中随机采样一个节点qrandqrandq_{rand}，然后在当前的树中找到距离该随机点最近的节点qnearqnearq_{near}，以固定的步长...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值