倪 朋，馬國(guó)軍，習(xí)朝輝，馬玉皓，王亞軍

（1.江蘇科技大學(xué)電子信息學(xué)院，江蘇 鎮(zhèn)江 212003；2.北方自動(dòng)控制技術(shù)研究所，太原 030006；3.解放軍32381 部隊(duì)，北京 100072）

0 引言

機(jī)器人在運(yùn)動(dòng)過程中需要利用自身傳感器獲取周圍的環(huán)境信息并創(chuàng)建地圖，進(jìn)而確定自己的位置，因此同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）具有重要的意義。機(jī)器人通過“感知-預(yù)測(cè)-校正”不斷更新信息，從而對(duì)自身位置的預(yù)測(cè)更加準(zhǔn)確［1］。

卡爾曼濾波以最小線性方差為基礎(chǔ)，運(yùn)用逐步迭代的方式預(yù)測(cè)狀態(tài)從而減少噪聲影響，但僅適用于線性系統(tǒng)。擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）算法雖然方法簡(jiǎn)單易于實(shí)現(xiàn)，但是存在計(jì)算量大，非線性精度差的缺點(diǎn)［2］。無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）采取對(duì)函數(shù)的概率密度分布求近似的方法，保留高階項(xiàng)，解決了擴(kuò)展卡爾曼濾波精度差的缺點(diǎn)［3］。

UKF 是一種非線性動(dòng)態(tài)系統(tǒng)狀態(tài)估計(jì)方法。但是當(dāng)系統(tǒng)模型存在不確定性時(shí)，UKF 的結(jié)果會(huì)退化甚至發(fā)散。文獻(xiàn)［4-7］針對(duì)這些問題進(jìn)行了研究。

針對(duì)機(jī)器人SLAM 中計(jì)算復(fù)雜、定位精度差、獲取地圖中路標(biāo)信息少的問題，考慮到單發(fā)多框探測(cè)器（Single Shot MultiBox Detector，SSD）可以檢測(cè)到物體的形狀，為此結(jié)合SSD 算法進(jìn)行SLAM 研究。本文在UKF-SLAM 算法的基礎(chǔ)上，通過SSD 算法進(jìn)行目標(biāo)檢測(cè)并獲取路標(biāo)信息，然后加入到UKF-SLAM 算法中，提高SLAM 的精度。

1 SSD 目標(biāo)檢測(cè)算法

基于深度學(xué)習(xí)的目標(biāo)檢測(cè)是機(jī)器人SLAM 的研究方向之一。SSD 作為主要的目標(biāo)檢測(cè)框架之一，其無(wú)需生成候選框，把目標(biāo)邊框定位轉(zhuǎn)變成回歸問題進(jìn)行解決，是一種直接預(yù)測(cè)目標(biāo)類別和邊界框的目標(biāo)檢測(cè)方法。SSD 通過提取不同尺度圖像的特征，減少訓(xùn)練參數(shù)量，同時(shí)提高準(zhǔn)確度和實(shí)時(shí)性。

為提高檢測(cè)的效率，SSD 對(duì)不同大小的卷積層輸出結(jié)果進(jìn)行回歸處理，同時(shí)改變檢測(cè)器寬度和高度的比值，從而增加檢測(cè)目標(biāo)的種類［8-9］。

1.1 網(wǎng)絡(luò)結(jié)構(gòu)

SSD 選取VGG16 作為基本框架，在VGG16 框架上增加卷積層以得到更加豐富的特征圖進(jìn)行目標(biāo)檢測(cè)。SSD 的網(wǎng)絡(luò)結(jié)構(gòu)［9］如圖1 所示。

圖1 SSD 網(wǎng)絡(luò)結(jié)構(gòu)

1.2 損失函數(shù)

