基于擴展卡爾曼濾波的2D激光SLAM研究

王 博，張國偉，盧秋紅，徐曉康，疏雅麗

（1.上海電力大學自動化工程學院，上海 200082；2.上海合時智能科技有限公司，上海 201100）

0 引言

隨著國家科技的發展以及戰略方案的引導，無人車、無人機等智能移動機器人設備的應用場景日益廣泛，涵蓋了工業、農業、軍事、日常使用等方面。無人清潔洗地車作為移動機器人的一個分支，在國家提出的“中國制造2025”大戰略背景的情況下誕生。放眼環境衛生領域，人們對環境衛生的要求在不斷提高，加之近年來人力成本在逐年上升，無人智能清潔設備取代傳統方式進行清潔作業將成為發展的主流趨勢，人工智能和清潔設備的結合，也是技術和思維融合的一個過程。針對一些大型公共場所如商場、停車場、展覽館等，如果配置較多的作業人員進行場地清潔，人工成本勢必會大大增加。而使用智能清潔車進行作業，預先設定工作任務，不受時間和人員等不穩定因素的限制，有效地提高了工作效率。

無人智能清潔車中核心技術就是無人駕駛概念，這也使得同時定位與地圖構建（SLAM）技術成為熱點問題。SLAM 問題最初是在1988 年由Smith 等人提出。SLAM 作為移動機器人的核心技術，在過去的幾十年里發展迅速，并且已經完成了很多開源算法以及系統構建。激光SLAM 方法主要基于濾波器和圖優化兩種求解方法，其中基于濾波器的方法主要包含了2002 年的Fast-SLAM，2007 年的Gmapping 等開源算法。此后主要的研究方向轉向了基于圖優化的方法，主要包含了2010 年的KartoSLAM，2016年的Gartograph等開源算法。

卡爾曼濾波狀態估計在SLAM問題中主要作為傳感器數據融合手段。激光傳感器可靠性高精度高且具有深度信息，但其垂直分辨率低只能獲得稀疏點云，并且價格昂貴。視覺傳感器可以獲得環境語義動態等豐富的信息，并且可以實現人臉識別、物體追蹤等擴展功能，但其受光照、運動等噪聲影響較大，魯棒性較差。超聲波傳感器具有短距離測量高精度的特點，由于其在空氣中傳播損失較大，無法在大范圍環境穩定工作。IMU 傳感器模塊集成陀螺儀、加速度計、磁場儀等可以提供高精度的姿態信息。編碼器里程計可以獲得機器的準確位置信息，但增量式缺少位置記憶功能。因此多傳感器的融合方法綜合各種傳感器優勢，有效提高了SLAM問題中移動機器人定位精度，以及在復雜環境的噪聲干擾下的魯棒性。目前，SLAM 問題中的多傳感器融合技術也是國內外學者的研究熱點，2019 年張艷國等提出了IMU融合激光構建稀疏點云地圖方法，依據歷史點云記錄，有效提高了傳感器對環境信息的獲取。2018 年潘爽等提出了擴展卡爾曼濾波進行聯接式的IMU 以及聲吶傳感器融合方案解決水下SLAM 技術，有效提高了水下機器人自主導航的定位精度。本文基于上海合時智能科技公司的無人清潔車作為研究硬件平臺，對無人車進行SLAM 數學模型建立，在此基礎上完成基于擴展卡爾曼濾波的多傳感器融合理論以及程序實現，同時在地下車庫完成實際運行試驗并對結果進行數據分析說明。

1 SLAM系統

1.1 數據模型及問題描述

SLAM 問題可以描述為：無人車在未知環境下開始運行，在運行過程中通過自身位置的估算以及路標觀測信息，構建出一個增量式的地圖，在此基礎上可以實現機器人自身定位以及導航。根據該問題可以定義兩個變量分別為運動狀態量以及觀測量：

其中，狀態變量指每一個時刻下無人車運動傳感器的數據，包括位置速度偏航等信息，為噪聲干擾。觀測量指當前狀態下相對于路標點的觀測值，對于不同傳感器有所不同，為噪聲干擾。

此時需要完成對無人車的狀態估計，將SLAM問題轉化為求后驗概率的問題：

對于直接求解后驗概率十分困難，根據貝葉斯推斷可知，求一個狀態最優估計使得在該狀態下后驗概率最大（maximize a posterior，MAP）是可行的：

由于貝葉斯公式右側的分母和狀態無關，后驗概率最大化無需考慮分母，此時只需要當前時刻的狀態，又可寫為：

綜上所述，在對SLAM問題的運動學以及觀測模型建模后，SLAM 問題的本質即為求解貝葉斯公式最大后驗概率。

1.2 SLAM系統框架

激光SLAM系統框架主要如圖1所示。

圖1 SLAM框架

根據框圖可以將整個SLAM 流程分為以下部分：

（1）傳感器信息讀取。在激光SLAM 中主要為激光雷達的掃描信息和預處理，在無人車中還包含了編碼器、IMU 等傳感器信息的讀取和同步。

