謝洪樂(lè)，陳衛(wèi)東，范亞嫻，王景川

上海交通大學(xué) 電子信息與電氣工程學(xué)院 自動(dòng)化系，上海 200240

月面科學(xué)探測(cè)是一項(xiàng)具有重大科研價(jià)值的任務(wù)[1-2]，在月球車執(zhí)行月面探測(cè)任務(wù)時(shí)，首先需要解決的一個(gè)關(guān)鍵難題是確定其自身的準(zhǔn)確位置，以保證其能夠?qū)Ш街聊繕?biāo)位置[3-4]。但是由于月球表面沒(méi)有全球衛(wèi)星定位與導(dǎo)航系統(tǒng)，所以月球車很難直接獲取其準(zhǔn)確的定位信息[3-5]。

目前，現(xiàn)有的月球車主要是依據(jù)其所攜帶的傳感器去感知周圍的環(huán)境信息，結(jié)合月球車的運(yùn)動(dòng)狀態(tài)進(jìn)行定位，常用的定位方式有：航跡推算、無(wú)線電測(cè)量和星敏觀測(cè)等定位方法[3]。由于航跡推算存在較大的累計(jì)誤差，無(wú)線電測(cè)量和星敏定位等方法的準(zhǔn)確度較低，一般只能實(shí)現(xiàn)十米級(jí)的定位精度，且無(wú)線電定位存在信號(hào)較弱、星敏定位存在星系的能觀性比較差等缺點(diǎn)，導(dǎo)致月球車基于上述方法都難以實(shí)現(xiàn)準(zhǔn)確的定位[3-4]。

近年來(lái)，隨著計(jì)算機(jī)視覺(jué)的迅速發(fā)展，基于視覺(jué)的定位方法逐漸成熟，同時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)在許多場(chǎng)景取得了顯著的定位效果提升。目前，現(xiàn)有的視覺(jué)SLAM方法主要包括直接法[6-7]和間接法[8]。直接法采用像素梯度信息進(jìn)行連續(xù)圖像幀之間的數(shù)據(jù)關(guān)聯(lián)[6]，而間接法采用特征點(diǎn)進(jìn)行圖像幀之間的匹配[8]。由于月面的沙質(zhì)環(huán)境難以提取穩(wěn)定、魯棒的視覺(jué)特征進(jìn)行可靠的跟蹤定位，且紋理信息較少，存在相似重復(fù)的環(huán)境特征會(huì)影響定位系統(tǒng)的精度和穩(wěn)定性?；谙袼靥荻刃畔⒌钠ヅ浞椒ㄐЧ容^差，所以比較適合采用基于特征點(diǎn)法的視覺(jué)SLAM方法。同時(shí)，由于月面環(huán)境為典型的非結(jié)構(gòu)化環(huán)境，不滿足曼哈頓世界結(jié)構(gòu)特征假設(shè)[9]，不存在較多的直線和平面等結(jié)構(gòu)化信息，因此不能用直線特征，所以本文采用基于稀疏特征點(diǎn)的視覺(jué)SLAM方法。

由于月面場(chǎng)景的紋理信息比較少，采用純視覺(jué)的SLAM方法在特征稀疏的場(chǎng)景定位效果比較差，所以考慮增加一些輔助的傳感器以提升定位的性能。近年來(lái)，基于多傳感器融合的方案逐漸取得了更好的定位準(zhǔn)確性和魯棒性，特別是視覺(jué)慣性融合定位方法，例如OKVIS[10]、VI-ORB-SLAM[11]和VINS-Mono[12]等方法，將慣性測(cè)量單元(Inertial Measurement Unit，IMU)的信息作為短期的連續(xù)幀之間運(yùn)動(dòng)估計(jì)，用于修正視覺(jué)定位的誤差，取得了更高的定位精度[13]。所以本文主要研究一種視覺(jué)慣性融合SLAM方法。

針對(duì)月面環(huán)境的特征稀疏的特點(diǎn)，在之前工作的基礎(chǔ)上[14]，提出一種基于多層光流跟蹤的前端幀間數(shù)據(jù)關(guān)聯(lián)方法，采用四元樹結(jié)構(gòu)，依據(jù)特征值響應(yīng)強(qiáng)弱選擇有效的特征點(diǎn)進(jìn)行跟蹤，實(shí)現(xiàn)了一個(gè)高效、魯棒的前端光流跟蹤方法。

