多傳感器融合的運(yùn)輸機(jī)器人導(dǎo)航系統(tǒng)＊

劉孝童，孫浩宇，王小橋，彭昕月，虞玲

（安徽理工大學(xué)電氣與信息工程學(xué)院，安徽淮南 232001）

在當(dāng)今社會(huì)快速發(fā)展下，具有自主導(dǎo)航和環(huán)境感知能力的無(wú)人機(jī)在物流、交通、監(jiān)控等工業(yè)領(lǐng)域有著廣闊的應(yīng)用前景。目前的工業(yè)無(wú)人機(jī)大多局限于有GPS 數(shù)據(jù)的戶外使用。雖然無(wú)人駕駛飛行器室內(nèi)自主導(dǎo)航已經(jīng)在機(jī)器人方面得到了積極的研究，但實(shí)用化的工業(yè)案例還很少。

在倉(cāng)庫(kù)中使用無(wú)人機(jī)進(jìn)行物品庫(kù)存檢查與人工檢查方式相比有很大優(yōu)勢(shì)，它可以節(jié)省時(shí)間和成本，并避免了工人在快速移動(dòng)的高位叉車上進(jìn)行人工條形碼掃描時(shí)受傷的風(fēng)險(xiǎn)。然而，大多數(shù)的商用（非自主）無(wú)人機(jī)不適合在倉(cāng)庫(kù)中使用，因?yàn)闊o(wú)人機(jī)操控員很難進(jìn)行安全的遠(yuǎn)程操作。一些無(wú)人機(jī)制造商給出用于清點(diǎn)任務(wù)的無(wú)人機(jī)解決方案，但目前沒有關(guān)于倉(cāng)庫(kù)實(shí)際應(yīng)用的文章及方案，因?yàn)椴话l(fā)生碰撞的安全操作相當(dāng)困難。

為了安全和成功地進(jìn)行室內(nèi)倉(cāng)庫(kù)的庫(kù)存檢查，設(shè)計(jì)了一種基于EKF 的多傳感器融合框架，該框架使用了低成本的傳感器，包括3 個(gè)攝像機(jī)、1 個(gè)二維激光掃描儀、1 個(gè)一維距離傳感器和1 個(gè)慣性測(cè)量單元。提出了兩種魯棒的數(shù)據(jù)融合方法：①利用馬氏范數(shù)分量檢驗(yàn)有效地剔除離群點(diǎn)；②通過引入“偽協(xié)方差”將視覺SLAM 添加到測(cè)量更新中。

設(shè)計(jì)的系統(tǒng)主要特點(diǎn)如下：①提出了一種低成本的多傳感器系統(tǒng)和基于擴(kuò)展卡爾曼濾波的多傳感器融合框架，以此來(lái)獲得最優(yōu)的位姿估計(jì)。②提出了魯棒數(shù)據(jù)融合方法，增強(qiáng)了魯棒性，消除了碰撞風(fēng)險(xiǎn)，保證了物料倉(cāng)庫(kù)內(nèi)庫(kù)存檢查的成功。

1 相關(guān)工作

若干課題組已經(jīng)發(fā)布了針對(duì)倉(cāng)庫(kù)庫(kù)存應(yīng)用的自主無(wú)人機(jī)系統(tǒng)的開發(fā)結(jié)果。文獻(xiàn)[1]提出了用于倉(cāng)庫(kù)內(nèi)部操作的高性能自主清點(diǎn)無(wú)人機(jī)。其傳感器設(shè)置包括RFID 讀取器、2 個(gè)高分辨率攝像頭和1 個(gè)昂貴的3D 激光雷達(dá)，并在一個(gè)具有外部倉(cāng)庫(kù)管理系統(tǒng)和高級(jí)檢查任務(wù)的可操作倉(cāng)庫(kù)中進(jìn)行實(shí)驗(yàn)。他們的方法還需要定期構(gòu)建3D 特征地圖，并且有將語(yǔ)義地圖[2]與3D 特征地圖對(duì)齊的步驟。文獻(xiàn)[3]提出了一種導(dǎo)航系統(tǒng)，使無(wú)人機(jī)能夠在黑暗和沒有GPS 定位的環(huán)境（如倉(cāng)庫(kù)）中操作，以保證人員的安全。他們使用2 個(gè)2D 激光雷達(dá)和1 個(gè)攝像頭進(jìn)行標(biāo)記識(shí)別。

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

本文所設(shè)計(jì)系統(tǒng)的總體結(jié)構(gòu)如圖1 所示，其中包括多個(gè)激光雷達(dá)和攝像頭傳感器、連接到傳感器的軟件算法模塊、基于EKF 的多傳感器融合框架[4-5]和其他額外組件等。此外，圖1 中灰色虛線矩形內(nèi)為魯棒數(shù)據(jù)融合方法，此方法將在后續(xù)詳細(xì)說(shuō)明。

圖1 自主無(wú)人機(jī)的總體系統(tǒng)架構(gòu)

從傳感器采集的數(shù)據(jù)通過相應(yīng)的算法以相對(duì)/絕對(duì)姿態(tài)、角度或距離的形式饋入基于EKF 的測(cè)量更新模塊[6]。參考文獻(xiàn)[7]提供了無(wú)人機(jī)的六維相對(duì)姿勢(shì)，參考文獻(xiàn)[8]識(shí)別算法給出了它的六維絕對(duì)姿勢(shì)。

