基于多傳感器融合的全地形機(jī)器人系統(tǒng)設(shè)計(jì)

朱 穎，吳鈺鑫，侯夏嘉，張亞婉，陳慶盛

(廣州華立學(xué)院，廣州 511325)

0 引言

小城鎮(zhèn)規(guī)模快速擴(kuò)大，但隨著防洪排澇設(shè)施老舊、排澇能力不足等，提高了發(fā)生洪災(zāi)的可能性。災(zāi)后的環(huán)境經(jīng)過重塑，使得搜救難度大大增加。以往的搜救機(jī)器人一般注重于救援行為上，可以輔助搜救人員進(jìn)行搜救，卻無法有效的提高實(shí)際的搜救效率[1]。

傳統(tǒng)的洪澇搜救機(jī)器人多為氣墊船的運(yùn)動(dòng)結(jié)構(gòu)且運(yùn)用單一傳感器，反饋的信息有限。文獻(xiàn)[2]提出了一種以ARM的水面機(jī)器人控制系統(tǒng)，該系統(tǒng)使用攝像頭反饋現(xiàn)場(chǎng)信息，以GPS作為導(dǎo)航定位器，設(shè)計(jì)復(fù)雜、功耗高。文獻(xiàn)[3]使用SolidWorks設(shè)計(jì)了一種輪船式搜救機(jī)器人，并按照設(shè)定好的航行軌跡進(jìn)行巡邏，這種機(jī)器人能夠在穩(wěn)定的環(huán)境內(nèi)進(jìn)行工作，但不適應(yīng)于復(fù)雜多樣的災(zāi)后地形。文獻(xiàn)[4]通過MSP430F147單片機(jī)設(shè)計(jì)低功耗的救援機(jī)器人，通過上位機(jī)可獲取機(jī)器人的實(shí)時(shí)定位，但是缺少了環(huán)境信息的獲取。在此背景下，針對(duì)災(zāi)后環(huán)境信息收集，利用慣性測(cè)量單元、輪式里程計(jì)及GPS融合數(shù)據(jù)作為定位框架，LORA擴(kuò)頻無線模塊回傳生命、空氣及煙霧等環(huán)境數(shù)據(jù)給救援中心。結(jié)構(gòu)上設(shè)計(jì)了履帶氣墊一體化運(yùn)動(dòng)機(jī)構(gòu)。在積水環(huán)境下，機(jī)器人使用氣墊船運(yùn)動(dòng)模式，可在積水中自由行進(jìn)；待積水退去，機(jī)器人將氣墊收起，切換履帶運(yùn)動(dòng)模式。

1 系統(tǒng)總體設(shè)計(jì)

全地形搜救機(jī)器人通過多傳感器模塊獲取環(huán)境信息，F(xiàn)PGA控制機(jī)器人自主行走和對(duì)信息進(jìn)行處理。上位機(jī)下達(dá)機(jī)器人運(yùn)動(dòng)目標(biāo)指令，機(jī)器人開始自主行走，輪式里程計(jì)和姿態(tài)傳感器MPU6050獲取機(jī)器人的位姿信息，主控器對(duì)位姿信息進(jìn)行擴(kuò)展卡爾曼濾波算法處理后，對(duì)機(jī)器人位姿進(jìn)行預(yù)測(cè)，控制機(jī)器人自主穩(wěn)定行走；同時(shí)生命探測(cè)儀探測(cè)遇險(xiǎn)人員，當(dāng)發(fā)現(xiàn)遇險(xiǎn)人員，F(xiàn)PGA通過GPS北斗雙模定位模塊獲取機(jī)器人的位置信息并通過LORA無線擴(kuò)頻模塊發(fā)送給上位機(jī)并發(fā)出警報(bào)，使救援人員迅速開展救援行動(dòng)；視覺傳感器負(fù)責(zé)采集環(huán)境圖像信息并回傳給上位機(jī)，并通過空氣質(zhì)量檢測(cè)儀實(shí)時(shí)監(jiān)測(cè)災(zāi)區(qū)環(huán)境狀況。

