如何通过python进行编写RRT（快速随机生成树）算法（路径规划问题）？

最新推荐文章于 2025-03-18 08:33:12 发布

置顶

Metroplitan

最新推荐文章于 2025-03-18 08:33:12 发布

阅读量5k

点赞数 9

分类专栏： python python算法文章标签： python RRT 路径规划自动路径规划快速随机生成树

本文链接：https://blog.youkuaiyun.com/qq_41883755/article/details/87948579

版权

RRT

RRT，快速随机生成树，顾名思义，是一种路径规划问题。
假如，我们要设计一个自动规划路径的机器人，适用场景在室内。但是，在室内有很多家具的阻碍，我们需要机器人避开阻碍到达终点。现在，我们可以进行编写程序了。

第一步，我们设定起点和终点，以及每次随机路径的步长，并导入一些python实现RRT算法的一些库。

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

start_x= 0
start_y= 0
goal_x=100
goal_y=-2


max_size=1000
truepath=[]
#initial the start and the goal
px=start_x
py=start_y

foot_length=10
i=0
x=[0]
y=[0]

第二步，我们生成随机点，然后进行判断新生成的路劲是否在障碍物上，如果符合条件，则存放在队列中。

for j in range(1000000):
    num1=random.random()
    num2=random.random()
    random_x=120 * num1
    random_y=5 * (-0.5 + num2)
    
    if random_x>px:
        new_x=px+(random_x-px)/foot_length
        new_y=py+(random_y-py)/foot_length
        #75<x80  1<y<2