針對(duì)月球車的相機(jī)視野特點(diǎn)，由于月球沒(méi)有大氣層，月球車的相機(jī)能夠觀測(cè)到遙遠(yuǎn)的恒星點(diǎn)，但是恒星點(diǎn)往往處于無(wú)窮遠(yuǎn)點(diǎn)的位置，這對(duì)于視覺(jué)慣性里程計(jì)(Visual-Inertial Odometry, VIO)是無(wú)效的觀測(cè)信息，這些無(wú)窮遠(yuǎn)的路標(biāo)點(diǎn)會(huì)嚴(yán)重影響視覺(jué)慣性里程計(jì)滑動(dòng)窗口優(yōu)化的準(zhǔn)確性，進(jìn)而影響定位的精度。所以針對(duì)月球車相機(jī)的成像特點(diǎn)，提出了一種恒星點(diǎn)剔除算法，基于局部圖像灰度二階矩進(jìn)行恒星區(qū)域特征判斷，通過(guò)圖像多層金字塔進(jìn)行尺度優(yōu)化，能夠快速、有效地剔除恒星點(diǎn)，實(shí)現(xiàn)了定位性能的進(jìn)一步提升。

基于位姿圖優(yōu)化的方法，將視覺(jué)相機(jī)和慣性測(cè)量單元的測(cè)量信息加入后端因子圖優(yōu)化，最終實(shí)現(xiàn)了一個(gè)準(zhǔn)確、高效的月球車視覺(jué)慣性SLAM方法，能夠持續(xù)地進(jìn)行高精度的六自由度位姿解算，并且搭建了月球車仿真樣機(jī)和月面仿真環(huán)境，通過(guò)在多種不同的仿真月面場(chǎng)景下的測(cè)試，仿真結(jié)果驗(yàn)證了本文方法的定位準(zhǔn)確性和魯棒性。

1 系統(tǒng)整體框架

本文的視覺(jué)慣性SLAM方法在之前工作[14]的基礎(chǔ)上進(jìn)行改進(jìn)和優(yōu)化，由于之前工作主要針對(duì)室內(nèi)無(wú)人機(jī)定位系統(tǒng)，其俯仰角和滾轉(zhuǎn)角狀態(tài)是可觀測(cè)的，所以只進(jìn)行了位置和偏航角的四自由度(Degree of Freedom, DoF)優(yōu)化，本文針對(duì)月球車的實(shí)際傳感器特點(diǎn)，在原始四自由度優(yōu)化的基礎(chǔ)上改進(jìn)，增加了六自由度(6DoF)位姿優(yōu)化，提升了位姿估計(jì)的準(zhǔn)確度。并且針對(duì)月面環(huán)境的特點(diǎn)，提出一種恒星點(diǎn)剔除算法，改善了前端基于光流的數(shù)據(jù)關(guān)聯(lián)的魯棒性。本文視覺(jué)慣性SLAM系統(tǒng)框架如圖1所示，主要包括傳感器數(shù)據(jù)預(yù)處理、前端基于光流的特征跟蹤和后端視覺(jué)慣性SLAM位姿圖聯(lián)合優(yōu)化等3個(gè)部分。

在預(yù)處理階段，慣性測(cè)量單元的數(shù)據(jù)采用IMU預(yù)積分的形式[15]，在短期內(nèi)提供相鄰幀之間的相對(duì)位姿估計(jì)，相機(jī)圖像采用區(qū)域直方圖均衡化進(jìn)行圖像增強(qiáng)。然后，相鄰圖像幀之間采用光流法進(jìn)行前端數(shù)據(jù)關(guān)聯(lián)，并且基于四元樹進(jìn)行改進(jìn)，在多層圖像金字塔中選擇魯棒的特征點(diǎn)進(jìn)行跟蹤，采用恒星點(diǎn)剔除算法將無(wú)窮遠(yuǎn)點(diǎn)進(jìn)行剔除，提升了特征關(guān)聯(lián)的魯棒性。利用慣性測(cè)量數(shù)據(jù)優(yōu)化視覺(jué)測(cè)量的尺度因子，并對(duì)齊2個(gè)傳感器的位姿估計(jì)結(jié)果，實(shí)現(xiàn)視覺(jué)慣性聯(lián)合位姿校準(zhǔn)。

在后端視覺(jué)慣性SLAM位姿圖聯(lián)合優(yōu)化部分，基于視覺(jué)慣性里程計(jì)滑動(dòng)窗口優(yōu)化方法，將新增的關(guān)鍵幀依次通過(guò)滑動(dòng)窗口進(jìn)行優(yōu)化。并且在全局位姿圖四自由度優(yōu)化方法[14]基礎(chǔ)上進(jìn)行改進(jìn)，在全局位姿圖優(yōu)化時(shí)，聯(lián)合優(yōu)化位移、偏航角、俯仰角和滾轉(zhuǎn)角，實(shí)現(xiàn)了六自由度的位姿圖優(yōu)化，能夠提供魯棒、高精度的位姿估計(jì)。

2 前端基于光流的特征點(diǎn)跟蹤方法

2.1 多層光流跟蹤方法

