基于動態(tài)地圖的智能巡航小車設(shè)計

馮保壯，龔元明，潘敬宇

（201620 上海市 上海工程技術(shù)大學(xué) 機械與汽車工程學(xué)院）

0 引言

目前大多數(shù)自動化工場、自動化立庫使用傳統(tǒng)的固定路徑引導(dǎo)的 AGV 已逐漸不能滿足物聯(lián)時代智能化需求及信息化管理模式，轉(zhuǎn)而需要自動化程度更高的自主規(guī)劃路徑的AGV[1]。

目前，國內(nèi) AGV 技術(shù)主要有2 種發(fā)展模式：一是引進(jìn)模仿再創(chuàng)新；二是依托科研院所、大學(xué)、企業(yè)等體系自主研發(fā)。蘇州大學(xué)的肖曉晴[2]研究了適用于移動物體的動態(tài)連續(xù)跟蹤與定位的基于卡爾曼濾波的動態(tài)融合定位算法；電子科技大學(xué)的顧靈茹[3]根據(jù)UWB 和慣性導(dǎo)航系統(tǒng)（INS）的優(yōu)勢與不足,將兩種技術(shù)組合以實現(xiàn)優(yōu)勢互補；中國科學(xué)技術(shù)大學(xué)的曹帥[4]以自行研制的低成本音頻室內(nèi)定位硬件平臺為設(shè)備基礎(chǔ)，面向?qū)嶋H應(yīng)用中的諸多復(fù)雜因素，開展了對音頻檢測算法、定位算法以及異常處理和可靠性判斷方法的研究。

針對傳統(tǒng)的固定路徑引導(dǎo)的 AGV 小車不確定環(huán)境適應(yīng)度低、多車協(xié)同難度大等問題，本文將研究如何實現(xiàn)在不確定環(huán)境中實現(xiàn)小車群體的智能巡航協(xié)同，在動態(tài)地圖更新、小車定位信息實時采集處理、巡航路徑制定及控制等方面進(jìn)行探討。

1 總體方案設(shè)計

在眾多室內(nèi)定位解決方案中，UWB 定位方法因其高帶寬、高分辨率、高傳輸速率、強魯棒性、低功耗以及成本低等優(yōu)勢，成為室內(nèi)定位中最有前景的解決方案之一[5]。如圖1所示，系統(tǒng)由定位基站、智能小車、自動巡航控制算法3 部分組成。其中定位基站由3 個UWB 定位基站模塊構(gòu)成；智能小車由底盤、車輪、減速器裝置、電機、樹莓派3B 模塊、電源模塊、鋰電池、電機驅(qū)動模塊、UWB 定位標(biāo)簽、位姿傳感器、超聲波傳感器組成；自動巡航控制算法實現(xiàn)功能包括定軌巡航和避障2 個部分。

圖1 自動巡航小車系統(tǒng)框圖Fig.1 Block diagram of automatic cruise car system

1.1 系統(tǒng)組成

智能小車的控制單元由樹莓派3B 模塊和一個擴展板構(gòu)成，擴展板上集成有電源模塊以及電機驅(qū)動模塊，樹莓派3B 模塊通過I/O 口控制電機驅(qū)動模塊的輸出。UWB 標(biāo)簽與樹莓派3B 模塊之間通過UART 接口進(jìn)行通信。HY-SRF05 模塊是超聲波傳感器，其由樹莓派3B 模塊的I/O 口控制。MPU9250 模塊是位姿傳感器模塊，其與樹莓派3B模塊之間通過I2C 接口進(jìn)行通信。定位基站由3 個UWB 模塊構(gòu)成，UWB 定位基站與UWB 定位標(biāo)簽共同實現(xiàn)定位的功能。

1.2 工作原理

在系統(tǒng)布設(shè)的區(qū)域建立位置坐標(biāo)系，在所述坐標(biāo)系中選取一系列位置點作為路標(biāo)點，并將所述坐標(biāo)點構(gòu)成的軌跡作為小車的巡航軌跡，通過小車上的UWB 標(biāo)簽來獲取小車的當(dāng)前位置信息，通過當(dāng)前位置與最近的路標(biāo)點構(gòu)成的方向向量來引導(dǎo)小車行駛，通過增量式PID 控制小車兩側(cè)電機的PWM輸入，實現(xiàn)對小車行駛方向和速度的控制，再結(jié)合相關(guān)的定軌巡航控制算法，小車按照指定軌跡行駛，最終實現(xiàn)自動巡航功能[5-6]。

