1、前言：

經(jīng)典狀態(tài)估計中協(xié)方差的計算能準確反映機器人狀態(tài)的完整性嗎？

在每一次機器人執(zhí)行各種目的的自動導(dǎo)航任務(wù)時，擁有高精度且可靠狀態(tài)估計的能力可以說是機器人順利、安全完成任務(wù)的必要條件之一。簡而言之，機器人狀態(tài)估計算法不單單需要提供精準的狀態(tài)估計量，還需要能準確地表征狀態(tài)估計的準確度，比如在退化和傳感器失效等場景，狀態(tài)估計器能提示狀態(tài)機或者用戶此時刻的估計值的準確度風(fēng)險，不能機器人的狀態(tài)估計明顯“飄”了，還在提示“一切安好”。現(xiàn)在回到本章節(jié)的問題：經(jīng)典狀態(tài)估計中協(xié)方差的計算能準確反映機器人狀態(tài)的完整性嗎？，該解答需要的是理解經(jīng)典狀態(tài)估計算法中的前提假設(shè)和模型構(gòu)建。

首先，典中典的狀態(tài)估計算法就是“濾波器”系列了，以擴展卡爾曼濾波器EKF為例，狀態(tài)轉(zhuǎn)移階段（state evolution）的公式如下：

公式中右邊w聲明的是一個高斯白噪聲，我本人理解是用該高斯白噪聲表示在狀態(tài)轉(zhuǎn)移階段各個系統(tǒng)輸入量（system inputs）引入后計算所得預(yù)測結(jié)果誤差，由于在實際使用過程中各個傳感器性能和校驗都較好，故而該高斯白噪聲w是一個以0為均值的較為集中的分布。另一個就是觀測模型（measurement models）的公式了：

觀測模型計算公式右邊v聲明的也是一個以0為均值的較為集中的高斯分布，本人的理解是用其表示在觀測階段各個觀測數(shù)據(jù)量引入后計算所得觀測結(jié)果誤差。而EKF算法中的其他部分，即使在預(yù)測和觀測更新的過程中利用w和v對狀態(tài)量協(xié)方差進行更新與校正，其考慮的也是在傳感器運行過程中正常范圍內(nèi)的“性能誤差”引起估計值波動。由此，使用該方法計算所得的狀態(tài)量協(xié)方差不能用以在退化和傳感器失效等場景下機器人狀態(tài)的完整性監(jiān)控。

另外一個經(jīng)典幀匹配算法論文：Real-Time Correlative Scan Matching（CSM幀匹配算法），該論文的主要貢獻點之一就是展示了在CSM配準過程中如何獲取估計協(xié)方差，換言之就是提供了一個“基于CSM方法的位姿不確定性”的判斷方法。

圖：CSM估計的樣本協(xié)方差。左：在x和y方向都有很好的約束，產(chǎn)生可信的協(xié)方差估計。右：長走廊只提供少數(shù)y方向縱向約束，x方向則產(chǎn)生一個拉長的不確定性橢圓

通過研讀論文我們可以發(fā)現(xiàn)The Uncertainty of CSM計算模型是只考慮了“激光點云的噪聲”和“激光點云與地圖（Map）中corresponding cells的不確定性”，并不是考慮和分析完整環(huán)境模型（Global Map Model），故而其獲取得到的估計協(xié)方差、不確定性受限于搜索窗口中的“離散采樣點”，即使Map Model中存在很多“高概率”的cells，倘若這些cells不能被“離散采樣點”構(gòu)成的搜索窗口包含，則無法反映到The Uncertainty of CSM的計算中。所以，論文作者也提到如若想基于CSM方法來計算其位姿估計的不確定性，“離散采樣點”構(gòu)成的搜索窗口應(yīng)該適當調(diào)整大一些（稍微大于匹配過程的搜索窗口），以保證盡可能多的將Map Model加入Uncertainty計算，但如此一來，計算量會適當增加，在具體的場景中如何設(shè)置這個計算方法中的搜索窗口參數(shù)就是一個trade off了。由此，CSM算法提供了在退化場景下估計的不確定性計算，且越是準確的不確定性表征值背后是越大的計算消耗，但CSM算法仍未針對傳感器失效場景下為機器人狀態(tài)的完整性監(jiān)控提供相應(yīng)的解決策略。