基于EKF 的多傳感器融合框架與魯棒數(shù)據(jù)融合方法相結(jié)合，將最優(yōu)估計(jì)的無(wú)人機(jī)姿態(tài)發(fā)送給飛行控制器和任務(wù)規(guī)劃，F(xiàn)CU 根據(jù)反饋的姿態(tài)數(shù)據(jù)輸出四旋翼框架的低速電機(jī)命令，穩(wěn)定飛行。

設(shè)計(jì)一個(gè)多傳感器融合框架[9]，以優(yōu)化估計(jì)無(wú)人機(jī)六維姿態(tài)在室內(nèi)（沒有GPS）環(huán)境。以EKF 為基礎(chǔ)，其中IMU位姿作為時(shí)間更新階段預(yù)測(cè)的狀態(tài)變量。

假設(shè)IMU 傳感器的位姿與無(wú)人機(jī)傳感器的位姿相同。卡爾曼濾波器的狀態(tài)由IMU在世界坐標(biāo)系中的位置向量{W}構(gòu)成，其速度矢量，其取向四元數(shù)描述IMU 對(duì)W的旋轉(zhuǎn)，陀螺偏差bω和加速度計(jì)偏差ba。整個(gè)狀態(tài)產(chǎn)生一個(gè)十六維狀態(tài)向量x：

推導(dǎo)出以下微分方程：

3 魯棒數(shù)據(jù)融合

提出兩種魯棒數(shù)據(jù)融合方法[10]來(lái)提高EKF 的魯棒性，以克服漂移、異常值和干擾問題。這些都有助于多傳感器融合框架成功地應(yīng)對(duì)具有挑戰(zhàn)性的倉(cāng)庫(kù)環(huán)境。

3.1 馬氏范數(shù)檢驗(yàn)剔除離群值

離群值通常來(lái)自傳感器故障、較大的識(shí)別誤差或惡劣的測(cè)量環(huán)境。為了剔除這種測(cè)量異常值，計(jì)算和監(jiān)控測(cè)量更新的馬氏范數(shù)M：

式（7）中：z為測(cè)量向量；H為線性化后的測(cè)量向量狀態(tài)估計(jì)矩陣；x?為狀態(tài)預(yù)測(cè)；S為狀態(tài)協(xié)方差矩陣。

所以在飛行任務(wù)期間單獨(dú)檢查該規(guī)范的特定組成部分。如果它們中的一些大于預(yù)定閾值，本文采用的EKF 框架將拒絕測(cè)量的相應(yīng)部分。這種基于分量的馬氏范數(shù)測(cè)試通過將測(cè)量向量分離成更小的維向量來(lái)降低錯(cuò)誤拒絕率。

3.2 利用Visual SLAM 實(shí)現(xiàn)偽協(xié)方差

為了極大限度地減少激光掃描儀的Hector-SLAM（通常發(fā)生在垂直移動(dòng)期間）造成的干擾影響，加入了視覺SLAM算法，該算法將x、y、z位置和偏航方向數(shù)據(jù)融合到擴(kuò)展卡爾曼濾波中，以減弱來(lái)自Hector-SLAM 或其他未知源的干擾影響。

由于基于關(guān)鍵點(diǎn)的視覺SLAM，不提供輸出的協(xié)方差，這是EKF 的測(cè)量更新階段所需的。因此，采用運(yùn)動(dòng)模型中的匹配點(diǎn)數(shù)量，如圖2 所示，來(lái)提取ORB-SLAM2 的不確定性信息，即“偽協(xié)方差”。通過反復(fù)實(shí)驗(yàn)和調(diào)整過程，得到了匹配點(diǎn)數(shù)目和偽協(xié)方差之間的最佳比例常數(shù)和偏移量。運(yùn)動(dòng)模型中匹配點(diǎn)的數(shù)目為圖2 的288 個(gè)和圖3 的47 個(gè)。隨著位姿估計(jì)次數(shù)的減少，位姿估計(jì)的不確定性有增大的趨勢(shì)。因此，使用這個(gè)數(shù)字來(lái)計(jì)算偽協(xié)方差引入EKF 以減少干擾的影響。

圖2 向上攝像機(jī)未改進(jìn)掃描結(jié)果

圖3 向上攝像機(jī)的ORB SLAM2 結(jié)果

4 結(jié)束語(yǔ)

本文設(shè)計(jì)了一種具有低成本和多傳感器融合框架的自主無(wú)人機(jī)，可以有效用于狹窄和黑暗的倉(cāng)庫(kù)環(huán)境。為了解決普通的無(wú)人機(jī)定位方法存在的問題，提出了魯棒數(shù)據(jù)融合方法，基于分量的馬氏范數(shù)檢驗(yàn)剔除離群值，通過引入偽協(xié)方差來(lái)結(jié)合視覺SLAM。這使自主飛行任務(wù)更安全，有利于在倉(cāng)庫(kù)進(jìn)行循環(huán)庫(kù)存檢查。作為一個(gè)完全自主的系統(tǒng)，這種方法不需要人工操作，同時(shí)也節(jié)省了時(shí)間和成本，并且解決了工人高空危險(xiǎn)作業(yè)的問題。下一步將繼續(xù)對(duì)系統(tǒng)進(jìn)行改進(jìn)研究，進(jìn)一步降低成本，并將此方法推廣到各種室內(nèi)和室外應(yīng)用中。