李澤寰，肖美華

(華東交通大學(xué)軟件學(xué)院，江西 南昌 330013)

隨著機(jī)器人技術(shù)逐漸成熟且成本降低，越來越多公司開始使用工業(yè)機(jī)器人進(jìn)行生產(chǎn)，以提高產(chǎn)能，首先就是對自動導(dǎo)引車(Automated Guided Vehicle，AGV)的使用。AGV屬于一種移動運輸機(jī)器人，在工業(yè)上應(yīng)用廣泛，但是普通AGV在應(yīng)對環(huán)境改變時不能做出良好的反應(yīng)，并且AGV產(chǎn)品在進(jìn)入生產(chǎn)車間或其它工作場所后需要專業(yè)人員進(jìn)行專業(yè)調(diào)試，極大影響了工廠產(chǎn)品生產(chǎn)，亟需增強(qiáng)AGV的環(huán)境適應(yīng)能力以及縮短AGV進(jìn)廠測試時間。在當(dāng)前工廠環(huán)境下，AGV部署存在以下問題：1.AGV進(jìn)廠工作后需要專業(yè)人員進(jìn)行測試和調(diào)試；2.靈活性較差，如果工廠需要對生產(chǎn)環(huán)境進(jìn)行改變,需要專業(yè)人員對AGV進(jìn)行調(diào)整；3.市場上大規(guī)模使用的電磁導(dǎo)引和磁帶導(dǎo)引AGV雖然可靠性高，但是需要對場地進(jìn)行大規(guī)模改造，并且需要定時對場地進(jìn)行維護(hù)；4.市場上普遍使用的AGV光學(xué)引導(dǎo)方法對場地的光學(xué)環(huán)境要求較高。上述問題終究是AGV自動化不足的問題，許多開發(fā)人員為了增強(qiáng)AGV自動控制能力，通常使用即時定位與建圖算法(Simultaneous Localization And Mapping，SLAM)來實現(xiàn)智能AGV對環(huán)境的適應(yīng)[1]。SLAM是如今常用的一種導(dǎo)航和建圖方法，SLAM可以通過激光雷達(dá)或深度攝像頭準(zhǔn)確構(gòu)建地圖[2]，極大提高了機(jī)器人的環(huán)境感知能力，但是SLAM需要大量的硬件資源作為支撐，而且現(xiàn)實情況下的工廠環(huán)境相對簡單，并不需要構(gòu)建一張如此詳細(xì)的地圖，SLAM構(gòu)建地圖的速度也過于緩慢，這不利于AGV的快速部署。

工廠環(huán)境下需要的是一種低資源且高效可靠的運動規(guī)劃和建圖算法。向量直方圖法(Vector Field Histogram，VFH)是Johann Borenstein和Yoram Koren提出的一種實時運動規(guī)劃算法[3]。VFH算法的主要思想是量化機(jī)器人當(dāng)前時刻各方向與障礙物的距離，根據(jù)量化出的障礙物強(qiáng)度值得出最優(yōu)運動方向[4-6]。拓?fù)涞貓D是一種描述點與線關(guān)系的地圖，其忽略大量的環(huán)境細(xì)節(jié)，能緊湊地表示AGV所在的環(huán)境信息[7]，正因如此，建立拓?fù)涞貓D所需要的硬件資源和計算力較小。

本文組織結(jié)構(gòu)如下：第1節(jié)介紹AGV軟件系統(tǒng)的總體設(shè)計，優(yōu)化VFH算法用于計算最優(yōu)行駛方向；第2節(jié)構(gòu)建拓?fù)涞貓D，應(yīng)用于AGV運動控制中；第3節(jié)對VFH+拓?fù)涞貓D的方案進(jìn)行驗證，分析AGV生成的節(jié)點信息，驗證結(jié)果表明該方案可以滿足AGV的快速部署；第4節(jié)為本文的結(jié)語。

1 基于VFH算法的運動規(guī)劃

1.1 總體設(shè)計

本研究目標(biāo)是使得AGV在室內(nèi)可以自動規(guī)劃路徑并同時構(gòu)建拓?fù)涞貓D，從而實現(xiàn)快速部署。在室內(nèi)，AGV面對地通常是相對固定的環(huán)境，所以使用激光雷達(dá)作為感知傳感器；激光雷達(dá)與慣性測量單元(Inertial-Measurement Unit,IMU)結(jié)合為自動導(dǎo)引車提供最優(yōu)路徑與全場定位[8]。

