一、 題目背景

　　海康威視智能泊車機器人依托海康威視多年在圖像處理、硬件設計及嵌入式軟件領域的技術積累，以及阡陌系列智能倉儲機器人歷經兩年多來精雕細琢已成熟應用的核心技術，瞄準“停車難”痛點，為用戶帶來完美的停車體驗。智能泊車機器人采用海康機器人成熟的視覺和慣性雙導航技術實現自主定位，定位精度誤差小於5mm，可完成2000kg汽車的升舉、搬運、旋轉、下放，智能泊車機器人系統可同時調度500輛汽車，同等面積停車場停車位數量增加40%。系統配備海康威視智能停車系統的倉庫，最多可並排停放4排汽車，自主研發的智能算法保證用戶需取走停在中間排的汽車時，系統會根據車庫內車輛停泊的實際情況合理安排其周邊車輛以最短路線避讓，挪出空位，供待取汽車以最短時間進入客戶所在的交互區，2000平方米的停車場，取車平均時間僅2分鐘。作為全球首個機器人智能停車應用案例，海康威視智能泊車機器人已應用於第三屆世界互聯網大會·烏鎮峰會，出色呈現真正“互聯網化”的智能停車體驗。

二、 設計目標

　　在車庫中安排若幹泊車機器人，根據給定的車位地圖，合理優化機器人的數量及其運動路徑，盡量減少客戶在停車和取車中的等待時間，並使總成本最小。

三、 總體設計

本算法主要分為兩部分，第一部分是預處理，第二部分是調度算法。

預處理包括拒載預處理和車位分配預處理，預處理中忽略了機器人的影響，只從車位和汽車信息來計算出初步方案。其中拒載預處理解決了車位資源緊缺問題，當車位不夠時合理拒載一部分車輛以獲得更低的總體代價。車位預分配是指在調度算法工作前為每輛車計算好預留車位數，入庫時通過預留車位數選擇合理車位，相比安排具體車位，這種方式能夠提高車位的利用率。通過這種預處理方式將原本的三維數據降低至二維，大大加快了數據的處理速度，從而可以得到較優的全局解。

調度算法包括入庫處理，出庫處理，返回處理和信息更新。

調度算法采用時間驅動來調度機器人，每隔一個單位時間更新所有的機器人和車輛狀態。

　　本方案的總體設計如圖1所示，首先將初始待入庫車進行預處理，預拒載一部分，然後對剩余的車分配預留車位數。接下來將處理後的待入庫車傳給調度系統進行處理。入庫前需要計算入庫代價來判斷選擇入庫還是拒載。出庫時需要機器人提前到車位上做好準備。機器人完成入庫或出庫任務後都要立即返回入口，然後執行下一個任務。

一、 細節設計

4.1機器人任務調度狀態機

機器人有五種狀態，分別是入庫狀態，取車狀態，出庫狀態，返回狀態和空閑狀態。

狀態內容說明：

1）入庫狀態：機器人運載車輛入庫。

2）取車狀態：機器人到車位上準備載車出庫

3）出庫狀態：機器人運載車輛出庫

4）返回狀態：機器人返回入口

5）空閑狀態：無任務等待被調度

4.2車位預處理算法

4.2.1拒載預處理

原理：

本方案采用鏈表來預處理拒載車輛。如3是拒載處理示意圖，方格中的數字是車重。在入庫時將車輛添加至鏈表，出庫時則移出鏈表。若鏈表發生溢出，則從鏈表中移出重量最大的車，然後繼續入庫。

優點：

預先拒載一部分車輛後，不僅可以避免後續入庫算法中對車位資源緊缺的處理，同時也減少了計算量，加快了運行速度。

4.2.2預留車位數計算
原理：
預留車位數含義：車入庫選擇車庫時，為後面待入庫車輛預留數個更優的車庫。
具體步驟如下：

1.計算每個車位離出入口的總距離，並進行降序排序。
2.計算每輛車的優先級系數，其中是車重量，是車的申請入庫時間，是車的申請入庫時間，是車得停車時間，是車的平均停車時間，β和θ是比重系數，P是車的優先級：是車的平均重量，

3. 建立任務隊列和待出庫隊列，將所有待入庫車添加到任務隊列，其中任務隊列按時間排序，待出庫隊列按優先級系數排序。設置每輛車的記錄為0。

4. 依次讀取任務：如果是入庫任務，將該車插入到待出庫隊列中，根據優先級和輔助計數值更新預留車位數，然後將該任務修改為出庫任務。如果是出庫任務，更新待出庫隊列中的輔助計數值。

5. 任務隊列為空時，得到最終的預留車位數。

優點：

　　總距離越長的庫，機器人處理時的功耗和時間代價相應也越較高，因此提高總距離較短車庫的使用率有利於降低總體代價。預留車位數的機制不涉及具體車位，車位計算和當前車庫使用情況有關，因此入庫中不會因車輛拒載而浪費車位資源，從而提高了車庫的使用率。

　　車的優先級決定了車庫的安排，對結果有很大影響。因此需要合理設置β和θ這兩個比重系數。本方案采用分段搜索的方法，每段使用最速下降法搜索最優解，這樣既加快了搜索速度，有能避免陷入局部最優解。

