摘要：尋路模塊通過一番尋找，發(fā)現(xiàn)這系列文章，其不僅包含算法，連尋路算法中的一些基礎(chǔ)知識(shí)也一并介紹了，不愧是斯坦福出品，也很感謝譯者要實(shí)現(xiàn)點(diǎn)到點(diǎn)最短路徑，還需要做一些微小的工作，下面逐個(gè)說明計(jì)算曼哈頓距離的函數(shù)目的是尋路，肯定需要一個(gè)方法來估算兩點(diǎn)

通過一番尋找，發(fā)現(xiàn)這系列文章，其不僅包含A*算法，連尋路算法中的一些基礎(chǔ)知識(shí)也一并介紹了，不愧是斯坦福出品，也很感謝譯者
要實(shí)現(xiàn)點(diǎn)A到點(diǎn)B最短路徑，還需要做一些微小的工作，下面逐個(gè)說明

目的是尋路，肯定需要一個(gè)方法來估算兩點(diǎn)間距離，由于我在移動(dòng)方式上限定了只有上下左右4個(gè)方向可動(dòng)，那么很自然，兩點(diǎn)間最短距離就是就是曼哈頓距離
代碼:

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

之前提到 “U型走法清掃一次，然后通過A*算法走到最近的未清潔點(diǎn)，如此反復(fù)直到所有點(diǎn)都被清潔/走過”
說起來容易，具體實(shí)現(xiàn)還幾個(gè)步驟：

找出未清潔的點(diǎn)的合集

找出這些點(diǎn)中，離當(dāng)前所在點(diǎn)最近的點(diǎn)

如果其中有多個(gè)點(diǎn)到離當(dāng)前所在點(diǎn)的距離一樣，則需要按某個(gè)規(guī)則，在這些點(diǎn)中取一個(gè)出來

未清潔的點(diǎn)=未路過的點(diǎn)，我們現(xiàn)在手頭上有coordinate_list，impassable_coordinate_list和robot.path_log，所以我們可以:

在coordinate_list中去掉impassable_coordinate_list的那些點(diǎn)（差集）得到passable_coordinate_list

在passable_coordinate_list中，中去掉robot.path_log的那些點(diǎn)（差集），得到uncleaned_coordinate_list

這里有需要做兩次交集，本來打算直接用numpy中的setdiff1d函數(shù)，但因setdiff1d不支持多維(多維指(x,y)的形式)，參考了SO上大神的辦法寫一個(gè)（用了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)

然后就可以得到未清潔的點(diǎn)列表了

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

寫完上面這段時(shí)候我突然想到，上面的曼哈頓距離沒考慮障礙，如果未清潔點(diǎn)和當(dāng)前所在點(diǎn)隔了一堵墻，反而會(huì)繞遠(yuǎn)路。但不用曼哈頓距離，對(duì)每個(gè)未清潔點(diǎn)求最短路徑似乎也不太合適
再次思考，想到這個(gè)搜索動(dòng)作是在U型走法后執(zhí)行的，U型走法覆蓋區(qū)域還是比較規(guī)律的，當(dāng)前點(diǎn)到之前路過的區(qū)域，不至于很繞路，那么是不是可以找U型走法覆蓋區(qū)域的周圍的點(diǎn)，在這些點(diǎn)里再找最近的呢？
于是把上面的2和3改為:
2 . 找出所有走過的點(diǎn)，取其上下左右4個(gè)點(diǎn)到passed_by_coordinate_list
3 . 去重復(fù)，去掉非法點(diǎn)(和coordinate_list做交集)，找到未清潔的點(diǎn)(和uncleaned_coordinate_list做交集)，更新passed_by_coordinate_list
4 . 計(jì)算passed_by_coordinate_list中所有點(diǎn)到當(dāng)前點(diǎn)的曼哈頓距離，找到距離最近的那個(gè)(多個(gè)符合只取第一個(gè))
代碼如下：

def multidim_intersect(coordinate_list_1, coordinate_list_2):  # 兩個(gè)坐標(biāo)集的交集
    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)

至此，我們實(shí)現(xiàn)了A*算法的目標(biāo)/前提，即出發(fā)點(diǎn)和終點(diǎn)，之后再說A*算法的實(shí)現(xiàn)