2 硬件電路

智能巡航小車的硬件設(shè)計電路包括：電源模塊、UWB 定位模塊、電機驅(qū)動模塊、超聲波避障模塊等，本文針對主要電路設(shè)計展開研究。

2.1 電源模塊

小車采用12V 的鋰電池進(jìn)行供電，本設(shè)計中選用SY8008B 穩(wěn)壓芯片將12V 的輸入電壓降到5V供樹莓派模塊使用。SY8008B 穩(wěn)壓芯片能夠提供高達(dá)1.2A 的輸出電流，因此完全滿足樹莓派模塊的供電要求。直流電機直接采用12V 輸入電壓進(jìn)行供電。電源模塊的電路設(shè)計如圖2 所示。

圖2 電源模塊電路原理圖Fig.2 Schematic diagram of power module circuit

2.2 電機驅(qū)動模塊

一方面，為增強小車整體的驅(qū)動能力，本設(shè)計中選用4 個直流電機來驅(qū)動小車；另一方面為降低硬件控制的成本，小車同側(cè)的兩個直流電機采用同一路PWM 來控制。綜合上述因素，本設(shè)計選用L289 電機驅(qū)動芯片。L289 芯片可提供兩路電機驅(qū)動輸出，通過對使能引腳以及PWM 輸入引腳的控制可以實現(xiàn)電機的啟停以及轉(zhuǎn)速的控制。圖3 是電機驅(qū)動電路圖。圖3 中L289 芯片的輸入引腳與樹莓派GPIO 相連。樹莓派3B 模塊內(nèi)置有PWM 發(fā)生器，最多可同時提供兩路PWM 輸出。

圖3 直流電機驅(qū)動模塊電路圖Fig.3 DC motor drive module circuit diagram

2.3 UWB 定位模塊

本設(shè)計中，UWB 定位模塊選用DecaWave 公司研制的DW1000，通過SPI 與STM32 數(shù)據(jù)交互，由于DW1000 內(nèi)置上拉與下拉電阻，因此僅需將兩者引腳連接即可。電路原理圖參見圖4。

圖4 UWB 定位模塊電路原理圖Fig.4 Circuit schematic diagram of UWB positioning module

3 定位系統(tǒng)

巡航小車的定位系統(tǒng)是基于靜態(tài)地圖與動態(tài)地圖結(jié)合的全局動態(tài)地圖搭建的，可在樹莓派內(nèi)部存儲地圖數(shù)據(jù)，并供巡航控制系統(tǒng)調(diào)用。

3.1 定位計算

基于UWB 的定位技術(shù)通常采用測向和測距來實現(xiàn),按照其測量參數(shù)的不同可分為4 種方法：信號強度檢測法、到達(dá)角度定位法、到達(dá)時間定位法、到達(dá)時間差定位法[5-6]。

UWB 定位通常采用三邊測量法，三邊測量法的原理如圖5所示。以3個節(jié)點A、B、C為圓心作圓，坐標(biāo)分別為（Xa，Ya），（Xb，Yb），（Xc，Yc），這3 個圓周相交于D點，交點D即為移動節(jié)點，A、B、C為參考節(jié)點，A、B、C與交點D的距離分別為da、db、dc。

圖5 三邊定位法原理圖Fig.5 Principle diagram of trilateral positioning method

由上述幾何關(guān)系可以得到式（1）：

采用最小二乘法求解式（1）可得D點坐標(biāo)公式[7-9]：

3.2 地圖編碼

根據(jù)巡航小車移動區(qū)域大小，將場地坐標(biāo)系劃分成若干個小方格，用行列來標(biāo)識各個小方格區(qū)域，為了便于數(shù)據(jù)的存儲和發(fā)送，將坐標(biāo)系中劃分的方格分組，8個方格一組，可用一個8位二進(jìn)制數(shù)，即字節(jié)（Byte）來表示。當(dāng)某一方格區(qū)域存在其他小車或堆放其他物體時，該區(qū)域涂黑。各行暫時不可通過區(qū)域的分布可記作如圖6 所示的示意圖。

圖6 地圖編碼示意圖Fig.6 Schematic diagram of map coding

3.3 全局動態(tài)地圖

