森林消防用實時三維定位建圖系統(tǒng)*

張景利，張 鵬，吳愛枝，蔡 曄，文 明，于富才，肖文科※

（1.北京清杉科技有限公司，北京 100095；2.北京市安全生產(chǎn)科學技術(shù)研究院，北京 101100）

0 引言

近些年，國內(nèi)外森林火災(zāi)頻發(fā)，造成了嚴重的生命和財產(chǎn)損失。例如，四川涼山的森林火災(zāi)，因為山火爆燃，造成了數(shù)十人傷亡的慘痛教訓，舉國哀悼[1-2]。森林消防裝備的現(xiàn)代化是減少生命和財產(chǎn)損失的一個重要途徑；但現(xiàn)狀與指揮系統(tǒng)“信息化”的目標距離尚遠：具體表現(xiàn)在以下方面：（1）火場中缺少可靠的人員定位手段，在森林消防現(xiàn)場難以獲知消防人員的實時位置信息；（2）缺少現(xiàn)場的三維地圖，在搶險救災(zāi)中通常依賴傳統(tǒng)的紙質(zhì)地圖，不夠形象具體，缺少對災(zāi)害現(xiàn)場的實時航拍監(jiān)測。另外，現(xiàn)有裝備無法實現(xiàn)對針葉/闊葉林木分類（需要使用模式識別技術(shù)），這是因為針葉和闊葉林木的滅火方法不同。

森林消防還存在下列特點和挑戰(zhàn)：（1）需要快速可靠的方法和設(shè)備來進行應(yīng)急救援；（2）復(fù)雜三維非結(jié)構(gòu)化環(huán)境、且林木山體遮擋嚴重，增大了救援人員定位及SLAM應(yīng)用的難度；（3）現(xiàn)場多煙塵、能見度差（也可能在夜間行動及監(jiān)測，如圖1所示）、林中穿行困難。

近年來無人機技術(shù)在森林消防中的應(yīng)用日益廣泛。如圖1所示，美國加州的消防部門采用無人機在夜間航拍，監(jiān)控火災(zāi)發(fā)展態(tài)勢。但現(xiàn)階段無人機的應(yīng)用多停留在航空攝影階段，仍存在森林環(huán)境中的人員定位問題、復(fù)雜環(huán)境的快速航拍建圖以及模式識別（如針葉、闊葉林木的識別）等難題需解決。為此，北京市安全生產(chǎn)科學技術(shù)研究院聯(lián)合北京清杉科技有限公司圍繞森林消防應(yīng)急場景中林木遮擋環(huán)境下消防人員定位難，以及在復(fù)雜非結(jié)構(gòu)化災(zāi)害現(xiàn)場三維地形和態(tài)勢信息無法及時獲取的難題[3-4]，研制了森林消防用實時三維定位建圖系統(tǒng)。該系統(tǒng)中存在多架搭載無線定位基站的無人機。無人機載無線定位基站通過互相測距實現(xiàn)實現(xiàn)自主時間同步及快速自組網(wǎng)，在公里級范圍的火場內(nèi)為救援人員提供定位服務(wù)，解決森林消防應(yīng)用中因林木或山體遮擋等難以使用衛(wèi)星定位的難題。森林消防用實時三維定位建圖系統(tǒng)中還采用了基于無人機航拍的多數(shù)據(jù)融合三維重建[5]及模式識別技術(shù)[6-7]。搭載多種傳感器的無人機通過在空中巡航對災(zāi)害現(xiàn)場進行快速建模及實時災(zāi)情監(jiān)測（火勢蔓延監(jiān)測等），為指揮中心提供火場地理、狀態(tài)信息，為指揮人員判斷火情提供依據(jù)。本文將重點介紹無人機實時三維SLAM定位建圖系統(tǒng)，它是機器人領(lǐng)域的研究難題[8-10]。

圖1 美國加州的消防部門應(yīng)用無人機的情況

1 系統(tǒng)構(gòu)成

圖2所示為森林消防用SLAM定位建圖系統(tǒng)原型機，該系統(tǒng)采用模塊化設(shè)計，可適配于多種型號的無人機，包括圖中的大疆M600pro無人機。該SLAM系統(tǒng)包括下列單元模塊。

圖2 基于無人機和無線信標的森林消防用SLAM定位建圖系統(tǒng)樣機

（1）無線定位信標模塊：即機載GNSS衛(wèi)星導航定位模塊，主要獲取航拍無人機的位置與航向信息，為SLAM建圖提供地理位置信息。需要指出，本系統(tǒng)采用自主算法，在GNSS拒止或信號微弱環(huán)境中仍能很好地完成實時三維建圖任務(wù)。

（2）激光雷達：采用Velodyne多線掃描激光雷達，周圍環(huán)境可以通過激光測距的方式來感知，并形成激光點云信息。

