基于超寬帶定位的自動(dòng)巡航小車設(shè)計(jì)

付仁杰，楊亞莉

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

0 引 言

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

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

定位技術(shù)隨著科技的發(fā)展，廣泛應(yīng)用在汽車、無(wú)人機(jī)以及先進(jìn)設(shè)備領(lǐng)域當(dāng)中。將GPS與通訊技術(shù)相結(jié)合，能夠?qū)崿F(xiàn)實(shí)時(shí)監(jiān)測(cè)、報(bào)警、調(diào)度等功能［8］。

本設(shè)計(jì)中引入了位置坐標(biāo)信息，采用一系列的坐標(biāo)點(diǎn)構(gòu)成的軌跡作為小車的引導(dǎo)路徑，運(yùn)用UWB定位技術(shù)來(lái)指引小車按照指定路徑行駛。

1 系統(tǒng)硬件設(shè)計(jì)

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

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

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

1.2 電源模塊

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

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

1.3 電機(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)芯片，該芯片可提供兩路電機(jī)驅(qū)動(dòng)輸出，通過對(duì)使能引腳以及PWM輸入引腳的控制可以實(shí)現(xiàn)電機(jī)的啟停以及轉(zhuǎn)速的控制。L289芯片的輸入引腳與樹莓派GPIO相連，樹莓派3B模塊內(nèi)置有PWM發(fā)生器，最多可同時(shí)提供兩路PWM輸出，直流電機(jī)驅(qū)動(dòng)電路圖如圖3所示。

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

2 系統(tǒng)軟件設(shè)計(jì)

2.1 軟件的總體設(shè)計(jì)

軟件設(shè)計(jì)的主要任務(wù)包括:

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

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

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

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

（5）完成軟件的整體設(shè)計(jì)。

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

圖4 軟件整體設(shè)計(jì)流程圖Fig.4 Flow chart of overall software design

2.2 巡航功能設(shè)計(jì)

2.2.1 UWB定位

UWB定位通常采用3邊測(cè)量法，原理如圖5所示，以3個(gè)節(jié)點(diǎn)A、B、C為圓心作圓，坐標(biāo)分別為（Xa，Ya），（Xb，Yb），（Xc，Yc），這3個(gè)圓周相交于一點(diǎn)D，交點(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 Schematic diagram of three-side positioning method

由上述幾何關(guān)系可以得出公式（1）:

采用最小二乘法求解上述方程可求得D點(diǎn)坐標(biāo)公式（2）:

2.2.2 PID控制算法

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

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

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

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

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

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

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

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

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

其中，θ是目標(biāo)方位角；xt、yt為目標(biāo)位置坐標(biāo)；xc、yc為當(dāng)前位置的坐標(biāo)；

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

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

2.3 避障功能設(shè)計(jì)

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

3 實(shí)驗(yàn)測(cè)試

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

圖7 “O”型軌跡測(cè)試結(jié)果圖Fig.7 Test results of the＂O＂trajectory

圖8 “S”型軌跡測(cè)試結(jié)果圖Fig.8 Test results of the＂S＂trajectory

圖7和圖8中虛線表示目標(biāo)軌跡，實(shí)線表示實(shí)際運(yùn)行軌跡。對(duì)比目標(biāo)軌跡和實(shí)際運(yùn)行軌跡可以看出所設(shè)計(jì)的小車能夠?qū)崿F(xiàn)定軌巡航的功能，但局部有較大的波動(dòng)出現(xiàn)，其產(chǎn)生的主要原因有兩點(diǎn):

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

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

4 結(jié)束語(yǔ)

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