2、影響定位精度和安全的條件分析

考慮到篇幅和文章的主題，影響定位精度和安全的條件分析局限在離群點outliers對狀態(tài)估計精度和對估計不確定性計算的影響。

以點云配準算法為例，輸入初值initial-pose的擾動級別越大、相鄰點云間的重疊度越差，則配準的精度和穩(wěn)定性就越低；以RTK-GNSS定位為例，作業(yè)過程中天空遮擋越多、四周環(huán)境越復(fù)雜，則獲取和解算出的定位信息精度和穩(wěn)定性越低；以視覺SLAM技術(shù)為例，當視角變化越復(fù)雜、天氣和光照變化越大，則視覺匹配的精度和穩(wěn)定性越差……

前面提到擁有高精度且可靠狀態(tài)估計的能力是機器人順利、安全完成任務(wù)的必要條件之一。具體的，在精細需求的應(yīng)用過程中，高精度的狀態(tài)估計是機器人能順利每一項具體精細動作的前提條件之一；在一些life- or mission- critical的應(yīng)用過程中，穩(wěn)定可靠的狀態(tài)估計是機器人能避免在作業(yè)中造成人員損傷或物料損失的前提之一，即是通過監(jiān)測導(dǎo)航完整性的風(fēng)險概率值來保證作業(yè)“安全”，更準確地說，導(dǎo)航完整性的風(fēng)險概率是指真實的機器人狀態(tài)估計誤差超出了預(yù)先設(shè)定的可接受閾值且沒有任何報警發(fā)生的情景。

下文將分享本人整理的一些針對“特征”離群點判斷與剔除和定位狀態(tài)完整性監(jiān)控相關(guān)方向的技術(shù)論文，推薦的原因是本人覺得這些論文提出的理論方法、技術(shù)方案對解決上面的問題很有幫助，也能為更深入的研究提供方向。

3、“特征”離群點判斷與剔除

3.1 研究意義

該離群點判斷與剔除類技術(shù)方案是受到RAIM（Receiver Autonomous Integrity Monitoring）的啟發(fā)，RAIM其是針對GNSS數(shù)據(jù)的完整性監(jiān)控，雖然完整性監(jiān)測是GNSS應(yīng)用的一個很好的發(fā)展領(lǐng)域，但將其應(yīng)用于基于優(yōu)化、濾波的移動機器人、無人機狀態(tài)估計（例如激光、視覺建圖定位）仍然不是一件簡單的事情。首先，在大多數(shù)機器人狀態(tài)估計算法中，系統(tǒng)依賴于特征（點云特征、視覺特征、路標點等）來提供觀測測量，而不是衛(wèi)星，與GNSS應(yīng)用相比，觀測測量量要多得多。

其次，在RAIM中，兩顆衛(wèi)星同時故障的情況很少見。然而，對于機器人狀態(tài)估計，不正確測量（傳感器失效）的絕對數(shù)量或異常值（離群觀測量）要高得多，異常值比率通常可以大于10%。因此，需要一種不同的離群點排除方法來處理多個離群點。此外，在GNSS應(yīng)用中，每個衛(wèi)星提供一個測量值，但在機器人狀態(tài)估計中，每個特征可能為定位軌跡中關(guān)鍵幀提供2或3個觀測測量值。

3.2 相關(guān)技術(shù)論文

以上提及的相關(guān)技術(shù)延申到機器人狀態(tài)估計的問題僅是一部分，多年來針對此類問題提出解決策略的技術(shù)論文也有很多，如下：

1. W. Ochieng， K. Sheridan， K. Sauer， X. Han， P. Cross， S. Lannelongue， N. Ammour， and K. Petit， “An assessment of the raim performance of a combined galileo/gps navigation system using the marginally detectable errors （mde） algorithm，” GPS Solutions， vol. 5， no. 3， pp. 42–51， 2002.

2. C. H. Tong and T. D. Barfoot， “Batch heterogeneous outlier rejection for feature-poor slam，” in 2011 IEEE International Conference on Robotics and Automation. IEEE， 2011， pp. 2630–2637.

