基于超寬帶定位的自動巡航小車設計

付仁杰，楊亞莉

(上海工程技術大學 機械與汽車工程學院，上海201620)

0 引 言

隨著機器人技術和自動化技術的發(fā)展，具有循跡功能的自動導引運輸車在工業(yè)生產(chǎn)過程中得到了大量的應用，其中應用最為廣泛的有光電式循跡小車以及電磁式循跡小車。這2種方式的循跡小車在實際運用中部署都相對復雜。光電式循跡小車需要預先在地面上印刷黑色引導線，而電磁式循跡小車需要在地下鋪設磁針，一旦軌跡設定好就不能變動，因此應用的靈活性較差。曾貴苓［1］等利用定位算法獲取小車實時位置，采用傳感器實現(xiàn)小車的傳感單元，實現(xiàn)小車各種模式下的智能搬運工作；陳雷［2］等對RFID定位系統(tǒng)以及車間RSSI算法模型進行了系統(tǒng)研究，開發(fā)車間定位原型系統(tǒng)，對實驗測試結果分析較好；王世峰［3］等以AVR單片機為控制樞紐，利用硬件及電路設計對小車運動控制良好。

UWB是一種短距離無線通信技術，安全性能高，定位精度準確，被廣泛應用于精確的室內(nèi)定位［4、5］。UWB定位系統(tǒng)采用三維空間坐標來表示具體的空間位置，因此在UWB有效定位范圍內(nèi)的空間點都可以用坐標來表示［6］。同理，預先鋪設的小車行駛路徑也可用一系列的坐標點來表示，通過修改坐標點的集合就可以實現(xiàn)小車行駛路徑的重新設定［7］。

定位技術隨著科技的發(fā)展，廣泛應用在汽車、無人機以及先進設備領域當中。將GPS與通訊技術相結合，能夠實現(xiàn)實時監(jiān)測、報警、調(diào)度等功能［8］。

本設計中引入了位置坐標信息，采用一系列的坐標點構成的軌跡作為小車的引導路徑，運用UWB定位技術來指引小車按照指定路徑行駛。

1 系統(tǒng)硬件設計

1.1 總體設計方案

系統(tǒng)主要由定位基站以及自動巡航小車主體兩個部分組成。定位基站由3個UWB定位基站構成，能夠實現(xiàn)2D空間上的定位。自動巡航小車以車模為載體，主要功能模塊包括:電源模塊、電機驅動模塊、UWB定位標簽、姿態(tài)傳感器模塊、避障模塊、樹莓派3B模塊。其中電源模塊以及電機控制模塊同屬于擴展板。樹莓派3B模塊是小車的控制核心，通過UWB標簽以及姿態(tài)傳感器獲得小車的當前的位置信息以及姿態(tài)信息；根據(jù)給定的軌跡信息控制小車上電機的轉速，從而實現(xiàn)對小車的行駛軌跡的控制，最終實現(xiàn)小車根據(jù)指定軌跡巡航的功能。自動巡航小車系統(tǒng)的結構框圖如圖1所示。圖1中MPU9250為姿態(tài)傳感器，可以測量小車轉角、加速度、以及角速度等；HY-SRF05是超聲波傳感器模塊，系統(tǒng)中用作避障模塊，實現(xiàn)小車前方障礙物的檢測。

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

1.2 電源模塊

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

圖2 電源模塊電路圖Fig.2 Circuit diagram of power module

1.3 電機驅動模塊

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

圖3 直流電機驅動電路圖Fig.3 Dc motor drive circuit diagram

2 系統(tǒng)軟件設計

2.1 軟件的總體設計

軟件設計的主要任務包括:

（1）通過UART接口從UWB標簽讀取其到UWB各基站之間的距離信息，并根據(jù)距離信息計算出小車的位置；

（2）通過I2C接口從MPU9250模塊中讀取小車的車身方位角、車速等信息；

（3）實現(xiàn)自動加載路徑的功能，并結合PID控制算法實現(xiàn)自動巡航的功能；

（4）通過超聲波測距模塊實現(xiàn)避障功能；

（5）完成軟件的整體設計。

本設計中存在著大量的延時操作，因此不適合在主線程中同時處理上述操作。為保證整個系統(tǒng)的實時性和穩(wěn)定性，軟件設計采用多線程方式，本設計中共創(chuàng)建了兩個子線程分別完成各自的任務。主線程負責完成小車定位、路徑加載以及緊急控制等任務，子線程1主要完成PID控制算法以及小車的控制等任務，子線程2負責時刻檢測小車前方障礙物的任務。軟件的整體設計流程圖如圖4所示。

圖4 軟件整體設計流程圖Fig.4 Flow chart of overall software design

