前言

今天再次仔細讀了UVIO: An UWB-Aided Visual-Inertial Odometry Framework with Bias-Compensated Anchors Initialization這篇論文，這是23年8月份的論文，researchgate里說發(fā)表在2023IROS上，主要工作是修改openvins代碼利用超寬帶技術(shù)和視覺慣性里程計提供魯棒低漂移定位，提出了一種多步初始化程序。

一、問題背景

衛(wèi)星拒止環(huán)境下定位問題，UWB的引入解決VIO長期累積漂移的問題，對于UWB來說多個未知錨點位置確定是重要的初始化問題。UWB錨節(jié)點的初始化是通過求解線性最小二乘問題一步完成的，這會導(dǎo)致不準(zhǔn)確或完全錯誤的初始化結(jié)果。迄今為止的所有研究都顯示了將UWB技術(shù)用于定位的潛力，特別是在VIO導(dǎo)航系統(tǒng)中抑制漂移的潛力。然而，上述研究均未考慮錨節(jié)點初始化的求解高度依賴于各種因素，包括錨節(jié)點部署的幾何形狀以及收集初始化數(shù)據(jù)的無人機或移動機器人的運動軌跡，進而影響狀態(tài)估計的精度。

過去在UWB錨點初始化上的工作成果如下所示：

在文獻[ 11 ]中，作者提出了一種基于擴展卡爾曼濾波( EKF )的多傳感器融合框架來聯(lián)合估計錨和無人機的位置。UWB錨節(jié)點的初始化是通過求解線性最小二乘問題一步完成的，這會導(dǎo)致不準(zhǔn)確或完全錯誤的初始化結(jié)果。

[ 12 ]提出了一種利用低成本IMU的測距測量和讀數(shù)進行UWB錨自定位的精確且易于使用的方法。通過自由移動標(biāo)簽來估計錨點的位置，使用緊耦合的誤差狀態(tài)卡爾曼濾波器( ESKF )來融合UWB和慣性測量。在不考慮測量偏差的情況下，首先通過迭代最小二乘法初始化錨點。

在文獻[ 13 ]中，提出了一種基于主動錨節(jié)點的超寬帶定位系統(tǒng)的自校準(zhǔn)算法。該算法使用迭代梯度下降和卷積神經(jīng)網(wǎng)絡(luò)的誤差檢測來估計固定錨節(jié)點的位置，即使在非視距( NLOS )條件下也具有較高的精度，但需要錨節(jié)點之間交換距離信息。

在[ 14 ]中的研究通過將單個UWB錨點納入定位過程，解決了VIO在室內(nèi)定位中的漂移問題。錨點的初始化是通過求解一個非線性最小二乘問題來完成的，該問題需要對必須提供的錨點位置進行初始猜測，并且先驗已知

Nguyen等人在[ 15 ]和[ 16 ]中的研究提出了兩種緊耦合的里程計框架，將單目視覺特征觀測與單個UWB錨點提供的距離測量相結(jié)合。第一種框架使用利文貝格-馬夸特非線性優(yōu)化算法的變體來同時估計尺度因子和錨點位置，而第二種框架通過解決每個距離數(shù)據(jù)的時間偏移并利用所有可用的測量值來更有效地利用UWB測量值。隨后，在[ 17 ]和[ 18 ]中，作者提出了一種基于圖優(yōu)化的無人機多傳感器同時定位與地圖構(gòu)建( Simultaneous Localization and Mapping，SLAM )方法。該系統(tǒng)融合了多種傳感器的數(shù)據(jù)，包括IMU，相機，激光雷達和UWB距離測量來進行估計。

最近，Jia等人[ 19 ]提出了一種視覺慣性測距( VIRO )方法，通過結(jié)合至少3個錨點的UWB測距測量來減少VIO系統(tǒng)中的定位漂移。該方法利用長短時間窗口結(jié)構(gòu)來初始化錨點的位置和協(xié)方差。論文還分析了UWB錨節(jié)點位置未知時VIRO系統(tǒng)的可觀測性，指出理想情況下存在4個不可觀測方向，并通過多狀態(tài)約束卡爾曼濾波( Multi-State Constraint Kalman Filter，MSCKF )融合UWB量測數(shù)據(jù)降低了不可觀測方向。

