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

Python RRT源代码

# -*- coding: utf-8 -*-"""Spyder EditorThis is a temporary script file."""import randomimport mathimport turtle as tt.speed(10000)#initial the map    500*500 map +-250t.pu()t.goto(-250,250)t.pd()t.fd(500)for i in range(3):    t.right(90)    t.fd(500)#initial the obstacle  x1     -250
   
    <-100    25
    
     <75t.begin_fill()t.color("black")t.pu()t.goto(-250,75)t.pd()t.right(90)t.fd(150)t.right(90)t.fd(50)t.right(90)t.fd(150)t.right(90)t.fd(50)t.end_fill()#initial the obstacle   x2     -150
     
      <-50   -250
      
       <-50t.begin_fill()t.color("black")t.pu()t.goto(-150,-50)t.pd()t.right(90)t.fd(100)t.right(90)t.fd(200)t.right(90)t.fd(100)t.right(90)t.fd(200)t.end_fill()#initial the obstacle    x3     0
       
        <250   0
        
         <50t.begin_fill() t.color("black")t.pu()t.goto(0,50)t.pd()t.right(90)t.fd(250)t.right(90)t.fd(50)t.right(90)t.fd(250)t.right(90)t.fd(50)t.end_fill()#initial the start and the goalstart_x = -220start_y = 220goal_x = -220goal_y = -220t.pu()t.goto(start_x,start_y)t.pd()t.circle(2)t.pu()t.goto(goal_x,goal_y)t.pd()t.circle(3)tree=[]foot_length=10tree.append((start_x,start_y))def nearest(new_x,new_y): i=0 Min=math.sqrt((tree[0][0]-new_x)**2+(tree[0][1]-new_y)**2) for j in range(len(tree)): distance=math.sqrt((tree[j][0]-new_x)**2+(tree[j][1]-new_y)**2) if tree[j][0]==new_x and tree[j][1]==new_y: break if distance<=Min and distance!=0: Min=distance i=j return tree[i]px=start_xpy=start_ynew_x=start_xnew_y=start_ywhile math.sqrt((new_x-goal_x)**2+(new_y-goal_y)**2)>=10: if random.random()<0.2: random_x=goal_x random_y=goal_y else: num1=random.random() num2=random.random() random_x=500 * (-0.5 + num1) random_y=500 * (-0.5 + num2) node_x,node_y = nearest(random_x,random_y) new_x=node_x+(random_x-node_x)/foot_length new_y=node_y+(random_y-node_y)/foot_length if 1: if new_x>-250 and new_x<-100 and new_y<75 and new_y>25: flag_1=1 else: flag_1=0 if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250: flag_11=1 else: flag_11=0 if new_x>0 and new_x<250 and new_y<50 and new_y>0: flag_111=1 else: flag_111=0 if flag_1==0 and flag_11==0 and flag_111==0: tree.append((new_x,new_y)) t.pu() t.goto(node_x,node_y) t.pd() t.goto(new_x,new_y) t.circle(1)final_path = []node_x,node_y = goal_x,goal_ywhile True: node_x,node_y=nearest(node_x,node_y) t.begin_fill() t.goto(node_x,node_y) final_path.append((node_x,node_y)) t.color("red") t.end_fill() if node_x == start_x and node_y == start_y: break;print(final_path)
        
       
      
     
    
   

最终输出结果如下图

RRT connect 算法源代码

