基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法*

張文安汪 偉付明磊陸春校何軍強(qiáng)

(1.浙江工業(yè)大學(xué)信息工程學(xué)院，浙江 杭州 310023；2.浙江富春江通信集團(tuán)有限公司，浙江 杭州 311400；3.杭州鴻泉物聯(lián)網(wǎng)技術(shù)股份有限公司，浙江 杭州 310030)

近年來，隨著人工智能技術(shù)的興起，無人駕駛得到了快速發(fā)展。獲得實時、準(zhǔn)確的車輛位姿信息是實現(xiàn)無人駕駛的關(guān)鍵技術(shù)之一[1]。其中，全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)與慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)的融合可以提供高精度的全局定位[2－3]，是實現(xiàn)這一關(guān)鍵技術(shù)的重要方法。然而，在隧道、地下車庫等缺失GNSS信號的場景中，基于GNSS/INS組合導(dǎo)航系統(tǒng)的車輛定位性能迅速下降。因此，缺失GNSS信號條件下的低速無人車定位問題具有重要的研究價值。

由于單一傳感器的定位精度和適用范圍有限，目前無人車定位普遍采用多傳感器數(shù)據(jù)融合方法[4]。其中，基于激光雷達(dá)(Light Detection and Ranging，LIDAR)里程計的定位技術(shù)具有算法成熟、精度高、實時性好等優(yōu)點(diǎn)[5]，但是LIDAR定位技術(shù)不適合成本敏感的定位產(chǎn)品。而基于相機(jī)的視覺里程計(Visual Odometry，VO)因其具有感知信息豐富、體積小、成本低等優(yōu)點(diǎn)而受到無人駕駛領(lǐng)域的廣泛關(guān)注[6]。但是，由于其無法有效地應(yīng)對運(yùn)動模糊、低紋理場景、光照變化、遮擋和動態(tài)對象的影響，因此常與INS相融合，形成基于相機(jī)與慣性測量單元(Inertial Measur-ement Unit，IMU)融合的視覺慣性里程計(Visual Inertial Odometry，VIO)[7]。VIO利用相機(jī)和IMU具有互補(bǔ)的特性，有效融合慣性測量與圖像觀測信息，從而獲得比使用單個傳感器更好的累積性能。

VIO方向的研究工作主要分為基于優(yōu)化框架的方法和基于濾波框架的方法。其中，基于優(yōu)化框架的方法通過一個最小二乘誤差函數(shù)的迭代最小化來估計未知參數(shù)，被廣泛認(rèn)為在精度上優(yōu)于基于濾波框架的方法[8－9]。但是，由于其迭代機(jī)制，這類方法需要較高的計算量。與基于優(yōu)化框架的方法相反，基于濾波框架的方法在觀測數(shù)據(jù)可用時以遞歸方式估計參數(shù)[10－11]，具有較高的計算效率。文獻(xiàn)[12]提出了一種基于誤差狀態(tài)擴(kuò)展卡爾曼濾波器(Extended Kalman Filter，EKF)的多狀態(tài)約束卡爾曼濾波(Multi-State Constraint Kalman Filter，MSCKF)算法。MSCKF推導(dǎo)出一種能夠表達(dá)從多個相機(jī)位姿觀察到靜態(tài)特征時產(chǎn)生的幾何約束的測量模型，并在系統(tǒng)狀態(tài)向量中保留了一個含固定數(shù)量的過去相機(jī)位姿的滑動窗口和當(dāng)前的IMU狀態(tài)，在計算效率、算法精度等方面均取得了良好的效果，非常適合部署在計算資源受限的低速無人車平臺上。同時，由于MSCKF算法的獨(dú)特優(yōu)勢，在其框架下出現(xiàn)了大量的后續(xù)工作來提升其性能和適用性[13－16]。

