太陽敏感器輔助的分布式EKF-SLAM火星車自主導(dǎo)航方法

裴?？?，嚴(yán) 鴻，朱明君

（1. 北京工業(yè)大學(xué) 信息學(xué)部，北京 100124；2. 計算智能與智能系統(tǒng)北京市重點實驗室，北京 100124）

引 言

火星車是代替人類在火星表面行駛，利用自身攜帶的科學(xué)儀器進(jìn)行考察探測、樣品收集等工作，最后將各種探測數(shù)據(jù)傳送回地球的自主式巡視機(jī)器人。自主導(dǎo)航是火星車進(jìn)行路徑規(guī)劃、自主避障、遙操作等任務(wù)的前提和基礎(chǔ)，是其順利實現(xiàn)火星表面巡視探測的一項關(guān)鍵的核心技術(shù)。

慣性導(dǎo)航和天文導(dǎo)航方法一直以來都是深空探測機(jī)器人（火星車、月球車等）的必備導(dǎo)航系統(tǒng)[1-3]，即使是中國2013年發(fā)射的月球車也配備了光纖捷聯(lián)慣性導(dǎo)航系統(tǒng)與星敏感器作為導(dǎo)航傳感器。但是，受到傳感器測量原理和火星環(huán)境的限制，慣性/天文組合導(dǎo)航方法雖然具有較高的姿態(tài)精度，卻存在定位精度低和環(huán)境信息描述缺乏的問題，無法滿足現(xiàn)代行星探測機(jī)器人對高精度位姿和環(huán)境信息的需求[4]。1986年，Randall C. Smith和Peter Cheeseman用擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EKF）對機(jī)器人和環(huán)境路標(biāo)的位置進(jìn)行估計[5]，由此開啟了SLAM（Simultaneous Localization and Mapping）算法研究的序幕。經(jīng)過20多年的發(fā)展，SLAM算法以其定位精度高和能夠獲取未知環(huán)境信息的特點，逐漸成為解決機(jī)器人自主導(dǎo)航問題的關(guān)鍵技術(shù)，并被應(yīng)用于解決行星機(jī)器人自主導(dǎo)航問題[6-7]。

現(xiàn)有的SLAM算法主要包括EKF-SLAM[8-9]、SEIFSLAM[10]、PF-SLAM[11]以及分布式PF-SLAM[12-13]。其中，前面幾種算法為集中式結(jié)構(gòu)，由于其狀態(tài)量將隨著路標(biāo)點的變化而不斷擴(kuò)大，造成了無限高維的狀態(tài)量，在大地圖場景和復(fù)雜環(huán)境下，算法的實時性和穩(wěn)定性無法保證。分布式PF-SLAM具有結(jié)構(gòu)上的明顯優(yōu)勢，但是其采用的粒子濾波算法不可避免地存在粒子貧化和計算量大的問題，并不適合于火星車的實際應(yīng)用。

針對已有SLAM算法的問題，本文綜合現(xiàn)有SLAM算法的特點，提出了采用分布式結(jié)構(gòu)實現(xiàn)EKFSLAM算法，實現(xiàn)了一種利用太陽敏感器輔助的分布式EKF-SLAM算法。該方法采用雙軸模擬式太陽敏感器[14]解算火星車的航向，并將航向信息引入到分布式SLAM的各子系統(tǒng)中，建立航向輔助的觀測方程，從而提高整體的觀測性、一致性和收斂性。文中詳細(xì)給出了太陽敏感器輔助分布式SLAM的系統(tǒng)模型和實現(xiàn)原理，并通過實驗驗證了算法的可行性和有效性。

1 太陽敏感器確定機(jī)器人航向

1.1 太陽相對方位角確定

裝載在機(jī)器人上的太陽敏感器敏感入射光的強(qiáng)度，由測得的太陽高度角hs和方位角 δs計算得到敏感器坐標(biāo)系下的太陽矢量，如圖1所示。經(jīng)過車體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的矢量可確定太陽相對方位角，即太陽入射光投影和機(jī)器人前進(jìn)方向的夾角。

圖1 太陽敏感器坐標(biāo)系下的太陽單位矢量Fig. 1 Solar unit vector in coordinate system of sun sensor

假設(shè)O2O=1，在圖1所示的三維坐標(biāo)系中，可得O2O1=sinhs，O1O=coshs。則太陽敏感器坐標(biāo)系下太陽單位矢量為

由于太陽敏感器是固定在車體上的，根據(jù)太陽敏感器安裝的位置確定其坐標(biāo)系SS和車體坐標(biāo)系Sb之間的約束關(guān)系滿足Sb=SS。

為了得到相對于導(dǎo)航坐標(biāo)系下的太陽單位向量Sn，必須將車體坐標(biāo)系下的矢量Sb轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下。根據(jù)慣性導(dǎo)航設(shè)備測得車體相對當(dāng)?shù)貙?dǎo)航坐標(biāo)系的X軸的翻滾角為 θ，相對于Y軸的俯仰角為? 。故通過歐拉角坐標(biāo)旋轉(zhuǎn)矩陣進(jìn)行兩次旋轉(zhuǎn)（不考慮航向）得到車體坐標(biāo)系和導(dǎo)航坐標(biāo)系的關(guān)系式為

