一文教你用python编写Dijkstra算法进行机器人路径规划

时间:2022-06-10 12:01:01

前言

为了机器人在寻路的过程中避障并且找到最短距离,我们需要使用一些算法进行路径规划(path planning),常用的算法有djikstra算法、a*算法等等,在github上有一个非常好的项目叫做pythonrobotics,其中给出了源代码,参考代码,可以对djikstra算法有更深的了解。

一、算法原理

一文教你用python编写Dijkstra算法进行机器人路径规划

如图所示,dijkstra算法要解决的是一个有向权重图中最短路径的寻找问题,图中红色节点1代表起始节点,蓝色节点6代表目标结点。箭头上的数字代表两个结点中的的距离,也就是模型中所谓的代价(cost)。

贪心算法需要设立两个集合,open_set(开集)和closed_set(闭集),然后根据以下程序进行操作:

  • 把初始结点放入到open_set中;
  • 把open_set中代价最小的节点取出来放入到closed_set中,并且作为当前节点;
  • 把与当前节点相邻的节点放入到open_set中,如果代价更小更新代价
  • 重复2-3过程,直到找到终点。

注意open_set中的代价是可变的,而closed_set中的代价已经是最小的代价了,这也是为什么叫做open和close的原因。

至于为什么closed_set中的代价是最小的,是因为我们使用了贪心算法,既然已经把节点加入到了close中,那么初始点到close节点中的距离就比到open中的距离小了,无论如何也不可能找到比它更小的了。

二、程序代码

python" id="highlighter_568394">
?
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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
"""
 
grid based dijkstra planning
 
author: atsushi sakai(@atsushi_twi)
 
"""
 
import matplotlib.pyplot as plt
import math
 
show_animation = true
 
 
class dijkstra:
 
    def __init__(self, ox, oy, resolution, robot_radius):
        """
        initialize map for a star planning
 
        ox: x position list of obstacles [m]
        oy: y position list of obstacles [m]
        resolution: grid resolution [m]
        rr: robot radius[m]
        """
 
        self.min_x = none
        self.min_y = none
        self.max_x = none
        self.max_y = none
        self.x_width = none
        self.y_width = none
        self.obstacle_map = none
 
        self.resolution = resolution
        self.robot_radius = robot_radius
        self.calc_obstacle_map(ox, oy)
        self.motion = self.get_motion_model()
 
    class node:
        def __init__(self, x, y, cost, parent_index):
            self.x = # index of grid
            self.y = # index of grid
            self.cost = cost
            self.parent_index = parent_index  # index of previous node
 
        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(
                self.cost) + "," + str(self.parent_index)
 
    def planning(self, sx, sy, gx, gy):
        """
        dijkstra path search
 
        input:
            s_x: start x position [m]
            s_y: start y position [m]
            gx: goal x position [m]
            gx: goal x position [m]
 
        output:
            rx: x position list of the final path
            ry: y position list of the final path
        """
 
        start_node = self.node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.node(self.calc_xy_index(gx, self.min_x),
                              self.calc_xy_index(gy, self.min_y), 0.0, -1)
 
        open_set, closed_set = dict(), dict()
        open_set[self.calc_index(start_node)] = start_node
 
        while 1:
            c_id = min(open_set, key=lambda o: open_set[o].cost)
            current = open_set[c_id]
 
            # show graph
            if show_animation:  # pragma: no cover
                plt.plot(self.calc_position(current.x, self.min_x),
                         self.calc_position(current.y, self.min_y), "xc")
                # for stopping simulation with the esc key.
                plt.gcf().canvas.mpl_connect(
                    'key_release_event',
                    lambda event: [exit(0) if event.key == 'escape' else none])
                if len(closed_set.keys()) % 10 == 0:
                    plt.pause(0.001)
 
            if current.x == goal_node.x and current.y == goal_node.y:
                print("find goal")
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break
 
            # remove the item from the open set
            del open_set[c_id]
 
            # add it to the closed set
            closed_set[c_id] = current
 
            # expand search grid based on motion model
            for move_x, move_y, move_cost in self.motion:
                node = self.node(current.x + move_x,
                                 current.y + move_y,
                                 current.cost + move_cost, c_id)
                n_id = self.calc_index(node)
 
                if n_id in closed_set:
                    continue
 
                if not self.verify_node(node):
                    continue
 
                if n_id not in open_set:
                    open_set[n_id] = node  # discover a new node
                else:
                    if open_set[n_id].cost >= node.cost:
                        # this path is the best until now. record it!
                        open_set[n_id] = node
 
        rx, ry = self.calc_final_path(goal_node, closed_set)
 
        return rx, ry
 
    def calc_final_path(self, goal_node, closed_set):
        # generate final course
        rx, ry = [self.calc_position(goal_node.x, self.min_x)], [
            self.calc_position(goal_node.y, self.min_y)]
        parent_index = goal_node.parent_index
        while parent_index != -1:
            n = closed_set[parent_index]
            rx.append(self.calc_position(n.x, self.min_x))
            ry.append(self.calc_position(n.y, self.min_y))
            parent_index = n.parent_index
 
        return rx, ry
 
    def calc_position(self, index, minp):
        pos = index * self.resolution + minp
        return pos
 
    def calc_xy_index(self, position, minp):
        return round((position - minp) / self.resolution)
 
    def calc_index(self, node):
        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
 
    def verify_node(self, node):
        px = self.calc_position(node.x, self.min_x)
        py = self.calc_position(node.y, self.min_y)
 
        if px < self.min_x:
            return false
        if py < self.min_y:
            return false
        if px >= self.max_x:
            return false
        if py >= self.max_y:
            return false
 
        if self.obstacle_map[node.x][node.y]:
            return false
 
        return true
 
    def calc_obstacle_map(self, ox, oy):
 
        self.min_x = round(min(ox))
        self.min_y = round(min(oy))
        self.max_x = round(max(ox))
        self.max_y = round(max(oy))
        print("min_x:", self.min_x)
        print("min_y:", self.min_y)
        print("max_x:", self.max_x)
        print("max_y:", self.max_y)
 
        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        print("x_width:", self.x_width)
        print("y_width:", self.y_width)
 
        # obstacle map generation
        self.obstacle_map = [[false for _ in range(self.y_width)]
                             for _ in range(self.x_width)]
        for ix in range(self.x_width):
            x = self.calc_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    d = math.hypot(iox - x, ioy - y)
                    if d <= self.robot_radius:
                        self.obstacle_map[ix][iy] = true
                        break
 
    @staticmethod
    def get_motion_model():
        # dx, dy, cost
        motion = [[1, 0, 1],
                  [0, 1, 1],
                  [-1, 0, 1],
                  [0, -1, 1],
                  [-1, -1, math.sqrt(2)],
                  [-1, 1, math.sqrt(2)],
                  [1, -1, math.sqrt(2)],
                  [1, 1, math.sqrt(2)]]
 
        return motion
 
 