2.2 巡航功能設計

2.2.1 UWB定位

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

圖5 三邊定位法原理圖Fig.5 Schematic diagram of three-side positioning method

由上述幾何關系可以得出公式（1）:

采用最小二乘法求解上述方程可求得D點坐標公式（2）:

2.2.2 PID控制算法

本設計中小車以路標點為指引向前行駛，但行駛的過程中存在不確定的因素使小車偏離原定方向，因此需要引入閉環(huán)控制來時刻修正小車的行駛方向。PID控制器是自動控制系統(tǒng)中應用最為廣泛的閉環(huán)控制器，而在計算機控制系統(tǒng)中通常采用數(shù)字PID控制器。數(shù)字型PID控制器的算法分為位置式PID控制算法和增量式PID控制算法。

本設計首先通過UWB定位標簽獲取小車的位置，再結合目標位置計算得到小車的目標位置角，以目標位置角作為輸入，以位置傳感器測得的位置角作為反饋數(shù)據(jù)，采用增量式PID控制算法控制小車兩側的PWM輸出值，使小車能夠跟隨目標點行駛。整個閉環(huán)控制系統(tǒng)的系統(tǒng)框圖如圖6所示。

圖6 PID控制系統(tǒng)框圖Fig.6 PID control system block diagram

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

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

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

本設計中引入位置坐標信息，采用坐標信息表示實際的物理位置，采用一系列的位置坐標構成的軌跡作為小車的指定行駛路徑，通過UWB定位技術來獲取小車的位置信息。巡航功能的實現(xiàn)過程:

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

（2）計算小車車身的目標方位角，計算公式（4）為:

其中，θ是目標方位角；xt、yt為目標位置坐標；xc、yc為當前位置的坐標；

（3）獲取當前小車車身方位角，并通過PID算法控制小車前進；

（4）判斷是否到達目標點，若到達則加載下一個路徑點并重復執(zhí)行（2）（3）過程，否則重復執(zhí)行（4）。具體的流程如圖4中（a）主線程和（b）子線程1所示。

2.3 避障功能設計

由于不能保證小車在巡航過程中其行進路徑上不存在障礙物，設計中需添加避障功能來保證小車的行駛安全。本設計中采用超聲波傳感器來進行障礙物檢測，小車在運行過程中通過超聲波傳感器時刻檢測小車前方物體，當小車前方安全距離內(nèi)出現(xiàn)障礙物，避障功能模塊就會向主控模塊發(fā)出避障提示，主控模塊響應避障操作。避障功能的流程設計如圖4中（c）子線程2。

3 實驗測試

本實驗主要對小車的定軌巡航功能進行測試，主要測試了“O”型和“S”型2種指定軌跡下小車的運行情況。首先，通過小車上的位置記錄程序記錄小車所經(jīng)過空間點坐標；其次，將上述空間點導入到二維平面地圖上；最后，將所有空間點連接繪制成軌跡圖，如圖7和圖8所示。

圖7 “O”型軌跡測試結果圖Fig.7 Test results of the＂O＂trajectory

圖8 “S”型軌跡測試結果圖Fig.8 Test results of the＂S＂trajectory

圖7和圖8中虛線表示目標軌跡，實線表示實際運行軌跡。對比目標軌跡和實際運行軌跡可以看出所設計的小車能夠實現(xiàn)定軌巡航的功能，但局部有較大的波動出現(xiàn)，其產(chǎn)生的主要原因有兩點:

（1）UWB定位時引入噪聲干擾使得所獲得的位置信息存在著波動，因此需要對UWB的定位算法進行優(yōu)化；

（2）PID控制算法的比例、積分、微分環(huán)節(jié)系數(shù)選擇不當，可考慮用模糊PID控制算法來取代增量式PID控制算法。

4 結束語

本文針對傳統(tǒng)循跡小車系統(tǒng)在部署和應用中存在的不足，提出并設計了一種能夠按照指定路徑自動巡航的小車系統(tǒng)。設計中引入了位置坐標信息，采用一系列位置坐標構成的軌跡作為小車的引導路徑，通過UWB定位技術來引導小車追蹤指定路徑。本設計采用樹莓派3B模塊做為主控單元，運用增量式PID控制算法來控制小車追蹤路徑，通過MPU9250模塊獲取小車的速度、方向等信息，通過HY-SRF05超聲波傳感器模塊實現(xiàn)避障功能。測試結果表明，所設計小車系統(tǒng)能夠實現(xiàn)定軌巡航的功能，通過各個模塊和算法控制，可以實現(xiàn)設計的目標，該小車系統(tǒng)具有一定實際應用價值，能夠自動準確靈活的按指定路徑達到指定位置。