前言
由于本人这学期修了一门《智能工程》的课程,课程的大作业要求大家自主编写机器人导航程序,包括:路径规划、轨迹规划、控制器、机器人建模等相关程序,因此特在此写下一篇博客,记录开发过程。
这是第一篇博客,用于记录如何将自己编写的A星算法用于ROS中。
python编写A星算法
描述
先声明:在使用Python编写A*算法的过程中,深深的体会到了C++的链表、指针的便利性,但奈何已经1年多没使用C++写过程序了,所以还是用了Python来完成的。我编写的A星算法的时间复杂度和空间复杂度肯定都很高,希望大家看了轻喷。
由于要用在ROS的map中寻找路径,所以我把A星算法写成了一个类,只需要传进去三个参数,一个是二维的List(ROS中规定:可走区域的数值为0,障碍物数值为100,未知领域数值为-1),一个是起始点的坐标,一个是终点的坐标。
最后程序会返回一个List,里面的内容是从终点到起点的最短路径,用每一个点的坐标表示。
比如下面是一张地图:
0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | |
---|---|---|---|---|---|---|---|---|
0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
1 | 0 | 0 | 0 | 100 | 0 | 0 | 0 | 0 |
2 | 0 | 0 | 0 | 100 | 0 | 0 | 0 | 0 |
3 | 0 | 0 | 0 | 100 | 0 | 0 | 0 | 0 |
4 | 0 | 0 | 0 | 100 | 0 | 0 | 0 | 0 |
5 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
6 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
规定起点为[2,2],终点为[2,4],则返回的内容为:[(2,4),(1,4),(0,3),(1,2),(2,2)]
具体实现
对于地图中每一个像素点,如果这个像素点可到达,那就计算当前的G代价(self.cost_g),H代价(self.cost_h),F代价(self.cost_f)
这个类,相当于C++中的struct
class map_node():
def __init__(self):
# x、y代表该像素点在地图中的横纵坐标
self.x = 0
self.y = 0
# F = G + H
self.cost_f = 0
self.cost_g = 0
self.cost_h = 0
# 父节点的横纵坐标
self.parent = [0,0]
主要类:
class find_path():
类初始化函数:
-
extend_map函数是将地图扩展一圈,加一圈障碍物(像素值为1)
-
声明一个state_map用于保存map中每一个像素点的状态:
- 2代表已经在open表中
- 3代表已经在close表中
- 0代表还没有处理过
-
起点和终点自然横纵坐标都+=1
-
初始化一些变量 openlist、closelist
def __init__(self, map, start, goal):
# map是一个二维地图, start是起点坐标[],goal是终点坐标[]
self.map = self.extend_map(map)
# 2代表在open表中 3代表在close表中
self.state_map = np.zeros([len(map) + 2, len(map[0]) + 2])
# print self.map
self.start = start
self.start[0] += 1
self.start[1] += 1
self.goal = goal
self.goal[0] += 1
self.goal[1] += 1
self.open_list = []
self.cloase_list = []
self.path = []
self.if_reach = False
扩展地图边界的函数
def extend_map(self, map):
new_row = np.ones(len(map[0]))
new_col = np.ones(len(map) + 2)
x = np.insert(map, 0, new_row, axis=0)
x = np.insert(x, len(map) + 1, new_row, axis=0)
x = np.insert(x, 0 , new_col, axis=1)
x = np.insert(x, len(map[0]) + 1 , new_col, axis=1)
return x
主要的寻找路径的函数
- 如果设置的起始点和终点不可达,print 提示,然后直接退出
- append_around_open函数是把该点周围的8个点加到open表中
- 把起始点加到close表中
- 进入循环,利用find_min_cost_f每次寻找open表中最小的cost_f,把它周围的8个点加到open表中,然后把这个节点加到close表,直到找到了到终点的路径,然后利用append_path函数回溯父节点直到回溯到起点。
def start_find(self):
#第一次操作,把起点的周围的点指向起点,起点和周围的点加到open list,
# print "-----start point-----",self.start
if self.map[self.start[0]][self.start[1]] != 0:
print "\033[0;31m[E] : Please set the valid start point\033[0m"
print "value = ", self.map[self.start[0]][self.start[1]]
return "None"
if self.map[self