視覺(jué)慣性SLAM方法的前端圖像特征數(shù)據(jù)關(guān)聯(lián)的準(zhǔn)確性，會(huì)很大程度地直接影響視覺(jué)慣性SLAM方法的整體定位精度。目前現(xiàn)有的視覺(jué)慣性SLAM主要是基于特征點(diǎn)匹配和光流2種方法，由于特征匹配方法的實(shí)時(shí)性一般較差，所以在視覺(jué)慣性里程計(jì)中，往往使用光流法作為一種輕量化的前端視覺(jué)特征數(shù)據(jù)關(guān)聯(lián)的主要方法。常使用 KLT(Kanade-Lucas-Tomasi)光流跟蹤方法[16]，假設(shè)相鄰的圖像之間滿足灰度一致性，通過(guò)局部像素灰度信息進(jìn)行匹配，進(jìn)而擬合出最優(yōu)的圖像特征點(diǎn)的運(yùn)動(dòng)狀態(tài)，并且通過(guò)多層圖像金字塔結(jié)構(gòu)優(yōu)化，能夠提升光流法在較大視角變化情況下的魯棒性和準(zhǔn)確度[16]。

稀疏光流的效果直接取決于其所跟蹤特征點(diǎn)的質(zhì)量，所以在之前工作的基礎(chǔ)上[14]，采用基于四元樹的二維平面搜索策略改進(jìn)多層光流跟蹤方法。假設(shè)圖像金字塔層數(shù)為L(zhǎng)，在τ時(shí)刻的關(guān)鍵幀Kτ中，采用四元樹搜索策略篩選光流穩(wěn)定跟蹤的特征點(diǎn)集合φ(Kτ)，依據(jù)關(guān)鍵幀圖像高斯核尺度的二次冪，正比分配在各層的特征點(diǎn)數(shù)，在每一層的圖像中采用四元樹搜索局部Harris響應(yīng)值最大的特征點(diǎn)，用于KLT光流跟蹤[17]，假設(shè)四元樹的子節(jié)點(diǎn)總數(shù)為N，在相鄰幀之間光流跟蹤過(guò)程中，特征點(diǎn)集合φ(Kτ)的迭代公式為

(1)

相比于基于非極大值抑制(Non-Maximum Suppression，NMS)的特征點(diǎn)篩選方法[18]，本文方法在保證特征點(diǎn)均勻分布的同時(shí)，也能夠有效地選擇可以被光流魯棒跟蹤的特征點(diǎn)。為驗(yàn)證本文方法的有效性，進(jìn)行對(duì)比測(cè)試如圖2所示。

在基于非極大值抑制約束的特征點(diǎn)篩選方法中，采用光流法常用的GFTT(Good Features to Track)特征[19]用于KLT光流跟蹤[17]。其中，為保證特征點(diǎn)能夠均勻地分布在圖像平面，采用像素距離閾值進(jìn)行約束。假設(shè)特征點(diǎn)之間最小距離閾值為Fm，若特征點(diǎn)之間的距離小于閾值Fm時(shí)，則依據(jù)非極大值抑制方法[18]進(jìn)行特征選擇，保留特征值最大的特征點(diǎn)，最終得到關(guān)鍵幀Kτ的特征點(diǎn)集合φ(Kτ)，采用KLT光流跟蹤方法[17]實(shí)現(xiàn)相鄰關(guān)鍵幀(Kτ和Kτ+1)之間的特征關(guān)聯(lián)。由于光流跟蹤往往存在誤匹配點(diǎn)，所以采用與VINS-Mono[12]一致的策略，利用基礎(chǔ)矩陣模型估計(jì)的隨機(jī)抽樣一致性(Random Sample Consensus，RANSAC)方法去除誤匹配點(diǎn)，最終得到滿足運(yùn)動(dòng)一致性的光流跟蹤結(jié)果。其中，不同方法的光流跟蹤測(cè)試結(jié)果對(duì)比，如圖2所示。

在光流跟蹤性能測(cè)試中，關(guān)鍵幀Kτ特征點(diǎn)數(shù)均滿足|φ(Kτ)|≤300，即提取特征點(diǎn)數(shù)量的最大值為300個(gè)，像素分辨率為848×480。在基于非極大值抑制方法中，依據(jù)像素距離閾值不同，設(shè)置了3組對(duì)比仿真測(cè)試，分別為Fm=10、Fm=20和Fm=30。其中，F(xiàn)m值越大，對(duì)應(yīng)的特征分布越均勻，但是特征的魯棒性往往會(huì)越差。在本文方法中，保持與非極大值抑制方法一致的特征點(diǎn)數(shù)量約束|φ(Kτ)|≤300，并且采用相同基礎(chǔ)矩陣約束的RANSAC策略，保持嚴(yán)格控制變量的對(duì)比測(cè)試，最終的測(cè)試結(jié)果，如圖2所示。