然而，低速無人車在實際運(yùn)行過程中經(jīng)常會由于避讓行人和其他車輛，或者交通堵塞而出現(xiàn)的較長時間停車等待。在這種靜態(tài)場景中，由于無法發(fā)生特征點(diǎn)三角化將導(dǎo)致MSCKF中基于相機(jī)的測量更新過程停止，而基于IMU的狀態(tài)傳播過程正常進(jìn)行。受到IMU累積誤差的影響，無人車的定位性能迅速下降。為了解決上述問題，本文引入零速修正技 術(shù)(Zero Velocity Update，ZUPT)[17－18]，結(jié) 合MSCKF框架，提出一種基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法(Adaptive Zero Velocity Update Mechanism MSCKF，AZUPT-MSCKF)。相比于傳統(tǒng)MSCKF算法，AZUPT-MSCKF方法能夠有效地處理無人車由于較長時間處于靜止?fàn)顟B(tài)而導(dǎo)致的定位性能下降等問題，具有更高的定位精度和更強(qiáng)的魯棒性。

本文的主要貢獻(xiàn)包括:1)為了準(zhǔn)確檢測出無人車的運(yùn)動狀態(tài)，提出一種基于多條件約束的靜止?fàn)顟B(tài)檢測算法；2)為了消除無人車由于較長時間處于靜止?fàn)顟B(tài)而導(dǎo)致的定位累積誤差，利用零速度、相同位置和相同姿態(tài)等靜態(tài)約束，提出一種基于自適應(yīng)因子的自適應(yīng)零速更新策略；3)將靜止?fàn)顟B(tài)檢測算法和自適應(yīng)零速更新策略與MSCKF有效融合，提出AZUPT-MSCKF方法，并進(jìn)行實驗驗證。

1 低速無人車定位系統(tǒng)描述

1.1 坐標(biāo)系定義

考慮一類基于相機(jī)與IMU融合的低速無人車定位系統(tǒng)，并對該系統(tǒng)中使用到的相關(guān)坐標(biāo)系進(jìn)行定義。如圖1所示，主要包括世界坐標(biāo)系{W}、IMU坐標(biāo)系{I}和相機(jī)坐標(biāo)系{C}。其中，{W}是固定不動的，其原點(diǎn)Ow與{I}的原點(diǎn)O i在算法初始運(yùn)行時刻重合，且{W}的X w－O w－Yw平面平行于地面，Z w軸指向與重力g相反的方向，Yw軸指向無人車的正前方，X w軸由右手定則決定；{I}，又稱為無人車車體坐標(biāo)系，與無人車車體固連，隨無人車運(yùn)動而變化；{C}是設(shè)置在左目相機(jī)的坐標(biāo)系上。同時，由于{C}和{I}之間的相對位姿是剛性的，不會隨無人車運(yùn)動而發(fā)生改變，所以在實驗前對相機(jī)和IMU進(jìn)行聯(lián)合標(biāo)定，獲得{C}和{I}間的轉(zhuǎn)換矩陣，用于兩者之間的相對位姿轉(zhuǎn)換。最后，低速無人車定位系統(tǒng)的任務(wù)轉(zhuǎn)換為實時估計{I}相對于{W}的六自由度位姿。

圖1 低速無人車定位系統(tǒng)中坐標(biāo)系示意圖

1.2 狀態(tài)向量定義

所提出的AZUPT-MSCKF 方法是基于MSCKF框架設(shè)計，因此，在任意時刻，系統(tǒng)狀態(tài)向量中包括當(dāng)前IMU狀態(tài)和一個含N個過去相機(jī)位姿的滑動窗口。

IMU狀態(tài)向量定義為:

根據(jù)式(1)，IMU誤差狀態(tài)向量定義為:

式中:對于四元數(shù)，誤差值δq、真實值q和估計值＾q之間滿足:q＝δq?＾q，且?表示四元數(shù)乘法；對于速度、位置和偏差，誤差值等于真實值減去估計值(例如:W~v I＝W v I－W＾v I)。同時，誤差軸角δθ是誤差四元數(shù)δq的最小表示，且兩者之間近似滿足:

假設(shè)在K時刻，系統(tǒng)狀態(tài)向量中保留了一個含N個過去相機(jī)位姿(位置和姿態(tài))的滑動窗口，則AZUPT-MSCKF狀態(tài)向量定義為:

類似于式(2)，AZUPT-MSCKF誤差狀態(tài)向量定義為:

最后，低速無人車定位系統(tǒng)的任務(wù)進(jìn)一步轉(zhuǎn)換為實時估計W p I和

2 基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法

