CN109029459A

CN109029459A - 一种运动目标轨迹追踪系统及基于该系统的计算方法

Info

Publication number: CN109029459A
Application number: CN201810818364.9A
Authority: CN
Inventors: 张秀再; 孙亚培; 宋佳; 李博远; 吴宇航
Original assignee: Nanjing University of Information Science and Technology
Current assignee: Nanjing University of Information Science and Technology
Priority date: 2018-07-24
Filing date: 2018-07-24
Publication date: 2018-12-18
Anticipated expiration: 2038-07-24
Also published as: CN109029459B

Abstract

本发明公开了一种运动目标轨迹追踪系统，基于MPU6050六轴传感器，以MSP430F169为主控芯片，本发明还公开了基于运动目标轨迹追踪系统的运动目标轨迹计算方法，通过MPU6050六轴传感器检测运动目标的加速度和角速度以及倾角等参数，进行静态误差校准后，通过串口传输到PC端。在PC端中对运动参数进行Kalman滤波消除随机噪声，再对滤波后的加速度数据做积分运算，得到运动目标在三维空间运动轨迹坐标位置，可以实现三维空间的物体运动轨迹追踪并计算出位移值。

Description

一种运动目标轨迹追踪系统及基于该系统的计算方法

技术领域

本发明涉及人机交互的目标轨迹测量与跟踪技术领域，尤其涉及一种运动目标轨迹追踪系统及基于该系统的计算方法。

背景技术

物体运动是一个无处不在的现象。随着科技的进步，人类正运用各种技术监测与追踪物体运动轨迹。在人机交互领域，激光追踪系统、磁空间追踪系统和基于空间机器视觉的定位系统使用最广泛，具有较高的测量精度，也存在受外界参考系限定、系统开销大、设备运作复杂等缺陷。磁空间追踪系统经常受到场源限制，容易受各种外界环境影响，如铁制品的屏障作用、其他磁场的干扰；激光追踪系统会受到激光源光照范围的制约，且激光光源常会被损坏，造成不必要的损失；基于三维机器视觉的定位系统通常因为摄像头的放置地点和拍摄的场景范围的影响，具有一定的局限性，且数据计算过程繁杂、系统开销大。所以，上述轨迹追踪系统用于运动目标跟踪、定位时，各有优缺点。因此，设计一种测量精度高、应用范围广的轨迹追踪系统具有较强的现实意义和实用价值。

发明内容

发明目的：本发明的目的在于解决现有的激光追踪系统、磁空间追踪系统和基于空间机器视觉的定位系统受外界参考系限定、系统开销大、设备运作复杂等缺陷，磁空间追踪系统经常受到场源限制，容易受各种外界环境影响，如铁制品的屏障作用、其他磁场的干扰；激光追踪系统会受到激光源光照范围的制约，且激光光源常会被损坏，造成不必要的损失；基于三维机器视觉的定位系统通常因为摄像头的放置地点和拍摄的场景范围的影响，具有一定的局限性，且数据计算过程繁杂、系统开销大的问题。

技术方案：为实现上述目的，本发明采用以下技术方案：

一种运动目标轨迹追踪系统，包括运动检测模块、控制模块、显示模块、串口通信模块和PC端处理模块，其中：

运动检测模块：采用MPU6050六轴传感器，检测运动目标的加速度和角速度参数；

控制模块：以MSP430F169为主控芯片，主要负责采集运动数据、控制串口通信和LCD显示，由程序对采集的加速度数据进行零偏校准，然后把校准数据传输给PC端处理；

显示模块：将MPU6050传感器的3轴加速度、角速度数据在LCD上显示；

串口通信模块：采用PL2303HX芯片，将运动数据传输到PC端；

PC端处理模块：在MATLAB环境中，对运动数据进行Kalman滤波，再对其进行两次积分，得到运动目标随时间移动的坐标点，连线绘出运动路径图。

进一步地，MPU6050的管脚XDA、XCL不与MSP430F169相连，管脚GND与MSP430F169共地，管脚VCC连接到MSP430F169的3.3V引脚，SDA引脚连接MSP430F169的P2.1口，SCL引脚连接MSP430F169的P2.0端。

进一步地，PL2303HX芯片的+5V连接到MSP430F169的+5V引脚，且共地；PL2303HX芯片的RXD引脚连接到MSP430F169的TXD即P3.4引脚，TXD引脚连接到MSP430F169的RXD即P3.5引脚。

进一步地，MSP430F169通过IIC总线与MPU6050六轴传感器连接。

一种运动目标轨迹追踪方法，包括以下步骤：

