一種基于回溯搜索的無(wú)人機(jī)空中加油位姿估計(jì)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于回溯搜索的無(wú)人機(jī)空中加油位姿估計(jì)方法，屬于無(wú)人機(jī)自主 控制領(lǐng)域。
【背景技術(shù)】
[0002] 無(wú)人機(jī)（Unmanned Aerial Vehicle, UAV)自走上歷史舞臺(tái)以來(lái)已在軍事民用方面 得到了廣泛的應(yīng)用，其在軍事上的作用更是隨著軍事化需求的不斷升級(jí)而越來(lái)越重要。無(wú) 人機(jī)自主空中加油（Autonomous Aerial Refueling, ARR)技術(shù)可以顯著提升無(wú)人機(jī)的作戰(zhàn) 效能和作戰(zhàn)機(jī)動(dòng)性，已逐漸成為無(wú)人機(jī)的必備能力之一，是未來(lái)無(wú)人機(jī)技術(shù)發(fā)展的重要方 向。實(shí)現(xiàn)無(wú)人機(jī)自主空中加油的關(guān)鍵是實(shí)現(xiàn)受油機(jī)和加油機(jī)之間相對(duì)位置與姿態(tài)的準(zhǔn)確實(shí) 時(shí)測(cè)量，并有效控制受油機(jī)和加油機(jī)，從而實(shí)現(xiàn)自主加油精確導(dǎo)引，保證整個(gè)加油過(guò)程的順 利進(jìn)行。為使受油機(jī)和加油機(jī)能夠安全無(wú)碰撞地進(jìn)行緊密對(duì)接，其相對(duì)位姿信息的準(zhǔn)確性 顯得尤為重要，是整個(gè)過(guò)程中進(jìn)行飛行控制的基礎(chǔ)。
[0003] 基于計(jì)算機(jī)視覺(jué)的無(wú)人機(jī)位姿測(cè)量近年來(lái)成為了無(wú)人機(jī)導(dǎo)航領(lǐng)域的一大研究熱 點(diǎn)，并逐漸為該問(wèn)題提供切實(shí)可行的解決方法。與傳統(tǒng)的測(cè)量方法相比，視覺(jué)測(cè)量?jī)r(jià)格低、 功耗低、提供的信息量大，可遠(yuǎn)離電磁對(duì)抗的頻率范圍，具有很強(qiáng)的應(yīng)用現(xiàn)實(shí)性?；谟?jì)算 機(jī)視覺(jué)的無(wú)人機(jī)位姿測(cè)量系統(tǒng)主要由三部分組成：目標(biāo)檢測(cè)、特征點(diǎn)提取以及位姿估計(jì)。本 專(zhuān)利主要針對(duì)無(wú)人機(jī)空中加油的位姿估計(jì)模塊，提出一種基于回溯搜索的估計(jì)方法。
[0004] 透視η點(diǎn)定位（Perspective-n-Point，PnP)問(wèn)題起源于攝像機(jī)標(biāo)定，其根據(jù)空間 中η點(diǎn)在攝像機(jī)模型下的2維與3維映射關(guān)系，以及η點(diǎn)在參考坐標(biāo)系和相機(jī)坐標(biāo)系下的 不同坐標(biāo)表示，來(lái)決定攝像機(jī)的位置與方位，是目前計(jì)算機(jī)視覺(jué)領(lǐng)域解決位姿估計(jì)問(wèn)題的 常用方法。Shiqi Li等人提出的魯棒透視11點(diǎn)（Robust Perspective-n_Point，RPnP)算法 為PnP問(wèn)題提供了一個(gè)魯棒的解析解，適用面廣，能在普通3維點(diǎn)、共面點(diǎn)、擬奇異點(diǎn)等多種 空間特征點(diǎn)結(jié)構(gòu)下進(jìn)行位姿估計(jì)。
[0005] 回溯搜索算法是由Pinar Civicioglu于2013年提出的一種新型的基于群體智能 的啟發(fā)式優(yōu)化算法。該算法繼承了差分進(jìn)化算法的總體框架，結(jié)構(gòu)簡(jiǎn)單，但搜索過(guò)程中的變 異和交叉操作又與差分進(jìn)化算法有著本質(zhì)的區(qū)別，其通過(guò)產(chǎn)生歷史種群并控制搜索方向和 邊界，大大地提高了算法的優(yōu)化效率。在該算法的迭代過(guò)程中，歷史種群能夠不斷設(shè)置為本 次迭代之前的種群位置，從而實(shí)現(xiàn)記憶功能。在歷史種群的基礎(chǔ)上，回溯搜索算法將兩個(gè)基 本算子結(jié)合起來(lái)解決優(yōu)化問(wèn)題。
[0006] (1)變異算子
[0007] 不同于其他的優(yōu)化算法，在種群的變異過(guò)程中，歷史種群P°ld的位置對(duì)搜索方向起 到了決定性的作用，具體的更新規(guī)則如下式所示：
[0008] M = P+F (Pold-P) (1)
[0009] 式中，P和M分別是當(dāng)前種群位置和變異后的種群位置，變異尺度系數(shù)F的大小控 制了在（p° ld-p)方向上搜索的幅度。
[0010] ⑵交叉算子
[0011] 交叉算子是在變異后的種群和當(dāng)前種群之間進(jìn)行的操作，為了更好地保持種群多 樣性，提高算法跳出局部最優(yōu)的能力，回溯搜索算法通過(guò)定義一個(gè)map矩陣來(lái)控制種群間 實(shí)際進(jìn)行交叉的個(gè)體個(gè)數(shù)，具體步驟如下式所示：
[0013] 式中的map為一個(gè)取值為0或1的二元矩陣，該矩陣的生成過(guò)程為：首先將所有元 素初始化為1，再按照如下方式進(jìn)行隨機(jī)初始化：
[0015] 式中，D為種群的維數(shù)，rnd為（0, 1)區(qū)間均勻分布的隨機(jī)數(shù)，mr為混合比例參數(shù)； u為向量[1，2，...，D]進(jìn)行隨機(jī)排序后的整數(shù)向量，代表個(gè)體維數(shù)的索引。當(dāng)隨機(jī)數(shù)a〈b 時(shí)，:「/wwm/·/) i)選出向量u中排在前「嫌個(gè)的元素，對(duì)map^量相應(yīng)的維數(shù) 位置進(jìn)行置零操作；當(dāng)a多b時(shí)，量?jī)H在randi (D)維上為0,其余元素均為初始化的 值1。
[0016] 進(jìn)行完上述兩個(gè)基本算子后，回溯搜索算法將采用貪婪選擇策略在交叉后的種群 T與當(dāng)前種群P中選擇出適應(yīng)度更高的個(gè)體作為新的種群個(gè)體，再根據(jù)一定的規(guī)則更新歷 史種群P°ld，進(jìn)入下一輪的迭代優(yōu)化，直到滿足停止迭代的條件。回溯搜索算法的整體流程 圖如圖1所示。

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