（2）掃描匹配。掃描匹配主要根據2D 激光點云對相鄰時刻的無人車運動的狀態估計以及局部地圖的構建。

（3）精確定位。精確定位主要包含了后端優化，接收不同時刻的點云信息，以及回環檢測的信息，可以獲得全局一致的軌跡和地圖。對于優化方法，主要包含了濾波以及圖優化等方案。

（4）回環檢測。回環檢測用于判斷無人車是否曾經到達過先前位置，根據信息提供可以進一步進行處理。

（5）地圖構建。根據估計的軌跡以及自身的位姿信息建立未知環境下的地圖。對于不同的傳感器建立的地圖可能包含2D 柵格地圖、拓撲地圖、網格地圖等，對于本文實驗平臺只在低矮平面中運動的無人清潔車，根據2D 激光傳感器可以構建2D 柵格地圖，可以滿足多種任務的實現。

（6）路徑規劃。在地圖和定位精確的基礎上，無人車可以根據規劃路徑完成自主循跡任務，同時可以實現實時避障等功能。

2 卡爾曼濾波

卡爾曼濾波是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。在SLAM問題的觀測系統中容易受到環境噪聲的干擾影響，所以系統狀態估計也可以看作是濾波的過程。本文利用卡爾曼濾波對無人車的目標位置數據進行追蹤，由于激光傳感器、IMU 以及編碼器里程計的相關觀測值均會受到環境或者本身特性的噪聲影響，利用濾波器將無人車的動態信息和觀測結果相結合，從而有效抑制噪聲。

2.1 卡爾曼濾波器原理

使用卡爾曼濾波算法主要由兩個階段構成一個迭代循環，一個是預測階段，一個是更新階段。其流程如圖2所示。

圖2 卡爾曼濾波流程

2.1.1 預測部分

卡爾曼濾波器更新初始值預測新值，根據系統中存在的各種過程噪聲引入預測中的不確定性。

預測狀態：

其中即位置和速度，利用狀態轉移矩陣，再加上外部影響，預測下一時刻的狀態。

預測誤差的協方差矩陣：

其中，狀態向量中的兩個變量（位置）和（速度）存在相關性，用誤差協方差矩陣表示。而外部一些無法檢測的干擾當作協方差為的噪聲進行處理。為過程噪音協方差矩陣，增加不確定性。

2.1.2 更新部分

更新步驟中，根據無人清潔車上傳感器獲取的實際測量值，通過計算預測值和實際測量值之間的差值，利用卡爾曼增益來決定預測值或者測量值的權重。根據卡爾曼增益的決策，迭代計算新值和新的不確定性。

測量和預測值之差：

其中是測量向量，表示傳感器測量的數據（本文利用激光雷達和編碼器里程計測量的無人車位置速度信息，IMU 測量角度信息）。為測量值和更新后的預測值之間存在的誤差。

計算卡爾曼增益的中間變量：

其中傳感器測量由于存在噪聲導致測量結果存在噪聲協方差，此處R為位置信息的方差。

卡爾曼增益：

更新狀態矩陣：

更新預測誤差協方差矩陣：

卡爾曼濾波算法如下：

卡爾曼濾波算法

2.2 傳感器融合

使由于單一傳感器在移動無人車SLAM的問題解決上有所局限，本文結合激光、IMU、輪式里程計多種傳感器的優勢對無人洗地車的定位問題進行優化。利用激光傳感器獲取無人車當前位姿信息，IMU 獲得無人車航向信息以及航向角速度，輪式里程計獲得無人車線速度信息。多傳感器融合框圖如圖3所示。

圖3 多傳感器融合流程

3 擴展卡爾曼濾波

該無人清潔車在實際環境運行下為非線性系統，此時數據不符合高斯分布，因此無法直接利用卡爾曼濾波進行數據融合，針對此提出擴展卡爾曼濾波方法。擴展卡爾曼濾波是標準卡爾曼濾波在非線性情況下的擴展形式，利用泰勒級數展開將非線性系統線性化，將系統方程和狀態方程改寫為雅可比矩陣形式，從而進行后驗估計。

在卡爾曼濾波基礎上引入非線性模型：

此時擴展卡爾曼濾波算法流程為：

擴展卡爾曼濾波算法

本文設計針對無人清潔車進行運動學建模分析，通過激光雷達，IMU 以及輪式里程計數據進行狀態更新。假定以地面-無人車坐標系，無人清潔車的運動學模型觀測矩陣為：

實驗平臺為兩輪驅動差速無人車模型，根據輪式編碼器可以獲得其左右輪線速度分別為v，v，輪間距為。由此可得無人車線速度和角速度：

根據線速度和角速度可得無人車圓弧運動半徑：

可得圓心坐標為

則+Δ時刻：

由此推出運動模型的雅可比矩陣：

將其代入公式（13）中可以得到非線性模型，至此完成了擴展卡爾曼濾波中核心的非線性模型推導。而在實際工程應用中，由于算法時間復雜度較大，遇到復雜工作環境時運行負載過重，針對時間間隔引入趨于無窮小，針對誤差的多方面來源引入加速度方差作為過程誤差協方差矩陣。在保證定位精度的情況下，提高運行效率。

