Li_阴宅

这个屌丝很懒，什么也没留下！

热门标签

使用Minifly打造基于视觉感知的跟踪无人机_正点原子minifly定高定点

作者：Li_阴宅 | 2024-07-10 16:02:05

踩

正点原子minifly定高定点

摘自：https://www.cnblogs.com/kryo/p/11278565.html

使用Minifly打造基于视觉感知的跟踪无人机

前言：本文介绍一种可行的解决方案来实现基于视觉感知的跟踪无人机。由于本人能力和资源有限，所以在无人机系统的选择上，选用正点原子开发的开源算法无人机Minifly四轴和摄像头。视觉感知模块（目标检测与跟踪）采用OpenCV + MobileNet SSD + KCF。本文已分享经验和记录开发过程为主，推荐使用其他更好的无人机模块和图像识别算法。

知识基础：Linux、Python 3、STM32（嵌入式相关）

解释一下为什么要用Linux，其实只用Windows也可以，但实际运行中发现OpenCV的效率在Linux上更高。该方案建议安装Windows 7（驱动、软件支持较好） + Linux 双系统。

Python在人工智能中的影响不用多说，简单了解一下这种语言即可。

STM32F103 和 STM32F411（Cortex-M）是本方案种的无人机系统的处理器核心，并且采用FreeRTOS作为操作系统，所以关于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处理器设备提供了一个完整的开发环境。

二、四轴飞行器原理

四轴飞行器主要是由电机、电调、电池、浆叶、机架、遥控器、飞控组成。飞行器基本原理是通过飞控控制四个电机旋转带动浆叶产生升力，分别控制每一个电机和浆叶产生不同的升力从而控制飞行器的姿态和位置。四轴在空中可以实现八种运动，分别为垂直上升、垂直下降、向前运动、向后运动、向左运动、向后运动、顺时针改变航向、逆时针改变航向。飞行器在空中任何一种姿态都可以通过姿态角旋转后得到。

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客户端技术，发现其以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)    # 打开串口资源（Windows） 通过设备管理查看是COM几，确保驱动安装正确
 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），直到跟踪过程结束为止。

上述算法流程可用下图归纳表示：