基于GPS和UWB混合定位的自動搜救機(jī)器人設(shè)計

游雪輝 張松

摘要：為解決人工搜救效率低且需耗費大量時間和資源的問題，提出基于GPS和UWB混合定位方式的自動搜救機(jī)器人方案。通過混合定位的方式，搜救機(jī)器人實現(xiàn)了給定目標(biāo)位置后，進(jìn)行自動路徑規(guī)劃和運動決策控制，自動從起點行駛至目標(biāo)點，為搜救機(jī)器人提供高精度的定位信息。在行進(jìn)過程中，搜救機(jī)器人使用無線數(shù)據(jù)傳輸方式將采集到的環(huán)境信息、圖像信息等發(fā)送到搜救站。樣機(jī)測試結(jié)果表明系統(tǒng)實現(xiàn)了預(yù)期的設(shè)計目標(biāo)，提高了搜救效率，縮短了搜救時間。

關(guān)鍵詞：GPS;UWB;搜救機(jī)器人;自動巡航

中圖分類號：TP242文獻(xiàn)標(biāo)志碼：A

文章編號：2095-5383（2019）02-0029-05

Abstract：At present，the search and rescue mission mainly relies on manual completion. However，manual search and rescue is often inefficient and takes a lot of time and resources.?? An automatic search and rescue robot based on GPS and UWB hybrid positioning was proposed in this paper. Through hybrid positioning，the search and rescue robot performs automatic path planning and motion decision control，and automatically travels from the starting point to the target point. During the travel process，the search and rescue robot sends the collected environmental information，image information，etc. to the search and rescue station using wireless data transmission.? The prototype test results show that the system achieves the expected design goals，improves search and rescue efficiency，and shortens search and rescue time.

Keywords： Global Positioning System; Ultra Wide-Band; search and rescue robot; automatic cruise

我國是一個自然災(zāi)害頻發(fā)的國家自然災(zāi)害發(fā)生后，搜救工作一般依靠人工進(jìn)行，由于災(zāi)后現(xiàn)場環(huán)境的復(fù)雜性和危險性不利于災(zāi)后救援工作的開展，大面積受災(zāi)時人工搜救效率低，所以結(jié)合現(xiàn)階段的搜救要求，設(shè)計一個能應(yīng)用于搜救工作的自動搜救機(jī)器人顯得至關(guān)重要。本文研究了搜救機(jī)器人的選型和系統(tǒng)設(shè)計方案，給出了自動搜救機(jī)器人的測試結(jié)果。

1 搜救機(jī)器人系統(tǒng)設(shè)計

1.1 搜救機(jī)器人設(shè)計與選型

設(shè)計之初，為保證搜救機(jī)器人能在復(fù)雜的路面自如前行，擬定了3個設(shè)計方案：

普通四輪搜救機(jī)器人：搜救工作需要經(jīng)過的路面往往不同于城市道路，四輪搜救機(jī)器人在通過泥濘和較大坑洼路面時會發(fā)生打滑、高強(qiáng)度抖動，甚至翻車的情況，因此普通四輪搜救機(jī)器人不適于此設(shè)計。

六足機(jī)器人：六足機(jī)器人基于仿生學(xué)原理，模擬六足昆蟲的機(jī)械結(jié)構(gòu)，通過控制多個舵機(jī)實現(xiàn)機(jī)器人的姿態(tài)控制。六足機(jī)器人能夠有效的克服路面不平坦而帶來的阻礙搜救設(shè)備前進(jìn)的問題。但六足機(jī)器人采用了多個獨立控制的舵機(jī)，加大了控制難度，其行進(jìn)速度較為緩慢，在實際救援過程中應(yīng)盡量縮短救援時間[1-2]。

履帶搜救機(jī)器人：履帶搜救機(jī)器人通過電機(jī)帶動齒輪，齒輪帶動履帶的方式使搜救機(jī)器人前行。若搜救機(jī)器人左側(cè)履帶的速度大于右側(cè)履帶的速度，搜救機(jī)器人實現(xiàn)右轉(zhuǎn)彎;當(dāng)右側(cè)履帶的速度大于左側(cè)履帶的速度，搜救機(jī)器人將實現(xiàn)左轉(zhuǎn)彎的功能。履帶車在山地能夠有效越過普通障礙物，即使在泥濘和坑洼路段也能自如前行，不會發(fā)生破胎等事故，并且履帶搜救機(jī)器人具有較快速度和較為容易的控制算法。因此本系統(tǒng)選用履帶搜救機(jī)器人設(shè)計，以提高搜救效率，降低人工搜救強(qiáng)度[3-4]。

