前言：無人機和人工智慧現在是非常熱門的話題，將兩者結合起來是一個比較好的創意，本文介紹一種可行的解決方案來實現基於視覺感知的跟蹤無人機。從零開始搭建無人機系統工作量和難度（以及錢）都是非常大的，所以在無人機系統的選擇上，選用正點原子開發的開源演算法無人機Minifly四軸和攝像頭。視覺感知模塊（目標檢 ...
前言：無人機和人工智慧現在是非常熱門的話題，將兩者結合起來是一個比較好的創意，本文介紹一種可行的解決方案來實現基於視覺感知的跟蹤無人機。從零開始搭建無人機系統工作量和難度（以及錢）都是非常大的，所以在無人機系統的選擇上，選用正點原子開發的開源演算法無人機Minifly四軸和攝像頭。視覺感知模塊（目標檢測與跟蹤）採用OpenCV + MobileNet SSD + KCF。
請移步http://www.openedv.com/thread-105197-1-1.html瞭解更多關於Minifly相關信息，如果確定實現本方案需下載所有資料併購買相關產品。
理解本文需要如下基礎：Linux、Python（3.5以上）、STM32（嵌入式相關）
解釋一下為什麼要用Linux，其實Windows也可以（Python的跨平臺特性），但實際運行中發現OpenCV（V4.0以上）的效率在Linux上更高。該方案建議安裝Windows 7（必須） + Linux 雙系統。
Python在人工智慧中的地位不用多說，降低了不少開發難度，簡單瞭解一下這種語言即可。
STM32F103 和 STM32F411（Cortex-M）是本方案種的無人機系統的處理器核心，所以關於STM32的C語言庫函數開發是必要的，儘管本方案涉及到它的部分不多。
整體框架如下:
正文：
一、開發軟體及平臺
Deepin Linux --- deepin操作系統是由武漢深之度科技有限公司開發的Linux發行版。主要優點：安裝簡單、界面美觀、集成wine QQ 微信，真正做到開箱即用。
PyCharm --- PyCharm是一種Python IDE，帶有一整套可以幫助用戶在使用Python語言開發時提高其效率的工具，比如Python解釋器選擇、Pip包管理器、調試、語法高亮、Project管理、代碼跳轉、智能提示、自動完成、單元測試、版本控制等。
Keil MDK5 --- MDK-ARM軟體為基於Cortex-M、Cortex-R4、ARM7、ARM9處理器設備提供了一個完整的開發環境。Minifly開發中還有一套軟體必須使用其相容的Windows 7，這一點要尤其註意。
二、四軸飛行器原理
四軸飛行器主要是由電機、電調、電池、漿葉、機架、遙控器、飛控組成。飛行器基本原理是通過飛控控制四個電機旋轉帶動漿葉產生升力，分別控制每一個電機和漿葉產生不同的升力從而控制飛行器的姿態和位置。四軸在空中可以實現八種運動，分別為垂直上升、垂直下降、向前運動、向後運動、向左運動、向後運動、順時針改變航向、逆時針改變航向。飛行器在空中任何一種姿態都可以通過姿態角旋轉後得到。
2.1姿態角的旋轉關係圖
俯仰角（pitch）：機體坐標系 X 軸與水平面的夾角，圍繞X軸旋轉。當X軸的正半軸位於過坐標原點的水平面之上（抬頭）時，俯仰角為正，否則為負。
偏航角（yaw）：機體坐標系 X 軸在水平面上投影與地面坐標系 X軸之間的夾角，圍繞Y軸旋轉。機頭右偏航為正，反之為負。
滾轉角（roll）：機體坐標系 Z 軸與通過機體 Z 軸的鉛垂面間的夾角，圍繞 Z 軸旋轉。機體向右滾為正，反之為負。
簡單來說，通過Pitch可以控制機體向前後飛行，Roll可以控制機體左右飛行、Yaw可以控制機頭偏轉。下文會針對遙控器模塊做很多深入的分析，遙控器對四軸的 “控制數據”包含了這三個重要的值。
至於四軸如何通過各種感測器、數學模型和公式、PID自動控制原理來做到真正的飛行控制已不在本文的內容範圍。如果想獲得更好的飛行控制效果，關於PID控制原理倒是可以細究一下，PID控制原理提出的歷史也比較長，在自動控制的應用中也非常廣泛。
在本方案中由於四軸的空間自由度太高導致調試的不便，本方案採用定高、定點飛行。（需要購買光流定點模塊）需要註意的是，Minifly並不能支持兩個以上的模塊，下文會涉及到對攝像頭和四軸的簡單改造。
三、Minifly程式分析
3.1 代碼框架
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.1 Wi-Fi攝像頭
首先說明Minifly官方提供的微型WiFi攝像頭並不好用，它使用私有的通信協議，視頻流編碼格式為H.264，只能按照提供的客戶端軟體來進行訪問，能達到20FPS。通過分析其Web客戶端技術，發現其以GCI協議為訪問介面，但是並不包含視頻流的GCI指令，只有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意為油門搖桿位置沒有改變，四軸的程式會自動調整高度）。
五、目標檢測和跟蹤
基於電腦視覺的應用比較成熟，並不是本文要討論的重點。這裡簡單介紹一種檢測與跟蹤的方法，模型採用MobileNets SSD 和核相關濾波演算法（KCF）的目標檢測與跟蹤實現。
圖5.1 MobileNets:高效（深度）神經網路
（1）目標跟蹤開始時，將被跟蹤目標所在區域的圖像塊送入 SSD 演算法所建立的各個離線模型中，檢測出目標類型。SSD演算法進行目標檢測時，首先產生多個不同尺度、不同長寬比的目標框假設。然後，再將多個不同的捲積濾波器應用於各個捲積層上，從而得出各個目標框假設的分值和位置偏移，終確定一系列候選目標框。然後再通過非極大值抑制策略來確定終的檢測結果。
（2）跟蹤開始後，獲得每一幀時，都使用 SSD演算法檢測出目標所在的位置。同時將該幀圖片信息存儲在新的訓練集中。
（3）通過 SSD 演算法，使用新的訓練集對已有的模型進行繼續訓練，得到新的目標外觀模型。這樣一來，原有的模型得到了更新，而更新時所用的訓練樣本來自於線上獲得的目標信息，從而使得更新後的模型中具有了專屬於被跟蹤目標的一些外觀信息。因此，用更新後的模型進行後續幀中的目標檢測時，精度能夠得到進一步提高。另外，由於線上獲得的圖像樣本數量較少，所以線上訓練的計算量不大，不會對演算法的速度產生明顯影響。
（4）當新的訓練集中圖像數量達到預先設定的閾值時，說明對於原有模型的更新達到了一定的程度。此時用新的模型替代原有的模型，用於在後續幀中進行目標檢測。同時清空新的訓練集。
（5）重覆前面的步驟（2）至步驟（4），直到跟蹤過程結束為止。
上述演算法流程可用下圖歸納表示：
圖5.2 目標檢測與跟蹤演算法
六、聯合調試
6.1 模塊整合
除了一套Minifly四軸，還需另外要購買的兩個模塊如同所示：
圖6.1.1 四軸、WiFi圖傳模塊、光流模塊
上文已提到MiniFly並不能同時擴展多個模塊，必須做如下改動：
WiFi攝像頭模塊最外面一層PCB板（用於固定到四軸模塊的母排）用電烙鐵拆卸，用導線引出VCC、GND供電，連接一個微型撥動開關，再把導線焊接在四軸PCB的電池連接處，WiFi攝像頭模塊位置如圖，攝像頭用泡沫膠固定，再固定光流模塊，將攝像頭模塊壓住。
註意：VCC 、GND一定不要接反，攝像頭排線容易鬆動，固定時小心。
圖6.1.2 四軸及模塊的改動
此時四軸飛行時的耗電嚴重，需另購較大的電池，推薦 702035 規格 400mAH 容量。
四軸與遙控器， 攝像頭與上位機 兩者之間的通信經常互相干擾，官方給出的說法是通道干擾，重置四軸和遙控器重新匹配直到不再干擾（四軸起飛後也能傳圖）。
6.2 代碼改進
多線程的圖像傳輸、圖像目標檢測與跟蹤、四軸控制數據的生成及發送，程式框架：
圖6.2.1程式總體框架
具體程式不再粘貼，Python項目網盤鏈接：
https://pan.baidu.com/s/1ZLsgkLoJUJLBRifi4GO73Q 提取碼：pao0
圖6.2.2 項目結構
圖6.2.3 代碼功能
七、總結
本方案的優勢是減去了自己設計無人機系統的工作，並且可以通過Python來將所有模塊結合起來。
由於四軸自身大小和升力的限制，攝像頭的選用變得有限，在傳圖速度和穩定性上並不滿意，時常丟失跟蹤目標。雖然無人機採用了高精的感測器，但實際運行過程中因為環境的影響，飛行狀態的效果有時並不太理想，導致對跟蹤演算法反饋的信息得不到及時調整，軟硬體還需進一步優化。
視覺跟蹤無人機的創意來自我的畢設導師，感謝他提供的想法和在項目進行時的鼓勵，也感謝隊友在選擇四軸和指令分析中的大量工作，同時也延用了學長的視覺感知模塊，一開始接到這個項目時內心是拒絕的，因為關於無人機和電腦視覺是一無所知，經過近一年的醞釀草草收場，寫個東西記錄一下開發路程，分享經驗，由於各種原因，我可能不會對該項目繼續進行深入了，就留給學弟學妹們或者感興趣的小伙伴吧。
