基于云平臺(tái)的消防員自主導(dǎo)航與搜救系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

李婷婷 王 琪,2 徐勇軍 安郎平 王 莽

火災(zāi)給人們的生命及財(cái)產(chǎn)帶來(lái)了極大的威脅,火場(chǎng)救援面臨的環(huán)境結(jié)構(gòu)復(fù)雜、封閉性強(qiáng)、信號(hào)較弱且易受干擾,這嚴(yán)重影響了消防員火場(chǎng)救援效率,甚至給救援人員帶來(lái)生命威脅.在復(fù)雜的室內(nèi)環(huán)境下,如何精準(zhǔn)定位、有效部署、科學(xué)救援、多方協(xié)同作戰(zhàn)是消防救援領(lǐng)域研究的重點(diǎn)問(wèn)題.

消防員的火場(chǎng)救援還存在以下難點(diǎn)：

1）消防員自主定位難.室內(nèi)環(huán)境下由于衛(wèi)星拒止、建筑物遮擋的影響,無(wú)法很好地接收到衛(wèi)星信號(hào),引起較大的定位誤差,單一定位手段難以實(shí)現(xiàn)衛(wèi)星拒止環(huán)境下的準(zhǔn)確定位.

2）消防員環(huán)境感知難.由于火場(chǎng)煙霧彌漫,消防員配備的視覺(jué)傳感設(shè)備工作性能大大下降,難以獲取現(xiàn)場(chǎng)的清晰圖像,致使難以探測(cè)到障礙物信息.另外,火災(zāi)容易致使建筑物的結(jié)構(gòu)破壞,造成搜救環(huán)境的目標(biāo)檢測(cè)和地圖重建難度增大,使得自主定位的精度受限,搜救環(huán)境的環(huán)境感知信息缺少,影響消防員搜救路徑和逃生路徑的選擇.

3）消防班組指揮控制協(xié)同難.消防官兵在救援現(xiàn)場(chǎng)的位置信息不能實(shí)時(shí)共享,現(xiàn)場(chǎng)監(jiān)控指揮人員缺乏對(duì)室內(nèi)救援的消防員進(jìn)行定位以及運(yùn)動(dòng)狀態(tài)等信息的監(jiān)測(cè).指揮中心通過(guò)喊話的方式與救援人員溝通,效率低下,信息交互質(zhì)量和協(xié)同作戰(zhàn)效率都有待提高.另外當(dāng)消防員出現(xiàn)安全問(wèn)題時(shí),指揮終端無(wú)法及時(shí)發(fā)現(xiàn)并對(duì)隊(duì)員實(shí)施有效救援.

本文針對(duì)以上問(wèn)題,重點(diǎn)研究消防火場(chǎng)環(huán)境下的自主導(dǎo)航與救援的問(wèn)題,主要貢獻(xiàn)有：

1）設(shè)計(jì)了一個(gè)面向火場(chǎng)環(huán)境的消防員火場(chǎng)自主導(dǎo)航與搜救系統(tǒng).實(shí)現(xiàn)了消防員的精準(zhǔn)定位、運(yùn)動(dòng)分析和狀態(tài)監(jiān)督,動(dòng)態(tài)火場(chǎng)下的自適應(yīng)導(dǎo)航和三維場(chǎng)景重構(gòu).

2）提出一種基于因子圖的多源融合導(dǎo)航方法.融合多源信息實(shí)現(xiàn)了室內(nèi)復(fù)雜環(huán)境下消防員的高精度自主導(dǎo)航,并通過(guò)信息源的即插即用實(shí)現(xiàn)自適應(yīng)智能導(dǎo)航.

3）設(shè)計(jì)了一種面向火場(chǎng)環(huán)境的視覺(jué)定位與建圖（simultaneous localization and mapping,SLAM）動(dòng)態(tài)場(chǎng)景自適應(yīng)方法.在傳統(tǒng)SLAM 算法的基礎(chǔ)上增加目標(biāo)識(shí)別和語(yǔ)義分割模塊,檢測(cè)場(chǎng)景中有價(jià)值的目標(biāo),過(guò)濾掉火場(chǎng)中與移動(dòng)對(duì)象相關(guān)的數(shù)據(jù),并融合多源信息實(shí)現(xiàn)了對(duì)復(fù)雜火場(chǎng)環(huán)境的自適應(yīng)導(dǎo)航算法.

1 相關(guān)工作