（3）紅外/彩色相機模塊：包括彩色和紅外通道。其中彩色相機為三維建圖算法提供前端視覺特征點信息，實現(xiàn)視覺里程計功能以及點云染色/渲染功能。紅外相機獲取現(xiàn)場紅外和溫度信息（態(tài)勢感知及發(fā)展預(yù)判），并將其回傳以便后續(xù)的調(diào)度指揮。

（4）高精度IMU慣導：采用Xsens MTi300系列慣導，高精度、低漂移，為快速建圖算法提供慣導信息，包括線加速度以及角速度等。

（5）機載計算單元：基于Intel NUC電腦設(shè)計，可執(zhí)行快速SLAM定位建圖算法，并實時生成三維點云地圖。

（6）無線數(shù)傳鏈路模塊：將航拍圖像和其他傳感器等獲得的信息傳輸至遠程控制端。

（7）電源及配套模塊：為SLAM系統(tǒng)機載端供電，配套結(jié)構(gòu)采用輕量化設(shè)計，同時配有減震單元，用來降低無人機震動對航拍監(jiān)測建模效果的影響。

2 軟件及算法原理

該無人機SLAM定位建圖系統(tǒng)的軟件部分基于Ubuntu18.04系統(tǒng)開發(fā)，其中，其中利用ROS（機器人操作系統(tǒng)）進行消息映射和管理，傳感器信息讀取，以及地圖構(gòu)建等。該軟件能實現(xiàn)在機載端的三維地圖快速構(gòu)建，同時將傳感信息回傳給后方調(diào)度指揮部分的高性能服務(wù)器，進行進一步的回環(huán)檢測及地圖優(yōu)化等操作。如圖3所示，本系統(tǒng)中的SLAM算法采用魯棒可拓展的冗余設(shè)計結(jié)構(gòu)，通過合理可靠的處理IMU慣導模塊、無線信標（GNSS衛(wèi)星導航定位模塊）、激光雷達和相機模塊的數(shù)據(jù)，以有效實現(xiàn)項目指標。該算法具體包括：傳感器的預(yù)處理，前端的運動里程計，后端的數(shù)據(jù)優(yōu)化處理，檢測度高的回環(huán)檢測以及比較直觀可視化地圖構(gòu)建等多個模塊。詳細描述如下。

圖3 無人機SLAM定位建圖系統(tǒng)的軟件及算法

（1）傳感器預(yù)處理

不同傳感器的特性不同，使用方法及適用場景等都可能隨著測試環(huán)境等的改變發(fā)生變化；并且其相對位置關(guān)系及初始化配置等可能與設(shè)計圖等存在較大的相對誤差[5,10]。為保證相對精確的數(shù)據(jù)獲取，必須進行一系列初始化數(shù)據(jù)處理，提升可靠性，同時進一步保證后續(xù)算法進行處理過程中的數(shù)據(jù)穩(wěn)定性和準確性。通過比對及實驗分析，通過傳感器數(shù)據(jù)預(yù)處理能夠保證持續(xù)輸出合理有用的信息。

（2）前端里程計

通過對處理后的數(shù)據(jù)進行融合，結(jié)合不同傳感器的特性，得到實時可靠的自身位姿變化信息。慣導能獲取相對可靠的空間角速度和線加速度信息，但其存在長時間數(shù)據(jù)漂移誤差過大的問題，不足以持續(xù)輸出可靠的自身運動信息。GNSS衛(wèi)星導航定位信號能夠提供較為準確的位置航向信息，但在山林等復(fù)雜環(huán)境區(qū)域，可靠性及精度無法保證。激光點云定位能夠保證相對精確的信息，但其輸出頻率過低，無法在運行速度非?？斓臅r候輸出實時信息。而相機受視野、光照、運動等因素影響，無法保障可靠的視覺定位[11]。該模塊考慮傳感器的優(yōu)缺點，采用融合定位方法進行多傳感器間的數(shù)據(jù)處理和融合。通過慣導和無線信標進行短時間位姿信息獲取，將其作為高頻率預(yù)測信息，進一步融合頻率較低但精度較高的激光和視覺信息，進而能夠輸出6自由度的位姿信息，且能保證更新頻率和精度特性。

（3）回環(huán)檢測

由于傳感器必然會受到噪聲干擾，并且里程計（Odometry，包括激光和視覺里程計）計算過程中也存在相應(yīng)數(shù)據(jù)誤差，這些都不可避免地產(chǎn)生累計誤差，最終造成較大的輸出誤差，這對后續(xù)處理造成影響，導致較難建立一致魯棒的大場景地圖[12]。為此可通過回環(huán)檢測來解決位置估計隨時間漂移的問題，該部分算法可運行在后端服務(wù)器端。

（4）后端優(yōu)化

