[發明專利]一種基于預積分理論的慣性/輪速里程計融合定位與參數優化方法

有效

• 申請號: 202011517160.5
• 申請日: 2020-12-21
• 公開（公告）號: CN112697142B
• 公開（公告）日: 2023-03-10
• 發明（設計）人: 白師宇;賴際舟;呂品;王炳清;岑益挺;孫鑫
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/34
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 顏盈靜

本發明公開了一種基于預積分理論的慣性/輪速里程計融合定位與參數優化方法，包括以下步驟：步驟1：周期讀取t時刻陀螺輸出、加速度計輸出和輪速里程計輸出；步驟2：計算慣性/輪速里程計預積分量測、雅克比矩陣和協方差矩陣：步驟3：基于協方差矩陣，構建慣性/輪速里程計融合的目標函數，利用圖優化算法對載體位置、速度、姿態、陀螺零偏、加速度計零偏、輪速里程計標度因數、輪速里程計與慣性傳感器外參進行估計，得到優化結果：步驟4：利用優化結果和雅克比矩陣，對步驟2計算得到的慣性/輪速里程計預積分量測進行校正。

技術領域

本發明屬于自主導航技術領域，具體涉及一種基于預積分理論的慣性/輪速里程計融合定位與參數優化方法。

背景技術

慣性/輪速里程計融合是一種常見的組合導航方式，其都是利用遞推的方式對載體的導航信息進行解算。慣性/輪速里程計組合導航具有自主性強，不受外接干擾的優勢，在車載導航領域具有廣泛的應用。

傳統的慣性/輪速里程計組合導航普遍是基于卡爾曼濾波器的方式，對載體的導航信息以及傳感器參數進行解算。但是卡爾曼濾波器無法有效利用歷史信息，同時其對非線性量測的處理也會引入誤差，進而影響解算精度。盡管基于圖優化的方法能夠解決上述問題，但是歷史狀態的優化會導致輪速里程計的量測約束需要進行重復積分，從而影響計算效率。

發明內容

發明目的：為提高解算精度和計算效率，本發明提出了一種基于預積分理論的慣性/輪速里程計融合定位與參數優化方法，通過引入預積分理論與圖優化算法，提高慣性/輪速里程計的融合估計精度以及計算效率。

技術方案：一種基于預積分理論的慣性/輪速里程計融合定位與參數優化方法，包括以下步驟：

步驟1：周期讀取t時刻陀螺輸出、加速度計輸出和輪速里程計輸出；

步驟2：計算慣性/輪速里程計預積分量測、雅克比矩陣和協方差矩陣：

步驟3：基于協方差矩陣，構建慣性/輪速里程計融合的目標函數，利用圖優化算法對載體位置、速度、姿態、陀螺零偏、加速度計零偏、輪速里程計標度因數、輪速里程計與慣性傳感器外參進行估計，得到優化結果：

步驟4：利用優化結果和雅克比矩陣，對步驟2計算得到的慣性/輪速里程計預積分量測進行校正。

進一步的，所述步驟2具體包括：

S210：假設慣性坐標系與機體系重合，建立參考系為世界系的狀態傳播方程；

S220：將狀態傳播方程的參考系由世界系轉換為t_k時刻的機體系，記為b_k系；

S230：采用遞推方法計算t_k+1時刻的機體相對于b_k系的慣性/輪速里程計預積分量測；

S240：構建t_k時刻與t_k+1時刻之間的慣性/輪速里程計預積分量測誤差狀態傳播方程；

S250：基于慣性/輪速里程計預積分量測誤差狀態傳播方程構建雅克比矩陣和協方差矩陣的遞推方程；

S260：基于雅克比矩陣和協方差矩陣的遞推方程，計算得到t_k+1時刻的雅克比矩陣和協方差矩陣；

其中，所述慣性/輪速里程計預積分量測包括利用陀螺與加速度計量測得到的t_k+1時刻機體相對于b_k系的位置變化量利用陀螺與加速度計量測得到的t_k+1時刻機體相對于b_k系的速度變化量利用陀螺量測得到的t_k+1時刻機體相對于b_k系的四元數變化量和利用陀螺與輪速里程計量測得到的t_k+1時刻機體相對于b_k系的位置變化量