在最近的一項研究[ 20 ]中，作者應(yīng)用FIM的原理來確定指定航班量內(nèi)的最優(yōu)點，以收集用于初始化單個UWB錨的位置及其測量偏差的數(shù)據(jù)。該方法包括使用隨機車輛位置的粗三角剖分，然后使用FIM優(yōu)化的點進行精細三角剖分，為單個錨提供最大的信息。

[11] K. Hausman, S. Weiss, R. Brockers, L. Matthies, and G. S. Sukhatme, “Self-calibrating multi-sensor fusion with probabilistic measurement validation for seamless sensor switching on a uav,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), 2016, pp. 4289–4296.

[12] Q. Shi, S. Zhao, X. Cui, M. Lu, and M. Jia, “Anchor self-localization algorithm based on uwb ranging and inertial measurements,” Tsinghua Science and Technology, vol. 24, no. 6, pp. 728–737, 2019.

[13] M. Ridolfi, J. Fontaine, B. Van Herbruggen, W. Joseph, J. Hoebeke, and E. De Poorter, “Uwb anchor nodes self-calibration in nlos conditions: a machine learning and adaptive phy error correction approach,” Wireless Networks, vol. 27, 05 2021.

[14] B. Gao, B. Lian, D. Wang, and C. Tang, “Low drift visual inertial odometry with uwb aided for indoor localization,” IET Communications, vol. 16, no. 10, pp. 1083–1093, 2022.

[15] T. H. Nguyen, T.-M. Nguyen, and L. Xie, “Tightly-coupled singleanchor ultra-wideband-aided monocular visual odometry system,” in 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 665–671.

[16] ——, “Range-focused fusion of camera-imu-uwb for accurate and drift-reduced localization,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1678–1685, 2021.

[17] T.-M. Nguyen, S. Yuan, M. Cao, T. H. Nguyen, and L. Xie, “Viral slam: Tightly coupled camera-imu-uwb-lidar slam,” 2021.

[18] T.-M. Nguyen, M. Cao, S. Yuan, Y. Lyu, T. H. Nguyen, and L. Xie, “Viral-fusion: A visual-inertial-ranging-lidar sensor fusion approach,” IEEE Transactions on Robotics, vol. 38, no. 2, pp. 958–977, 2022.

[19] S. Jia, Y. Jiao, Z. Zhang, R. Xiong, and Y. Wang, “Fej-viro: A consistent first-estimate jacobian visual-inertial-ranging odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022, pp. 1336–1343.

[20] J. Blueml, A. Fornasier, and S. Weiss, “Bias compensated uwb anchor initialization using information-theoretic supported triangulation points,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), 2021, pp. 5490–5496.

二、主要假設(shè)

固定靜止錨點假設(shè)。

假設(shè)機器人的姿態(tài)(通過VIO)，以及UWB的距離測量值對系統(tǒng)是可用的。

三、理論流程

1、UWB輔助的視覺慣性里程計濾波器設(shè)計

（1）狀態(tài)向量

狀態(tài)向量如下

依次為

a、IMU坐標(biāo)系在全局坐標(biāo)系中的位置速度方向以及零偏

b、復(fù)制的不同時刻的IMU位姿

c、包括N個特征在全局框架中的位置

d、包括每個相機presen的相機內(nèi)參和相機外參

e、包括UWB外參數(shù)，即UWB標(biāo)簽相對于IMU框架的位置，M個錨點在全局框架中的位置，以及每個錨點的偏置參數(shù)。

（2）錨點固定策略

在所提出的濾波器中，每個UWB錨點可以視為固定的，即它們的值保持不變，或者可以在線修正，從而添加到UWB狀態(tài)中。通過固定兩個錨點，我們可以設(shè)置一個無漂移的全局框架(事實上,非線性可觀測性分析表明,將一個錨固定在位置上,朝向另一個錨就足夠了;為了簡化結(jié)果部分的實現(xiàn),將2個錨固定在位置上)。

（3）UWB測距模型與雅可比

UWB測距模型：

分別是一個乘性標(biāo)量，充當(dāng)一個距離依賴的偏置項，和一個常數(shù)偏置項。

距離測量的雅可比為：

（4）時間對齊