多傳感器融合的全地形搜救機(jī)器人系統(tǒng)主要包括兩部分：硬件系統(tǒng)和軟件系統(tǒng)。系統(tǒng)硬件電路主要包含：EP4CE10-FPGA現(xiàn)場(chǎng)可編程開發(fā)平臺(tái)、MPU6050慣性傳感器、霍爾編碼器、LORA無線擴(kuò)頻模塊、GPS北斗雙模定位模塊、COMS攝像頭、生命特征儀、MQ-2檢測(cè)模塊以及機(jī)器人的底盤驅(qū)動(dòng)電路與電機(jī)。硬件系統(tǒng)框圖如圖1所示。

圖1 系統(tǒng)總框圖

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

2.1 主控器

全地形搜救機(jī)器人主控器需要對(duì)傳感器采集到的信息進(jìn)行處理并回傳至上位機(jī)，同時(shí)還需要對(duì)機(jī)器人自主移動(dòng)進(jìn)行實(shí)時(shí)控制，要求主控器的處理速度快、接口豐富、通訊快速穩(wěn)定。機(jī)器人以英特爾Cyclone系列的EP4CE10芯片作為整個(gè)系統(tǒng)的處理和控制中心，相比于ARM、DSP等處理器，F(xiàn)PGA的并行性及可重構(gòu)性，更能滿足數(shù)據(jù)復(fù)雜的機(jī)器人系統(tǒng)設(shè)計(jì)。板卡主要由FPGA、SDRAM和FLASH構(gòu)成，EP4CE10采用FBGA進(jìn)行封裝，工作溫度最高達(dá)85℃，器件運(yùn)行速度最高達(dá)400 Mhz，擁有1萬個(gè)邏輯單元，同時(shí)，還板載了先進(jìn)的視頻解碼晶片、百兆網(wǎng)卡、環(huán)境光傳感器和其他各類接口芯片，滿足機(jī)器人在復(fù)雜災(zāi)后環(huán)境下應(yīng)用的需求。

2.2 環(huán)境信息采集模塊

該機(jī)器人環(huán)境采集模塊包括：視覺傳感器、空氣質(zhì)量檢測(cè)傳感器、生命探測(cè)傳感器和環(huán)境定位模塊。

全地形機(jī)器人通過視覺傳感器獲取災(zāi)后的環(huán)境圖像傳給上位機(jī)，為救援人員提供直觀的災(zāi)后環(huán)境，以便救援人員對(duì)環(huán)境進(jìn)行分析做出正確的救援計(jì)劃。為獲取清晰穩(wěn)定的環(huán)境圖像，機(jī)器人采用OV5640視覺傳感器實(shí)現(xiàn)圖像采集。OV5640作為單晶片圖像感應(yīng)器，支持自動(dòng)曝光，其像素達(dá)到500 W，能實(shí)現(xiàn)最快90 fps的圖像采集，且OV5640攝像頭具有極強(qiáng)的擴(kuò)展性及重構(gòu)性，能為機(jī)器人的圖像感知處理留有發(fā)展空間。

災(zāi)后環(huán)境可能充斥著氨氣、硫化物及苯系蒸氣等有害氣體，機(jī)器人對(duì)空氣質(zhì)量進(jìn)行監(jiān)測(cè)可方便救援人員根據(jù)環(huán)境準(zhǔn)備救援工具與防護(hù)。機(jī)器人采用MQ-2傳感器對(duì)空氣進(jìn)行檢測(cè)，該傳感器可將硬件電路電導(dǎo)率的改變量換算為與危害氣體含量相對(duì)應(yīng)的輸出信息。相比于MQ-135等傳感器，MQ-2的精度、穩(wěn)定性更高，能到達(dá)對(duì)環(huán)境氣體檢測(cè)的目的[5]。

