使用Minifly打造基於視覺感知的跟蹤無人機

時間 2020-03-26

原文原文鏈接

前言：本文介紹一種可行的解決方案來實現基於視覺感知的跟蹤無人機。因爲本人能力和資源有限，因此在無人機系統的選擇上，選用正點原子開發的開源算法無人機Minifly四軸和攝像頭。視覺感知模塊（目標檢測與跟蹤）採用OpenCV + MobileNet SSD + KCF。本文已分享經驗和記錄開發過程爲主，推薦使用其餘更好的無人機模塊和圖像識別算法。html

知識基礎：Linux、Python 三、STM32（嵌入式相關）python

解釋一下爲何要用Linux，其實只用Windows也能夠，但實際運行中發現OpenCV的效率在Linux上更高。該方案建議安裝Windows 7（必須） + Linux 雙系統。算法

Python在人工智能中的影響不用多說，簡單瞭解一下這種語言便可。微信

STM32F103 和 STM32F411（Cortex-M）是本方案種的無人機系統的處理器核心，而且採用FreeRTOS做爲操做系統，因此關於STM32的C語言庫函數開發是必要的，儘管本方案涉及到它的部分很少。數據結構

總體框架以下:多線程

正文：併發

1、開發軟件及平臺

Deepin Linux --- deepin操做系統是中國人開發的Linux發行版。主要優勢：安裝簡單、界面美觀、集成wine QQ 微信，真正作到開箱即用。框架

PyCharm --- PyCharm是一種Python IDE，帶有一整套能夠幫助用戶在使用Python語言開發時提升其效率的工具，好比Python解釋器選擇、Pip包管理器、調試、語法高亮、Project管理、代碼跳轉、智能提示、自動完成、單元測試、版本控制等，選用免費的社區版便可。ide

Keil MDK5 --- MDK-ARM軟件爲基於Cortex-M、Cortex-R四、ARM七、ARM9處理器設備提供了一個完整的開發環境。函數

2、四軸飛行器原理

四軸飛行器主要是由電機、電調、電池、漿葉、機架、遙控器、飛控組成。飛行器基本原理是經過飛控控制四個電機旋轉帶動漿葉產生升力，分別控制每個電機和漿葉產生不一樣的升力從而控制飛行器的姿態和位置。四軸在空中能夠實現八種運動，分別爲垂直上升、垂直降低、向前運動、向後運動、向左運動、向後運動、順時針改變航向、逆時針改變航向。飛行器在空中任何一種姿態均可以經過姿態角旋轉後獲得。

2.1姿態角的旋轉關係圖

俯仰角（pitch）：機體座標系 X 軸與水平面的夾角，圍繞X軸旋轉。當X軸的正半軸位於過座標原點的水平面之上（擡頭）時，俯仰角爲正，不然爲負。

偏航角（yaw）：機體座標系 X 軸在水平面上投影與地面座標系 X軸之間的夾角，圍繞Y軸旋轉。機頭右偏航爲正，反之爲負。

滾轉角（roll）：機體座標系 Z 軸與經過機體 Z 軸的鉛垂面間的夾角，圍繞 Z 軸旋轉。機體向右滾爲正，反之爲負。

簡單來講，經過Pitch能夠控制機體向先後飛行，Roll能夠控制機體左右飛行、Yaw能夠控制機頭偏轉。下文會針對遙控器模塊作不少深刻的分析，遙控器對四軸的 「控制數據」包含了這三個重要的值。

至於四軸如何經過各類傳感器、數學模型和公式、PID自動控制原理來作到真正的飛行控制已不在本文的內容範圍。若是想得到更好的飛行控制效果，關於PID控制原理卻是能夠細究一下，PID控制原理提出的歷史也比較長，在自動控制的應用中也很是普遍。

在本方案中因爲四軸的空間自由度過高致使調試的不便，本方案採用定高、定點飛行。（須要購買光流定點模塊）須要注意的是，Minifly並不能支持兩個以上的模塊，下文會涉及到對攝像頭和四軸的簡單改造。

3、Minifly程序分析

3.1 代碼框架

資料下載：http://www.openedv.com/thread-105197-1-1.html

3.1.1 Minifly遙控器代碼框架（FirmwareF103）：

圖3.1.1 Minifly遙控器代碼框架

經過Minifly遙控器發給四軸的控制信息有兩條鏈路：

1. 搖桿狀態 -->模數轉換 --> 控制數據生成 -->ATKP包 -->無線電模塊 --> 四軸

2. 上位機數據 --> USB轉串口 -->ATKP包 -->無線電模塊 -->四軸

爲實現無人機的自動控制，必須採用第二條鏈路來進行數據的傳遞控制數據，要搞清楚什麼數據能被無人機接收並解析，也就是ATKP包的具體內容。在下文中將結合具體程序解答。