當(dāng)Fm=10時(shí)，光流跟蹤的特征點(diǎn)分布比較集中，存在較多特征分布在月面和太空的交界區(qū)域，如圖2(a1)、圖2(b1)和圖2(c1)所示，位于交界曲線上的特征點(diǎn)雖然滿足GFTT特征值約束，但是此處的特征點(diǎn)沿著交界曲線方向具有較大的不確定性，所以會(huì)導(dǎo)致光流跟蹤不穩(wěn)定。而且在快速運(yùn)動(dòng)情況下，若特征點(diǎn)分布過(guò)于密集時(shí)，會(huì)存在較多的特征點(diǎn)移出相機(jī)的視野范圍的情況，此時(shí)會(huì)導(dǎo)致較大的光流數(shù)據(jù)關(guān)聯(lián)誤差。

當(dāng)Fm=20和Fm=30時(shí)，光流跟蹤的特征點(diǎn)分布比較均勻，但由于固定的最小像素距離Fm限制，在紋理顯著的區(qū)域不能提取大量的特征點(diǎn)，反而在圖像模糊的區(qū)域提取到一些次優(yōu)的特征點(diǎn)，所以導(dǎo)致光流跟蹤的魯棒性下降。

本文方法能夠在多層圖像金字塔中，采用四元樹搜索策略檢索魯棒的特征點(diǎn)，優(yōu)化了光流跟蹤策略。特征點(diǎn)光流跟蹤成功率比較，如表1所示。本文方法的平均跟蹤(Tracking)和RANSAC匹配的成功率分別為94.27%和75.80%，僅略低于非極大值抑制Fm=10方法，但本文方法相對(duì)于Fm=10方法，具有更好的特征分布均勻性和魯棒性。綜上所述，本文方法實(shí)現(xiàn)了較高的特征跟蹤成功率和較好的光流估計(jì)的一致性。

在上述測(cè)試結(jié)果中，基于非極大值抑制方法的前端特征跟蹤的平均時(shí)間消耗約為19.5 ms/幀，而本文方法的前端特征跟蹤的平均時(shí)間消耗約為17 ms/幀。所以，本文方法具有更好的實(shí)時(shí)性，滿足計(jì)算的實(shí)時(shí)性需求。

表1 光流跟蹤成功率比較Table 1 Comparison of optical flow tracking success rates

2.2 特征點(diǎn)的逆深度估計(jì)

特征點(diǎn)逆深度估計(jì)是指在視覺(jué)SLAM方法中，通過(guò)估計(jì)特征點(diǎn)深度值的倒數(shù)實(shí)現(xiàn)其坐標(biāo)參數(shù)化，進(jìn)而求解特征點(diǎn)三維空間坐標(biāo)的一種有效方法[20]。設(shè)一個(gè)特征點(diǎn)在世界坐標(biāo)系下的齊次坐標(biāo)位置為Gfk∈R4×1，其中Gfk=[Gxk,Gyk,Gzk,1]T，上標(biāo)G表示在世界坐標(biāo)系下。假設(shè)在視覺(jué)慣性SLAM滑動(dòng)窗口中，有n個(gè)關(guān)鍵幀對(duì)該特征點(diǎn)存在有效的觀測(cè)，記這些關(guān)鍵幀集合為Φ={Kτ},τ=1,2,…,n。在每次觀測(cè)中，特征點(diǎn)在對(duì)應(yīng)的圖像坐標(biāo)系下的像素坐標(biāo)為pk=[uk,vk,1]T，設(shè)該特征點(diǎn)的深度值為λk，依據(jù)相機(jī)的投影關(guān)系可得

λkpk=Mc[Rk,Tk]Gfk

(2)

式中：Mc∈R3×3為相機(jī)傳感器的參數(shù)矩陣；Pk=[Rk,Tk]∈R3×4為世界坐標(biāo)系到相機(jī)坐標(biāo)系的投影矩陣，即表示當(dāng)前關(guān)鍵幀的位姿矩陣。其中Rk∈R3×3為旋轉(zhuǎn)矩陣，Tk∈R3×1為平移向量。依據(jù)投影關(guān)系可簡(jiǎn)化為

λkpk=McPkGfk

(3)

(4)

(5)

在特征點(diǎn)三維坐標(biāo)Gfk的參數(shù)化計(jì)算過(guò)程中，其對(duì)應(yīng)的深度λk估計(jì)值的取值范圍為(λmin,+∞)，其中λmin為相機(jī)有效觀測(cè)的最小距離。因?yàn)闊o(wú)窮大在數(shù)值求解過(guò)程中存在較大的誤差，所以采用1/λk形式進(jìn)行參數(shù)化求解[20]，也稱為逆深度計(jì)算。所以，式(5)可轉(zhuǎn)化成下面方程進(jìn)行求解：

(6)

通過(guò)連續(xù)幀之間特征點(diǎn)的共視觀測(cè)關(guān)系，可求解上述方程。但是月面沒(méi)有大氣層，月球車裝載的相機(jī)往往可以看到很遙遠(yuǎn)的恒星，此時(shí)恒星特征點(diǎn)位于無(wú)窮遠(yuǎn)點(diǎn)，采用有限值去逼近無(wú)窮遠(yuǎn)點(diǎn)，會(huì)導(dǎo)致上述方程式(6)的極大似然估計(jì)的解會(huì)存在較大的非線性擬合誤差。所以提出一種恒星點(diǎn)消除算法，將無(wú)窮遠(yuǎn)的特征點(diǎn)進(jìn)行剔除，以保證視覺(jué)慣性里程計(jì)的滑動(dòng)窗口優(yōu)化的準(zhǔn)確性。

