用于農場自主管理的四旋翼飛行器控制系統(tǒng)及控制方法
【專利摘要】本發(fā)明涉及一種用于農場自主管理的四旋翼飛行器控制系統(tǒng)及控制方法，智能移動終端通過GPRS網(wǎng)絡將作業(yè)農場的范圍和設定入侵者信息送四旋翼飛行器，四旋翼飛行器通過搭載的GPS模塊獲取搜索區(qū)域位置執(zhí)行飛行任務，利用搭載的語音模塊播放刺激聲波對設定入侵者進行恐嚇。任務過程中四旋翼飛行器采集圖像數(shù)據(jù)并無線傳送至地面控制中心，地面控制中心通過圖像處理獲得四旋翼飛行器的實時位置及與入侵者的相對姿態(tài)數(shù)據(jù)，通過GPRS網(wǎng)絡將飛行引導數(shù)據(jù)送至四旋翼飛行器使之接近入侵者并播放刺激聲波，直至入侵者驅離作業(yè)農場范圍。用于實現(xiàn)非干預情況下識別、跟蹤和驅逐隱藏在作物下的鳥類?？蓮V泛用于大型農場、機場等需要自動驅趕鳥類等入侵物的場景。
【專利說明】
用于農場自主管理的四旋翼飛行器控制系統(tǒng)及控制方法
技術領域
[0001] 本發(fā)明涉及一種控制系統(tǒng)，特別涉及一種用于農場自主管理的四旋翼飛行器控制 系統(tǒng)及控制方法。
【背景技術】
[0002] 隨著科技和社會發(fā)展，傳統(tǒng)農業(yè)正加速轉型，將來會形成具有一定規(guī)模的農場。大 型農場存在鳥類、家畜等動物"入侵破壞"的現(xiàn)象，雇傭工人每天進行巡查存在著成本高、巡 視盲點多、巡視周期長等缺點。因此，采用四旋翼飛行器代替人工進行監(jiān)測、巡邏、干預和驅 逐，實現(xiàn)農場非干預管理是現(xiàn)代農業(yè)發(fā)展的必然趨勢。目前，國內尚無此類專利公開。

【發(fā)明內容】