SSD 構(gòu)建的損失函數(shù)與目標(biāo)搜索框的位置及類別置信度有關(guān)，以利于提高優(yōu)化速度和增加訓(xùn)練結(jié)果的穩(wěn)定性，它是定位損失（locatization loss）與置信度損失（confidence loss）的加權(quán)和，表示為

其中，N 為匹配的默認(rèn)框數(shù)量，c 代表置信度，l 是預(yù)測(cè)框，g 是真實(shí)框，x 用來(lái)判斷預(yù)測(cè)框和真實(shí)框是否匹配，Lconf是置信度損失，Lloc是定位損失，α 為權(quán)衡定位損失和置信度損失的加權(quán)值。

2 基于UKF 的SLAM 算法

2.1 SLAM 系統(tǒng)模型

機(jī)器人傳感器在位置pi處觀測(cè)到的數(shù)據(jù)為zk，i，nk，i為觀測(cè)噪聲，則觀測(cè)方程為

當(dāng)機(jī)器人采用深度相機(jī)傳感器時(shí)，其獲取的圖像中包含機(jī)器人到目標(biāo)的距離信息，利用SSD 算法檢測(cè)場(chǎng)景中的目標(biāo)作為路標(biāo)信息，最后將路標(biāo)信息加入到SLAM 算法中，算法總體流程如圖2 所示。

2.2 無(wú)跡變換

1）計(jì)算得到2n+1 個(gè)采樣點(diǎn)，此處n 為狀態(tài)的維度；

圖2 算法流程圖

2）計(jì)算采樣點(diǎn)對(duì)應(yīng)的權(quán)值。

2.3 UKF-SLAM 算法

隨機(jī)變量X 和觀測(cè)變量Z 包含的高斯白噪聲分別為W（k）和V（k），k 取不同值時(shí)，X 和Z 組成的非線性系統(tǒng)為

式中，f 代表非線性狀態(tài)函數(shù)，h 代表非線性觀測(cè)函數(shù)［10］。假設(shè)W（k）的協(xié)方差陣為Q，V（k）的協(xié)方差陣為R。隨機(jī)變量X 的無(wú)跡卡爾曼濾波的過程如下所示。

1）根據(jù)式（5）和式（6）獲取采樣點(diǎn)和相應(yīng)的權(quán)值；

2）計(jì)算2n+1 個(gè)采樣點(diǎn)集合的一步預(yù)測(cè)結(jié)果，i=1，2，…，2n+1；

4）由一步預(yù)測(cè)方程，運(yùn)用無(wú)跡變換，生成新的采樣點(diǎn)集合；

5）根據(jù)預(yù)測(cè)的采樣點(diǎn)集合的觀測(cè)方程，得出預(yù)測(cè)的觀測(cè)值，i=1，2，…，2n+1；

6）根據(jù)得出的采樣點(diǎn)集合的預(yù)測(cè)結(jié)果，由加權(quán)求和可得系統(tǒng)預(yù)測(cè)的均值和協(xié)方差；

7）計(jì)算得到卡爾曼增益矩陣；

8）求解得到系統(tǒng)的狀態(tài)更新以及協(xié)方差更新。

3 實(shí)驗(yàn)結(jié)果與分析

3.1 實(shí)驗(yàn)環(huán)境

本文使用Intel Realsense D435i 深度相機(jī)，實(shí)物如圖3 所示，尺寸為90 mm×25 mm×25 mm，并且搭載了慣性傳感單元（Inertial Measurement Unit，IMU）。Intel Realsense D435i 采用結(jié)構(gòu)光技術(shù)，可以直接得到相機(jī)到目標(biāo)物體的距離。

圖3 Inter Realsense D435i 深度相機(jī)

實(shí)驗(yàn)場(chǎng)景和示意圖分別如圖4 和圖5 所示，4個(gè)路標(biāo)分別為“瓶”、“椅子”、“寵物貓”、“寵物鳥”。實(shí)驗(yàn)過程中，利用深度相機(jī)獲取路標(biāo)的信息，以靠近墻面的起始點(diǎn)為原點(diǎn)，選取寬200 cm，長(zhǎng)500 cm的區(qū)域進(jìn)行實(shí)驗(yàn)。

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