國(guó)內(nèi)外對(duì)救援場(chǎng)景下搜救導(dǎo)航定位技術(shù)的研究很多,提高導(dǎo)航定位系統(tǒng)的精度是一個(gè)備受關(guān)注的研究課題.在國(guó)內(nèi)研究中目前關(guān)于消防員室內(nèi)導(dǎo)航定位算法主要以慣性導(dǎo)航、無(wú)線射頻識(shí)別（radio frequency identification,RFID）、ZigBee、WIFI、超寬帶（ultra wide band,UWB）等方面的研究為主.文獻(xiàn)[1]提出了一種基于慣性導(dǎo)航技術(shù)的消防室內(nèi)三維定位系統(tǒng),利用慣性導(dǎo)航技術(shù),利用位置估算、步伐估計(jì)以及零速修正等算法來(lái)提供救援人員的三維位置信息；文獻(xiàn)[2]利用計(jì)算機(jī)視覺(jué)來(lái)輔助RFID 定位,通過(guò)目標(biāo)檢測(cè)行人的腳步坐標(biāo),融合RFID 的標(biāo)簽坐標(biāo),從而得到救援人員的位置坐標(biāo)；文獻(xiàn)[3]將BP 神經(jīng)網(wǎng)絡(luò)加入到ZigBee 的室內(nèi)定位系統(tǒng)中,彌補(bǔ)了ZigBee 的缺陷與不足,改善了目標(biāo)節(jié)點(diǎn)的定位精度；文獻(xiàn)[4]提出了一種基于擴(kuò)展卡爾曼濾波的WIFI-PDR 融合定位算法,通過(guò)降低WIFI 定位回跳和行人航位推算（pedestrian dead reckonin,PDR）累計(jì)誤差,來(lái)提高定位精度；文獻(xiàn)[5]提出了一種火場(chǎng)惡劣環(huán)境下救援人員的定位方法,該方法通過(guò)雙極卡爾曼濾波框架慣性導(dǎo)航系統(tǒng),并利用UWB 技術(shù)具有的低功耗、高速度、良好的抗多徑效應(yīng)、高安全性獲得了較高的定位精度.但是它受室內(nèi)障礙物的影響,很大程度上取決于基站的事前布置.目前國(guó)內(nèi)的很多研究都無(wú)法滿足實(shí)際需求,存在實(shí)時(shí)性差、依賴定位基站等問(wèn)題,在復(fù)雜的室內(nèi)環(huán)境下不能得到很好的適用,且現(xiàn)有產(chǎn)品的成本高,體積大,不便于救援人員攜帶和部署.相較于國(guó)內(nèi),國(guó)外很多關(guān)于室內(nèi)搜救導(dǎo)航定位的相關(guān)研究更加成熟.文獻(xiàn)[6]提出了一種利用UWB 和到達(dá)相位差估計(jì)實(shí)現(xiàn)無(wú)基礎(chǔ)設(shè)施三維自定定位的新方法,以支持救援部隊(duì)在低能見(jiàn)度危險(xiǎn)行動(dòng)中執(zhí)行任務(wù)；文獻(xiàn)[7]提出了一個(gè)基于AR 技術(shù)的六自由度（6 degree of freedom,6DoF）定位系統(tǒng)LocAR,LocAR 設(shè)計(jì)了一個(gè)協(xié)同粒子濾波器,聯(lián)合估計(jì)所有AR 用戶相對(duì)于彼此的6DoF 姿勢(shì),使用來(lái)自視覺(jué)慣性里程計(jì)（visual-inertial odometry,VIO）的運(yùn)動(dòng)信息和UWB 無(wú)線電用戶之間的距離測(cè)量,建立一個(gè)精確的相對(duì)坐標(biāo)系來(lái)解決消防員協(xié)調(diào)定位難的問(wèn)題；文獻(xiàn)[8]通過(guò)追蹤難以進(jìn)入的倒塌地點(diǎn)和未知障礙的位置,識(shí)別危險(xiǎn)因素障礙物信息,生成最新的災(zāi)難現(xiàn)場(chǎng)數(shù)字地圖,為消防人員快速搜救工作提供有效解決方案.

雖然消防員火場(chǎng)自主導(dǎo)航與搜救在國(guó)內(nèi)外的消防領(lǐng)域均存在不同程度上的研究與應(yīng)用,但單一方法存在一定的局限性,尤其是在室內(nèi)、障礙物多的火災(zāi)場(chǎng)景導(dǎo)航精度會(huì)大大降低,導(dǎo)航質(zhì)量變差.因此,本文在現(xiàn)有消防導(dǎo)航搜救研究成果的技術(shù)上,綜合考慮了多源信息融合的導(dǎo)航定位、動(dòng)態(tài)場(chǎng)景下視覺(jué)導(dǎo)航、三維場(chǎng)景重建、自主學(xué)習(xí)等方法,設(shè)計(jì)了一個(gè)消防員火場(chǎng)救援的體系化系統(tǒng),并首次將毫米波雷達(dá)應(yīng)用于火場(chǎng)環(huán)境的探測(cè),實(shí)現(xiàn)了復(fù)雜火場(chǎng)環(huán)境的高精度自主導(dǎo)航定位.

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

2.1 系統(tǒng)功能概述

系統(tǒng)基于因子圖框架融合慣性、視覺(jué)、GPS、毫米波雷達(dá)、UWB 和激光等信息源,實(shí)現(xiàn)對(duì)復(fù)雜火場(chǎng)環(huán)境的搜救人員精準(zhǔn)定位和煙霧夜晚等復(fù)雜環(huán)境下的場(chǎng)景感知構(gòu)建三維場(chǎng)景與智能目標(biāo)識(shí)別,在此基礎(chǔ)上基于增強(qiáng)現(xiàn)實(shí)頭盔智能顯示指揮控制信息,并利用云平臺(tái)實(shí)現(xiàn)信息的交互共享、健康監(jiān)測(cè)、人人協(xié)同/人機(jī)協(xié)同,來(lái)提高消防班組的救援和指揮能力.

系統(tǒng)體系設(shè)計(jì)框架圖如圖1 所示：系統(tǒng)第1 層實(shí)現(xiàn)消防員對(duì)自身狀態(tài)的感知；第2 層實(shí)現(xiàn)消防員對(duì)自身及環(huán)境的感知；第3 層實(shí)現(xiàn)人人協(xié)同、人機(jī)協(xié)同；第4 層與云平臺(tái)交互,實(shí)現(xiàn)系統(tǒng)整體的指揮控制.消防員火場(chǎng)自主導(dǎo)航與搜救系統(tǒng)以火災(zāi)救援需求為牽引,針對(duì)火場(chǎng)救援的自主定位難、環(huán)境感知難和指揮控制協(xié)同難的問(wèn)題,設(shè)計(jì)了以下5 個(gè)主要功能：

圖1 系統(tǒng)體系設(shè)計(jì)框架圖Fig.1 System of systems design framework