AGV系統(tǒng)包含上位機(jī)和下位機(jī)兩個部分。下位機(jī)由單片機(jī)(STM32F103)、IMU(MPU6050)、碼盤組成 ，用來控制AGV的運動，其中IMU與STM32使用IIC進(jìn)行通信；上位機(jī)由激光雷達(dá)和嵌入式開發(fā)板(Jetson Tk1)組成，用來規(guī)劃AGV的運行方向。上位機(jī)是基于ROS開發(fā)的，上位機(jī)通過串口與下位機(jī)、激光雷達(dá)通信，如圖1。AGV軟件系統(tǒng)整體設(shè)計流程如圖2。

1.2 動態(tài)路徑規(guī)劃

本文采用VFH算法對AGV進(jìn)行動態(tài)路徑規(guī)劃。VFH算法的核心思想是以移動機(jī)器人為中心建立極坐標(biāo)系，將障礙物對AGV的影響量化為AGV在每個極坐標(biāo)方向上的障礙物強(qiáng)度，以此構(gòu)建AGV在此位置的向量直方圖[9-12]，如圖3；向量直方圖可以反映出AGV周圍的環(huán)境情況，通過在向量直方圖中設(shè)置閾值可以判斷出AGV的可通行方向。在軟件設(shè)計上，首先要做得是計算障礙強(qiáng)度值。障礙強(qiáng)度計算公式如式(1)。

(1)

針對距離閾值dth計算出障礙強(qiáng)度閾值M，M的取值關(guān)系到AGV的運行方向；在VFH算法中，選擇障礙強(qiáng)度值小于M值的方向作為備選方向，所以如果M值較大，可以較早的發(fā)現(xiàn)障礙物及時調(diào)整方向，如果M值較小可以提高備選方向的可靠性，使得AGV可以向著更安全的地方行駛，但是這樣會容易導(dǎo)致AGV進(jìn)入“死區(qū)”。因為AGV工作的環(huán)境相對穩(wěn)定，所以為了得到合適的閾值， AGV在可能需要通過的最窄路徑上測量四周的障礙強(qiáng)度值并觀測向量直方圖，在向量直方圖中選取最佳閾值。障礙強(qiáng)度值二值化處理求得可通行扇區(qū)直方圖，以此簡化數(shù)據(jù)處理難度，如式(2)與圖4。

(2)

經(jīng)過二值化處理后，障礙強(qiáng)度值為0的方向為可行駛方向，把求出的所有可行駛方向稱為集合A，但是集合A中的候選可行駛方向依然太多，不能指出最優(yōu)行駛方向，為此引入代價函數(shù)[13-15]，求出各候選方向的代價值，代價最小的為行駛方向，如式(3)

g(c)=μ1Δ(c,kt)+μ2Δ(c,kr)+μ3Δ(c,kb_pre)

(3)

在式(3)中，c為候選行駛方向，Δ(c,kt)為候選方向與目標(biāo)方向的夾角，Δ(c,kr)為候選方向與機(jī)器人當(dāng)前方向的夾角，Δ(c,kb_pre)為候選方向與上一次運動方向的夾角。μ1、μ2、μ3為常數(shù)且滿足μ1>μ2+μ3。比較各候選方向的代價值，代價值最小的為最優(yōu)行駛方向。

上述方法求出的最優(yōu)行駛方向有圖5所示問題，圖中箭頭所指的AGV最優(yōu)方向是沒有障礙物的，但是因為AGV自身長寬影響，AGV無法通過這個路口，這是因為代價函數(shù)沒有考慮候選方向、AGV自身長寬、障礙物三者間的關(guān)系。

為了避免在AGV長寬影響下得出錯誤的候選方向，本文提出一種關(guān)聯(lián)相鄰方向的方法來剔除無效方向。如圖6，候選方向向左偏移θ得到左偏方向，同理得到右偏方向，計算候選方向與左右偏移方向之間存在的方向有多少存在于集合A中，如果左邊(右邊)存在于集合A中的方向小于3θ/5個，或者左偏方向(右偏方向)不在集合A中，那么此候選方向無效，不應(yīng)該和其它候選方向進(jìn)行代價比較，如式(4)。剔除集合A中的無效候選方向后，再進(jìn)行代價值計算，代價值最小的方向為最優(yōu)行駛方向，可以成功的避開障礙物，如圖7。

(4)

&&fL(θ)=1&&fR(θ)=1?Valid

2 基于拓?fù)涞貓D的建圖方法

2.1 拓?fù)涞貓D的建立

拓?fù)涞貓D是一種保持點與線相對位置關(guān)系而不必保持圖形形狀的抽象地圖[7,16]。

在本研究中，AGV在探索周圍環(huán)境時自動生成拓?fù)涔?jié)點，節(jié)點中包含此時AGV的全局坐標(biāo)和下一時刻的運動方向，在AGV探索完成周圍環(huán)境后產(chǎn)生的所有節(jié)點構(gòu)成拓?fù)涞貓D。AGV在起點調(diào)用拓?fù)涞貓D，根據(jù)拓?fù)涞貓D上的信息，AGV可以在相鄰節(jié)點間移動。圖8為實驗場所示意圖。