圖5 實(shí)驗(yàn)示意圖

3.2 實(shí)驗(yàn)數(shù)據(jù)及結(jié)果分析

由Realsense D435i 相機(jī)獲取的深度圖像需要經(jīng)過配準(zhǔn)，以生成配準(zhǔn)深度圖像。通過配準(zhǔn)，將深度圖和彩色圖對(duì)齊，結(jié)果如圖6 所示。

圖6 彩色圖和深度圖配準(zhǔn)后的結(jié)果

利用SSD 模型來(lái)檢測(cè)場(chǎng)景中的目標(biāo)。由于彩色圖和深度圖已經(jīng)配準(zhǔn)對(duì)齊，可以將彩色圖檢測(cè)到的邊界框投影到深度圖，如圖7 所示。對(duì)其中的深度數(shù)據(jù)求平均值，得到相機(jī)到目標(biāo)距離，進(jìn)而得到路標(biāo)的位置坐標(biāo)。

圖7 瓶的檢測(cè)結(jié)果

測(cè)量得到瓶的坐標(biāo)為（53，23），單位為厘米。然后，對(duì)路標(biāo)“椅子”、“寵物貓”、“寵物鳥”進(jìn)行測(cè)量，坐標(biāo)分別為（452，20），（443，185），（52，182）。

根據(jù)測(cè)量得到的數(shù)據(jù)，分別以EKF-SLAM 和UKF-SLAM 算法為基礎(chǔ)進(jìn)行仿真。設(shè)機(jī)器人初始位姿為（0，0），沿設(shè)定的路徑逆時(shí)針移動(dòng)。速度為3 cm/s，附加零均值且方差為0.1 的高斯噪聲，系統(tǒng)協(xié)方差初始化為0。仿真結(jié)果如圖8 所示，其中“*”表示路標(biāo)點(diǎn)，虛線表示真實(shí)的軌跡，實(shí)線表示估計(jì)的結(jié)果。

圖8 SLAM 結(jié)果圖

為評(píng)估算法精度，分別計(jì)算X 軸和Y 軸方向的誤差，對(duì)比EKF-SLAM 算法和UKF-SLAM 算法的精度，結(jié)果如圖9 所示。虛線為EKF-SLAM 的誤差，實(shí)線為UKF-SLAM 的誤差。

圖9 誤差結(jié)果圖

由圖9 可以看出UKF-SLAM 的誤差小于EKF-SLAM。分別計(jì)算X 軸方向和Y 軸方向的平均誤差，如表1 所示。

由表1 可知，對(duì)于EKF-SLAM 算法，X 軸方向的平均誤差為1.81 cm，Y 軸方向的平均誤差為3.07 cm；對(duì)于UKF-SLAM 算法，X 軸方向的平均誤差為0.55 cm，Y 軸方向的平均誤差為2.68 cm?？傮w來(lái)說，UKF-SLAM 的誤差小于EKF-SLAM 的誤差。

表1 算法結(jié)果對(duì)比

4 結(jié)論

本文提出融合深度學(xué)習(xí)和UKF 的機(jī)器人SLAM 方法，通過Intel Realsense D435i 深度相機(jī)獲取彩色圖和深度圖，并將彩色圖和深度圖進(jìn)行配準(zhǔn)；利用深度學(xué)習(xí)SSD 算法獲取路標(biāo)坐標(biāo)并加入到UKF-SLAM 算法中，提高了SLAM 的精度。

隨著人工智能技術(shù)的發(fā)展，今后將通過研究輕量化的深度學(xué)習(xí)及SLAM 方法，以提高移動(dòng)機(jī)器人獲取路標(biāo)的速度和準(zhǔn)確度，進(jìn)一步提升SLAM的效率。