洪災(zāi)常伴有房屋倒塌泥石流等災(zāi)難造成人員被困，機(jī)器人搭載生命探測(cè)儀對(duì)被困人員進(jìn)行探測(cè)。生命探測(cè)儀采用HLK-LD1115H-24 G傳感器，其主要由24 G人體存在傳感器與熱釋傳感器組成。人體存在傳感器使用24 GHz頻率的毫米波雷達(dá)對(duì)機(jī)器人附近人體進(jìn)行檢測(cè)，該模塊除了多普勒人體檢測(cè)雷達(dá)模塊對(duì)大幅動(dòng)作和微小肢體動(dòng)作檢測(cè)人體存在還可通過對(duì)微小呼吸進(jìn)行累計(jì)探測(cè)人體。熱釋傳感器通過紅外信號(hào)檢測(cè)，彌補(bǔ)人體存在傳感器的不足，兩者互補(bǔ)相比于單一的熱釋傳感器，本系統(tǒng)的生命檢測(cè)組合的檢測(cè)精度及范圍更高，能夠高效對(duì)生命進(jìn)行檢測(cè)。

當(dāng)機(jī)器人發(fā)現(xiàn)被困人員時(shí)，機(jī)器人迅速向上位機(jī)發(fā)出救援信號(hào)，并發(fā)送經(jīng)度、緯度和高度位置信息，便于救援人員迅速準(zhǔn)確定位開展救援行動(dòng)，縮短救援時(shí)間。機(jī)器人采用ATK1218模塊作為雙定位傳感器，該傳感器板載S1216芯片，使用NMEA-0183作為通信協(xié)議，最大精確度達(dá)到2.5 mCEP，最大捕獲追蹤靈敏度為-165 dBm，是一種GPS/北斗雙模定位模塊[6]，滿足機(jī)器人位置定位需求。環(huán)境信息架構(gòu)圖如圖2所示。

圖2 環(huán)境信息架構(gòu)圖

2.3 慣性-定位導(dǎo)航器

機(jī)器人自主定位導(dǎo)航系統(tǒng)主要由輪式里程計(jì)和慣性測(cè)量單元組成。

輪式里程計(jì)提供的位姿信息連續(xù)且較為穩(wěn)定，其結(jié)構(gòu)簡(jiǎn)單，主要使用安裝在直流電機(jī)對(duì)尾部的霍爾編碼器進(jìn)行工作，由霍爾碼盤和霍爾元件組成，編碼器通過角位移或者非線性的角位移轉(zhuǎn)換來確定轉(zhuǎn)化信號(hào)為一個(gè)電信號(hào)。輪子轉(zhuǎn)動(dòng)時(shí)能夠?qū)⒔俏灰苹蛘呓撬俣绒D(zhuǎn)換成電數(shù)字脈沖，可以在短時(shí)間內(nèi)較精確的測(cè)量到移動(dòng)機(jī)器人的位姿[9]。

慣性測(cè)量單元IMU加速度計(jì)和陀螺儀，分別提供加速度和角速度[7]。IMU采集角速度與加速度信息通過積分運(yùn)算獲得載體的位置姿態(tài)信息，具有短時(shí)間內(nèi)精度高、響應(yīng)快速、可捕獲載體快速運(yùn)動(dòng)信息、能夠估計(jì)出載體的絕對(duì)尺度信息和不易受外界環(huán)境干擾等優(yōu)點(diǎn)[8]。全地形機(jī)器人慣性測(cè)量單元MPU6050整合了三軸陀螺儀、三軸加速度及一個(gè)運(yùn)動(dòng)處理器[10]，外界信號(hào)對(duì)其干擾幾乎為零，因此得到的定位結(jié)果更為穩(wěn)定，適合災(zāi)后復(fù)雜的地形環(huán)境。

2.4 全地形行走機(jī)構(gòu)