1）實(shí)現(xiàn)復(fù)雜火場(chǎng)環(huán)境的高精度自主導(dǎo)航定位.基于北斗系統(tǒng)和北斗錨點(diǎn),結(jié)合慣性、UWB 和地圖約束等多源信息,實(shí)現(xiàn)消防員的高精度自主導(dǎo)航定位,通過(guò)信息源的即插即用實(shí)現(xiàn)自適應(yīng)智能導(dǎo)航.

2）實(shí)現(xiàn)復(fù)雜環(huán)境下的三維場(chǎng)景重建和視覺(jué)SLAM自適應(yīng).系統(tǒng)通過(guò)視覺(jué)SLAM、激光雷達(dá)和毫米波雷達(dá)等傳感器感知三維場(chǎng)景,尤其是毫米波雷達(dá)的高穿透性有助于在火場(chǎng)的煙霧環(huán)境下對(duì)場(chǎng)景重建.系統(tǒng)采集環(huán)境中的圖像信息,并進(jìn)行智能目標(biāo)監(jiān)測(cè),有助于消防員的定位搜救目標(biāo)和選擇求生通道,解決消防救援火場(chǎng)環(huán)境感知難題.

3）實(shí)現(xiàn)多成員多平臺(tái)自組網(wǎng)深層協(xié)同.基于北斗錨點(diǎn)和UWB 基站,提高室內(nèi)導(dǎo)航的精度并賦予全局坐標(biāo),實(shí)現(xiàn)天地協(xié)同、室內(nèi)外協(xié)同和班組協(xié)同三位一體,提高消防救援分隊(duì)協(xié)同定位能力,提高搜救效率.

4）實(shí)現(xiàn)救援平臺(tái)一體化顯示與控制.將系統(tǒng)的指揮控制信息集成于增強(qiáng)現(xiàn)實(shí)智能顯示終端,結(jié)合增強(qiáng)現(xiàn)實(shí)方法,設(shè)計(jì)增強(qiáng)現(xiàn)實(shí)智能信息交互系統(tǒng),有助于智能顯示、信息交互和指揮控制協(xié)同[9].

5）消防員健康監(jiān)測(cè).基于云平臺(tái)對(duì)消防員救援信息的大數(shù)據(jù)分析,并基于自主學(xué)習(xí)算法和慣性數(shù)據(jù)分析方法實(shí)現(xiàn)消防員健康監(jiān)測(cè),對(duì)消防員傷病情況作出及時(shí)的預(yù)警.此外,將模擬實(shí)戰(zhàn)從各個(gè)角度記錄下來(lái),并上傳動(dòng)作數(shù)據(jù)到云端,對(duì)消防員后續(xù)訓(xùn)練提供數(shù)據(jù)參考.

2.2 系統(tǒng)硬件架構(gòu)

消防員火場(chǎng)自主導(dǎo)航與搜救系統(tǒng)在硬件上以可穿戴傳感器組件和增強(qiáng)現(xiàn)實(shí)智能終端為核心,輔以云平臺(tái)進(jìn)行數(shù)據(jù)分析與處理.系統(tǒng)的硬件結(jié)構(gòu)如圖2所示.

圖2 系統(tǒng)在消防員身上的安裝示意圖Fig.2 Installation diagram of the system on firefighters

系統(tǒng)硬件組成如表1 所示.可穿戴傳感器組件有微型慣性測(cè)量單元（miniature inertial measurement unit,MIMU）（包括陀螺儀、加速度計(jì)、磁力計(jì)氣壓計(jì)）、UWB、北斗模塊、毫米波雷達(dá)、激光雷達(dá)和視覺(jué)傳感器.定位信息、建圖數(shù)據(jù)、目標(biāo)檢測(cè)信息和上級(jí)與隊(duì)友的指揮控制信息,通過(guò)HoloLens2 解算并實(shí)現(xiàn)實(shí)時(shí)顯示.云平臺(tái)負(fù)責(zé)記錄每名運(yùn)動(dòng)人員的狀態(tài)數(shù)據(jù)和傳感器數(shù)據(jù)進(jìn)行大數(shù)據(jù)分析和處理.

表1 系統(tǒng)硬件組成Table 1 Composition of system hardware

MIMU：可結(jié)合零速修正提供相對(duì)位置和方向信息,實(shí)現(xiàn)姿態(tài)測(cè)量、加速度測(cè)量、高度測(cè)量、方向測(cè)量等功能,適用于消防員自主導(dǎo)航和定位系統(tǒng)的導(dǎo)航定位跟蹤.

UWB：可以實(shí)現(xiàn)高精度的定位和距離測(cè)量,還可以提供距離觀測(cè)量,降低MIMU 陀螺和加速度計(jì)漂移帶來(lái)的誤差發(fā)散,提高導(dǎo)航定位的精度,實(shí)現(xiàn)消防員間的協(xié)同定位.

北斗模塊：可以提供絕對(duì)位置信息,但無(wú)法適用于室內(nèi)等拒止環(huán)境中.結(jié)合衛(wèi)星基站、北斗模塊、MIMU 和UWB 等其他傳感器,系統(tǒng)可實(shí)時(shí)獲得消防員的絕對(duì)位置信息,實(shí)現(xiàn)室內(nèi)火場(chǎng)環(huán)境消防員的精準(zhǔn)定位.

毫米波雷達(dá)：可以穿透煙霧和濃霧等環(huán)境,可以檢測(cè)到人、障礙物、墻壁和其他結(jié)構(gòu)物,適用于火場(chǎng)內(nèi)障礙物檢測(cè)和導(dǎo)航.

激光雷達(dá)：可以用于生成火場(chǎng)內(nèi)的三維稠密地圖,適用于消防員自主導(dǎo)航和搜救系統(tǒng)的地圖生成和路徑規(guī)劃.

