《智能機(jī)器人創(chuàng)新設(shè)計(jì)》 課件 實(shí)踐：多機(jī)器人路徑規(guī)劃

多機(jī)器人路徑規(guī)劃——一段及多段路徑實(shí)踐智慧物流系統(tǒng)：從設(shè)計(jì)到實(shí)現(xiàn)教學(xué)內(nèi)容CONTENTS4實(shí)踐準(zhǔn)備3任務(wù)要求1項(xiàng)目目標(biāo)5一段路徑-程序框圖6一段路徑-操作步驟7一段路徑-實(shí)操效果2項(xiàng)目原理8多段路徑-程序框圖9多段路徑-操作步驟10多段路徑-實(shí)操效果11拓展思考31項(xiàng)目目標(biāo)了解D*算法對一段路徑及多段路徑進(jìn)行規(guī)劃的設(shè)計(jì)思路，通過分析其算法邏輯，學(xué)習(xí)使用Python編程語言進(jìn)行相應(yīng)的函數(shù)模塊設(shè)計(jì)。不同函數(shù)模塊的設(shè)計(jì)與算法原理的結(jié)合將有助于同學(xué)們完全理解算法本身?；陬愓鎸?shí)的物流場景任務(wù)，完成算法的調(diào)試，最終在應(yīng)用軟件Flex-PF實(shí)現(xiàn)基于D*算法給多個物流機(jī)器人規(guī)劃路徑，使物流機(jī)器人根據(jù)最優(yōu)的路徑實(shí)現(xiàn)從起點(diǎn)位置移動到貨架位置。42項(xiàng)目原理D*（D-Star,DynamicAStar）算法是典型的動態(tài)網(wǎng)路求解最短路徑算法，用于計(jì)算一個節(jié)點(diǎn)到其他所有節(jié)點(diǎn)的最短路徑。主要特點(diǎn)是以起點(diǎn)為中心向外層擴(kuò)展，直到擴(kuò)展到終點(diǎn)位置，該算法實(shí)現(xiàn)原理為在機(jī)器人按照預(yù)定路線行駛的過程中，當(dāng)與其他機(jī)器人發(fā)生碰撞時，把其他機(jī)器人當(dāng)作障礙物，并且重新規(guī)劃路徑。52項(xiàng)目原理(一)D*算法的主要步驟:①計(jì)算起點(diǎn)到終點(diǎn)的曼哈頓距離；②利用A*算法給每個機(jī)器人都規(guī)劃最優(yōu)路徑，并在路徑中出現(xiàn)轉(zhuǎn)彎時增加轉(zhuǎn)彎懲罰；③根據(jù)A*算法規(guī)劃出來的路線，去檢測是否沖突，獲取某一時刻其他小車的位置，如果出現(xiàn)沖突，將其他小車當(dāng)作障礙物，并重新規(guī)劃路徑；④遍歷start_end_group中的開始結(jié)束坐標(biāo)對，依次調(diào)用D*算法給機(jī)器人規(guī)劃路線。(二)通過命令行的方式或者Pycharm開發(fā)環(huán)境運(yùn)行(run)，查看結(jié)果，檢查路徑規(guī)劃是否有錯誤。63任務(wù)要求場景任務(wù)一：針對一段路徑，對多個機(jī)器人進(jìn)行路徑規(guī)劃。使用Python編程語言實(shí)現(xiàn)D*算法功能函數(shù)模塊；通過D*算法功能模塊實(shí)現(xiàn)物流機(jī)器人從等待區(qū)到貨架區(qū)的路徑規(guī)劃任務(wù)。7使用python語言編寫一個Dstar類(或函數(shù))，該類(或函數(shù))根據(jù)D*算法多個機(jī)器人規(guī)劃起點(diǎn)到貨架的路徑：起點(diǎn)到貨架3任務(wù)要求83任務(wù)要求場景任務(wù)二：針對多段路徑，對多個機(jī)器人進(jìn)行路徑規(guī)劃。需要考慮機(jī)器人的前進(jìn)時間、轉(zhuǎn)彎時間、進(jìn)貨架區(qū)的減速時間、抬升貨架的時間、延時等待時間。使用Python編程語言實(shí)現(xiàn)D*算法功能函數(shù)模塊；通過D*算法功能模塊實(shí)現(xiàn)物流機(jī)器人從等待區(qū)到貨架區(qū)，再從貨架區(qū)移動到取貨區(qū)，最后再從取貨區(qū)返回貨架區(qū)的路徑規(guī)劃任務(wù)。9使用python語言編寫一個Dstar類(或函數(shù))，該類(或函數(shù))根據(jù)D*算法多個機(jī)器人規(guī)劃多段路徑：機(jī)器人到貨架①從機(jī)器人到貨架的路徑；貨架到取貨點(diǎn)取貨點(diǎn)到貨架②從貨架到取貨點(diǎn)的路徑；③從取貨點(diǎn)到貨架的路徑；3任務(wù)要求104實(shí)踐準(zhǔn)備序號教學(xué)環(huán)境數(shù)量要求1電腦1Windows102物流機(jī)器人1無問題、滿電3路由器1品牌：TP-Link型號：TL-WAR452L4場地18x75貨架≥2貨架緊固無松散115一段路徑-程序框圖12(1)場地設(shè)置：8*7的地圖在地圖中標(biāo)記靜態(tài)的障礙物和貨架區(qū)，明確貨架區(qū)的進(jìn)出規(guī)則；配置地圖大小、貨架區(qū)、障礙物、機(jī)器人的起終點(diǎn)、機(jī)器人初始朝向等相關(guān)參數(shù)；6一段路徑-操作步驟13(2)D*算法的主要步驟：①計(jì)算起點(diǎn)到終點(diǎn)的曼哈頓距離；②利用A*算法給每個機(jī)器人都規(guī)劃最優(yōu)路徑，并在路徑中出現(xiàn)轉(zhuǎn)彎時增加轉(zhuǎn)完懲罰；③根據(jù)A*算法規(guī)劃出來的路線，去檢測是否沖突，獲取某一時刻其他小車的位置，如果出現(xiàn)沖突時，將其他小車當(dāng)作障礙物，并重新規(guī)劃路徑；④遍歷start_end_group中的開始結(jié)束坐標(biāo)對，依次調(diào)用D*算法給機(jī)器人規(guī)劃路線；(3)通過命令行的方式或者Pycharm開發(fā)環(huán)境運(yùn)行(run)，查看結(jié)果，檢查規(guī)劃出的路徑是否有錯誤；6一段路徑-操作步驟14(4)引用庫：本例程中自定義的Dstar類需要依賴heapq庫和numpy庫，載入相關(guān)庫；numpy

