二维地图的全局状态更新路径规划
一、 概述
此次仍然使用A*算法进行路径规划，通过K210进行实时路径规划，所以本次将路径规划算法使用python进行面向对象开发，因为使用python数字计算更加方便，考虑使用人工势场法进行动态路径规划，初期先使用A*进行循环的静态规划。
中心思想与之前的C++版本一样，仍通过对启发值（描述当前点到终点距离的预估代价和当前代价）的运用。
OpenList和CloseList表示已探索区域边界和已探索区域，从OpenList中寻找下一个探索的点，探索完毕保存到CloseList。当终点在CloseList中被保存，则路径已经被找到，通过对CloseList进行回溯找到路径，注意找不到路径的结束条件。
二、 实现
1. 核心层：负责通过k210摄像头获取的图像获得地图进行地图的更新和绘制、加载和保存，以及小车/无人机移动导致的起点变化或状态更新。
2. 算法层：复制A*+人工势场算法的实现
1. 核心层
（1） 地图：
1） 目前进行二维的路径规划，所以利用一个二维数组表示地图（无人机开发后续需要调整维度）。
2） 为了方便地图加载，选择将二维数组保存为csv文件序列化到磁盘上，加载地图再从磁盘上反序列化csv文件到内存从而读取地图。
3） 设置障碍物，针对地图中不同状态的区域使用不同的数值表示当前区域状态。
4） 使用光栅图（基本构成单位为像素）绘制地图（python中的Matplotlib、Seaborn、Plotly这类光栅图绘图库）
（2） 地图动态加载：
地图的绘制加载非常消耗资源，大大减少算法速度，所以进行地图记录，只对变化的节点进行绘制。
2. 算法实现层
负责实现A*算法，实现算法层和核心层耦合，二维地图状态由核心层提供，在算法实现层只需要调用核心层提供的接口进行地图状态修改就可以。
三、 算法实现层的实现
算法由构建的Astar类完成，初始化成员为地图大小默认为30*30，将地图初始化全为无穷大（不可通行）
def __init__(self, mapsize: Tuple[int, int] = (30, 30)):
# 初始化地图
self.mapsize = mapsize
self.map = np.full(mapsize, n)
使用八方向遍历节点，x和y都为[-1，0，1]，并检查是否可通行
def _get_neighbor(self, point: Tuple[int, int]):
x, y = point
neighbor = []
for i in range(-1, 2):
for j in range(-1, 2):
if i == 0 and j == 0:
continue # 忽略当前点
dx, dy = x + i, y + j
if 0 <= dx < self.mapsize[0] and 0 <= dy < self.mapsize[1] and self.map[dx, dy] != np.inf:
neighbor.append((dx, dy))
return neighbor
定义欧式距离和曼哈顿距离两种距离计算方式
@staticmethod
def manhattan_distance(p1: Tuple[int, int], p2: Tuple[int, int]) -> int:
x1, y1 = p1
x2, y2 = p2
return abs(x2 - x1) + abs(y2 - y1)
@staticmethod
def euler_distance(p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
x1, y1 = p1
x2, y2 = p2
dx = x2 - x1
dy = y2 - y1
return np.sqrt(dx ** 2 + dy ** 2)
计算两点之间的代价
def _get_cost(self, point1: Tuple[int, int], point2: Tuple[int, int]):
return self.euler_distance(point1, point2)
寻找路径，八方向遍历节点入队，利用优先队列每次将代价最小的节点进行出队，将这些节点进行记录，直到找到终点，最后从终点反向找到起点然后逆置返回路径信息。
def find_path(self, start_point: Tuple[int, int], goal_point: Tuple[int, int]) -> List[Tuple[int, int]]:
# 执行A*搜索，返回找到的路径
if_can_find_goal = False
frontier = queue.PriorityQueue()
frontier.put((0, start_point))
came_from: Dict[Tuple[int, int], Union[Tuple[int, int], None]] = {start_point: None}
cost_so_far: Dict[Tuple[int, int], float] = {start_point: 0}
while not frontier.empty():
current_p = frontier.get()[1]
if current_p == goal_point:
if_can_find_goal = True
break
for next_p in self._get_neighbor(current_p):
new_cost = cost_so_far[current_p] + self._get_cost(next_p, current_p)
if next_p not in cost_so_far.keys() or new_cost < cost_so_far[next_p]:
cost_so_far[next_p] = new_cost
priority = new_cost + self._get_cost(next_p, goal_point)
frontier.put((priority, next_p))
came_from[next_p] = current_p
if not if_can_find_goal:
return [(n, n)]
path = []
current_p = goal_point
while current_p != start_point:
path.append(current_p)
current_p = came_from[current_p]
path.append(start_point)
path.reverse()
return path
问题1：在实际k210获取地图时，地图过大超出k210内存限制，考虑的解决办法：跳过部分节点，考虑每五个像素点取一个地图值
问题2：在k210使用A*算法类的时候，因为使用MicroPython环境开发，因为资源有限并没有使用python的完整环境和完整的库，多边形-线段交叉计算的科学计算没有办法计算，考虑使用自定义函数计算障碍物的大致碰撞体积。
问题3：k210没有queue库，无法利用优先结构体队列，本质上无法运算A*代码。考虑使用异步I/O的uasyncio模块，创建了一个PriorityItem类表示队列中的元素。生产者线程产生一些带有不同优先级的数据，并将其放入队列。消费者线程从队列中获取并处理数据。但是出现了新的问题。
问题4：优先结构体队列实现是基于线程实现的，并非协程，在MicroPython的异步支持有限，优先结构体队列并不被支持。