全地形搜救機(jī)器人為滿足陸地與水面行走的需要設(shè)計(jì)了履帶氣墊一體化運(yùn)動(dòng)機(jī)構(gòu)。

在陸地采用“粗齒鋸”模型無人履帶機(jī)構(gòu)，該機(jī)構(gòu)能自動(dòng)將適當(dāng)大小的動(dòng)力依次傳輸給每一塊履帶板，并且相比于普通履帶具有更好的平衡感，在任何地況都能活動(dòng)自如。履帶由TB6612對(duì)370大扭矩電機(jī)進(jìn)行驅(qū)動(dòng)控制，每個(gè)TB6612由FPGA根據(jù)航跡信息輸出兩個(gè)PWM控制，TB6612對(duì)FPGA的PWM進(jìn)行放大，使電機(jī)改變轉(zhuǎn)動(dòng)方向和轉(zhuǎn)速，可以實(shí)現(xiàn)全方位移動(dòng)。同時(shí)考慮災(zāi)后地面的不平整性，在機(jī)器人底盤設(shè)置液壓避震筒來提升整車攀爬能力和避免小車在跨越障礙時(shí)發(fā)生側(cè)翻。

機(jī)器人駛?cè)胨驎r(shí)，通過主風(fēng)機(jī)轉(zhuǎn)動(dòng)向氣墊進(jìn)行充氣，同時(shí)連接在氣墊中部的鏈條機(jī)構(gòu)向下旋轉(zhuǎn)配合風(fēng)機(jī)撐開氣墊，是氣墊能迅速展開充氣，通過氣墊的氣壓是機(jī)器人底盤抬升。當(dāng)氣墊充滿后，啟動(dòng)尾部?jī)蓚?cè)的風(fēng)機(jī)從而推進(jìn)機(jī)器人向目標(biāo)方向行駛。通過對(duì)尾部風(fēng)機(jī)的風(fēng)速調(diào)控來調(diào)整機(jī)體速度，并根據(jù)機(jī)器人尾部擺塊來改變風(fēng)向，達(dá)到轉(zhuǎn)向的效果。返回陸地時(shí)，主風(fēng)機(jī)向外排氣，鏈條機(jī)構(gòu)向上旋轉(zhuǎn)收納氣墊。

3 多信息融合算法

軟件系統(tǒng)主要通過混合式多傳感器信息融合技術(shù)進(jìn)行構(gòu)建框架，部分傳感器采用聚集式融合方法，其余的傳感器采取散布式融合方式[11]。主要包括融合構(gòu)架、多傳感器信息融合與卡爾曼濾波。

3.1 混合式信息融合框架設(shè)計(jì)

全地形機(jī)器人的應(yīng)用主要在災(zāi)后地區(qū)，具有固定區(qū)域的特點(diǎn)，信息采集工作需要結(jié)合GPS定位，為機(jī)器人的作業(yè)規(guī)定平面范圍。在災(zāi)區(qū)范圍內(nèi)，全地形機(jī)器人根據(jù)上位機(jī)指令，初級(jí)信息融合中心1獲取里程計(jì)及慣性單元信息反饋給中級(jí)信息中心1進(jìn)行位移，中級(jí)信息融合中心2，在指定的時(shí)間段完成環(huán)境信息采集任務(wù)[12]。混合式信息融合框架如圖3所示。

圖3 混合式信息框架

在終級(jí)信息融合中心中，系統(tǒng)搭建了云端及自適應(yīng)低功耗模塊。在云端模塊中，F(xiàn)PGA將LORA模塊的AUX及MDO端口進(jìn)行拉低處理，進(jìn)入模塊的配置中心，將波特率設(shè)為115 200 bps，空中速率為19.2 k，發(fā)射功率20 dBm，模塊通過UART協(xié)議將環(huán)境信息數(shù)據(jù)傳回用戶中心。在低功耗模塊中，對(duì)各個(gè)傳感器進(jìn)行自適應(yīng)低功耗設(shè)置，傳感器在5 s內(nèi)未進(jìn)行讀寫操作，即進(jìn)入休眠狀態(tài)，待下次指令到達(dá)再進(jìn)行喚醒操作，從而降低了系統(tǒng)的整體功耗，提高了系統(tǒng)的續(xù)航能力[13]。

