[發明專利]基于異構傳感器信息融合的位姿跟蹤系統及方法

在審

• 申請號: 202111295309.4
• 申請日: 2021-11-03
• 公開（公告）號: CN114022553A
• 公開（公告）日: 2022-02-08
• 發明（設計）人: 王景川;趙文韜;茅天陽
• 申請（專利權）人: 上海交通大學
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06K9/62;G06F17/18
• 代理公司: 上海漢聲知識產權代理有限公司 31236
• 代理人: 胡晶

1.一種基于異構傳感器信息融合的位姿跟蹤方法，其特征在于，包括：

步驟S1：基于內部傳感器提取運動數據建立六自由度運動模型，基于六自由度運動模型進行位姿狀態預測；

步驟S2：基于外部傳感器提取環境特征信息建立觀測模型，基于觀測模型進行觀測更新位姿狀態預測；

步驟S3：針對異構傳感器位姿狀態進行非線性狀態空間建模，得到非線性狀態空間模型；

步驟S4：利用PaRIS算法對非線性狀態空間模型進行在線正切濾波估計，并利用隨機梯度下降算法實時更新多傳感器標定外參，同時獲得高精度位姿估計。

2.根據權利要求1所述的基于異構傳感器信息融合的位姿跟蹤方法，其特征在于，所述步驟S1采用：基于內部運動數據構建六自由度運動模型，確定狀態轉移先驗；

t時刻的位姿跟蹤系統輸入u_t＝[v_t，ω_t]，包括線速度v_t和角速度ω_t，且由內部傳感器獲得；服從高斯噪聲n_S表示服從高斯分布的白噪聲；表示高斯分布；∑_S表示高斯分布的協方差矩陣；狀態向量x_t＝[p_t q_t]，狀態向量由位置p_t和朝向q_t組成；

所述內部運動數據是通過直接獲取位姿信息的傳感器獲取的運動數據，所述傳感器包括IMU以及里程計；

所述六自由度運動模型采用：

p_t+1＝p_t+v_tΔt (1)

其中，位置p_t基于勻速運動模型傳播；v_t表示線速度；ω_t表示角速度；Δt表示兩次內部傳感器測量之間的時間差；I_4×4表示大小為4×4的單位矩陣；朝向q_t表示基于零階四元數積分傳播；∈表示防止數值不穩定而設置的極小數；角速度ω＝[ω_x ω_y ω_z]。

3.根據權利要求2所述的基于異構傳感器信息融合的位姿跟蹤方法，其特征在于，所述觀測模型采用：基于外部采集數據構建觀測模型；

所述外部采集數據是通過直接從外部采集數據的傳感器獲取的環境信息，所述傳感器包括相機以及激光雷達；

所述觀測模型采用：

其中，下標B表示對應運動模型的體坐標系；O表示傳感器坐標系；W表示世界坐標系；P_W表示世界坐標系下3D特征點空間位置；C(·)表示將四元數轉換為旋轉矩陣的函數；C(q_XY)和p_XY分別表示從坐標系Y到坐標系X的旋轉矩陣和位移向量；θ＝[p_BO q_BO]表示外部環境傳感器外參；上標T表示轉置矩陣；

觀測模型帶有高斯噪聲在t時刻的特征點具有如下概率密度，包括：

其中，x_t表示六自由度位姿，y_t表示外部傳感器的實際觀測信息，∑_O表示傳感器的觀測協方差矩陣，表示預測的外部傳感器觀測信息；

對數似然函數關于θ的雅克比為：

其中，^是反對稱矩陣運算符。