本文提出的基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法利用相機(jī)與消費(fèi)級IMU等低成本傳感器，設(shè)計了一種自適應(yīng)零速修正機(jī)制，并將其集成到MSCKF框架中作為一種自然測量更新過程，能夠有效地處理無人車由于較長時間處于靜止?fàn)顟B(tài)而導(dǎo)致的定位性能下降等問題，其整體流程如圖2所示。

圖2 基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法整體流程

本文提出的自適應(yīng)零速修正機(jī)制相關(guān)步驟已在流程圖中使用藍(lán)色重點(diǎn)標(biāo)注；同時，黑色表示傳統(tǒng)MSCKF算法部分。系統(tǒng)輸入為相機(jī)圖像和IMU數(shù)據(jù)。一旦接收到新的測量，首先判斷是否完成初始化，若未完成初始化，則執(zhí)行初始化，并構(gòu)造AZUPT-MSCKF狀態(tài)向量，若已經(jīng)完成初始化，則執(zhí)行IMU狀態(tài)及協(xié)方差傳播、圖像特征提取及跟蹤和相機(jī)狀態(tài)及協(xié)方差擴(kuò)增；其次通過靜止?fàn)顟B(tài)檢測部分檢測出無人車的當(dāng)前運(yùn)動狀態(tài)，若無人車處于靜止?fàn)顟B(tài)，執(zhí)行本文新增的自適應(yīng)零速更新過程，而MSCKF測量更新過程將會停止，若無人車處于運(yùn)動狀態(tài)，則執(zhí)行MSCKF測量更新過程；同時，任何可能會導(dǎo)致IMU傳播出現(xiàn)偏差的變量都可以在自適應(yīng)零速更新過程中進(jìn)行修正；最后進(jìn)行滑動窗口狀態(tài)管理，并實時輸出無人車的位姿估計值，從而實現(xiàn)無人車定位。

綜上，所提出的AZUPT-MSCKF方法主要包括傳統(tǒng)MSCKF算法和自適應(yīng)零速修正機(jī)制兩部分。有關(guān)傳統(tǒng)MSCKF算法的內(nèi)容可參考文獻(xiàn)[12]，在此不再敘述；接下來將重點(diǎn)介紹本文新增的自適應(yīng)零速修正機(jī)制的相關(guān)內(nèi)容。

3 自適應(yīng)零速修正機(jī)制

本文提出一種自適應(yīng)零速修正機(jī)制，主要包括靜止?fàn)顟B(tài)檢測和自適應(yīng)零速更新兩部分。一旦通過靜止?fàn)顟B(tài)檢測部分檢測出無人車當(dāng)前處于靜止?fàn)顟B(tài)，將啟動自適應(yīng)零速更新。接下來將詳細(xì)介紹基于多條件約束的靜止?fàn)顟B(tài)檢測算法和基于自適應(yīng)因子的自適應(yīng)零速更新策略。

3.1 基于多條件約束的靜止?fàn)顟B(tài)檢測算法

結(jié)合車載IMU中的陀螺儀和加速度計測量數(shù)據(jù)，提出一種基于多條件約束的靜止?fàn)顟B(tài)檢測算法，用于實時檢測無人車的運(yùn)動狀態(tài)。

定義a k∈R3和ωk∈R3，分別表示k時刻的三軸加速度矢量和三軸角速度矢量。靜止?fàn)顟B(tài)檢測就是給定測量序列在區(qū)間[n，n＋M－1]內(nèi)，滑動窗口大小為M的條件下，根據(jù)下式完成無人車的當(dāng)前運(yùn)動狀態(tài)檢測:

式中:T(a n，ωn)表示檢測算法的統(tǒng)計數(shù)據(jù)，g表示當(dāng)?shù)刂亓铀俣确謩e表示加速度計和陀螺儀的測量噪聲方差，且兩者之間的比值反應(yīng)了來自加速度計與陀螺儀數(shù)據(jù)中的擾動，影響檢測算法的性能，表示矢量a的大小，λ表示檢測閾值表示區(qū)間[n，n＋M－1]內(nèi)的樣本均值，定義如下:

最后，若式(6)成立，則判斷出無人車處于靜止?fàn)顟B(tài)；反之，無人車處于運(yùn)動狀態(tài)。綜上，其整體流程如表1所示。

表1 基于多條件約束的靜止?fàn)顟B(tài)檢測算法

3.2 基于自適應(yīng)因子的自適應(yīng)零速更新策略

當(dāng)檢測到無人車處于靜止?fàn)顟B(tài)時，首先利用IMU速度為零、滑動窗口內(nèi)最新與第二新的相機(jī)位姿相同等靜態(tài)約束條件定義測量殘差；其次構(gòu)建測量殘差與AZUPT-MSCKF誤差狀態(tài)向量之間的殘差模型；最后執(zhí)行測量更新，并采用基于自適應(yīng)因子的測量噪聲協(xié)方差矩陣在線調(diào)整策略實現(xiàn)自適應(yīng)零速更新，完成狀態(tài)修正，其整體流程如表2所示。

表2 基于自適應(yīng)因子的自適應(yīng)零速更新策略

3.2.1 測量殘差定義

①IMU零速度殘差

無人車處于靜止?fàn)顟B(tài)時，IMU當(dāng)前速度的真實值為零，定義為:

此時，通過IMU狀態(tài)傳播得到IMU當(dāng)前速度的估計值，定義為:

由式(8)和式(9)，可得IMU零速度殘差，定義為:

由上式可知，IMU零速度殘差可由IMU當(dāng)前速度的估計值來獲得，并且該殘差是關(guān)于誤差狀態(tài)W~v I的線性函數(shù)。

②相機(jī)位置相同殘差

無人車處于靜止?fàn)顟B(tài)時，滑動窗口內(nèi)最新與第二新相機(jī)位置的真實值相同，定義為:

式中:W p C N為滑動窗口內(nèi)最新相機(jī)位置的真實值，W p C N－1為滑動窗口內(nèi)第二新相機(jī)位置的真實值。

此時，通過相機(jī)狀態(tài)擴(kuò)增得到滑動窗口內(nèi)最新與第二新相機(jī)位置的估計值，定義為:

由式(11)和式(12)，可得滑動窗口內(nèi)最新與第二新相機(jī)位置相同殘差，定義為:

由式(13)可知，滑動窗口內(nèi)最新與第二新相機(jī)位置相同殘差可由滑動窗口內(nèi)最新與第二新相機(jī)位置的估計值來獲得，并且該殘差是關(guān)于誤差狀態(tài)的線性函數(shù)。

③相機(jī)姿態(tài)相同殘差

無人車處于靜止?fàn)顟B(tài)時，滑動窗口內(nèi)最新與第二新相機(jī)姿態(tài)的真實值相同，即定義為:

此時，通過相機(jī)狀態(tài)擴(kuò)增得到滑動窗口內(nèi)最新與第二新相機(jī)姿態(tài)的估計值，定義為:

綜上，可得滑動窗口內(nèi)最新與第二新相機(jī)姿態(tài)相同殘差，定義為:

由于AZUPT-MSCKF誤差狀態(tài)向量中使用誤差軸角δθ代替誤差四元數(shù)δq，則該殘差又可近似表達(dá)為:

式中:{·}i jk表示取四元數(shù)虛部的三個系數(shù)構(gòu)成一個3×1的向量。

由上式可知，滑動窗口內(nèi)最新與第二新相機(jī)姿態(tài)相同殘差可由滑動窗口內(nèi)最新與第二新相機(jī)姿態(tài)的估計值來獲得，并且該殘差是關(guān)于誤差狀態(tài)和的線性函數(shù)。

3.2.2 殘差模型構(gòu)建

3.2.3 測量更新執(zhí)行

由前文構(gòu)建的殘差模型，按照EKF更新方式執(zhí)行AZUPT-MSCKF測量更新。

計算卡爾曼增益KZUPT:

式中:P表示AZUPT-MSCKF狀態(tài)協(xié)方差矩陣，與MSCKF狀態(tài)協(xié)方差矩陣相同，可參考文獻(xiàn)[12]，QZUPT表示測量噪聲nZUPT的協(xié)方差矩陣，定義為:

執(zhí)行狀態(tài)更新:

執(zhí)行狀態(tài)協(xié)方差矩陣更新:

