掃地機器人的模擬程序 (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*算法的實現