二维运动目标姿态检测和轨迹追踪系统

邱鸿鑫　佘淑仪　林玲璇　梁梓豪　徐福仁　熊爱民

摘   要： 传统运动目标定位系统存在可移动性差、实时性受限、对目标姿态检测不适用等缺陷。提出一种基于六轴传感器MPU6050的二维运动目标姿态检测和轨迹追踪系统。系统采用MPU6050传感器获取目标运动数据，将数据传送到STM32F103RCT6主控芯片进行处理;利用无线通信模块将数据处理结果传输至PC端;在PC端实时显示目标的运动姿态和运动轨迹。通过静止、加速、转弯等运动对系统进行测试，总体路线偏差控制在6.25%以内，达到了设计要求。可以在汽车行驶状态判断、车载GPS盲区导航、室内机器人定位等领域推广使用。

关键词： 姿态检测;轨迹追踪;MPU6050;STM32F103RCT6;无线通信

中图分类号：TP37    文献标识码：A    文章编号：2095-8412 （2020） 04-063-06

工业技术创新 URL： http：//gyjs.cbpt.cnki.net    DOI： 10.14103/j.issn.2095-8412.2020.04.012

引言

为了检测运动目标的姿态和轨迹，进而判断运动目标是否按照预定模式正常工作，已经开发了各种定位技术和定位系统。传统的室内外定位系统具有较高的测量精度，但它们往往对使用环境有特定要求，也存在目标定位受外界参考系限定、测量量程受基准参考系制约、对目标姿态检测不适用等缺陷[1]。

张秀再等[2]提出了利用惯性元件进行轨迹追踪的系统，其利用串口通信将数据传输到PC端，这种有线传输的方式在测试和实际使用中存在可移动性差等缺点，会受到一定限制。无线通信可移动性好，但在GPS信号不佳的环境下会受到实时性的限制[3]。

为了解决上述问题，本文设计一种基于六轴传感器MPU6050的二维运动目标姿态检测和轨迹追踪系统。系统通过MPU6050传感器获取运动目标的加速度、姿态角等参数，进行数据处理，进而将处理结果无线传输至PC端;PC端将实时显示目标的运动姿态并描绘其运动轨迹，具有设计简单、准确率高、实时性强等优点，可以在室内环境及GPS信号不好的环境下实现运动物体的检测、定位和追踪。

1 系统构成与工作原理

1.1  系统构成

系统结构框架如图1所示。系统主要包括四个模块：运动感测模块、主控模块、无线通信模块、PC端处理模块。运动感测模块采用六轴传感器MPU6050，通过IIC通信向主控模块提供加速度和姿态角信息。主控模块以STM32F103RCT6作为主控芯片，主要负责处理和计算运动感测模块传送的信息。无线通信模块采用ESP8266模块，将处理的结果数据传输至PC端。PC端处理模块令界面显示姿态检测的结果，并在二维坐标轴上描绘目标运动轨迹。

1.2  工作原理

本系统主要涉及室内定位，所以在无GPS信号或GPS信号不好的情况下，由运动感测模块向主控模块提供运动目标的各轴加速度和姿态角信息。主控模块在对原始的运动数据进行滤波及积分等处理后，结合不同姿态的数据变化特征，便可以推断运动目标的姿态并计算其轨迹坐标。最后，利用无线通信模块将各项结果数据传输至PC端，使用者便可在现有的界面上实时获取运动目标的姿态信息、追踪其在二维平面上的轨迹了。工作原理图亦如图1所示。

2  系统软件设计

系统STM32F103RCT6芯片运行程序用C语言编写，包括MPU6050原始数据处理程序、姿态检测判断程序、轨迹坐标计算程序。PC端执行用Python编写的界面显示程序，即接收无线通信模块发送的各项结果数据。姿态检测结果将在界面中予以显示。

2.1  原始數据处理流程

首先，MPU6050传感器采集得到运动目标的三轴加速度和三轴旋转角速度，其中三轴旋转角速度经过DMP库的计算处理被转换为姿态角。三轴加速度包括了空间直角坐标系下x、y、z三轴方向上的加速度数据;姿态角包括物体绕空间直角坐标系下x、y、z三轴方向旋转的角度，它们分别称为横滚角、俯仰角、航偏角。接着，由于采集到的原始数据存在较大的噪声干扰，且受干扰程度是随机的，因此难以测得精确值。为了保证加速度数据的实时性和准确性，需要再对加速度进行卡尔曼滤波和加权移动平均滤波。原始数据处理流程如图2所示。