式中:τ＝15＋6N表示協(xié)方差矩陣的維度。

此外，任何可能會導(dǎo)致IMU偏差傳播的變量都可以通過此次測量更新過程進(jìn)行修正。

3.2.4 基于自適應(yīng)因子的測量噪聲協(xié)方差矩陣在

線調(diào)整策略

在零速更新過程中，考慮到可能會存在將慢速或勻速運(yùn)動檢測為靜止?fàn)顟B(tài)等錯誤檢測情況，因此可以采用少量的QZUPT來提高算法的容錯性。然而，在工程實現(xiàn)中，確定最優(yōu)的QZUPT較為復(fù)雜。對此，本文提出了一種基于自適應(yīng)因子的測量噪聲協(xié)方差矩陣在線調(diào)整策略來提高算法的適應(yīng)性，即通過構(gòu)造自適應(yīng)因子，并利用測量噪聲協(xié)方差矩陣與自適應(yīng)因子的比值實現(xiàn)測量噪聲協(xié)方差矩陣的在線修正。

首先，定義自適應(yīng)因子αZUPT為:式中:1.0＜c0＜1.5和3.5＜c1＜4.5分別表示常數(shù)閾值，表示預(yù)報殘差，定義為:

式中:σnZUPT表示測量噪聲nZUPT的均方差。

其次，定義測量噪聲協(xié)方差矩陣QZUPT與自適應(yīng)因子αZUPT的比值為等效測量噪聲協(xié)方差矩陣

4 實驗結(jié)果及分析

為了驗證本文所提方法的有效性和優(yōu)越性，在校園內(nèi)的實際場景中進(jìn)行實驗測試，分別對比本文所提方法與傳統(tǒng)MSCKF算法[12]和VINS-Mono算法[8]的定位效果，且部分實驗過程圖如圖3所示。實驗使用搭載小覓雙目攝像頭深度版(D1000－IR－120/Color)和帶有FR－07阿克曼線控底盤的低速無人車，在裝有Linux操作系統(tǒng)(Ubuntu 16.04)和機(jī)器人操作系統(tǒng)(ROS)的Lenovo G50筆記本電腦(核心處理器:Intel Core i5)上進(jìn)行，如圖3所示。其中，D1000－IR－120/Color包含一個BMI088 MEMS IMU和兩個AR0135全局快門相機(jī)，輸出硬件同步的30 Hz全局快門圖像和200 Hz IMU測量，安裝在低速無人車上的高度約為0.95 m。另外，由于本文所提方法屬于單目VIO，所以實驗中只使用了左目相機(jī)圖像和IMU測量數(shù)據(jù)。

圖3 實驗平臺及部分實驗過程圖

另一方面，圖像特征提取及跟蹤采用FAST特征檢測器和KLT光流跟蹤算法相結(jié)合的方式，且相關(guān)實驗參數(shù)的設(shè)置如表3所示。

表3 實驗參數(shù)設(shè)置

4.1 交通燈路口等候紅燈實驗

實驗一，在一段總長約10 m的模擬交通燈路口中進(jìn)行，模擬了無人車在交通燈路口遇紅燈停車等候然后向左拐彎行駛?cè)^程，且無人車停車位置已在圖中標(biāo)注，如圖4(a)所示。實驗中，首先考慮到無人車在交通燈路口遇紅燈停車等候的時間不同，故將停車等候時間劃分為短時間(5 s～10 s)、中等時間(20 s～25 s)和長時間(35 s～40 s)等三個區(qū)段；其中，單次運(yùn)行以上每組實驗花費(fèi)的時間分別約為25 s、40 s、55 s，且無人車行駛時的平均速度約為0.5 m/s；其次按照圖4(a)中所示軌跡以及不同停車等候時間分別運(yùn)行本文所提方法和傳統(tǒng)MSCKF算法，得到以上兩種不同方法估計出的無人車運(yùn)動軌跡，并通過EVO軟件繪制成相應(yīng)曲線，如圖4(b)、(c)、(d)所示；然后根據(jù)曲線定性分析出兩種不同方法對應(yīng)的無人車定位軌跡在停車處的軌跡漂移結(jié)果；最后重復(fù)運(yùn)行每組實驗各10次，分別計算出兩種不同方法對應(yīng)的無人車定位軌跡的均方根誤差(Root-Mean-Squared Error，RMSE)，如表4所示。