1)获得传输至PC端的各参数信息；

2)MPU6050传感器输出离散数据，所以曲线在采样时间间隔Δt足够小的情况下，能够简化为直线；这样就能把曲线拆分成无数个直角梯形，对v(t)进行积分也便是对拆分的直角梯形面积进行求解；

3)将v(n)、s(n)的推导结果运用到加速度传感器中，得到三维空间位移公式；

4)计算一次运动轨迹需要进行2*3*n次积分运算，根据实际位移简化为2*3*n次加法演算。

进一步地，所述步骤2)中，令初始条件s(t₀)＝0，即：

Δt＝t₁-t₀＝t₂-t₁＝...＝t_n-t_n-1，t₀为起始时刻，t₁、t₂…t_n为等间隔时间序列，令Δt为时间间隔；

离散域中n>1时,

由式2、式3可得：

由式4、式5可看出，求当前瞬时运动速度v(n)和运动位移s(n)，可由s(n-1)、v(n-1)、a(n-1)和当前a(n)计算出。

进一步地，所述步骤3)中将式4、式5中v(n)、s(n)的推导结果运用到加速度传感器中，可以得到三维空间位移公式为：

Kalman滤波方法，包括以下步骤：

预测阶段为：假设此时为k时刻，首先根据状态系统模型，可以基于系统上一时刻的状况而预计出当下的状态方程：

X(k|k-1)＝AX(k-1|k-1)+BU(k) (9)

式(9)中，X(k|k-1)是通过上一时刻的状态估计得到的当下状态，X(k-1|k-1)是上一时刻最优的预计值，U(k)是此刻对系统的控制量。

预测阶段也对估计值精确程度进行预计。系统状态的协方差也就是预计值，如式(10)所示：

P(k|k-1)＝AP(k-1|k-1)A^T+Q (10)

式(10)中，P(k-1|k-1)是X(k|k-1)的协方差，P(k-1|k-1)是X(k-1|k-1)的协方差。

更新阶段为：已知目前状况的估计值，再结合当前状况的观测值，可得当前状况的最优化估计值：

X(k|k)＝X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)] (11)

其中的Kg,就是Kalman增益(Kalman Gain),它可以因为不同的时刻而不断改变自身的值：

Kg(k)＝P(k|k-1)H^T/[HP(k|k-1)H^T+R] (12)

虽然现在已经得到k时刻系统状态最优的预计值X(k|k),但是还需要更新P(k|k)来保证Kalman滤波过程能循环工作到最终。

P(k|k)＝[I-Kg(k)H]P(k|k-1) (13)

I为单位矩阵。随机线性离散系统Kalman滤波的基本步骤就是依靠以上5个公式完成的。

有益效果：本发明与现有技术相比：

系统以MSP430F169为主控芯片，通过MPU6050六轴传感器检测运动目标的加速度和角速度以及倾角等参数，进行静态误差校准后，通过串口传输到PC端。在PC端中对运动参数进行Kalman滤波消除随机噪声，再进行积分运算，从而得到目标在三维空间运动的轨迹坐标位置，最终绘制出运动轨迹线路并计算位移值。该系统实现了对运动轨迹的测量与追踪，具有较强的现实意义和实用价值。

附图说明

图1为本发明时间-速度曲线；

图2为本发明系统框图；

图3为本发明MSP430F169单片机电路图；

图4为本发明MPU6050传感器电路图；

图5为本发明LCD显示电路图；

图6为本发明PL2303HX串口模块电路图；

图7为本发明单片机主程序流程图；

图8为本发明数据处理算法流程图；

图9为本发明加速度数据滤波前后对比图；

图10为本发明运动目标线路图。

具体实施方式

下面结合附图和实施例对本发明进行进一步的说明。

本发明为一种基于MPU6050的运动目标轨迹追踪方法及系统，具体实现方法如下：硬件电路设计

1.1单片机模块

系统采用TI公司生产的16位超低功耗与高强处理能力微控制器MSP430F169控制硬件系统。该芯片采用“冯诺依曼”结构,它的随机存储内存(RAM)、只读内存(ROM)和所有外部模块都在相同的地址空间内，寻址范围和时钟频率分别可达到62KB与8MHz。而且它的内部包含2个定时器、12位ADC与2个UART口和48个I/O口等，所以能快速地完成对外部传感器的初始化与应用。

本系统的传感器采集数据通过IIC总线传输，IIC总线是一种串行数据总线，它只有两根双向信号线，分别是双向的数据线SDA和双向时钟线SCL，数据线是用来传输数据，时钟线是用来传输时钟脉冲。微控制器是整个设计中的最核心的部分，是由晶振电路(本系统采用8MHZ)和复位电路组成。MSP430F169最小系统如图3所示。