卡尔曼滤波通过建立目标的状态模型并估计目标的运动速度和加速度，对目标的未来点进行预测[4]，使估计值尽可能地接近真实值。尽管卡尔曼滤波得到的是一个最优估计，但加速度作为一个模拟量，在短时间内不会出现极剧烈的波动变化。为经过卡尔曼滤波后的数据选择合适的帧长度进行加权移动平均滤波，权衡好平均滤波的滞后性和响应度，可进一步减小最优估计值的误差，提高对数据变化的响应能力[5]。数据处理结果对比如图3所示。

2.2  姿态检测判断程序

姿态检测判断程序包括加减速判断程序和转弯判断程序，其姿态判断流程如图4所示。

对于加减速判断，首先将最初的一部分加速度样本数据取平均值，作为初始状态的基准值，接着让后续的加速度都减去该基准值，得到实际偏差值。根据一段时间内实际偏差值的变化趋势及程度，利用阈值法判断目标处于加速还是减速的姿态。

对于转弯判断，为防止航偏角在一定时间内变化比较小而被误判为不变，首先将航偏角数据每20个分为一组，然后将每组数据逐个与前一组对应位置的数据作差，对差值大于和小于阈值的值分别进行计数。当某段时间内的计数达到一定数量时，则判定目标处于左转或右转的姿态，否则认为目标直线运动。

2.3  轨迹坐标计算程序

本系统以目标在追踪开始时刻的前进方向为二维平面x轴的正方向，以x轴正方向逆时针旋转90°的方向为二维平面y轴的正方向，即坐标系是在系统开始追踪目标轨迹的第一时间确定的。

由于系统无法保证处于完全水平状态，因此重力加速度会在x轴、y轴上产生分量，造成由位移计算的加速度数据不准确，因此需要对加速度数据进行偏移修正。具体做法是在开启MPU6050传感器后，前150个加速度样本保持不变，计算这部分样本的平均值，后续获取的加速度都减去该平均值，即得到偏移修正的加速度。

在静止状态下，经偏移修正后的加速度仍会在零点周围小范围波动，这种零点漂移是由MPU6050传感器的抖动等产生的，可视为非零均值噪声，该噪声在速度和位移的积分计算中会引起累积误差，因此考虑使用阈值法减小该噪声带来的影响。设定一个合适的阈值，在该阈值以下，任何加速度值都将被设置为零，从而保证了静止状态下代入积分计算的加速度值为零，确保后续计算得到的速度、位移数据的准确性。

以牛顿第二定律为基础建立运动学模型，加速度、速度、位移之间的关系为

（1）

（2）

其中，a是运动载体的加速度，v是运动载体的速度，s是运动载体的位移。式（1）和式（2）表明，对加速度进行一次时间积分可以得到运动速度，对运动速度再进行一次时间积分得到位移长度[6]。航偏角对位移长度进行偏转，即可计算得出实际位移矢量。若k-1时刻下运动目标的轨迹坐标已知，则可以计算出k时刻下目标的轨迹坐标。即

（3）

（4）

其中，为k时刻下的航偏角。轨迹坐标计算流程如图5所示。

3  PC端程序设计及界面设计

首先，在PC端配置好PyCharm +QTDesigner+PyUIC环境，使用QTdesigner布局设计用户界面，并生成.ui文件;然后，利用PyUIC将.ui文件转换为.py文件，并导入Pycharm;第三，实例化socket对象并绑定与无线通信模块对应的网络IP和端口;第四，开辟新的线程用于监听和接收端口数据，对不同类型数据进行分类筛选和临时存放;第五，编写界面文本显示函数，将姿态判断结果等数据实时显示于界面对应的文本框;第六，编写二维坐标轴轨迹函数，将轨迹坐标描绘成二维坐标轴上实时变化的曲线;最后，编写按钮点击事件的处理函数和主函数。

PC端界面效果展示如图6所示。

4  系统测试和数据分析