視覺(jué)傳感器：可以實(shí)現(xiàn)物體識(shí)別、跟蹤、測(cè)距、測(cè)量和導(dǎo)航等功能.在消防員自主導(dǎo)航系統(tǒng)中,視覺(jué)傳感器可以用于檢測(cè)和識(shí)別火場(chǎng)內(nèi)的人員和障礙物,為消防員提供更加直觀的導(dǎo)航和搜救支持,同時(shí)可以輔助其他傳感器進(jìn)行定位和距離測(cè)量,提高系統(tǒng)的準(zhǔn)確度和穩(wěn)定性,適用于消防員自主導(dǎo)航和搜救系統(tǒng)中提供直觀的導(dǎo)航和搜救支持.

融合上述多個(gè)傳感器可以彌補(bǔ)單個(gè)傳感器的不足之處,提高系統(tǒng)的魯棒性和可靠性,為消防員提供更加精準(zhǔn)的導(dǎo)航和搜救支持.在導(dǎo)航定位方面,衛(wèi)星導(dǎo)航提供絕對(duì)位置信息,但無(wú)法適用于室內(nèi)等拒止環(huán)境,MIMU 受環(huán)境影響較小但誤差隨時(shí)間累積,UWB 利用消防員與錨點(diǎn)之間以及消防員之間的距離測(cè)量提供相對(duì)位置信息,本系統(tǒng)采用在室外架設(shè)衛(wèi)星基站,室內(nèi)設(shè)置UWB 錨點(diǎn),MIMU 實(shí)時(shí)慣性導(dǎo)航的方法,可以實(shí)現(xiàn)室內(nèi)等環(huán)境的消防員準(zhǔn)確定位,為指揮決策和搜救行動(dòng)提供基礎(chǔ)信息支撐.在感知和搜救支持方面,視覺(jué)傳感器信息豐富但在低光環(huán)境下可能無(wú)法正常運(yùn)行,激光雷達(dá)對(duì)物體反射率敏感但建圖稠密,毫米波雷達(dá)可以穿透煙霧和濃霧等環(huán)境,因此,結(jié)合視覺(jué)、毫米波雷達(dá)和激光雷達(dá),可以實(shí)現(xiàn)復(fù)雜火場(chǎng)環(huán)境的實(shí)時(shí)準(zhǔn)確感知.

2.3 系統(tǒng)算法框架

系統(tǒng)的算法設(shè)計(jì)架構(gòu)如圖3 所示.導(dǎo)航定位算法模塊在卡爾曼濾波框架下組合慣性和衛(wèi)星信息,并結(jié)合人體運(yùn)動(dòng)約束實(shí)現(xiàn)準(zhǔn)確的導(dǎo)航定位.解決了在室內(nèi)衛(wèi)星拒止環(huán)境下消防員定位難的問(wèn)題.在第3章將詳細(xì)介紹基于因子圖的多源融合導(dǎo)航方法.

智能目標(biāo)檢測(cè)算法模塊基于MASK R-CNN[10]檢測(cè)場(chǎng)景中的目標(biāo),獲得語(yǔ)義信息,并對(duì)圖像進(jìn)行實(shí)例分割.基于目標(biāo)檢測(cè)的視覺(jué)SLAM 動(dòng)態(tài)場(chǎng)景自適應(yīng)方法將在第4 章詳細(xì)介紹.

三維場(chǎng)景重建算法模塊結(jié)合深度學(xué)習(xí),在SLAM點(diǎn)云地圖的基礎(chǔ)上融合場(chǎng)景語(yǔ)義信息,構(gòu)建三維語(yǔ)義場(chǎng)景.將毫米波雷達(dá)應(yīng)用于消防員火場(chǎng)救援,通過(guò)高穿透性的特征,實(shí)現(xiàn)在火場(chǎng)煙霧環(huán)境下的場(chǎng)景重建.

人員狀態(tài)監(jiān)測(cè)算法模塊以慣性測(cè)量和深度學(xué)習(xí)為基礎(chǔ),將原始慣性數(shù)據(jù)進(jìn)行預(yù)處理,解算出基礎(chǔ)運(yùn)動(dòng)指標(biāo),如觸地騰空比、左右平衡和配速等.該模塊基于深度學(xué)習(xí)方法解算更加準(zhǔn)確的基礎(chǔ)指標(biāo),并分析更深層次的運(yùn)動(dòng)信息,如是否跨步、觸地方式等.

智能自學(xué)習(xí)算法模塊通過(guò)多時(shí)間尺度采集的用戶運(yùn)動(dòng)大數(shù)據(jù),分析人體運(yùn)動(dòng)規(guī)律和能量消耗等指標(biāo),挖掘更有價(jià)值的深層次信息,提供損傷預(yù)防建議和健康方案.本文基于Hadoop 構(gòu)建用于大數(shù)據(jù)分析的云平臺(tái),通過(guò)足部慣性信息和北斗衛(wèi)星數(shù)據(jù),采用衛(wèi)星/慣性組合導(dǎo)航算法、數(shù)據(jù)驅(qū)動(dòng)的運(yùn)動(dòng)指標(biāo)解算算法,對(duì)救援過(guò)程采集到的數(shù)據(jù)進(jìn)行分析,實(shí)現(xiàn)對(duì)消防員救援的過(guò)程監(jiān)控、運(yùn)動(dòng)分析、效果評(píng)估和救援指導(dǎo),從而提高救援效率,預(yù)防訓(xùn)練傷病,增強(qiáng)消防員體能水平.

3 基于因子圖的多源融合導(dǎo)航方法