與[ 19 ]不同的是，我們沒有插值距離測量以使其與相機測量對齊，但是，為了避免插值引入額外的誤差源，我們執(zhí)行了我們稱之為延遲更新的方法。如圖所示，這種更新策略包括在時間tn的最近一次更新和時間tk的下一次相機圖像之間收集距離測量。在tk時刻接收到新的相機圖像后，進行一系列從tn時刻到tk時刻的鏈?zhǔn)絺鞑ジ隆?/p>

2、UWB錨點初始化

首先，從一個短的隨機或標(biāo)準(zhǔn)軌跡中收集數(shù)據(jù)，通過求解線性最小二乘問題得到一個粗略的初始解。隨后，在相同數(shù)據(jù)上建立一個非線性優(yōu)化問題，并對其進行求解，以細化第一層粗定位。然后，利用得到的錨點位置估計計算一組路點，優(yōu)化我們提出的GDOP度量，以最大化所有參與的UWB錨點的三角定位信息。然后將這些最優(yōu)航路點的數(shù)據(jù)用于一個非線性優(yōu)化問題，以同時優(yōu)化所有UWB錨點的位置。

（1）粗錨定位：最優(yōu)雙法

最小二乘建模實際中經(jīng)常為病態(tài)的，在這方面，[ 11 ]介紹了一個測量包含檢驗，其中測量被添加到最小二乘問題中只有當(dāng)問題的條件數(shù)通過添加這樣的測量而減少。在這項工作中，我們用所謂的最優(yōu)二重方法重新表述了[ 20 ]中描述的最小二乘問題，旨在增加最小二乘問題的條件數(shù)和最小化解的不確定性。

我們對錨點粗初始化問題的最小二乘形式的條件化進行改進的第一種選擇是在測量模型中考慮一個已知的β = 1。因此，對于錨點A的距離測量z，有

在忽略測量噪聲的情況下，我們可以將之前的方程平方化，并將其改寫為如下形式：

應(yīng)用雙方法[ 21 ]，可以得到關(guān)于Gp A和γ的線性關(guān)系。更準(zhǔn)確地說，考慮zi和zj是來自同一錨點A的兩個不同的(不一定連續(xù))范圍測量，而GpU i和GpU j是采集測量時的UWB標(biāo)簽位置。將兩式相減可得：

最小二乘建模為：

為了識別對zi和zj，應(yīng)該確定一個從任何其他測量中減去的最佳樞軸測量，以使解決方案的不確定性最小化。為此，令nk?N 0，σ2n，為與錨點A的第k個距離測量相關(guān)的噪聲，εk?N ( 0 , Σ ε)為與UWB標(biāo)簽的第k個位置相關(guān)的噪聲。在接下來的分析中，我們只考慮向量b是一個隨機實體，而將矩陣A視為確定性的。盡管這是一個近似，但它使分析變得更容易，從而為最佳支點的選擇提供了一個有意義的標(biāo)準(zhǔn)。

最小二乘的信息矩陣為：

（2）非線性細化

我們的錨點初始化程序的下一步是基于利文貝格-馬夸特( LM )算法的第一個粗解的非線性精化，在第一個隨機軌跡期間收集的相同數(shù)據(jù)上執(zhí)行。特別地，我們尋求最小化下面的成本函數(shù)：

與線性求解只估計每個錨點的常值偏差γ不同，在非線性優(yōu)化問題中，我們包含了每個錨點的距離依賴偏差β。距離偏差的初始猜測值設(shè)定為1。

最終解的估計參數(shù)協(xié)方差矩陣為：

（3）最優(yōu)航路點

盡管上面介紹了最優(yōu)的雙重方法，但使用在隨機軌跡期間收集的測距測量來估計UWB錨點的位置具有明顯的局限性。錨點初始化的軌跡關(guān)于信息的最優(yōu)性可以基于不同的測度進行評估。在[ 20 ]中，錨的位置的第一個估計是通過收集通過一組通過Fisher信息矩陣最大化計算的航路點的數(shù)據(jù)來精化的。在這項研究中，GDOP [ 21 ]被用作同時估計多個錨點位置的最優(yōu)性度量。最小化UWB映射不確定性的軌跡合成如下。令H為非線性測距量余弦方向的雅克比