3 恒星特征點(diǎn)剔除算法

由于恒星點(diǎn)往往都是無(wú)窮遠(yuǎn)點(diǎn)，當(dāng)月球車前進(jìn)時(shí)，無(wú)窮遠(yuǎn)點(diǎn)對(duì)于前進(jìn)方向的位移估計(jì)幾乎沒(méi)有作用，僅對(duì)于旋轉(zhuǎn)運(yùn)動(dòng)會(huì)有一些輔助作用，所以對(duì)恒星點(diǎn)進(jìn)行剔除。針對(duì)月面環(huán)境的特點(diǎn)，在月球車相機(jī)所觀測(cè)的圖像中，恒星位置一般為無(wú)窮遠(yuǎn)點(diǎn)，且位于圖像的上半部分，恒星所在的像素區(qū)域往往處于顏色較深的太空背景區(qū)域，而恒星為這一區(qū)域的唯一的中心亮點(diǎn)，所以針對(duì)月球車圖像像素灰度分布特點(diǎn)，提出一種基于局部像素區(qū)域的灰度二階矩方法，能夠?qū)⒑阈翘卣鼽c(diǎn)進(jìn)行有效地剔除，提升特征點(diǎn)的魯棒性。

假設(shè)在當(dāng)前幀圖像中光流跟蹤的一個(gè)特征點(diǎn)坐標(biāo)為Ak(uk,vk)，計(jì)算其所在高斯金字塔圖像的區(qū)域圖像的灰度二階矩的值來(lái)進(jìn)行恒星點(diǎn)的判斷。任取圖像上一個(gè)像素點(diǎn)Pi(xi,yi)，設(shè)這個(gè)點(diǎn)所在的圖像對(duì)應(yīng)位置像素的灰度函數(shù)為I(xi,yi)，通過(guò)計(jì)算圖像局部窗口灰度特征來(lái)判斷是否為恒星點(diǎn)區(qū)域，其中，像素灰度二階矩的計(jì)算公式為

(7)

式中：α為權(quán)重；Θ為像素點(diǎn)Ak的鄰域；Ω為對(duì)應(yīng)鄰域的面積。Θ和Ω的計(jì)算公式為

(8)

(9)

式中：γ為選取的鄰域半徑。計(jì)算以特征點(diǎn)坐標(biāo)(uk,vk)為中心，半徑范圍在γmin至γmax之間的環(huán)形區(qū)域Θ的像素灰度二階矩H值。通過(guò)判斷圖像的像素灰度二階矩H值是否小于局部灰度閾值來(lái)判斷是否為恒星點(diǎn)。其中s為尺度因子，在多層光流跟蹤時(shí)，s與光流跟蹤的金字塔層數(shù)一致。

恒星特征點(diǎn)剔除方法的測(cè)試結(jié)果如圖3所示，圖3(a)為原始的KLT光流跟蹤的測(cè)試結(jié)果[12]，由于恒星點(diǎn)為局部亮點(diǎn)區(qū)域，往往是一個(gè)易于光流跟蹤的特征點(diǎn)，但是由于此特征點(diǎn)一般位于無(wú)窮遠(yuǎn)處，而數(shù)值求解該特征的空間位置會(huì)導(dǎo)致系統(tǒng)定位性能下降，所以采用恒星點(diǎn)剔除算法，如圖3(b)所示，圖中紅色和藍(lán)色的方塊分別為穩(wěn)定跟蹤和新增的光流特征點(diǎn)，白色圓圈的位置為剔除的恒星點(diǎn)區(qū)域，通過(guò)四元樹改進(jìn)的光流能夠更均勻地跟蹤魯棒的特征點(diǎn)，并且通過(guò)恒星點(diǎn)剔除算法，能夠有效地將恒星點(diǎn)剔除。

本文提出的恒星點(diǎn)剔除方法只需要在光流穩(wěn)定跟蹤的特征點(diǎn)中增加恒星點(diǎn)判斷，所以方法具有較高的實(shí)時(shí)性，平均每幀圖像大約能夠穩(wěn)定光流跟蹤100～150個(gè)有效特征點(diǎn)，恒星點(diǎn)剔除方法的平均耗時(shí)約5～8 ms/幀，滿足月球車視覺(jué)慣性SLAM方法的計(jì)算實(shí)時(shí)性需求。

4 視覺(jué)慣性SLAM位姿圖聯(lián)合優(yōu)化

