馮保壯，龔元明，潘敬宇

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

0 引言

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

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

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

1 總體方案設(shè)計(jì)

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

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

1.1 系統(tǒng)組成

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

1.2 工作原理

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

2 硬件電路

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

2.1 電源模塊

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

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

2.2 電機(jī)驅(qū)動(dòng)模塊

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

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

2.3 UWB 定位模塊

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

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

3 定位系統(tǒng)

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

3.1 定位計(jì)算

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

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

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

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

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

3.2 地圖編碼

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

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

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

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

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

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

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

4.1 算法原理

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

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

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

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

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

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

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

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

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

4.2 程序設(shè)計(jì)

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

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

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

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

如圖9 所示，系統(tǒng)以由位置信息和目標(biāo)點(diǎn)確定的車身方位角作為輸入，以由位姿傳感器測(cè)量的方位角作為輸出，通過(guò)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)前測(cè)得的位置角；u（k）——經(jīng)過(guò)PID 環(huán)節(jié)得出的小車兩側(cè)的PWM 修正值。PID 算法公式為

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

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

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

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

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

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

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

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

4.4 串口通信設(shè)計(jì)

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

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

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

5 結(jié)語(yǔ)

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