[發明專利]一種多傳感器融合定位系統的位姿優化方法和裝置

在審

• 申請號: 202011060481.7
• 申請日: 2020-09-30
• 公開（公告）號: CN114322996A
• 公開（公告）日: 2022-04-12
• 發明（設計）人: 韓冰;張濤;邊威;黃帥
• 申請（專利權）人: 阿里巴巴集團控股有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 北京思格頌知識產權代理有限公司 11635
• 代理人: 楊超

本發明公開了一種多傳感器融合定位系統的位姿優化方法和裝置。所述方法包括：將獲取到的預設時間段內的視覺傳感器拍攝的圖像數據與慣導測量數據融合，得到相對世界坐標系下的第一位姿；基于述第一位姿和全球定位系統在絕對世界坐標系下的觀測位置和觀測航向，對第一位姿轉換到絕對世界坐標系下的第二位姿和相對世界坐標系與絕對世界坐標系間的旋轉外參進行迭代優化，得到第二位姿的優化值和旋轉外參的優化值，旋轉外參的優化值用于下一時間段內的視覺傳感器拍攝的圖像數據與慣導測量數據融合。能夠快速準確的對多傳感器融合定位系統中的位姿進行優化。

技術領域

本發明涉及高精定位技術領域，特別涉及一種多傳感器融合定位系統的位姿優化方法和裝置。

背景技術

多傳感器融合定位系統一般是指包括視覺傳感器、慣性測量單元(InertialMeasurement Unit，IMU，簡稱“慣導”)和全球導航衛星系統((Global NavigationSatellite System，GNSS)等的定位系統，兼具定位精度高和成本低的優勢，所以多傳感器融合是高精度定位的重要選擇。而通過多傳感器融合提供全場景、高精度和多源融合的精準定位信息過程中，各傳感器位置和姿態的優化精度對后續定位的精度起到至關重要的作用。

多傳感器融合定位系統的位姿優化的現有技術一般包括：

(1)VINS_MONO算法

僅加入回環檢測的累積誤差校正，存在定位誤差累計漂移的問題。

(2)VI-ORB視覺慣導初始化算法

在位姿圖優化Pose Graph的基礎上進行局部的Bundle Adjustment，首先未加入全球定位系統的絕對位置信息，且如果加入絕對信息，需要足夠的窗口大小，此時進行Bundle Adjustment優化，會消耗過大的計算資源。

(3)傳統的Pose Graph方案

僅優化相對位姿信息和全球定位系統的絕對位置信息，世界坐標系旋轉矩陣是通過最新幀恢復的，當最新幀的融合位姿或者原始觀測值不準時，容易導致世界坐標系旋轉矩陣不準，進而導致優化不穩定；且通過不準確的世界系旋轉矩陣解算的初始地圖位姿也會不準，容易使算法陷入局部優化。

綜上所述，現有技術中多傳感器融合定位系統的位姿優化存在誤差累計、局部優化等精度低和優化耗時長的問題。

發明內容

鑒于上述問題，提出了本發明以便提供一種克服上述問題或者至少部分地解決上述問題的一種多傳感器融合定位系統的位姿優化方法和裝置。

第一方面，本發明實施例提供一種多傳感器融合定位系統的位姿優化方法，包括：

將獲取到的預設時間段內的視覺傳感器拍攝的圖像數據與慣導測量數據融合，得到相對世界坐標系下的第一位姿；

基于所述第一位姿和全球定位系統在絕對世界坐標系下的觀測位置和觀測航向，對所述第一位姿轉換到絕對世界坐標系下的第二位姿和相對世界坐標系與絕對世界坐標系間的旋轉外參進行迭代優化，得到所述第二位姿的優化值和旋轉外參的優化值，所述旋轉外參的優化值用于下一時間段內的視覺傳感器拍攝的圖像數據與慣導測量數據融合。

在一些可選的實施例中，所述對所述第一位姿轉換到絕對世界坐標系下的第二位姿和相對世界坐標系與絕對世界坐標系間的旋轉外參進行迭代優化，得到所述第二位姿的優化值和旋轉外參的優化值，具體包括：

迭代優化所述第二位姿中的第二位置和第二航向角及所述旋轉外參，得到所述第二位置的優化值、第二航向角的優化值和旋轉外參的優化值。

在一些可選的實施例中，所述基于所述第一位姿和全球定位系統在絕對世界坐標系下的觀測位置和觀測航向，對所述第一位姿轉換到絕對世界坐標系下的第二位姿和相對世界坐標系與絕對世界坐標系間的旋轉外參進行迭代優化，具體包括：