當(dāng)有多輛巡航小車時，不可通過的區(qū)域會動態(tài)變化，定位標(biāo)簽所在的小方格也會被涂黑。如圖7 所示，在記錄有靜態(tài)障礙物的靜態(tài)地圖基礎(chǔ)上增加不同巡航小車構(gòu)成的動態(tài)障礙物，實時更新形成針對每輛小車的全局動態(tài)地圖[10]。由此，再根據(jù)生產(chǎn)調(diào)度的優(yōu)先級，系統(tǒng)自動生成路徑規(guī)劃，多輛巡航小車將按照分配到的路書有序完成特定作業(yè)。

圖7 全局動態(tài)地圖流程圖Fig.7 Flow chart of global dynamic map

4 巡航控制系統(tǒng)

小車巡航行駛的路徑由一系列路標(biāo)點構(gòu)成，小車在行駛過程中的任何時刻都有且只有唯一的路標(biāo)點來指引（目標(biāo)點），當(dāng)小車到達(dá)目標(biāo)點之后就會加載下一個路標(biāo)點作為目標(biāo)點，重復(fù)上述過程就能實現(xiàn)按照指定軌跡巡航功能[11-13]。

4.1 算法原理

如圖8 所示，小車閉環(huán)控制系統(tǒng)的輸入是小車當(dāng)前位置和目標(biāo)位置構(gòu)成的方向向量角，其輸出是小車當(dāng)前車身的位置角。

圖8 巡航自動控制算法的程序流程圖Fig.8 Program flow chart of automatic cruise control algorithm

第1 步：將所有路標(biāo)點加入小車行駛路徑中；

第2 步：獲取小車的位置坐標(biāo)信息，并根據(jù)當(dāng)前位置信息以及行駛方向（順時針或逆時針）從路徑中選取相應(yīng)的路標(biāo)點作為目標(biāo)點；

第3 步：獲取小車當(dāng)前的位置信息以及小車車身的方位角信息，并由當(dāng)前位置和目標(biāo)位置信息計算小車車身的目標(biāo)方位角；

第4 步：求出小車目標(biāo)車身方位角與當(dāng)前車身方位角的差值信息，并根據(jù)差值信息通過增量式PID 控制算法來控制小車兩側(cè)電機的PWM 輸入，從而控制小車跟隨目標(biāo)點行駛；

第5 步：檢測小車前方是否有障礙物，如有則小車停止并執(zhí)行第7 步，否則執(zhí)行下一步；

第6 步：檢測小車是否到達(dá)目標(biāo)點，如果到達(dá)目標(biāo)點則加載下一個目標(biāo)點，重復(fù)步驟3—步驟6。

第7 步：檢測前方障礙物是否移除，如果移除則執(zhí)行第6 步，否則重復(fù)執(zhí)行第7 步。

4.2 程序設(shè)計

巡航功能自動控制方法的程序?qū)崿F(xiàn)采用多線程的方式。所述多線程包括主線程和兩個子線程，主線程負(fù)責(zé)完成小車定位、路徑加載以及緊急控制等任務(wù)。子線程1 主要完成PID 控制算法以及小車的控制等任務(wù)，子線程2 負(fù)責(zé)時刻檢測小車前方障礙物的任務(wù)。

主線程：首先初始化智能小車所用到的硬件設(shè)備，包括超聲波傳感器、位姿傳感器、電機驅(qū)動模塊等；然后初始化小車數(shù)據(jù)結(jié)構(gòu)體（主要用于線程之間的數(shù)據(jù)交互）；再將路標(biāo)點加載到小車的路徑中并等待啟動指令，啟動指令是由上位機通過WiFi 傳輸給下位機小車的；當(dāng)接收到啟動指令后，創(chuàng)建子線程1 和子線程2 分別用來完成各自的任務(wù)；接著獲取小車的當(dāng)前位置坐標(biāo)并判斷是否到達(dá)目標(biāo)點。如果到達(dá)則加載下一個目標(biāo)點，否則繼續(xù)上一步操作，最后等待終止信號到來；如果接收到終止信號則關(guān)閉子線程并進(jìn)入啟動信號的等待，否則重復(fù)執(zhí)行上一步操作。

子線程1：首先計算小車的目標(biāo)方位角并讀取車身的方位角，然后執(zhí)行PID 程序并調(diào)節(jié)小車兩側(cè)電機的PWM 輸入，接著讀取前方障礙物信息。當(dāng)沒有障礙物時，使能小車兩側(cè)電機的PWM 輸入。當(dāng)檢查到小車前方有障礙物或接收到終止信號時，將斷開小車兩側(cè)電機的PWM 輸入并退出子線程，否則繼續(xù)執(zhí)行上述過程。

