使用python实现路径规划算法_python 二维路径规划

使用python实现路径规划算法

本文给出了使用python完成的常用路径规划算法，语法简洁，体现了Python的特点（基于Python 3.6）。仅限交流与学习使用！！

##A star algorithm

import math

def heuristic_distace(Neighbour_node,target_node):
    H = abs(Neighbour_node[0] - target_node[0]) + abs(Neighbour_node[1] - target_node[1])
    return H

def go_around(direction):
    box_length = 1
    diagonal_line = box_length * 1.4
    if (direction==0 or direction==2 or direction==6 or direction==8):
        return diagonal_line
    elif (direction==1 or direction==3 or direction==4 or direction==5 or direction==7):
        return diagonal_line

def find_coordinate(map,symble):
    #store coordinate
    result=[]
    for index1,value1 in enumerate(map):
        if symble in value1:
            row = index1
            for index2, value2 in enumerate(map[index1]):
                if symble==value2:
                   column = index2
                   result.append([row, column])
    return result

map =[[".", ".", ".", "#", ".", "#", ".", ".", ".", "."],
      [".", ".", "#", ".", ".", "#", ".", "#", ".", "#"],
      ["s", ".", "#", ".", "#", ".", "#", ".", ".", "."],
      [".", "#", "#", ".", ".", ".", ".", ".", "#", "."],
      [".", ".", ".", ".", "#", "#", ".", ".", "#", "."],
      [".", "#", ".", ".", ".", ".", "#", ".", ".", "."],
      [".", "#", ".", ".", ".", "#", "#", ".", "#", "."],
      [".", ".", ".", ".", ".", ".", ".", ".", "#", "."],
      [".", "#", "#", ".", ".", ".", "#", ".", ".", "."],
      [".", ".", ".", "#", "#", "#", ".", ".", "#", "f"],
      ["#", "#", ".", ".", "#", "#", "#", ".", "#", "."],
      [".", "#", "#", ".", ".", ".", "#", ".", ".", "."],
      [".", ".", ".", ".", "#", "#", ".", ".", "#", "."]]

#these datas are store in the form of list in a singal list

obstacle = find_coordinate(map,"#")
start_node = find_coordinate(map,"s")[0]
target_node = find_coordinate(map,"f")[0]
current_node = start_node
path_vertices = [start_node]
#visited_vertices should be stored in the form of a singal list
Neighbour_vertices = []

while current_node != target_node:

    x_coordinate = current_node[0]
    y_coordinate = current_node[1]
    F = []
    Neighbour_vertices =   [[x_coordinate - 1, y_coordinate - 1],
                            [x_coordinate - 1, y_coordinate    ],
                            [x_coordinate - 1, y_coordinate + 1],
                            [x_coordinate,     y_coordinate - 1],
                            [x_coordinate    , y_coordinate    ],
                            [x_coordinate,     y_coordinate + 1],
                            [x_coordinate + 1, y_coordinate - 1],
                            [x_coordinate + 1, y_coordinate    ],
                            [x_coordinate + 1, y_coordinate + 1]]

    for index, value in enumerate(Neighbour_vertices):
        if value[0] in range(len(map)):
            if value[1] in range(len(map)):
               if value not in obstacle+path_vertices:
                    F.append(heuristic_distace(value, target_node) + go_around(index))
               else:
                    F.append(10000)
            else:
                    F.append(10000)
        else:
                    F.append(10000)
               #a very large number
    print(F)
    current_node=Neighbour_vertices[F.index(min(total_distance for total_distance in F))]
    print(current_node)

    path_vertices.append(current_node)
      # if current_node not in visited_vertices:
      #     visited_vertices.append(current_node)
      # else:
      #     print("there is no route between")
      #     break

print(path_vertices)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89

Dijkstra algorithm

import numpy

weigh_graph = [[10000, 2, 4, 5],
                 [2,10000, 7, 8],
                 [4, 7, 10000, 4],
                 [5, 8, 4,10000]]
# weigh_graph = numpy.array(a + numpy.transpose(a))
source_node = 0
target_node = 3

vertices=set(range(len(weigh_graph)))

path=[]
for j in vertices:
  path.append(0 if j==source_node else float("inf"))

current_node=source_node
visited_node=[]
unvisited_node=vertices
orders=[source_node]

while target_node in unvisited_node:

    for j in unvisited_node:
        if path[j] < path[current_node]+weigh_graph[current_node][j]:
             path[j] =path[j]
        else:
             path[j] = path[current_node]+weigh_graph[current_node][j]

    unvisited_node.discard(current_node)
    print(unvisited_node)

    for index,value in enumerate(weigh_graph[current_node]):
        if index in unvisited_node:
            if value==min(weigh_graph[current_node][j] for j in unvisited_node):
                 print(min(weigh_graph[current_node][j] for j in unvisited_node))
                 current_node = index

    # current_node=list(weigh_graph[current_node]).index(min(weigh_graph[current_node][j] for j in unvisited_node))
    #这样找索引会出现索引第一个的情况，但是需要确定所引导的位置并没有被作为顶点

    print(current_node)
    orders.append(current_node)
    if current_node==target_node:
        break