表4 基于交通燈路口等候紅燈實驗的平均RMSE單位:m

通過對比圖4(b)、(c)、(d)中的無人車定位軌跡可以發(fā)現(xiàn)，隨著停車時間的增長，停車處(圖中圓圈處)出現(xiàn)的軌跡漂移量逐漸增大，且停車處之后的軌跡偏差也逐漸增大；同時，本文提出的AZUPTMSCKF方法對應(yīng)的停車處軌跡漂移小于傳統(tǒng)MSCKF算法，且整體軌跡更接近于真實軌跡，取得了更好的定位效果。這是由于實驗中使用的BMI088屬于低成本IMU，有偏傳播的誤差是不可忽視的。當(dāng)無人車處于靜止?fàn)顟B(tài)時，由于無法發(fā)生特征點(diǎn)三角化而導(dǎo)致傳統(tǒng)MSCKF算法中的相機(jī)測量更新停止，此時無人車定位軌跡由于IMU的有偏傳播而發(fā)生漂移，而當(dāng)無人車再次開始移動時，即將到來的有效相機(jī)測量將導(dǎo)致傳統(tǒng)MSCKF算法對應(yīng)的無人車定位軌跡發(fā)生突變。然而，本文所提方法通過新增的自適應(yīng)零速修正機(jī)制來校正IMU的偏差傳播，使得無人車定位軌跡具有較小的漂移量。表4給出了兩種方法在交通燈路口等候紅燈實驗中的平均RMSE結(jié)果，其中本文所提方法對應(yīng)的無人車定位軌跡的RMSE值小于傳統(tǒng)MSCKF算法。綜上，說明了本文所提方法的定位性能優(yōu)于傳統(tǒng)MSCKF算法。

圖4 交通燈路口等候紅燈實驗結(jié)果

4.2 高樓之間閉合軌跡運(yùn)行實驗

實驗二，在一段總長約106 m的室外高樓之間的閉合道路上進(jìn)行，環(huán)境較開闊，部分道路不平穩(wěn)，時有行人穿過，且路邊有部分結(jié)構(gòu)化設(shè)施；同時，使無人車做閉合軌跡運(yùn)行，行駛過程中共在七處停車，已在圖中標(biāo)注，如圖5(a)所示。其中，單次實驗花費(fèi)的時間約為109 s，且無人車行駛時的平均速度約為1.2 m/s。實驗中，首先按照圖5(a)中所示軌跡分別運(yùn)行本文所提方法和傳統(tǒng)MSCKF算法，得到以上兩種不同方法對應(yīng)的無人車定位軌跡，并通過EVO軟件繪制成相應(yīng)曲線，如圖5(b)所示；然后重復(fù)運(yùn)行每組實驗各10次，分別計算出兩種不同方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差，如表5所示。

表5 實驗二的平均軌跡閉環(huán)誤差 單位:m

圖5 實驗二結(jié)果

另外，為了進(jìn)一步驗證本文所提方法的有效性和優(yōu)越性，在該實驗環(huán)境下與VINS-Mono算法進(jìn)行定位精度對比，即實驗三?？紤]到本文所提方法中無回環(huán)檢測，且VINS-Mono算法中無零速修正機(jī)制，因此，為了公平對比，在進(jìn)行對比實驗時，關(guān)閉VINS-Mono算法中的回環(huán)檢測，且無人車在行駛過程中未有停車，其他實驗步驟與上述實驗二相同。最后，重復(fù)運(yùn)行該實驗10次，分別計算出兩種不同方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差，如表6所示。

表6 實驗三的平均軌跡閉環(huán)誤差 單位:m

