扫地机器人的模拟程序 (4)

寻路模块 (2)

通过一番寻找，发现这系列文章，其不仅包含A*算法，连寻路算法中的一些基础知识也一并介绍了，不愧是斯坦福出品，也很感谢译者
要实现点A到点B最短路径，还需要做一些微小的工作，下面逐个说明

计算曼哈顿距离的函数

目的是寻路，肯定需要一个方法来估算两点间距离，由于我在移动方式上限定了只有上下左右4个方向可动，那么很自然，两点间最短距离就是就是曼哈顿距离
代码:

import numpy as np
def get_manhanttan_distance(coordinate1, coordinate2):
    return np.abs(np.array(coordinate1) - np.array(coordinate2)).sum()

确定目的地

之前提到 “U型走法清扫一次，然后通过A*算法走到最近的未清洁点，如此反复直到所有点都被清洁/走过”
说起来容易，具体实现还几个步骤：

1. 找出未清洁的点的合集
2. 找出这些点中，离当前所在点最近的点
3. 如果其中有多个点到离当前所在点的距离一样，则需要按某个规则，在这些点中取一个出来

找出未清洁的点的合集

未清洁的点=未路过的点，我们现在手头上有coordinate_list，impassable_coordinate_list和robot.path_log，所以我们可以:

1. 在coordinate_list中去掉impassable_coordinate_list的那些点（差集）得到passable_coordinate_list
2. 在passable_coordinate_list中，中去掉robot.path_log的那些点（差集），得到uncleaned_coordinate_list

这里有需要做两次交集，本来打算直接用numpy中的setdiff1d函数，但因setdiff1d不支持多维(多维指(x,y)的形式)，参考了SO上大神的办法写一个（用了view来降维）

def multidim_diff(coordinate_list_1, coordinate_list_2):  # 在1不在2中的集合
    coordinate_list_1, coordinate_list_2 = np.array(coordinate_list_1), np.array(coordinate_list_2)
    arr1_view = coordinate_list_1.view([('', coordinate_list_1.dtype)] * coordinate_list_1.shape[1])
    arr2_view = coordinate_list_2.view([('', coordinate_list_2.dtype)] * coordinate_list_2.shape[1])
    intersected = np.setdiff1d(arr1_view, arr2_view)
    return np.ndarray.tolist(intersected)

然后就可以得到未清洁的点列表了

def get_uncleaned_coordinate_list(self):
    passable_coordinate_list = multidim_diff(self.coordinate_list, self.impassable_coordinate_list)
    uncleaned_coordinate_list = multidim_diff(passable_coordinate_list, self.path_log)
    return uncleaned_coordinate_list

找出最近的未清洁点

写完上面这段时候我突然想到，上面的曼哈顿距离没考虑障碍，如果未清洁点和当前所在点隔了一堵墙，反而会绕远路。但不用曼哈顿距离，对每个未清洁点求最短路径似乎也不太合适
再次思考，想到这个搜索动作是在U型走法后执行的，U型走法覆盖区域还是比较规律的，当前点到之前路过的区域，不至于很绕路，那么是不是可以找U型走法覆盖区域的周围的点，在这些点里再找最近的呢？
于是把上面的2和3改为:
2 . 找出所有走过的点，取其上下左右4个点到passed_by_coordinate_list
3 . 去重复，去掉非法点(和coordinate_list做交集)，找到未清洁的点(和uncleaned_coordinate_list做交集)，更新passed_by_coordinate_list
4 . 计算passed_by_coordinate_list中所有点到当前点的曼哈顿距离，找到距离最近的那个(多个符合只取第一个)
代码如下：

def multidim_intersect(coordinate_list_1, coordinate_list_2):  # 两个坐标集的交集
    coordinate_list_1, coordinate_list_2 = np.array(coordinate_list_1), np.array(coordinate_list_2)
    arr1_view = coordinate_list_1.view([('', coordinate_list_1.dtype)] * coordinate_list_1.shape[1])
    arr2_view = coordinate_list_2.view([('', coordinate_list_2.dtype)] * coordinate_list_2.shape[1])
    intersected = np.intersect1d(arr1_view, arr2_view)
    return np.ndarray.tolist(intersected)

def get_passed_by_coordinate_list(self):
    passed_by_coordinate_list = []
    for coordinate in self.path_log:
        x, y = coordinate
        up = (x, y + 1)
        down = (x, y - 1)
        left = (x - 1, y)
        right = (x + 1, y)
        passed_by_coordinate_list.append(up)
        passed_by_coordinate_list.append(down)
        passed_by_coordinate_list.append(left)
        passed_by_coordinate_list.append(right)
    passed_by_coordinate_list = list(set(passed_by_coordinate_list))
    passed_by_coordinate_list = multidim_intersect(passed_by_coordinate_list, self.coordinate_list)
    passed_by_coordinate_list = multidim_intersect(passed_by_coordinate_list, self.get_uncleaned_coordinate_list())
    return passed_by_coordinate_list
    
def find_nearest_coordinate_by_manhanttan(coordinate1, coordinate_list):
    record = 50000
    for coordinate2 in coordinate_list:
        distance = get_manhanttan_distance(coordinate1, coordinate2)
        if distance < record:
            record = distance
            result = coordinate2
    return result
    
def get_nearest_uncleaned_coordinate(self):
    passed_by_coordinate_list = get_passed_by_coordinate_list(self)
    return find_nearest_coordinate_by_manhanttan(self.current_coordinate, passed_by_coordinate_list)

至此，我们实现了A*算法的目标/前提，即出发点和终点，之后再说A*算法的实现