是一個運(yùn)算速度非?？斓臄?shù)學(xué)函數(shù)庫heapq

提供了堆排序算法的實(shí)現(xiàn)6一段路徑-操作步驟15(5)創(chuàng)建Dstar類，包含15個屬性，10個方法。在初始化方法中定義15個屬性6一段路徑-操作步驟16(5)創(chuàng)建Dstar類，包含15個屬性，8個方法。①②③④⑤⑥⑦⑧重置地圖，設(shè)置地圖中的障礙貨架區(qū)以及動態(tài)障礙計(jì)算起點(diǎn)終點(diǎn)的曼哈頓距離搜索地圖，判斷相鄰坐標(biāo)是否可搜索路徑中出現(xiàn)轉(zhuǎn)彎時，添加轉(zhuǎn)彎懲罰利用A*算法規(guī)劃路徑，只考慮靜態(tài)障礙物獲取某一時刻其他小車的坐標(biāo)位置利用D*算法優(yōu)化A*算法規(guī)劃處的路徑同時控制多個機(jī)器人按照規(guī)劃的路徑移動6一段路徑-操作步驟17①函數(shù)resetMap：將非目標(biāo)的貨架設(shè)置為障礙，小車不可以在貨架區(qū)隨意穿行。在固定位置放置障礙，將某一時刻遇到的小車設(shè)置為障礙循環(huán)貨架坐標(biāo)，將貨架坐標(biāo)設(shè)置為障礙；獲取起點(diǎn)終點(diǎn)坐標(biāo)，判斷終點(diǎn)是否在貨架區(qū)，將終點(diǎn)設(shè)置為可搜索循環(huán)障礙物坐標(biāo)，設(shè)置障礙物坐標(biāo)不可搜索；循環(huán)動態(tài)障礙坐標(biāo)，設(shè)置動態(tài)障礙坐標(biāo)不可搜索；6一段路徑-操作步驟18②函數(shù)Manhattan：計(jì)算每一對起點(diǎn)到終點(diǎn)的曼哈頓距離，提供給A*算法規(guī)劃路徑：曼哈頓距離計(jì)算公式：S=|X2-X1|+|Y2-Y1|曼哈頓距離有一個轉(zhuǎn)彎的節(jié)點(diǎn)，1.5權(quán)重為轉(zhuǎn)彎懲罰6一段路徑-操作步驟19③函數(shù)isNextAxisValid：搜索地圖的過程中，判斷相鄰坐標(biāo)是否可搜索；將超出地圖的坐標(biāo)判定為不合法坐標(biāo)6一段路徑-操作步驟20④函數(shù)pathAddturnTime：根據(jù)得到的路徑，當(dāng)機(jī)器人需要改變朝向轉(zhuǎn)彎時增加轉(zhuǎn)彎的時間，添加一次原地坐標(biāo)；獲取路徑和機(jī)器人朝向，設(shè)定決定方向和轉(zhuǎn)向權(quán)重循環(huán)路徑，判斷機(jī)器人的下一坐標(biāo)的方向，需要轉(zhuǎn)彎時添加轉(zhuǎn)彎步數(shù)6一段路徑-操作步驟21⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：確定起點(diǎn)終點(diǎn)都沒有障礙物；判斷地圖中的起點(diǎn)坐標(biāo)，若在障礙物列表中，無法規(guī)劃路徑，返回空列表判斷地圖中終點(diǎn)坐標(biāo)是否在障礙物列表中，若在則返回起點(diǎn)坐標(biāo)6一段路徑-操作步驟22⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：搜索地圖；根據(jù)曼哈頓距離，計(jì)算估值函數(shù)，創(chuàng)建open表用于搜索地圖，估值函數(shù)小的坐標(biāo)會被優(yōu)先排在對首根據(jù)open表中隊(duì)首的元素來搜索地圖，判斷坐標(biāo)的上下左右方向的坐標(biāo)是否合法，將合法的坐標(biāo)加入到open表中，當(dāng)搜索到終點(diǎn)時跳出循環(huán)，結(jié)束搜索6一段路徑-操作步驟23⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：回溯路徑；如果搜索結(jié)束，未找到終點(diǎn)，返回起點(diǎn)坐標(biāo)回溯路徑，選擇上下左右方向中權(quán)值最小的坐標(biāo)行進(jìn)，找到從終點(diǎn)到起點(diǎn)間的最短路徑6一段路徑-操作步驟24⑥函數(shù)getSameTimeBarrier：獲取t時刻其他小車的位置，將其他小車在t時刻設(shè)置為障礙物；獲取某一時刻，對于該車來說動態(tài)障礙物(其他車)的坐標(biāo)位置；6一段路徑-操作步驟25⑦函數(shù)Dstar：利用D*算法優(yōu)化小車的路徑?jīng)_突：先調(diào)用A*算法規(guī)劃不考慮動態(tài)障礙物的路徑；獲取起點(diǎn)和終點(diǎn)，先調(diào)用重置地圖方法，設(shè)置固定障礙、貨架區(qū)等，然后調(diào)用A*算法規(guī)劃出不考慮動態(tài)障礙的路徑。6一段路徑-操作步驟26⑦函數(shù)Dstar：利用D*算法優(yōu)化小車的路徑?jīng)_突：根據(jù)A*算法規(guī)劃好的路徑進(jìn)行沖突檢測；獲取路徑中的坐標(biāo)和機(jī)器人朝向和動態(tài)障礙物位置，判斷坐標(biāo)是否在動態(tài)障礙物中，不在，不發(fā)生沖突；在，發(fā)生沖突，有三種解決沖突的方法。6一段路徑-操作步驟27⑧函數(shù)multiControl：循環(huán)起終點(diǎn)的坐標(biāo)組，多次調(diào)用D*算法給多個機(jī)器人規(guī)劃路徑；循環(huán)起終點(diǎn)的坐標(biāo)對長度，調(diào)用D*算法6一段路徑-操作步驟28(6)將帶有轉(zhuǎn)彎路徑的示意圖在控制臺輸出。設(shè)置地圖中的權(quán)值將路徑中坐標(biāo)點(diǎn)的下標(biāo)寫入地圖中對應(yīng)的位置6一段路徑-操作步驟29通過命令行方式或PyCharm集成開發(fā)環(huán)境運(yùn)行（run）例程：在主函數(shù)中設(shè)置參數(shù)，調(diào)用算法，實(shí)例化算法，輸出結(jié)果：地圖大小、貨架區(qū)坐標(biāo)、障礙區(qū)坐標(biāo)、起終點(diǎn)坐標(biāo)組、機(jī)器人朝向輸出每個機(jī)器人的起終點(diǎn)、規(guī)劃的路徑以及路徑在地圖中的表示7一段路徑-實(shí)操效果30控制臺輸出結(jié)果：0號機(jī)器人的路徑和在地圖中的走向1號機(jī)器人的路徑和在地圖中的走向200：表示地圖中的坐標(biāo)都可以被搜索規(guī)劃路徑：前面列表表示坐標(biāo)，后面數(shù)字表示機(jī)器人的朝向(0上、1右、2下、3左)。7一段路徑-實(shí)操效果31控制臺輸出結(jié)果：2號機(jī)器人的路徑和在地圖中的走向3號機(jī)器人的路徑和在地圖中的走向規(guī)劃路徑：前面列表表示坐標(biāo)，后面數(shù)字表示機(jī)器人的朝向(0上、1右、2下、3左)。7一段路徑-實(shí)操效果32控制臺輸出結(jié)果：4號機(jī)器人的路徑和在地圖中的走向規(guī)劃路徑：前面列表表示坐標(biāo)，后面數(shù)字表示機(jī)器人的朝向(0上、1右、2下、3左)。7一段路徑-實(shí)操效果338多段路徑-程序框圖34(1)場地設(shè)置：8*7的地圖在地圖中標(biāo)記靜態(tài)的障礙物和貨架區(qū)，明確貨架區(qū)的進(jìn)出規(guī)則；將機(jī)器人、貨架、障礙放置在地圖中的指定位置；9多段路徑-操作步驟35(2)多機(jī)器人路徑規(guī)劃在規(guī)劃多段路徑時，D*算法的原理是一樣的，但是在規(guī)劃時需要考慮注意一些機(jī)器人相關(guān)的時間參數(shù)：①機(jī)器人轉(zhuǎn)90度角的用時；②機(jī)器人轉(zhuǎn)180度角的用時；③機(jī)器人前進(jìn)一格的用時；④機(jī)器人進(jìn)貨價區(qū)減速用時；⑤機(jī)器人抬升貨架的用時；⑥機(jī)器人的延時等待用時；⑦最長延時的限制；9多段路徑-操作步驟36(3)多機(jī)器人路徑規(guī)劃在規(guī)劃多段路徑時，需要注意的點(diǎn)：①設(shè)置地圖中的貨架區(qū)、固定障礙物的位置，將非目標(biāo)的貨架設(shè)為固定障礙；②在搜索地圖時，需要判斷是否可以從貨架區(qū)的側(cè)面進(jìn)入，將從貨架區(qū)側(cè)面搜索到的坐標(biāo)判定為不合法坐標(biāo)；③判斷某一時間段內(nèi)，其他機(jī)器人對于某機(jī)器人來說是否是障礙；9多段路徑-操作步驟37(3)多機(jī)器人路徑規(guī)劃在規(guī)劃多段路徑時，需要注意的點(diǎn)：④在規(guī)劃出路徑后，當(dāng)路徑中出現(xiàn)轉(zhuǎn)彎、減速、等待、抬升貨架時，在路徑中加入各種時間參數(shù)；⑤在進(jìn)行沖突檢測時，判斷某一時間段內(nèi)，對于某機(jī)器人來說動態(tài)的障礙物(其他機(jī)器人)，如果規(guī)劃的路徑某一時刻在時間段內(nèi)，將這個坐標(biāo)加入到動態(tài)障礙中；之前規(guī)劃的其他機(jī)器人路徑在該時間段內(nèi)，將坐標(biāo)加入到動態(tài)障礙物中；9多段路徑-操作步驟38(3)多機(jī)器人路徑規(guī)劃在規(guī)劃多段路徑時，需要注意的點(diǎn)：⑥在發(fā)生沖突時，返回發(fā)生沖突的前一坐標(biāo)，然后重新規(guī)劃路徑；⑦控制給多個機(jī)器人規(guī)劃時，先根據(jù)曼哈頓距離判斷對路徑長度的進(jìn)行估計(jì)和排序，記錄機(jī)器人的優(yōu)先級，然后根據(jù)機(jī)器人的狀態(tài)決定規(guī)劃幾段路徑，規(guī)劃出路徑后精簡要輸出的路徑；9多段路徑-操作步驟39(4)引用庫：和規(guī)劃一段路徑的例程一樣，自定義的Dstar類需要依賴heapq庫和numpy庫，載入相關(guān)庫；numpy

