飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置制造方法
【專利摘要】本發(fā)明公開了一種飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置。該方法包括獲取攝像單元的一歷史狀態(tài)信息、攝像單元拍攝的當(dāng)前圖像和當(dāng)前圖像之前的一歷史圖像、以及至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息；根據(jù)歷史狀態(tài)信息預(yù)測(cè)與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)，并由匹配特征點(diǎn)計(jì)算當(dāng)前攝像單元的外部信息或內(nèi)部信息；根據(jù)預(yù)先標(biāo)定的至少一種傳感器相對(duì)于攝像單元的位置關(guān)系計(jì)算攝像單元的外部信息與至少一種傳感器的外部信息之間或者攝像單元的內(nèi)部信息與至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系；利用相對(duì)比例關(guān)系修正由歷史狀態(tài)信息估計(jì)得到的攝像單元的當(dāng)前狀態(tài)信息。實(shí)施本發(fā)明修正飛行器的狀態(tài)信息，減小偏差。
【專利說(shuō)明】飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及無(wú)人飛行控制領(lǐng)域，特別是涉及一種飛行器狀態(tài)實(shí)時(shí)修正的方法，還涉及一種飛行器狀態(tài)實(shí)時(shí)修正的裝置。
【背景技術(shù)】
[0002]近年來(lái)，攜帶攝像設(shè)備的無(wú)人駕駛的飛行器(例如固定翼飛機(jī)、旋翼飛行器包括直升機(jī))在監(jiān)控、偵測(cè)、搜救等應(yīng)用領(lǐng)域扮演著十分重要的作用。對(duì)于這些飛行器的操縱通常由用戶通過(guò)遙控器來(lái)實(shí)現(xiàn)。
[0003]操控者在遙控飛信器過(guò)程中，由于操控者視距有限，在飛行器飛遠(yuǎn)或被阻礙視線的情況下操控者很難觀察出飛行器的狀態(tài)，如果飛行器沒(méi)有輔助飛行手段，那么飛行器極容易飛丟或者撞擊障礙物。因此，具備輔助飛行手段的飛行器應(yīng)運(yùn)而生?，F(xiàn)有的飛行器通常借助于攝像設(shè)備進(jìn)行視覺(jué)導(dǎo)航，從而估計(jì)自身的狀態(tài)信息。然而，估計(jì)的狀態(tài)信息與實(shí)際的狀態(tài)信息之間存在著偏差，當(dāng)視覺(jué)導(dǎo)航時(shí)間越長(zhǎng)，偏差發(fā)散越大，從而降低了視覺(jué)導(dǎo)航的可靠性。

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