3.1.2Minifly四軸代碼框架（FirmwareF411）：

圖3.1.2 Minifly四軸代碼框架

本方案採用遙控器做爲中轉站控制四軸飛行，也就是圖3.1.2中的綠框部分。

3.2 通訊協議

通訊協議相關的源碼以FirmwareF103工程代碼爲例：

ATKP通訊協議部分主要在 atkp.h 中，ATKP 數據包格式及 msgID 功能字定義代碼以下:

 1 /*上行幀頭*/ 
 2 #define UP_BYTE1 0xAA 
 3 #define UP_BYTE2 0xAA 
 4 /*下行幀頭*/ 
 5 #define DOWN_BYTE1 0xAA 
 6 #define DOWN_BYTE2 0xAF
 7  #define ATKP_MAX_DATA_SIZE 30 
 8 /*ATKP 通信數據結構*/
 9  typedef struct { 
10   u8 msgID; 
11   u8 dataLen; 
12   u8 data[ATKP_MAX_DATA_SIZE]; 
13 }atkp_t;

四軸通訊協議中下行指令有兩種控制信息DOWN_REMOTOR 指令 ID 是用來指定是遙控器下行給四軸的命令。而後使用 Data[0]分區分發送控制命令和控制數據發送。控制命令和控制數據枚舉以下

1 /*遙控數據類別*/
2  typedef enum 
3  { 
4   REMOTOR_CMD, 
5   REMOTOR_DATA, 
6 }remoterType_e;

控制命令主要是控制四軸實現一些功能性操做的命令，好比一鍵起飛降落、一鍵翻滾、一鍵緊急中止等。控制數據主要是發送給四軸姿態控制數據。當 Data[0] == REMOTOR_CMD時，Data[1]爲控制命令；當 Data[0]== REMOTOR_DATA 時，Data[1]以後爲控制數據。控制數據結構以下：

 1 /*遙控控制數據結構*/
 2  typedef __packed struct 
 3 { 
 4   float roll;       
 5   float pitch;   
 6   float yaw;       
 7   float thrust; 
 8   float trimPitch; 
 9   float trimRoll; 
10   bool ctrlMode; 
11   bool flightMode; 
12   bool RCLock; } remoterData_t;
13 /*關於飛行與控制模式枚舉*/
14 enum ctrlMode
15 {
16     ALTHOLD_MODE,
17     MANUAL_MODE,
18 };
19 enum flightMode
20 {
21     HEAD_LESS,
22     X_MODE,
23 };

發送控制數據時，數據格式以下：

當須要控制數據時，先使用 remoterData_t 定義一個 send 結構體數據，然後調用 sendRmotorData((u8*)&send, sizeof(send)) 便可發送控制數據了。代碼示意以下：

 1 remoterData_t  send; 
 2 send.roll = 0.0; …………/*給 send 結構體賦值*/ 
 3 sendRmotorData((u8*)&send, sizeof(send));
 4 
 5 /*發送遙控控制數據*/
 6  void sendRmotorData(u8 *data, u8 len)
 7  { 
 8   if(radioinkConnectStatus() == false) 
 9    return;
10   atkp_t p; 
11   p.msgID = DOWN_REMOTOR; 
12   p.dataLen = len + 1; 
13   p.data[0] = REMOTOR_DATA; 
14   memcpy(p.data+1, data, len); 
15   radiolinkSendPacket(&p); 
16 }

經過以上代碼和表格咱們就能知道發送ATKP包的具體內容，如今看起來可能一頭霧水，舉兩個例子簡單解釋一下：

1.控制命令：一鍵起飛降落命令完整格式：

AA AF 50 02 00 03 AE

分析：

0xAA 0xAF （下行幀頭）

0x50（msgID:DOWN_REMOTOR 下行指令)）

0x02（LEN + 1)）

0x00（DATA[0] = 0x00 控制命令）

0x03（CMD_FLIGHT_LAND 一鍵起飛/降落參看頭文件remoter_ctrl.h中的宏定義）

0xAE（CHECK SUM 校驗和從幀頭到數據最後一位逐字節相加）

2.控制數據：讓四軸在手動模式下已50%油門和Roll角爲5的姿態下飛行

AAAF501D010000a04000000000000000000000484200000000000000000000000031

儘管看起來很長，逐步分析一下：

0xAA 0xAF 0x50（下行幀頭、下行指令msgID）

0x1D （數據長度29 -1 =28 也就是結構體remoterData_t的長度，注意字節對齊）

0x01 （data[0] = 0x01控制數據）

0x0000A040（send.roll = 5.0f IEEE754標準32位浮點數小端字節序）

0x00000000（send.pitch = 0.0f）

0x00000000（send.yaw = 0.0f）