3.2 建立機(jī)器人動(dòng)態(tài)模型

系統(tǒng)運(yùn)用擴(kuò)展卡爾曼濾波算法對(duì)各個(gè)傳感器的數(shù)據(jù)進(jìn)行分析調(diào)和之前，需要構(gòu)建機(jī)器人履帶動(dòng)態(tài)模型，把全地形機(jī)器人坐標(biāo)系和傳感器坐標(biāo)系相融合，用直角坐標(biāo)系表示機(jī)器人的運(yùn)動(dòng)范圍，在機(jī)器人中心建立局部坐標(biāo)，便于確定機(jī)器人的位置相對(duì)于環(huán)境中的觀察點(diǎn)[14]。

圖4 機(jī)器人動(dòng)態(tài)模型

根據(jù)機(jī)器人履帶差速運(yùn)動(dòng)模型，建立無噪聲情況下全地形運(yùn)動(dòng)方程式：

(1)

在式(1)中，θ為機(jī)器人實(shí)時(shí)姿態(tài)角，表示機(jī)器人的運(yùn)動(dòng)方向與X軸方向之間的角度；點(diǎn)O1為機(jī)器人的轉(zhuǎn)動(dòng)中心；w為機(jī)器人轉(zhuǎn)動(dòng)的角度；L為兩履帶之間距離；v為履帶底盤中心的線速度；[x,y,θ]T為上一時(shí)刻機(jī)器人的位置和朝向矩陣；[x1,y1,θ1]T為經(jīng)過時(shí)間間隔△t后機(jī)器人的位置和朝向矩陣[15]。

根據(jù)線速度、角速度、轉(zhuǎn)彎半徑關(guān)系式：v=wr，得到機(jī)器人中心線速度、角速度與兩履帶中心的線速度之間的關(guān)系式：

(2)

(3)

3.3 傳感器數(shù)據(jù)分析

3.3.1 里程計(jì)測(cè)速數(shù)據(jù)

在由光電編碼器組成的測(cè)量體系中，最常見的測(cè)速方式大致有三類:即M法、T法、M/T法。定位系統(tǒng)主要使用M/T法，因?yàn)镸/T法結(jié)合了M法和T法的優(yōu)點(diǎn)[16]，既適合高速測(cè)量也適合低速測(cè)量，測(cè)量公式為：

(4)

將測(cè)量公式結(jié)合動(dòng)態(tài)模型，得到履帶機(jī)器人的線速度式：

(5)

將線速度代入關(guān)系式(3)可求得角速度。再將角速度、線速度代入運(yùn)動(dòng)方程式即可得到位姿矩陣。

3.3.2 慣性單元航跡推算

系統(tǒng)利用MPU6050陀螺儀獲取機(jī)體的加速度及角速度。加速度在導(dǎo)航坐標(biāo)系中，分解為x,y,z三軸的加速度，對(duì)3個(gè)速度進(jìn)行一次積分得到機(jī)器人運(yùn)動(dòng)的速度，進(jìn)行二次位移得到機(jī)器人運(yùn)動(dòng)的位移。在使用陀螺儀時(shí)首先給定一個(gè)基準(zhǔn)方向，機(jī)器人運(yùn)動(dòng)中的位姿將由自身角度變化量的不斷累加得到[17]。