子線程2：首先測量前方障礙物的距離，然后判斷是否小于安全距離，如果小于安全距離則修改障礙物警報信息，否則取消障礙物報警信息。最后等待線程終止信號的到來。如果接收到終止信號則退出子線程，否則重復(fù)執(zhí)行上述步驟。

如圖9 所示，系統(tǒng)以由位置信息和目標(biāo)點確定的車身方位角作為輸入，以由位姿傳感器測量的方位角作為輸出，通過PID 控制環(huán)節(jié)進(jìn)行修正使輸出始終跟隨輸入。

圖9 小車閉環(huán)控制系統(tǒng)的方框圖Fig.9 Block diagram of closed-loop control system of the car

圖9 中：r（k）——輸入的目標(biāo)位置角；c（k）——當(dāng)前測得的位置角；u（k）——經(jīng)過PID 環(huán)節(jié)得出的小車兩側(cè)的PWM 修正值。PID 算法公式為

式中：e（k）——第k個采樣時刻的誤差角；T——采樣周期；Kp——比例環(huán)節(jié)系數(shù)；Ti——積分環(huán)節(jié)系數(shù)；Td——微分環(huán)節(jié)系數(shù)。

4.3 巡航功能的實現(xiàn)

本設(shè)計引入位置坐標(biāo)信息，采用坐標(biāo)信息表示實際的物理位置，采用一系列位置坐標(biāo)構(gòu)成的軌跡作為小車的指定行駛路徑，通過UWB 定位技術(shù)獲取小車的位置信息。巡航功能的實現(xiàn)步驟如下：

（1）獲取小車的當(dāng)前位置并找到距離最近的路徑點作為下一個目標(biāo)點；

（2）計算小車車身的目標(biāo)方位角，公式為

式中：θ——目標(biāo)方位角；xt，yt——目標(biāo)位置坐標(biāo)；xc，yc——當(dāng)前位置的坐標(biāo)；

（3）獲取當(dāng)前小車車身方位角并通過PID 算法控制小車前進(jìn)；

（4）判斷是否到達(dá)目標(biāo)點，若到達(dá)則加載下一個路徑點并重復(fù)執(zhí)行步驟（2）、步驟（3），否則重復(fù)執(zhí)行步驟（4）。

4.4 串口通信設(shè)計

UWB 定位模塊獲取的定位數(shù)據(jù)通過串口ttyACM0 上傳，樹莓派通過串口工具minicom 接收，同一網(wǎng)段的工作人員可通過安全外殼協(xié)議（Secure Shell，SSH）遠(yuǎn)程登錄查看。在通信界面，選擇目標(biāo)小車的IP 端口號，確認(rèn)連接后，巡航小車的當(dāng)前位置、所處小方格區(qū)域、速度、任務(wù)終點等信息經(jīng)由WiFi 傳送至工作站，可根據(jù)需要對巡航小車進(jìn)行遠(yuǎn)程調(diào)度。

如圖10 所示，樹莓派接收來自UWB 定位模塊發(fā)送的數(shù)據(jù)流，包含定位標(biāo)簽和各個定位基站之間的原始距離和濾波校正后距離[14]。樹莓派后臺接收到距離數(shù)據(jù)后，調(diào)用Python程序進(jìn)行數(shù)據(jù)解析，轉(zhuǎn)發(fā)至通信界面，調(diào)度人員只需下達(dá)任務(wù)命令，即可實現(xiàn)多輛小車的智能巡航[15]。

圖10 小車遠(yuǎn)程控制通信界面Fig.10 Remote control communication interface of the car

5 結(jié)語

本文提出的基于超寬帶定位的智能巡航小車系統(tǒng)設(shè)計方案，針對一般循跡小車系統(tǒng)場地布置成本高、靈活性低等問題，在樹莓派的基礎(chǔ)上增加UWB 定位模塊和超聲波傳感避障模塊，建立了數(shù)字定位系統(tǒng)，實現(xiàn)虛擬電子軌跡導(dǎo)航。同時，根據(jù)任務(wù)需要，可依據(jù)全局動態(tài)地圖同時調(diào)度多輛巡航小車，實現(xiàn)小車多任務(wù)協(xié)同進(jìn)行。