1.2 搜救機(jī)器人系統(tǒng)設(shè)計

搜救人員將搜救機(jī)器人放置在道路上，并在搜救機(jī)器人上設(shè)置目標(biāo)點的位置信息。啟動搜救機(jī)器人后，搜救機(jī)器人開始和全球定位系統(tǒng)（Global Positioning System，GPS）衛(wèi)星進(jìn)行通信，獲取自己的位置信息。同時搜救機(jī)器人會初始化電子羅盤，擁有自身位置和目標(biāo)點位置信息，根據(jù)兩個位置進(jìn)行計算，求得偏航角。對比電子羅盤獲得的數(shù)據(jù)，搜救機(jī)器人旋轉(zhuǎn)修正航向角度。當(dāng)搜救機(jī)器人進(jìn)入到超帶寬（Ultra Wideband，UWB）[5]定位區(qū)域后，將開啟UWB定位功能，通過GPS+UWB的混合定位方式繼續(xù)獲取自身位置信息。搜救機(jī)器人在行進(jìn)過程中，位置會不斷改變，在遇到障礙物時需要越過或者繞過障礙物，航向?qū)⒏淖?，所以搜救機(jī)器人必須不斷更新航行角度，以到達(dá)正確的位置。搜救機(jī)器人系統(tǒng)設(shè)計如圖1所示。

2 搜救機(jī)器人系統(tǒng)各模塊設(shè)計

搜救機(jī)器人設(shè)計框圖如圖2所示。

2.1 主控芯片的選型

主控芯片是搜救機(jī)器人的核心部件，屏幕LCD選用并行24位可變靜態(tài)存儲控制器（Flexible Static Memory Controller，F(xiàn)SMC）總線，觸摸屏使用了集成電路總線（Inter-Integrated Circuit，IIC）接口，UWB模塊和GPS模塊使用了通用異步收發(fā)傳輸器（Universal Asynchronous Receiver/Transmitter）接口，通過微控制器產(chǎn)生脈沖寬度調(diào)制（Pulse Width Modulation，PWM）信號驅(qū)動電機(jī)，編碼器用以計算搜救機(jī)器人當(dāng)前速度，MPU9250使用了IIC接口，4G無線通信模塊使用了通用串行總線（Universal Serial Bus，USB）接口，針對以上外設(shè)資源，主控芯片必須具有FSMC總線接口、IIC接口、SPI接口、UART接口以及比較和捕獲模塊（用于產(chǎn)生PWM信號），根

據(jù)硬件資源配置和性格比的考慮，選擇了STM32F767做主控芯片。

2.2 定位模塊設(shè)計

本系統(tǒng)使用了GPS+UWB混合定位的模式。選用GPS+北斗雙定位模塊ATGM332D，此模塊經(jīng)過數(shù)據(jù)解算后直接將位置信息通過UART發(fā)送至搜救機(jī)器人主控模塊，搜救機(jī)器人再對接收到的位置信息進(jìn)行處理。UWB定位采用目前最流行的技術(shù)TDOA技術(shù)方案[6]。TDOA通過檢測信號到達(dá)不同監(jiān)測站的時間差，來計算信號源的位置進(jìn)行定位，因為利用了時間差而非信號到達(dá)的絕對時間，TDOA降低了時間同步的要求。DOA的原理如圖3所示：不同監(jiān)測站將同一時間測得的同一信號獲得的數(shù)據(jù)發(fā)送給監(jiān)測站A，監(jiān)測站A經(jīng)過解算得到信號到達(dá)不同的監(jiān)測站的時間差，將時間差轉(zhuǎn)換為距離將得到3條雙曲線，雙曲線的交點為信號源的位置。

如果要建立一個三維空間坐標(biāo)，至少需要3個參考點來建立另外一個點的方程，其建立的方程式為：