全地形機(jī)器人使用履帶結(jié)構(gòu)，故機(jī)器人只能進(jìn)行前進(jìn)(Vt)與旋轉(zhuǎn)(Wt)，不能隨意規(guī)定方位移動(dòng)。推算系統(tǒng)航跡時(shí)，需要優(yōu)先考慮相鄰時(shí)刻(MS)，把兩臨近的運(yùn)動(dòng)軌跡視為直線，即向X軸運(yùn)動(dòng)Vt*△t，將其代入運(yùn)動(dòng)坐標(biāo)系，得機(jī)器人在t+1期間相對(duì)于t期間在運(yùn)動(dòng)坐標(biāo)中移動(dòng)的位移：

(6)

求得到相鄰時(shí)刻的位移后，如欲知一段時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，則只需將這一時(shí)期的位移增量加以累積求和：

(7)

3.3.3 GPS定位信息采集

GPS對(duì)機(jī)器人進(jìn)行區(qū)域劃定之前，主要需要完成兩項(xiàng)任務(wù)：接收數(shù)據(jù)及處理數(shù)據(jù)。GPS與FPGA連接后，F(xiàn)PGA開始采集GPS數(shù)據(jù)，并對(duì)ATK1218模塊數(shù)據(jù)協(xié)議進(jìn)行約定，GPS模式具有多種模式，全地形搜救機(jī)器人采用GNRMC推薦定位信息模式，其他數(shù)據(jù)位包含UTC時(shí)間、經(jīng)緯度、衛(wèi)星數(shù)量、海拔高度等信息，模塊支持最快1秒輸出20次定位信息，系統(tǒng)需要在指定時(shí)間內(nèi)，將模塊數(shù)據(jù)緩存到FIFO中，并進(jìn)行解析。解析后，中級(jí)信息融合中心獲的機(jī)器人此時(shí)坐標(biāo)A，終級(jí)信息融合中心以A為起點(diǎn)坐標(biāo)構(gòu)建矩形坐標(biāo)系，既可獲得機(jī)器人作業(yè)區(qū)域。

3.3.4 數(shù)據(jù)傳輸及緩存

全地形機(jī)器人信息龐大，各個(gè)傳感器時(shí)鐘域不同，數(shù)據(jù)更新頻率不同，此時(shí)需對(duì)數(shù)據(jù)緩存做處理[18]。系統(tǒng)設(shè)計(jì)了FIFO+SDRAM一體的讀寫緩存器。在數(shù)據(jù)進(jìn)行采集之前，用戶需要對(duì)外部存儲(chǔ)芯片W9825G6DH進(jìn)行初始化、行激活，接著用戶將傳感器數(shù)據(jù)寫入FIFO-A，得到SDRAM控制器響應(yīng)后，F(xiàn)IFO-A中的數(shù)據(jù)寫入W9825G6DH，當(dāng)FIFO-B中的數(shù)據(jù)量小于突發(fā)長(zhǎng)度是將W9825G6DH中的數(shù)據(jù)讀出，送入下一級(jí)處理中心。

3.4 擴(kuò)展卡爾曼濾波信息融合

傳統(tǒng)的卡爾曼濾波算法可以在線性高斯模型條件下，對(duì)目標(biāo)狀態(tài)作出最優(yōu)估算，不過，機(jī)器人在實(shí)際運(yùn)動(dòng)中，并不會(huì)固定直線行駛，在廢墟或者積水區(qū)域作業(yè)中，也會(huì)使機(jī)器人產(chǎn)生顛簸或者側(cè)傾，機(jī)器人運(yùn)動(dòng)中會(huì)具有非線性的特征[20]，此時(shí)就需要通過擴(kuò)展卡爾曼濾波把非線性問題轉(zhuǎn)化為一種類似線性濾波問題。擴(kuò)展卡爾曼濾波算法(EKF)適用于非線性系統(tǒng)，原理是通過一階泰勒展開將非線性方程轉(zhuǎn)換為線性方程，再采用卡爾曼濾波算法對(duì)狀態(tài)量進(jìn)行估計(jì)。擴(kuò)展卡爾曼濾波算法本質(zhì)上由以下兩部分構(gòu)成：預(yù)測(cè)模型和觀測(cè)模型兩個(gè)部分[21]。

