[發(fā)明專利]基于多模態(tài)融合的定位方法及系統(tǒng)

有效

• 申請?zhí)枺?/td>: 202011530551.0
• 申請日: 2020-12-22
• 公開（公告）號: CN112729285B
• 公開（公告）日: 2023-03-10
• 發(fā)明（設計）人: 王濛;朱雅鑫;孫喆;徐迎慶
• 申請（專利權）人: 清華大學
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 北京路浩知識產權代理有限公司 11002
• 代理人: 楊明月

本發(fā)明提供一種基于多模態(tài)融合的定位方法及系統(tǒng)，該方法包括：基于地磁坐標系，獲取目標空間內待追蹤物體的第一姿態(tài)矩陣；獲取地磁方向和目標空間的變化矩陣，根據(jù)第一姿態(tài)矩陣和變化矩陣，獲取第二姿態(tài)矩陣；當待追蹤物體的表面與目標空間的表面接觸時，根據(jù)待追蹤物體和目標空間之間的二維交互平面位置關系，獲取待追蹤物體和目標空間之間的二維交互平面基準定位信息，并根據(jù)二維交互平面基準定位信息，獲取待追蹤物體在目標空間的位置信息和第三姿態(tài)矩陣，以根據(jù)位置信息對待追蹤物體進行三維定位，根據(jù)第三姿態(tài)矩陣對待追蹤物體進行姿態(tài)傳感。本發(fā)明實現(xiàn)對實體空間內的多物體高精度實時追蹤。

技術領域

本發(fā)明涉及空間定位技術領域，尤其涉及一種基于多模態(tài)融合的定位方法及系統(tǒng)。

背景技術

傳統(tǒng)高精度三維空間定位，需要多個射頻或視頻基站進行全局位置傳感，布置復雜且價格高昂；而基于IMU等設備的相對位置傳感則精度有限，且存在極高的積累誤差。

因此，現(xiàn)在亟需一種基于多模態(tài)融合的定位方法及系統(tǒng)來解決上述問題。

發(fā)明內容

針對現(xiàn)有技術存在的問題，本發(fā)明提供一種基于多模態(tài)融合的定位方法及系統(tǒng)。

本發(fā)明提供一種基于多模態(tài)融合的定位方法，包括：

基于地磁坐標系，獲取目標空間內待追蹤物體的第一姿態(tài)矩陣；

獲取地磁方向和所述目標空間的變化矩陣，根據(jù)所述第一姿態(tài)矩陣和所述變化矩陣，獲取第二姿態(tài)矩陣；

當所述待追蹤物體的表面與所述目標空間的表面接觸時，根據(jù)所述待追蹤物體和所述目標空間之間的二維交互平面位置關系，獲取所述待追蹤物體和所述目標空間之間的二維交互平面基準定位信息，并根據(jù)所述二維交互平面基準定位信息，獲取所述待追蹤物體在所述目標空間的位置信息和第三姿態(tài)矩陣，以根據(jù)所述位置信息對所述待追蹤物體進行三維定位，根據(jù)所述第三姿態(tài)矩陣對所述待追蹤物體進行姿態(tài)傳感。

根據(jù)本發(fā)明提供的一種基于多模態(tài)融合的定位方法，在所述根據(jù)所述二維交互平面基準定位信息，獲取所述待追蹤物體在所述目標空間的位置信息和第三姿態(tài)矩陣，以根據(jù)所述位置信息對所述待追蹤物體進行三維定位，根據(jù)所述第三姿態(tài)矩陣對所述待追蹤物體進行姿態(tài)傳感之后，所述方法還包括：

當所述待追蹤物體的位置發(fā)生變化時，獲取加速度信息，對所述加速度信息進行處理，得到所述待追蹤物體當前的空間位置。

根據(jù)本發(fā)明提供的一種基于多模態(tài)融合的定位方法，所述獲取加速度信息，對所述加速度信息進行處理，得到所述待追蹤物體當前的空間位置，包括：

獲取待追蹤物體的加速度信息，當所述待追蹤物體的表面與所述目標空間的表面不接觸時，對所述第二姿態(tài)矩陣進行更新，得到第四姿態(tài)矩陣，并根據(jù)所述第四姿態(tài)矩陣對所述待追蹤物體進行姿態(tài)傳感；

通過所述第四姿態(tài)矩陣，對所述加速度信息進行轉換，得到所述目標空間對應空間坐標下的三軸加速度信息；

對所述三軸加速度信息進行濾波積分處理，得到所述待追蹤物體在所述目標空間內的位移信息；

根據(jù)所述位移信息和所述位置信息，得到所述待追蹤物體當前的空間位置。

根據(jù)本發(fā)明提供的一種基于多模態(tài)融合的定位方法，所述方法還包括：

當所述待追蹤物體的表面再次與所述目標空間的表面接觸時，停止對所述三軸加速度信息進行濾波積分處理，并得到濾波積分處理生成的位置誤差；

通過所述位置誤差，對所述待追蹤物體在所述目標空間中新的位置信息進行修正，得到修正后的位置信息。

根據(jù)本發(fā)明提供的一種基于多模態(tài)融合的定位方法，所述方法還包括：

獲取所述第一姿態(tài)矩陣和所述第三姿態(tài)矩陣之間的目標變換矩陣；