[發(fā)明專(zhuān)利]一種基于直接法與慣導(dǎo)融合的飛行器位姿估計(jì)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201711190702.0
• 申請(qǐng)日: 2017-11-24
• 公開(kāi)（公告）號(hào): CN108036785A
• 公開(kāi)（公告）日: 2018-05-15
• 發(fā)明（設(shè)計(jì)）人: 許超;翁一楨
• 申請(qǐng)（專(zhuān)利權(quán)）人: 浙江大學(xué)
• 主分類(lèi)號(hào): G01C21/16
• 分類(lèi)號(hào): G01C21/16
• 代理公司: 杭州求是專(zhuān)利事務(wù)所有限公司 33200
• 代理人: 邱啟旺

1.一種基于直接法與慣導(dǎo)融合的飛行器位姿估計(jì)方法，其特征在于，包括如下步驟：

(1)通過(guò)加速度計(jì)和陀螺儀分別測(cè)量飛行器實(shí)時(shí)的加速度和角速度信息，通過(guò)磁力計(jì)確定絕對(duì)偏航角，從而獲得飛行器實(shí)時(shí)的姿態(tài)信息，所述的實(shí)時(shí)姿態(tài)信息包括飛行器的俯仰角、橫滾角和偏航角；

(2)利用視覺(jué)傳感器，采用直接法來(lái)獲得尺度缺失的視覺(jué)傳感器運(yùn)動(dòng)位姿估計(jì)，該步驟中獲得的運(yùn)動(dòng)位姿包括：視覺(jué)傳感器相對(duì)于參考坐標(biāo)系的旋轉(zhuǎn)和視覺(jué)傳感器在參考坐標(biāo)系中缺乏尺度的平移。

(3)建立運(yùn)動(dòng)方程，構(gòu)造擴(kuò)展卡爾曼濾波，利用加速度輸出和角速度信息，進(jìn)行擴(kuò)展卡爾曼濾波器預(yù)測(cè)，結(jié)合視覺(jué)直接法輸出的尺度缺失的視覺(jué)傳感器運(yùn)動(dòng)位姿估計(jì)作為量測(cè)值，進(jìn)行更新，獲得飛行器位姿估計(jì)。

2.根據(jù)權(quán)利要求1所述的一種基于直接法與慣導(dǎo)融合的飛行器定位方法，其特征在于，所述步驟(2)中采用直接法獲得視覺(jué)傳感器尺度缺失的運(yùn)動(dòng)位姿估計(jì)的實(shí)施方法為：

建立總光度誤差Ephoto：

其中，表示滑動(dòng)窗口中圖像幀的集合，p表示中的一個(gè)特征點(diǎn)坐標(biāo)，表示第i幀圖像中所有特征點(diǎn)坐標(biāo)的集合，obs(p)表示特征點(diǎn)p所有觀測(cè)的集合；

其中Epj表示單個(gè)特征點(diǎn)的重投影光度誤差，Np表示特征點(diǎn)所在的區(qū)塊，wp表示權(quán)重，Ii和Ij分別表示第i幀圖像和第j幀圖像，p’表示第i幀圖像中的p點(diǎn)重投影到第j幀圖像上的坐標(biāo)；ai、bi均表示第i幀圖像的光度參數(shù)，aj、bj均表示第j幀圖像的光度參數(shù)，ti和tj分別表示第i幀和第j幀圖像的曝光時(shí)間，||·||γ表示使用Huber核函數(shù)；

第i幀圖像中的p點(diǎn)重投影到第j幀圖像上的坐標(biāo)p’為：

π c ( R π c - 1 ( p , d p ) + t ) ( 3 ) ]]>

其中πc為3D-2D的投影模型，為針孔成像模型，針孔成像模型的內(nèi)參矩陣為：

K = f x 0 c x 0 f y c y 0 0 1 , K - 1 = f x - 1 0 - f x - 1 c x 0 f y - 1 - f y - 1 c y 0 0 1 - - - ( 4 ) ]]>

R表示從第i幀到第j幀的旋轉(zhuǎn)，dp表示p點(diǎn)的逆深度，t表示從第i幀到第j幀的平移關(guān)系；內(nèi)參矩陣K中：fx、fy、cx、cy分別表示相機(jī)x軸焦距、y軸焦距、光心在x軸的坐標(biāo)和光心在y軸的坐標(biāo)；

優(yōu)化式(3)中攝像頭的相對(duì)位姿R和t，使總光度誤差Ephoto最小，從而恢復(fù)攝像頭的運(yùn)動(dòng)位姿估計(jì)R和t。