[0017] 1、發(fā)明目的：
[0018] 本發(fā)明提出了一種基于回溯搜索的無(wú)人機(jī)空中加油位姿估計(jì)方法，其目的是提供 一種基于視覺(jué)測(cè)量的位姿估計(jì)方法，為無(wú)人機(jī)空中加油的精確導(dǎo)引提供一種解決方案，以 彌補(bǔ)其他測(cè)量方法在某些情況下的不足，提高無(wú)人機(jī)位姿測(cè)量的魯棒性和精度，為空中加 油的順利實(shí)現(xiàn)提供有效保障。
[0019] 該方法利用Matlab軟件編寫(xiě)相應(yīng)的位姿估計(jì)算法程序，利用Simulink三維視景 模擬整個(gè)空中加油環(huán)境，通過(guò)仿真得到空中加油過(guò)程中加油機(jī)和受油機(jī)之間相對(duì)六自由度 位姿信息的測(cè)量誤差，在此誤差基礎(chǔ)上構(gòu)建優(yōu)化問(wèn)題目標(biāo)函數(shù)，利用回溯搜索算法求解出 最優(yōu)的姿態(tài)估計(jì)值。
[0020] 2、技術(shù)方案：
[0021] 本發(fā)明利用基于群體智能的回溯搜索算法的全局搜索能力強(qiáng)，收斂速度快等特 點(diǎn)，開(kāi)發(fā)一種基于回溯搜索的無(wú)人機(jī)空中加油位姿估計(jì)方法，該方法的具體步驟如下：
[0022] 步驟一：獲取進(jìn)行位姿估計(jì)所需信息
[0023] 本方法中用透視η點(diǎn)定位解法進(jìn)行位姿估計(jì)，因此需要獲取參與位姿估計(jì)運(yùn)算的 所有特征點(diǎn)的信息，包括特征點(diǎn)的總個(gè)數(shù)，這些特征點(diǎn)在受油機(jī)坐標(biāo)系下的位置信息、在安 裝在加油機(jī)上的攝像機(jī)中的圖像坐標(biāo)值以及兩者的對(duì)應(yīng)關(guān)系，獲取攝像機(jī)的內(nèi)參信息等。
[0024] 步驟二：初始化回溯搜索算法參數(shù)
[0025] (1)初始化優(yōu)化參數(shù)維數(shù)D
[0026] 本方法中用群體智能算法在所有的特征點(diǎn)中尋找兩個(gè)最優(yōu)的特征點(diǎn)來(lái)建立正交 坐標(biāo)系，因此D設(shè)置為2。
[0027] (2)初始化種群數(shù)量N
[0028] 種群數(shù)量N與算法的優(yōu)化效果關(guān)系緊密，較大的種群數(shù)量可能加速優(yōu)化算法的收 斂速度，但同時(shí)也會(huì)增加計(jì)算復(fù)雜度，根據(jù)實(shí)際問(wèn)題選擇一個(gè)適當(dāng)?shù)姆N群數(shù)量，兼顧算法優(yōu) 化的準(zhǔn)確性和效率。
[0029] (3)初始化種群P和歷史種群Fld位置
[0030] 由于回溯搜索算法中初始值對(duì)優(yōu)化性能的影響較小，故在定義好的搜索空間中隨 機(jī)產(chǎn)生種群個(gè)體，以初始化種群和歷史種群。low為搜索空間的下限，up為搜索空間的上 限，則種群和歷史種群中個(gè)體的每一維被初始化為：
[0032] 式中，i e [1，2,3，···Ν]，j e [1，2,3，...，D]，U 是隨機(jī)均勻分布函數(shù)。
[0033] (4)設(shè)置算法重要參數(shù)
[0034] 對(duì)于隨機(jī)搜索算法，一些重要參數(shù)的設(shè)置會(huì)影響算法的優(yōu)化性能，本方法中，變異 尺度系數(shù)F的大小直接控制了變異算子操作中的搜索幅度，混合比例參數(shù)mr將影響種群間 交叉的個(gè)體個(gè)數(shù)。
[0035] (5)設(shè)置算法迭代次數(shù)NC
[0036] 算法迭代次數(shù)將直接影響算法的優(yōu)化結(jié)果，設(shè)置算法的迭代次數(shù)應(yīng)考慮算法對(duì)于 實(shí)際問(wèn)題的平均收斂速度，以及應(yīng)用中對(duì)于算法優(yōu)化性能和時(shí)間的要求。
[0037] 步驟三：設(shè)計(jì)適應(yīng)度函數(shù)
[0038] 適應(yīng)度函數(shù)是群智能優(yōu)化方法的核心，決定優(yōu)化結(jié)果的實(shí)用性。本方法中，適應(yīng)度 函數(shù)直接設(shè)計(jì)為估計(jì)出的位姿結(jié)果與真值之間的誤差，如下式所示：
[0040] 式中，！\和R [R u R12 R13]分別為第i個(gè)個(gè)體所對(duì)應(yīng)的平移向量和旋轉(zhuǎn)矩陣的 估計(jì)值，T。和R。= [Rm Rra R1J分別為平移向量和旋轉(zhuǎn)矩陣的真實(shí)值。
[0041] 步驟四：利用RPnP算法進(jìn)行位姿估計(jì)
[0042] 種群中的每一個(gè)個(gè)體分別代表了兩個(gè)不同的特征點(diǎn)，基于該兩點(diǎn)組成的線段 ，可建立一個(gè)正交坐標(biāo)系O aXaYaZa,建立方法如圖2所示：定義^；的中點(diǎn)作為該坐 標(biāo)系的原點(diǎn)oa，且Zjft的正方向與巧巧；;方向相同。建立好正交坐標(biāo)系后，將所有特征點(diǎn)的 坐標(biāo)值從受油機(jī)坐標(biāo)系轉(zhuǎn)換到該坐標(biāo)系下，則加油機(jī)相對(duì)于受油機(jī)的相對(duì)位姿估計(jì)轉(zhuǎn)換為 相機(jī)坐標(biāo)系相對(duì)于坐標(biāo)系OaXaYaZa的位姿解算問(wèn)題。
[0043] 為求解該問(wèn)題，將η個(gè)特征點(diǎn)分為（η-2)個(gè)子集，每一個(gè)子集包括三個(gè)不同的特征 點(diǎn)，如{Pi〇Pj〇P klk乒i〇, k乒jO}，根據(jù)三點(diǎn)約束，可得到如下多項(xiàng)式：
[0045] 求解上述多項(xiàng)式