[發(fā)明專利]一種基于多傳感器融合的三維姿態(tài)估計方法及裝置

在審

• 申請?zhí)枺?/td>: 202010108437.2
• 申請日: 2020-02-21
• 公開（公告）號: CN111354043A
• 公開（公告）日: 2020-06-30
• 發(fā)明（設(shè)計）人: 蔡國榕;吳則彪;陳桂錕;沈啟金;王宗躍;吳云東;徐煥
• 申請（專利權(quán)）人: 集美大學(xué)
• 主分類號: G06T7/73
• 分類號: G06T7/73;G06T7/269;G06T7/246;G06T7/33;G06K9/62;G01C21/16
• 代理公司: 廈門福貝知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 35235
• 代理人: 陳遠洋

1.一種基于多傳感器融合的三維姿態(tài)估計方法，其特征在于，該方法包括：

RGB圖像獲取步驟，使用圖像傳感器獲取被測對象的RGB圖像，并提取所述RGB圖像的FAST特征點以及使用KLT稀疏光流法進行特征點的跟蹤；

IMU測量值獲取步驟，使用IMU傳感器采集被測對象的IMU測量值，并對所述IMU測量值進行積分得到當前時刻的被測對象的位置、速度和旋轉(zhuǎn)；

深度圖像獲取步驟，使用深度圖像傳感器獲取被測對象的深度圖像，使用迭代最近點(ICP)算法對所述深度圖像進行處理得到迭代最近點；

目標函數(shù)構(gòu)建步驟，基于先驗信息、迭代最近點、IMU測量值和視覺重投影構(gòu)建目標函數(shù)；

位姿估計步驟，使用迭代法對所述目標函數(shù)求解最小值得到所述對象最優(yōu)的位姿。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述方法還包括：

回環(huán)檢測步驟，檢測當前幀是否發(fā)生回環(huán)，如果是，則執(zhí)行全局的位姿圖優(yōu)化來矯正所述對象的定位軌跡，以消除累積誤差。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述目標函數(shù)為：

min_(R，t){PP+∑||D||²+∑||I||²+∑||V||²}；

通過最小二乘法求得一個最合適的運動參數(shù)：旋轉(zhuǎn)參數(shù)和位移參數(shù)使四個殘差項PP、∑||D||²、∑||I||²與∑||V||²的和的值最低，其中，PP表示先驗信息的先驗分布、D表示迭代最近點殘差項、I表示所述對象的兩幀之間位置、速度、旋轉(zhuǎn)、陀螺儀偏差和加速度偏差的變化量的差、V表示重投影誤差。

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，基于滑動窗口求解該最小二乘，滑動窗口內(nèi)包含最小二乘法計算所需要的輸入?yún)?shù)，每次最小二乘的除先驗信息之外的誤差項都由滑動窗口內(nèi)的值提供。

5.根據(jù)權(quán)利要求4所述的方法，其特征在于，基于以下條件確定RGB圖像中的關(guān)鍵幀：

該幀的平均視差與上一幀關(guān)鍵幀的平均視差相比差值超過第一閾值；

或者，該幀跟蹤的特征點數(shù)量小于第二閾值。

6.根據(jù)權(quán)利要求3所述的方法，其特征在于，

通過深度圖像得到空間的幾何結(jié)構(gòu)，從而估計兩幀之間的變換矩陣，迭代最近點(ICP)殘差項D使用點到面的距離為：

D＝(T·P_j-P_i)·n_i

其中，P_j和P_i是上述深度圖像中已匹配的三維點，n_i是上一幀對應(yīng)的法向量，旋轉(zhuǎn)參數(shù)和位移參數(shù)是兩幀之間的運動參數(shù)；

重投影誤差V作為視覺約束，并將其定義在單位球面的正切平面上：

其中，是特征點經(jīng)過剛體變換后三維點的單位向量的預(yù)測值，P是特征點由像素坐標經(jīng)針孔相機模型反投影得到的三維點，和是P對應(yīng)的切平面的任意兩個正交基。

7.一種基于多傳感器融合的三維姿態(tài)估計裝置，其特征在于，該裝置包括：

RGB圖像獲取單元，用于使用圖像傳感器獲取被測對象的RGB圖像，并提取所述RGB圖像的FAST特征點以及使用KLT稀疏光流法進行特征點的跟蹤；

IMU測量值獲取單元，用于使用IMU傳感器采集被測對象的IMU測量值，并對所述IMU測量值進行積分得到當前時刻的被測對象的位置、速度和旋轉(zhuǎn)；

深度圖像獲取單元，用于使用深度圖像傳感器獲取被測對象的深度圖像，使用迭代最近點(ICP)算法對所述深度圖像進行處理得到迭代最近點；

目標函數(shù)構(gòu)建單元，用于基于先驗信息、迭代最近點、IMU測量值和視覺重投影構(gòu)建目標函數(shù)；

位姿估計單元，用于使用迭代法對所述目標函數(shù)求解最小值得到所述對象最優(yōu)的位姿。