導(dǎo)航坐標(biāo)系到車體坐標(biāo)系的轉(zhuǎn)換矩陣為

太陽單位矢量在導(dǎo)航坐標(biāo)系下可表示為

因此，太陽在導(dǎo)航坐標(biāo)系下的方位角為

1.2 天文星歷解算太陽絕對方位角

絕對方位角即太陽入射光投影和機(jī)器人所在地平坐標(biāo)系正北方向的夾角?；谔煳男菤v數(shù)據(jù)的絕對方位角解算是在已知時間和機(jī)器人的經(jīng)度和緯度的前提下，利用星歷數(shù)據(jù)以及坐標(biāo)系間的換算實現(xiàn)的，如圖2所示。

地平坐標(biāo)系的基圈是地平圈，它的緯線圈是與地平圈平行的小圓，經(jīng)線是天球上通過天頂和天底且垂直于地平圈的圓。時角坐標(biāo)系的基圈是天赤道，它的緯線是與天赤道平行的小圓，經(jīng)線是天球上通過北天極和南天極的圈。

圖2 地平坐標(biāo)和時角坐標(biāo)變換圖示Fig. 2 Diagram of coordinate transformation of horizon and hour angle coordinate

若已知太陽的時角t和赤緯 δ，以及觀測點的地理緯度 λ。求解天體的方位A和天頂距Z，利用球面三角公式即可得

由式（5）和式（6）可以解算絕對方位角

最終可得機(jī)器人航向角為

2 分布式SLAM系統(tǒng)模型

本文使用激光傳感器獲取環(huán)境中的特征點相對于機(jī)器人的距離和方位角。對于航跡的推算是借助安裝在車輪上的里程計測得機(jī)器人位置變化量，確定機(jī)器人的位置。在SLAM系統(tǒng)中，機(jī)器人模型如圖3所示。

圖3 全局坐標(biāo)系下機(jī)器人模型Fig. 3 Robot model in global coordinate system

分布式SLAM結(jié)構(gòu)即根據(jù)有效特征點構(gòu)建多個相互平行的子濾波器，每個子濾波器具有各自的狀態(tài)方程和觀測方程，將各個子濾波器獨立濾波估計的結(jié)果傳送到主濾波器進(jìn)行融合，融合結(jié)果即為最終的狀態(tài)估計。

分布式SLAM子系統(tǒng)的運(yùn)動方程描述為

系統(tǒng)的運(yùn)動模型一般描述了機(jī)器人在控制信號作用下的狀態(tài)變化情況。其中機(jī)器人在 (t?1,t)的時間間隔內(nèi)控制信息為ut=(?Dt,?φt)T，?Dt是機(jī)器人移動距離的變化量，?φt是機(jī)器人航向角的變化量。wt是過程噪聲，用來表征運(yùn)動誤差的不確定性。

對于常用的兩輪差分里程計，可利用編碼器信息測得控制量如下

其中： ?dL和?dR分別是編碼器測得的左右輪的前進(jìn)距離；l為兩輪間的距離。

分布式SLAM系統(tǒng)的觀測方程為

系統(tǒng)的觀測模型是機(jī)器人通過激光傳感器獲取的t時刻的環(huán)境信息。其中：是機(jī)器人與第i個有效特征點之間的距離；是機(jī)器人相對于特征點的偏轉(zhuǎn)角；vi(t)表示系統(tǒng)的測量噪聲。

3 航向輔助的分布式SLAM

3.1 航向輔助觀測

在解決SLAM問題時，與其他濾波方法相比，擴(kuò)展卡爾曼濾波因其良好的穩(wěn)定性和一致性，仍然是首選濾波算法，EKF-SLAM的計算復(fù)雜度和實時性能夠有效滿足實際應(yīng)用的要求。

文獻(xiàn)[15-16]已經(jīng)證明，機(jī)器人航向的不確定性對EKF-SLAM算法的收斂性和一致性有顯著的影響。而且，機(jī)器人航向的不確定性也會導(dǎo)致特征點位置的不確定性。也就是說即使知道機(jī)器人與特征點在之間的距離，因為航向的不確定性，也無法得到特征點的準(zhǔn)確位置，特征點的位置又與機(jī)器位姿相互作用，影響估計精度。

為了解決上述問題，在EKF-SLAM系統(tǒng)中引入太陽敏感器作為輔助觀測的傳感器。通過太陽敏感器獲得的航向信息作為加入觀測方程量中新的測量量。改進(jìn)后的系統(tǒng)觀測方程如下

其中，Zφ表示由太陽敏感器解算的機(jī)器人航向角，其計算方法同式（8）。

假設(shè)t時刻觀測到n個有效的特征點，則分布式系統(tǒng)模型如下

3.2 濾波融合過程

由于實際應(yīng)用中SLAM問題是非線性的，在使用EKF解決問題時，首先需要利用泰勒級數(shù)展開將運(yùn)動模型和觀測模型線性化。離散化的系統(tǒng)模型為

