[發明專利]一種位姿融合估計方法、裝置及存儲介質

在審

• 申請號: 202110495613.7
• 申請日: 2021-05-07
• 公開（公告）號: CN113203413A
• 公開（公告）日: 2021-08-03
• 發明（設計）人: 胡心怡;楊揚;徐嵩
• 申請（專利權）人: 上海伯鐳智能科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01S19/47;G01S19/49;G06K9/62
• 代理公司: 北京知匯林知識產權代理事務所(普通合伙) 11794
• 代理人: 楊華

1.一種位姿融合估計方法，其特征在于，包括：

確定車身慣導位姿狀態和車頂天線模塊位姿狀態；

確定所述車身慣導位姿狀態和所述車頂天線模塊位姿狀態之間的柔性連接因子；

根據所述車身慣導位姿狀態、所述車頂天線模塊位姿狀態和所述柔性連接因子，確定車頂天線的定位因子。

2.根據權利要求1所述的方法，其特征在于，所述確定車身慣導位姿狀態包括：

通過安裝在車輛主框架上的位姿傳感器確定所述車身慣導位姿狀態。

3.根據權利要求1到2之一所述的方法，其特征在于，包括：

第i時刻的車身慣導位姿狀態估計值x_ci通過第i-1時刻的車身慣導位姿狀態估計值x_ci-1和所述x_ci和x_ci-1之間的預積分因子z_Ici確定。

4.根據權利要求1所述的方法，其特征在于，所述確定車頂天線模塊位姿狀態包括：

通過安裝在車頂的全球導航衛星系統GNSS接收機確定所述車頂天線模塊位姿狀態。

5.根據權利要求4所述的方法，其特征在于，包括：

第i時刻的車頂天線模塊位姿狀態估計值x_ri通過第i-1時刻的車頂天線模塊位姿狀態估計值x_ri-1和所述x_ri和x_ri-1之間的車頂天線內置慣導預積分因子z_Iri確定。

6.根據權利要求1所述的方法，其特征在于，所述確定所述車身慣導位姿狀態和所述車頂天線模塊位姿狀態之間的柔性連接因子包括：

預先標定車頂天線到車身IMU的外參初值，并預先確定車頂天線到車身IMU的柔性約束代價函數，在線標定車頂天線與車身IMU的相對位姿值。

7.根據權利要求6所述的方法，其特征在于，包括：

第i時刻的柔性連接因子T_i的轉移矩陣為：

其中，R_i為旋轉矩陣和均為3維列向量；R_0i為姿態初始標定值，為姿態在線標定值，為姿態相對晃動值，表示姿態旋轉疊加；

t_i為平移向量t_i＝t_0i+τ_0i+τ_si，t_0i為位移初始標定值，τ_0i為位移在線標定值，τ_si為位移相對振動值。

8.根據權利要求7所述的方法，其特征在于，包括：

通過下列代價函數確定相對晃動值的最優估計值：

f(φ_si)表示姿態晃動代價函數，exp()表示自然指數運算，是正常的姿態晃動范圍，λ是姿態異常晃動的懲罰參數；

f(τ_si)表示位移振動的代價函數，exp()表示自然指數運算，c_τ是位移，γ是位移異常晃動的懲罰參數。

9.一種位姿融合估計裝置，其特征在于，包括：

車身慣導模塊，被配置用于確定車身慣導位姿狀態；

車頂天線模塊，被配置用于確定車頂天線模塊位姿狀態；

算法模塊，被配置用于確定所述車身慣導位姿狀態和所述車頂天線模塊位姿狀態之間的柔性連接因子，根據所述車身慣導位姿狀態、所述車頂天線模塊位姿狀態和所述柔性連接因子，確定車頂天線的定位因子。