其中：（x，y，z）為被定位點位置;（xi，yi，zi）為空間中參考點的位置坐標(biāo);C為光速，3×108 m/s;ti為參考點位置的到達(dá)時間;t0為定位點發(fā)出信號的時間。

若已知3個參考點的坐標(biāo)信息（

xi，yi，zi）以及t0，可通過解四元二次方程組得到被定位點的位置信息（x，y，z）。為提高四元二次方程組有解的概率，將參考點增加至4個，并選用方程組的最優(yōu)解，由此提高被定位點位置的坐標(biāo)精度。

主控模塊使用Fang算法對UWB終端天線接收到的數(shù)據(jù)進(jìn)行解算，并將解算的數(shù)據(jù)添加幀頭、幀類型、幀編號、幀校驗后通過UART發(fā)送到主控模塊。

2.3 姿態(tài)捕獲

主控制器需要不斷地獲取搜救機(jī)器人姿態(tài)數(shù)據(jù)，通過計算目標(biāo)位置和當(dāng)前位置的斜率以及當(dāng)前方向獲得轉(zhuǎn)向角度。本系統(tǒng)獲取搜救機(jī)器人姿態(tài)使用MPU9250，MPU9250內(nèi)部集成陀螺儀、加速度傳感器和電子羅盤、2個內(nèi)部時鐘源和一個鎖相環(huán)（Phase Locked Loop，PLL）。把空間劃分為X軸，Y軸和Z軸，MPU9250可以對3個軸的角速度、加速度以及磁感應(yīng)強(qiáng)度進(jìn)行測量。加速度傳感器和電子羅盤具有較高頻率的噪聲，為了抑制噪聲，在搜機(jī)器人系統(tǒng)中，使用MPU9250的電子羅盤來獲取搜救機(jī)器人的方向，獲取的方向數(shù)據(jù)通過IIC總線傳輸?shù)轿⒖刂破鳌?/p>

2.4? 電機(jī)驅(qū)動模塊設(shè)計

單片機(jī)I/O口輸出的最大電壓為3.3 V，最大輸出電流為15 mA，最大輸出功率約為50 mW，I/O口直接帶負(fù)載能力極弱，故需要增加驅(qū)動電路以提高其帶負(fù)載能力。

PWM信號的頻率越高，對輸出的響應(yīng)速度越快，頻率越低對輸出響應(yīng)速度越慢。為了使輸出響應(yīng)速度快，電機(jī)工作穩(wěn)定，主控模塊產(chǎn)生一個10 kHz的PWM信號作為搜救機(jī)器人的控制信號。在一個周期內(nèi)高電平持續(xù)時間決定了信號的有效值，即占空比的值越大PWM信號的有效值就越大，后級輸出的功率越大。本系統(tǒng)中驅(qū)動電路選用TB6612，TB6612具有兩路PWM輸入和兩路PWM輸出信號，可以同時驅(qū)動兩個電機(jī)，通過AIN1，AIN2，BIN1，BIN2引腳的置位或者復(fù)位控制電機(jī)的正轉(zhuǎn)、反轉(zhuǎn)及停止功能。將TB6612的輸出引腳AO1、AO2和BO1，BO2分別連接到電機(jī)上，通過程序設(shè)置AIN1、AIN2和BIN1、BIN2及2個輸入PWM信號的占空比，便能實現(xiàn)搜救機(jī)器人的運動狀態(tài)。TB6612控制真值表如表1所示。

驅(qū)動電路需要單片機(jī)輸出2組PWM（PWMA、PWMB），同時還需要輸出4個控制信號AIN1、AIN2、AIN3、AIN4。驅(qū)動電路工作電壓為12 V，內(nèi)部的數(shù)字器件工作電壓為5 V，驅(qū)動電路如圖4所示。

2.5 4G無線通信模塊