子系統(tǒng)的時間更新方程為

系統(tǒng)的量測更新方程為

其中，Pi,k是與對應(yīng)的誤差協(xié)方矩陣。

卡爾曼濾波增益為

在分布式SLAM算法中，根據(jù)關(guān)聯(lián)成功的有效特征點建立若干個平行的子濾波器。每個子濾波器獨立地進(jìn)行濾波估計，各估計結(jié)果最終會傳送到主濾波器。在主濾波器中，按照各子濾波器信息所占權(quán)重進(jìn)行數(shù)據(jù)融合，融合計算的結(jié)果即為機(jī)器人狀態(tài)的最終估計。

由于協(xié)方差矩陣能夠表征濾波器的估計精度，所以利用協(xié)方差矩陣定義子濾器的融合權(quán)重ηi，則有

主濾波器融合處理后的機(jī)器人狀態(tài)估計結(jié)果為

4 實驗驗證

本文中的驗證實驗是在北京工業(yè)大學(xué)科學(xué)樓前的廣場上進(jìn)行的。實驗時，在場地的道路兩側(cè)放置圓柱形球筒作為人工的特征點，實驗環(huán)境如圖4所示。

圖4 實驗環(huán)境Fig. 4 The experimental environment

實驗移動平臺如圖5所示。激光傳感器采用SICK LMS111，有效測量距離是20 m，測量誤差保證在±35 mm內(nèi)，實驗中設(shè)定掃描范圍為 0?～1 80?，其用于采集機(jī)器人和特征點間的距離和偏角數(shù)據(jù)。實驗中使用的雙軸模擬式太陽敏感器可在雙軸±64°視場范圍內(nèi)實現(xiàn)姿態(tài)測量精度為±0.3°，通過四象限硅光電池敏感太陽光光強(qiáng)，實現(xiàn)太陽矢量兩軸方向角的測量。裝載在車輪上的里程計編碼器在采樣時間內(nèi)采集機(jī)器人位移和角度的變化量。里程計的最大缺點就是累積誤差，故采用太陽敏感器獲取的航向信息修正偏差。GPS系統(tǒng)可以實時獲取機(jī)器人移動軌跡，用于算法驗證。

圖5 實驗移動平臺Fig. 5 The experimental mobile platform

實驗過程中，機(jī)器人從標(biāo)記位置出發(fā)，在實驗場地緩慢繞行一個完整的大圈。機(jī)器人在運(yùn)動過程中通過激光掃描球筒邊緣的一系列光束提取特征點，其運(yùn)行軌跡為橢圓形。分別用集中式EKF-SLAM（CEKFSLAM）、分布式EKF-SLAM（DEKF-SLAM）、航向輔助的分布式EKF-SLAM（HADEKF-SLAM）算法對機(jī)器人運(yùn)動軌跡進(jìn)行估計，結(jié)果如圖6所示。

圖6中黑色虛線、綠色點劃線、紅色實線分別表示的是CEKF-SLAM、DEKF-SLAM HADEKF-SLAM算法對機(jī)器人軌跡的估計。藍(lán)色虛線是里程計對軌跡的推算結(jié)果，星號表示環(huán)境中的特征點。很顯然，里程計的誤差不斷累加，并且很快偏離真實路線。3種SLAM算法都能夠有效修正里程計產(chǎn)生的偏差，在一定程度上保證算法的精度和收斂性。

圖6 機(jī)器人軌跡估計結(jié)果Fig. 6 The trajectory estimation results of robot

以GPS采集的實際數(shù)據(jù)為基準(zhǔn)，對3種算法的估計結(jié)果進(jìn)行誤差分析。利用式（23）計算算法估計結(jié)果的均方誤差，如表1所示。

表1 實驗數(shù)據(jù)與GPS比較的均方誤差Table 1 The mean square error of experimental data compared with GPS m

由表1中數(shù)據(jù)可知，與CEKF-SLAM相比，分布式EKF-SLAM算法的估計精度更高。同時，HADEKFSLAM無論是在X軸方向還是在Y軸方向算法的估計誤差都在0.6 m以內(nèi)，這說明引入航向輔助觀測對算法性能有顯著影響。

5 結(jié) 論

針對現(xiàn)有SLAM算法在實時性和準(zhǔn)確性方面無法滿足火星車自主導(dǎo)航實際需求的問題，并考慮火星車航向?qū)KF-SLAM算法性能的影響，本文提出了一種基于航向輔助的分布式EKF-SLAM火星車的自主導(dǎo)航定位方法。對于傳統(tǒng)集中式SLAM算法，單一濾波器的狀態(tài)量因有效路標(biāo)點的數(shù)量增加而不斷增大。然而，分布式EKF-SLAM算法中，多個平行的子濾波器的維數(shù)固定不變，可以有效地提高系統(tǒng)的運(yùn)算效率和容錯率。同時實驗結(jié)果表明，引入航向輔助的分布式EKF-SLAM能夠保證算法的收斂性和一致性，從而有效地提高了算法的估計精度。