基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法
1.本發明屬于無人機技術領域，具體涉及一種無人機跟蹤地面目標半物理仿真平臺驗證方法。
背景技術：
2.無人機良好的隱身性、機動性和跟蹤能力使其廣泛應用于偵察監視、搜救等領域。無人機在執行敵情偵察、視覺導航、目標檢測和目標跟蹤等任務時，視覺系統是其中不可缺少的一部分。
3.無人機的視覺系統由云臺、可見光及不可見光攝像機，以及圖像傳輸設備等部分組成，主要用于對地面目標的檢測，定位和跟蹤。基于視覺的戰場目標自動檢測與跟蹤技術系統已成為無人機對戰場實現態勢感知與精準打擊的基本手段。無人機的視覺系統對戰場環境進行圖像采集，通過對圖像進行一定的預處理、感興趣區域分析和目標特征提取，實現對感興趣目標的自動檢測和跟蹤，并計算出目標的位置，為攻擊武器提供目標位置等信息以實現跟蹤和精確打擊。
4.但是在無人機跟蹤地面目標的過程中，無人機容易受到多種因素的影響，如天氣環境、風和傳感器故障等。此外，由于無人機飛行速度快、起降困難和空域開放限制，實際試飛成本高、難度大且危險性大。因此，如何降低實驗成本，有效開展實驗是一個亟待解決的問題。
技術實現要素：
5.為了克服現有技術的不足，本發明提供了一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，該平臺由上位機、實時仿真計算機、自駕儀和圖像處理板卡組成。上位機運行仿真軟件vegaprime(vp)，顯示無人機的運動狀態；利用vp開發視景顯示系統，可以直觀地顯示無人機動力學特性，并模擬攝像機拍攝，通過udp輸出視頻流信息給圖像處理板卡；圖像處理板卡利用yolov5算法和eco算法檢測和跟蹤目標，計算目標位置，利用串口輸入到自駕儀。自駕儀根據目標位置利用協調轉彎制導律進行控制律解算，發送控制指令到實時仿真計算機，實時仿真計算機運行simulink模型，將無人機及目標的姿態位置信息發送到上位機，驅動視景更新，從而完成閉環仿真。本發明仿真平臺操作簡單、成本低、仿真還原度高。
6.本發明解決其技術問題所采用的技術方案包括如下步驟：
7.步驟1：所述半物理仿真平臺的硬件部分包括上位機、自駕儀、圖像處理板卡和實時仿真計算機；所述上位機與圖像處理板卡電連接，圖像處理板卡與自駕儀電連接，自駕儀與實時仿真計算機電連接，實時仿真計算機與上位機電連接，形成一個閉環系統；
8.所述半物理仿真平臺的軟件部分包括視景顯示系統、目標檢測定位系統、無人機控制系統和simulink模型系統；
9.所述視景顯示系統運行在上位機中，用于模擬攝像機進行拍攝并輸出視頻流；
10.所述目標檢測定位系統運行在圖像處理板卡中，用于跟蹤檢測地面目標并計算目標位置；
11.所述無人機控制系統運行在自駕儀中，用于根據目標定位結果控制無人機飛行，引導無人機飛向目標；
12.所述simulink模型系統運行在實時仿真計算機中，用于無人機模型的搭建；
13.步驟2：視景顯示系統輸出圖片序列到目標檢測定位系統；
14.步驟2-1：在simulink模型系統的無人機仿真模型底部設置transform模塊模擬云臺，通過設置transform模塊的俯仰角和水平旋轉角模擬云臺的俯仰角和水平旋轉；視景顯示系統中的主窗口從transform模塊的角度顯示場景；無人機的運動和云臺的俯仰與水平旋轉改變仿真攝像機的視場，視景顯示系統中的主窗口顯示不同的場景，主窗口的屏幕不斷保存，以模擬攝像機的圖像捕獲；
15.步驟2-2：視景顯示系統采用3d仿真軟件vege prime作為開發平臺；包括：視景更新模塊，接收simulink模型系統發送的無人機的狀態量，驅動視景更新；圖像捕獲模塊，采用window句柄方式截取視景顯示系統顯示的圖像，模擬攝像機的拍攝，并進行圖片壓縮；視景顯示系統不斷保存圖片序列，發送給圖像處理板卡；
16.步驟2-3：求解視景顯示系統的內參矩陣；
17.步驟2-3-1：在視景顯示系統中設置無人機、汽車和云臺的位置及姿態信息，獲取一張對應的圖片；汽車在圖像中的位置通過人工標記獲得，視景顯示系統的內參矩陣共有5個參數，標記一張圖片獲得兩個方程，利用三張圖片求解出內參矩陣m，再使用最大似然估計繼續進行優化；
18.步驟2-3-2：世界坐標系和像素坐標系之間的關系用旋轉矩陣ri和平移向量t表示，假設得到不同條件下的n幅圖片，每幅圖片上均有一個汽車的像點，因此目標圖像模型表示為：
[0019][0020]
式中：p
t,i
表示第i幅圖片上汽車像點的實際坐標，表示計算得到的p
t,i
在圖像上的坐標，mi表示第i幅圖像對應的內參數；
[0021]
定義p
f,i
為第i幅圖片上汽車的實際像素點坐標，則像點p
f,i
的概率密度函數為：
[0022][0023]
式中，σ表示標準差參數；
[0024]
構造似然函數：
[0025][0026]
為了能夠讓l取得最大值，將問題轉化為：
[0027][0028]
式(3)是一個非線性優化問題，使用levenberg-marquardt方法進行優化，利用人工標記作為初始值，不斷進行迭代得到內參矩陣最優解；
[0029]
步驟3：目標檢測定位系統檢測圖片序列中的地面目標即汽車，將目標位置信息發送到無人機控制系統；
[0030]
步驟3-1：使用yolov5算法進行目標檢測，隨后利用eco圖像跟蹤算法對目標進行持續跟蹤，得到目標在圖像中的像素位置；目標的像素位置使用矩形框進行描述，而目標所在矩形框中心點的像素坐標則作為汽車的像素坐標；
[0031]
步驟3-2：根據無人機的位置和姿態以及圖像保存時云臺的姿態，利用目標圖像模型計算目標在世界坐標系中的位置；
[0032]
無人機在跟蹤過程中，攝像機和云臺的位置重合，跟隨云臺轉動；目標的位置p
t
＝[x
t
,y
t
,z
t
]
t
表示為：
[0033]
p
t
＝p1+p2+p3????????????????????????????
(5)
[0034]
式中p1＝[x1,y1,z1]
t
為無人機在世界坐標系下的位置；p2＝[x2,y2,z2]
t
為無人機質心到攝像機光心的距離，由式(6)得到
[0035][0036]
式中為攝像機在機體坐標系下的位置，為機體坐標系到世界坐標系的轉換矩陣；
[0037]
向量p3表示攝像機到目標的距離，滿足如下關系：
[0038][0039]
式中為目標在攝像機坐標系下的位置，zc為圖像深度，表示攝像機的光心沿光軸到目標的距離，為攝像機坐標系到機體坐標系的轉換矩陣；
[0040]
式(5)轉化為：
[0041][0042]
在考慮攝像頭畸變的情況下，得到：
[0043][0044]
式中，θ表示攝像頭的畸變角度，f表示鏡頭的焦距。
[0045]
由式(8)和式(9)得目標圖像模型的表達式為：
[0046][0047]
式中，(u,v)表示目標圖像在像素坐標系中的位置。
[0048]
為了估計zc，假定地面高程地圖已知，則地面目標的海拔高度z
t
已知；現在設則代入式(10)得：
[0049][0050]
式中表示的第3行第j(j＝1,2,3)列元素，κ
31
,κ
32
,κ
33
分別表示κ的第3行第j(j＝1,2,3)列元素。
[0051]
則zc通過下式計算得到
[0052][0053]
獲得深度信息zc之后，式(10)右側的剩余變量通過傳感器量測獲得，則利用式(13)估算出目標的在世界坐標系下的位置：
[0054][0055]
式中和κ
ij
分別為和κ的第i行第j列元素；
[0056]
步驟3-3：將目標位置發送給無人機控制系統，用于計算飛行制導指令；
[0057]
步驟4：無人機控制系統接收到目標在世界坐標系下的位置，利用制導律生成控制指令，并通過控制律生成控制面偏轉指令；自動駕駛儀向實時仿真計算機發送控制命令；
[0058]
假設地面目標的位置在世界坐標系下的位置為(x
t
,y
t
,z
t
)，無人機在世界坐標系下的位置為(x,y,z)，假設無人機和地面目標均在固定高度上運動，因此僅考慮橫向二維平面；無人機與地面目標之間的水平距離為無人機與地面目標之間的相對運動關系：
[0059][0060]
其中η為側軸角，即橫向加速度方向與無人機目標中心連線的夾角，v為無人機的速度，a為控制輸入，即需要設計的制導律；控制目標就是無人機在制導律的作用下，在經過一段時間飛行后，收斂到期望的跟蹤圓上，即r
→
r，r為收斂半徑；
[0061]
無人機在圍繞目標以半徑r進行協調轉彎飛行時，所需的橫向加速度指令為：
[0062][0063]
引入相對距離偏差r＝ρ-r和側軸角η，得到如下的協調轉彎制導律：
[0064][0065]
其中k》0表示制導增益，ξ》0表示阻尼比；系統的運動(14)在制導律(16)的作用下是全局漸進收斂的，選取如下的李雅普諾夫候選函數：
[0066][0067]
對式(17)兩端同時求導得：等號僅在平衡點(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
；因此，在制導律式(17)的作用下，無人機在任意位置任意航向運動在經過一段時間的飛行均能收斂到期望的參考圓上；在獲取到橫向制導指令后，無法直接輸入到控制系統中，需要對制導指令轉化成滾轉角指令，轉化公式為：
[0068][0069]
φg為滾轉角指令，g為重力加速度，至此能進行對地面靜止目標的跟蹤仿真驗證；
[0070]
在跟蹤地面移動目標時，需要將制導律進行擴展，增加相對變量來修正制導律：
[0071][0072]
其中vr為無人機相對地面目標的移動速度，ηr為相對側軸角；ar垂直于vr，首先使用將ar轉化為垂直于v的加速命令a，再用式(18)即將加速度a轉化為滾轉角指令；
[0073]
步驟5：simulink模型系統運行無人機模型和目標模型，將無人機和目標的狀態信息發送給無人機控制系統，同時接收無人機控制系統發送的控制指令；實時仿真計算機將無人機和目標的狀態信息發送給視景顯示系統，驅動視景更新，從而實現閉環仿真。
[0074]
優選地，所述上位機與圖像處理板卡的通信方式為udp；自駕儀與實時仿真計算機的通信方式為串口；圖像處理板卡與自駕儀的通信方式為串口；上位機與實時仿真計算機的通信方式為udp。
[0075]
優選地，所述上位機為惠普工作站z240；所述自駕儀型號為dreambuilder01，處理器是stm32f429；所述圖像處理板卡，型號為jetson xavier nx；所述實時仿真計算機型號為研華生產的ipc-610h。
[0076]
優選地，所述無人機控制系統利用協調轉彎制導律生成控制指令，并通過pid控制律生成控制面偏轉指令。
[0077]
本發明的有益效果如下：
[0078]
本發明的一種基于視覺的無人機檢測跟蹤地面目標仿真平臺驗證方法，通過包
括：運行于上位機的視覺顯示系統和地面站、運行于圖像處理板卡的目標檢測定位系統、運行于自駕儀的無人機控制系統和運行于實時仿真計算機的simulink模型系統，實現了無人機跟蹤地面目標的仿真驗證，有效降低了實驗成本，所得結果具有工程價值。本發明仿真平臺操作簡單、成本低、仿真還原度高，并且各個子系統采用模塊化設計，方便進行推廣和替換，有效解決了無人機跟蹤地面目標實驗難度高成本大的問題。
附圖說明
[0079]
圖1為本發明實仿真平臺的系統構成示意圖。
[0080]
圖2為本發明仿真平臺的整體構成示意圖。
[0081]
圖3為本發明實施例的視景顯示系統輸出示意圖。
[0082]
圖4為本發明仿真平臺的通信方式示意圖。
[0083]
圖5為本發明實施例提供的仿真平臺的目標圖像模型示意圖。
[0084]
圖6為本發明實施例提供的無人機跟蹤地面目標二維運動模型示意圖。
[0085]
圖7為本發明實施例提供的制導方法跟蹤靜止目標的數字仿真結果；(a)水平面運動軌跡；(b)水平面相對距離；(c)滾轉角指令與響應；(d)高度。
[0086]
圖8為本發明實施例提供的真實運動目標的數據。(a)目標實際運動軌跡；(b)目標實際運動速度。(a)水平面運動軌跡；(b)水平面相對距離；(c)滾轉角指令與響應；(d)高度。
[0087]
圖9為本發明實施例提供的制導方法跟蹤真實運動目標的數字仿真結果。
[0088]
圖10為本發明實施例提供的仿真平臺的運行示意圖。
[0089]
圖11為本發明實施例提供的仿真平臺的圖像跟蹤結果。
[0090]
圖12為本發明實施例提供的仿真平臺的直線運動仿真結果；(a)三維位置軌跡；(b)水平面位置軌跡；(c)相對距離；(d)舵面偏角；(e)制導指令和響應；(f)速度。
[0091]
圖13為本發明實施例提供的仿真平臺的曲線運動仿真結果；(a)三維位置軌跡；(b)水平面位置軌跡；(c)相對距離；(d)舵面偏角；(e)制導指令和響應；(f)速度。
具體實施方式
[0092]
下面結合附圖和實施例對本發明進一步說明。
[0093]
本發明實施例提出一種基于視覺的無人機檢測跟蹤地面目標半物理仿真平臺驗證方法，以解決無人機跟蹤地面目標實驗，由于無人機飛行速度快，起降難度大，空域管控等問題導致試飛實驗成本大，難以開展的問題。
[0094]
仿真平臺包括：視景顯示系統、目標檢測定位系統、無人機控制系統和simulink模型系統；
[0095]
視景顯示系統，用于直觀地研究飛機的動力學特性，模擬攝像機進行拍攝并輸出視頻流；
[0096]
目標檢測定位系統，用于跟蹤檢測地面目標并計算目標位置；
[0097]
無人機控制系統，用于根據目標定位結果控制無人機飛行，引導無人機飛向目標；
[0098]
simulink模型系統，用于無人機的數學模型的搭建，進行無人機六自由度非線性模型解算。
[0099]
無人機檢測跟蹤地面目標仿真平臺的硬件條件包括上位機，自駕儀，圖像處理板
卡，工控機(實時仿真計算機)；
[0100]
上位機，為惠普工作站z240，運行視景顯示系統和地面站；
[0101]
自駕儀，型號為dreambuilder01，處理器是stm32f429，運行無人機制導和控制系統；
[0102]
圖像處理板卡，型號為jetson xavier nx，運行目標檢測定位系統；
[0103]
工控機，型號為研華生產的ipc-610h，運行simulink模型系統，具有嚴格的實時解算能力。
[0104]
無人機檢測跟蹤地面目標仿真平臺中視覺仿真系統采用3d軟件vege prime進行開發，包括：視景更新模塊，使用udp接收simulink模型系統發送的無人機的狀態量，驅動視景更新；圖像捕獲模塊，采用window句柄方式截取視景軟件顯示的圖像，模擬攝像機的拍攝，并進行圖片壓縮，方便進行udp發送。
[0105]
目標檢測定位系統運行基于深度學習的目標檢測算法和基于相關濾波的目標跟蹤算法實現對目標的定位和跟蹤，并利用一種目標圖像模型，實現單目相機對地面目標的定位。
[0106]
無人機控制系統根據目標的位置，使用協調轉彎制導律和pid控制律，生成控制指令，實現無人機對目標的持續跟蹤。
[0107]
simulink模型系統采用matlab/xpc技術，在實時仿真計算機上啟動一個32位保護模式的實時內核，運行無人機的simulink模型。
[0108]
無人機檢測跟蹤地面目標仿真平臺中，上位機與圖像處理板卡的通信方式為udp；自駕儀與實時仿真計算機的通信方式為串口；圖像處理板卡與自駕儀的通信方式為串口，上位機與實時仿真計算機的通信方式為udp。
[0109]
目標圖像模型采用地形匹配方式獲取圖像深度信息，并實現對視景顯示系統中虛擬攝像機的標定。
[0110]
仿真平臺搭建：
[0111]
仿真系統由視景顯示系統、目標檢測定位系統、無人機控制系統和simulink模型系統組成。系統的結構如圖1所示。整個跟蹤系統的示意圖如圖2所示。
[0112]
視景顯示系統采用3d仿真軟件vege prime作為開發平臺。在仿真無人機底部設置了一個transform模塊來模擬云臺。通過設置transform模塊的俯仰角和水平旋轉角來模擬云臺的俯仰角和水平旋轉。視景仿真系統中的主窗口從transform模塊的角度顯示場景。無人機的運動和云臺的俯仰與水平旋轉將改變攝像機的視場，主窗口將顯示不同的場景。主窗口的屏幕不斷保存，以模擬攝像機的圖像捕獲。視景顯示系統的輸出如圖3所示。
[0113]
目標跟蹤定位系統運行在jetson xavier nx硬件平臺上。首先使用yolov5算法進行目標檢測，隨后利用eco圖像跟蹤算法對感興趣的目標進行持續跟蹤。一旦eco開始跟蹤，它將得到目標在圖像中的像素位置。目標的像素位置使用矩形框進行描述，而目標所在矩形框中心點的像素坐標則作為汽車的像素坐標。然后，根據無人機的位置和姿態以及圖像保存時云臺的姿態，利用目標圖像模型計算目標在世界坐標系中的位置。隨后將目標位置發送給無人機控制系統，用于計算飛行制導指令。為了更好的訓練yolov5網絡，本發明實例收集了2347張含有航拍汽車的圖片，標記了共34900輛汽車。訓練出的yolov5網絡可以對航拍汽車進行有效的檢測，航拍車輛檢測準確率達到67.7％，召回率達到75.5％。
1)轉化為
[0131][0132]
在考慮攝像頭畸變的情況下，可得
[0133][0134]
式中m表示相機的內參矩陣，需要進行標定。
[0135]
由式(1-4)和式(1-5)可得目標圖像模型的表達式為
[0136][0137]
圖像深度zc通常是未知的。為了估計zc，假定地面高程地圖已知，則地面目標的海拔高度z
t
已知。現在設則代入式(1-6)得
[0138][0139]
則zc通過下式計算得到
[0140][0141]
獲得深度信息zc之后，式(1-6)右側的剩余變量可通過傳感器量測獲得，則利用式(1-9)可估算出目標的在世界坐標系下的位置。
[0142][0143]
式中和κ
ij
分別為和κ的第i行第j列元素。
[0144]
攝像機標定：
[0145]
為了利用式(1-6)計算目標位置，首先需要對視景軟件的內參矩陣進行標定。實際工程中，最常用的標定方法是張正友提出的基于單平面棋盤格的標定方法。該方法只需利用攝像機拍攝若干張棋盤格照片，步驟簡單，實用性較強。但是在仿真系統中，視景系統是使用了虛擬攝像機來模擬真實攝像機的拍攝，無法拍攝棋盤格，則張正友標定法無法應用。受到張正友標定的啟發，本發明提出一種精確可靠的標定方法，可直接標定視景仿真軟件中的內參矩陣。
[0146]
目標圖像模型如式(1-6)所示。通過在視景軟件中設置無人機、汽車和云臺的位置和姿態信息，可獲得一張對應的圖片。汽車在圖像中的位置通過人工標記獲得，深度信息zc的解算如式(1-8)，因此，式(1-6)中僅有內參矩陣未知。內參矩陣共有5個參數，標記一張圖片獲得兩個方程，因此利用與類似的三張圖片就可求解出內參矩陣m，但是這樣計算出的結果并沒有實際的物理意義，并且存在誤差。為進一步增加標定結果的可靠性，將使用最大似然估計來優化上面得到的結果。
[0147]
世界坐標系和像素坐標系之間的關系用一個旋轉矩陣r和平移向量t表示，假設得到不同條件下的n幅圖片，每幅圖片上均有一個汽車的像點，因此目標圖像模型表示為
[0148][0149]
式中：p
t,i
表示第i幅圖片上汽車像點的實際坐標，表示由式(1-10)計算得到的p
t,i
在圖像上的坐標，mi表示第i幅圖像對應的內參數。定義p
f,i
為第i幅圖片上汽車的實際像素點坐標，則像點p
f,i
的概率密度函數為
[0150][0151]
構造似然函數
[0152][0153]
為了能夠讓l取得最大值，將問題轉化為
[0154][0155]
式(1-13)是一個非線性優化問題，將使用levenberg-marquardt方法進行優化。利用最開始標定得到的解作為初始值，不斷進行迭代得到最優解。
[0156]
視景仿真軟件分辨率設置為1280
×
960，橫向和縱向的視場角均設置為60
°
，經過計算，模擬攝像機的內參矩陣的值為
[0157][0158]
至此可以獲取地面目標的位置，假設地面目標的位置在世界坐標系下的位置為(x
t
,y
t
,z
t
)，無人機在世界系下的位置為(x,y,z)，假設無人機和地面目標均在固定高度上運動，因此僅考慮橫向二維平面。無人機與地面目標之間的水平距離為無人機與地面目標之間的相對運動關系如圖6所示：
[0159][0160]
其中η為側軸角，橫向加速度方向與無人機目標中心連線的夾角，v為無人機的速度，a為控制輸入，即需要設計的制導律。控制目標就是無人機在制導律的作用下，在經過一段時間飛行后，收斂到期望的跟蹤圓上，即r
→
r，r為收斂半徑。
[0161]
無人機在圍繞目標以半徑r進行協調轉彎飛行時，所需的橫向加速度指令為：
[0162][0163]
當無人機距離目標較遠時，這種指令不在適用。于是在協調轉彎運動的基礎上引入相對距離偏差r＝ρ-r和側軸角η，提出如下的協調轉彎制導律：
[0164][0165]
其中k》0表示制導增益，ξ》0表示阻尼比。相對距離偏差d的引入可以消除跟蹤靜差，側軸角η的引入可以增加系統阻尼，減少收斂時的超調量。系統的運動(1-15)在制導律(1-17)的作用下是全局漸進收斂的，選取如下的李雅普諾夫候選函數：
[0166][0167]
對(1-18)兩端同時求導得：等號僅在平衡點(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
。因此，在制導律(1-18)的作用下，無人機在任意位置任意航向運動在經過一段時間的飛行均可收斂到期望的參考圓上。在獲取到橫向制導指令后，無法直接輸入到控制系統中，需要對制導指令轉化成滾轉角指令，轉化公式為：
[0168][0169]
φg為滾轉角指令，g為重力加速度，至此可以進行對地面靜止目標的跟蹤仿真驗證。
[0170]
圖7顯示了無人機對于地面靜止目標跟蹤的數字仿真結果，可以看到無人機可以快速穩定地收斂到跟蹤圓上。
[0171]
在跟蹤地面移動目標時，需要將制導律進行擴展，增加相對變量來修正制導律：
[0172][0173]
其中vr為無人機相對地面目標的移動速度，ηr為相對側軸角。ar垂直于vr，因此，需要首先使用將ar轉化為垂直于v的加速命令a，再用式(1-19)即可將加速度a轉化為滾轉角指令。
[0174]
選取法國瓦爾德馬恩總部門委員會收集的真實數據集，這組數據集中地面車輛的運動是多種多樣的，包括高速運動、低速運動和動-停-轉運動，而且由于實際數據是在早高峰和晚高峰時段采集的，受交通擁堵和紅綠燈的影響，汽車有時行駛緩慢，有時甚至會停下來，如圖8所示。
[0175]
利用制導律(1-20)來跟蹤真實移動目標，數字仿真結果如圖9所示，制導律仍可實現對目標的穩定跟蹤，且無人機的飛行狀態保持平穩。至此，可以形成整個仿真平臺的閉環控制。
[0176]
仿真平臺的運行場景如圖10所示。在半物理仿真時，vp視景仿真軟件不斷輸出視頻流信號，jetson xavier nx運行yolov5檢測算法對視頻流信號進行檢測，一旦發現目標，無人機將使用eco算法和制導律對目標進行跟蹤。在仿真過程中，跟蹤算法的部分輸出如圖11所示，綠框表示跟蹤算法eco的輸出。從結果可以看出，eco算法可以較為準確的定位汽車在圖片中的位置。
[0177]
圖12顯示了跟蹤直線運動目標的半物理仿真結果，直線運動的目標在東向和北向的速度分量分別為8m/s和8m/s。
[0178]
圖13顯示了跟蹤變速變方向運動目標的半物理仿真的跟蹤結果，目標的運動軌跡為一個正弦曲線。
技術特征：
1.一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，其特征在于，包括如下步驟：步驟1：所述半物理仿真平臺的硬件部分包括上位機、自駕儀、圖像處理板卡和實時仿真計算機；所述上位機與圖像處理板卡電連接，圖像處理板卡與自駕儀電連接，自駕儀與實時仿真計算機電連接，實時仿真計算機與上位機電連接，形成一個閉環系統；所述半物理仿真平臺的軟件部分包括視景顯示系統、目標檢測定位系統、無人機控制系統和simulink模型系統；所述視景顯示系統運行在上位機中，用于模擬攝像機進行拍攝并輸出視頻流；所述目標檢測定位系統運行在圖像處理板卡中，用于跟蹤檢測地面目標并計算目標位置；所述無人機控制系統運行在自駕儀中，用于根據目標定位結果控制無人機飛行，引導無人機飛向目標；所述simulink模型系統運行在實時仿真計算機中，用于無人機模型的搭建；步驟2：視景顯示系統輸出圖片序列到目標檢測定位系統；步驟2-1：在simulink模型系統的無人機仿真模型底部設置transform模塊模擬云臺，通過設置transform模塊的俯仰角和水平旋轉角模擬云臺的俯仰角和水平旋轉；視景顯示系統中的主窗口從transform模塊的角度顯示場景；無人機的運動和云臺的俯仰與水平旋轉改變仿真攝像機的視場，視景顯示系統中的主窗口顯示不同的場景，主窗口的屏幕不斷保存，以模擬攝像機的圖像捕獲；步驟2-2：視景顯示系統采用3d仿真軟件vege prime作為開發平臺；包括：視景更新模塊，接收simulink模型系統發送的無人機的狀態量，驅動視景更新；圖像捕獲模塊，采用window句柄方式截取視景顯示系統顯示的圖像，模擬攝像機的拍攝，并進行圖片壓縮；視景顯示系統不斷保存圖片序列，發送給圖像處理板卡；步驟2-3：求解視景顯示系統的內參矩陣；步驟2-3-1：在視景顯示系統中設置無人機、汽車和云臺的位置及姿態信息，獲取一張對應的圖片；汽車在圖像中的位置通過人工標記獲得，視景顯示系統的內參矩陣共有5個參數，標記一張圖片獲得兩個方程，利用三張圖片求解出內參矩陣m，再使用最大似然估計繼續進行優化；步驟2-3-2：世界坐標系和像素坐標系之間的關系用旋轉矩陣r
i
和平移向量t表示，假設得到不同條件下的n幅圖片，每幅圖片上均有一個汽車的像點，因此目標圖像模型表示為：式中：p
t,i
表示第i幅圖片上汽車像點的實際坐標，表示計算得到的p
t,i
在圖像上的坐標，m
i
表示第i幅圖像對應的內參數；定義p
f,i
為第i幅圖片上汽車的實際像素點坐標，則像點p
f,i
的概率密度函數為：式中，σ表示標準差參數；
構造似然函數：為了能夠讓l取得最大值，將問題轉化為：式(3)是一個非線性優化問題，使用levenberg-marquardt方法進行優化，利用人工標記作為初始值，不斷進行迭代得到內參矩陣最優解；步驟3：目標檢測定位系統檢測圖片序列中的地面目標即汽車，將目標位置信息發送到無人機控制系統；步驟3-1：使用yolov5算法進行目標檢測，隨后利用eco圖像跟蹤算法對目標進行持續跟蹤，得到目標在圖像中的像素位置；目標的像素位置使用矩形框進行描述，而目標所在矩形框中心點的像素坐標則作為汽車的像素坐標；步驟3-2：根據無人機的位置和姿態以及圖像保存時云臺的姿態，利用目標圖像模型計算目標在世界坐標系中的位置；無人機在跟蹤過程中，攝像機和云臺的位置重合，跟隨云臺轉動；目標的位置p
t
＝[x
t
,y
t
,z
t
]
t
表示為：p
t
＝p1+p2+p3????????????????????????????
(5)式中p1＝[x1,y1,z1]
t
為無人機在世界坐標系下的位置；p2＝[x2,y2,z2]
t
為無人機質心到攝像機光心的距離，由式(6)得到式中為攝像機在機體坐標系下的位置，為機體坐標系到世界坐標系的轉換矩陣；向量p3表示攝像機到目標的距離，滿足如下關系：式中為目標在攝像機坐標系下的位置，z
c
為圖像深度，表示攝像機的光心沿光軸到目標的距離，為攝像機坐標系到機體坐標系的轉換矩陣；式(5)轉化為：在考慮攝像頭畸變的情況下，得到：
式中，θ表示攝像頭的畸變角度，f表示鏡頭的焦距；由式(8)和式(9)得目標圖像模型的表達式為：式中，(u,v)表示目標圖像在像素坐標系中的位置；為了估計z
c
，假定地面高程地圖已知，則地面目標的海拔高度z
t
已知；現在設κ∈r3×3，則代入式(10)得：式中表示的第3行第j(j＝1,2,3)列元素，κ
31
,κ
32
,κ
33
分別表示κ的第3行第j(j＝1,2,3)列元素；則z
c
通過下式計算得到獲得深度信息z
c
之后，式(10)右側的剩余變量通過傳感器量測獲得，則利用式(13)估算出目標的在世界坐標系下的位置：式中和κ
ij
分別為和κ的第i行第j列元素；步驟3-3：將目標位置發送給無人機控制系統，用于計算飛行制導指令；步驟4：無人機控制系統接收到目標在世界坐標系下的位置，利用制導律生成控制指令，并通過控制律生成控制面偏轉指令；自動駕駛儀向實時仿真計算機發送控制命令；假設地面目標的位置在世界坐標系下的位置為(x
t
,y
t
,z
t
)，無人機在世界坐標系下的位置為(x,y,z)，假設無人機和地面目標均在固定高度上運動，因此僅考慮橫向二維平面；無人機與地面目標之間的水平距離為無人機與地面目標之間的相對運動關系：
其中η為側軸角，即橫向加速度方向與無人機目標中心連線的夾角，v為無人機的速度，a為控制輸入，即需要設計的制導律；控制目標就是無人機在制導律的作用下，在經過一段時間飛行后，收斂到期望的跟蹤圓上，即r
→
r，r為收斂半徑；無人機在圍繞目標以半徑r進行協調轉彎飛行時，所需的橫向加速度指令為：引入相對距離偏差r＝ρ-r和側軸角η，得到如下的協調轉彎制導律：其中k>0表示制導增益，ξ>0表示阻尼比；系統的運動(14)在制導律(16)的作用下是全局漸進收斂的，選取如下的李雅普諾夫候選函數：對式(17)兩端同時求導得：等號僅在平衡點(r,η)＝(r,2nπ),n＝0,
±
1,
±
2,
…
；因此，在制導律式(17)的作用下，無人機在任意位置任意航向運動在經過一段時間的飛行均能收斂到期望的參考圓上；在獲取到橫向制導指令后，無法直接輸入到控制系統中，需要對制導指令轉化成滾轉角指令，轉化公式為：φ
g
為滾轉角指令，g為重力加速度，至此能進行對地面靜止目標的跟蹤仿真驗證；在跟蹤地面移動目標時，需要將制導律進行擴展，增加相對變量來修正制導律：其中v
r
為無人機相對地面目標的移動速度，η
r
為相對側軸角；a
r
垂直于v
r
，首先使用將a
r
轉化為垂直于v的加速命令a，再用式(18)即將加速度a轉化為滾轉角指令；步驟5：simulink模型系統運行無人機模型和目標模型，將無人機和目標的狀態信息發送給無人機控制系統，同時接收無人機控制系統發送的控制指令；實時仿真計算機將無人機和目標的狀態信息發送給視景顯示系統，驅動視景更新，從而實現閉環仿真。2.根據權利要求1所述的一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，其特征在于，所述上位機與圖像處理板卡的通信方式為udp；自駕儀與實時仿真計算機的通信方式為串口；圖像處理板卡與自駕儀的通信方式為串口；上位機與實時仿真計算機
的通信方式為udp。3.根據權利要求1所述的一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，其特征在于，所述上位機為惠普工作站z240；所述自駕儀型號為dreambuilder01，處理器是stm32f429；所述圖像處理板卡，型號為jetson xavier nx；所述實時仿真計算機型號為研華生產的ipc-610h。4.根據權利要求1所述的一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，其特征在于，所述無人機控制系統利用協調轉彎制導律生成控制指令，并通過pid控制律生成控制面偏轉指令。
技術總結
本發明公開了一種基于視覺的無人機跟蹤地面目標半物理仿真平臺驗證方法，該平臺由上位機、實時仿真計算機、自駕儀和圖像處理板卡組成。上位機運行仿真軟件VegaPrime(VP)，顯示無人機的運動狀態；利用VP開發視景顯示系統，可以直觀地顯示無人機動力學特性，并模擬攝像機拍攝，通過UDP輸出視頻流信息給圖像處理板卡；圖像處理板卡利用YOLOv5算法和ECO算法檢測和跟蹤目標，計算目標位置，利用串口輸入到自駕儀。自駕儀根據目標位置利用協調轉彎制導律進行控制律解算，發送控制指令到實時仿真計算機，實時仿真計算機運行Simulink模型，將無人機及目標的姿態位置信息發送到上位機，驅動視景更新，從而完成閉環仿真。本發明仿真平臺操作簡單、成本低、仿真還原度高。仿真還原度高。仿真還原度高。
