本文共 2830 字，大约阅读时间需要 9 分钟。

RRT

RRT，快速随机生成树，顾名思义，是一种路径规划问题。

假如，我们要设计一个自动规划路径的机器人，适用场景在室内。但是，在室内有很多家具的阻碍，我们需要机器人避开阻碍到达终点。现在，我们可以进行编写程序了。

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

import numpy as npimport matplotlib.pyplot as pltimport randomimport mathstart_x= 0start_y= 0goal_x=100goal_y=-2max_size=1000truepath=[]#initial the start and the goalpx=start_xpy=start_yfoot_length=10i=0x=[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
   
    50 and new_x<55 and new_y<2 and new_y>0.5:            flag_1=1        else:            flag_1=0        #35
    
     <40 1
     
      <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
      
       <40    -1
       
        <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算法还需要通过现代算法进行优化，笔者能力有限，希望大家一起学习。

转载地址：http://ioprn.baihongyu.com/