在視覺(jué)慣性里程計(jì)后端優(yōu)化部分，在VINS-Mono系統(tǒng)[12]的基礎(chǔ)之上進(jìn)行改進(jìn)和優(yōu)化，由于視覺(jué)慣性里程計(jì)往往適用于無(wú)人機(jī)等平臺(tái)，配備了高性能的慣性測(cè)量單元，其中，飛控系統(tǒng)的俯仰角和滾轉(zhuǎn)角的狀態(tài)往往是可觀的，所以往往只需要進(jìn)行四自由度的優(yōu)化，包括位移和偏航角。但是由于月面環(huán)境重力加速度值往往不夠準(zhǔn)確，將4DoF優(yōu)化改成6DoF優(yōu)化，包括位移、偏航角、俯仰角和滾轉(zhuǎn)角。在視覺(jué)慣性里程計(jì)滑動(dòng)窗口優(yōu)化中，優(yōu)化的狀態(tài)變量Xτ為

(10)

(11)

(12)

5 仿真驗(yàn)證與分析

針對(duì)實(shí)際月球車在月面執(zhí)行科學(xué)探測(cè)的任務(wù)特點(diǎn)，搭建了一個(gè)月球車仿真樣機(jī)和一套模擬月球表面場(chǎng)景的計(jì)算機(jī)仿真系統(tǒng)，用于驗(yàn)證月球車定位方法的性能。

仿真系統(tǒng)采用開源機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)框架下的Gazebo仿真引擎作為開發(fā)工具，采用Gazebo-7.4.0機(jī)器人仿真平臺(tái)，使用C++語(yǔ)言在Ubuntu 16.04系統(tǒng)下開發(fā)，軟件開發(fā)平臺(tái)為Microsoft Visual Studio Code 2019。計(jì)算機(jī)硬件配置為：CPU處理器為Intel Core i7-7700HQ, 主頻為2.80 GHz，計(jì)算機(jī)內(nèi)存為8 Gb RAM。使用的機(jī)器人操作系統(tǒng)為Ubuntu 16.04 ROS Kinetic。

5.1 月球車仿真樣機(jī)平臺(tái)

利用Gazebo仿真引擎搭建的月球車仿真樣機(jī)平臺(tái)如圖4所示。仿真月球車樣機(jī)的整體高度為1.046 m，寬度為0.75 m，長(zhǎng)度為1.005 m，輪子直徑為0.5 m。仿真月球車采用星球車常用的四輪獨(dú)立驅(qū)動(dòng)結(jié)構(gòu)，利用4個(gè)相互獨(dú)立的電機(jī)控制器來(lái)分別調(diào)節(jié)4個(gè)車輪的控制力矩，進(jìn)而控制4個(gè)獨(dú)立車輪運(yùn)動(dòng)，通過(guò)差速驅(qū)動(dòng)方式實(shí)現(xiàn)月球車能夠平穩(wěn)地運(yùn)動(dòng)在崎嶇的月面。月球車具有較好的越障能力，能夠正常行駛在最大傾斜度為25°的斜坡區(qū)域，而且能夠翻越最大高度約20 cm的隕石、沙堆等障礙物。在仿真平臺(tái)中，設(shè)置可調(diào)節(jié)的月球車平移速度范圍為0.05～3 m/s，角速度范圍為0.05～1.5 rad/s，滿足實(shí)際月球車仿真測(cè)試的平移速度與角速度需求。

月球車仿真樣機(jī)平臺(tái)搭載了多種傳感器，能夠進(jìn)行傳感器測(cè)量數(shù)據(jù)的仿真，主要傳感器包括：雙目視覺(jué)傳感器、三維激光雷達(dá)和慣性測(cè)量單元。其中，采用的視覺(jué)傳感器型號(hào)為PointGrey Bumblebee2，為了保證實(shí)時(shí)性和定位性能，在仿真系統(tǒng)中，設(shè)置相機(jī)的分辨率為848×480，頻率為20 Hz。其中，相機(jī)安裝高度為0.984 m，相機(jī)與水平方向的向下傾斜角度為13.5°，仿真測(cè)試中采用雙目相機(jī)的左側(cè)相機(jī)的圖像數(shù)據(jù)。IMU位于雙目相機(jī)的中心位置，IMU型號(hào)為ADIS16448，在仿真中設(shè)置更新頻率為100 Hz。

通過(guò)仿真平臺(tái)可以進(jìn)行月球表面重力模型的仿真，能夠模擬月球表面的重力情況。在月球車仿真平臺(tái)中，月面重力加速度為1.62 m/s2，約為地球表面重力加速度的1/6。

并且，在仿真月球車平臺(tái)上，具備定位軌跡真值(Ground Truth)的保存功能，能夠?qū)崟r(shí)地記錄月球車在移動(dòng)過(guò)程中真實(shí)軌跡和位姿。定位軌跡真值與月球車平臺(tái)上IMU的實(shí)時(shí)位置一致，能夠表征月球車在世界坐標(biāo)系下的實(shí)際移動(dòng)軌跡，真實(shí)軌跡可用于評(píng)估不同算法的定位性能。