在圖8所示環(huán)境中，AGV可以生成四個節(jié)點，設(shè)這四個節(jié)點為a、b、c、d，拓?fù)涞貓D為圖9所示。

節(jié)點中包含此點的全局坐標(biāo)、此點與相鄰節(jié)點的方位信息和此時速度，AGV正常工作時就檢索儲存的節(jié)點，根據(jù)節(jié)點信息AGV可以獲取移動到下一點處需要的參數(shù)。拓?fù)涞貓D符號化為G=(V,E)，其中節(jié)點集合表示為：

V={a,b,c,d}

連接節(jié)點的邊的集合表示為：

E={(a,b),(b,a),(b,c),(c,b),(c,d),(d,c)}

拓?fù)涔?jié)點的生成是通過判斷AGV行駛方向突變程度的高低。如圖7的拐角，AGV需要左拐進(jìn)入一條新的通道；在此環(huán)境下，AGV偏左的方向必然比其它方向更加空曠，因此計算系統(tǒng)得出下一刻行駛角度將在此刻行駛方向上向左偏，環(huán)境中拐角越大，兩個時刻間的偏移量就越大，根據(jù)以上規(guī)律得出生成拓?fù)涔?jié)點的基本原理為：通過偏移量的大小判斷是否到達(dá)關(guān)鍵節(jié)點。關(guān)鍵節(jié)點在存儲到ROM前需要和已確定的節(jié)點進(jìn)行模板匹配，確保存儲的節(jié)點為新節(jié)點。如式(5)。

φ=vfh_aimR-vfh_aim_N

(5)

式(5)中，vfh_aimR是當(dāng)前行駛方向，vfh_aimN為下一刻行駛方向，φ為偏移量，Q為一常量，point是采集到的節(jié)點，Points是拓?fù)涔?jié)點集合，ma表示新得到的節(jié)點在節(jié)點集中進(jìn)行匹配，ma(point)=1表示匹配成功，此節(jié)點為舊節(jié)點，ma(point)=0表示匹配失敗，此節(jié)點為新節(jié)點。

2.2 拓?fù)涞貓D的使用

為了便于控制，引入鄰接矩陣來表示點與邊的關(guān)系，當(dāng)AGV需要從拓?fù)涞貓D中的一個節(jié)點移動到另一個節(jié)點時，只需要改變鄰接矩陣中的參數(shù)，鄰接矩陣的行列由拓?fù)涞貓D決定。在本研究環(huán)境里，取4×4鄰接矩陣AG，通過給各條邊賦予不同的權(quán)值來區(qū)分行駛方向和阻礙方向，這里將行駛方向的權(quán)值設(shè)為2，阻礙方向的權(quán)值設(shè)為1。

(6)

如式(6)所示的鄰接矩陣表示了AGV從節(jié)點a移動到節(jié)點d的一個過程：AGV從a開始運動，先移動到b再從b移動到c，從c移動到d，在d停止運動。拓?fù)涞貓D與鄰接矩陣的結(jié)合使得AGV復(fù)雜的運動控制抽象為簡單的參數(shù)修改，從而實現(xiàn)AGV在建立完成地圖后，工作人員可以快速定義AGV的行駛路線。

鄰接矩陣設(shè)置AGV可行駛方向，而在控制細(xì)節(jié)上，由復(fù)合PI控制器來實現(xiàn)AGV在極坐標(biāo)上的行進(jìn)和轉(zhuǎn)向，具體流程為：鄰接矩陣決定AGV行駛方向，復(fù)合PI控制器接收鄰接矩陣對應(yīng)的拓?fù)涔?jié)點信息，在控制器收到節(jié)點信息后，AGV先轉(zhuǎn)到命令角度，再向著這個角度前進(jìn)。如此控制模式的好處就是，上位機(jī)只需修改此時鄰接矩陣的信息，就可以控制AGV的運動，這種方法降低了上位機(jī)與下位機(jī)的通信成本和控制復(fù)雜度?？刂破鲗崿F(xiàn)如式(7)。

(7)

BiasL=EncoderL-left

BiasR=EncoderR-right

left_pwm+=P×(BiasL-Last_biasL)+I×BiasL

right_pwm+=P×(BiasR-Last_biasR)+I×BiasR

式(8)中，left與right為左輪和右輪的目標(biāo)轉(zhuǎn)速，angle為當(dāng)前航向角和目標(biāo)角度的間隔，EncoderL與EncoderR為當(dāng)前左輪和右輪的轉(zhuǎn)速，BiasL與BiasR為當(dāng)前電機(jī)轉(zhuǎn)速和目標(biāo)轉(zhuǎn)速的差值，left_pwm與right_pwm為左右輪的pwm值。