分別為航路點位置相對于錨點的方位角和俯仰角。

現(xiàn)在考慮至少四個目標(biāo)位置，我們可以計算如下的GDOP：

最大化式( 8 )等價于最小化估計不確定橢球的體積，該體積僅是航路點相對于UWB錨點位置的方位角和俯仰角的函數(shù)，因此將其作為優(yōu)化參數(shù)。盡管如此，還是要考慮多重錨。此外，對于航路點的操作搜索卷可以有任意形狀的( UWB錨的位置可能在無人機的業(yè)務(wù)量之外)。因此，航路點的確定成為一個具有挑戰(zhàn)性的優(yōu)化問題，特別是在非結(jié)構(gòu)化環(huán)境中。此外，與[ 20 ]中對每個錨點單獨優(yōu)化多條軌跡不同，我們感興趣的是生成一條同時最小化所有UWB錨點估計精度的最優(yōu)軌跡。為此，我們不考慮角度，而是直接將航路點位置作為下面優(yōu)化問題的參數(shù)：

為了解決這個問題，我們使用[ 23 ]中應(yīng)用的基于網(wǎng)格的進化算法。該算法首先將無人機的操作體積看作一個高度為h，寬度為w，長度為l的正方體，np個初始航路點(即q ( 1 , -)，..，q( np , -) ) (見圖2)。立方體體積被劃分為np = nhp × nlp × nwp小立方體，其中nhp、nlp和nwp分別表示沿Z (高度)、X (長度)和Y (寬度)軸的坐標(biāo)變化數(shù)。每個小立方體是一個3D網(wǎng)格，用G1 ..表示。Gnp，它定義了對應(yīng)路點的所有可能位置。進化Alg . 1將每條染色體定義為一個3 × np的矩陣，該矩陣中的基因沿樣本格點的3個坐標(biāo)軸表示對應(yīng)的航路點。每條染色體表示離散空間(見圖3)中路點集合的一個特定配置。

[23] S. Yang, M. Li, X. Liu, and J. Zheng, “A grid-based evolutionary algorithm for many-objective optimization,” IEEE Transactions on Evolutionary Computation, vol. 17, no. 5, pp. 721–736, 2013.

在此，我們報告了應(yīng)用基于網(wǎng)格的進化算法的結(jié)果。如下圖所示。

在獲得最優(yōu)航路點集合后，無人機可以飛過這些航路點并收集數(shù)據(jù)，然后在Sec中解釋的非線性求精中再次使用這些數(shù)據(jù)。在最后一步中，我們最好利用前面計算出的最優(yōu)航路點所包含的信息，這樣就可以同時對距離內(nèi)的所有UWB錨點進行精確初始化。

四、實驗方案

實驗部分在上一節(jié)最有航路點中有一些模擬仿真實驗，此外還有如下。

1、仿真實驗

在MATLAB中對初始化過程進行了仿真和廣泛的測試，并對不同錨配置、無人機軌跡、參數(shù)值和噪聲水平的多個場景進行了全面的探索。特別地，在圖6中，我們比較了以下每一步初始化后的解：初始粗最小二乘解( LS )，相同初始化數(shù)據(jù)上的非線性最小二乘解( NLS )，通過隨機路徑點對非線性最小二乘解進行精化( Rnd-WPS )，以及通過最優(yōu)路徑點對非線性最小二乘解進行精化( Opt-WPS )。需要強調(diào)的是，相對于前兩種方法，隨機和最優(yōu)航路點精化是在"新"數(shù)據(jù)上進行的。為了比較的公平性，我們考慮所有方法的樣本量相同。在所有情況下，與其他測試方法相比，通過最優(yōu)路徑點的優(yōu)化過程表現(xiàn)出優(yōu)異的性能，顯著改善了解的方差和平均誤差的最小化。這一結(jié)果強調(diào)了結(jié)合信息論支持的基于優(yōu)化的方法來改進錨映射的初始解的有效性，即使在具有挑戰(zhàn)性的錨配置的情況下，也提高了初始化過程的可靠性。

2、真實場景實驗

擴展OpenVINS編寫UVIO。采用文獻[ 24 ]中介紹的系統(tǒng)自治框架，使無人機能夠在給定的飛行體積內(nèi)通過航路點自主飛行。