def main():
    print(__file__ + " start!!")
 
    # start and goal position
    sx = -5.0  # [m]
    sy = -5.0  # [m]
    gx = 50.0  # [m]
    gy = 50.0  # [m]
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m]
 
    # set obstacle positions
    ox, oy = [], []
    for i in range(-10, 60):
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40):
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)
 
    if show_animation:  # pragma: no cover
        plt.plot(ox, oy, ".k")
        plt.plot(sx, sy, "og")
        plt.plot(gx, gy, "xb")
        plt.grid(true)
        plt.axis("equal")
 
    dijkstra = dijkstra(ox, oy, grid_size, robot_radius)
    rx, ry = dijkstra.planning(sx, sy, gx, gy)
 
    if show_animation:  # pragma: no cover
        plt.plot(rx, ry, "-r")
        plt.pause(0.01)
        plt.show()
 
 
if __name__ == '__main__':
    main()

三、运行结果

一文教你用python编写Dijkstra算法进行机器人路径规划

四、 a*算法:djikstra算法的改进

dijkstra算法实际上是贪心搜索算法,算法复杂度为o( n 2 n^2 n2),为了减少无效搜索的次数,我们可以增加一个启发式函数(heuristic),比如搜索点到终点目标的距离,在选择open_set元素的时候,我们将cost变成cost+heuristic,就可以给出搜索的方向性,这样就可以减少南辕北辙的情况。我们可以run一下pythonrobotics中的astar代码,得到以下结果:

一文教你用python编写Dijkstra算法进行机器人路径规划

总结

到此这篇关于python编写dijkstra算法进行机器人路径规划的文章就介绍到这了,更多相关python写dijkstra算法内容请搜索服务器之家以前的文章或继续浏览下面的相关文章希望大家以后多多支持服务器之家!

原文链接:https://blog.csdn.net/weixin_41855010/article/details/119358243