3.4.1 狀態(tài)預(yù)測(cè)

擴(kuò)展卡爾曼濾波狀態(tài)預(yù)測(cè)方程為：

u1=Gtut-1+Wt

(8)

∑t=Gt∑t+1GtT+At

(9)

系統(tǒng)使用輪式里程計(jì)為狀態(tài)預(yù)測(cè)提供控制量ut+1=(Vodo，Vodo，Wodo)。根據(jù)運(yùn)動(dòng)模型預(yù)測(cè)t+1時(shí)刻的位姿估計(jì)值為：

(10)

式中，Xt+1為機(jī)器人t+1時(shí)刻的位姿估值[20]。∑t為預(yù)測(cè)狀態(tài)向量估計(jì)值的協(xié)方差矩陣，∑t+1即為t+1時(shí)刻的狀態(tài)協(xié)方差矩陣[22]。將式(5)所求的履帶中心線速度代入運(yùn)動(dòng)方程式，再結(jié)果代入狀態(tài)方程式，便能獲取機(jī)器人目前時(shí)刻的行駛狀態(tài)估計(jì)值。

3.4.2 觀測(cè)預(yù)測(cè)

卡爾曼濾波觀測(cè)方程式為：

Zt=Htut+Vt

(11)

式中，Zt為系統(tǒng)觀測(cè)量，Ht為系統(tǒng)量測(cè)矩陣，Vt為t時(shí)刻的測(cè)量噪聲。對(duì)卡爾曼濾波算法進(jìn)行降維擴(kuò)展得到新的觀測(cè)方程式：

Zt=h(ut)+Vt

(12)

式中，h(ut)是非線性函數(shù)，Vt符合統(tǒng)計(jì)學(xué)特征的高斯白噪聲[23]。計(jì)算卡爾曼濾波增益矩陣：

(13)

通過卡爾曼增益矩陣可改變機(jī)器人的狀態(tài)矩陣，從而獲得新的觀測(cè)值，然后通過改變估計(jì)系統(tǒng)狀態(tài)的協(xié)方差矩陣，最終將系統(tǒng)的協(xié)方差矩陣均方誤差最小化，并以此實(shí)現(xiàn)數(shù)據(jù)融合的目的。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)場(chǎng)景

實(shí)驗(yàn)場(chǎng)景選擇雨后天氣，在城市工地內(nèi)，將GPS定位模塊波特率設(shè)置為38 400，初始化接收衛(wèi)星數(shù)據(jù)為4顆，等待3 S，接收到7顆衛(wèi)星及以上時(shí)，開始利用NMEA-0183協(xié)議對(duì)接收的數(shù)據(jù)進(jìn)行解析，劃定廢墟以及積水區(qū)域(大致1 000 m2)模擬災(zāi)后多種地形環(huán)境。在實(shí)驗(yàn)區(qū)域內(nèi)，以A點(diǎn)為原點(diǎn)為全地形機(jī)器人設(shè)置4個(gè)目標(biāo)點(diǎn)A(0 m，0 m),B(0 m，50 m)，C(20 m,55 m)，D(22 m,0 m)，并建立實(shí)驗(yàn)區(qū)域坐標(biāo)系。

圖5 GPS區(qū)域云端圖

圖6 位置點(diǎn)坐標(biāo)示意

4.2 實(shí)驗(yàn)效果

全地形機(jī)器人在GPS規(guī)定的實(shí)驗(yàn)場(chǎng)地內(nèi)，按照A→B→C→D路線進(jìn)行行駛及信息采集。在場(chǎng)景內(nèi)，機(jī)器人以0.2 m/s的初速度進(jìn)行行駛，F(xiàn)PGA通過慣性測(cè)量單元獲取機(jī)器人y與x方向的速度值，存入讀寫緩存器，并進(jìn)行狀態(tài)預(yù)測(cè)。如圖7所示：X方向最大速度誤差為0.1 m/s，Y方向最大誤差為0.15 m/s，機(jī)器人在廢墟等崎嶇的位置傳感器測(cè)量值震蕩較為強(qiáng)烈。

