[發明專利]雙IMU單目視覺組合測量非慣性系下目標物位姿方法

有效

• 申請號: 201610095977.5
• 申請日: 2016-02-22
• 公開（公告）號: CN105698765B
• 公開（公告）日: 2018-09-18
• 發明（設計）人: 孫長庫;徐懷遠;郭肖亭;王鵬
• 申請（專利權）人: 天津大學
• 主分類號: G01C11/00
• 分類號: G01C11/00;G01C21/16
• 代理公司: 天津市北洋有限責任專利代理事務所 12201
• 代理人: 劉國威

1.一種雙IMU單目視覺組合測量非慣性系下目標物位姿方法,其特征是，根據兩個慣性測量單元IMU輸出的差異得到非慣性系下的目標物與運動參考系之間的相對角速度和相對加速度，相對角速度經過補償并求出相對姿態角，相對加速度經過積分求出相對位置；利用單目視覺直接測出在非慣性系中目標物上的多特征點靶標相對攝像機坐標系的旋轉矩陣和位置矢量，由旋轉矩陣計算相對姿態角；統一兩者測量結果到相同的坐標系，再根據數據融合方法最終實現相對位姿測量，其中，采用一個IMU和特征點靶標固連在目標物上，其上的IMU坐標系作為目標系，另一個IMU和攝像機固連在運動平臺上，其上的IMU坐標系作為參考系。

2.如權利要求1所述的雙IMU單目視覺組合測量非慣性系下目標物位姿方法，其特征是，目標物與運動參考系之間相對角速度的測量值存在漂移誤差，其粗補償步驟具體是，；

將目標物靜止時兩個IMU輸出的角速度做差，再根據方向余弦矩陣迭代公式、方向余弦矩陣與歐拉角的關系得到三個相對姿態角即方位角ψ、俯仰角θ和橫滾角φ隨時間的變化情況；再用三條一元多次函數曲線擬合，每條曲線對應一個相對姿態角：

式中，d_ij為第i條n次曲線的第j次項系數，τ為IMU采樣周期，l為整數表示第l個IMU采樣周期結束，為第l個周期結束時三個相對姿態角的漂移的估計，上標T表示轉置；目標物靜止時對相對姿態角進行m次測量，采用最小二乘法擬合m次測量結果，得到系數矩陣：

式中，為與的內積，g為每次測量時IMU采樣的周期數，f_p為第i條曲線對應的相對姿態角的第p次測量結果，為f_p與的內積；將擬合得到的曲線作為相對姿態角漂移的粗估計，再由各時刻的粗估計值逆推出相對角速度各時刻的粗補償值。

3.如權利要求1所述的雙IMU單目視覺組合測量非慣性系下目標物位姿方法，其特征是，單一測量方式位姿信息的輸出過程：由兩個IMU得到相對加速度和經過補償的相對角速度，相對加速度經兩次積分得到相對位移；相對角速度經離散歐拉角迭代方法得到相對姿態角；單目攝像機拍攝靶標上4個非共面點，用POSIT算法得到C系到W系間的位姿信息，將視覺測量結果統一為N系到B系的位姿信息，其中，僅用一個IMU和特征點靶標固連在目標物上，其上的IMU坐標系作為目標系B系，另一個IMU和CCD攝像機固連在運動平臺上，其上的IMU坐標系作為參考系N系，轉臺系R系與運動平臺系S系的軸與各自的旋轉軸對齊；世界坐標系W系設定在靶標上；攝像機坐標系C系設定在攝像機CCD面上。

4.如權利要求3所述的雙IMU單目視覺組合測量非慣性系下目標物位姿方法，其特征是，數據融合之前的兩次判定步驟是，判定條件1：POSIT算法是否報錯，該判定條件旨在避免采集的圖像受遮擋而缺失靶標點；判定條件2：

式中，W單目為單目視覺測量得到的N系到B系的位姿量，W^IMU為雙IMU慣性測量得到的N系到B系的位姿量，ε₁為閾值，方陣P的特征值反映了在其特征向量方向上視覺測量結果和雙IMU測量結果之間的差異的大小，故用P的跡作為判定量，旨在剔除受環境干擾而質量太差的圖像。