3 實驗與驗證

3.1 節(jié)點坐標(biāo)轉(zhuǎn)換

節(jié)點坐標(biāo)的精準(zhǔn)度決定AGV是否能安全運行，VFH算法是通過激光雷達(dá)的數(shù)據(jù)來決定AGV的行駛方向，但是激光雷達(dá)的數(shù)據(jù)是以局部角度輸出的[17-18]，如圖10(a)，為此需要把激光雷達(dá)的角度信息與IMU測出的姿態(tài)參數(shù)進(jìn)行轉(zhuǎn)換，得出激光雷達(dá)對應(yīng)的全局極坐標(biāo)角度，也就是集合A里的候選行駛方向。IMU測出的姿態(tài)參數(shù)為航向角、橫滾角、俯仰角，在這里只需要使用航向角，利用航向角建立極坐標(biāo)如圖10(b)所示。

激光雷達(dá)角度轉(zhuǎn)換為全局極坐標(biāo)角度的原理為：一組環(huán)形序列映射到極坐標(biāo)上后，再加上IMU上的AGV偏移量，如式(8)。

Global=yaw+Lida

(8)

式(8)中，Lida為激光雷達(dá)的角度，yaw為IMU測出的航向角，Global為激光雷達(dá)轉(zhuǎn)換后的角度。經(jīng)過式(7)的計算，激光雷達(dá)的角度轉(zhuǎn)換為了全局極坐標(biāo)角度。

3.2 生成節(jié)點

本研究中使用Nvidia的Jetson TK1嵌入式開發(fā)板作為AGV的上位機(jī)，下位機(jī)使用了STM32F103ZET6開發(fā)板，激光雷達(dá)選用了HLS-LFCD2激光雷達(dá)，IMU使用的是MPU6050，串口模塊使用了CP2102 USB轉(zhuǎn)串口模塊，電機(jī)為減數(shù)比30的帶編碼器電機(jī)，圖11為團(tuán)隊開發(fā)的AGV。

實驗環(huán)境為圖8所示，此環(huán)境得到的節(jié)點數(shù)據(jù)為表1。表1中的數(shù)據(jù)為節(jié)點全局極坐標(biāo)映射出的全局直角坐標(biāo)，通過數(shù)據(jù)點可以獲得AGV的運動軌跡和各節(jié)點時的運動狀態(tài)。

表1 節(jié)點數(shù)據(jù)

表1的結(jié)果表示4個節(jié)點可以抽象出該環(huán)境，節(jié)點1為初始節(jié)點，它的方向沒有被記錄，一般來說，初始節(jié)點的方向為0或者通過節(jié)點2的位置求出；節(jié)點4為終止節(jié)點，因為AGV在節(jié)點4停止探索，所以節(jié)點4方向設(shè)為0。利用表中的節(jié)點數(shù)據(jù)，AGV在圖8中的環(huán)境下可以快速移動到任意目標(biāo)節(jié)點。

本文與文獻(xiàn)[1]比較。文獻(xiàn)[1]使用了基于3D Orbbe攝像機(jī)的視覺SLAM方法為機(jī)器人導(dǎo)航，構(gòu)建的地圖如圖12所示。

此地圖比較詳細(xì)地展現(xiàn)出測試場地的環(huán)境，但是相對的，高精度往往伴隨著建圖的低速度和高資源消耗。文獻(xiàn)[1]使用配置為i5 CPU、8GB RAM、GTX970顯卡的個人電腦實現(xiàn)視覺SLAM，并且為了實現(xiàn)物體的識別，花費了7天的時間訓(xùn)練模型；所以和視覺SLAM相比，本文提出的AGV快速部署方案更能在低資源的情況下，把AGV快速部署到工廠。

4 結(jié)語

本文為解決AGV快速部署問題設(shè)計了一種新的建圖方案。該方案以VFH算法為基礎(chǔ)進(jìn)行路徑規(guī)劃，在VFH算法中加入AGV自身大小約束，確保AGV可以通過各障礙物，AGV探索四周環(huán)境時生成拓?fù)涔?jié)點形成拓?fù)涞貓D，最終通過拓?fù)涞貓D部署在固定場地?？刂品矫?，建立極坐標(biāo)系，以輸出角度的方式控制AGV的行駛方向。實驗結(jié)果表明該設(shè)計方案可行且高效，可以滿足預(yù)期要求，但AGV建立節(jié)點的方式過于單一，節(jié)點只能表示環(huán)境中的岔口和拐點，未來的工作將把重點放在如何增加節(jié)點信息多樣性方面。