由圖5(b)可以看出，本文所提方法和傳統(tǒng)MSCKF算法均能成功估計出無人車真實軌跡的大致走勢。然而，一旦無人車處于靜止?fàn)顟B(tài)并再次運(yùn)行后，傳統(tǒng)MSCKF算法對應(yīng)的無人車定位軌跡明顯出現(xiàn)漂移現(xiàn)象，并且隨著無人車停車次數(shù)的增加，總體軌跡偏差也逐漸增大；而本文所提方法相比于傳統(tǒng)MSCKF算法則出現(xiàn)較小的軌跡漂移，且估計出的終點(diǎn)位置也更接近于真實的終點(diǎn)位置，如圖5(b)中的圓圈處所示。表5給出了兩種方法的平均軌跡閉環(huán)誤差結(jié)果，其中本文所提方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差小于傳統(tǒng)MSCKF算法，取得了較好的定位效果，說明了本文所提方法在室外環(huán)境下的定位精度高于傳統(tǒng)MSCKF算法。同時，由表6可以看出，相比于VINS-Mono算法，本文所提方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差較小，具有較好的定位性能，從而進(jìn)一步驗證了本文所提方法的有效性和優(yōu)越性。

4.3 地下停車場規(guī)定軌跡運(yùn)行實驗

實驗四，在一段總長約25 m的地下停車場中進(jìn)行，模擬了無人車倒車入庫出庫全過程，且起點(diǎn)與終點(diǎn)之間的距離約0.94 m；同時車輛運(yùn)動情況較復(fù)雜，包括直線、轉(zhuǎn)彎、快速、慢速和停車等狀態(tài)，且無人車在行駛過程中共有兩處停車，已在圖中標(biāo)注，如圖6(a)所示。其中，單次實驗花費(fèi)的時間約為44 s，且無人車行駛時的平均速度約為0.7 m/s。實驗中，首先在路徑上設(shè)置四個坐標(biāo)點(diǎn)(0.00，0.00)、(－7.90，1.10)、(－5.20，4.00)、(0.00，0.94)；其次按照圖6(a)中所示軌跡分別運(yùn)行本文所提方法和傳統(tǒng)MSCKF算法，得到以上兩種不同方法對應(yīng)的無人車定位軌跡，并通過EVO軟件繪制出相應(yīng)曲線，如圖6(b)所示；然后重復(fù)運(yùn)行每組實驗各10次，分別計算出兩種不同方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差，如表7所示。

圖6 地下停車場規(guī)定軌跡運(yùn)行實驗結(jié)果

表7 基于地下停車場規(guī)定軌跡運(yùn)行實驗的平均軌跡閉環(huán)誤差 單位:m

由圖6(b)可以看出，相比于傳統(tǒng)MSCKF算法，本文所提方法對應(yīng)的無人車定位軌跡更接近于真實軌跡，且估計出的無人車終點(diǎn)位置也更接近于真實的終點(diǎn)位置；同時，當(dāng)無人車處于靜止?fàn)顟B(tài)時，本文所提方法對應(yīng)的無人車定位軌跡也具有較小的漂移量，如圖6(b)中的圓圈處所示。表7給出了兩種不同方法在地下停車場規(guī)定軌跡運(yùn)行實驗中的平均軌跡閉環(huán)誤差結(jié)果，其中本文所提方法對應(yīng)的無人車定位軌跡的平均閉環(huán)誤差小于傳統(tǒng)MSCKF算法，取得了較好的定位效果，說明了本文所提方法在室內(nèi)環(huán)境下的定位精度高于傳統(tǒng)MSCKF算法。

5 結(jié)論

本文針對缺失GNSS信號條件下的低速無人車定位問題，在MSCKF框架下，提出了一種基于自適應(yīng)零速修正機(jī)制的低速無人車定位方法(AZUPTMSCKF)。該方法首先設(shè)計了一種自適應(yīng)零速修正機(jī)制，并將其集成到MSCKF框架中作為一種自然測量更新過程，能夠有效處理無人車由于較長時間處于靜止?fàn)顟B(tài)而導(dǎo)致的定位性能下降等問題。實驗結(jié)果表明，本文所提方法相比于傳統(tǒng)MSCKF算法及VINS-Mono算法(關(guān)閉回環(huán)檢測)具有更高的定位精度和更強(qiáng)的魯棒性。在下一階段的工作中，我們將繼續(xù)優(yōu)化AZUPT-MSCKF方法中的圖像特征提取機(jī)制，并在真實行車環(huán)境中進(jìn)一步測試算法的定位性能。