月球基地車(chē)輛自校準(zhǔn)導(dǎo)航定位方法

崔 軼， 傅惠民

（北京航空航天大學(xué) 小樣本技術(shù)研究中心， 北京 100191）

0 引言

構(gòu)建月球基地是月球探測(cè)的核心目標(biāo)之一， 也是人類(lèi)實(shí)現(xiàn)對(duì)月球資源的深度開(kāi)發(fā)和利用的重要手段[1，2]。 在月球基地中， 可以通過(guò)機(jī)器人和智能車(chē)輛來(lái)控制和照料基地。 因此，為機(jī)器人和智能車(chē)輛提供導(dǎo)航服務(wù)就非常重要。 這種月球基地的導(dǎo)航與一般的月面導(dǎo)航不完全一樣，前者是小范圍的，后者是大范圍的，前者對(duì)導(dǎo)航精度的要求遠(yuǎn)高于后者， 這導(dǎo)致月球基地機(jī)器人和車(chē)輛的導(dǎo)航面臨更大的挑戰(zhàn)。 為此，文獻(xiàn)[3]提出一種新的步長(zhǎng)估計(jì)模型和行人航位自校準(zhǔn)推算方法， 并建立一種室內(nèi)自校準(zhǔn)導(dǎo)航定位方法，不但解決了室內(nèi)行人導(dǎo)航定位問(wèn)題，而且能夠?yàn)樵虑蚧貦C(jī)器人提供高精度的導(dǎo)航定位。 在此基礎(chǔ)上， 本文進(jìn)一步提出一種月球基地車(chē)輛自校準(zhǔn)導(dǎo)航定位方法，可以實(shí)現(xiàn)月球基地車(chē)輛高精度導(dǎo)航定位。

1 慣性導(dǎo)航自校準(zhǔn)方程

由于慣性導(dǎo)航系統(tǒng)每次使用之前需要較長(zhǎng)的初始對(duì)準(zhǔn)時(shí)間，加上受環(huán)境的影響和設(shè)備老化等原因，其陀螺儀和加速度計(jì)往往存在較大漂移。因此，下面根據(jù)慣性導(dǎo)航方程， 給出能夠?qū)ν勇輧x和加速度計(jì)漂移進(jìn)行有效修正的自校準(zhǔn)方程。

1.1 自校準(zhǔn)狀態(tài)方程

由于慣性傳感器的采樣頻率非常高， 相鄰兩次采樣之間加速度變化不大，因此其狀態(tài)方程可以表示為：

式中：ax，k，ay，k，az，k為第k 次采樣時(shí)導(dǎo)航坐標(biāo)系下車(chē)輛質(zhì)心的三軸加速度，bax，k-1，bay，k-1，baz，k-1為未知輸入 （模型誤差、環(huán)境干擾等），wax，k-1，way，k-1，waz，k-1為噪聲。

由運(yùn)動(dòng)學(xué)方程可知， 車(chē)輛行進(jìn)過(guò)程中的位置和速度由下面四式給出：

式中：（xk，yk） 為第k 次采樣時(shí)導(dǎo)航坐標(biāo)系下車(chē)輛質(zhì)心的位 置 坐 標(biāo)，vx，k，vy，k為 第k 次 采 樣 時(shí) 導(dǎo) 航 坐 標(biāo) 系 下 車(chē) 輛 質(zhì)心的速度， △tk-1為相鄰兩次采樣之間的時(shí)間間隔，bx，k-1，by，k-1，bvx，k-1，bvy，k-1為未知輸入（模型誤差、環(huán)境干擾等），wx，k-1，wy，k-1，wvx，k-1，wvy，k-1為噪聲。

同理， 相鄰兩次采樣之間車(chē)輛繞質(zhì)心轉(zhuǎn)動(dòng)的角速度也變化不大，因此有：