0x00004842（send. thrust = 50.0f 50%油門）

0x00000000（send. trimPitch = 0.0f trim是修正系統偏差，默認0）

0x00000000（send. trimRoll = 0.0f）

0x00 （u8-CtrlMode 0x00-手動模式 0x01-定高定點模式）

0x00 （bool-FlyMode true-X模式 false-無頭模式）

0x00 （bool-RCLok 解鎖相關，用不上）

0x00 （1byte-字節對齊）

0x31 （前面全部字節的校驗和）

關於大小端：數據存儲的大端字節序仍是小端字節序取決於CPU，STM32 採用小端字節序。

關於四軸各項控制參數的範圍請參看源碼FirmwareF103 – COMMUNICATE –remoter_ctrl.c。

3.3 二次編譯

下載最新的源碼後（V1.3），須要微調代碼，從新編譯並升級固件。

3.3.1.遙控器

如圖3.3.1 MDK打開工程FirmwareF103找到相關代碼並註釋掉箭頭位置，使得上位機數據能經過USB串口被遙控器正常接收併發放給四軸飛行器。

圖3.3.1.1

保存代碼，如圖3.3.1.2在編譯器配置中勾選生成BIN文件，再進行編譯，最後編譯日誌必定要提示生成新的BIN文件。下載BIN固件請參看固件升級手冊。

圖3.3.1.2

圖3.3.1.3

3.3.2 無人機

如圖3.3.2.1：MDK打開工程FirmwareF411，四軸飛行高度調整（建議高度爲1.4m 即140.f），修改後同上配置後編譯下載

圖3.3.2.1

注意：四軸的固件下載可能存在失敗的狀況，須要屢次下載

4、驅動模塊的開發

4.1 Wi-Fi攝像頭

首先說明Minifly官方提供的微型WiFi攝像頭並很差用，它使用私有的通訊協議，視頻流編碼格式爲H.264，只能按照提供的客戶端軟件來進行訪問，能達到20FPS。經過分析其Web客戶端技術，發現其以CGI協議爲訪問接口，可是並不包含視頻流的CGI指令，只有snapshot的指令，也就是發送截屏的指令，返回一個JPG格式圖片，最高只能達到8FPS。

PyCharm安裝opencv-python、imutils包，鏈接Minifly，經過Python咱們能夠實現傳圖：

 1 import cv2
 2 import imutils
 3 
 4 # CGI IPcamare
 5 url = 'http://192.168.1.1:80/snapshot.cgi?user=admin&pwd='
 6 # im.src = "videostream.cgi?stream="+Status.sever_push_stream_number+"&id="+d.id;
 7 # url = 'http://192.169.1.1:80/
 8 # url = 'http://192.168.1.1:80/videostream.cgi?user=&pwd=&resolution=32&rate=0'
 9 # url = 'http://192.168.1.1:80/livestream.cgi?user=admin&pwd='
