【技術實現(xiàn)步驟摘要】
本專利技術涉及移動機器人定位領域，特別是一種基于流形處理的視覺慣性濾波方法。
技術介紹
1、近年來，隨著科學技術的發(fā)展，定位技術的研究和應用呈現(xiàn)出迅猛發(fā)展的勢頭。但是由于單一的定位系統(tǒng)難以滿足實際定位應用的需求，綜合環(huán)境下的定位服務需要一種實用性強且穩(wěn)定性佳的定位技術。研究發(fā)現(xiàn)，視覺傳感器精度高但是穩(wěn)定性差，而慣性測量單元imu穩(wěn)定性高，但是長時間使用時容易產生累積誤差。將視覺定位和慣性定位技術進行融合能夠形成優(yōu)勢互補，逐漸發(fā)展成為導航及移動機器人定位領域的研究熱點。
2、然而在視覺慣性定位中，流形相關問題尤為關鍵，尤其是在處理旋轉、位姿以及imu數(shù)據(jù)時，傳統(tǒng)的線性空間運算已經(jīng)無法滿足精度和魯棒性的要求。由于定位中的位姿變化涉及旋轉和位移，而這些操作在流形上的表現(xiàn)往往更為復雜，計算的復雜也變的更大。需要在李群這一特殊流形結構中進行更為精確的處理，通過將狀態(tài)空間嵌入到李群中，可以有效處理旋轉和位姿的非線性問題。
技術實現(xiàn)思路
1、本專利技術的目的是提供一種基于流形處理的視覺慣性濾波方法，將狀態(tài)空間嵌入到構建的李群中，在李群這一特殊流形結構中更為精確的處理視覺慣性定位中的非線性問題，在復雜的流形結構上提高了系統(tǒng)的穩(wěn)定性與魯棒性。本專利技術是通過以下技術方案實現(xiàn)的。
2、本專利技術提供一種基于流形處理的視覺慣性濾波方法，包括：
3、s1，基于慣性imu傳感器得到機器人的位姿信息，基于流形的imu預積分方法對所述位姿信息進行積分操作得到所述位姿信息增量，
4、s2，基于更新后的位姿信息和多個地標點位置信息進行李群構建，將狀態(tài)空間嵌入到所述李群之中，得到機器人在當前時刻包含偏差信息的完整狀態(tài)；
5、s3，基于所述李群和偏差信息，構建基于ukf的不變卡爾曼濾波的當前時刻的不確定性表示、狀態(tài)模型以及測量模型；
6、s4，基于無偏輸入和所述當前時刻的狀態(tài)模型更新下一時刻的狀態(tài)模型；基于當前狀態(tài)傳播的不確定性和過程噪聲協(xié)方差矩陣更新下一時刻的狀態(tài)協(xié)方差矩陣分解因子；
7、s5，若所述下一時刻存在地標的視覺觀測數(shù)據(jù)，則基于所述測量模型計算出視覺觀測值并結合所述視覺觀測值進行該時刻的狀態(tài)模型更新和狀態(tài)協(xié)方差矩陣分解因子的更新，否則跳轉至步驟s4；
8、s6，進入再下一個時刻后，重復進行步驟s4-s5。
9、在實際應用時，本專利技術所述的算法嚴格按照上述步驟實現(xiàn)。首先利用慣性imu傳感器獲取移動機器人在當前時刻的位姿信息，即旋轉矩陣r、速度向量v和位置向量x，并利用流形的imu預積分方法對所述位姿信息進行積分操作得到所述位姿信息增量。在實際應用中結合移動機器人在當前時刻的位姿信息以及位姿信息增量，能夠計算出移動機器人在下一時刻的位姿信息。基于流形的imu預積分方法，使得imu數(shù)據(jù)的處理更加準確且簡化了計算復雜度。通過計算得到移動機器人在下一時刻更新后的位姿信息和多個地標點位置信息進行李群構建，能夠得到移動機器人在當前時刻包含偏差信息的完整狀態(tài)。通過將狀態(tài)空間嵌入到李群中，可以有效處理旋轉和位姿的非線性問題。
10、再通過構建的李群和偏差信息構建基于ukf的不變卡爾曼濾波的當前時刻的不確定性表示、狀態(tài)模型以及測量模型。基于ukf的不變卡爾曼濾波算法，能夠確保視覺慣性定位在復雜環(huán)境中的高精度表現(xiàn)。?基于無偏輸入和所述當前時刻的狀態(tài)模型更新下一時刻的狀態(tài)模型；基于當前狀態(tài)傳播的不確定性和過程噪聲協(xié)方差矩陣更新下一時刻的狀態(tài)協(xié)方差矩陣分解因子。最后在該時刻通過視覺傳感器判斷是否存在移動機器人的視覺觀測數(shù)據(jù)，若存在，則利用視覺傳感器觀測移動機器人在各個地標點上的觀測數(shù)據(jù)并結合所獲取的視覺觀測數(shù)據(jù)更新移動機器人在該時刻的狀態(tài)模型和狀態(tài)協(xié)方差矩陣分解因子，否則重復進行更新下一時刻狀態(tài)模型和狀態(tài)協(xié)方差矩陣分解因子的步驟。通過結合視覺觀測數(shù)據(jù)，利用視覺傳感器提供的地標信息，可以使得機器人的狀態(tài)估計更加精確，降低系統(tǒng)的誤差，增強了系統(tǒng)應對復雜和不確定環(huán)境的能力。
11、可選的，在步驟s1中，所述位姿信息包括旋轉矩陣、速度向量和位置向量；所述基于流形的imu預積分方法對所述位姿信息進行積分操作得到所述位姿信息增量，具體通過以下公式獲得：
12、，
13、式中，分別表示機器人從時刻 i到時刻 j的旋轉增量、速度增量和位置增量，和分別表示機器人在時刻 i到時刻 j的旋轉矩陣，和分別表示機器人在時刻 i到時刻 j的速度向量，和分別表示機器人在時刻 i到時刻 j的位置向量；表示從機器人在時刻 i到時刻 j的旋轉噪聲，即角度誤差，為零均值高斯噪聲；表示使用指數(shù)映射（exp），將角度誤差從李代數(shù)映射回到李群，分別表示機器人從時刻 i到時刻 j的速度噪聲和位置噪聲，均為零均值高斯噪聲，表示從時刻 i到時刻 j的時間間隔，表示重力向量。
14、可選的，在步驟s1中，所述更新后的位姿信息包括更新后的旋轉矩陣、速度向量和位置向量；
15、在步驟s2中，所述基于更新后的位姿信息和多個地標點位置信息進行李群構建包括通過更新后的旋轉矩陣、速度向量、位置向量以及多個地標點的位置構建李群，所述李群表示當前時刻n機器人的狀態(tài)向量，所述李群的表達式如下：
16、，
17、式中，表示一個的零矩陣，表示一個的單位矩陣， m表示機器人的位置信息， p表示所觀測到的地標點的數(shù)量；
18、在步驟s2中，所述得到機器人在當前時刻包含偏差信息的完整狀態(tài)包括：機器人在當前時刻n包含偏差信息的完整狀態(tài)表示為，其中是慣性imu傳感器的偏差向量，表示為：
19、，
20、式中，是陀螺儀的偏差，是加速度計的偏差，兩者組合為一個偏差向量。
21、可選的，在步驟s3中，所述不確定性表示包括狀態(tài)的不確定性表示和偏差的不確定性表示，所述狀態(tài)的不確定性表示通過狀態(tài)誤差表示，所述偏差的不確定性表示通過偏差誤差表示，所述狀態(tài)誤差和所述偏差誤差服從均值為0、協(xié)方差為的高斯分布，表示為：
22、，
23、狀態(tài)協(xié)方差矩陣描述了狀態(tài)誤差和偏差誤差的不確定性大小，狀態(tài)協(xié)方差矩陣進行cholesky分解能夠得到cholesky因子，分解過程如下：，
24、式中，s為狀態(tài)協(xié)方差矩陣進行cholesky分解得到的cholesky因子，所述cholesky本文檔來自技高網(wǎng)...
【技術保護點】
1.一種基于流形處理的視覺慣性濾波方法，其特征是，包括：
2.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，在步驟S1中，所述位姿信息包括旋轉矩陣、速度向量和位置向量；所述基于流形的IMU預積分方法對所述位姿信息進行積分操作得到所述位姿信息增量，具體通過以下公式獲得：
3.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，
4.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，在步驟S3中，所述不確定性表示包括狀態(tài)的不確定性表示和偏差的不確定性表示，所述狀態(tài)的不確定性表示通過狀態(tài)誤差表示，所述偏差的不確定性表示通過偏差誤差表示，所述狀態(tài)誤差和所述偏差誤差服從均值為0、協(xié)方差為的高斯分布，表示為：
5.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，機器人在當前時刻n的狀態(tài)向量的運算如下：
6.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，在步驟S3中，所述測量模型表達式為：
7.根據(jù)權利要求5所述的視覺慣性濾波方法，其特征是，在步驟S4中，所述基于無偏輸入和所述當前時刻的狀態(tài)模型更新下一時刻的狀態(tài)模型，具體包
8.根據(jù)權利要求7所述的視覺慣性濾波方法，其特征是，在步驟S4中，所述基于當前狀態(tài)傳播的不確定性和過程噪聲協(xié)方差矩陣更新下一時刻的狀態(tài)協(xié)方差矩陣分解因子，包括：
9.根據(jù)權利要求8所述的視覺慣性濾波方法，其特征是，若所述下一時刻存在地標的視覺觀測數(shù)據(jù)，則基于所述測量模型計算出視覺觀測值并結合所述視覺觀測值進行該時刻的狀態(tài)模型更新和狀態(tài)協(xié)方差矩陣分解因子的更新，包括：
...【技術特征摘要】
1.一種基于流形處理的視覺慣性濾波方法，其特征是，包括：
2.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，在步驟s1中，所述位姿信息包括旋轉矩陣、速度向量和位置向量；所述基于流形的imu預積分方法對所述位姿信息進行積分操作得到所述位姿信息增量，具體通過以下公式獲得：
3.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，
4.根據(jù)權利要求1所述的視覺慣性濾波方法，其特征是，在步驟s3中，所述不確定性表示包括狀態(tài)的不確定性表示和偏差的不確定性表示，所述狀態(tài)的不確定性表示通過狀態(tài)誤差表示，所述偏差的不確定性表示通過偏差誤差表示，所述狀態(tài)誤差和所述偏差誤差服從均值為0、協(xié)方差為的高斯分布，表示為：
5.根據(jù)權利要求1所述的視覺慣性濾波方法，其特...
【專利技術屬性】
技術研發(fā)人員：陸音，牛碩，陸浥辰，馬子賢，陳昕瑤，王澤恒，黃一凡，朱曉榮，朱斌，袁源，
申請(專利權)人：南京郵電大學，
類型：發(fā)明
國別省市：
還沒有人留言評論。發(fā)表了對其他瀏覽者有用的留言會獲得科技券。