# -*- coding: utf-8 -*-"""Created on Sun Jan 19 20:27:02 2020@author: 1918358"""# -*- coding: utf-8 -*-"""Spyder EditorThis is a temporary script file."""import randomimport mathimport turtle as timport datetimestarttime = datetime.datetime.now()t.speed(10000)#initial the map    500*500 map +-250t.pu()t.goto(-250,250)t.pd()t.fd(500)for i in range(3):    t.right(90)    t.fd(500)#initial the obstacle  x1     -250
   
    <-100    25
    
     <75t.begin_fill()t.color("black")t.pu()t.goto(-250,75)t.pd()t.right(90)t.fd(150)t.right(90)t.fd(50)t.right(90)t.fd(150)t.right(90)t.fd(50)t.end_fill()#initial the obstacle   x2     -150
     
      <-50   -250
      
       <-50t.begin_fill()t.color("black")t.pu()t.goto(-150,-50)t.pd()t.right(90)t.fd(100)t.right(90)t.fd(200)t.right(90)t.fd(100)t.right(90)t.fd(200)t.end_fill()#initial the obstacle    x3     0
       
        <250   0
        
         <50t.begin_fill() t.color("black")t.pu()t.goto(0,50)t.pd()t.right(90)t.fd(250)t.right(90)t.fd(50)t.right(90)t.fd(250)t.right(90)t.fd(50)t.end_fill()#initial the start and the goalstart_x = -220start_y = 220goal_x = 220goal_y = -220t.pu()t.goto(start_x,start_y)t.pd()t.circle(2)t.pu()t.goto(goal_x,goal_y)t.pd()t.circle(3)tree1=[]tree2=[]foot_length=15tree1.append((start_x,start_y))tree2.append((goal_x,goal_y))def nearest(new_x,new_y,tree): i=0 Min=math.sqrt((tree[0][0]-new_x)**2+(tree[0][1]-new_y)**2) for j in range(len(tree)): distance=math.sqrt((tree[j][0]-new_x)**2+(tree[j][1]-new_y)**2) if tree[j][0]==new_x and tree[j][1]==new_y: break if distance<=Min and distance!=0: Min=distance i=j return tree[i]px=start_xpy=start_ynew_x=start_xnew_y=start_ynew2_x = goal_xnew2_y = goal_yTree = []x2,y2 = nearest(new_x,new_y,tree2)x1,y1 = nearest(new2_x,new2_y,tree1)while math.sqrt((new_x-x2)**2+(new_y-y2)**2)>=25 or math.sqrt((new2_x-x1)**2+(new2_y-y1)**2)>=25: if random.random()<0.1: random_x=new2_x random_y=new2_y else: num1=random.random() num2=random.random() random_x=500 * (-0.5 + num1) random_y=500 * (-0.5 + num2) node_x,node_y = nearest(random_x,random_y,tree1) new_x=node_x+(random_x-node_x)/foot_length new_y=node_y+(random_y-node_y)/foot_length if 1: if new_x>-250 and new_x<-100 and new_y<75 and new_y>25: flag_1=1 else: flag_1=0 if new_x>-150 and new_x<-50 and new_y<-50 and new_y>-250: flag_11=1 else: flag_11=0 if new_x>0 and new_x<250 and new_y<50 and new_y>0: flag_111=1 else: flag_111=0 if flag_1==0 and flag_11==0 and flag_111==0: tree1.append((new_x,new_y)) t.pu() t.goto(node_x,node_y) t.pd() t.goto(new_x,new_y) t.circle(1) if random.random()<0.1: random2_x=new_x random2_y=new_y else: num11=random.random() num22=random.random() random2_x=500 * (-0.5 + num11) random2_y=500 * (-0.5 + num22) node2_x,node2_y = nearest(random2_x,random2_y,tree2) new2_x=node2_x+(random2_x-node2_x)/foot_length new2_y=node2_y+(random2_y-node2_y)/foot_length if 1: if new2_x>-250 and new2_x<-100 and new2_y<75 and new2_y>25: flag_1=1 else: flag_1=0 if new2_x>-150 and new2_x<-50 and new2_y<-50 and new2_y>-250: flag_11=1 else: flag_11=0 if new2_x>0 and new2_x<250 and new2_y<50 and new2_y>0: flag_111=1 else: flag_111=0 if flag_1==0 and flag_11==0 and flag_111==0: tree2.append((new2_x,new2_y)) t.pu() t.goto(node2_x,node2_y) t.pd() t.goto(new2_x,new2_y) t.circle(1) x2,y2 = nearest(new_x,new_y,tree2) x1,y1 = nearest(new2_x,new2_y,tree1) #tree2 = tree2[::-1]Tree = tree1 + tree2node_x,node_y = new2_x,new2_ynode2_x,node2_y = new_x,new_ywhile True: node_x,node_y=nearest(node_x,node_y,tree1) t.begin_fill() t.goto(node_x,node_y) t.color("red") t.end_fill() if node_x == start_x and node_y == start_y: breakt.pu()t.goto(new_x,new_y)t.pd()t.begin_fill()t.goto(new2_x,new2_y)t.color("red")t.end_fill()while True: node2_x,node2_y=nearest(node2_x,node2_y,tree2) t.begin_fill() t.goto(node2_x,node2_y) t.color("red") t.end_fill() if node2_x == goal_x and node2_y == goal_y: break endtime = datetime.datetime.now()print (endtime - starttime).seconds
        
       
      
     
    
   

结果如下：

这个阶段的学习存档，请大家互相批评指正，共同学习！

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