是一個運(yùn)算速度非?？斓臄?shù)學(xué)函數(shù)庫heapq

提供了堆排序算法的實(shí)現(xiàn)9多段路徑-操作步驟40(5)創(chuàng)建Dstar類，包含25個屬性，11個方法；在初始化方法中定義25個屬性9多段路徑-操作步驟41①函數(shù)resetMap：重置地圖，設(shè)置貨架區(qū)、固定障礙位置，將非目標(biāo)貨架區(qū)設(shè)為固定障礙，將距離近的小車設(shè)為動態(tài)障礙；將非目標(biāo)貨架區(qū)設(shè)置為固定障礙;根據(jù)機(jī)器人狀態(tài)確定起點(diǎn)終點(diǎn)；設(shè)置動態(tài)障礙，將距離機(jī)器人兩個單位的其他機(jī)器人設(shè)置為動態(tài)障礙9多段路徑-操作步驟42②函數(shù)Manhattan：計(jì)算每一對起點(diǎn)到終點(diǎn)的曼哈頓距離，提供給A*算法規(guī)劃路徑：曼哈頓距離計(jì)算公式：S=|X2-X1|+|Y2-Y1|曼哈頓距離有一個轉(zhuǎn)彎的節(jié)點(diǎn)，1.5權(quán)重為轉(zhuǎn)彎懲罰9多段路徑-操作步驟43③函數(shù)isNextAxisValid：判斷相鄰的坐標(biāo)是否可搜索，超出地圖限制的坐標(biāo)不合法，從側(cè)面進(jìn)去貨架區(qū)的坐標(biāo)不合法；判斷超出地圖限制判斷從貨架的側(cè)面進(jìn)入貨架區(qū)搜索9多段路徑-操作步驟44④函數(shù)pathAddRealTime：給已規(guī)劃的路徑添加實(shí)際的運(yùn)行時間，前進(jìn)一步的時間、轉(zhuǎn)彎的時間、今日貨架區(qū)減速的時間、抬升貨架的時間；絕對方向，機(jī)器人的朝向當(dāng)前坐標(biāo)，下一步坐標(biāo)、計(jì)算下一步機(jī)器人的朝向如果下一步的朝向等于絕對方向，記錄機(jī)器人下一步朝向9多段路徑-操作步驟45④函數(shù)pathAddRealTime：給已規(guī)劃的路徑添加實(shí)際的運(yùn)行時間，前進(jìn)一步的時間、轉(zhuǎn)彎的時間、今日貨架區(qū)減速的時間、抬升貨架的時間；當(dāng)前朝向不等于下一步朝向時：加入機(jī)器人轉(zhuǎn)向的實(shí)際時間如果下一步是貨架區(qū)，添加減速時間；不是，添加正常的前進(jìn)時間；如果當(dāng)前位置是貨架區(qū)，添加抬升貨架時間；記錄每一步的朝向和時間9多段路徑-操作步驟46⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：確定起點(diǎn)終點(diǎn)都沒有障礙物；判斷地圖中的起點(diǎn)坐標(biāo)是否在障礙物列表中，若在無法規(guī)劃路徑，返回空列表判斷地圖中終點(diǎn)坐標(biāo)是否在障礙物列表中，若在則返回起點(diǎn)坐標(biāo)9多段路徑-操作步驟47⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：搜索地圖；根據(jù)曼哈頓距離，計(jì)算估值函數(shù)，創(chuàng)建open表用于搜索地圖，估值函數(shù)小的坐標(biāo)會被優(yōu)先排在對首根據(jù)open表中隊(duì)首的元素來搜索地圖，判斷坐標(biāo)的上下左右方向的坐標(biāo)是否合法，將合法的坐標(biāo)加入到open表中，當(dāng)搜索到終點(diǎn)時跳出循環(huán)，結(jié)束搜索9多段路徑-操作步驟48⑤函數(shù)Astar：不考慮動態(tài)障礙物，利用A*算法規(guī)劃路徑：回溯路徑；如果搜索結(jié)束，未找到終點(diǎn)，則返回起點(diǎn)坐標(biāo)、機(jī)器人朝向、等待時間回溯路徑，選擇上下左右方向中權(quán)值最小的坐標(biāo)行進(jìn)，找到從終點(diǎn)到起點(diǎn)間的最佳路徑9多段路徑-操作步驟49⑥函數(shù)getSameTimeBarrier：判斷某一時間段內(nèi)，對于某車來說，地圖中的動態(tài)障礙物(其他小車)坐標(biāo)；本次規(guī)劃的路徑中，將其他機(jī)器人在時間段內(nèi)的路徑設(shè)置為動態(tài)障礙之前規(guī)劃的機(jī)器人剩余路徑在時間段內(nèi)也將設(shè)置為動態(tài)障礙9多段路徑-操作步驟50⑦函數(shù)timeBackOneStep：機(jī)器人在某一步發(fā)生沖突時，返回沖突點(diǎn)的前一個坐標(biāo)在路徑中的下標(biāo)；反向循環(huán)路徑的長度，如果路徑中的坐標(biāo)不等于發(fā)生沖突的坐標(biāo)，返回發(fā)生沖突前一坐標(biāo)的下標(biāo)9多段路徑-操作步驟51⑧函數(shù)getPathTotalTime：計(jì)算執(zhí)行已規(guī)劃路徑的總耗時；獲取帶有真實(shí)執(zhí)行時間的路徑，獲取執(zhí)行時間，計(jì)算執(zhí)行已規(guī)劃路徑的總耗時9多段路徑-操作步驟52⑨函數(shù)Dstar：D*算法優(yōu)化A*算法規(guī)劃的路徑，對路徑中的坐標(biāo)進(jìn)行沖突檢測；先重置地圖中的權(quán)值，規(guī)劃出不考慮動態(tài)障礙的最短路徑，判斷起點(diǎn)和終點(diǎn)，防止出現(xiàn)重復(fù)抬升貨架的情況9多段路徑-操作步驟53⑨函數(shù)Dstar：D*算法優(yōu)化A*算法規(guī)劃的路徑，對路徑中的坐標(biāo)進(jìn)行沖突檢測；獲取機(jī)器人走到某一步所耗的時間；獲取機(jī)器人走的前一步所耗的時間；獲取在t~t_step時間段的移動障礙；獲取在pre_t~t時間段的移動障礙；判斷坐標(biāo)是否在移動障礙列表中：不在，表示不發(fā)生沖突；判斷是否會發(fā)生追尾：如果發(fā)生追尾，獲取前一步坐標(biāo)添加延時等待；9多段路徑-操作步驟54⑨函數(shù)Dstar：D*算法優(yōu)化A*算法規(guī)劃的路徑，對路徑中的坐標(biāo)進(jìn)行沖突檢測；在動態(tài)障礙列表中，表示會發(fā)生沖突；第i步發(fā)生沖突，將第i、i+1步都設(shè)置為障礙，在i-1步重新規(guī)劃如果從i-1規(guī)劃起點(diǎn)沖突，那就獲取i-1步的上一步坐標(biāo)，循環(huán)沒有路徑的話就一直找前一個坐標(biāo)并重新規(guī)劃如果順利找到結(jié)束點(diǎn)，那就拼接兩段路徑去掉一個i-1步9多段路徑-操作步驟55⑩函數(shù)getDalayOnlyPath：在D*算法規(guī)劃出路徑之后，輸出時需要精簡路徑，只輸出坐標(biāo)朝向時間；獲取路徑中的第i步和第i+1步，當(dāng)出現(xiàn)時間延遲的路徑時只輸出一次坐標(biāo)點(diǎn)累加延時的時間，如果超出時間限制，放棄分配組合否則，將延時清零或者就等于初次延時的時間9多段路徑-操作步驟56函數(shù)multiControl：先根據(jù)曼哈頓距離對路徑長度進(jìn)行估計(jì)并排序；獲取car_shelf_target中的坐標(biāo)點(diǎn)、機(jī)器人朝向，記錄機(jī)器人真實(shí)時間的路徑，估計(jì)真實(shí)路徑用時根據(jù)機(jī)器人的狀態(tài)確定需要給機(jī)器人規(guī)劃幾段路徑，并計(jì)算路徑距離給路徑排序9多段路徑-操作步驟57函數(shù)multiControl：根據(jù)機(jī)器人的狀態(tài)給機(jī)器人規(guī)劃對應(yīng)的路徑；當(dāng)機(jī)器人的狀態(tài)為0(空閑)或1(機(jī)器人到貨架)時：規(guī)劃三段路徑；機(jī)器人到貨架貨架到取貨點(diǎn)取貨點(diǎn)到貨架9多段路徑-操作步驟58函數(shù)multiControl：根據(jù)機(jī)器人的狀態(tài)給機(jī)器人規(guī)劃對應(yīng)的路徑；當(dāng)機(jī)器人的狀態(tài)為2(貨架到取貨點(diǎn))時：規(guī)劃兩段路徑貨架到取貨點(diǎn)取貨點(diǎn)到貨架9多段路徑-操作步驟59函數(shù)multiControl：根據(jù)機(jī)器人的狀態(tài)給機(jī)器人規(guī)劃對應(yīng)的路徑；當(dāng)機(jī)器人的狀態(tài)為3時：規(guī)劃一段路徑獲取路徑最后一個坐標(biāo)點(diǎn)，機(jī)器人朝向和時間，將最后一步的時間改成抬升貨架的時間取貨點(diǎn)到貨架9多段路徑-操作步驟60函數(shù)multiControl：精簡已規(guī)劃好的路徑，輸出；精簡已規(guī)劃的路徑，并記錄路徑執(zhí)行的的時間9多段路徑-操作步驟61(6)將帶有轉(zhuǎn)彎路徑的示意圖在控制臺輸出