1.2 MPU-6050检测模块

MPU6050作为全球第一例9轴运动处理传感器，具有高度集成化的特点，内部包含MEMS陀螺仪和加速度计，并且测得的运动数据直接以数字量输出。本传感器集成的陀螺仪测量范围为±250、±500、±1000、±2000°/秒(dps)，加速度计测量范围为±2、±4、±8、±16g，可根据实际需要选择测量范围。

MPU6050的管脚XDA、XCL不与单片机相连，管脚GND与单片机MSP430F169共地，管脚VCC连接到单片机的3.3V引脚，SDA引脚连接单片机P2.1口，SCL引脚连接单片机P2.0端。MSP430F169通过IIC总线与MPU-6050进行串行通信，将采集的运动数据传输到PC端。MPU6050传感器电路如图4所示，当MPU6050正常工作时，发光二极管会发出黄绿色的光。

1.3 LCD显示电路

12864液晶屏是一种采用低功耗CMOS技术实现的点阵图形LCD模块，具有显示零辐射、低功耗、屏幕调节方便、画面稳定等特点。12864液晶屏内部含有KS0108B/HD61202控制器，通过内部的128×64位映射DDRAM，完成128点、64点显示，内置国际化简体中文字库。系统工作时，12864液晶屏实时显示X、Y、Z方向加速度、角度信息。12864液晶屏显示电路如图5所示。

1.4 PL2303串口模块

串口通信模块采用PL2303HX芯片，即PL2303USB TO TTL模块，是一种RS232-USB接口转换器，能使RS232全双工异步串行通信设备方便地和USB功能接口连接。该模块内部包含USB功能控制器、收发器、振荡器，能够实现USB信号与RS232信号的转换。该器件作为USB/RS232双向转换器，对USB数据格式与RS232信息流格式进行互换。PL2303的通讯波特率高达6Mb/s，具有强大的高兼容驱动能力，能在众多操作系统上模拟成传统COM端口,从而可简便地转换成USB接口应用。

系统将PL2303HX芯片的+5V连接到单片机的+5V引脚，且共地。PL2303HX芯片的RXD引脚连接到MSP430F169的TXD即P3.4引脚，TXD引脚连接到MSP430F169的RXD即P3.5引脚。系统传输数据时，采集的运动数据实时由PL2303串口传输到PC端的串口调试助手并保存。PL2303HX串口模块电路如图6所示。

软件设计

系统的单片机运行程序是C语言编写，包括主程序、MPU6050初始化数据读取程序、零偏校准程序、LCD显示程序、串口通信程序等。而上位机执行算法是MATLAB程序，包括消除随机误差的Kalman算法、加速度积分算法、轨迹绘图。

根据MPU6050的寄存器读写函数，对传感器进行初始化，主要是参数配置，如采样率、滤波频率等，如无特殊要求使用典型值。在程序中，通过调用MPU6050_Get_Data()函数获取运动数据信息，如MPU6050_Get_Data(GYRO_ZOUT_H)表示读取16位Z轴角速度数据。

为消除静态误差，读取200次加速度的值，取平均值对原始数据进行校正。然后，通过调用RS232_jiao()、RS232_jia()和Display10BitData()函数将运动数据显示到12864显示屏并传输到PC端。单片机主程序流程如图7所示。

PC端数据处理软件主要功能为：对接收到的运动数据进行Kalman滤波，通过对加速度值做两次积分运算来实时获得运动过程中的位置坐标点，用程序绘制运动轨迹。PC端数据处理算法以MATLAB程序编写，数据处理算法流程如图8所示。

Kalman滤波方法，包括以下步骤：

X(k|k-1)＝AX(k-1|k-1)+BU(k) (9)

P(k|k-1)＝AP(k-1|k-1)A^T+Q (10)

X(k|k)＝X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)] (11)

Kg(k)＝P(k|k-1)H^T/[HP(k|k-1)H^T+R] (12)

P(k|k)＝[I-Kg(k)H]P(k|k-1) (13)

当检测模块完成对物体运动数据的采集后，采用串口模块传输数据给PC端的串口调试助手，获得实时测量的加速度与角速度参数，并保存为“AccDataok.txt”文件。PC端即可执行MATLAB程序对数据进行处理，并绘出目标运动轨迹曲线。

实验结果与分析

