李澤寰，肖美華

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

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

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

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

1 基于VFH算法的運(yùn)動(dòng)規(guī)劃

1.1 總體設(shè)計(jì)

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

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

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

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

(1)

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

(2)

經(jīng)過(guò)二值化處理后，障礙強(qiáng)度值為0的方向?yàn)榭尚旭偡较?，把求出的所有可行駛方向稱為集合A，但是集合A中的候選可行駛方向依然太多，不能指出最優(yōu)行駛方向，為此引入代價(jià)函數(shù)[13-15]，求出各候選方向的代價(jià)值，代價(jià)最小的為行駛方向，如式(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)為候選方向與上一次運(yùn)動(dòng)方向的夾角。μ1、μ2、μ3為常數(shù)且滿足μ1>μ2+μ3。比較各候選方向的代價(jià)值，代價(jià)值最小的為最優(yōu)行駛方向。

上述方法求出的最優(yōu)行駛方向有圖5所示問(wèn)題，圖中箭頭所指的AGV最優(yōu)方向是沒(méi)有障礙物的，但是因?yàn)锳GV自身長(zhǎng)寬影響，AGV無(wú)法通過(guò)這個(gè)路口，這是因?yàn)榇鷥r(jià)函數(shù)沒(méi)有考慮候選方向、AGV自身長(zhǎng)寬、障礙物三者間的關(guān)系。

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

(4)

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

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

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

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

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

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

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

V={a,b,c,d}

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

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

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

φ=vfh_aimR-vfh_aim_N

(5)

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

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

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

(6)

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

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

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

節(jié)點(diǎn)坐標(biāo)的精準(zhǔn)度決定AGV是否能安全運(yùn)行，VFH算法是通過(guò)激光雷達(dá)的數(shù)據(jù)來(lái)決定AGV的行駛方向，但是激光雷達(dá)的數(shù)據(jù)是以局部角度輸出的[17-18]，如圖10(a)，為此需要把激光雷達(dá)的角度信息與IMU測(cè)出的姿態(tài)參數(shù)進(jìn)行轉(zhuǎn)換，得出激光雷達(dá)對(duì)應(yīng)的全局極坐標(biāo)角度，也就是集合A里的候選行駛方向。IMU測(cè)出的姿態(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測(cè)出的航向角，Global為激光雷達(dá)轉(zhuǎn)換后的角度。經(jīng)過(guò)式(7)的計(jì)算，激光雷達(dá)的角度轉(zhuǎn)換為了全局極坐標(biāo)角度。

3.2 生成節(jié)點(diǎn)

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

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

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

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

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

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

4 結(jié)語(yǔ)

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