3. A. Das and S. L. Waslander， “Outlier rejection for visual odometry using parity space methods，” in 2014 IEEE International Conference on Robotics and Automation （ICRA）。 IEEE， 2014， pp. 3613–3618.

4. M. A. Fischler and R. C. Bolles， “Random sample consensus： a paradigm for model fitting with applications to image analysis and automated cartography，” Communications of the ACM， vol. 24， no. 6， pp. 381–395， 1981.

5. B. Kitt， A. Geiger， and H. Lategahn， “Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme，” in 2010 IEEE intelligent vehicles symposium. IEEE， 2010， pp. 486–492.

6. V. Tzoumas， P. Antonante， and L. Carlone， “Outlier-robust spatial perception： Hardness， general-purpose algorithms， and guarantees，” arXiv preprint arXiv:1903.11683， 2019.

7. Mu， He-Qing， and Ka-Veng Yuen. “Novel Outlier-Resistant Extended Kalman Filter for Robust Online Structural Identification.” Journal of Engineering Mechanics， vol. 141， no. 1， 2015.

8. I. E. Potter and M. C. Sunman， “Threshold-less redundancy management with arrays of skewed instruments，” AGARD， Tech. Rep. AGARDOGRAPH-224 （pp 15-25）， 1977.

9. F. Gustafsson， “Statistical signal processing approaches to fault detection，” Annual Reviews in Control， vol. 31， no. 1， pp. 41–54， 2007.

10. F. Gustafsson and F. Gustafsson， Adaptive filtering and change detection. Wiley Londres， 2000， vol. 1.

11. D. Borrmann， J. Elseberg， K. Lingemann， A. Nuchter， and J. Hertzberg， “Globally consistent 3D mapping with scan matching，” Robotics and Autonomous Systems， vol. 56， no. 2， pp. 130–142， February 2008.

12. Huang， Feng， et al. “A Coarse-to-Fine Lidar-Based Slam with Dynamic Object Removal in Dense Urban Areas.” Proceedings of the 34th International Technical Meeting of the Satellite Division of The Institute of Navigation （ION GNSS+ 2021）， 2021.

13. Wen， Weisong， et al. “Integrity Monitoring for GNSS Positioning via Factor Graph Optimization in Urban Canyons.” Proceedings of the 34th International Technical Meeting of the Satellite Division of The Institute of Navigation （ION GNSS+ 2021）， 2021.

14. Li， Chengyao， and Steven L. Waslander. “Visual Measurement Integrity Monitoring for UAV Localization.” 2019 IEEE International Symposium on Safety， Security， and Rescue Robotics （SSRR）， 2019.

3.3 部分論文摘要

注：本小節(jié)選取僅僅是個人覺得對理解該類研究很有幫助的論文，并不是對上述論文做任何的質(zhì)量評價和重要性評估等主觀排名行為。

針對No.14 Visual Measurement Integrity Monitoring for UAV Localization， 作者在文中首要定義和討論了如何檢測出outliers并剔除的算法（I.P.S.O.R）及其相關(guān)的Protection Level，其次定義了R.B.T Metric，該metric用于定量評估“誤差上界”的設(shè)計、選擇估計計算的效果與合理性，最后在文中討論了如何去計算出一個大致的誤差上界（error bound），Visual Measurement Integrity Monitoring for UAV Localization這篇論文發(fā)布的年份較新，可以說其中提出來的技術(shù)方案是博采眾長，美中不足的是具體的理論推導(dǎo)不夠細膩。

針對No.3 Outlier rejection for visual odometry using parity space methods，論文作者就較為原理性地說明和推導(dǎo)了Parity Space是如何生成的，該Parity Space的概念也是被No.14 Visual Measurement Integrity Monitoring for UAV Localization參考并優(yōu)化設(shè)計，再者就是作者給出了fault vector的選取示例，幫助理解fault vector在機器人狀態(tài)估計模型中的作用，最后提出PI-RANSAC的算法來進行離群點的剔除，該算法可以與上面的I.P.S.O.R算法進行對比分析以助于相互理解。

再根據(jù)No.2 Batch heterogeneous outlier rejection for feature-poor slam，雖然這篇論文針對的feature-poor的場景，但是在當時該論文提出了較為“新穎”地使用了批量分塊（Batch）處理思想，并且詳細地對比了三種設(shè)計下的剔除效果：第一個測試單獨使用測量模型，第二個測試將不確定度納入假設(shè)模型，第三個測試將不確定度納入假設(shè)模型并測量數(shù)據(jù)作為一個批量分塊考慮。