對于第一組實驗，我們將無人機部署在飛行體積的中心，并執(zhí)行Sec描述的初始化過程。Ⅳ.為了評估所提出方法的準(zhǔn)確性，我們使用無人機的地面真實位置來執(zhí)行初始化。我們對錨點進行了兩種配置下的3次實驗，對應(yīng)的位置誤差如表所示。與仿真實驗相比，誤差的增加是由UWB測量模型的非理想性解釋的，該模型在初始化階段假設(shè)偏差為常值，而在現(xiàn)實中干擾、反射和其他不需要的物理現(xiàn)象可能會改變它們的值。此外，實驗結(jié)果表明，所提出的方法的準(zhǔn)確性受錨相對于飛行體積的位置的影響。特別地，位于體積之外和角落的錨點會導(dǎo)致更高的初始化誤差。另一方面，放置在有利位置的錨點，例如靠近飛行體積的中心，會產(chǎn)生更低的初始化誤差。與文獻[ 20 ]的結(jié)果比較表明，所提出的方法是優(yōu)越的，因為它同時考慮了多個錨點，并且能夠?qū)蝹€錨點實現(xiàn)可比的甚至更低的初始化誤差，例如圖7中的錨點19333。

為了評估UVIO的實時性能，進行了第二組實驗。此時，初始化是考慮VIO框架估計的位姿進行的，并隨著無人機在位懸停而停止。隨后進行了持續(xù)時間大致相同的手動飛行，與通過航路點的自動飛行相比，允許更靈活的機動。對于軌跡的評估，我們選擇了絕對軌跡誤差( ATE )作為評估UVIO性能的指標(biāo)[ 25 ]。在所有的實驗中，我們注意到使用UVIO比只使用VIO在定位性能上有整體的提高。這體現(xiàn)在ATE均方根誤差( RMSE )的降低(圖8 )。為了進一步測試和驗證我們的框架的可靠性，與純VIO相比，我們模擬了嚴(yán)重的幀率下降和相機的完全丟失(圖9 )。這些實驗的結(jié)果證明了UVIO在視覺條件差或相機故障的情況下也是有效的，在這些情況下，純VIO會失效。

五、創(chuàng)新總結(jié)

我們的方法擴展和改進了[ 20 ]中關(guān)于信息驅(qū)動的UWB錨初始化和使用的想法，通過開發(fā)一種完全自動化的初始化方法來同時初始化和隨后使用具有測量偏差的多個初始未知的UWB錨。我們利用GDOP作為一種新的度量標(biāo)準(zhǔn)來指導(dǎo)可擴展UWB錨點集的數(shù)據(jù)收集、初始化和不確定性降低的優(yōu)化。更確切地說，我們的貢獻是：

在視覺- UWB支持的估計方法中，完全自動化地檢測、初始化和使用初始未知的UWB錨點。

新的最小二乘公式，被稱為最佳雙方法( Sec。Ⅳ- A)，增加了矩陣條件數(shù)，以改進粗錨點初始化。

基于GDOP度量在任意形狀的區(qū)域中生成最佳航路點，以實現(xiàn)精確和同時初始化多個UWB錨點的最佳數(shù)據(jù)收集。

通過一個自主飛行的無人機(和代碼的開源)，在仿真和真實實驗中驗證了所提方法的有效性。

在真實世界中驗證了我們的方法對相機丟失的彈性。

這篇論文解決的也是UWB-VIO系統(tǒng)的初始化問題，重點在精準(zhǔn)估計UWB錨點上，隨機的軌跡進行UWB錨點估計之前很多成果分析過觀測性問題，最優(yōu)化出一條精準(zhǔn)軌跡可以避免這個問題并且提高估計的精度。本文沒有卷UWB-VIO的定位精度，采用濾波方法進行后端優(yōu)化更注重效率，重點是本文提出的未知錨點初始化方法是創(chuàng)新之處，是核心的地方。文章來源地址http://www.zghlxwxcb.cn/news/detail-756775.html

到了這里，關(guān)于論文閱讀：UVIO: An UWB-Aided Visual-Inertial Odometry Framework with Bias-Compensated Anchors Initial的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！