搜救機(jī)器人在行進(jìn)過程中，需要將獲取到的信息實時的發(fā)送回搜救站，搜救站也要通過無線網(wǎng)絡(luò)下發(fā)指令給搜救機(jī)器人。因此需要一個能夠?qū)崿F(xiàn)遠(yuǎn)距離無線通信的電路，此設(shè)計中使用了華為ME909S-821 4G全網(wǎng)通無線通信模塊。通過PCIe轉(zhuǎn)USB后連接至搜救機(jī)器人。將搜救機(jī)器人各個部分獲得的數(shù)據(jù)經(jīng)過濾波、壓縮、編碼和打包后通過4G無線通信模塊發(fā)送給搜救站，搜救站接收到搜救機(jī)器人發(fā)送的數(shù)據(jù)后對數(shù)據(jù)進(jìn)行解壓縮后，得到搜救機(jī)器人的數(shù)據(jù)。搜救站通過無線方式下發(fā)控制指令給搜救機(jī)器人，通信模塊將接收到的控制指令傳送給MCU，MCU通過分析和處理后，控制搜救機(jī)器人各個部件執(zhí)行相應(yīng)的操作，從而控制搜救機(jī)器人進(jìn)行搜救工作。

2.6 搜救機(jī)器人原理圖設(shè)計

在以上各模塊分析的基礎(chǔ)上，完成搜救機(jī)器人系統(tǒng)原理圖如圖5所示。

3 樣機(jī)及性能測試

搜救機(jī)器人原理圖設(shè)計完成后，按圖6所示將對搜救機(jī)器人進(jìn)行焊接和調(diào)試。各個模塊連接完成并確認(rèn)無誤后，進(jìn)行上電調(diào)試。使用上位機(jī)配置UWB的工作模式，將固定在搜救機(jī)器人上的UWB模塊配置成定位模式，另外4個UWB模塊配置成基站模式。配置成功后，將UWB基站模塊進(jìn)行順序擺放，將UWB模塊的串口通過USB轉(zhuǎn)串口模塊連接至電腦，使用串口調(diào)試助手能夠查看到UWB模塊通過串口發(fā)送的數(shù)據(jù)，這些數(shù)據(jù)中的有效信息為UWB定位模塊的位置信息。將搜救機(jī)器人的UWB模塊通過USB數(shù)據(jù)線連接至電腦，上位機(jī)將看到搜救機(jī)器人相對于基站的位置信息。移動搜救機(jī)器人的位置，在上位機(jī)中將看到定位點的位置也隨之發(fā)生移動。通過系統(tǒng)調(diào)試后，能實現(xiàn)自動巡航與偏航計算，遠(yuǎn)距離無線數(shù)據(jù)傳輸與控制，實現(xiàn)搜救機(jī)器人樣機(jī)的各項功能。搜救機(jī)器人樣機(jī)及工作狀態(tài)如圖6所示。

4 結(jié)語

基于GPS和UWB混合定位的自動搜救機(jī)器人完成了GPS定位，UWB定位，搜救機(jī)器人姿態(tài)解算，航向角度計算與規(guī)劃，遠(yuǎn)距離無線數(shù)據(jù)傳輸與控制，LCD屏顯示數(shù)據(jù)的基本功能，搜救機(jī)器人履帶轉(zhuǎn)動實現(xiàn)了搜救機(jī)器人的轉(zhuǎn)向與移動。通過樣機(jī)驗證，基本實現(xiàn)了GPS+UWB混合定位搜救機(jī)器人的各項功能。

參考文獻(xiàn)：

[1]韓建海，趙書尚，李濟(jì)順.六足機(jī)器人行走步態(tài)的協(xié)調(diào)控制[J].機(jī)電工程，2004，21（4）：8-10.

[2] 柳天虹，姜樹海.仿生六足機(jī)器人穩(wěn)定性分析與仿真[J].計算機(jī)仿真，2013，30（12）：360-364.

[3] 羅建國，卜澤昊.具有類柔性結(jié)構(gòu)的履帶救援機(jī)器人越障分析[J].華北科技學(xué)院學(xué)報，2018（1）：55-61.

[4] 李秋生，閆小軍.履帶式煤礦救援機(jī)器人行走機(jī)構(gòu)研究[J].煤炭技術(shù)，2017，36（10）：199-201.

[5] FONTANA R J.Recent system applications of short-pulse ultra-wideband （UWB） technology[J].IEEE Transactions on Microwave Theory and Techniques，2004，52（9）：2087-2104.

[6] 張新躍，沈樹群.UWB超寬帶無線通信技術(shù)及其發(fā)展前景[J].數(shù)據(jù)通信，2004（2）：9-12.