5.2 月面仿真場(chǎng)景數(shù)據(jù)集

利用Gazebo仿真引擎，在WIP Moon Habitat開源模型庫(kù)的基礎(chǔ)上[21]，實(shí)現(xiàn)了一套模擬月面環(huán)境的計(jì)算機(jī)仿真系統(tǒng)。為了模擬多種不同場(chǎng)景的月面環(huán)境，在仿真模型庫(kù)[18]的基礎(chǔ)上進(jìn)行改進(jìn)，將原本固定的單一月面場(chǎng)景改進(jìn)成可調(diào)節(jié)的多種月面場(chǎng)景。在搭建的月面環(huán)境仿真系統(tǒng)中，可以調(diào)節(jié)多種典型的月面場(chǎng)景元素的狀態(tài)，進(jìn)而模擬多種不同的月面場(chǎng)景，例如可以調(diào)節(jié)月面場(chǎng)景中隕石塊的形狀、尺寸、數(shù)量與空間分布等情況。通過(guò)調(diào)節(jié)不同月面場(chǎng)景元素的狀態(tài)，能夠模擬不同特征稀疏程度的月面場(chǎng)景，用于驗(yàn)證本文提出的視覺(jué)慣性SLAM方法的性能。

在仿真系統(tǒng)中，模擬了不同程度的特征稀疏的典型月面場(chǎng)景，包括沙質(zhì)土壤、隕石坑、隕石塊、環(huán)形山和起伏不平的山丘地形等典型月面場(chǎng)景元素，實(shí)現(xiàn)了一套較高仿真度的月面場(chǎng)景。其中，整個(gè)月面仿真場(chǎng)景的俯視圖近似為正方形區(qū)域，月面場(chǎng)景規(guī)模大小約為：100 m×100 m，總面積約為1.0×104m2。

為了量化評(píng)估本文方法的定位性能，采用國(guó)際通用的視覺(jué)慣性里程計(jì)Benchmark EuRoC數(shù)據(jù)集的格式[22]，依據(jù)不同的最大移動(dòng)速度、角速度和場(chǎng)景特征的稀疏程度，構(gòu)建了3種不同難度的視覺(jué)慣性數(shù)據(jù)集，參數(shù)設(shè)置如表2所示。

構(gòu)建的月面環(huán)境仿真數(shù)據(jù)集包括特征密集和特征稀疏等2種典型的環(huán)境。依據(jù)如表2中所示的數(shù)據(jù)集參數(shù)設(shè)置，構(gòu)建了6組用于定位性能測(cè)試的數(shù)據(jù)集，圖像總數(shù)約為1.8萬(wàn)張。其中，不同場(chǎng)景數(shù)據(jù)集的部分圖像樣本，如圖5所示。

表2 數(shù)據(jù)集參數(shù)設(shè)置Table 2 Setting of dataset parameters

5.3 定位結(jié)果與分析

為了驗(yàn)證本文方法的定位性能，在多個(gè)不同的月面場(chǎng)景數(shù)據(jù)集下進(jìn)行了仿真測(cè)試。通過(guò)仿真測(cè)試，驗(yàn)證了本文方法的有效性，并且量化分析了位移定位誤差和姿態(tài)角估計(jì)誤差。

5.3.1 位移定位誤差

依據(jù)國(guó)際上通用的SLAM系統(tǒng)定位準(zhǔn)確度評(píng)估方法[23-25]，采用絕對(duì)軌跡誤差(Absolute Trajectory Error，ATE)的均方根誤差(Root Mean Square Error，RMSE)作為定位準(zhǔn)確度的定量評(píng)估指標(biāo)[26-28]。在3種不同難度的場(chǎng)景下，利用一共6組數(shù)據(jù)集進(jìn)行了定位性能的對(duì)比測(cè)試，位移定位準(zhǔn)確度的仿真測(cè)試結(jié)果，如表3所示。

表3 位移定位誤差比較Table 3 Comparison of localization errors m

本文方法在大部分?jǐn)?shù)據(jù)集上的定位準(zhǔn)確率均高于VINS-Mono[12]方法。其中，本文方法的平均定位均方根誤差(RMSE)、平均誤差的中值(Med.)和最大值(Max.)分別為0.348、0.290、0.984 m，平均定位誤差均低于VINS-Mono[12]，本文方法實(shí)現(xiàn)了高精度的定位。

