基于STM32F4的视觉追踪与手机控制运动平台 - STM32团队 ST意法半导体中文论坛

基于STM32F4的视觉追踪与手机控制运动平台精华

[复制链接]

原来是他发布时间：2015-7-23 12:08

技术帖
文章封面:	-
文章简介:	-

1. 引言

智能手机平台与STM32F4的通信可以有多种方式，本方案选用STM32F4平台进行Android设备的外部扩展，两平台间使用USB接口或蓝牙接口进行通讯。STM32平台可以实现自主视觉追踪，同时通过通信接收Android设备发来的指令，实现人机交互。

2. 系统指标

针对当前智能监控系统的发展状况，本系统需要实现的系统指标如下表所示：

表1 系统指标

视觉帧速	15fps
可识别最高移动速率	3m/s (3.1mm镜头下，距离2m)
视野范围	102°（3.1mm镜头下）
控制周期	5ms
最高移动速度	10m/s
驱动板输出能力	200w
姿态控制精度	±0.5°

3. 系统方案

3.1系统简介

整个系统分为运动平台和视觉追踪平台两部分。运动平台主要由Android设备、扩展板和执行机构构成，执行机构可以是很多种类的运动平台，本项目中是由电机驱动、电机、机架组成的全向移动平台。视觉追踪部分由摄像头、主控板和LCD构成。运动平台中的扩展板与视觉追踪部分的主控板均由同一块STM32F4 Discovery来实现。一种可实现的具体系统架构如图1、图2所示。它们的功能如下：

图1 视觉跟踪系统

图2 运动机构组成

l 主控板：扩展板是一块以STM32F407VGT6为主控的核心控制板。其上有1路USB Host接口用以连接Android设备，3路USART接口以实现和电机驱动器的通信。扩展板的软件基于ST官方提供的USBH ADK类库，以USBHost方式与手机进行通讯。

l 摄像头：摄像头设备为视觉输入传感器，本平台选用OV7725和MT9M111两种CMOS传感器作为视觉输入设备，其中，OV7725能提供VGA分辨率60fps输出，而MT9M111能提供1280*960分辨率，15fps输出，且在色彩上更好一些，对于本次应用，两种平台均能成功实现。

l Android设备：Android设备是一个运行有Android系统的硬件平台，最常见的为Android智能手机。它的作用是提供一个用户交互界面，采集自身传感器信息，将用户意图转化为信号量，并提供提示界面和拟物化的操作面板。在本项目中，Android手机负责收集自身的加速度计、陀螺仪、电子罗盘的信息，通过DCM互补滤波以及四元数矩阵融合转化成为Yaw，Pitch，Roll三个姿态角。通过人操纵手机改变手机的姿态，从而代替手柄的功能。同时，屏幕上放置按钮控件以实现抓取、放开等机械臂动作的控制。

l 电机驱动：电机驱动是一块基于STM32F103CBT6的控制板，通过USART接口与上位机通讯获得指令（目标速度、目标位置等），生成不同占空比的PWM波驱动一个H全桥。同时，电机上的编码器接回电机驱动板，以实现速度的闭环控制。

l 电机：电机是直接执行机构，本项目中使用有刷直流电机，电机自带两相正交旋转编码器，可以实现速度的大小和方向的测量。电机以及其安装的机架构成了3自由度机械臂执行机构，可以实现弯曲、伸展、转向等动作。

3.2系统算法分析

3.2.1 视觉算法

本项目旨在进行于Cortex-M4平台上的视觉处理实时算法的可行性实验，鉴于此平台较好的运算性能，可以实现一些实时性较高的新型算法。本实力实现的是HSL色域shift算法进行的特征匹配与标记，最终实现对特定色调色块的定位，从而实现追踪。在目标色调提取时，通过统计中央1/2区域的HUE直方图来获得。当光线变化或者移动太快造成目标失锁，程序可以根据直方图的数据进行参数调整，从而提高了追踪算法的鲁棒性。得益于STM32F4不错的性能，本算法在ROI内搜索开销仅有1ms左右，全图搜索也仅需15ms，完全可以在帧传输间隙完成搜索。

图3 RGB与HSL色彩空间

3.2.2 手机端姿态解算

嵌入式的算法开发需要在算法的性能和实现复杂度之间进行折中。对于多传感器姿态融合目前一般有三种方法：欧拉法、方向余弦法和四元数法，其中，欧拉法通过直接求解欧拉角，方法较为直观，但方程会出现奇点，故不能全姿态工作；方向余弦法没有退化问题，但要求解9个微分方程，计算量太大；四元数法也没有方程退化问题，且计算量较小，因此较适合本项目的运用。四元数示意如图4所示。

图4 四元数示意图

本项目多传感器融合使用DCM互补滤波。互补滤波算法相较EKF卡尔曼滤波而言计算量较小，且在弱耦合情况下DCM的效果也很好，故采用互补滤波。算法流程如图5所示。

图5 互补滤波示意图

在实际使用中，可以看出陀螺仪与加速计具有较为互补的传感器特性。加速度计具有长期稳定特性但响应速度慢，而陀螺仪实时性很好，却因为零漂、温漂、非线性等的存在而会积分出累积误差，自身无法消除。从数据看来，融合之前加速计曲线在震动之下稳定性很差，而陀螺仪曲线不受影响，所以在互补滤波中陀螺仪的融合权重较高，而使用加速计进行长期融合修正陀螺仪的累积误差。如图6所示，红色和黄色分别为融合前加速计、陀螺仪获得的角度曲线，绿色为融合后的角度曲线（Pitch）。

图6 融合前后的传感器曲线

4. 系统硬件设计

本系统最终要实现传感器的采集、姿态的结算、控制逻辑的生成和控制等任务，为了缩减开发周期，使用比较常见的LG Nexus 4手机作为Android平台，扩展板使用意法官方的低成本评估板STM32F4 Discovery。

下图7给出了使用到的模块的简化原理图（USB连接），图8为通过蓝牙连接的简化原理图，主要区别在于，蓝牙服务模块存在于一个Service当中，当手机关闭屏幕、回到主界面或是切换至其他进程时，在后台运行的Service可以保证蓝牙服务不断线。各个模块的详细介绍如下：

图7 模块简化原理图

图8 蓝牙版简化原理图

l Android设备：此为LG Nexus 4手机，配备1.5Ghz Qualcomm Snapdragon APQ8064 四核处理器，Adreno320图形显示处理器，2GB LPDDR3 RAM

l STM32F407：此为STM32F407VGT6 主控芯片，基于ARM Cortex-M4架构，168Mhz主频，具备完备的通信接口与连接能力，集成32位浮点运算单元和单周期多指令执行器，可以完成一些较复杂的控制逻辑和算法

5. 系统软件设计

在本系统中，软件设计分为三个部分，第一部分为Android软件设计，第二部分为扩展板软件设计，第三部分为驱动板软件设计。Android设备程序为实现一个Activity类，在这个类中构造了读取传感器、姿态解算、读取按钮状态、发送指令、刷新UI等方法。在扩展板软件设计中，主要为USBH类的实现和消息读取，转化为电机目标速度。在电机驱动板程序中，主要实现了UART读取、PWM生成、正交编码器读取等函数。简要的软件流程如图9所示。蓝牙连接的流程图和图9基本一致，只是将USB发送和接收数据部分替换为蓝牙方式。