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

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

0 引言

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

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

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

1 SLAM系統(tǒng)

1.1 數(shù)據(jù)模型及問題描述

SLAM 問題可以描述為：無人車在未知環(huán)境下開始運行，在運行過程中通過自身位置的估算以及路標(biāo)觀測信息，構(gòu)建出一個增量式的地圖，在此基礎(chǔ)上可以實現(xiàn)機器人自身定位以及導(dǎo)航。根據(jù)該問題可以定義兩個變量分別為運動狀態(tài)量以及觀測量：

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

此時需要完成對無人車的狀態(tài)估計，將SLAM問題轉(zhuǎn)化為求后驗概率的問題：

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

由于貝葉斯公式右側(cè)的分母和狀態(tài)無關(guān)，后驗概率最大化無需考慮分母，此時只需要當(dāng)前時刻的狀態(tài)，又可寫為：

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

1.2 SLAM系統(tǒng)框架

激光SLAM系統(tǒng)框架主要如圖1所示。

圖1 SLAM框架

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

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

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

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

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

（5）地圖構(gòu)建。根據(jù)估計的軌跡以及自身的位姿信息建立未知環(huán)境下的地圖。對于不同的傳感器建立的地圖可能包含2D 柵格地圖、拓?fù)涞貓D、網(wǎng)格地圖等，對于本文實驗平臺只在低矮平面中運動的無人清潔車，根據(jù)2D 激光傳感器可以構(gòu)建2D 柵格地圖，可以滿足多種任務(wù)的實現(xiàn)。

（6）路徑規(guī)劃。在地圖和定位精確的基礎(chǔ)上，無人車可以根據(jù)規(guī)劃路徑完成自主循跡任務(wù)，同時可以實現(xiàn)實時避障等功能。

2 卡爾曼濾波

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

2.1 卡爾曼濾波器原理

使用卡爾曼濾波算法主要由兩個階段構(gòu)成一個迭代循環(huán)，一個是預(yù)測階段，一個是更新階段。其流程如圖2所示。

圖2 卡爾曼濾波流程

2.1.1 預(yù)測部分

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

預(yù)測狀態(tài)：

其中即位置和速度，利用狀態(tài)轉(zhuǎn)移矩陣，再加上外部影響，預(yù)測下一時刻的狀態(tài)。

預(yù)測誤差的協(xié)方差矩陣：

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

2.1.2 更新部分

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

測量和預(yù)測值之差：

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

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

其中傳感器測量由于存在噪聲導(dǎo)致測量結(jié)果存在噪聲協(xié)方差，此處R為位置信息的方差。

卡爾曼增益：

更新狀態(tài)矩陣：

更新預(yù)測誤差協(xié)方差矩陣：

卡爾曼濾波算法如下：

卡爾曼濾波算法

2.2 傳感器融合

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

圖3 多傳感器融合流程

3 擴展卡爾曼濾波

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

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

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

擴展卡爾曼濾波算法

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

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

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

可得圓心坐標(biāo)為

則+Δ時刻：

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

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

4 實驗

4.1 實驗系統(tǒng)設(shè)計

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

圖4 無人清潔車平臺

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

圖5 系統(tǒng)整體架構(gòu)

4.2 實驗結(jié)果分析

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

圖6 無人車定位柵格地圖

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

圖7 激光傳感器測量

圖8 編碼器測量值

圖9 傳感器融合后測量值

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

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

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

5 結(jié)語

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