[發(fā)明專利]視覺(jué)里程計(jì)的實(shí)現(xiàn)方法和實(shí)現(xiàn)裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 201910554021.0
• 申請(qǐng)日: 2019-06-25
• 公開（公告）號(hào): CN112129272B
• 公開（公告）日: 2022-04-26
• 發(fā)明（設(shè)計(jì)）人: 查紅彬;王欣;姜立;方奕庚
• 申請(qǐng)（專利權(quán)）人: 京東方科技集團(tuán)股份有限公司;北京大學(xué)
• 主分類號(hào): G01C11/00
• 分類號(hào): G01C11/00;G01C22/00
• 代理公司: 北京銀龍知識(shí)產(chǎn)權(quán)代理有限公司 11243
• 代理人: 黃燦;胡影

1.一種視覺(jué)里程計(jì)的實(shí)現(xiàn)方法，其特征在于，包括：

建立用于表述攝像機(jī)的位置變化的第一模型函數(shù)，所述第一模型函數(shù)中采用作用在攝像機(jī)上的力表述所述攝像機(jī)的位置變化；

建立用于表述所述攝像機(jī)的朝向變化的第二模型函數(shù)，所述第二模型函數(shù)中采用所述攝像機(jī)的角速度表述所述攝像機(jī)的朝向變化；

根據(jù)所述第一模型函數(shù)、所述第二模型函數(shù)以及慣性傳感器觀測(cè)到的所述攝像機(jī)的加速度和角速度，得到目標(biāo)函數(shù)；

對(duì)所述目標(biāo)函數(shù)進(jìn)行優(yōu)化，得到優(yōu)化后的作用在所述攝像機(jī)上的力和所述攝像機(jī)的角速度；

作用在所述攝像機(jī)上的力分解成：世界坐標(biāo)系下方向和大小保持不變的力F_w，攝像機(jī)坐標(biāo)系下大小保持不變的力F_b，和，世界坐標(biāo)系下在時(shí)刻t_u新增加的力F_u；

所述對(duì)所述目標(biāo)函數(shù)進(jìn)行優(yōu)化，得到優(yōu)化后的作用在所述攝像機(jī)上的力和所述攝像機(jī)的角速度包括：

對(duì)所述目標(biāo)函數(shù)進(jìn)行優(yōu)化，得到一時(shí)間窗內(nèi)的優(yōu)化后的作用在所述攝像機(jī)上的力和所述攝像機(jī)的角速度，所述時(shí)間窗的開始時(shí)刻由F_u確定。

2.如權(quán)利要求1所述的視覺(jué)里程計(jì)的實(shí)現(xiàn)方法，其特征在于，所述第一模型函數(shù)為：

其中，s(t)是所述攝像機(jī)在時(shí)刻t在三維空間中的位置，t_i為第i個(gè)時(shí)間窗內(nèi)的開始時(shí)刻，Δt為第i個(gè)時(shí)間窗內(nèi)的時(shí)刻t與t_i的時(shí)間差，m為所述攝像機(jī)的質(zhì)量，q(t)為所述攝像機(jī)在時(shí)刻t的朝向的四元數(shù)表示，R[q(t)]為在時(shí)刻t攝像機(jī)坐標(biāo)系變換到世界坐標(biāo)系下的變換矩陣。

3.如權(quán)利要求1所述的視覺(jué)里程計(jì)的實(shí)現(xiàn)方法，其特征在于，所述第二模型函數(shù)為：

其中，q(t)為所述攝像機(jī)在時(shí)刻t的朝向的四元數(shù)表示，ω(t)為所述攝像機(jī)在時(shí)刻t的角速度，t_i為第i個(gè)時(shí)間窗內(nèi)的開始時(shí)刻，Δt為第i個(gè)時(shí)間窗內(nèi)的時(shí)刻t與t_i的時(shí)間差，°為四元數(shù)的乘法運(yùn)算。

4.如權(quán)利要求1所述的視覺(jué)里程計(jì)的實(shí)現(xiàn)方法，其特征在于，所述目標(biāo)函數(shù)為：

其中，i為時(shí)間窗的計(jì)數(shù)值，θ為作用在所述攝像機(jī)上的力的集合，φ為所述攝像機(jī)的角速度的集合，N為第i個(gè)時(shí)間窗內(nèi)的圖像的幀數(shù)，k為第i個(gè)時(shí)間窗內(nèi)的圖像的幀的計(jì)數(shù)值，j為第k幀圖像中的特征點(diǎn)的計(jì)數(shù)值，n_k為第k幀圖像中的特征點(diǎn)的總數(shù)，π(·)為三維點(diǎn)X投影到二維的函數(shù)，是重投影誤差，p為三維特征點(diǎn)對(duì)應(yīng)的二維特征點(diǎn)，T_cw(t)＝[R(q(t)),s(t)]，s(t)是所述攝像機(jī)在時(shí)刻t在三維空間中的位置，q(t)為所述攝像機(jī)在時(shí)刻t的朝向的四元數(shù)表示，R[q(t)]為在時(shí)刻t攝像機(jī)坐標(biāo)系變換到世界坐標(biāo)系下的變換矩陣，h為指定的權(quán)重值，ω(t)為所述攝像機(jī)在時(shí)刻t的角速度，α，β為指定數(shù)值，m為所述攝像機(jī)的質(zhì)量，a_imu(t)為慣性傳感器在時(shí)刻t觀測(cè)得到的加速度值，ω_imu(t)為慣性傳感器在時(shí)刻t觀測(cè)得到的角速度值。

5.如權(quán)利要求1所述的視覺(jué)里程計(jì)的實(shí)現(xiàn)方法，其特征在于，所述對(duì)所述目標(biāo)函數(shù)進(jìn)行優(yōu)化之后，還包括：

若優(yōu)化后的F_u大于或等于指定閾值，確定新的時(shí)間窗開始，所述新的時(shí)間窗的開始時(shí)刻為t_u。