通過檢測到的回環(huán)數(shù)據(jù)，可對之前輸出的軌跡信息進行調(diào)整，以消除累計誤差和運動過程中的各類噪聲，輸出全局一致的位姿優(yōu)化信息。通過對信息的分析和誤差進行估計優(yōu)化，進一步進行重定位，其能夠解決狀態(tài)信息的不確定程度，即對自身運動信息和周圍環(huán)境信息進行不確定的表示，之后就能夠通過優(yōu)化的方法將這種誤差狀態(tài)進行估計，進而進行相應(yīng)的坐標轉(zhuǎn)換并將這一信息發(fā)布處理。

（5）地圖模塊

通過上述一系列模塊的處理，將自身狀態(tài)估計信息（Ego Motion Estimation）和對周圍環(huán)境的描述信息進行結(jié)合，并能夠建立全局一致的地圖。同時這種對環(huán)境信息的描述也不是一成不變的，其能夠根據(jù)不同的SLAM場景應(yīng)用而進行獲取，最后能夠處理對應(yīng)的特征地圖，包括度量地圖中的稀疏和稠密地圖，視具體情況而定。

3 實驗研究

圖4所示為本文中所開發(fā)的無人機實時三維建圖系統(tǒng)的現(xiàn)場實驗研究。圖4（a）～（b）所示為無人機遠程控制界面，以及兩次測試的飛行軌跡。圖4（c）為現(xiàn)場實驗錄像中的兩幀圖像，方框中為搭載機載三維建圖系統(tǒng)的無人機。圖4（d）為三維建圖的效果，圖中同時給出了無人機的飛行軌跡（由SLAM算法給出），它與機載GPS反饋的無人機飛行軌跡相同。

需要指出：在整個測試過程中（約5 min），有意關(guān)閉SLAM系統(tǒng)中的無線信標（即GNSS定位信號），也就是SLAM算法在融合過程中沒有使用任何外界輔助定位信息（例如，傳統(tǒng)建圖方法中十分依賴的GNSS/RTK衛(wèi)星定位信息），來驗證算法的魯棒性和可靠性。實驗表明：本系統(tǒng)采用獨有SLAM建圖算法，不使用GNSS定位仍能達到良好的建圖效果。也就是說本系統(tǒng)在GNSS信號微弱/拒止環(huán)境中仍能正常工作。

本文所敘述的SLAM定位建圖系統(tǒng)采用模塊化設(shè)計，可以根據(jù)實際需求來對軟硬件系統(tǒng)進行定制。同時，該系統(tǒng)也可采用無人車/機器人搭載或者人工背負的形式，來實現(xiàn)實時三維建圖。圖5所示為北京清杉科技有限公司研發(fā)的背包式建圖系統(tǒng)樣機，在建圖過程中，用戶可通過手持顯示交互端實時監(jiān)控建圖效果。需要指出，該系統(tǒng)在GNSS衛(wèi)星導航信號微弱或拒止環(huán)境中仍可正常工作，可實現(xiàn)大場景定位及三維建圖。

表1所示為本SLAM建模定位系統(tǒng)在不同距離軌跡下的精度分析。其中，Max為誤差的最大值，Mean為誤差的平均值，Median為誤差的中位數(shù)，而STD為誤差的標準差?？梢钥闯?，建模設(shè)備的最大建模誤差可以穩(wěn)定維持在1 m以內(nèi)。

4 結(jié)束語

圖4 無人機實時三維建圖實驗研究及效果演示

圖5 背包式實時三維建圖及效果演示

表1 誤差分析

針對森林消防和應(yīng)急救援中對復(fù)雜非結(jié)構(gòu)化災(zāi)害現(xiàn)場的快速建模問題，研發(fā)了一種基于無人機和無線信標（GNSS導航）的森林消防用實時三維SLAM定位建圖系統(tǒng)。文中詳細介紹了系統(tǒng)組成和算法原理等，并對該系統(tǒng)進行了實地測試。實測表明：該SLAM系統(tǒng)能夠?qū)崿F(xiàn)對森林環(huán)境的實時三維建圖，特別是在GNSS衛(wèi)星導航信號拒止/微弱環(huán)境中仍能達到良好的實時三維建圖效果。該系統(tǒng)將會是森林消防中的有力工具，其對于應(yīng)急管理的信息化建設(shè)也有十分重要的意義。

未來，還可將背包式建圖（或采用地面無人車/機器人搭載）與無人機建圖系統(tǒng)結(jié)合起來，實現(xiàn)功能互補。在使用過程中，無人機可精確測高，能看到樹頂和樹冠，但不能完全看到樹干；而地面端的背包式系統(tǒng)在森林中可看到樹干，采用RANSAC和ICP迭代匹配算法，可將無人機與地面端的三維地圖及地理信息匹配起來，進一步提高系統(tǒng)表現(xiàn)。