1、前言：

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

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

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

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

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

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

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

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

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

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

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

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

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

　　3、“特征”離群點(diǎn)判斷與剔除

　　3.1 研究意義

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

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

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

　　以上提及的相關(guān)技術(shù)延申到機(jī)器人狀態(tài)估計(jì)的問題僅是一部分，多年來針對(duì)此類問題提出解決策略的技術(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é)選取僅僅是個(gè)人覺得對(duì)理解該類研究很有幫助的論文，并不是對(duì)上述論文做任何的質(zhì)量評(píng)價(jià)和重要性評(píng)估等主觀排名行為。

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

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

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

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

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

　　4.1 研究意義

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

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

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

　　多年來針對(duì)此類問題提出解決策略的技術(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é)選取僅僅是個(gè)人覺得對(duì)理解該類研究很有幫助的論文，并不是對(duì)上述論文做任何的質(zhì)量評(píng)價(jià)和重要性評(píng)估等主觀排名行為。

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

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

　　5、總結(jié)與延申

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

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

更多信息可以來這里獲取==>>電子技術(shù)應(yīng)用-AET<<