式中：ωx，k，ωy，k，ωz，k為第k 次采樣時(shí)載體坐標(biāo) 系下車(chē)輛繞質(zhì) 心 轉(zhuǎn) 動(dòng) 的 三 軸 角 速 度，bωx，k-1，bωy，k-1，bωz，k-1為 未 知 輸 入（模型誤差、環(huán)境干擾等），wωx，k-1，wωy，k-1，wωz，k-1為噪聲。

根據(jù)歐拉角微分方程[4]，車(chē)輛行進(jìn)過(guò)程中的航向角、俯仰角和滾轉(zhuǎn)角可分別由下面三式計(jì)算：

式中：ψk是航向角，即車(chē)輛縱軸在水平面的投影與地理北向之間的夾角，偏東為正，偏西為負(fù)；θk是俯仰角，即車(chē)輛縱軸與縱向水平軸之間的夾角，向上為正，向下為負(fù)；γk是滾轉(zhuǎn)角，即車(chē)輛縱向?qū)ΨQ(chēng)面與鉛垂面之間的夾角，右傾為正，左傾為負(fù)。 bψ，k-1，bθ，k-1，bγ，k-1為未知輸入（模型誤差、環(huán)境干擾等），wψ，k-1，wθ，k-1，wγ，k-1為噪聲。

1.2 自校準(zhǔn)量測(cè)方程

根據(jù)導(dǎo)航坐標(biāo)系和載體坐標(biāo)系之間的變換關(guān)系[4]，加速度量測(cè)方程為：

式中：acx，k，acy，k，acz，k為第k 次采樣時(shí)載體坐標(biāo)系下的加速度計(jì)量測(cè)，dax，k，day，k，daz，k為未知輸入（加速度計(jì)漂移、環(huán)境干擾等），ξax，k，ξay，k，ξaz，k為噪聲。 ωgx，k，ωgy，k，ωgz，k為第k 次采樣時(shí)載體坐標(biāo)系下的陀螺儀量測(cè)，dωx，k，dωy，k，dωz，k為未知輸入（陀螺儀漂移、環(huán)境干擾等），ξωx，k，ξωy，k，ξωz，k為噪聲。Cij，k為坐標(biāo)變換矩陣Ck第i 行第j 列的元素，由下面九式計(jì)算[4]：

2 基于TOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法

下面以超寬帶（UWB）信號(hào)為例對(duì)基于到達(dá)時(shí)間（TOA）的車(chē)輛自校準(zhǔn)導(dǎo)航方法進(jìn)行討論，其他信號(hào)也可同樣處理。

設(shè)第k 次采樣時(shí)，在位置（xk，yk）處能夠接收到n 個(gè)UWB 信號(hào)，則基于TOA 的UWB 距離量測(cè)方程為：

采用文獻(xiàn)[5]中的非線性系統(tǒng)雙未知輸入自校準(zhǔn)濾波方法，對(duì)狀態(tài)方程式（1）～式（13），量測(cè)方程式（14）～式（19）和式（29）進(jìn)行濾波，即為基于TOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法。濾波時(shí)，可選取視距環(huán)境中最小的兩個(gè)距離量測(cè)值，令其方程中的未知輸入dri，k=0。 也可以采用文獻(xiàn)[5]中的兩步自校準(zhǔn)濾波方法來(lái)進(jìn)一步提高濾波的魯棒性和精度。

3 基于TDOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法

基于TOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法要求基站和標(biāo)簽的時(shí)間完全同步，這在工程實(shí)際中有時(shí)難以實(shí)現(xiàn)。 為此，下面進(jìn)一步給出只需各個(gè)基站之間時(shí)間同步的基于到達(dá)時(shí)間差 （TDOA） 的方法。 仍然以UWB 信號(hào)為例對(duì)基于TDOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法進(jìn)行討論， 其他信號(hào)也可同樣處理。

