輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法
【專利摘要】本發(fā)明公開了輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，屬于視覺導航的技術(shù)領(lǐng)域。現(xiàn)有的方法由于圖像處理耗時的不確定性造成圖像數(shù)據(jù)的非等速率輸出，不利于系統(tǒng)間的集成和信息交流；并且采集到的圖像數(shù)據(jù)會受到噪聲的干擾，干擾過大會造成數(shù)據(jù)丟失。針對以上兩點，本發(fā)明采用多線程的方式，使圖像處理和數(shù)據(jù)輸出模塊化，從而實現(xiàn)數(shù)據(jù)的等速率輸出；并且對于圖像解算后的位置信息進行卡爾曼濾波處理，實現(xiàn)視覺導航最優(yōu)信息的估計。本發(fā)明可根據(jù)實際應用情況調(diào)節(jié)導航數(shù)據(jù)輸出速率。
【專利說明】
輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明公開了輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，屬于視覺導航的技術(shù) 領(lǐng)域。
【背景技術(shù)】
[0002] 隨著導航技術(shù)的不斷發(fā)展，越來越多新的導航方式被發(fā)現(xiàn)和研究。視覺導航便是 其中尤為突出的一種導航方式。視覺導航是通過攝像機對周圍環(huán)境進行圖像采集，并對圖 像進行濾波和計算，完成自身位姿確定和路徑識別，并做出導航?jīng)Q策的一種新的導航技術(shù)。 由于視覺導航采用被動工作方式，設備簡單，成本低廉，其應用范圍很廣，廣泛應用于航空 航天、機器人以及智能車輛交通領(lǐng)域。
[0003] 視覺導航技術(shù)對于圖像處理有著很高的要求，一方面，要求圖像處理解算后的信 息的準確性盡可能高;另一方面，對于圖像信息的解算速率及輸出更新速率要求比較嚴格。 從圖像中提取定位特征具有較大的不確定性，導致在視覺導航中不同圖像的處理時間不 同，而往往視覺導航系統(tǒng)需要跟其他子系統(tǒng)(如與慣性導航進行融合、輸出數(shù)據(jù)到飛行控制 系統(tǒng)等)進行交互，等速率的導航參數(shù)輸出對于視覺導航系統(tǒng)的模塊化及與其它子系統(tǒng)的 集成具有重要意義?；谝曈X導航技術(shù)對圖像處理的要求，提出一套適合視覺導航的等速 率數(shù)據(jù)輸出方法，以解決圖像解算精度和圖像輸出更新速率的問題。

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

