[發(fā)明專利]視覺慣性里程計(jì)初始化方法、裝置及飛行汽車

在審

• 申請(qǐng)?zhí)枺?/td>: 202211639868.7
• 申請(qǐng)日: 2022-12-20
• 公開（公告）號(hào): CN115993132A
• 公開（公告）日: 2023-04-21
• 發(fā)明（設(shè)計(jì)）人: 董博;陶永康;彭登;傅志剛;南志捷
• 申請(qǐng)（專利權(quán)）人: 廣東匯天航空航天科技有限公司
• 主分類號(hào): G01C22/00
• 分類號(hào): G01C22/00;G01C25/00
• 代理公司: 北京匯鑫君達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 11769
• 代理人: 黃紅勝

本申請(qǐng)涉及一種視覺慣性里程計(jì)初始化方法、裝置及飛行汽車。該方法包括：獲取視覺慣導(dǎo)里程計(jì)檢測到的圖像幀中的特征點(diǎn)，根據(jù)所述特征點(diǎn)確定相鄰圖像幀之間的單應(yīng)矩陣；根據(jù)所述單應(yīng)矩陣確定對(duì)應(yīng)的多組位姿數(shù)據(jù)，根據(jù)預(yù)設(shè)方式從所述多組位姿數(shù)據(jù)中確定目標(biāo)位姿數(shù)據(jù)；根據(jù)所述單應(yīng)矩陣和第一預(yù)設(shè)算法確定尺度因子，根據(jù)所述尺度因子和所述目標(biāo)位姿數(shù)據(jù)，確定實(shí)際位姿數(shù)據(jù)。本申請(qǐng)?zhí)峁┑姆桨福瑢?duì)于飛行器從地面起飛的階段可以較好地實(shí)現(xiàn)視覺慣性里程計(jì)的初始化。

技術(shù)領(lǐng)域

本申請(qǐng)涉及飛行汽車技術(shù)領(lǐng)域，尤其涉及一種視覺慣性里程計(jì)初始化方法、裝置及飛行汽車。

背景技術(shù)

目前，VIO(Visual-Inertial?Odometry，視覺慣性里程計(jì))綜合了IMU(InertialMeasurement?Unit，慣性測量單元)和視覺傳感器各自的特點(diǎn)，主要通過IMU和視覺傳感器例如相機(jī)的輸入數(shù)據(jù)，估計(jì)出當(dāng)前載體的運(yùn)動(dòng)狀態(tài)信息，其中包括速度、姿態(tài)和位置等。

VIO系統(tǒng)中包括初始化過程。初始化過程主要是給狀態(tài)估計(jì)變量找到一個(gè)較好的初值。目前的視覺慣性里程計(jì)初始化方案都是針對(duì)一些比較普遍的場景進(jìn)行初始化，例如可以將視覺慣性里程計(jì)分為動(dòng)態(tài)初始化和靜態(tài)初始化，首先進(jìn)行動(dòng)態(tài)初始化，根據(jù)動(dòng)態(tài)初始化的結(jié)果判斷系統(tǒng)是否滿足靜態(tài)初始化條件，如果滿足，則繼續(xù)進(jìn)行靜態(tài)初始化。

但是，對(duì)于飛行器從地面起飛的階段，相關(guān)技術(shù)中的初始化方案并不能得到很好的初始化效果。

發(fā)明內(nèi)容

為解決或部分解決相關(guān)技術(shù)中存在的問題，本申請(qǐng)?zhí)峁┮环N視覺慣性里程計(jì)初始化方法、裝置及飛行汽車，對(duì)于飛行器從地面起飛的階段可以較好地實(shí)現(xiàn)視覺慣性里程計(jì)的初始化。

本申請(qǐng)第一方面提供一種視覺慣性里程計(jì)初始化方法，包括：

獲取視覺慣導(dǎo)里程計(jì)檢測到的圖像幀中的特征點(diǎn)，根據(jù)所述特征點(diǎn)確定相鄰圖像幀之間的單應(yīng)矩陣；

根據(jù)所述單應(yīng)矩陣確定對(duì)應(yīng)的多組位姿數(shù)據(jù)，根據(jù)預(yù)設(shè)方式從所述多組位姿數(shù)據(jù)中確定目標(biāo)位姿數(shù)據(jù)；

根據(jù)所述單應(yīng)矩陣和第一預(yù)設(shè)算法確定尺度因子，根據(jù)所述尺度因子和所述目標(biāo)位姿數(shù)據(jù)，確定實(shí)際位姿數(shù)據(jù)。

在一實(shí)施方式中，所述方法還包括：

根據(jù)所述單應(yīng)矩陣確定所述相鄰圖像幀的特征點(diǎn)的速度關(guān)系；

根據(jù)所述相鄰圖像幀的特征點(diǎn)的速度關(guān)系、相鄰圖像幀的特征點(diǎn)的速度和第二預(yù)設(shè)算法，更新所述相鄰圖像幀的特征點(diǎn)的速度。

在一實(shí)施方式中，所述根據(jù)預(yù)設(shè)方式從所述多組位姿數(shù)據(jù)中確定目標(biāo)位姿數(shù)據(jù)，包括：

根據(jù)先驗(yàn)法向量從所述多組位姿數(shù)據(jù)中選擇先驗(yàn)法向量與單應(yīng)法向量二范數(shù)符合預(yù)設(shè)關(guān)系的一組位姿數(shù)據(jù)為目標(biāo)位姿數(shù)據(jù)。

在一實(shí)施方式中，所述根據(jù)所述單應(yīng)矩陣和第一預(yù)設(shè)算法確定尺度因子，包括：

根據(jù)所述單應(yīng)矩陣和PnP算法確定尺度因子。

在一實(shí)施方式中，所述根據(jù)所述單應(yīng)矩陣和PnP算法確定尺度因子，包括：

獲取所述特征點(diǎn)的深度信息，根據(jù)PnP算法和所述深度信息確定所述特征點(diǎn)從相機(jī)坐標(biāo)系到世界坐標(biāo)系的轉(zhuǎn)換位姿；

根據(jù)所述轉(zhuǎn)換位姿和所述單應(yīng)矩陣確定的對(duì)應(yīng)位姿數(shù)據(jù)，確定尺度因子。

在一實(shí)施方式中，所述第二預(yù)設(shè)算法包括高斯牛頓迭代法。

在一實(shí)施方式中，所述獲取視覺慣導(dǎo)里程計(jì)檢測到的圖像幀中的特征點(diǎn)，包括：

獲取視覺慣導(dǎo)里程計(jì)檢測到的圖像幀中大于預(yù)設(shè)數(shù)量閾值的特征點(diǎn)，并通過RANSAC算法刪除異常的特征點(diǎn)。