基于機器人操作系統和向量場直方圖的自動導引車快速部署方案設計

李澤寰，肖美華

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

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

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

本文組織結構如下：第1節介紹AGV軟件系統的總體設計，優化VFH算法用于計算最優行駛方向；第2節構建拓撲地圖，應用于AGV運動控制中；第3節對VFH+拓撲地圖的方案進行驗證，分析AGV生成的節點信息，驗證結果表明該方案可以滿足AGV的快速部署；第4節為本文的結語。

1 基于VFH算法的運動規劃

1.1 總體設計

本研究目標是使得AGV在室內可以自動規劃路徑并同時構建拓撲地圖，從而實現快速部署。在室內，AGV面對地通常是相對固定的環境，所以使用激光雷達作為感知傳感器；激光雷達與慣性測量單元(Inertial-Measurement Unit,IMU)結合為自動導引車提供最優路徑與全場定位[8]。

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

1.2 動態路徑規劃

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

(1)

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

(2)

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

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

(3)

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

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

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

(4)

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

2 基于拓撲地圖的建圖方法

2.1 拓撲地圖的建立

拓撲地圖是一種保持點與線相對位置關系而不必保持圖形形狀的抽象地圖[7,16]。

在本研究中，AGV在探索周圍環境時自動生成拓撲節點，節點中包含此時AGV的全局坐標和下一時刻的運動方向，在AGV探索完成周圍環境后產生的所有節點構成拓撲地圖。AGV在起點調用拓撲地圖，根據拓撲地圖上的信息，AGV可以在相鄰節點間移動。圖8為實驗場所示意圖。

在圖8所示環境中，AGV可以生成四個節點，設這四個節點為a、b、c、d，拓撲地圖為圖9所示。

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

V={a,b,c,d}

連接節點的邊的集合表示為：

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

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

φ=vfh_aimR-vfh_aim_N

(5)

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

2.2 拓撲地圖的使用

為了便于控制，引入鄰接矩陣來表示點與邊的關系，當AGV需要從拓撲地圖中的一個節點移動到另一個節點時，只需要改變鄰接矩陣中的參數，鄰接矩陣的行列由拓撲地圖決定。在本研究環境里，取4×4鄰接矩陣AG，通過給各條邊賦予不同的權值來區分行駛方向和阻礙方向，這里將行駛方向的權值設為2，阻礙方向的權值設為1。

(6)

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

鄰接矩陣設置AGV可行駛方向，而在控制細節上，由復合PI控制器來實現AGV在極坐標上的行進和轉向，具體流程為：鄰接矩陣決定AGV行駛方向，復合PI控制器接收鄰接矩陣對應的拓撲節點信息，在控制器收到節點信息后，AGV先轉到命令角度，再向著這個角度前進。如此控制模式的好處就是，上位機只需修改此時鄰接矩陣的信息，就可以控制AGV的運動，這種方法降低了上位機與下位機的通信成本和控制復雜度?？刂破鲗崿F如式(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為左輪和右輪的目標轉速，angle為當前航向角和目標角度的間隔，EncoderL與EncoderR為當前左輪和右輪的轉速，BiasL與BiasR為當前電機轉速和目標轉速的差值，left_pwm與right_pwm為左右輪的pwm值。

3 實驗與驗證

3.1 節點坐標轉換

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

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

Global=yaw+Lida

(8)

式(8)中，Lida為激光雷達的角度，yaw為IMU測出的航向角，Global為激光雷達轉換后的角度。經過式(7)的計算，激光雷達的角度轉換為了全局極坐標角度。

3.2 生成節點

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

實驗環境為圖8所示，此環境得到的節點數據為表1。表1中的數據為節點全局極坐標映射出的全局直角坐標，通過數據點可以獲得AGV的運動軌跡和各節點時的運動狀態。

表1 節點數據

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

本文與文獻[1]比較。文獻[1]使用了基于3D Orbbe攝像機的視覺SLAM方法為機器人導航，構建的地圖如圖12所示。

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

4 結語

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