[發明專利]一種基于魚眼視覺和慣性測量的目標定位方法及裝置

有效

• 申請號: 202010693248.6
• 申請日: 2020-07-17
• 公開（公告）號: CN111623773B
• 公開（公告）日: 2022-03-04
• 發明（設計）人: 王冬生;李軍;褚文博;林昱;楊文
• 申請（專利權）人: 國汽（北京）智能網聯汽車研究院有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C11/02;G01C11/04;G01C21/20
• 代理公司: 北京三聚陽光知識產權代理有限公司 11250
• 代理人: 沈惠娟

1.一種基于魚眼視覺和慣性測量的目標定位方法，其特征在于，包括：

獲取目標對象當前幀的魚眼圖像數據和慣性測量數據，當前幀的魚眼圖像數據和慣性測量數據的時間戳相同，所述魚眼圖像數據為采用魚眼相機拍攝到的圖像數據；

根據所述慣性測量數據和所述目標對象的前一幀的位姿獲取所述目標對象在當前幀的第一位姿和第一速度，第一位姿包括第一位置和第一姿態；

從所述當前幀的魚眼圖像數據提取視覺特征；

獲取局部地圖中距離所述第一位姿第一預設范圍內的與所述視覺特征相對應的第一地圖點；

利用第一優化函數基于所述第一地圖點、第一位姿、第一速度、慣性測量單元的第一偏置計算所述目標對象在最小誤差下的第二位姿，以對所述第一位姿進行優化，其中，所述誤差包括所述第一地圖點在所述當前幀的魚眼圖像數據的重投影誤差，以及慣性測量變量誤差；

獲取目標對象當前幀的魚眼圖像數據和慣性測量數據的步驟，包括：

獲取目標對象的當前幀的魚眼圖像數據和所述當前幀的魚眼圖像數據的時間戳；

若不存在時間戳與所述當前幀的魚眼數據相同的原始慣性測量數據，獲取所述原始慣性測量數據中時間戳與所述當前幀的魚眼圖像數據的時間戳向前相鄰的第一原始慣性測量數據和向后相鄰的第二原始慣性測量數據；

獲取所述第一原始慣性測量數據與所述當前幀的魚眼圖像數據的時間差；

根據所述第一原始慣性測量數據、第二原始慣性測量數據、第一原始慣性測量數據的時間戳、第二原始慣性測量數據的時間戳、第一原始慣性測量數據與當前幀的魚眼圖像數據的時間差獲取所述慣性測量數據。

2.根據權利要求1所述的基于魚眼視覺和慣性測量的目標定位方法，其特征在于，利用第一優化函數基于所述第一地圖點、第一位姿、第一速度、慣性測量單元的第一偏置計算所述目標對象在最小誤差下的第二位姿之后，還包括：

根據所述第二位姿確定所述第一地圖點中與所述第二位姿相匹配的地圖點的數量；

若所述第一地圖點的數量小于預設閾值，或第一地圖點中與所述第二位姿相匹配的地圖點的數量小于預設閾值，獲取局部地圖中距離所述第一位姿第二預設范圍內的與所述視覺特征相對應的第二地圖點；

利用第一優化函數基于所述第二地圖點、第一位姿、第一速度、慣性測量單元的第一偏置重新計算所述目標對象在最小誤差下的第二位姿，以對所述第一位姿進行優化。

3.根據權利要求1所述的基于魚眼視覺和慣性測量的目標定位方法，其特征在于，所述利用第一優化函數基于所述第一地圖點、第一位姿、第一速度、慣性測量單元的第一偏置計算所述目標對象在最小誤差下的第二位姿的步驟，包括：

分別計算每個第一地圖點在所述當前幀的魚眼圖像數據上的重投影誤差；

計算當前幀的魚眼圖像數據對應的慣性測量數據與前一幀的魚眼圖像數據對應的慣性測量數據之間的慣性測量預積分誤差；

獲取所有第一地圖點的重投影誤差和慣性測量預積分誤差的誤差和最小時對應的第二姿態、第二位置、第二速度、第二偏置，將所述第二姿態和所述第二位置作為所述第二位姿。

4.根據權利要求3所述的基于魚眼視覺和慣性測量的目標定位方法，其特征在于，所述第一優化函數為：

其中，x表示優化變量，，表示第k個第一地圖點在當前幀j的魚眼圖像數據上的重投影誤差，表示當前幀j的魚眼圖像數據對應的慣性測量數據與前一幀i的魚眼圖像數據對應的慣性測量數據之間的慣性測量預積分誤差；ρ為誤差項核函數，為與第k個第一地圖點對應的二維圖像點，表示相機坐標系中三維點到圖像坐標系中二維點的映射函數；，R_cb表示慣性測量單元坐標系到本體坐標系的旋轉變換，R_wb表示慣性測量單元測量到的姿態，R_cw表示第二姿態，表示第一地圖點，，t_cb表示慣性測量單元坐標系到本體坐標系的平移變換，P_wb表示慣性測量單元測量到的位置，t_cw表示第二位置，表示重投影誤差的信息矩陣；分別表示第二姿態與第一姿態的姿態誤差、第二速度與第一速度的速度誤差、第二位置與第一位置的位置誤差、第二偏置與第一偏置的偏置誤差；分別表示運動狀態和慣性測量單元偏置測量的信息矩陣。