测试实验时，将MPU6050传感器水平放置，Z轴方向垂直于水平面，X与Y轴与水平面平行。MSP430F169通过PL2303HX串口与PC端相连，打开串口调试助手，设置串口波特率为9600，串口选择与电脑设备管理器中的串口号一致。手持传感器在水平面1m²范围内做曲线移动。系统以125Hz采样频率采集的13组运动数据如表1所示，第一、二、三列分别为X、Y、Z方向的加速度值，单位为g；第四、五、六列分别为X、Y、Z方向的角速度值，单位为dps。

表1

采集的运动数据存在一些噪声干扰，如传感器自身误差、系统随机误差等，降低了数据的准确性。因此，通过卡尔曼滤波消除加速度数据中的干扰，得到更加准确的数据，卡尔曼滤波前后的加速度数据对比如图9所示。

对滤波后的加速度数据进行二次积分，得到三维空间目标的运动路径如图10所示，目标物在水平面做曲线运动，追踪轨迹和实际运动轨迹基本一致。

为了检测系统对运动物体位移测量的精度，分别对20cm、50cm、80cm和110cm的位移进行运动测量，且每种距离测量15组，计算平均测量位移及误差如表2所示。

表2

由表2可知，在对20cm到110cm运动物体位移测量中，平均误差不超过7％，能够比较精确地实现对运动目标物位移的测量。

Claims

1.一种运动目标轨迹追踪系统，其特征在于：包括运动检测模块、控制模块、显示模块、串口通信模块和PC端处理模块，其中：

运动检测模块：采用MPU6050六轴传感器；

控制模块：以MSP430F169为主控芯片；

串口通信模块：采用PL2303HX芯片。

2.根据权利要求1运动目标轨迹追踪系统，其特征在于：MPU6050的管脚XDA、XCL不与MSP430F169相连，管脚GND与MSP430F169共地，管脚VCC连接到MSP430F169的3.3V引脚，SDA引脚连接MSP430F169的P2.1口，SCL引脚连接MSP430F169的P2.0端。

3.根据权利要求1运动目标轨迹追踪系统，其特征在于：PL2303HX芯片的+5V连接到MSP430F169的+5V引脚，且共地；PL2303HX芯片的RXD引脚连接到MSP430F169的TXD即P3.4引脚，TXD引脚连接到MSP430F169的RXD即P3.5引脚。

4.根据权利要求1运动目标轨迹追踪系统，其特征在于：MSP430F169通过IIC总线与MPU6050六轴传感器连接。

5.一种基于如权利要求1所述的运动目标轨迹追踪系统的计算方法，其特征在于：包括以下步骤：

1)获得传输至PC端的各参数信息，并进行Kalman滤波消除随机噪声；

6.根据权利要求5所述的基于运动目标轨迹追踪系统的计算方法，其特征在于：所述步骤2)中，令初始条件s(t₀)＝0，即：

Δt＝t₁-t₀＝t₂-t₁＝...＝t_n-t_n-1，t₀为起始时刻，t₁、t₂…t_n为等间隔时间序列，Δt为时间间隔；

离散域中n>1时,

由式2、式3可得：

7.根据权利要求5所述的基于运动目标轨迹追踪系统的计算方法，其特征在于：所述步骤3)中将式4、式5中v(n)、s(n)的推导结果运用到加速度传感器中，可以得到三维空间位移公式为：

[x₀,y₀,z₀]＝[s_x0,s_y0,s_z0] (6)

8.根据权利要求5所述的基于运动目标轨迹追踪系统的计算方法，其特征在于：所述步骤1)中，Kalman滤波方法，包括以下步骤：

X(k|k-1)＝AX(k-1|k-1)+BU(k) (9)

式(9)中，X(k|k-1)是通过上一时刻的状态估计得到的当下状态，X(k-1|k-1)是上一时刻最优的预计值，U(k)是此刻对系统的控制量；

系统状态的协方差也就是预计值，如式(10)所示：

P(k|k-1)＝AP(k-1|k-1)A^T+Q (10)

式(10)中，P(k-1|k-1)是X(k|k-1)的协方差，P(k-1|k-1)是X(k-1|k-1)的协方差；

X(k|k)＝X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)] (11)

其中的Kg,就是Kalman增益，它可以根据不同的时刻而不断改变自身的值，如下式：

Kg(k)＝P(k|k-1)H^T/[HP(k|k-1)H^T+R] (12)

虽然现在已经得到k时刻系统状态最优的预计值X(k|k),但是还需要更新P(k|k)来保证Kalman滤波过程能循环工作到最终，如下式：

P(k|k)＝[I-Kg(k)H]P(k|k-1) (13)

I为单位矩阵。