4.3機器人數量計算
原理：
　　機器人數量大小對總成本有很大的影響，設置過大會導致機器人使用率低下，設置過小又會導致機器人超負荷，造成入庫出庫困難。因此需要計算出一個合理機器人數量。
本文采用了固定機器人數量和動態分配相結合的策略。首先采用動態機器人分配算法，盡量滿足出入庫的需求，以此得到機器人的最大個數。然後往小搜索機器人個數，用固定機器人的方式求出最優解。
優點：
　　本算法叠代次數少，有很強的適用性，總能在理想的時間內計算出最優機器人個數。

4.4調度算法設計
4.4.1整體流程
　　調度算法有四部分組成：機器人返回處理，入庫處理，出庫處理和信息更新。調度系統以時間驅動方式編寫，前三部分負責給機器人安排任務，信息更新用於更新調度系統的運行狀態。

4.4.2入庫流程

原理：

　　入庫前首先要計算入庫代價，如果入庫代價大於拒載代價，則執行拒載處理。如果入庫代價滿足入庫要求，則計算出最優路徑，然後執行入庫任務。

　　入庫代價計算公式如下所示，其中Walt是入庫功耗，μ是單位時間成本，是入庫時間延遲。

　　入庫時還涉及到機器人分配模式，如果是固定機器人數模式，當入口無機器人時，估算得到空閑機器人的等待時間，然後重新計算入庫代價，從而作出更優決策。如果是動態分配模式，入口無機器人可用時無條件新增一輛機器人，然後執行入庫任務。

優點：

機器人在運動過程中實時記錄著它離入口的時間距離，這樣就能得到精確地入庫的延遲時間，進而計算延遲對本車以後面待入庫車產生的總代價，然後作出合理的拒載決策。

4.4.3 出庫流程
原理：
　　出庫處理時需要同時規劃兩條路徑，一條是取車路徑，另一條是出庫路徑。
　　當出庫剩余時間長度小於該車庫的最短入庫路徑長時， 釋放車庫資源。
優點：
　　由於出庫路徑長度計入功耗，而取車路徑不計，因此提前規劃出庫路徑較容易得到最短路徑，降低功耗成本。
　　提前釋放車庫資源有利於提高車庫的利用率。

4.5.1二維A*算法

原理：

A*是一種啟發式的算法，所謂的"啟發式"，就是對每一個搜索的位置進行評估，也就是把找的位置離目標的距離當成找點的一個依據，然後猜測這個點是否最佳("啟發式"就是猜測)。

步驟：

先設定兩個集合，open集，close集

1、將起始點加入open集（設置父親節點為空）

2、在open集中選著一個F值最小的節點作為當前節點

2.1 將其添加到close集，並從open集中移除！

2.2 如果為終點節點，那麽結束搜索

2.3 處理當前節點的所有鄰接節點

如果不在open集中，那麽就將其添加到open集，（最好設置該節點的父節點為當前節點，以便確定路徑）；

如果已經添加到open集中，重新計算G值，如果G值小於先前的G值，那麽就跟新此值(同時跟新父節點)；

如果該節點不可通過或者已經被添加到close集，那麽不予處理；

3、如果open集不為空，那麽轉到步驟2繼續執行

4.5.2三維A*算法

原理：

1.可達點判斷改進：

　　為了解決時間沖突問題，改進了二維A*的可達點判斷函數，當時間發生相交或沖突時，使該點不可達，這樣就可以進一步在時間維度上避免沖突。

　　在時間維度上有兩種沖突類型：時間點重疊沖突和時間點互換沖突。由於出入口允許重疊，因此對出入口特殊處理，只檢查時間點互換沖突，而其他區域需要檢查兩種沖突。

1.路徑優化：

　　改進了可達點判斷函數後，雖然徹底避免了碰撞和相遇沖突，但是很容易導致路徑規劃過長甚至規劃失敗。

　　如下圖所示。藍色的線是一號機器人的入庫路徑，綠色的的線是二號機器人的返回路徑，從圖中可以看到出入庫路徑和返回路徑有很大一段發生了重疊，如果路徑重疊出時間剛好出現沖突，其中一條路徑必須往一邊躲避一格，這會增加路徑搜索長度，特別是當機器人較多的時候運算時間會急劇上升。

　　針對以上問題，本文改進了啟發函數，在曼哈頓距離公式的基礎上修改y軸的比例系數，式中為比例系數：

　　這樣一來，搜索路徑時便會優先搜索y軸方向。圖10中黑色線便是改進後的入庫路徑，減少路徑重疊，加快了路徑搜索的同時也更容易獲得更優路徑。

4.5.3最優路徑求解

原理：

　　在多機器人環境中，運動錯綜復雜，因此往往導致規劃的路徑過長甚至規劃失敗，因此本文采用了任務延遲機制，通過延遲時間來規劃更優路徑。

　　下圖是最優路徑求解流程，設置任務延時初始值為0，然後調用三維A*算法尋找最優路徑，如果找到路徑是最短的，則路徑規劃成功。否則遞增時間延時，每次叠代記錄最小路徑代價，直至時間延遲代價大於最小路徑代價，終止循環，返回最優路徑。

優點：

　　僅通過增加一個任務延遲變量優化了路徑，算法簡單利於實現。

海康威視復賽題 --- 算法說明書