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

付仁杰，楊亞莉

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

0 引 言

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

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

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

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

1 系統硬件設計

1.1 總體設計方案

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

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

1.2 電源模塊

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

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

1.3 電機驅動模塊

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

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

2 系統軟件設計

2.1 軟件的總體設計

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

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

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

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

（4）通過超聲波測距模塊實現避障功能；

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

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

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

2.2 巡航功能設計

2.2.1 UWB定位

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

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

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

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

2.2.2 PID控制算法

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

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

圖6 PID控制系統框圖Fig.6 PID control system block diagram

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

其中，e（k）為第k個采樣時刻的誤差角；T為采樣周期；kp為比例環節系數；Ti為積分環節系數；Td為微分環節系數。

2.2.3 巡航功能的實現

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

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

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

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

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

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

2.3 避障功能設計

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

3 實驗測試

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

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

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

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

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

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

4 結束語

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