系统测试开始前，将MPU6050传感器水平放置，z轴方向垂直于水平面，x轴和y轴与水平面平行。电路板通过无线通信模块与PC端进行TCP数据透传，把姿态检测与轨迹追踪分开，单独进行测试。

在姿态检测测试阶段，为确保系统可在不同类型的运动目标上适用，分别在室内静止环境、室外电瓶车骑行环境、室外四轮玩具小车行驶环境三个条件进行了相应的測试，测试结果如表1所示。

在轨迹追踪测试阶段，控制室外四轮玩具小车在预定的路径上分别按直线行驶和左转弯行驶，在PC端界面的坐标轴上得到行驶轨迹，如图7所示，描绘的追踪轨迹和目标实际运动轨迹基本一致。

其余轨迹追踪的测试结果如表2所示。

由以上测试数据和结果可以得出：在误差允许范围内，系统对运动目标的静止、加速、转弯等姿态判断有较高的准确率，对目标在二维平面运动所描绘的轨迹与实际的运动轨迹基本一致，总体路线偏差控制在6.25%以内，达到了设计要求，可以推广使用。

5  结束语

本文设计了一种基于六轴传感器MPU6050的二维运动目标姿态检测和轨迹追踪系统。系统利用了MPU6050传感器模块、无线传输模块等，在室内环境及GPS信号不好的环境下实现运动物体的检测、定位和追踪。

测试表明：系统不仅能较为准确地判断目标的运动姿态，也可以进行轨迹的追踪，并实时描绘轨迹曲线，总体路线偏差控制在6.25%以内。

系统具有体积小、操作简单、实时性高、运行稳定等特点，能为汽车行驶状态判断、车载GPS盲区导航、室内机器人定位等研究提供一定的借鉴。

参考文献

[1] 胡三庆. 基于MEMS加速度传感器的空间运动轨迹追踪系统设计与实现[D]. 武汉： 华中科技大学， 2009.

[2] 张秀再， 王跃飞， 王锡宁， 等. 基于MPU6050的运动目标轨迹追踪系统[EB/OL]. 北京： 中国科技论文在线 [2018-09-30]. http：//www.paper.edu.cn/releasepaper/content/201809-73.

[3] 陈培才， 施文振， 田敬北， 等. 热水器远程智能控制系统设计[J]. 科技风， 2018（20）： 88.

[4] 王建东， 王亚飞， 张晶. 基于卡尔曼滤波器的运动目标跟踪算法[J]. 数字通信， 2009， 36（6）： 53-57.

[5] Pitale A N， Bendre A A， Jadhav P M， et al. Signal conditioning algorithms on accelerometers in an Inertial Navigation System （INS）[C]// International Conference on Signal & Information Processing. IEEE， 2016.

[6] 陈义华. 基于加速度传感器的定位系统研究[D]. 厦门： 厦门大学， 2006.

作者简介：

邱鸿鑫（1999—），男，广东汕头人，华南师范大学，本科生。主要研究方向：电子信息。

熊爱民（1970—），通信作者，男，江西吉安人，华南师范大学，硕士，副教授。主要研究方向：电子与通信技术。

E-mail： 77057181@qq.com

（收稿日期：2020-05-12）

Two-dimensional Motion Target Gesture Detection and Trajectory Tracking System

QIU Hong-xin， SHE Shu-yi， LIN Ling-xuan， LIANG Zi-hao， XU Fu-ren， XIONG Ai-min

（College of Physics and Telecommunications Engineering， South China Normal University， Guangzhou 510006， China）

Abstract： The traditional motion target positioning system has some defects such as poor mobility， limited real-time performance and inapplicability to the target gesture detection. A two-dimensional motion target gesture detection and trajectory tracking system based on six-axis sensor MPU6050 is proposed. The system uses MPU6050 sensor to obtain the motion data of the target， transmits the data to STM32F103RCT6 main control chip for processing， adopts the wireless communication module to transmit the data processing results to the PC terminal， and displays the motion gesture and movement trajectory of the target in real-time at the PC terminal. The system is tested through motions including static， acceleration and turning， and the overall deviation of the route is controlled within 6.25%， which meets the design requirements. It can be promoted and used in the fields including vehicle driving state judgment， onboard GPS blind zone navigation and indoor robot positioning.

Key words： Gesture Detection; Trajectory Tracking; MPU6050; STM32F103RCT6; Wireless Communication