其中，用于定位仿真測(cè)試的6組月面數(shù)據(jù)集的移動(dòng)軌跡總長(zhǎng)度約為733.08 m。通過(guò)不同算法估計(jì)的軌跡和真實(shí)軌跡的均方根誤差來(lái)比較算法的定位性能，繪制均方根誤差的箱型圖，如圖6所示，橫坐標(biāo)分別為數(shù)據(jù)集序號(hào)的簡(jiǎn)寫，例如E1為Easy-1數(shù)據(jù)集，依此類推。依據(jù)箱型圖的均值可以發(fā)現(xiàn)，本文方法的均方根誤差比VINS-Mono[12]方法更小，且定位誤差有界，由于在D1數(shù)據(jù)集中，場(chǎng)景存在大量的紋理重復(fù)的區(qū)域，所以本文方法定位性能略低于VINS-Mono[12]。但是本文方法具有更好的魯棒性，本文方法在快速運(yùn)動(dòng)、特征稀疏的D2數(shù)據(jù)集仍能夠保持0.5 m左右的高精度定位，高于VINS-Mono[12]方法，且根據(jù)誤差箱型圖的分布可知，本文系統(tǒng)的定位誤差的波動(dòng)更小，定位估計(jì)具有更好的一致性，實(shí)現(xiàn)了誤差有界的月面環(huán)境下準(zhǔn)確、魯棒的定位。

本文方法的定位軌跡能夠更準(zhǔn)確地接近真實(shí)軌跡，其中，在Difficult-2數(shù)據(jù)集的測(cè)試結(jié)果，如圖7所示，本文方法的定位誤差小于VINS-Mono，仿真測(cè)試結(jié)果表明本文方法的定位更準(zhǔn)確。

為進(jìn)一步量化分析絕對(duì)位移的定位誤差，選擇Difficult-2場(chǎng)景數(shù)據(jù)集進(jìn)行定位誤差的量化分析，其中X、Y和Z軸的位移定位誤差曲線，如圖8所示，仿真測(cè)試結(jié)果表明，本文方法的平均定位誤差的均方根誤差約為0.509 m，在3個(gè)坐標(biāo)軸方向的平均定位誤差均小于VINS-Mono[12]方法。

5.3.2 姿態(tài)定位誤差

針對(duì)姿態(tài)角估計(jì)情況，在上述6組不同的月面場(chǎng)景數(shù)據(jù)集下面進(jìn)行了仿真測(cè)試，偏航角(Yaw)、俯仰角(Pitch)和滾轉(zhuǎn)角(Roll)等姿態(tài)角度的估計(jì)誤差如表4所示。本文方法在偏航角和滾轉(zhuǎn)角的平均定位精度均高于VINS-Mono[12]，其中，只有俯仰角的平均誤差略低于VINS-Mono[12]，并且整體姿態(tài)角估計(jì)的準(zhǔn)確度均有一定程度的提升。

表4 姿態(tài)角估計(jì)誤差比較Table 4 Comparison of attitude angle estimation errors (°)

其中，在Difficult-2場(chǎng)景數(shù)據(jù)集進(jìn)一步地分析姿態(tài)角度估計(jì)誤差，如圖9所示，本文方法在偏航角和滾轉(zhuǎn)角的角度的估計(jì)誤差分別為0.791°和0.152°，均小于VINS-Mono[12]的1.972°和0.528°，且俯仰角誤差僅為0.192°，本文方法實(shí)現(xiàn)了更加準(zhǔn)確的姿態(tài)角估計(jì)，并且能夠適應(yīng)于不同的月面場(chǎng)景，具有更高的魯棒性和準(zhǔn)確性。

綜上所述，本文方法能夠在多種不同的月面仿真場(chǎng)景中準(zhǔn)確地估計(jì)位移和姿態(tài)角度，能夠持續(xù)地為月球車提供高精度、可靠的位姿估計(jì)，滿足月球車在特征稀疏月面環(huán)境下的定位需求。

6 結(jié) 論

本文針對(duì)月面特征稀疏環(huán)境下的月球車自主定位難題，研究了一種基于視覺(jué)慣性傳感器融合的高精度SLAM方法，主要貢獻(xiàn)在于：

1) 針對(duì)視覺(jué)慣性里程計(jì)前端數(shù)據(jù)關(guān)聯(lián)誤差較大的問(wèn)題，提出一種基于四元樹改進(jìn)的多層光流跟蹤算法，能夠有效地提高相鄰幀圖像之間運(yùn)動(dòng)的估計(jì)精度。

2) 針對(duì)月面環(huán)境的恒星無(wú)窮遠(yuǎn)點(diǎn)的干擾問(wèn)題，提出一種恒星點(diǎn)剔除方法，能夠有效地減小恒星無(wú)窮遠(yuǎn)點(diǎn)導(dǎo)致的定位性能下降影響。

3) 采用ROS Gazebo計(jì)算機(jī)仿真引擎搭建了一套模擬月面場(chǎng)景的仿真系統(tǒng)，構(gòu)建了不同條件下的月面環(huán)境視覺(jué)慣性SLAM評(píng)測(cè)的數(shù)據(jù)集。

本文方法通過(guò)6DoF位姿圖優(yōu)化，進(jìn)一步提升了定位的精度，在多個(gè)不同的數(shù)據(jù)集上進(jìn)行了仿真測(cè)試驗(yàn)證，測(cè)試結(jié)果表明本文方法具有更高的定位魯棒性和準(zhǔn)確度。