TDOA 需將基站兩兩分組，第1 個(gè)和第2 個(gè)基站為一組，第3 個(gè)和第4 個(gè)基站為一組，以此類(lèi)推。 下面給出一種以基站距離遠(yuǎn)近進(jìn)行分組的方案：布置n 個(gè)UWB 基站（n 為偶數(shù)），計(jì)算它們兩兩之間的距離；選取距離最小的兩個(gè)基站，令其為第1 個(gè)和第2 個(gè)基站；再在剩下的n-2個(gè)基站中選取距離最小的兩個(gè)基站， 令其為第3 個(gè)和第4 個(gè)基站；按照這個(gè)規(guī)則選取下去，直至最后剩下兩個(gè)基站，令其為第n-1 個(gè)和第n 個(gè)基站。 此時(shí)，基于TDOA 的UWB 距離差量測(cè)方程為：

采用文獻(xiàn)[5]中的非線性系統(tǒng)雙未知輸入自校準(zhǔn)濾波方法，對(duì)狀態(tài)方程式（1）～式（13），量測(cè)方程式（14）～式（19）和式（30）進(jìn)行濾波，即為基于TDOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法。 濾波時(shí)，可選取式（30）所示的n/2 個(gè)距離差中絕對(duì)值最小的兩個(gè)，直接令其未知輸入dri（i+1），k=0。 也可以采用文獻(xiàn)[5]中的兩步自校準(zhǔn)濾波方法來(lái)進(jìn)一步提高濾波的魯棒性和精度。

4 基于RSSI 的車(chē)輛自校準(zhǔn)導(dǎo)航方法

基于接收信號(hào)強(qiáng)度指示（RSSI）的車(chē)輛自校準(zhǔn)導(dǎo)航方法中采用的慣性導(dǎo)航自校準(zhǔn)方程仍然為式 （1）～式（19），下面進(jìn)一步給出RSSI 量測(cè)方程。

設(shè)第k 次（加速度計(jì)和陀螺儀）采樣時(shí)，在位置（xk，yk）處能夠接收到n 個(gè)熱點(diǎn)（AP）發(fā)出的信號(hào)，則基于RSSI 的量測(cè)方程為[3]：

式中：Si，k為第k 次（加速度計(jì)和陀螺儀）采樣時(shí)來(lái)自第i個(gè)AP 的RSSI，（xk，yk） 為此時(shí)導(dǎo)航坐標(biāo)系下車(chē)輛質(zhì)心的位置坐標(biāo)，（Xi，Yi）為第i 個(gè)AP 的位置坐標(biāo)，r0i為參考距離，S0i為參考距離處的RSSI，ηi為信號(hào)強(qiáng)度的路徑損耗系數(shù)，dsi，k為未知輸入（量測(cè)誤差、非視距誤差等）。 當(dāng)?shù)趇 個(gè)AP發(fā)出的信號(hào)在視距環(huán)境中傳播時(shí)，直接令dsi，k=0，且這樣的AP 不少于2 個(gè)。ξsi，k為零均值服從正態(tài)分布的噪聲，即ξsi，k～N（0，σs2i，k）。 考慮到采樣頻率的不同，設(shè)加速度計(jì)和陀螺儀的采樣頻率是RSSI 信號(hào)的采樣頻率的m 倍， 則當(dāng)k是m 的正整數(shù)倍時(shí)，濾波的狀態(tài)方程為式（1）～式（13），量測(cè)方程為式（14）～式（19）和式（35）；當(dāng)k 不是m 的正整數(shù)倍時(shí)，濾波的量測(cè)方程中將不包含RSSI 量測(cè)方程式（35）。

同樣，采用文獻(xiàn)[5]中的非線性系統(tǒng)雙未知輸入自校準(zhǔn)濾波方法進(jìn)行濾波，也可以采用文獻(xiàn)[5]中的兩步自校準(zhǔn)濾波方法來(lái)進(jìn)一步提高濾波的魯棒性和精度。

5 月球基地自校準(zhǔn)定位方法

文獻(xiàn)[3]給出了基于RSSI 的室內(nèi)自校準(zhǔn)定位方法，該方法也可用于基于TDOA 的自校準(zhǔn)定位方法。 下面仍以UWB 為例進(jìn)行討論。