[0003] 本發(fā)明是針對農場中存在的鳥類破壞作物的問題，提出了一種用于農場自主管理 的四旋翼飛行器控制系統(tǒng)及控制方法，用于實現(xiàn)非干預情況下識別、跟蹤和驅逐隱藏在作 物下的鳥類。
[0004] 本發(fā)明的技術方案為：一種用于農場自主管理的四旋翼飛行器控制系統(tǒng)，包括四 旋翼飛行器和遠程控制子系統(tǒng)，飛行控制子系統(tǒng)、偵測和驅逐子系統(tǒng)裝在四旋翼飛行器上， 飛行控制子系統(tǒng)包括嵌入式微控制器、2對螺旋槳電機、GPRS網(wǎng)絡、GPS模塊和慣性測量單 元，偵測和驅逐子系統(tǒng)包括攝像頭、語音模塊和5.8GHz圖像傳輸模塊，遠程控制子系統(tǒng)包括 圖像接收模塊、地面控制中心和智能移動終端；
[0005] 智能移動終端通過GPRS網(wǎng)絡將作業(yè)農場的范圍和設定入侵者信息送四旋翼飛行 器中嵌入式微控制器，攝像頭采集的圖像數(shù)據(jù)通過5.8GHz圖像傳輸模塊采用無線傳輸方式 送出，由圖像接收模塊接收后送到地面控制中心，地面控制中心通過圖像處理獲得四旋翼 飛行器的實時位置及設定入侵者的相對姿態(tài)數(shù)據(jù)，通過GPRS網(wǎng)絡將飛行引導數(shù)據(jù)送至四旋 翼飛行器，四旋翼飛行器中嵌入式微控制器控制2對螺旋槳電機進行飛行引導，四旋翼飛行 器通過GPS模塊獲取搜索區(qū)域位置送嵌入式微控制器，嵌入式微控制器送信號到偵測和驅 逐子系統(tǒng)，控制攝像頭進行采集圖像，控制語音模塊播放刺激聲波對設定入侵者進行恐嚇；
[0006] 慣性測量單元測量加速度及角速度送嵌入式微控制器，嵌入式微控制器根據(jù)濾波 和姿態(tài)解算處理后進行姿態(tài)融合并輸出PWM波到電機，以此來控制四個電機的轉速的方式 改變四旋翼的姿態(tài)，超聲波信號送嵌入式微控制器，嵌入式微控制器根據(jù)超聲波測距原理 控制飛行器在搜索目標時的飛行高度。
[0007] 所述嵌入式微控制器采用兩層雙串級PID控制進行目標跟蹤，位置PID作為外環(huán)， 姿態(tài)PID為內環(huán)，控制對象為電機轉速，期望位置是地面控制中心根據(jù)接收到的視頻數(shù)據(jù)計 算并發(fā)送的飛行引導數(shù)據(jù)，包含世界坐標系下被跟蹤目標與四旋翼飛行器的水平位移，期 望位置與實際位置反饋信號差值送外環(huán)PID，外環(huán)PID輸出與當前姿態(tài)比較后送姿態(tài)PID，姿 態(tài)PID輸出為跟蹤信號，進而通過控制四個電機的轉速來改變四旋翼自身的姿態(tài)，實現(xiàn)對目 標的連續(xù)跟蹤。
[0008] 所述嵌入式微控制器采用兩層雙串級PID控制進行作業(yè)域搜索，位置PID作為外 環(huán)，高度PID作為內環(huán)，搜索域中心點坐標作為期望位置，期望位置與實際位置反饋信號動 差送外環(huán)PID，外環(huán)PID輸出與當前高度比較后送高度PID，高度PID輸出為搜索跟蹤信號，不 斷修正四旋翼飛行器姿態(tài)，逐步接近期望位置。
[0009] 所述地面控制中心對設定入侵者進行識別和跟蹤方法包含以下步驟：
[0010]①：四旋翼飛行器機載攝像頭采集圖像并通過無線方式發(fā)送；
[0011] ②:遠程控制中心的地面站PC端軟件獲取步驟①的視頻圖像；
[0012] ③:進行圖像處理，實現(xiàn)對圖像的特征點提取及特征匹配；
[0013] ④：目標識別，利用幀差法提取運動目標并進行二值化及膨脹腐蝕處理；
[0014] ⑤：目標跟蹤，使用粒子濾波算法與強跟蹤Kalman濾波算法對目標進行跟蹤；
[0015]⑥:通過GPRS網(wǎng)絡把步驟⑤的跟蹤坐標發(fā)送給嵌入式微控制器；
[0016] ⑦:嵌入式微控制器接收步驟⑥的數(shù)據(jù)并控制電機的轉速來改變姿態(tài)，從而對運 動的目標連續(xù)跟蹤。
[0017] 本發(fā)明的有益效果在于:本發(fā)明用于農場自主管理的四旋翼飛行器控制系統(tǒng)及控 制方法，能自動識別、跟蹤和驅逐農場鳥類等"入侵者"包括隱藏在作物下的鳥類，整個工作 過程無需人工操作干預。很大程度上節(jié)約了人力成本和時間，具有較好的實用性和便捷性。 可廣泛用于大型農場、機場等需要自動驅趕鳥類等"入侵物"的場景。
【附圖說明】
[0018] 圖1為本發(fā)明用于農場自主管理的四旋翼飛行器控制系統(tǒng)結構示意圖；
[0019] 圖2為本發(fā)明飛行器在跟蹤目標時使用的目標跟蹤PID控制器原理圖；
[0020]圖3為本發(fā)明四旋翼飛行器姿態(tài)控制PID控制器原理圖；
[0021 ]圖4為本發(fā)明攝像頭透視成像模型圖；
[0022]圖5為本發(fā)明飛行器在作業(yè)域搜索目標時使用的作業(yè)域搜索PID控制器原理圖；
[0023] 圖6為本發(fā)明地面站PC端對目標進行識別和跟蹤的流程圖。
【具體實施方式】
[0024] 如圖1所示用于農場自主管理的四旋翼飛行器控制系統(tǒng)結構示意圖，包括四旋翼 飛行器和遠程控制子系統(tǒng)，飛行控制子系統(tǒng)、偵測和驅逐子系統(tǒng)裝在四旋翼飛行器上，飛行 控制子系統(tǒng)包括嵌入式微控制器(飛行控制中心）、2對螺旋槳、GPRS網(wǎng)絡、GPS模塊和慣性測 量單元，偵測和驅逐子系統(tǒng)包括攝像頭、語音模塊和5.8GHz圖像傳輸模塊，遠程控制子系統(tǒng) 包括圖像接收模塊、地面控制中心和智能移動終端，其中，所述飛行控制子系統(tǒng)完成四旋翼 飛行器的空中姿態(tài)控制、飛行引導和搜索位置控制，所述偵測和驅逐子系統(tǒng)用于"入侵者" 發(fā)現(xiàn)、追蹤和驅逐，所述遠程控制子系統(tǒng)是用戶端控制設備，用于用戶確定作業(yè)農場的范圍 及遠程控制四旋翼飛行器作業(yè)過程。慣性測量單元測量加速度及角速度送嵌入式微控制 器，嵌入式微控制器根據(jù)濾波和姿態(tài)解算處理后進行姿態(tài)融合并輸出PWM波到電機，以此來 控制2對螺旋槳四個電機的轉速的方式改變四旋翼的姿態(tài)，超聲波信號送嵌入式微控制器， 嵌入式微控制器根據(jù)超聲波測距原理控制飛行器在搜索目標時的飛行高度。
[0025] 在上述方案中，所述的飛行控制子系統(tǒng)利用采用兩層串級閉環(huán)控制方法實現(xiàn)對所 述2對螺旋槳電機的控制，實現(xiàn)飛行姿態(tài)的穩(wěn)定；所述攝像頭采集的圖像數(shù)據(jù)通過所述 5.8GHz圖像傳輸模塊采用無線傳輸方式把圖像送到所述地面控制中心，所述地面控制中心 通過圖像處理獲得所述四旋翼飛行器的實時位置及其與"入侵者"的相對姿態(tài)數(shù)據(jù)，并通過 GPRS網(wǎng)絡將飛行引導數(shù)據(jù)送至所述四旋翼飛行器進行飛行引導；所述四旋翼飛行器利用 GPS模塊獲取當前搜索位置，并利用確定農場下一個搜索區(qū)域。
[0026]在上述方案中，所述的偵測和驅逐子系統(tǒng)利用所述地面控制站對所述攝像頭采集 的圖像數(shù)據(jù)進行處理，對麻雀等鳥類"入侵者"進行識別，并，經(jīng)GPRS向所屬飛行控制系統(tǒng)發(fā) 送并利用所述語音模塊播放刺激聲波對"入侵者"進行恐嚇，達到驅逐效果。
[0027]在上述方案中，智能移動終端通過GPRS網(wǎng)絡將作業(yè)農場的范圍和設定入侵者信息 送四旋翼飛行器中四旋翼飛行器，所述的遠程控制子系統(tǒng)利用所述智能手機終端在地圖上 確定作業(yè)農場的范圍，并啟動作業(yè)任務，所述四旋翼飛行器在接收任務后啟動并飛往指定 農場，作業(yè)過程中，所述智能手機終端可利用GPRS網(wǎng)絡發(fā)送命令遠程中斷所述四旋翼飛行 器的任務并返航。
[0028] 在上述方案中，所述的四旋翼飛行器，作業(yè)域搜索、"入侵"識別及跟蹤和驅逐過程 自動進行，不需要人為干預和操控。
[0029] 在上述方案中，所述的作業(yè)域搜索方法，包括以下幾個步驟：
[0030] 1、所述四旋翼飛行器到達待作業(yè)農場后，根據(jù)儲存的任務數(shù)據(jù)，包括待作業(yè)農場 的形狀和面積數(shù)據(jù)，進行最優(yōu)計算，確定搜索域的個數(shù)和搜索域中心點的地理坐標位置；
[0031] 2、使用所述飛行器上的超聲波傳感器，設定其飛行高度為距作物50公分；
[0032] 3、所述飛行器準備飛行，并到達第一個搜索域中心點；
[0033] 4、獲取所述攝像頭圖像信息并實時發(fā)送到地面控制站，啟動所述語音模塊放音功 能，發(fā)出刺激聲波實現(xiàn)預驅逐效果；
[0034] 5、若有麻雀等運動物體飛出，由地面控制站識別，采用"入侵"目標跟蹤方法進行 跟蹤，整個放音過程持續(xù)到被跟蹤目標超過農場邊界；
[0035] 6、重復步驟2~5直到搜索過程覆蓋整個作業(yè)域。
[0036]在上述方案中，所述的"入侵"目標跟蹤方法，包括以下幾個步驟：
[0037] 1、地面控制站接收圖像信息；
[0038] 2、基于粒子濾波和卡爾曼濾波的目標跟蹤算法對目標進行跟蹤，并預測目標下一 次可能出現(xiàn)的位置；
[0039] 3、通過GPRS網(wǎng)絡發(fā)送飛行引導數(shù)據(jù)，包含被跟蹤目標期望俯仰角和橫滾角；
[0040] 4、飛行器接收步驟4發(fā)送的數(shù)據(jù)，通過目標跟蹤PID控制器及姿態(tài)PID控制器構成 的兩層串級控制器不斷修正飛行器的姿態(tài)；
[0041 ] 5、重復以上步驟，直到目標離開農場邊界。
[0042]圖2為飛行器在跟蹤目標時使用的目標跟蹤PID控制器原理圖，目標跟蹤PID控制 器由兩層雙串級控制器組成，位置PID作為外環(huán)，姿態(tài)PID為內環(huán)共同構成了目標跟蹤控制 器，期望位置是地面控制中心根據(jù)接收到的視頻數(shù)據(jù)計算并發(fā)送的飛行引導數(shù)據(jù)(包含了 世界坐標系下被跟蹤目標與四旋翼的水平位移），期望位置與實際位置反饋信號動差經(jīng)過 外環(huán)PID后送內環(huán)姿態(tài)PID，與當前姿態(tài)比較后送姿態(tài)PID，姿態(tài)PID輸出為跟蹤信號，姿態(tài)控 制上應用了角速度作為內環(huán)，角度作為外環(huán)的串級PID控制。
[0043] 圖3為四旋翼飛行器姿態(tài)控制PID控制器原理圖，該PID控制器由兩層雙串級控制 器組成，角度PID作為外環(huán)，角速度PID為內環(huán)。期望姿態(tài)角根據(jù)公式(4)、（5)求得。
[0044] 圖4為攝像頭透視成像模型，根據(jù)世界坐標系與攝像機坐標系之間的轉換關系式 和攝影幾何學知識，目標在圖像平面的像素位置與目標和飛行器之間水平相對位移坐標變 換公式：
[0047]其中，x，y為水平位移(在二維平面上目標到飛行器的距離），h為由超聲波測量出 的飛行器當前高度，β為攝像頭的俯仰角，Θ為物像連線與鏡頭光軸之間的夾角，（m，Vl)為像 素點在圖像平面坐標系中的位置，（u Q，VQ)為圖像像素坐標系中的坐標原點。設每一個像素 在X軸與y軸上的物理尺寸分別為dx與dy，位置PID期望輸入為u= [ux Uy uz]T，則經(jīng)過位置 PID計算有：
[0049] 其中，0d，Yd，Wd分別為姿態(tài)PID期望輸入姿態(tài)角(俯仰角、橫滾角、偏航角），ho為u 到原點的距離。
[0050] 則得到姿態(tài)PID期望姿態(tài)角Θ4Ρ γ d:
[0051] yd = sin_1[(ux sinitd-Uy cositd)/ho] (4)
[0052] 0d = sin_1[(uxcositd-uysinitd)(cos γ d)0.5/h0] (5)
[0053] 圖5為飛行器在作業(yè)域搜索目標時使用的作業(yè)域搜索PID控制器原理圖，作業(yè)域搜 索PID控制器由串級控制器組成，位置PID作為外環(huán)，高度PID作為內環(huán)共同構成了作業(yè)域搜 索控制器。搜索域中心點坐標作為期望位置，經(jīng)過PID控制器后，不斷修正飛行器姿態(tài)，使其 逐步接近期望位置。
[0054] 對隱藏在作物下的鳥類等進行搜索時使用作業(yè)域搜索方法，該方法主要包括以下 幾個步驟：
[0055]步驟①:使用超聲波模塊測距(本實施例中超聲波模塊型號為HC-SR04)，利用公式 s = (Eh· c)/2,式中，s為測量距離(m)，Eh為高電平持續(xù)時間（高電平的持續(xù)時間就是超聲波 從發(fā)射到返回的時間），c為聲速(340m/s)，控制飛行器在搜索目標時的飛行高度；
[0056]步驟②:首先假定農場為規(guī)則的方形s，其邊長為m，均勾分成η個搜索域(mi，ni2， ni3，…），以A矩陣表不：
[0058] 式中，n = N2。
[0059] 步驟③:該區(qū)域由一系列方形組成，其邊長為m/n。由于逐一在每個區(qū)域進行檢測 會浪費大量的時間而且四旋翼攜帶的電池能量有限，考慮到鳥類等對周邊的環(huán)境變化比較 敏感，因此只要在一個中間區(qū)域進行干擾，則會影響到兩邊的區(qū)域。具體步驟為：
[0060] (1)剔除邊界點，實際搜索矩陣A°:
[0062]式中，u為不確定點，mj(i = 2,3,4......，j = 2,4,6......)為劃分的區(qū)域塊；
[0063] (2)簡化搜索矩陣sA:
[0065] (3)從左到右從上到下蛇形排列，分別提取矩陣SA的元素。
[0066] 以n22這點為例詳細說明搜索步驟。
[0068] 從矩陣Si可以看出：Π 22影響了周邊的四個點，用符號 > 表不。如果在這四個點中任 意一處發(fā)現(xiàn)目標，則進行驅逐。由于鳥類具有群體活動行為，某個區(qū)域有活動會影響到其它 區(qū)域鳥類的活動，因此排除點如^^咖心^以此方法進行搜索目標洇此實際需要檢測的 區(qū)域可以簡化為N2/9。假定區(qū)域1112有驅逐目標(丹），則四旋翼到達區(qū)域n 12有效高度處，此時 四旋翼懸停，近似為不動點。然后鳴笛干擾，利用幀差法對區(qū)域m2進行檢測。如果有驅逐目 標(Pi)飛出則會檢測到移動物體，對其進行追蹤直到該目標離開S區(qū)域，以防止(Pi)逃離n 12 再次到r^(i#l，j辛2);若此時沒有檢測到(P〇,則繼續(xù)對n44進行搜索，重復上面的步驟，最 后完成整個S域的搜索。
[0069] 圖6為地面中心PC端對目標進行識別和跟蹤的流程圖，該"入侵"目標跟蹤方法包 含了以下步驟：
[0070] 步驟①:飛行器機載攝像頭采集圖像并通過無線方式發(fā)送；
[0071 ]步驟②:遠程控制中心的地面站PC端軟件獲?、俚囊曨l圖像；
[0072]步驟③:進行圖像處理，實現(xiàn)對圖像的特征點提取及特征匹配；
[0073]步驟④：目標識別，利用幀差法提取運動目標并進行二值化及膨脹腐蝕處理；
[0074]步驟⑤：目標跟蹤，使用粒子濾波算法與強跟蹤Kalman濾波算法對目標進行跟蹤；
[0075] 步驟⑥:通過GPRS把⑤的跟蹤坐標發(fā)送給嵌入式微控制器；
[0076] 步驟⑦:嵌入式微控制器接收⑥的數(shù)據(jù)并控制電機的轉速來改變姿態(tài)，從而對運 動的目標連續(xù)跟蹤。
【主權項】
1. 一種用于農場自主管理的四旋翼飛行器控制系統(tǒng)，其特征在于，包括四旋翼飛行器 和遠程控制子系統(tǒng)，飛行控制子系統(tǒng)、偵測和驅逐子系統(tǒng)裝在四旋翼飛行器上，飛行控制子 系統(tǒng)包括嵌入式微控制器、2對螺旋槳電機、GPRS網(wǎng)絡、GPS模塊和慣性測量單元，偵測和驅 逐子系統(tǒng)包括攝像頭、語音模塊和5.8GHz圖像傳輸模塊，遠程控制子系統(tǒng)包括圖像接收模 塊、地面控制中心和智能移動終端； 智能移動終端通過GPRS網(wǎng)絡將作業(yè)農場的范圍和設定入侵者信息送四旋翼飛行器中 嵌入式微控制器，攝像頭采集的圖像數(shù)據(jù)通過5.8GHz圖像傳輸模塊采用無線傳輸方式送 出，由圖像接收模塊接收后送到地面控制中心，地面控制中心通過圖像處理獲得四旋翼飛 行器的實時位置及設定入侵者的相對姿態(tài)數(shù)據(jù)，通過GPRS網(wǎng)絡將飛行引導數(shù)據(jù)送至四旋翼 飛行器，四旋翼飛行器中嵌入式微控制器控制2對螺旋槳電機進行飛行引導，四旋翼飛行器 通過GPS模塊獲取搜索區(qū)域位置送嵌入式微控制器，嵌入式微控制器送信號到偵測和驅逐 子系統(tǒng)，控制攝像頭進行采集圖像，控制語音模塊播放刺激聲波對設定入侵者進行恐嚇； 慣性測量單元測量加速度及角速度送嵌入式微控制器，嵌入式微控制器根據(jù)濾波和姿 態(tài)解算處理后進行姿態(tài)融合并輸出PWM波到電機，以此來控制四個電機的轉速的方式改變 四旋翼的姿態(tài)，超聲波信號送嵌入式微控制器，嵌入式微控制器根據(jù)超聲波測距原理控制 飛行器在搜索目標時的飛行高度。2. 根據(jù)權利要求1所述用于農場自主管理的四旋翼飛行器控制系統(tǒng)的控制方法，其特 征在于，所述嵌入式微控制器采用兩層雙串級PID控制進行目標跟蹤，位置PID作為外環(huán)，姿 態(tài)PID為內環(huán)，控制對象為電機轉速，期望位置是地面控制中心根據(jù)接收到的視頻數(shù)據(jù)計算 并發(fā)送的飛行引導數(shù)據(jù)，包含世界坐標系下被跟蹤目標與四旋翼飛行器的水平位移，期望 位置與實際位置反饋信號差值送外環(huán)PID，外環(huán)PID輸出與當前姿態(tài)比較后送姿態(tài)PID，姿態(tài) PID輸出為跟蹤信號，進而通過控制四個電機的轉速來改變四旋翼自身的姿態(tài)，實現(xiàn)對目標 的連續(xù)跟蹤。3. 根據(jù)權利要求1所述述用于農場自主管理的四旋翼飛行器控制系統(tǒng)的控制方法，其 特征在于，所述嵌入式微控制器采用兩層雙串級PID控制進行作業(yè)域搜索，位置PID作為外 環(huán)，高度PID作為內環(huán)，搜索域中心點坐標作為期望位置，期望位置與實際位置反饋信號動 差送外環(huán)PID，外環(huán)PID輸出與當前高度比較后送高度PID，高度PID輸出為搜索跟蹤信號，不 斷修正四旋翼飛行器姿態(tài)，逐步接近期望位置。4. 根據(jù)權利要求1所述述用于農場自主管理的四旋翼飛行器控制系統(tǒng)的控制方法，其 特征在于，所述地面控制中心對設定入侵者進行識別和跟蹤方法包含以下步驟： ① :四旋翼飛行器機載攝像頭采集圖像并通過無線方式發(fā)送； ② :遠程控制中心的地面站PC端軟件獲取步驟①的視頻圖像； ③ :進行圖像處理，實現(xiàn)對圖像的特征點提取及特征匹配； ④ ：目標識別，利用幀差法提取運動目標并進行二值化及膨脹腐蝕處理； ⑤ ：目標跟蹤，使用粒子濾波算法與強跟蹤Kalman濾波算法對目標進行跟蹤； ⑥ :通過GPRS網(wǎng)絡把步驟⑤的跟蹤坐標發(fā)送給嵌入式微控制器； ⑦ :嵌入式微控制器接收步驟⑥的數(shù)據(jù)并控制電機的轉速來改變姿態(tài)，從而對運動的 目標連續(xù)跟蹤。
【文檔編號】G05D1/08GK106094849SQ201610436351
【公開日】2016年11月9日
【申請日】2016年6月17日
【發(fā)明人】盛存寶, 樂燕芬, 施偉斌, 羅紅玉, 莫禮東, 倪駕宇
【申請人】上海理工大學