[0004] 本發(fā)明所要解決的技術(shù)問題是針對上述【背景技術(shù)】的不足，考慮到視覺導航技術(shù)對 圖像解算精度的要求以及圖像輸出更新速率的要求，提供了輸出速率可調(diào)的視覺導航最優(yōu) 信息估計方法，在保證導航參數(shù)等速率輸出的同時，也保證了視覺導航系統(tǒng)輸出信息的最 優(yōu)性，解決了圖像解算精度和圖像輸出更新速率相互協(xié)調(diào)的技術(shù)問題。
[0005] 本發(fā)明為實現(xiàn)上述發(fā)明目的采用如下技術(shù)方案：
[0006] 輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，采用包括主線程、圖像處理子線程、 數(shù)據(jù)輸出子線程的多線程機制實現(xiàn)視覺導航最優(yōu)信息的估計和輸出，各線程通過調(diào)用全局 變量交換數(shù)據(jù)，具體為：
[0007] 圖像處理子線程:根據(jù)攝像頭采集的圖像解算位置信息的量測值以及飛機運動狀 態(tài)估計位置信息的預測值，對位置信息的量測值以及預測值進行卡爾曼濾波融合得到位置 信息的最優(yōu)估計值，更新全局變量并反饋當前位置信息的最優(yōu)估計值給主線程；
[0008]主線程:在接收到圖像處理子線程反饋的位置信息最優(yōu)估計值后，更新全局變量 并開啟多線程的并發(fā)執(zhí)行狀態(tài)；
[0009]數(shù)據(jù)輸出子線程:在輸出時刻到來時請求主線程查詢當前位置信息最優(yōu)估計值對 應的時刻，僅在當前輸出時刻與當前位置信息最優(yōu)估計值對應時刻的差值符合閾值要求 時，根據(jù)當前位置信息最優(yōu)估計值預估當前輸出時刻的位置信息，輸出當前輸出時刻的位 置信息預測值。
[0010] 進一步的，所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法中，圖像處理子線程 估計位置信息預測值的方法為：
[0011] 以上一時刻的位置信息最優(yōu)估計x(t〇)為參考值，采用卡爾曼濾波器對當前時刻 的位置信息預測值X(ti+A ti I to)進行估計:X(ti+ Δ ti I to) =AX(to)+BU(ti+ Δ ti)，A、B為系 統(tǒng)參數(shù)，U ( t i + Δ t i )為當前時刻對系統(tǒng)的控制量，U ( t i + Δ t i )取值為零，
[0012] 更新當前時刻的位置信息預測值X(t# Δ t! I to)的協(xié)方差PU1+Δ t! I to) JU1+ Δ乜 七0)=4?^〇)4〃 + 〇，？^())為上一時刻的位置信息最優(yōu)預測值父4())的協(xié)方差，
JT為卡爾曼濾波器相 鄰兩個估計時刻的時間差，dT的初始值取0.5。
[0013] 再進一步的，所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法中，圖像處理子線 程對位置信息的量測值以及預測值進行卡爾曼濾波融合得到位置信息的最優(yōu)估計值，具體 方法為：
[0014] 先由表達式:X(ti+ Δ ti) = X(ti+ Δ ti I to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)_HX(ti+ Δ ti to))得到當前時刻位置信息最優(yōu)估計值X(ti+A ti)，再由表達式:P(ti+A ti) = (I_Kg(ti+ Δ ti)H)P(ti+A ti I to)更新當前時刻位置信息最優(yōu)估計值X(ti+A ti)的協(xié)方差P(ti+A ti)，將 當前時刻位置信息最優(yōu)估計值XU1+At1)作為估計下一時刻位置信息預測值的參考值，Z U 1+At1)為當前時刻位置信息的測量值，I為單位矩陣，Kg(WAt1)為卡爾曼增益，KgU 1+ Δ ti) =PU1+ Δ ti I to)H-V(HP(ti+ Δ ti I to)H-T + R)，H為測量系統(tǒng)的參數(shù)矩陣，
[0015] 更進一步的，所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法中，數(shù)據(jù)輸出線程 輸出時刻間隔接近圖像處理時間的平均值或眾數(shù)。
[0016] 實現(xiàn)所述視覺導航最優(yōu)信息估計方法的系統(tǒng)，包括：
[0017] 根據(jù)攝像頭采集的圖像預測位置信息的圖像處理模塊，以及，
[0018] 在輸出時刻到來時根據(jù)當前位置信息進行最優(yōu)估計并輸出當前輸出時刻位置信 息的濾波預測輸出模塊。
[0019] 本發(fā)明采用上述技術(shù)方案，具有以下有益效果：
[0020] (1)對圖像采集的數(shù)據(jù)進行預處理和位姿解算，將所得到的數(shù)據(jù)作為量測值與通 過系統(tǒng)模型得到的預測值進行卡爾曼濾波融合得到最優(yōu)估計值，進而更新當前狀態(tài)，大大 提升可靠性和準確性，避免了因為某時刻噪聲干擾過大使得圖像數(shù)據(jù)丟失而導致導航失敗 的現(xiàn)象；
[0021] (2)數(shù)據(jù)輸出線程實現(xiàn)了等速率導航數(shù)據(jù)的輸出，并且可根據(jù)實際應用情況調(diào)節(jié) 導航數(shù)據(jù)輸出速率；
[0022] (3)通過多線程機制將圖像處理部分與數(shù)據(jù)輸出部分分離開，協(xié)調(diào)了圖像處理耗 時不確定與數(shù)據(jù)等速率輸出之間的矛盾，并使得圖像處理部分模塊化，便于與其他模塊集 成。
【附圖說明】
[0023]圖1為圖像數(shù)據(jù)處理以及圖像數(shù)據(jù)輸出的過程；
[0024]圖2(a)、圖2(b)分別為圖像處理子線程、數(shù)據(jù)輸出子線程的流程圖。
【具體實施方式】
[0025]下面結(jié)合附圖對發(fā)明的技術(shù)方案進行詳細說明。
[0026]現(xiàn)有的方法由于圖像處理耗時的不確定性造成圖像數(shù)據(jù)的非等速率輸出，不利于 系統(tǒng)間的集成和信息交流;并且采集到的圖像數(shù)據(jù)會受到噪聲的干擾，干擾過大會造成數(shù) 據(jù)丟失。針對以上兩點，本發(fā)明采用多線程的方式，使圖像處理和數(shù)據(jù)輸出模塊化，從而實 現(xiàn)數(shù)據(jù)的等速率輸出；并且對于圖像解算后的位置信息進行卡爾曼濾波處理，實現(xiàn)視覺導 航最優(yōu)信息的估計。本發(fā)明可根據(jù)實際應用情況調(diào)節(jié)導航數(shù)據(jù)輸出速率。
[0027] 步驟一:建立多線程機制。
[0028]在Linux操作系統(tǒng)環(huán)境下創(chuàng)建線程對象，然后對線程對象進行管理：
[0029] (1)定義一個DataShare類，將其實例化后的對象變量作為全局變量，供各個線程 之間相互調(diào)用，實現(xiàn)線程之間的數(shù)據(jù)交換；
[0030] (2)通過threading模塊的方法創(chuàng)建圖像處理線程和數(shù)據(jù)輸出線程，繼承Thread 類，對類中的run()方法進行重寫，來實現(xiàn)圖像處理線程以及數(shù)據(jù)輸出線程所要完成的任 務；
[0031] (3)通過類中的SetDaemon(True)方法將主線程設置為為守護線程，即主線程執(zhí)行 結(jié)束后，子線程（圖像處理線程、數(shù)據(jù)輸出線程）一并和主線程結(jié)束退出，然后通過類的 StartO方法開啟子線程；
[0032] (4)在主線程和子線程并發(fā)執(zhí)行時，全局變量作為連接多個線程的橋梁，子線程處 理圖像數(shù)據(jù)后對全局變量進行更新，同時將更新后的結(jié)果反饋給主線程進行處理，多線程 并發(fā)執(zhí)行。
[0033] 啟動圖像處理子線程，具體線程執(zhí)行步驟見步驟二，啟動數(shù)據(jù)輸出子線程，具體線 程執(zhí)行步驟見步驟三。
[0034] 步驟二:在圖像處理線程中加入卡爾曼濾波環(huán)節(jié)以實現(xiàn)對視覺導航信息的最優(yōu)估 計。
[0035] (1)卡爾曼濾波器根據(jù)上一時刻的位置信息最優(yōu)估計值X(to)進一步預測得到當 前時刻的位置信息預測值XU1+ Δ tl I to)(預測更新）：
[0036] X(ti+ Δ ti I to) =AX(to)+BU(/ ti+ A t, nI (I)
[0037] 式（I)中，A、B為系統(tǒng)參數(shù)， ilKti+Δ ti)是當前時刻 (t i)對系統(tǒng)的控制量，U (△ t i)取值為0。
[0038] (2)對父(1：1+&1：1|1:。)的協(xié)方差？(1：1+&1：1|1:。)進行更新：
[0039] P(ti+Ati|to)=AP(to)A-T+Q (2)
[0040] 式（2)中P (to)為上一時刻的位置信息最優(yōu)預測值X (to)的協(xié)方差，
IT為卡爾曼濾波器相 鄰兩個估計時刻的時間差，dT的初始值取0.5。
[0041] (3)圖像處理線程通過攝像頭采集圖像信息，然后對其進行圖像處理和導航解算 得到當前時刻位置信息的量測量ZU1+ Δ tl)，然后對ZU1+ Δ tl)和XU1+ Δ tl I to)進行卡爾 曼濾波融合得到當前時刻位置信息的最優(yōu)估計值XU1+At1K量測更新）：
[0042] X(ti+ Δ ti) = X(ti+ Δ ti | to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)-HX(ti+ Δ ti | to)) (3)
[0043] 式⑶中，Kg(^At1)為卡爾曼增益，
[0044] KgU1+ Δ ti) =PU1+ Δ ti I to)H-tAHPU1+ Δ ti I to)H-T+R)，H為測量系統(tǒng)的參數(shù)矩 陣
[0045] (4)對XUdAt1)的協(xié)方差進行更新：
[0046] P(ti+ Ati) = (I~Kg(ti+ Δ ti)H)P(ti+ Δ ti | to) (4)
[0047] 式(4)中，I為單位矩陣。
[0048] (S)X(WAt1)作為下一時刻位置信息預測值的參考值，以此類推。
[0049] (6)此外當出現(xiàn)當前時刻的測量值為空時，即沒有得到測量值時，卡爾曼濾波器會 根據(jù)上一時刻的最優(yōu)估計值進行預測估計，來作為當前的最優(yōu)估計值。
[0050] 步驟三:數(shù)據(jù)輸出線程在輸出時刻tP+nT到來時，查詢當前卡爾曼最優(yōu)估計值的時 刻t，然后用其時間差t P+nT-t預測tP+nT時刻的輸出值：
[0051] X(tP+nT)=AX(t)+BU(t) (5)
[0052] 最后將輸出的圖像位置數(shù)據(jù)供其它處理線程所用。
[0053] 通過調(diào)節(jié)數(shù)據(jù)輸出線程的時間常數(shù)T即可改變圖像數(shù)據(jù)的輸出更新速率，實現(xiàn)了 數(shù)據(jù)速率可調(diào)節(jié)的要求。T的選取并不是任意的，需要根據(jù)圖像的采集處理時間來調(diào)節(jié)，假 設圖像的采集、處理時間為20-40ms之間（每幀圖像不同，處理的時間會有所差異），那么我 們T的選取也要遵照這個時間，最好選擇在這個時間范圍附近，太大和太小都會使得整個系 統(tǒng)輸出的數(shù)據(jù)精度下降。如果T太大，會使圖像數(shù)據(jù)的更新速率過慢，延時嚴重，使得控制得 不到實時的矯正;反之如果T過小(T = 2ms)，每十幾次輸出才會有一次圖像數(shù)據(jù)的更新，速 率不匹配造成大量無效的數(shù)據(jù)訪問，影響了資源利用率。因此T的選擇盡量接近圖像的采集 處理時間的平均值或者其眾數(shù)。
[0054]鑒于本申請的多線程機制，構(gòu)建根據(jù)攝像頭采集的圖像預測位置信息的圖像處理 模塊，根據(jù)數(shù)據(jù)輸出線程和卡爾曼最優(yōu)估計值建立濾波預測輸出模塊，在濾波預測輸出模 塊外部建立數(shù)據(jù)輸出線程作為圖像處理模塊的外部接口。
[0055]圖1為圖像數(shù)據(jù)處理以及圖像數(shù)據(jù)輸出的過程，可以看出多線程的建立使得圖像 處理線程模塊化，圖像處理可以獨立的循環(huán)執(zhí)行，不受其他進程的影響而中斷。其中數(shù)據(jù)輸 出線程為圖像處理模塊與外界其他模塊間相互交流提供了訪問接口，并可以改變數(shù)據(jù)輸出 線程的數(shù)據(jù)更新速率。從圖中可以看出，在輸出時刻t P+nT到來時，查詢當前位置信息最優(yōu) 估計值對應的時刻tk+Atk，根據(jù)當前位置信息最優(yōu)估計值預估當前輸出時刻的位置信息， 輸出當前輸出時刻的位置信息預測值。
[0056]圖2(a)、圖2(b)分別為圖像處理線程、數(shù)據(jù)輸出線程的流程圖，多線程的建立使得 整個系統(tǒng)模塊化，便于系統(tǒng)內(nèi)部的集成。
[0057]綜上所述，本發(fā)明具有以下有益效果：
[0058] (1)對圖像采集的數(shù)據(jù)進行預處理和位姿解算，將所得到的數(shù)據(jù)作為量測值與通 過系統(tǒng)模型得到的預測值進行卡爾曼濾波融合得到最優(yōu)估計值，進而更新當前狀態(tài)，大大 提升可靠性和準確性，避免了因為某時刻噪聲干擾過大使得圖像數(shù)據(jù)丟失而導致導航失敗 的現(xiàn)象；
[0059] (2)數(shù)據(jù)輸出線程實現(xiàn)了等速率導航數(shù)據(jù)的輸出，并且可根據(jù)實際應用情況調(diào)節(jié) 導航數(shù)據(jù)輸出速率；
[0060] (3)通過多線程機制將圖像處理部分與數(shù)據(jù)輸出部分分離開，協(xié)調(diào)了圖像處理耗 時不確定與數(shù)據(jù)等速率輸出之間的矛盾，并使得圖像處理部分模塊化，便于與其他模塊集 成。
【主權(quán)項】
1. 輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，其特征在于，采用包括主線程、圖像處 理子線程、數(shù)據(jù)輸出子線程的多線程機制實現(xiàn)視覺導航最優(yōu)信息的估計和輸出，各線程通 過調(diào)用全局變量交換數(shù)據(jù)，具體為： 圖像處理子線程:根據(jù)攝像頭采集的圖像解算位置信息的量測值以及飛機運動狀態(tài)估 計位置信息的預測值，對位置信息的量測值以及預測值進行卡爾曼濾波融合得到位置信息 的最優(yōu)估計值，更新全局變量并反饋當前位置信息的最優(yōu)估計值給主線程； 主線程:在接收到圖像處理子線程反饋的位置信息最優(yōu)估計值后，更新全局變量并開 啟多線程的并發(fā)執(zhí)行狀態(tài)； 數(shù)據(jù)輸出子線程:在輸出時刻到來時請求主線程查詢當前位置信息最優(yōu)估計值對應的 時刻，僅在當前輸出時刻與當前位置信息最優(yōu)估計值對應時刻的差值符合閾值要求時，根 據(jù)當前位置信息最優(yōu)估計值預估當前輸出時刻的位置信息，輸出當前輸出時刻的位置信息 預測值。2. 根據(jù)權(quán)利要求1所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，其特征在于，圖像 處理子線程估計位置信息預測值的方法為： 以上一時刻的位置信息最優(yōu)估計X(t〇)為參考值，采用卡爾曼濾波器對當前時刻的位置 十目息預測值X(ti+ Δ ti I to)進彳丁估計:X(ti+ Δ ti I to) =AX(to)+BU(ti+ Δ ti)，A、B為系統(tǒng)參數(shù)， U( h)為當前時刻對系統(tǒng)的控制量，U(Δ )取值為零，更新當前時刻的位置?目息預測值X(ti+ Δ ti I to)的協(xié)方差P(ti+ Δ ti I to) :P(ti+ Δ ti I to)鄰兩個估計時刻的時間差，dT的初始值取0.5。3. 根據(jù)權(quán)利要求2所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，其特征在于，圖像 處理子線程對位置信息的量測值以及預測值進行卡爾曼濾波融合得到位置信息的最優(yōu)估 計值，具體方法為： 先由表達式:x(ti+ Δ ti) = X(ti+ Δ ti I to)+Kg(ti+ Δ ti) (Z(ti+ Δ ti)-HX(ti+ Δ ti I to))得 到當前時刻位置信息最優(yōu)估計值XU1+Δ t〇，再由表達式:PU1+Δ t〇 = (I-KgU1+Δ ")Η)Ρ (ti+Δ ti| to)更新當前時刻位置信息最優(yōu)估計值X(ti+A ti)的協(xié)方差P(ti+A ti)，將當前時 刻位置信息最優(yōu)估計值X(ti+A ti)作為估計下一時刻位置信息預測值的參考值，Z(ti+A t〇為當前時刻位置信息的測量值，I為單位矩陣，Kg(WAt 1)為卡爾曼增益，KgU1+At1) = Pft.! + Λ t.! I t. η?Η - T"HPft. ! + Λt. ! I t. η?Η - T + m 測骨蒸絲的暴數(shù)矩陣， 4 .根據(jù)權(quán)利要求3所述輸出速率可調(diào)的視覺導航最優(yōu)信息估計方法，其特征在于，數(shù)據(jù) 輸出線程輸出時刻間隔接近圖像處理時間的平均值或眾數(shù)。5.實現(xiàn)權(quán)利要求1所述視覺導航最優(yōu)信息估計方法的系統(tǒng)，其特征在于，包括： 根據(jù)攝像頭采集的圖像預測位置信息的圖像處理模塊，以及， 在輸出時刻到來時根據(jù)當前位置信息進行最優(yōu)估計并輸出當前輸出時刻位置信息的 濾波預測輸出模塊。
【文檔編號】G01C21/20GK105890596SQ201610192247
【公開日】2016年8月24日
【申請日】2016年3月30日
【發(fā)明人】李旺靈, 孫永榮, 黃斌, 王勇, 劉建業(yè), 吳玲, 熊智, 朱云峰
【申請人】南京航空航天大學