圖7 y與x方向速度誤差

為進(jìn)一步分析機(jī)器人航跡的實(shí)際情況，通過輪式里程計(jì)計(jì)算機(jī)器人的位移距離，并進(jìn)行狀態(tài)更新。如圖8所示。X方向最大位移誤差為反向0.45 m，Y方向最大位移誤差為反向0.58 m，從實(shí)驗(yàn)數(shù)據(jù)可看出，速度誤差與位移誤差成正比，機(jī)器人0.5 m的位移差，中等程度偏離航向，要實(shí)現(xiàn)機(jī)器人的航跡按指定程序完成作業(yè)，需對(duì)傳感器的數(shù)據(jù)進(jìn)行擴(kuò)展卡爾曼濾波融合。

圖8 y與x方向的位移誤差

經(jīng)過擴(kuò)展卡爾曼濾波后，得到的測(cè)量位置誤差低于0.1 m，雖然與機(jī)器人的實(shí)際位置有點(diǎn)差別，但也已經(jīng)無限貼近實(shí)際位置曲線，能夠按要求完成機(jī)器人準(zhǔn)確巡航任務(wù)。

圖9 全地形機(jī)器人修復(fù)結(jié)果對(duì)比圖

在機(jī)器人運(yùn)動(dòng)過程中，通過預(yù)存指令對(duì)ABCD四個(gè)點(diǎn)的空氣、經(jīng)緯度、高度、生命特征、光照系數(shù)進(jìn)行檢測(cè)，并將采集的數(shù)據(jù)與實(shí)驗(yàn)員現(xiàn)場(chǎng)勘測(cè)數(shù)據(jù)對(duì)比。從表格，可以看出機(jī)器人采集的環(huán)境空氣指數(shù)最高為B點(diǎn)廢墟環(huán)境37 PPM，最低為D點(diǎn)森林環(huán)境14，傳感器測(cè)量數(shù)據(jù)平均誤差0.75，A點(diǎn)出現(xiàn)較大誤差，經(jīng)檢查，為傳感器初始化預(yù)熱慢，導(dǎo)致出現(xiàn)較大誤差；環(huán)境高度最高為B點(diǎn)廢墟42米，最低為A點(diǎn)水域39，平均誤差0.425；光照系數(shù)平均誤差5 lx；生命特征正確檢測(cè)4次，在A點(diǎn)檢測(cè)到生命時(shí)，向用戶中心返回了A點(diǎn)經(jīng)緯度：23.248 69,113.864 036，提示生命所在位置。根據(jù)以上指標(biāo)，機(jī)器人能完成一般信息采集任務(wù)。采集信息如表1所示。

表1 機(jī)器人環(huán)境信息采集表

5 結(jié)束語

在災(zāi)區(qū)環(huán)境中，多傳感器信息融合是機(jī)器人感知周圍環(huán)境信息技術(shù)中一項(xiàng)行之有效、必不可缺的關(guān)鍵技術(shù)。

全地形信息采集機(jī)器人，在混合式融合架構(gòu)下充分利用了多傳感器技術(shù),以實(shí)現(xiàn)對(duì)多感應(yīng)器的探測(cè)信息的合理支配與利用,將多感應(yīng)器的空隙及時(shí)間上的多余或相互利用信息加以綜合。

實(shí)驗(yàn)結(jié)果表明，機(jī)器人憑借多種傳感器數(shù)據(jù)融合，能夠提高機(jī)器人定位精度，通過擴(kuò)展卡爾曼濾波算法，能夠校正崎嶇環(huán)境下累積的誤差，從而使得機(jī)器人得到最優(yōu)信息，助力災(zāi)后救援。