4 實驗

4.1 實驗系統設計

本提本文采用如圖所示無人清潔車作為實驗平臺，重點研究無人清潔車在車庫清潔工作環境下的激光SLAM多傳感器融合問題。

圖4 無人清潔車平臺

該無人清潔車實驗平臺根據其執行功能邏輯可以劃分為四個部分，分別為指令層、算法層、控制層以及硬件層。其中，硬件層主要包括上海合時科技智能有限公司的無人清潔車平臺，具有兩個驅動輪，兩個隨動輪，利用差速驅動實現對無人車的運動控制，承載性以及穩定性較好。同時驅動電機帶有2500 線增量式編碼器，通過CAN 總線在一定頻率內向主控設備反饋位置信息；傳感器采用德國sick 公司的TiM571 型號2D 激光傳感器，開啟角度為水平270°，掃描頻率15 Hz，角度分辨率為0.33°，可完成掃描0.05 ～25 m 之間的物體，通過以太網接口向主控設備返回掃描信息，激光傳感器安裝在無人清潔車頭部，保證其掃描范圍內不被遮擋；IMU 采用廣州維特智能公司的HWT901B九軸姿態角度傳感器，模塊集成高精度陀螺儀、加速度計以及地磁場傳感器，數據頻率為0.1 ～200 Hz 可調，通過CAN 總線向主控設備返回姿態信息。控制層主要為從無人清潔車底層獲取數據，在控制層完成簡單的處理后通過mcu 下位機對電機進行控制，實現無人車的移動、手控、燈光以及清潔設備的簡單控制。算法層主要包括SLAM模塊，在獲取底層數據后完成無人車的建圖以及實時定位；導航模塊完成定位信息以及地圖建立后的自主運行功能實現；通訊模塊主要為主控設備的通訊協議，其中包括CAN 總線通訊協議以及基于TCP/IP 的socket 通信協議。算法層的開發在Windows 系統中的Visual Studio 2013 利用C++語言完成。指令層主要為發送指令的主控終端，本次實驗采用ASUS筆記本配置為Intel Core i7-8750H CPU，64 位Windows10 操作系統。指令層通過VS2013C#開發的上位機界面完成輸入。整體架構如圖5所示。

圖5 系統整體架構

4.2 實驗結果分析

使本實驗將在算法層中加入多線程進行實驗過程中數據的提取，離線后利用Python 進行實驗數據進行整合以及結果分析。實驗場地選擇地下車庫環境，在長為40 m，寬為2 m 的環境下利用無人車完成地圖建立后沿預設軌跡運動一周，選取不同節點進行采樣，根據真實值和單一傳感器、多傳感器融合后軌跡進行對比并且進行誤差分析。完成環境建圖后柵格地圖如圖6所示。

圖6 無人車定位柵格地圖

圖7—圖9 分別為激光傳感器，編碼器分別單獨測量，通過擴展卡爾曼濾波進行傳感器融合后與真實路徑的匹配圖。

圖7 激光傳感器測量

圖8 編碼器測量值

圖9 傳感器融合后測量值

在不同位置進行數據采樣，可以獲得不同位置不同傳感器定位信息的誤差，如表1所示。

表1 不同傳感器定位信息分析

根據圖7—圖9 可以看出，在依靠單一編碼器進行定位信息獲取時，累計誤差過大，在無人清潔車行駛10 m 之后編碼器誤差趨于不可接受范圍。在依靠單一激光傳感器進行定位時在部分位置振幅較大，實際控制中容易導致無人清潔車行駛軌跡有所擺動，而在經過擴展卡爾曼濾波后路徑定位趨于平滑，無人車行駛也更加穩定。同時根據算法2可以進一步得出，在經過擴展卡爾曼濾波后定位信息誤差相較單一傳感器均有所減少。綜上所述，在多傳感器組合定位時，對于無人清潔車的定位精度以及實際行駛穩定性均有所提高。

5 結語

本文針對無人清潔車SLAM定位問題提出了多傳感器融合的應用研究。首先完成了對SLAM問題的建模以及框架解釋，同時對卡爾曼濾波的原理以及算法流程進行分析，在此基礎上提出了改進擴展卡爾曼濾波算法針對非線性系統中多種傳感器的優劣勢進行融合處理，有效改善了在單一傳感器解決SLAM 定位中的精度問題，增加了系統魯棒性以及兼容性。同時在實驗平臺下完成實驗結果分析，驗證了系統的可行性以及實時性。但由于擴展卡爾曼濾波忽略泰勒展開的高階項，容易對誤差形成累積，在動態環境下運行有所不足。隨著硬件以及算法的不斷成熟，在未來的卡爾曼濾波算法應用上結合日漸成熟的深度學習將會大大增加系統的精確度以及魯棒性。視覺SLAM技術的發展也趨于成熟，加入攝像頭傳感器對解決SLAM問題在性能上也能有所提高，未來在面對更加復雜的實際運行環境中更加穩定。