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

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
        if new_x>50 and new_x<55 and new_y<2 and new_y>0.5:
            flag_1=1
        else:
            flag_1=0
        #35<x<40 1<y<2
        if new_x>20 and new_x<25 and new_y<2 and new_y>0.5:
            flag_11=1
        else:
            flag_11=0
        #35<x<40    -1<y<0
        if new_x>20 and new_x<25 and new_y<-0.5 and new_y>-2:
            flag_111=1
        else:
            flag_111=0
        #
        if new_x>50 and new_x<55 and new_y<-0.5 and new_y>-2:
            flag_1111=1
        else:
            flag_1111=0
        distance_c=math.sqrt((goal_x-px)**2+(goal_y-py)**2)
        distance_a=math.sqrt((goal_x-new_x)**2+(goal_y-new_y)**2)
        if distance_c>distance_a:#last point > new point
            flag_2=1
        else:
            flag_2=0
        if flag_1==0 and flag_2==1 and flag_11==0 and flag_111==0 and flag_1111==0:
            x.append(new_x)
            y.append(new_y)
            px=new_x
            py=new_y

第三步，绘制图形部分，绘制障碍物以及机器人所要行走的路径。

qiang1_x=[]
qiang1_y=[]
qiang2_x=[]
qiang2_y=[]
#障碍物坐标
qiang3_x=[]
qiang3_y=[]
qiang4_x=[]
qiang4_y=[]

for i in range(100000):
   qiang1_x.append(random.randint(50,55))
   qiang1_y.append(0.5+2*random.random())
plt.scatter(qiang1_x,qiang1_y,s=10,color="blue")    

for i in range(100000):
   qiang2_x.append(random.randint(20,25))
   qiang2_y.append(0.5+2*random.random())   
plt.scatter(qiang2_x,qiang2_y,s=10,color="blue")   

for i in range(100000):
   qiang3_x.append(random.randint(50,55))
   qiang3_y.append(-0.5-2*random.random())
plt.scatter(qiang3_x,qiang3_y,s=10,color="blue")    

for i in range(100000):
   qiang4_x.append(random.randint(20,25))
   qiang4_y.append(-0.5-2*random.random())   
plt.scatter(qiang4_x,qiang4_y,s=10,color="blue")   
     
 
plt.scatter(start_x,start_y,s=100,color="black")
plt.scatter(goal_x,goal_y,s=100,color="black")
   

#RRT路径
plt.plot(x,y,color="red")
#plt.plot(x,y)#plot还有很多参数，可以查API修改，如颜色，虚线等
plt.title("");
plt.xlabel("x");
plt.ylabel("y");
plt.show()

然后可以得到如下机器人行走路径：

这里，我们发现机器人行走的路径较为稳定，符合我们RRT算法的需求。
但是，应用到实际，可能会有一些问题。
笔者改过输入的起点以及终点的坐标，发现变化情况很大
笔者将起点和终点分别更改后，得到如下图形

理论上是可行的，但是实际上，机器人是不可能贴着墙行走。
所以本RRT算法还需要通过现代算法进行优化，笔者能力有限，希望大家一起学习。