10 cnt = 0
11 while True:
12     timer = cv2.getTickCount()
13     cap = cv2.VideoCapture(url)
14     fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
15     if cap.isOpened():
16         cnt += 1
17         width, height = cap.get(3), cap.get(4)
18         print(cnt, '[', width, height, ']')
19         ret, frame = cap.read()
20         frame = imutils.resize(frame, width=640)
21         # frame = cv2.flip(frame, -180)
22         cv2.putText(frame, "FPS : " + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
23         cv2.imshow('frame', frame)
24     else:
25         print("Error")
26         break
27     if cv2.waitKey(1) & 0xFF == ord('q'):
28         break
29 cap.release()
30 cv2.destroyAllWindows()

4.2 串口數據

第三章中已經分析了ATKP包的控制指令及控制數據格式，如今咱們須要用Python構造可以生成這些格式的數據，而且經過串口發送到Minifly遙控器。須要安裝pyserial包。

一樣先以簡單的控制指令爲例如一鍵起飛/降落：

 1 # coding=utf-8
 2 import serial
 3 import time
 4 
 5 cmd_onekey_fly = 'AAAF50020003AE'     # 一鍵起飛/降落
 6 ser = serial.Serial('COM7', 500000, timeout=0.5)    # 打開串口資源
 7 u_byte = bytes.fromhex(cmd_onekey_fly)  # 字符串形式轉爲十六進制字節形式
 8 
 9 ser.write(u_byte)   # 發送到串口（遙控器）實現一鍵起飛
10 time.sleep(3)
11 ser.write(u_byte)   # 發送到串口 實現一鍵降落
12 
13 ser.close()     # 關閉資源

若是要發送控制數據，應先構造一個生成數據字符串的過程，以逐步拼接的方式完成：

固定的幀頭、LEN、data[0] + 浮點數小端字節序 + 控制模式 + 對齊字節 + 校驗和

給出代碼：

 1 # coding=utf-8
 2 import serial
 3 import struct
 4 import time
 5 
 6 
 7 def float_to_hex(data):                 # float --> Hex 小端字節序
 8     return (struct.pack('<f', data)).hex()
 9 
10 
11 cmd_onekey_fly = 'AAAF50020003AE'     # 一鍵起飛/降落
12 cmd_stop = 'AAAF50020004AF'           # 緊急停機
13 
14 cmd_Head = 'AAAF501D01'                 # 控制信息頭 AA AF[HEAD] 50[REMOTER] 1D 01(data[1-29])
15 Trim = '0000000000000000'               # Trim信息(不校準)
16 
17 Mode = '00000000'                       # 飛行控制模式 u8-CtrlMode bool-FlyMode bool-RCLok 1byte-字節對齊
18 send_str = ''
19 flydata = [5, 0, 0, 50]                 # 飛行數據【rol-滾轉角, pit-俯仰角, yaw-偏航角, thr-油門】
20 
21 send_str = cmd_Head + float_to_hex(flydata[0]) + float_to_hex(flydata[1]) + float_to_hex(flydata[2]) + \
22            float_to_hex(flydata[3]) + Trim + Mode
23 
24 u_byte = bytes.fromhex(send_str)
25 
26 checksum = 0
27 cnt = 0
28 
29 for a_byte in u_byte:
30     checksum += a_byte
31     cnt = cnt + 1
32 
33 H = hex(checksum % 256)
34 print(H, cnt)
35 print(send_str)
36 send_str = send_str + H[-2] + H[-1]
37 if H[-2] == 'x':  # 0xF -> 0x0F
38     send_str = send_str + '0' + H[-1]
39 
40 print(send_str)

運行結果：

0x31 33

AAAF501D010000a040000000000000000000004842000000000000000000000000

AAAF501D010000a04000000000000000000000484200000000000000000000000031

注意：

1. 上述控制指令和控制數據可在串口調試工具或代碼中實現發送和飛行調試，波特率設置爲500 000，注意安裝驅動，Linux上爲免驅的USB虛擬的串行口，設備路徑/dev/ttyACM0。

2. 一鍵起飛/降落指令發送一次當即起飛，再發送一次進入着陸狀態。

3. 發送控制數據時必須持續不斷以最快速度重複發送，兩次發送的時間間隔爲1ms最佳，大概發送300次爲1秒。

4. 關於控制模式，Mode = '00000000' 時爲手動模式，不用讓無人機起飛，通常在測試串口通訊是否聯通，也可經過改變rol-滾轉角, pit-俯仰角, yaw-偏航角來看數據控制的效果。

5. 定高定點模式下Mode = '01000000'，先讓無人機起飛，油門必須保持爲50%（thrust = 50意爲油門搖桿位置沒有改變，四軸的程序會自動調整高度）。

5、目標檢測和跟蹤

基於計算機視覺的應用比較成熟，並非本文要討論的重點。這裏簡單介紹一種檢測與跟蹤的方法，模型採用MobileNets SSD 和核相關濾波算法（KCF）的目標檢測與跟蹤實現。

圖5.1 MobileNets:高效（深度）神經網路

（1）目標跟蹤開始時，將被跟蹤目標所在區域的圖像塊送入 SSD 算法所創建的各個離線模型中，檢測出目標類型。SSD算法進行目標檢測時，首先產生多個不一樣尺度、不一樣長寬比的目標框假設。而後，再將多個不一樣的卷積濾波器應用於各個卷積層上，從而得出各個目標框假設的分值和位置偏移，終肯定一系列候選目標框。而後再經過非極大值抑制策略來肯定終的檢測結果。

（2）跟蹤開始後，得到每一幀時，都使用 SSD算法檢測出目標所在的位置。同時將該幀圖片信息存儲在新的訓練集中。

（3）經過 SSD 算法，使用新的訓練集對已有的模型進行繼續訓練，獲得新的目標外觀模型。這樣一來，原有的模型獲得了更新，而更新時所用的訓練樣原本自於在線得到的目標信息，從而使得更新後的模型中具備了專屬於被跟蹤目標的一些外觀信息。所以，用更新後的模型進行後續幀中的目標檢測時，精度可以獲得進一步提升。另外，因爲在線得到的圖像樣本數量較少，因此在線訓練的計算量不大，不會對算法的速度產生明顯影響。

（4）當新的訓練集中圖像數量達到預先設定的閾值時，說明對於原有模型的更新達到了必定的程度。此時用新的模型替代原有的模型，用於在後續幀中進行目標檢測。同時清空新的訓練集。

（5）重複前面的步驟（2）至步驟（4），直到跟蹤過程結束爲止。

上述算法流程可用下圖概括表示：

圖5.2 目標檢測與跟蹤算法