蔣如飛，張偉軍

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

0 引言

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

在此，提出了一種導(dǎo)航小車，并針對入庫問題改進(jìn)了規(guī)劃方法，可以讓導(dǎo)航小車平穩(wěn)地進(jìn)入較狹窄的終點(diǎn)，使自動入庫成為可能。

1 激光導(dǎo)航小車硬件設(shè)計(jì)

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

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

圖1 激光導(dǎo)航小車硬件示意

圖2 激光導(dǎo)航小車實(shí)物

xagv=xbeam+ρcosθ

(1)

yagv=ybeam+ρsinθ

(2)

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

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

(3)

(4)

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

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

2 動態(tài)窗口規(guī)劃及改進(jìn)

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

a.根據(jù)最大速度、最大角速度的限制離散采樣目標(biāo)速度，將小車盡量加速或減速到設(shè)定的速度。

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

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

d.選出得分最高的軌跡，將當(dāng)前速度和角速度下發(fā)到單片機(jī)執(zhí)行。

e.每隔一段時間重新采樣、打分；到達(dá)目標(biāo)左右后停止，原地旋轉(zhuǎn)到目標(biāo)角度。

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

圖3 入庫規(guī)劃改進(jìn)示意

對于狹窄通道，一個簡單有效的方法是先走到正對通道的位置，原地調(diào)整角度，然后直接沿直線走進(jìn)去即可。所以，本設(shè)計(jì)的改進(jìn)將動態(tài)窗口規(guī)劃分為2個階段：第1階段走到目標(biāo)位置的直線可達(dá)且可以任意原地旋轉(zhuǎn)調(diào)整方向的地方，然后調(diào)整角度到正對目標(biāo)的角度；第2階段沿直線規(guī)劃走到目標(biāo)點(diǎn)即可。由于目標(biāo)直線可達(dá)位置有無數(shù)個，最佳的選擇是直線上離目標(biāo)位置最近的點(diǎn)。同時，考慮到車庫周圍的障礙物地圖可能并不是一個長方形，而且最終目標(biāo)角度的反向延長線可能會與障礙物發(fā)生碰撞，因此不宜直接以車庫周圍障礙物的對稱軸或目標(biāo)方向?yàn)橹本€，而是以最不可能發(fā)生碰撞的角度為準(zhǔn)?；谝陨峡紤]，本改進(jìn)的具體步驟如下：

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

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

c.用動態(tài)窗口規(guī)劃使小車走到第1階段目標(biāo)點(diǎn)。

d.沿直線規(guī)劃走到最終目標(biāo)點(diǎn)，調(diào)整角度到最終目標(biāo)角度。

3 仿真實(shí)驗(yàn)結(jié)果

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

圖4 動態(tài)窗口規(guī)劃軌跡

圖5 改進(jìn)算法第1階段規(guī)劃軌跡

圖6 改進(jìn)算法最終軌跡

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

4 結(jié)束語

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