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

Kryo發表於2019-08-07
視覺無人機

前言：本文介紹一種可行的解決方案來實現基於視覺感知的跟蹤無人機。從零開始搭建無人機系統工作量和難度（以及錢）都是非常大的，所以在無人機系統的選擇上，選用正點原子開發的開源演算法無人機Minifly四軸和攝像頭。視覺感知模組（目標檢測與跟蹤）採用OpenCV + MobileNet SSD + KCF。本文已分享經驗和記錄開發過程為主，推薦使用其他更好的無人機模組和影象識別演算法。

知識基礎：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 程式碼框架

資料下載：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.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來將所有模組結合起來。

　　由於四軸自身大小和升力的限制，攝像頭的選用變得有限，在傳圖速度和穩定性上並不滿意，時常丟失跟蹤目標。雖然無人機採用了高精的感測器，但實際執行過程中因為環境的影響，飛行狀態的效果有時並不太理想，導致對跟蹤演算法反饋的資訊得不到及時調整，軟硬體還需進一步優化。

 

相關文章