[發明專利]基于多模態融合的定位方法及系統

有效

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

1.一種基于多模態融合的定位方法，其特征在于，包括：

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

獲取地磁方向和所述目標空間的變化矩陣，根據所述第一姿態矩陣和所述變化矩陣，獲取第二姿態矩陣；

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

所述當所述待追蹤物體的表面與所述目標空間的表面接觸時，根據所述待追蹤物體和所述目標空間之間的二維交互平面位置關系，獲取所述待追蹤物體和所述目標空間之間的二維交互平面基準定位信息，并根據所述二維交互平面基準定位信息，獲取所述待追蹤物體在所述目標空間的位置信息和第三姿態矩陣，具體包括：

在待追蹤物體底面或側面與目標空間內的平面之間部署二維平面基準定位，獲取待追蹤物體表面相對于所處平面的位置坐標及旋轉角，其中，位置坐標為二維位置坐標，旋轉角為二維旋轉角；當待追蹤物體與平面接觸時，根據該平面原點的空間位置和法線方向，計算出待追蹤物體在三維空間的位置信息P及姿態矩陣N。

2.根據權利要求1所述的基于多模態融合的定位方法，其特征在于，在所述根據所述二維交互平面基準定位信息，獲取所述待追蹤物體在所述目標空間的位置信息和第三姿態矩陣，以根據所述位置信息對所述待追蹤物體進行三維定位，根據所述第三姿態矩陣對所述待追蹤物體進行姿態傳感之后，所述方法還包括：

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

3.根據權利要求2所述的基于多模態融合的定位方法，其特征在于，所述獲取加速度信息，對所述加速度信息進行處理，得到所述待追蹤物體當前的空間位置，包括：

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

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

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

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

4.根據權利要求3所述的基于多模態融合的定位方法，其特征在于，所述方法還包括：

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

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

5.根據權利要求1所述的基于多模態融合的定位方法，其特征在于，所述方法還包括：

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

根據所述目標變換矩陣和所述第一姿態矩陣，得到第二姿態矩陣。

6.根據權利要求1所述的基于多模態融合的定位方法，其特征在于，所述方法還包括：

在所述待追蹤物體上設置慣性測量單元，并在所述待追蹤物體的表面和所述目標空間的表面設置光學標簽、電容標簽或NFC標簽。