激光導航小車入庫規劃

蔣如飛，張偉軍

(上海交通大學機械與動力工程學院,上海 200240)

0 引言

導航小車是智能工廠柔性化重要的一環，通過導航小車在不同工位之間穿梭、配合，工廠內的生產線可以做到按需搭配，大大提高生產線的效率和靈活性[1]。根據原理，導航小車可分為路標導航和激光導航[2]，其中路標導航需要在工廠內鋪設磁釘、二維碼等路標[3]，而且小車的運動路線必須經過這些路標以矯正位置；激光導航則靈活許多，小車可以通過激光雷達實時確定自身的位置，而且不需要事先改造工廠，因此成為許多智能工廠的選擇[4]。在導航小車結束任務后，一般需要進入事先規定好的停車位保管。然而，由于小車往往呈長方形，而目前的規劃算法大多沒有考慮到小車的形狀和朝向，需要小車先走到位置，再旋轉自身達到目標角度，所以算法目標位置為狹窄的停車位時，往往會規劃失敗。

在此，提出了一種導航小車，并針對入庫問題改進了規劃方法，可以讓導航小車平穩地進入較狹窄的終點，使自動入庫成為可能。

1 激光導航小車硬件設計

激光導航小車的硬件示意如圖1所示，實物如圖2所示。為了方便規劃，小車采用了差速的控制方法，位于中間的一對主動輪是小車主要的動力提供手段。差速控制可以實現直行、原地旋轉、固定半徑轉彎等功能，為更上層的路徑規劃提供了很好的基礎。主動輪的速度控制與跟蹤由電機驅動器完成，單片機只需要通過485通訊按格式發送固定加速度限制下的期望速度值即可。不只是控制電機，單片機還可以實時控制LED狀態燈，收到主控電腦的設定值后可更改LED燈的顏色顯示狀態。在這種設計中，電機控制任務被分給3個單元執行，最需要實時性的速度跟蹤部分由驅動器完成，需要穩定及時反饋設定速度值的任務由單片機完成，主控電腦只需要根據算法按周期發送速度值即可。

除了電機控制，傳感數據的反饋也是導航小車正常工作的重要依靠。傳感數據主要分為2個部分。一是周圍障礙物跟小車的位置關系，由2個270°的旋轉式激光雷達分布在對角感應。探測到障礙物后，得到的數據是障礙物相對雷達的角度和距離，需要合并到小車的中心坐標系，轉換公式為：

圖1 激光導航小車硬件示意

圖2 激光導航小車實物

xagv=xbeam+ρcosθ

(1)

yagv=ybeam+ρsinθ

(2)

xbeam和ybeam表示雷達的位置；xagv，yagv表示障礙物在導航小車下的位置；ρ為障礙物到雷達的距離；θ為障礙物在雷達坐標系中的角度。

另一個重要的傳感數據是主動輪的角度編碼器，經由驅動器、單片機反饋到主控電腦。主控電腦通過2個主動輪的角度值可以得到小車實際行走的角速度和前進速度，由主動輪半徑、主動輪距離決定，計算公式為：

(3)

(4)

ωwheel1為小車左輪角速度；ωwheel2為小車右輪的角速度；rwheel為車輪半徑；dwheel12為左右輪距離；ωagv為導航小車角速度；vagv為導航小車的速度。

除了發送電機速度指令和接收傳感數據，主控電腦還需要運行規劃和導航算法，并從路由器里接收操作人員的運行命令。路由器是操作人員和導航小車交流的媒介，操作人員通過Wi-Fi連接到路由器后，通過TCP協議往指定的IP地址和端口發送命令，可實現手動控制小車行走或設定目標讓小車自動導航。

2 動態窗口規劃及改進

為了使小車平穩、無碰撞地到達指定的地點和角度，需要規劃出一條滿足小車動力學約束并避開障礙物的軌跡。常見的一種方法是動態窗口規劃[5]，其主要由5個部分組成：

a.根據最大速度、最大角速度的限制離散采樣目標速度，將小車盡量加速或減速到設定的速度。

b.根據每個采樣的速度模擬一段時間，得到多條模擬的軌跡。

c.對每條軌跡打分，打分考慮的因素有離障礙物的距離、離目標的距離等，如發生碰撞則直接去除。

d.選出得分最高的軌跡，將當前速度和角速度下發到單片機執行。

e.每隔一段時間重新采樣、打分；到達目標左右后停止，原地旋轉到目標角度。