[0004]本發(fā)明的主要目的是提供一種飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置，能夠修正飛行器的狀態(tài)信息，減小偏差。
[0005]為解決上述技術(shù)問(wèn)題，本發(fā)明采用的一個(gè)技術(shù)方案是:提供一種飛行器狀態(tài)實(shí)時(shí)修正的方法，包括:獲取攝像單元的一歷史狀態(tài)信息、所述攝像單元拍攝的當(dāng)前圖像和所述當(dāng)前圖像之前的一歷史圖像、以及至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息；根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)，并由所述匹配特征點(diǎn)計(jì)算當(dāng)前所述攝像單元的外部信息或內(nèi)部信息；根據(jù)預(yù)先標(biāo)定的所述至少一種傳感器相對(duì)于所述攝像單元的位置關(guān)系計(jì)算所述攝像單元的外部信息與所述至少一種傳感器的外部信息之間或者所述攝像單元的內(nèi)部信息與所述至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系；利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)信息。
[0006]其中，所述利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)信息的步驟包括:根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)所述歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到所述攝像單元的當(dāng)前狀態(tài)信息；將所述相對(duì)比例關(guān)系與所述當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
[0007]其中，所述根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)的步驟包括:根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)所述歷史圖像中的目標(biāo)特征點(diǎn)在所述當(dāng)前圖像中的坐標(biāo)位置；選擇離所述坐標(biāo)位置最近的特征點(diǎn)作為與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)。
[0008]其中，所述攝像單兀包括單目攝像頭。[0009]其中，所述單目攝像頭為全景攝像頭，所述攝像單元還包括反射鏡，用于將外部光線反射至所述單目攝像頭，則在所述獲取所述攝像單元拍攝的當(dāng)前圖像的步驟之后，所述方法還包括:根據(jù)預(yù)先建立的與所述反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與所述攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用所述坐標(biāo)映射關(guān)系對(duì)所述攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
[0010]其中，所述狀態(tài)信息包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息。
[0011]其中，所述外部信息包括相對(duì)于外部對(duì)象的距離信息，所述內(nèi)部信息包括加速度Ih息、方向息、角速度息、速度息或里程息中的一種或幾種。
[0012]為解決上述技術(shù)問(wèn)題，本發(fā)明采用的另一個(gè)技術(shù)方案是:提供一種飛行器狀態(tài)實(shí)時(shí)修正的裝置，所述飛行器包括攝像單元和至少一個(gè)傳感器，所述裝置包括:獲取模塊，用于獲取所述攝像單元的一歷史狀態(tài)信息、所述攝像單元拍攝的當(dāng)前圖像和所述當(dāng)前圖像之前的一歷史圖像、以及所述至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息；匹配模塊，用于根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)，并由所述匹配特征點(diǎn)計(jì)算當(dāng)前所述攝像單元的外部信息或內(nèi)部信息；計(jì)算模塊，用于根據(jù)預(yù)先標(biāo)定的所述至少一種傳感器相對(duì)于所述攝像單元的位置關(guān)系計(jì)算所述攝像單元的外部信息與所述至少一種傳感器的外部信息之間或者所述攝像單元的內(nèi)部信息與所述至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系；修正模塊，用于利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)信息。
[0013]其中，所述修正模塊包括濾波器單元和修正單元，其中，所述濾波器單元用于根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)所述歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到所述攝像單元的當(dāng)前狀態(tài)信息；所述修正單元用于將所述相對(duì)比例關(guān)系與所述當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
[0014]其中，所述匹配模塊具體用于根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)所述歷史圖像中的目標(biāo)特征點(diǎn)在所述當(dāng)前圖像中的坐標(biāo)位置，并選擇離所述坐標(biāo)位置最近的特征點(diǎn)作為與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)。
[0015]其中，所述攝像單兀包括單目攝像頭。
[0016]其中，所述單目攝像頭為全景攝像頭，所述攝像單元還包括反射鏡，用于將外部光線反射至所述單目攝像頭，所述裝置還包括校正模塊，所述校正模塊用于在所述獲取模塊獲取所述攝像單元拍攝的當(dāng)前圖像之后，根據(jù)預(yù)先建立的與所述反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與所述攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用所述坐標(biāo)映射關(guān)系對(duì)所述攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
[0017]其中，所述狀態(tài)信息包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息。
[0018]其中，所述外部信息包括相對(duì)于外部對(duì)象的距離信息，所述內(nèi)部信息包括加速度Ih息、方向息、角速度息、速度息或里程息中的一種或幾種。
[0019]本發(fā)明的有益效果是:區(qū)別于現(xiàn)有技術(shù)的情況，本發(fā)明提供的飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置，通過(guò)歷史狀態(tài)信息以及攝像單元拍攝的歷史圖像和當(dāng)前圖像中的特征點(diǎn)，計(jì)算攝像單元的外部信息或內(nèi)部信息，再結(jié)合至少一種傳感器的外部信息或內(nèi)部信息得到兩個(gè)外部信息或內(nèi)部信息之間的相對(duì)比例關(guān)系，利用相對(duì)比例關(guān)系來(lái)修正基于歷史狀態(tài)信息得到的當(dāng)前狀態(tài)信息，從而能夠達(dá)到修正飛行器的狀態(tài)信息的目的，可以減小狀態(tài)信息的偏差，提高準(zhǔn)確性和可靠性。
【專利附圖】