No.2在論文中提到盡管在第二種檢驗中考慮了假設(shè)模型的不確定性，但它仍然是真實噪聲模型的近似值，因為它假設(shè)估計誤差之間獨立，導(dǎo)致計算出的error bound不能有效地反映真實的數(shù)據(jù)分布情況。雖然測量值可能是獨立獲得的，但估計的誤差都是基于一個共同的假設(shè)模型，這個共同的基礎(chǔ)將值連接在一起，提供了估計誤差之間的相關(guān)性。由此No.2 Batch heterogeneous outlier rejection for feature-poor slam證明了在離群點判斷和剔除過程中不僅需要考慮機器人模型的不確定性，還最好利用批量分塊的觀測數(shù)據(jù)作為一個處理單元進行分析。而這個關(guān)鍵的思想基本上被后續(xù)所有的論文所引用并優(yōu)化升級。

4、 定位狀態(tài)完整性監(jiān)控

4.1 研究意義

通過上述的離群點判斷和濾除算法，在數(shù)據(jù)被使用前就可以將不正確或者異常數(shù)據(jù)完成過濾，那么數(shù)據(jù)被利用在機器人狀態(tài)估計的預(yù)測或者觀測更新階段時，就是可以放心使用的了。但在每個時刻的后驗狀態(tài)估計該怎么進行不確定性評價呢？這就需要一個有效監(jiān)測移動機器人定位安全的方法了。

定位安全可通過測量機器人狀態(tài)系統(tǒng)的完整性風(fēng)險來量化的，這是一個著名的航空性能指標（aviation performance metric）。然而，航空完整性監(jiān)測解決方案幾乎完全依賴于全球衛(wèi)星導(dǎo)航系統(tǒng)（GNSS），而機器人導(dǎo)航通常需要狀態(tài)演化模型和/或相對定位傳感器提供的額外信息，這使得先前建立的方法不切實際。那么如何設(shè)計一類適用于基于優(yōu)化、濾波的移動機器人、無人機狀態(tài)估計（例如激光、視覺建圖定位）的高效完整性監(jiān)測方法成為了亟待解決的問題。

4.2 相關(guān)技術(shù)論文

多年來針對此類問題提出解決策略的技術(shù)論文也有很多，如下：

1. G. D. Arana， M. Joerger， and M. Spenko， “Efficient integrity monitoring for kf-based localization，” in 2019 IEEE International Conference on Robotics and Automation （ICRA）。 IEEE， 2019， pp. 6374–6380.

2. G. D. Arana， O. A. Hafez， M. Joerger， and M. Spenko， “Recursive integrity monitoring for mobile robot localization safety，” in 2019 IEEE International Conference on Robotics and Automation （ICRA）。 IEEE， 2019， pp. 305–311.

3. S. Ahn， S. Yoon， S. Hyung， N. Kwak， and K. S. Roh， “On-board odometry estimation for 3d vision-based slam of humanoid robot，” in 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2012， pp. 4006–4012.

4. G. Huang， M. Kaess， and J. J. Leonard， “Towards consistent visualinertial navigation，” in 2014 IEEE International Conference on Robotics and Automation （ICRA）。 IEEE， 2014， pp. 4926–4933.

5. M. Joerger， M. Jamoom， M. Spenko， and B. Pervan， “Integrity of laser-based feature extraction and data association，” in 2016 IEEE/ION PLANS， April 2016， pp. 557–571.

6. M. Joerger， G. D. Arana， M. Spenko， and B. Pervan， “A new approach to unwanted-object detection in gnss/lidar-based navigation，” in Sensors， 2018.

7. G. D. Arana， M. Joerger， and M. Spenko， “Local nearest neighbor integrity risk evaluation for robot navigation，” ICRA， 2018.

8. M. Joerger and B. Pervan， “Kalman Filter-Based Integrity Monitoring Against Sensor Faults，” AIAA Journal of Guidance， Control and Dynamics， vol. 36， no. 2， pp. 349–361， 2013.