動態窗口規劃兼顧了小車動力學限制、設定速度限制、障礙物限制等，而且計算量小，實時性強，是一種十分實用的基于采樣的規劃算法。然而，該算法僅把導航小車視為質點規劃，規劃軌跡在局部多為弧形。當小車不是圓形時，要經過狹窄的通道時往往需要較窄的邊正對著通道口才能通過。因此對于長方形的小車而言，除非小車剛好在當前位置、當前角度可以以弧形軌跡或直線軌跡通過，否則很難用動態窗口規劃的方法解決。所以，動態窗口規劃難以處理導航小車入庫的情況。對此情況，提出一種分段規劃入庫的改進方案，入庫規劃改進對比情況如圖3所示。

圖3 入庫規劃改進示意

對于狹窄通道，一個簡單有效的方法是先走到正對通道的位置，原地調整角度，然后直接沿直線走進去即可。所以，本設計的改進將動態窗口規劃分為2個階段：第1階段走到目標位置的直線可達且可以任意原地旋轉調整方向的地方，然后調整角度到正對目標的角度；第2階段沿直線規劃走到目標點即可。由于目標直線可達位置有無數個，最佳的選擇是直線上離目標位置最近的點。同時，考慮到車庫周圍的障礙物地圖可能并不是一個長方形，而且最終目標角度的反向延長線可能會與障礙物發生碰撞，因此不宜直接以車庫周圍障礙物的對稱軸或目標方向為直線，而是以最不可能發生碰撞的角度為準。基于以上考慮，本改進的具體步驟如下：

a.在障礙物地圖中進行模擬，向左旋轉目標位置處的小車，記錄發生碰撞的角度；同理向右旋轉得到右旋碰撞角度，取2個角度的均值為“退車”角度。

b.沿“退車”角度從近到遠離散采樣位置點，到某一點可以原地360°旋轉仍無任何碰撞為止，記錄該點為第1階段目標點。

c.用動態窗口規劃使小車走到第1階段目標點。

d.沿直線規劃走到最終目標點，調整角度到最終目標角度。

3 仿真實驗結果

為了測試改進算法的有效性，本文針對性地設計了入庫場景的仿真實驗。實驗中，目標點位于狹窄的通道中，通道兩旁是虛擬的障礙物；仿真小車長1 m，寬0.5 m，最大速度為0.5 m/s，加速度為0.2 m/s2，最大角速度為40(°)/s，角加速度為1(°)/s2；小車要求離障礙物有0.2 m的安全距離，同時運行必須滿足動力學的約束。改進前的動態窗口規劃算法如圖4所示，導航小車徑直走向目標，但由于障礙物的環繞無法到達目標點，最終卡住無法前進。改進后的算法分2個階段：第1階段到達一個要求較低的目標點，如圖5所示；在第2階段，小車從第1個目標點出發重新規劃，最終規劃出一條接近直線的平滑曲線達到目標點，如圖6所示。

圖4 動態窗口規劃軌跡

圖5 改進算法第1階段規劃軌跡

圖6 改進算法最終軌跡

經過改進，導航小車避免了因直接規劃較困難的目標點所導致的失敗，先走到較輕松的目標點；而第1階段的目標點到最終目標點要比一開始的起點要容易許多，因此結合2個階段可以較好地完成狹窄通道的路徑規劃。在障礙物不變的條件下，從目標左側x坐標5～7 m，y坐標11～13 m的矩形范圍內9個點出發多次仿真，改進前僅有1次成功，成功率為11.1%；而分階段規劃法改進后均能夠到達目標點。

4 結束語

動態窗口規劃是一種離散采樣的規劃方法，可根據各種條件靈活設置采樣的范圍、模擬的時間和模擬軌跡評價的方式等，因此可以滿足激光導航小車各種動力學、速度的限制，同時可以很好地避障。然而，由于動態窗口規劃沒有考慮到模擬時間之后的軌跡情況，往往會在狹窄的地方規劃失敗。通過改進規劃算法，本文提出了2個階段的規劃策略，其中第1階段仍然使用動態窗口規劃，第2階段考慮到狹窄位置對導航小車角度的限制，將第1階段的終點設置在直線可達且可任意旋轉角度的地方。由于選用的第1個終點對于第1階段規劃難度低，同時也方便了第2階段的規劃，所以大大提高了規劃成功的可能性。改進后，在原來動態窗口規劃難以處理的入庫場景中成功地完成了軌跡規劃，實現了導航小車的自動入庫。