前言: 本文介绍一种可行的解决方案来实现基于视觉感知的跟踪无人机. 从零开始搭建无人机系统工作量和难度 (以及钱) 都是非常大的, 所以在无人机系统的选择上, 选用正点原子开发的开源算法无人机 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 功能字定义代码如下:
- /* 上行帧头 */
- #define UP_BYTE1 0xAA
- #define UP_BYTE2 0xAA
- /* 下行帧头 */
- #define DOWN_BYTE1 0xAA
- #define DOWN_BYTE2 0xAF
- #define ATKP_MAX_DATA_SIZE 30
- /*ATKP 通讯数据结构 */
- typedef struct {
- u8 msgID;
- u8 dataLen;
- u8 data[ATKP_MAX_DATA_SIZE];
- }atkp_t;
四轴通信协议中下行指令有两种控制信息 DOWN_REMOTOR 指令 ID 是用来指定是遥控器下行给四轴的命令. 然后使用 Data[0]分区分发送控制命令和控制数据发送. 控制命令和控制数据枚举如下
- /* 遥控数据类别 */
- typedef enum
- {
- REMOTOR_CMD,
- REMOTOR_DATA,
- }remoterType_e;
控制命令主要是控制四轴实现一些功能性操作的命令, 比如一键起飞降落, 一键翻滚, 一键紧急停止等. 控制数据主要是发送给四轴姿态控制数据. 当 Data[0] == REMOTOR_CMD 时, Data[1]为控制命令; 当 Data[0]== REMOTOR_DATA 时, Data[1]之后为控制数据. 控制数据结构如下:
- /* 遥控控制数据结构 */
- typedef __packed struct
- {
- float roll;
- float pitch;
- float yaw;
- float thrust;
- float trimPitch;
- float trimRoll;
- bool ctrlMode;
- bool flightMode;
- bool RCLock; } remoterData_t;
- /* 关于飞行与控制模式枚举 */
- enum ctrlMode
- {
- ALTHOLD_MODE,
- MANUAL_MODE,
- };
- enum flightMode
- {
- HEAD_LESS,
- X_MODE,
- };
发送控制数据时, 数据格式如下:
当需要控制数据时, 先使用 remoterData_t 定义一个 send 结构体数据, 然 后调用 sendRmotorData((u8*)&send, sizeof(send)) 即可发送控制数据了. 代码示意如下:
- remoterData_t send;
- send.roll = 0.0; ............/* 给 send 结构体赋值 */
- sendRmotorData((u8*)&send, sizeof(send));
- /* 发送遥控控制数据 */
- void sendRmotorData(u8 *data, u8 len)
- {
- if(radioinkConnectStatus() == false)
- return;
- atkp_t p;
- p.msgID = DOWN_REMOTOR;
- p.dataLen = len + 1;
- p.data[0] = REMOTOR_DATA;
- memcpy(p.data+1, data, len);
- radiolinkSendPacket(&p);
- }
通过以上代码和表格我们就能知道发送 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 官方提供的微型 Wi-Fi 摄像头并不好用, 它使用私有的通信协议, 视频流编码格式为 H.264, 只能按照提供的客户端软件来进行访问, 能达到 20FPS. 通过分析其 web 客户端技术, 发现其以 GCI 协议为访问接口, 但是并不包含视频流的 GCI 指令, 只有 snapshot 的指令, 也就是发送截屏的指令, 返回一个 JPG 格式图片, 最高只能达到 8FPS.
PyCharm 安装 opencv-python,imutils 包, 连接 Minifly, 通过 Python 我们可以实现传图:
- import cv2
- import imutils
- # CGI IPcamare
- url = 'http://192.168.1.1:80/snapshot.cgi?user=admin&pwd='
- # im.src = "videostream.cgi?stream="+Status.sever_push_stream_number+"&id="+d.id;
- # url = 'http://192.169.1.1:80/
- # url = 'http://192.168.1.1:80/videostream.cgi?user=&pwd=&resolution=32&rate=0'
- # url = 'http://192.168.1.1:80/livestream.cgi?user=admin&pwd='
- cnt = 0
- while True:
- timer = cv2.getTickCount()
- cap = cv2.VideoCapture(url)
- fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer)
- if cap.isOpened():
- cnt += 1
- width, height = cap.get(3), cap.get(4)
- print(cnt, '[', width, height, ']')
- ret, frame = cap.read()
- frame = imutils.resize(frame, width=640)
- # frame = cv2.flip(frame, -180)
- cv2.putText(frame, "FPS :" + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
- cv2.imshow('frame', frame)
- else:
- print("Error")
- break
- if cv2.waitKey(1) & 0xFF == ord('q'):
- break
- cap.release()
- cv2.destroyAllWindows()
4.2 串口数据
第三章中已经分析了 ATKP 包的控制指令及控制数据格式, 现在我们需要用 Python 构造能够生成这些格式的数据, 并且通过串口发送到 Minifly 遥控器. 需要安装 pyserial 包.
同样先以简单的控制指令为例如一键起飞 / 降落:
- # coding=utf-8
- import serial
- import time
- cmd_onekey_fly = 'AAAF50020003AE' # 一键起飞 / 降落
- ser = serial.Serial('COM7', 500000, timeout=0.5) # 打开串口资源
- u_byte = bytes.fromhex(cmd_onekey_fly) # 字符串形式转为十六进制字节形式
- ser.write(u_byte) # 发送到串口 (遥控器) 实现一键起飞
- time.sleep(3)
- ser.write(u_byte) # 发送到串口 实现一键降落
- ser.close() # 关闭资源
如果要发送控制数据, 应先构造一个生成数据字符串的过程, 以逐步拼接的方式完成:
固定的帧头, LEN,data[0] + 浮点数小端字节序 + 控制模式 + 对齐字节 + 校验和
给出代码:
- # coding=utf-8
- import serial
- import struct
- import time
- def float_to_hex(data): # float --> Hex 小端字节序
- return (struct.pack('<f', data)).hex()
- cmd_onekey_fly = 'AAAF50020003AE' # 一键起飞 / 降落
- cmd_stop = 'AAAF50020004AF' # 紧急停机
- cmd_Head = 'AAAF501D01' # 控制信息头 AA AF[HEAD] 50[REMOTER] 1D 01(data[1-29])
- Trim = '0000000000000000' # Trim 信息(不校准)
- Mode = '00000000' # 飞行控制模式 u8-CtrlMode bool-FlyMode bool-RCLok 1byte - 字节对齐
- send_str = ''
- flydata = [5, 0, 0, 50] # 飞行数据[rol - 滚转角, pit - 俯仰角, yaw - 偏航角, thr - 油门]
- send_str = cmd_Head + float_to_hex(flydata[0]) + float_to_hex(flydata[1]) + float_to_hex(flydata[2]) + \
- float_to_hex(flydata[3]) + Trim + Mode
- u_byte = bytes.fromhex(send_str)
- checksum = 0
- cnt = 0
- for a_byte in u_byte:
- checksum += a_byte
- cnt = cnt + 1
- H = hex(checksum % 256)
- print(H, cnt)
- print(send_str)
- send_str = send_str + H[-2] + H[-1]
- if H[-2] == 'x': # 0xF -> 0x0F
- send_str = send_str + '0' + H[-1]
- 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 四轴, Wi-Fi 图传模块, 光流模块
上文已提到 MiniFly 并不能同时扩展多个模块, 必须做如下改动:
Wi-Fi 摄像头模块最外面一层 PCB 板 (用于固定到四轴模块的母排) 用电烙铁拆卸, 用导线引出 VCC,GND 供电, 连接一个微型拨动开关, 再把导线焊接在四轴 PCB 的电池连接处, Wi-Fi 摄像头模块位置如图, 摄像头用泡沫胶固定, 再固定光流模块, 将摄像头模块压住.
注意: 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 来将所有模块结合起来.
由于四轴自身大小和升力的限制, 摄像头的选用变得有限, 在传图速度和稳定性上并不满意, 时常丢失跟踪目标. 虽然无人机采用了高精的传感器, 但实际运行过程中因为环境的影响, 飞行状态的效果有时并不太理想, 导致对跟踪算法反馈的信息得不到及时调整, 软硬件还需进一步优化.
来源: https://www.cnblogs.com/kryo/p/11278565.html