9. C. Tanil， M. Joerger， S. Khanafseh， and B. Pervan， “Sequential integrity monitoring for kalman filter innovation-based detectors，” ION-GNNS ［accepted for publication］， 2018.

10. A. Hassani， G. D. Arana， M. Spenko， and M. Joerger， “Lidar data association risk reduction using tight integration with ins，” ION-GNNS， 2018.

10. Wen， Weisong， and Li-Ta Hsu. “Towards Robust GNSS Positioning and Real-Time Kinematic Using Factor Graph Optimization.” 2021 IEEE International Conference on Robotics and Automation （ICRA）， 2021.

4.3 部分論文摘要

注：本小節(jié)選取僅僅是個人覺得對理解該類研究很有幫助的論文，并不是對上述論文做任何的質(zhì)量評價和重要性評估等主觀排名行為。

上面論文列表中的No.1 Efficient integrity monitoring for kf-based localization和No.2 Recursive integrity monitoring for mobile robot localization safety是由同一位研究學(xué)者Guillermo Duenas Arana及其團隊完成的，其較為充分地提出了可以有效應(yīng)用在基于濾波的機器人狀態(tài)估計中的完整性監(jiān)測算法（Localization Integrity Monitoring）。論文分析到卡爾曼濾波器（KF）提供的是一種遞歸解決的方案，該解決方案采用的是關(guān)于先驗估計和狀態(tài)演化模型的信息和/或其他類型的相對定位傳感器信息，在設(shè)計評估KF解決方案時的完整性風(fēng)險時，首先需要將當前時刻之前發(fā)生的故障封裝成估計偏差，然后通過將先前的狀態(tài)估計作為額外的測量，將KF更新方程重新表述為最小二乘擬合問題。根據(jù)該公式，使用傳統(tǒng)的航空RAIM算法評估定位完整性。論文提出的算法允許在一個公共框架內(nèi)有效監(jiān)測當前和以前的傳感器故障。雖然這種方法在應(yīng)用于測量源有限的任務(wù)時可能有顯著的缺點，但它具有實際的好處，特別是對于低計算能力的系統(tǒng)。

再進一步概況，No.1 Efficient integrity monitoring for kf-based localization和No.2 Recursive integrity monitoring for mobile robot localization safety的主要貢獻先是定義了Hazardous Misleading Information（HMI）的概念并提供了計算方法，在具體fault vector和estimate error的分布估計時簡單闡明了具體示例中fault detector該如何使用，其本質(zhì)上還是基于卡方分布完成對statistic test的分析。特別是No.2 Recursive integrity monitoring for mobile robot localization safety，該論文有很詳細的理論推導(dǎo)，細致地展示fault vector該如何設(shè)計和推到求解，與No.1論文相互對比和分析后，可實現(xiàn)相關(guān)理論的舉一反三。

5、總結(jié)與延申

在“特征”離群值的判斷與剔除算法機制的幫助下可以在一定程度上規(guī)避垃圾觀測數(shù)據(jù)和傳感器失效引發(fā)的機器人狀態(tài)估計錯誤問題，同時機器人狀態(tài)完整性監(jiān)控算法可為幫助機器人能準確地表征其狀態(tài)估計量的準確值，以幫助機器人在傳感器退化和傳感器失效等場景下及時完成導(dǎo)航、定位策略的切換和多傳感器觀測模型的深度融合。

另一方面，“特征”離群值的判斷與剔除算法和機器人狀態(tài)完整性監(jiān)控算法也為全生命周期建圖定位系統(tǒng)Life-long SLAM的穩(wěn)定實現(xiàn)提供了基礎(chǔ)。“特征”離群值的判斷與剔除算法可以幫助克服Life-long對視角變化，天氣變化，光照變化條件下，以及動態(tài)障礙物環(huán)境下，如何確保定位和建模的精度的核心問題，而機器人狀態(tài)完整性監(jiān)控算法則輔助保證Life-long過程中約束構(gòu)建的準確性，當Life-long過程中的環(huán)境發(fā)生變化之后，通過完整性監(jiān)控可以避免匹配約束構(gòu)建后的歧義，即是可以通過完整性監(jiān)控獲取得到的匹配約束準確度，從而對“較差”的約束進行其他處理。

審核編輯 ：李倩