本文設(shè)計(jì)的多源融合導(dǎo)航方法面向消防員設(shè)計(jì),在慣性導(dǎo)航的基礎(chǔ)上融合視覺(jué)、GPS、磁力計(jì)等輔助信息建立因子圖,并對(duì)全局位姿進(jìn)行優(yōu)化.根據(jù)步態(tài)特征對(duì)救援人員的運(yùn)動(dòng)過(guò)程進(jìn)行分割,確定變量節(jié)點(diǎn),并以運(yùn)動(dòng)約束和場(chǎng)景約束為因子構(gòu)建因子圖,建立誤差能量函數(shù),再通過(guò)優(yōu)化的方法求取誤差能量函數(shù)的最小值,得到位姿最小二乘意義下的最優(yōu)估計(jì).

融合過(guò)程如圖4 所示,足部MIMU 數(shù)據(jù)經(jīng)捷聯(lián)慣導(dǎo)解算后的位姿信息,與視覺(jué)/慣性SLAM 位姿估計(jì)結(jié)果相結(jié)合,實(shí)現(xiàn)對(duì)消防員的局部位姿的融合估計(jì).在此基礎(chǔ)上,全局優(yōu)化模塊融合了視覺(jué)回環(huán)信息和其他傳感器,如Wi-Fi 指紋、GPS、UWB 測(cè)距信息和地圖信息,評(píng)估導(dǎo)航系統(tǒng)的全局誤差,并批量?jī)?yōu)化位姿圖.

圖4 多源融合導(dǎo)航框圖Fig.4 Block diagram of multi-source fusion navigation

基于零速修正的捷聯(lián)慣導(dǎo)解算算法利用消防員運(yùn)動(dòng)約束進(jìn)行誤差修正.運(yùn)動(dòng)約束主要指零速修正,若第k 時(shí)刻,足部處于靜止?fàn)顟B(tài),則足部此時(shí)的速度為零,利用足部MIMU 解算的速度vk作為系統(tǒng)速度誤差的觀測(cè)量,修正卡爾曼濾波的其他誤差量,可以抑制誤差發(fā)散.在零速階段,系統(tǒng)的觀測(cè)量Z 為

式中,δvk=vk-[0 0 0] 為第k 時(shí)刻的速度誤差.那么,系統(tǒng)的觀測(cè)矩陣H為：

由此在消防員捷聯(lián)慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system,SINS）誤差模型的基礎(chǔ)上,建立起了15 狀態(tài)的卡爾曼濾波器,并把腳步落地或靜止時(shí)的零速作為系統(tǒng)觀測(cè)量,對(duì)系統(tǒng)在零速階段的誤差進(jìn)行修正,進(jìn)而抑制導(dǎo)航誤差的發(fā)散,提高消防員的導(dǎo)航定位精度.

全局位姿優(yōu)化的因子圖結(jié)構(gòu)如圖5 所示.因子圖節(jié)點(diǎn)為救援人員六自由度的位姿（包含相對(duì)于初始點(diǎn)的位置和方向）,各節(jié)點(diǎn)的采樣頻率由步態(tài)更新頻率決定.相鄰節(jié)點(diǎn)之間的位姿約束由MIMU 提供.視覺(jué)信息不僅可利用相鄰節(jié)點(diǎn)之間的位姿變化約束節(jié)點(diǎn)狀態(tài),也可以通過(guò)圖像特征匹配提供全局約束,抑制誤差的漂移.磁強(qiáng)計(jì)提供航向信息,約束航向的發(fā)散.WIFI 指紋和GPS 因子提供全局位置的約束信息,地圖因子以通過(guò)特征匹配對(duì)比地圖中儲(chǔ)存的特征點(diǎn)及其對(duì)應(yīng)的相機(jī)幀的位姿,提供回環(huán)約束.

圖5 全局位姿優(yōu)化因子圖結(jié)構(gòu)Fig.5 Global attitude optimization factor graph structure

圖6 基于語(yǔ)義信息和深度信息的視覺(jué)SLAM 場(chǎng)景自適應(yīng)算法框架Fig.6 Visual SLAM scene adaptive algorithm framework based on semantic information and depth information

本文將系統(tǒng)在第ti時(shí)刻的位姿狀態(tài)定義為：

若測(cè)量值是條件獨(dú)立的,并且噪聲服從零均值高斯分布,則MAP 估計(jì)值可以寫(xiě)成殘差平方和：

在算法體系中,各因子及其信息矩陣的定義是由信息源的數(shù)量和質(zhì)量而定.

4 視覺(jué)SLAM 動(dòng)態(tài)場(chǎng)景自適應(yīng)方法

典型的動(dòng)態(tài)SLAM 基于先驗(yàn)信息不加判斷地剔除具有動(dòng)態(tài)性的物體,這將嚴(yán)重影響SLAM 算法的導(dǎo)航精度,而傳統(tǒng)的SLAM 系統(tǒng)仍采用動(dòng)態(tài)物體進(jìn)行特征匹配和位姿估計(jì),將導(dǎo)致更大的導(dǎo)航誤差[11-12].為了解決這一問(wèn)題,本文在結(jié)合深度信息和語(yǔ)義信息,基于ORB-SLAM2 設(shè)計(jì)了面向消防員的視覺(jué)SLAM 動(dòng)態(tài)場(chǎng)景自適應(yīng)算法.ORB-SLAM2[13]是目前比較完善、成熟的視覺(jué)SLAM 系統(tǒng),主要包括追蹤定位線程（tracking）、局部地圖構(gòu)建線程（local mapping）和回環(huán)檢測(cè)線程（looping closing）,同時(shí)還維護(hù)場(chǎng)景識(shí)別中的特征點(diǎn)BoW 字典和識(shí)別數(shù)據(jù)庫(kù),可以在閉環(huán)檢測(cè)時(shí)計(jì)算關(guān)鍵幀相似度.場(chǎng)景自適應(yīng)算法過(guò)濾掉與移動(dòng)對(duì)象相關(guān)的數(shù)據(jù),從而減少SLAM 前端中不正確的數(shù)據(jù)關(guān)聯(lián).算法主要包括面向消防員自主導(dǎo)航的目標(biāo)識(shí)別和邊界分割方法,以及基于區(qū)域分割和多視圖幾何的動(dòng)態(tài)物體去除方法.

