激光導(dǎo)航小車(chē)入庫(kù)規(guī)劃

蔣如飛，張偉軍

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

0 引言

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

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

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

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

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

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

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

xagv=xbeam+ρcosθ

(1)

yagv=ybeam+ρsinθ

(2)

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

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

(3)

(4)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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