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

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

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

0 引言

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

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

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

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

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

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

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

2.1 主控器

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

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

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

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

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

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

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

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

2.3 慣性-定位導航器

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

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

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

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

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

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

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

3 多信息融合算法

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

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

全地形機器人的應用主要在災后地區(qū)，具有固定區(qū)域的特點，信息采集工作需要結(jié)合GPS定位，為機器人的作業(yè)規(guī)定平面范圍。在災區(qū)范圍內(nèi)，全地形機器人根據(jù)上位機指令，初級信息融合中心1獲取里程計及慣性單元信息反饋給中級信息中心1進行位移，中級信息融合中心2，在指定的時間段完成環(huán)境信息采集任務[12]?；旌鲜叫畔⑷诤峡蚣苋鐖D3所示。

圖3 混合式信息框架

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

3.2 建立機器人動態(tài)模型

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

圖4 機器人動態(tài)模型

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

(1)

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

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

(2)

(3)

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

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

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

(4)

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

(5)

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

3.3.2 慣性單元航跡推算

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

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

(6)

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

(7)

3.3.3 GPS定位信息采集

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

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

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

3.4 擴展卡爾曼濾波信息融合

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

3.4.1 狀態(tài)預測

擴展卡爾曼濾波狀態(tài)預測方程為：

u1=Gtut-1+Wt

(8)

∑t=Gt∑t+1GtT+At

(9)

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

(10)

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

3.4.2 觀測預測

卡爾曼濾波觀測方程式為：

Zt=Htut+Vt

(11)

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

Zt=h(ut)+Vt

(12)

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

(13)

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

4 實驗驗證

4.1 實驗場景

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

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

圖6 位置點坐標示意

4.2 實驗效果

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

圖7 y與x方向速度誤差

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

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

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

圖9 全地形機器人修復結(jié)果對比圖

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

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

5 結(jié)束語

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

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

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