4.1 面向消防員導(dǎo)航的目標(biāo)識(shí)別算法設(shè)計(jì)

面向消防員導(dǎo)航的目標(biāo)識(shí)別融合了圖像分割與目標(biāo)識(shí)別兩個(gè)任務(wù),其目標(biāo)是將圖像分割為若干組具有某種特定語(yǔ)義含義的像素區(qū)域,并逐像素地判斷每一像素所屬的類別,最終生成標(biāo)注有像素語(yǔ)義的圖像,完成圖像的像素級(jí)分類[14-16].本文在傳統(tǒng)語(yǔ)義分割的基礎(chǔ)上,根據(jù)物體的剛性結(jié)構(gòu)進(jìn)行動(dòng)態(tài)區(qū)域分割,并結(jié)合深度信息確定區(qū)域邊界.通過(guò)MASK R-CNN[10]實(shí)例分割網(wǎng)絡(luò)對(duì)RGB 圖像進(jìn)行處理,實(shí)現(xiàn)像素級(jí)語(yǔ)義分割.在此基礎(chǔ)上,結(jié)合MASK R-CNN的目標(biāo)檢測(cè),對(duì)邊界框內(nèi)外延一定像素值之后進(jìn)行邊界分割,從而克服語(yǔ)義分割網(wǎng)絡(luò)對(duì)邊界的欠分割問(wèn)題.由于目標(biāo)識(shí)別可以完成對(duì)圖像中物體的檢測(cè)與位置識(shí)別,因此,可以用于檢測(cè)火場(chǎng)場(chǎng)景中的潛在運(yùn)動(dòng)物體.具體算法流程為：

使用大小為2×2 的滑塊遍歷深度圖像,并記錄滑塊中的深度值：

其中,（u,v）表示與滑塊左上角的像素對(duì)應(yīng)的圖像坐標(biāo).可通過(guò)以下方式獲得邊緣點(diǎn)的集合Edge：

4.2 面向消防員導(dǎo)航的目標(biāo)邊界分割算法

實(shí)例分割網(wǎng)絡(luò)可以基于先驗(yàn)信息實(shí)現(xiàn)對(duì)圖像幀中動(dòng)態(tài)對(duì)象的識(shí)別和像素級(jí)分割,而基于深度學(xué)習(xí)的目標(biāo)邊界分割算法基于目標(biāo)是凸的假設(shè),通過(guò)對(duì)比滑動(dòng)塊內(nèi)對(duì)角像素的深度,實(shí)現(xiàn)對(duì)物體的邊界檢測(cè).然后,基于Mask R-CNN 的實(shí)例分割網(wǎng)絡(luò)在處理高動(dòng)態(tài)性對(duì)象時(shí)常常無(wú)法對(duì)其邊界進(jìn)行準(zhǔn)確檢測(cè),且實(shí)時(shí)性較差.而“圖像中的目標(biāo)是凸的”的假設(shè)在某些情況下并不一定適用.

由于基于先驗(yàn)信息與基于深度學(xué)習(xí)兩種方法的優(yōu)點(diǎn)和缺點(diǎn)是互補(bǔ)的,而它們的組合使用是實(shí)現(xiàn)精確且實(shí)時(shí)性更高的邊界分割的有效方式.因此,本文設(shè)計(jì)了一種結(jié)合語(yǔ)義標(biāo)簽和深度信息的邊界分割算法,算法框圖如圖7 所示.將圖像輸入Mask R-CNN網(wǎng)絡(luò)進(jìn)行實(shí)例分割,獲得圖像中的對(duì)象數(shù)目,以及每一類對(duì)象所對(duì)應(yīng)的像素點(diǎn)和邊界框.在每個(gè)物體的邊界框的基礎(chǔ)上,對(duì)圖像所對(duì)應(yīng)的深度圖分別向兩邊擴(kuò)展n 個(gè)像素點(diǎn)（在本文中,n 取3）,然后對(duì)物體的邊界進(jìn)行分割,并于Mask R-CNN 輸出的掩模向結(jié)合,生成準(zhǔn)確的物體邊界.

圖7 結(jié)合語(yǔ)義標(biāo)簽和深度信息的邊界分割算法圖Fig.7 Boundary segmentation algorithm combining semantic labels and depth information

由圖7 可知,僅使用Mask R-CNN 網(wǎng)絡(luò),則對(duì)物體邊界無(wú)法實(shí)現(xiàn)準(zhǔn)確分割,而在Mask R-CNN 的基礎(chǔ)上結(jié)合深度信息,可以獲得較為準(zhǔn)確的動(dòng)態(tài)物體掩模,從而實(shí)現(xiàn)對(duì)動(dòng)態(tài)物體的準(zhǔn)確去除.

4.3 抑制環(huán)境動(dòng)態(tài)信息影響的視覺(jué)導(dǎo)航算法

環(huán)境中的運(yùn)動(dòng)物體對(duì)于圖像的特征匹配會(huì)產(chǎn)生較大影響且使相機(jī)視場(chǎng)被大面積遮擋,進(jìn)而影響SLAM 算法位姿估計(jì)的精度,因此,基于多視圖幾何對(duì)動(dòng)態(tài)物體檢測(cè)并基于語(yǔ)義分割和深度信息,對(duì)動(dòng)態(tài)物體邊界分割、去除.在去除運(yùn)動(dòng)目標(biāo)后對(duì)背景進(jìn)行修復(fù),通過(guò)環(huán)境靜態(tài)結(jié)構(gòu)的合成框架可提升SLAM系統(tǒng)的位姿估計(jì)精度.