【附圖說(shuō)明】
[0020]圖1是本發(fā)明飛行器狀態(tài)實(shí)時(shí)修正的方法一實(shí)施例的流程示意圖；
[0021]圖2是本發(fā)明飛行器狀態(tài)實(shí)時(shí)修正的方法中圖像校正前和校正后的對(duì)比示意圖；
[0022]圖3是圖1所示的飛行器狀態(tài)實(shí)時(shí)修正的方法在具體應(yīng)用場(chǎng)景中的流程示意圖；
[0023]圖4是本發(fā)明飛行器狀態(tài)實(shí)時(shí)修正的裝置一實(shí)施例的結(jié)構(gòu)示意圖。
【具體實(shí)施方式】
[0024]下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅是本發(fā)明的一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其它實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0025]圖1所示為本發(fā)明飛行器狀態(tài)實(shí)時(shí)修正的方法一實(shí)施例的流程示意圖，該方法包括:
[0026]SlOl:獲取攝像單元的一歷史狀態(tài)信息、攝像單元拍攝的當(dāng)前圖像和當(dāng)前圖像之前的一歷史圖像、以及至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息。
[0027]其中，在飛行器起飛時(shí)刻的狀態(tài)信息的信息量為0，起飛之后每個(gè)單位時(shí)刻的狀態(tài)信息都與前一單位時(shí)刻的狀態(tài)信息有關(guān)。由于攝像單元是固定在飛行器上的，飛行器的狀態(tài)信息可視為攝像單元的狀態(tài)信息。在本實(shí)施例中，狀態(tài)信息至少包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息等。在本實(shí)施例中，攝像單元包括單目攝像頭。
[0028]如果每個(gè)單位時(shí)刻之間間隔較長(zhǎng)，則歷史狀態(tài)信息可以優(yōu)選為前一單位時(shí)刻的狀態(tài)信息，歷史圖像優(yōu)選為前一單位時(shí)刻的圖像；如果每個(gè)單位時(shí)刻間隔較短，則歷史狀態(tài)信息可以為間隔多個(gè)單位時(shí)刻的狀態(tài)信息，歷史圖像為間隔多個(gè)單位時(shí)刻的圖像。
[0029]在本實(shí)施例中，外部信息包括相對(duì)于外部對(duì)象的距離信息，內(nèi)部信息包括加速度信息、方向信息、角速度信息、速度信息或里程信息中的一種或幾種。在具體實(shí)施時(shí)，可以采用激光傳感器、超聲波傳感器等外部傳感器測(cè)量相對(duì)于外部對(duì)象的距離，采用加速度傳感器、方向傳感器、里程器等內(nèi)部傳感器測(cè)量加速度、方向、里程等內(nèi)部信息。
[0030]S102:根據(jù)歷史狀態(tài)信息預(yù)測(cè)與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)，并由匹配特征點(diǎn)計(jì)算當(dāng)前攝像單元的外部信息或內(nèi)部信息。
[0031]具體來(lái)說(shuō)，攝像單元拍攝的圖像中包含很多物體信息，例如樹木、建筑物等，這些物體的邊緣角點(diǎn)均可以作為目標(biāo)特征點(diǎn)。由于歷史狀態(tài)信息包括方向、速度等表征運(yùn)動(dòng)向量的信息，當(dāng)選取歷史圖像中的目標(biāo)特征點(diǎn)后，根據(jù)歷史狀態(tài)信息中的方向和速度就可以推斷出與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)。
[0032]攝像單元的外部信息或內(nèi)部信息與傳感器測(cè)量的外部信息或內(nèi)部信息屬于不同的測(cè)量結(jié)果。攝像單元的外部信息或內(nèi)部信息均可以由匹配特征點(diǎn)計(jì)算得到，例如當(dāng)計(jì)算相對(duì)于外部對(duì)象的距離信息時(shí)，通過(guò)當(dāng)前特征點(diǎn)的坐標(biāo)值計(jì)算攝像單元與匹配特征點(diǎn)的距離，當(dāng)計(jì)算速度信息時(shí)，通過(guò)當(dāng)前特征點(diǎn)的坐標(biāo)值、目標(biāo)特征點(diǎn)的坐標(biāo)值以及間隔的單位時(shí)刻計(jì)算攝像單元的速度。[0033]作為本實(shí)施例的一種優(yōu)選的方案，根據(jù)歷史狀態(tài)信息預(yù)測(cè)與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)的步驟具體可以包括:根據(jù)歷史狀態(tài)信息預(yù)測(cè)歷史圖像中的目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置；選擇離坐標(biāo)位置最近的特征點(diǎn)作為與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)。
[0034]其中，通過(guò)歷史狀態(tài)信息可以得到目標(biāo)特征點(diǎn)所處坐標(biāo)位置的位移，從而確定目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置，該過(guò)程可以采用光流法、基于特征描述符的匹配法等方法。當(dāng)然，飛行器的狀態(tài)信息可能存在變化，目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置上可能并沒(méi)有坐標(biāo)點(diǎn)，所以可以尋找離該坐標(biāo)位置最近的特征點(diǎn)作為匹配特征點(diǎn)。
[0035]S103:根據(jù)預(yù)先標(biāo)定的至少一種傳感器相對(duì)于攝像單元的位置關(guān)系計(jì)算攝像單元的外部信息與至少一種傳感器的外部信息之間或者攝像單元的內(nèi)部信息與至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系。
[0036]其中，飛行器上傳感器與攝像單元之間的位置關(guān)系是通過(guò)預(yù)先標(biāo)定得到的，為已知量。位置關(guān)系包括傳感器與攝像單元之間的距離。根據(jù)位置關(guān)系、傳感器的外部信息或內(nèi)部信息、攝像單元的外部信息或內(nèi)部信息，就可以得到兩個(gè)外部信息之間或兩個(gè)內(nèi)部信息之間的相對(duì)比例關(guān)系。
[0037]S104:利用相對(duì)比例關(guān)系修正由歷史狀態(tài)信息估計(jì)得到的攝像單元的當(dāng)前狀態(tài)信
肩、O
[0038]作為本實(shí)施例的一種優(yōu)選的方案，S104具體包括:根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到攝像單元的當(dāng)前狀態(tài)信息；將相對(duì)比例關(guān)系與當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
[0039]其中，狀態(tài)模型和測(cè)量模型都是預(yù)先建立的，是已知的。采用卡爾曼濾波器得到的當(dāng)前狀態(tài)信息與實(shí)際狀態(tài)信息存在比例關(guān)系，通過(guò)將相對(duì)比例關(guān)系與當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算可以讓當(dāng)前狀態(tài)信息進(jìn)一步接近實(shí)際狀態(tài)信息，甚至等于實(shí)際狀態(tài)信息，從而達(dá)到修正目的。
[0040]需要說(shuō)明的是，本實(shí)施例除了采用卡爾曼濾波器之外，還可以采用其它濾波器，例如EKF (Extended Kalman Filter,擴(kuò)展卡爾曼濾波器)。
[0041]在更多實(shí)施例中，考慮到攝像單元的攝像范圍，單目攝像頭采用全景攝像頭，并且攝像單元還包括反射鏡，反射鏡用于將外部光線反射至單目攝像頭。則在獲取攝像單元拍攝的當(dāng)前圖像的步驟之后，該方法還包括:根據(jù)預(yù)先建立的與反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用坐標(biāo)映射關(guān)系對(duì)攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
[0042]其中，反射鏡的曲面通常為拋物線或雙曲線。以拋物線為例，單目攝像頭的成像點(diǎn)位于拋物線的準(zhǔn)線上，并且拋物線的焦點(diǎn)到成像點(diǎn)的連線與準(zhǔn)線垂直。由于拋物線上任一點(diǎn)到準(zhǔn)線的距離與到焦點(diǎn)的距離相等，則可以根據(jù)這個(gè)性質(zhì)建立校正模型，從而根據(jù)校正模型可以得到外部空間中任一點(diǎn)與攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用坐標(biāo)映射關(guān)系在展開單目攝像頭的當(dāng)前圖像時(shí)進(jìn)行校正，如圖2所示，校正前的當(dāng)前圖像由于反射鏡的影響即便較嚴(yán)重，經(jīng)過(guò)柱形展開校正后可以得到正確的圖像。
[0043]圖3所示為圖1所示的飛行器狀態(tài)實(shí)時(shí)修正的方法在具體應(yīng)用場(chǎng)景中的流程示意圖，在本實(shí)施例中，攝像單元包括反射鏡和單目攝像頭，傳感器采用激光傳感器。該方法包括:
[0044]S201:進(jìn)行初始化，獲取預(yù)先標(biāo)定的攝像單元與至少一種傳感器之間的位置關(guān)系、預(yù)先建立的狀態(tài)模型和測(cè)量模型、預(yù)先建立的與反射鏡對(duì)應(yīng)的校正模型、以及攝像機(jī)的初始狀態(tài)信息。
[0045]其中，狀態(tài)模型的方程為:
[0046]X (k I k~l) =Ax (k_l | k_l) +BU (k) +w (k)
[0047]其中，k為時(shí)間系數(shù)，x(k|k-l)為當(dāng)前狀態(tài)信息預(yù)測(cè)值，x(k-l |k-l)為上一狀態(tài)信息估算值，U (k)為已知的輸入量，w為過(guò)程噪聲，A、B均為狀態(tài)參數(shù)。
[0048]測(cè)量模型的方程為:
[0049]Z (k) =Hx (k I k-l)+v (k)
[0050]其中，Z(k)為匹配特征點(diǎn)測(cè)量值，V為過(guò)程噪聲，H為測(cè)量參數(shù)。
[0051]S202:根據(jù)上一狀態(tài)信息和狀態(tài)模型預(yù)測(cè)得到系統(tǒng)的當(dāng)前狀態(tài)信息預(yù)測(cè)值。
[0052]S203:獲取攝像單元拍攝的當(dāng)前圖像和當(dāng)前圖像之前的上一圖像以及激光傳感器測(cè)量的世界坐標(biāo)系下的特征點(diǎn)。
[0053]S204:根據(jù)校正模型獲取外部空間中任一點(diǎn)與當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用坐標(biāo)映射關(guān)系對(duì)當(dāng)前圖像進(jìn)行校正。
[0054]S205:根據(jù)上一狀態(tài)信息預(yù)測(cè)與上一圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的校正后的當(dāng)前圖像中的匹配特征點(diǎn)，根據(jù)預(yù)設(shè)的測(cè)量模型和匹配特征點(diǎn)得到匹配特征點(diǎn)測(cè)量值。
[0055]其中，在測(cè)量模型的方程中，x(k|k-l)為目標(biāo)特征點(diǎn)，是當(dāng)前狀態(tài)信息預(yù)測(cè)值中的
一項(xiàng)參數(shù)。
[0056]S206:對(duì)當(dāng)前狀態(tài)信息預(yù)測(cè)值和匹配特征點(diǎn)測(cè)量值采用卡爾曼濾波器進(jìn)行濾波，得到攝像單元的當(dāng)前狀態(tài)信息估算值。
[0057]其中，卡爾曼濾波器的時(shí)間更新方程為:
[0058]X (k I k) =x (k I k-1) +kg (k) (Z (k) -Hx (k I k-1))
[0059]kg(k)=P(k|k-l)H，/(HP(k|k-l))H/ +R)
[0060]其中，x(k|k)為當(dāng)前狀態(tài)信息估算值,kg(k)為卡爾曼增益,P(k| k_l)為x(k|k_l)對(duì)應(yīng)的協(xié)方差，R為V的協(xié)方差，H'為H的轉(zhuǎn)置矩陣。
[0061]S207:計(jì)算激光傳感器到與世界坐標(biāo)系下的特征點(diǎn)的第一距離以及攝像單元到匹配特征點(diǎn)測(cè)量值的第二距離，并根據(jù)位置關(guān)系計(jì)算第一距離與第二距離之間的相對(duì)比例關(guān)系O
[0062]其中，激光傳感器發(fā)出的激光束上如果沒(méi)有特征點(diǎn)，那么將選定離激光束最近的特征點(diǎn)。
[0063]S208:將相對(duì)比例關(guān)系與當(dāng)前狀態(tài)信息估算值進(jìn)行積運(yùn)算。
[0064]其中，進(jìn)行積運(yùn)算之后，可以利用當(dāng)前狀態(tài)信息以及外部信息，例如外部標(biāo)識(shí)點(diǎn)云建立局部地圖，通過(guò)該局部地圖可以識(shí)別障礙物空間位置，從而飛行器在操控者視野之外時(shí)可以進(jìn)行避障和導(dǎo)航。
[0065]圖4所示為本發(fā)明飛行器狀態(tài)實(shí)時(shí)修正的裝置一實(shí)施例的結(jié)構(gòu)示意圖。飛行器包括攝像單元和至少一個(gè)傳感器，該裝置包括獲取模塊10、匹配模塊20、計(jì)算模塊30和修正模塊40。[0066]獲取模塊10用于獲取攝像單元的一歷史狀態(tài)信息、攝像單元拍攝的當(dāng)前圖像和當(dāng)前圖像之前的一歷史圖像、以及至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息。
[0067]其中，在飛行器起飛時(shí)刻的狀態(tài)信息的信息量為0，起飛之后每個(gè)單位時(shí)刻的狀態(tài)信息都與前一單位時(shí)刻的狀態(tài)信息有關(guān)。由于攝像單元是固定在飛行器上的，飛行器的狀態(tài)信息可視為攝像單元的狀態(tài)信息。在本實(shí)施例中，狀態(tài)信息至少包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息等。在本實(shí)施例中，攝像單元包括單目攝像頭。
[0068]如果每個(gè)單位時(shí)刻之間間隔較長(zhǎng)，則歷史狀態(tài)信息可以優(yōu)選為前一單位時(shí)刻的狀態(tài)信息，歷史圖像優(yōu)選為前一單位時(shí)刻的圖像；如果每個(gè)單位時(shí)刻間隔較短，則歷史狀態(tài)信息可以為間隔多個(gè)單位時(shí)刻的狀態(tài)信息，歷史圖像為間隔多個(gè)單位時(shí)刻的圖像。
[0069]在本實(shí)施例中，外部信息包括相對(duì)于外部對(duì)象的距離信息，內(nèi)部信息包括加速度信息、方向信息、角速度信息、速度信息或里程信息中的一種或幾種。在具體實(shí)施時(shí)，可以采用激光傳感器、超聲波傳感器等外部傳感器測(cè)量相對(duì)于外部對(duì)象的距離，采用加速度傳感器、方向傳感器、里程器等內(nèi)部傳感器測(cè)量加速度、方向、里程等內(nèi)部信息。
[0070]匹配模塊20用于根據(jù)歷史狀態(tài)信息預(yù)測(cè)與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)，并由匹配特征點(diǎn)計(jì)算當(dāng)前攝像單元的外部信息或內(nèi)部信息。
[0071]其中，具體來(lái)說(shuō)，攝像單元拍攝的圖像中包含很多物體信息，例如樹木、建筑物等，這些物體的邊緣角點(diǎn)均可以作為目標(biāo)特征點(diǎn)。由于歷史狀態(tài)信息包括方向、速度等表征運(yùn)動(dòng)向量的信息，當(dāng)選取歷史圖像中的目標(biāo)特征點(diǎn)后，根據(jù)歷史狀態(tài)信息中的方向和速度就可以推斷出與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)。
[0072]攝像單元的外部信息或內(nèi)部信息與傳感器測(cè)量的外部信息或內(nèi)部信息屬于不同的測(cè)量結(jié)果。攝像單元的外部信息或內(nèi)部信息均可以由匹配特征點(diǎn)計(jì)算得到，例如當(dāng)計(jì)算相對(duì)于外部對(duì)象的距離信息時(shí)，通過(guò)當(dāng)前特征點(diǎn)的坐標(biāo)值計(jì)算攝像單元與匹配特征點(diǎn)的距離，當(dāng)計(jì)算速度信息時(shí)，通過(guò)當(dāng)前特征點(diǎn)的坐標(biāo)值、目標(biāo)特征點(diǎn)的坐標(biāo)值以及間隔的單位時(shí)刻計(jì)算攝像單元的速度。
[0073]作為本實(shí)施例的一種優(yōu)選的方案，匹配模塊20具體用于根據(jù)歷史狀態(tài)信息預(yù)測(cè)歷史圖像中的目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置，選擇離坐標(biāo)位置最近的特征點(diǎn)作為與歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的當(dāng)前圖像中的匹配特征點(diǎn)。
[0074]其中，通過(guò)歷史狀態(tài)信息可以得到目標(biāo)特征點(diǎn)所處坐標(biāo)位置的位移，從而確定目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置，該過(guò)程可以采用光流法、基于特征描述符的匹配法等方法。當(dāng)然，飛行器的狀態(tài)信息可能存在變化，目標(biāo)特征點(diǎn)在當(dāng)前圖像中的坐標(biāo)位置上可能并沒(méi)有坐標(biāo)點(diǎn)，所以可以尋找離該坐標(biāo)位置最近的特征點(diǎn)作為匹配特征點(diǎn)。
[0075]計(jì)算模塊30用于根據(jù)預(yù)先標(biāo)定的至少一種傳感器相對(duì)于攝像單元的位置關(guān)系計(jì)算攝像單元的外部信息與至少一種傳感器的外部信息之間或者攝像單元的內(nèi)部信息與至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系。
[0076]其中，飛行器上傳感器與攝像單元之間的位置關(guān)系是通過(guò)預(yù)先標(biāo)定得到的，為已知量。位置關(guān)系包括傳感器與攝像單元之間的距離。根據(jù)位置關(guān)系、傳感器的外部信息或內(nèi)部信息、攝像單元的外部信息或內(nèi)部信息，就可以得到兩個(gè)外部信息之間或兩個(gè)內(nèi)部信息之間的相對(duì)比例關(guān)系。
[0077]修正模塊40用于利用相對(duì)比例關(guān)系修正由歷史狀態(tài)信息估計(jì)得到的攝像單元的當(dāng)前狀態(tài)信息。
[0078]其中，作為本實(shí)施例的一種優(yōu)選的方案，修正模塊40包括濾波器單元410和修正單元420。濾波器單元410用于根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到攝像單元的當(dāng)前狀態(tài)信息。修正單元420用于將相對(duì)比例關(guān)系與當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
[0079]其中，狀態(tài)模型和測(cè)量模型都是預(yù)先建立的，是已知的。采用卡爾曼濾波器得到的當(dāng)前狀態(tài)信息與實(shí)際狀態(tài)信息存在比例關(guān)系，通過(guò)將相對(duì)比例關(guān)系與當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算可以讓當(dāng)前狀態(tài)信息進(jìn)一步接近實(shí)際狀態(tài)信息，甚至等于實(shí)際狀態(tài)信息，從而達(dá)到修正目的。
[0080]需要說(shuō)明的是，本實(shí)施例除了采用卡爾曼濾波器之外，還可以采用其它濾波器，例如 EKF。
[0081]在更多實(shí)施例中，考慮到攝像單元的攝像范圍，單目攝像頭采用全景攝像頭，并且攝像單元還包括反射鏡，反射鏡用于將外部光線反射至單目攝像頭。則該裝置還可以包括校正模塊50。校正模塊50用于在獲取模塊10獲取攝像單元拍攝的當(dāng)前圖像之后，根據(jù)預(yù)先建立的與反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用坐標(biāo)映射關(guān)系對(duì)攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
[0082]其中，反射鏡的曲面通常為拋物線或雙曲線。以拋物線為例，單目攝像頭的成像點(diǎn)位于拋物線的準(zhǔn)線上，并且拋物線的焦點(diǎn)到成像點(diǎn)的連線與準(zhǔn)線垂直。由于拋物線上任一點(diǎn)到準(zhǔn)線的距離與到焦點(diǎn)的距離相等，則可以根據(jù)這個(gè)性質(zhì)建立校正模型，從而根據(jù)校正模型可以得到外部空間中任一點(diǎn)與攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用坐標(biāo)映射關(guān)系在展開單目攝像頭的當(dāng)前圖像時(shí)進(jìn)行校正。
[0083]本發(fā)明實(shí)施例還提供一種飛行器，包括上述實(shí)施例的裝置，飛行棋的其它結(jié)構(gòu)請(qǐng)參照現(xiàn)有技術(shù)，此處不再詳述。
[0084]通過(guò)上述方式，本發(fā)明的飛行器狀態(tài)實(shí)時(shí)修正的方法和裝置，通過(guò)歷史狀態(tài)信息以及攝像單元拍攝的歷史圖像和當(dāng)前圖像中的特征點(diǎn)，計(jì)算攝像單元的外部信息或內(nèi)部信息，再結(jié)合至少一種傳感器的外部信息或內(nèi)部信息得到兩個(gè)外部信息或內(nèi)部信息之間的相對(duì)比例關(guān)系，利用相對(duì)比例關(guān)系來(lái)修正基于歷史狀態(tài)信息得到的當(dāng)前狀態(tài)信息，從而能夠達(dá)到修正飛行器的狀態(tài)信息的目的，可以減小狀態(tài)信息的偏差，提高準(zhǔn)確性和可靠性，避免飛行器飛丟，豐富操控者的飛行體驗(yàn)。
[0085]以上所述僅為本發(fā)明的實(shí)施例，并非因此限制本發(fā)明的專利范圍，凡是利用本發(fā)明說(shuō)明書及附圖內(nèi)容所作的等效結(jié)構(gòu)或等效流程變換，或直接或間接運(yùn)用在其它相關(guān)的【技術(shù)領(lǐng)域】，均同理包括在本發(fā)明的專利保護(hù)范圍內(nèi)。
【權(quán)利要求】
1.一種飛行器狀態(tài)實(shí)時(shí)修正的方法，其特征在于，包括: 獲取攝像單元的一歷史狀態(tài)信息、所述攝像單元拍攝的當(dāng)前圖像和所述當(dāng)前圖像之前的一歷史圖像、以及至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息； 根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)，并由所述匹配特征點(diǎn)計(jì)算當(dāng)前所述攝像單元的外部信息或內(nèi)部信息； 根據(jù)預(yù)先標(biāo)定的所述至少一種傳感器相對(duì)于所述攝像單元的位置關(guān)系計(jì)算所述攝像單元的外部信息與所述至少一種傳感器的外部信息之間或者所述攝像單元的內(nèi)部信息與所述至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系； 利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)息。
2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)信息的步驟包括: 根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)所述歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到所述攝像單元的當(dāng)前狀態(tài)信息； 將所述相對(duì)比例關(guān)系與所述當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
3.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征 點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)的步驟包括: 根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)所述歷史圖像中的目標(biāo)特征點(diǎn)在所述當(dāng)前圖像中的坐標(biāo)位置； 選擇離所述坐標(biāo)位置最近的特征點(diǎn)作為與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)。
4.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述攝像單元包括單目攝像頭。
5.根據(jù)權(quán)利要求4所述的方法，其特征在于，所述單目攝像頭為全景攝像頭，所述攝像單元還包括反射鏡，用于將外部光線反射至所述單目攝像頭，則在所述獲取所述攝像單元拍攝的當(dāng)前圖像的步驟之后，所述方法還包括: 根據(jù)預(yù)先建立的與所述反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與所述攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用所述坐標(biāo)映射關(guān)系對(duì)所述攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
6.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述狀態(tài)信息包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息。
7.根據(jù)權(quán)利要求1至6任一項(xiàng)所述的方法，其特征在于，所述外部信息包括相對(duì)于外部對(duì)象的距離信息，所述內(nèi)部信息包括加速度信息、方向信息、角速度信息、速度信息或里程信息中的一種或幾種。
8.一種飛行器狀態(tài)實(shí)時(shí)修正的裝置，其特征在于，所述飛行器包括攝像單元和至少一個(gè)傳感器，所述裝置包括: 獲取模塊，用于獲取所述攝像單元的一歷史狀態(tài)信息、所述攝像單元拍攝的當(dāng)前圖像和所述當(dāng)前圖像之前的一歷史圖像、以及所述至少一種傳感器感測(cè)當(dāng)前測(cè)量的外部信息或內(nèi)部信息； 匹配模塊，用于根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)，并由所述匹配特征點(diǎn)計(jì)算當(dāng)前所述攝像單元的外部信息或內(nèi)部信息； 計(jì)算模塊，用于根據(jù)預(yù)先標(biāo)定的所述至少一種傳感器相對(duì)于所述攝像單元的位置關(guān)系計(jì)算所述攝像單元的外部信息與所述至少一種傳感器的外部信息之間或者所述攝像單元的內(nèi)部信息與所述至少一種傳感器的內(nèi)部信息之間的相對(duì)比例關(guān)系； 修正模塊，用于利用所述相對(duì)比例關(guān)系修正由所述歷史狀態(tài)信息估計(jì)得到的所述攝像單元的當(dāng)前狀態(tài)信息。
9.根據(jù)權(quán)利要求8所述的裝置，其特征在于，所述修正模塊包括濾波器單元和修正單元，其中， 所述濾波器單元用于根據(jù)預(yù)先建立的狀態(tài)模型和測(cè)量模型對(duì)所述歷史狀態(tài)信息采用卡爾曼濾波器進(jìn)行濾波，得到所述攝像單元的當(dāng)前狀態(tài)信息； 所述修正單元用于將所述相對(duì)比例關(guān)系與所述當(dāng)前狀態(tài)信息進(jìn)行積運(yùn)算。
10.根據(jù)權(quán)利要求8所述的裝置，其特征在于，所述匹配模塊具體用于根據(jù)所述歷史狀態(tài)信息預(yù)測(cè)所述歷史圖像中的目標(biāo)特征點(diǎn)在所述當(dāng)前圖像中的坐標(biāo)位置，并選擇離所述坐標(biāo)位置最近的特征點(diǎn)作為與所述歷史圖像中的目標(biāo)特征點(diǎn)對(duì)應(yīng)的所述當(dāng)前圖像中的匹配特征點(diǎn)。
11.根據(jù)權(quán)利要求8所述的裝置，其特征在于，所述攝像單元包括單目攝像頭。
12.根據(jù)權(quán)利要求11所述的裝置，其特征在于，所述單目攝像頭為全景攝像頭，所述攝像單元還包括反射鏡，用于將外部光線反射至所述單目攝像頭，所述裝置還包括校正模塊，所述校正模塊用于在所述獲取模塊獲取所述攝像單元拍攝的當(dāng)前圖像之后，根據(jù)預(yù)先建立的與所述反射鏡對(duì)應(yīng)的校正模型獲取外部空間中任一點(diǎn)與所述攝像單元拍攝的當(dāng)前圖像中的對(duì)應(yīng)點(diǎn)的坐標(biāo)映射關(guān)系，利用所述坐標(biāo)映射關(guān)系對(duì)所述攝像單元拍攝的當(dāng)前圖像進(jìn)行校正。
13.根據(jù)權(quán)利要求5所述的裝置，其特征在于，所述狀態(tài)信息包括攝像單元的姿態(tài)、位置、速度、方向和周圍的環(huán)境信息。
14.根據(jù)權(quán)利要求8至13任一項(xiàng)所述的裝置，其特征在于，所述外部信息包括相對(duì)于外部對(duì)象的距離信息，所述內(nèi)部信息包括加速度信息、方向信息、角速度信息、速度信息或里程信息中的一種或幾種。
【文檔編號(hào)】G05D1/00GK103914065SQ201410112714
【公開日】2014年7月9日 申請(qǐng)日期:2014年3月24日 優(yōu)先權(quán)日:2014年3月24日
【發(fā)明者】周谷越, 楊碩, 劉昂 申請(qǐng)人:深圳市大疆創(chuàng)新科技有限公司