設(shè)在位置（x，y）處能夠接收到n 個(gè)UWB 信號(hào)，則其自校準(zhǔn)距離量測(cè)方程為：

6 實(shí)例

現(xiàn)在北京航空航天大學(xué)新主樓20m×30m 的一個(gè)長(zhǎng)方形走廊內(nèi)進(jìn)行導(dǎo)航實(shí)驗(yàn)。 在走廊的四個(gè)角和四條邊中點(diǎn)布置了8 個(gè)WiFi AP。 操控小車(chē)沿長(zhǎng)方形走廊行駛一圈，實(shí)時(shí)記錄陀螺儀、加速度計(jì)數(shù)據(jù)和接收到的每個(gè)WiFi AP 的三個(gè)信號(hào)強(qiáng)度平均值。

采用本文方法（自校準(zhǔn)秩濾波、自校準(zhǔn)無(wú)跡濾波）和傳統(tǒng)方法（擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波）進(jìn)行計(jì)算。計(jì)算150 個(gè)定位點(diǎn)的導(dǎo)航精度并繪于圖1，且計(jì)算其平均值列于表1。

圖1 導(dǎo)航精度比較

表1 平均導(dǎo)航精度比較

從圖1 可以看到， 本文方法的導(dǎo)航精度比傳統(tǒng)方法的導(dǎo)航精度高且穩(wěn)定， 而傳統(tǒng)方法的導(dǎo)航精度隨時(shí)間的增加而迅速下降。從表1 可以看到，自校準(zhǔn)秩濾波的平均導(dǎo)航精度比擴(kuò)展卡爾曼濾波的平均導(dǎo)航精度提高了5.8倍，比無(wú)跡卡爾曼濾波的平均導(dǎo)航精度提高了4.2 倍，這說(shuō)明本文方法能夠比傳統(tǒng)方法顯著提高導(dǎo)航精度。 自校準(zhǔn)無(wú)跡濾波的平均導(dǎo)航精度比無(wú)跡卡爾曼濾波的平均導(dǎo)航精度提高了3.5 倍， 這說(shuō)明自校準(zhǔn)濾波能顯著提高導(dǎo)航精度。而因?yàn)橹炔蓸訉儆谧罴巡蓸樱宰孕?zhǔn)秩濾波的導(dǎo)航精度比自校準(zhǔn)無(wú)跡濾波的導(dǎo)航精度要略高。

7 結(jié)論

（1） 提出了車(chē)輛自校準(zhǔn)導(dǎo)航方法， 包括基于TOA 和TDOA 的車(chē)輛自校準(zhǔn)導(dǎo)航方法以及基于RSSI 的車(chē)輛自校準(zhǔn)導(dǎo)航方法，可以實(shí)現(xiàn)月球基地車(chē)輛的高精度導(dǎo)航。

（2）給出了慣性導(dǎo)航的自校準(zhǔn)方程，包括位置坐標(biāo)、速度、加速度、角速度、角度等的自校準(zhǔn)狀態(tài)方程和自校準(zhǔn)量測(cè)方程，能夠有效地對(duì)模型誤差、陀螺儀和加速度計(jì)的漂移等進(jìn)行實(shí)時(shí)修正。

（3）建立了UWB 的距離差量測(cè)方程，給出了基站與標(biāo)簽之間距離差的設(shè)計(jì)方案及其協(xié)方差矩陣， 能夠?qū)α繙y(cè)誤差和非視距誤差等進(jìn)行實(shí)時(shí)自校準(zhǔn)。

（4） 建立了月球基地自校準(zhǔn)定位方法， 包括基于TDOA 的自校準(zhǔn)定位方法和能夠?qū)DOA 與RSSI 相融合的自校準(zhǔn)定位方法，可進(jìn)行高精度定位。

（5） 本文的月球基地車(chē)輛自校準(zhǔn)導(dǎo)航定位方法也可用于室內(nèi)（如地下車(chē)庫(kù)等）的車(chē)輛導(dǎo)航定位。