動(dòng)態(tài)物體檢測(cè)：基于多視圖通過(guò)當(dāng)前幀和其高重疊關(guān)鍵幀之間的幾何約束關(guān)系,來(lái)去除圖像中的動(dòng)態(tài)特征點(diǎn).具體來(lái)講,對(duì)當(dāng)前幀與具有較高相似度的關(guān)鍵幀之間的所有特征點(diǎn)進(jìn)行匹配,并去除視差角大于30°的特征點(diǎn).這是由于當(dāng)視角大于30°時(shí),圖像中可能會(huì)出現(xiàn)對(duì)物體深度測(cè)量的遮擋,從而影響對(duì)特征點(diǎn)動(dòng)態(tài)性的判斷.利用重投影求得空間點(diǎn)的深度值Δz,并與深度圖像對(duì)比,求得深度值

當(dāng)深度差Δz 大于動(dòng)態(tài)閾值時(shí),則判定該點(diǎn)屬于動(dòng)態(tài)點(diǎn)集合Dynapoint.即

動(dòng)態(tài)物體去除：當(dāng)檢測(cè)到場(chǎng)景中的目標(biāo)為動(dòng)態(tài)性時(shí),結(jié)合4.2 節(jié)所述的物體邊界分割和多視圖幾何方法,去除該物體上面的所有特征點(diǎn),實(shí)現(xiàn)環(huán)境動(dòng)態(tài)物體去除.

動(dòng)態(tài)場(chǎng)景下圖像背景修復(fù)：在去除運(yùn)動(dòng)目標(biāo)后,根據(jù)先前的靜態(tài)觀察結(jié)果修復(fù)被遮擋的背景,可合成一個(gè)沒(méi)有動(dòng)態(tài)內(nèi)容的真實(shí)圖像,如圖8 所示.這種包含環(huán)境靜態(tài)結(jié)構(gòu)的合成框架對(duì)于SLAM 系統(tǒng)后端的循環(huán)閉合和映射非常有用.

圖8 動(dòng)態(tài)場(chǎng)景下圖像背景修復(fù)圖Fig.8 Background restoration image in dynamic scene

5 實(shí)驗(yàn)及結(jié)果

5.1 基于多源融合的導(dǎo)航算法驗(yàn)證實(shí)驗(yàn)

如圖9 所示,在某室內(nèi)復(fù)雜環(huán)境下進(jìn)行實(shí)驗(yàn).行走路線為二樓折線,中間穿插辦公室、會(huì)議室、裝備室等復(fù)雜場(chǎng)景,設(shè)定3 個(gè)標(biāo)記點(diǎn)作為誤差計(jì)算依據(jù),正常行走3 圈,實(shí)驗(yàn)的起點(diǎn)和終點(diǎn)為同一標(biāo)記點(diǎn).

圖9 室內(nèi)復(fù)雜場(chǎng)景局部位姿融合實(shí)驗(yàn)測(cè)試路線Fig.9 Experimental test route of local attitude fusion in indoor complex scene

實(shí)驗(yàn)時(shí)間約為1 200.98 s（20 min 0.98 s）,運(yùn)動(dòng)距離約為923.22 m.圖10（a）中,藍(lán)色曲線為腳部解算軌跡,圖10（b）中,紅色曲線為融合軌跡.融合結(jié)果終點(diǎn)與起點(diǎn)的水平誤差距離為0.64 m.

圖10 室內(nèi)復(fù)雜場(chǎng)景局部位姿融合實(shí)驗(yàn)結(jié)果Fig.10 Experimental results of local attitude fusion in complex indoor scenes

從實(shí)驗(yàn)結(jié)果可以看出,慣性導(dǎo)航系統(tǒng)的誤差隨時(shí)間發(fā)散,而結(jié)合慣性和視覺(jué)的局部位姿優(yōu)化抑制了導(dǎo)航誤差的發(fā)散,提高了導(dǎo)航系統(tǒng)的性能.

5.2 動(dòng)態(tài)場(chǎng)景下視覺(jué)導(dǎo)航自適應(yīng)算法驗(yàn)證實(shí)驗(yàn)

為驗(yàn)證算法對(duì)復(fù)雜室內(nèi)動(dòng)態(tài)場(chǎng)景的適應(yīng)性,本文在某實(shí)驗(yàn)室內(nèi)搭建實(shí)驗(yàn)平臺(tái),設(shè)計(jì)了室內(nèi)動(dòng)態(tài)場(chǎng)景實(shí)驗(yàn).實(shí)驗(yàn)人員攜帶RealsenseD455 深度相機(jī),從室內(nèi)一個(gè)已知起點(diǎn)出發(fā),沿著固定線路行走一個(gè)閉環(huán),最終回到起點(diǎn),實(shí)驗(yàn)時(shí)長(zhǎng)13 min 43 s,約行走43.22 m.實(shí)驗(yàn)人員不僅途經(jīng)動(dòng)態(tài)物體占主要視場(chǎng)的場(chǎng)景,而且還遇到行走中的人.

將實(shí)驗(yàn)結(jié)果與ORB-SLAM2 進(jìn)行對(duì)比.由圖11可知,在實(shí)驗(yàn)前期的靜止場(chǎng)景和低動(dòng)態(tài)場(chǎng)景下,ORB-SLAM2 和本文提出的算法都能保持較高精度,且精度相近.但大約在第783 s 時(shí),遇到動(dòng)態(tài)物體占相機(jī)大部分視野的場(chǎng)景,這直接導(dǎo)致ORB-SLAM2算法無(wú)法進(jìn)行正確的特征匹配,從而無(wú)法進(jìn)行位姿估計(jì)；本文提出的算法去除了場(chǎng)景中的動(dòng)態(tài)物體,可以克服動(dòng)態(tài)場(chǎng)景的不利影響,在復(fù)雜場(chǎng)景下依然保持較高的定位精度,實(shí)現(xiàn)了對(duì)場(chǎng)景的自適應(yīng).