print(orders)
print(path)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

Rapid random exploring tree

# -*- coding:utf-8 -*-
import random
import numpy
import scipy.io

# locate where the nearest vertices is for the randomly choosen point
def min_ocilide_distace(random_vertice,vertice_set):
    distance_set = [numpy.linalg.norm([j - k for j, k in zip(m, n)])
                  for m, n in zip(vertice_set, len(vertice_set) * [random_vertice])]
    result = distance_set.index(numpy.min(distance_set))   # ？？这里是否存在先后顺序？？？
    return result

def find_path(path,vertice_number,vertice_set):
    which_number=vertice_number
    vertice_order=[vertice_number]
    trajectory_order = []
    cishu=0
    while which_number!=0:  #0represent the start
        cishu+=1
        which_number=path[which_number-1][0]
        vertice_order.append(which_number)
    vertice_order=reversed(vertice_order)
    trajectory_order.append(vertice_set[i] for i in vertice_order)
    return trajectory_order

# load map, free space o ; obstacle space 1
def rrt(start,end,height,width,data1):
    integrated_space=[]
    free_space=[]
    obstacle_space=[]

    for i in range(0, height):
        for j in range(0, width):
            integrated_space.append([i,j])
            if data1[i][j] == 0:
                free_space.append([i,j])
            if data1[i][j] == 1:
                obstacle_space.append([i,j])

    random_cishu=0
    max_random_cishu=2000
    chazhishu=40
    initial_vertice=[round(q) for q in start]
    target_vertive=[round(q) for q in end]
    vertice_set=[initial_vertice]
    radius=40
    next_vertice=[]
    vertice_number=0
    path=[]
    distance_set=[]

    while next_vertice!=target_vertive:
    #produce random point,vertify it and the find a ner_vertices，at the same time, add to a new_path
        random_cishu+=1
        print(random_cishu)
        if random_cishu>max_random_cishu:
            print("soory!sir!path not found, because not enough random point ")
            break
        y=random.randint(0, height-1)
        x=random.randint(0, width-1)
        random_veritce = [y, x] #note here!! not [x,y]!!
        near_vertice=vertice_set[min_ocilide_distace(random_veritce,vertice_set)]
    # find a new vertice
    # a very important step is mommited here !! F-word
    # that random_vertice choosen (random or target) should be decided!!
        if random.random() < 0.4:
            random_veritce = target_vertive
            # radius = numpy.linalg.norm(numpy.array([j - k for j, k in zip(random_veritce, near_vertice)]))
        near2next_vector=[i - j for i,j in zip(random_veritce,near_vertice)]
        #to get norm2 , first transform the list to array,using numpy.array. and the original list remains unchanged
        standard_vector=[radius*i/numpy.linalg.norm(numpy.array(near2next_vector), ord=2)
                         for i in near2next_vector]
        if [round(p) for p in [i+j for i, j in zip(next_vertice,standard_vector)]] not in integrated_space:
            standard_vector=near2next_vector
        #inner-point collision detection
        #first step, all the points on the edge are in free space
        chazhicishu=0
        distance=0
        while chazhicishu != chazhishu:
            chazhicishu += 1
            inner_point = [round(q) for q in [chazhicishu / chazhishu * j + k for j, k in zip(standard_vector, near_vertice)]]
            if inner_point in obstacle_space:
                if chazhicishu == 1:
                    break
                else:
                    print('sorry, inner point in obstacle!')
                    next_vertice = next_vertice #using the former ninner point as the next_vetice
                    distance = distance #using the former ninner point as the next_vetice
                    vertice_number = vertice_number + 1
                    path.append([min_ocilide_distace(random_veritce, vertice_set), vertice_number])
                    vertice_set.append(next_vertice)
                    distance_set.append(distance)
                    break
            else:
                next_vertice = inner_point
                distance = radius * chazhicishu / chazhishu
                if next_vertice == target_vertive:
                    print('congratulations！sir，path been found')
                    vertice_number = vertice_number + 1
                    path.append([min_ocilide_distace(random_veritce, vertice_set), vertice_number])
                    vertice_set.append(next_vertice)
                    distance_set.append(distance)
                    break
                else:
                    if chazhicishu < chazhishu:
                        continue
                    if chazhicishu == chazhishu:
                        print('good choice of random vertice, go go go!')
                        vertice_number = vertice_number + 1
                        path.append([min_ocilide_distace(random_veritce, vertice_set), vertice_number])
                        vertice_set.append(next_vertice)
                        distance_set.append(distance)
                        # print(vertice_set)

    return find_path(path,vertice_number,vertice_set)


def main():
  data = scipy.io.loadmat('map.mat')  # 读取mat文件，这他么是一个字典数据结构！！！
  # print(data.keys()) 查看mat文件中的所有键!得到 dict_keys(['__header__', '__version__', '__globals__', 'map'])
  data1 = data['map']
  height = len(data1)#683
  width = len(data1[0])#803
  start=[70, 80]
  end=[399, 607]
  print('the route is :' +
         str(rrt(start,end,height,width,data1)))

if __name__ == '__main__':
    main()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130