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

有效

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

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

步驟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系的位置變化量 ；

所述的參考系為世界系的狀態傳播方程形式如下：

其中，假設慣性坐標系與機體系重合，記為b；世界系記為w，和分別為t_k與t_k+1時刻機體在世界系下的位置向量，和分別為t_k與t_k+1時刻機體在世界系下的速度向量，和分別為t_k與t_k+1時刻機體系相對于世界系的四元數，Δt_k為[t_k,t_k+1]之間的時間間隔，為t時刻從機體系到世界系的旋轉矩陣，g^w為重力向量；和分別為t時刻加速度計與陀螺的零偏，n_a和n_w分別為加速度計和陀螺的噪聲，和為t_k與t_k+1時刻輪速里程計在機體系下的位置向量，為t時刻輪速里程計系到機體系的旋轉矩陣，為t時刻輪速里程計的標度因數，n_s為輪速里程計的量測噪聲；為陀螺輸出，為加速度計輸出，為輪速里程計輸出；

所述的將狀態傳播的參考系由世界系轉換為t_k時刻的機體系，記為b_k，形式如下：

其中，

其中，為利用陀螺與加速度計量測得到的t_k+1時刻機體相對于b_k系的位置變化量，為利用陀螺與加速度計量測得到的t_k+1時刻機體相對于b_k系的速度變化量，為利用陀螺量測得到的t_k+1時刻機體相對于b_k系的四元數變化量，為利用陀螺與輪速里程計量測得到的t_k+1時刻機體相對于b_k系的位置變化量，為t時刻的機體相對于b_k系的旋轉矩陣，為時刻的機體相對于b_k系的四元數變化量；

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

所述預積分量測遞推方程形式如下：

其中，和分別為t_i以及t_i+1時刻的機體相對于b_k系的標稱位置變化量，和分別為t_i以及t_i+1時刻的機體相對于b_k系的標稱速度變化量，和分別為t_i以及t_i+1時刻的機體相對于b_k系的標稱四元數變化量，和分別為t_i以及t_i+1時刻的機體相對于b_k系的位置變化量；和分別為t_i以及t_i+1時刻的加速度計標稱零偏，和分別為t_i以及t_i+1時刻的陀螺標稱零偏，和分別為t_i以及t_i+1時刻的輪速里程計標稱標度因數，和分別為t_i以及t_i+1時刻的機體相對于b_k系的標稱旋轉矩陣，和分別為t_i以及t_i+1時刻輪速里程計系到機體系的標稱旋轉矩陣，和分別為t_i以及t_i+1時刻輪速里程計在機體系下的位置向量，和分別為t_i以及t_i+1時刻加速度計輸出，和分別為t_i以及t_i+1時刻陀螺輸出，和分別為t_i以及t_i+1時刻輪速里程計輸出，δt為[t_i,t_i+1]之間的時間間隔；

所述慣性/輪速里程計預積分量測誤差狀態傳播方程形式如下：

δz_i+1＝Fδz_i+VN (6)

其中，δz_i與δz_i+1分別為t_i與t_i+1時刻的預積分量測誤差，F為誤差的狀態轉移矩陣，V為測量噪聲系數矩陣，N為量測噪聲向量；

和分別為t_i與t_i+1時刻的位置變化量誤差，和分別為t_i與t_i+1時刻的旋轉誤差，和分別為t_i與t_i+1時刻的速度變化量誤差，和分別為t_i與t_i+1時刻的加速度計零偏誤差，和分別為t_i與t_i+1時刻的陀螺零偏誤差，和分別為t_i與t_i+1時刻位置變化量誤差，和分別為t_i與t_i+1時刻的輪速里程計標度因數誤差，和分別為t_i與t_i+1時刻輪速里程計在機體系下的位置誤差，和分別為t_i與t_i+1時刻輪速里程計系與機體系的旋轉誤差；

誤差的狀態轉移矩陣、測量噪聲系數矩陣和量測噪聲向量采用如下公式：

其中，狀態轉移矩陣與測量噪聲系數矩陣中的元素采用如下所示：

其中，和分別為t_i以及t_i+1時刻加速度計的量測噪聲，和分別為t_i以及t_i+1時刻陀螺的量測噪聲，和分別為加速度計與陀螺的隨機游走噪聲，和分別為t_i以及t_i+1時刻輪速里程計的量測噪聲，和分別為輪速里程計標度因數以及安裝參數的隨機游走噪聲；

所述雅克比矩陣和協方差矩陣的遞推方程，表示為：

對應時刻的系統狀態的雅克比矩陣和協方差矩陣為：

雅克比矩陣和協方差矩陣的遞推方程為：

其中，J_i和J_i+1分別為t_i以及t_i+1時刻的雅克比矩陣，P_i和P_i+1分別為t_i以及t_i+1時刻的協方差矩陣，Q為噪聲信號N的協方差矩陣，表示為：

；

步驟3中的目標函數采用如下形式：

其中，為慣性/里程計測量殘差，表示為：

其中，分別為t_k時刻機體系相對于世界系的位置、速度與四元數，和為t_k時刻加速度計與陀螺的零偏，分別為t_k時刻輪速里程計的標度因數、輪速里程計系相對于機體系的位置、四元數；

待優化變量采用如下形式：

對進行求導，可得雅克比矩陣如下：

其中，L(·)和R(·)分別代表四元數的左乘與右乘，對于四元數q＝[x y z s]＝[ws]：

對進行求導，可得雅克比矩陣如下：

對進行求導，可得雅克比矩陣如下：

對進行求導，可得雅克比矩陣如下：

對進行求導，可得雅克比矩陣如下：

對進行求導，可得雅克比矩陣如下：

其中，和分別為對應和的雅克比矩陣，和分別為對應和的雅克比矩陣，為對應的雅克比矩陣，和分別為對應和的雅克比矩陣；上述雅克比矩陣均對應的子塊矩陣，從對應位置中獲?。?/p>

所述優化結果包括和 ；

其中，和分別為遞推得到的預積分量測，和分別為校正后的預積分量測。