圖11 動(dòng)態(tài)場(chǎng)景實(shí)驗(yàn)本文算法估計(jì)軌跡與ORB-SLAM2軌跡對(duì)比圖Fig.11 Dynamic scene experiment:comparison between the trajectory estimated by the proposed algorithm and the ORB-SLAM2 trajectory

5.3 場(chǎng)景重構(gòu)算法驗(yàn)證實(shí)驗(yàn)

實(shí)驗(yàn)人員攜帶LivoxMid-40 小型固態(tài)激光雷達(dá)、毫米波雷達(dá),以及足部安裝的微慣性傳感器在復(fù)雜室內(nèi)場(chǎng)景下采集得到相關(guān)數(shù)據(jù),利用三維場(chǎng)景重建相關(guān)算法構(gòu)建3D 室內(nèi)場(chǎng)景.

圖12（a）為整體的建圖效果,圖12（b）為點(diǎn)云地圖在實(shí)際地圖上的位置.由圖12（a）和圖12（b）可知,室內(nèi)建筑和道路部分建圖效果較好,點(diǎn)云地圖基本可以和實(shí)際地圖相匹配.

圖12 場(chǎng)景重構(gòu)效果圖Fig.12 Scene reconstruction rendering

5.4 云平臺(tái)健康監(jiān)測(cè)驗(yàn)證實(shí)驗(yàn)

消防員通過(guò)佩戴運(yùn)動(dòng)傳感器組件獲取其運(yùn)動(dòng)狀態(tài)和運(yùn)動(dòng)數(shù)據(jù)并上傳至云平臺(tái),通過(guò)云平臺(tái)對(duì)數(shù)據(jù)的分析和處理,選取典型訓(xùn)練階段的運(yùn)動(dòng)數(shù)據(jù)（2 387條）進(jìn)行指標(biāo)解算,結(jié)果如表2 所示.利用指標(biāo)結(jié)算結(jié)果,從爆發(fā)力、耐力、運(yùn)動(dòng)效率、健康檢測(cè)和傷病預(yù)警5 個(gè)方面進(jìn)行評(píng)估.根據(jù)評(píng)估結(jié)果可指導(dǎo)訓(xùn)練和健康維護(hù).

圖13（a）所示為運(yùn)動(dòng)初期的加速度特征,三軸加表數(shù)據(jù)整合峰值約150 m/s,由此可見(jiàn),受訓(xùn)人員爆發(fā)力較強(qiáng).圖13（b）所示為運(yùn)動(dòng)末期的加速度特征,加表數(shù)據(jù)整合峰值約60 m/s,對(duì)比運(yùn)動(dòng)初期和末期沖刺的加速度特征,可見(jiàn)其耐力較差.典型訓(xùn)練階段觸地騰空比為1.48,垂直幅度為8.1,故該受訓(xùn)人員運(yùn)動(dòng)效率一般,需要下一步改變運(yùn)動(dòng)姿勢(shì),提高運(yùn)動(dòng)效率.在跑步的全程,左右不平衡,左腳受力較大,說(shuō)明其右腿部有勞損,需加強(qiáng)右腿肌群的訓(xùn)練.因此,在綜合大量的運(yùn)動(dòng)數(shù)據(jù)后,可以根據(jù)運(yùn)動(dòng)特征曲線判斷潛在的運(yùn)動(dòng)風(fēng)險(xiǎn)對(duì)其進(jìn)行傷病預(yù)警.

圖13 典型路段的加速度峰值特征Fig.13 Acceleration peak characteristics of typical road sections

6 結(jié)論

本文著眼于解決消防火場(chǎng)環(huán)境下的自主導(dǎo)航與救援的問(wèn)題,重點(diǎn)開(kāi)展了基于多源信息融合的自主導(dǎo)航算法、復(fù)雜環(huán)境下視覺(jué)SLAM 自適應(yīng)方法、三維場(chǎng)景重構(gòu)方法等內(nèi)容研究,設(shè)計(jì)了一個(gè)消防員火場(chǎng)自主導(dǎo)航與搜救系統(tǒng).該系統(tǒng)基于北斗系統(tǒng)和北斗錨點(diǎn),結(jié)合慣性、UWB 和地圖約束等多源信息,實(shí)現(xiàn)對(duì)復(fù)雜火場(chǎng)環(huán)境下消防員的高精度自主導(dǎo)航定位；系統(tǒng)通過(guò)視覺(jué)SLAM、激光雷達(dá)和毫米波雷達(dá)等傳感器感知三維場(chǎng)景,實(shí)現(xiàn)火場(chǎng)煙霧環(huán)境下的場(chǎng)景重建；提出了基于語(yǔ)義信息和深度信息的視覺(jué)SLAM 場(chǎng)景自適應(yīng)方法,設(shè)計(jì)了邊界分割方法和基于運(yùn)動(dòng)一致性的動(dòng)態(tài)物體去除方法,提高了系統(tǒng)在復(fù)雜場(chǎng)景下的適應(yīng)性；在此基礎(chǔ)上利用云平臺(tái)實(shí)現(xiàn)信息的交互共享、健康監(jiān)測(cè)、人/人協(xié)同和人/機(jī)協(xié)同,來(lái)提高消防班組的救援和指揮能力.最后通過(guò)豐富的實(shí)驗(yàn)展示了本文算法的有效性.