CN208621076U - A moving target trajectory tracking system - Google Patents

Abstract

The utility model discloses a kind of movement objective orbit tracing systems, based on six axle sensor of MPU6050, using MSP430F169 as main control chip, using the design of the utility model, the parameters such as acceleration and angular speed and the inclination angle of moving target are detected by six axle sensor of MPU6050, after carrying out static error calibration, the end PC is transferred to by serial ports.Kalman filter Removing Random No is carried out to kinematic parameter in the end PC, integral operation is done to filtered acceleration information again, moving target is obtained in three-dimensional space motion trajectory coordinates position, the movement locus of object that three-dimensional space may be implemented is tracked and calculates shift value.

Description

A moving target trajectory tracking system

technical field

The utility model relates to the technical field of target trajectory measurement and tracking of human-computer interaction, in particular to a moving target trajectory tracking system.

Background technique

Object motion is a ubiquitous phenomenon. With the advancement of technology, humans are using various technologies to monitor and track the movement of objects. In the field of human-computer interaction, laser tracking systems, magnetic space tracking systems and positioning systems based on space machine vision are the most widely used and have high measurement accuracy. . The magnetic space tracking system is often limited by the field source and is easily affected by various external environments, such as the barrier effect of iron products and the interference of other magnetic fields; the laser tracking system is limited by the illumination range of the laser source, and the laser light source is often damaged, causing Unnecessary losses; positioning systems based on 3D machine vision usually have certain limitations due to the location of the camera and the scope of the shooting scene, and the data calculation process is complicated and the system overhead is high. Therefore, each of the above-mentioned trajectory tracking systems has advantages and disadvantages when used for moving target tracking and positioning. Therefore, it has strong practical significance and practical value to design a trajectory tracking system with high measurement accuracy and wide application range.

Utility model content

Purpose of the utility model: The purpose of the utility model is to solve the defects of the existing laser tracking system, the magnetic space tracking system and the positioning system based on space machine vision, which are limited by the external reference system, the system overhead is large, and the equipment operation is complicated. Often limited by the field source, it is easily affected by various external environments, such as the barrier effect of iron products and the interference of other magnetic fields; the laser tracking system is restricted by the illumination range of the laser source, and the laser light source is often damaged, causing unnecessary losses. ; The positioning system based on 3D machine vision usually has certain limitations due to the influence of the placement location of the camera and the scope of the shooting scene, and the data calculation process is complicated and the system overhead is high.

Technical scheme: In order to realize the above-mentioned purpose, the utility model adopts the following technical scheme:

A moving target trajectory tracking system includes a motion detection module, a control module, a display module, a serial communication module and a PC-side processing module, wherein:

Motion detection module: MPU6050 six-axis sensor is used to detect the acceleration and angular velocity parameters of moving targets;

Control module: MSP430F169 is the main control chip, which is mainly responsible for collecting motion data, controlling serial communication and LCD display. The program performs zero offset calibration on the collected acceleration data, and then transmits the calibration data to the PC for processing;

Display module: display the 3-axis acceleration and angular velocity data of the MPU6050 sensor on the LCD;

Serial communication module: use PL2303HX chip to transmit motion data to PC;

PC-side processing module: In the MATLAB environment, Kalman filtering is performed on the motion data, and then two integrations are performed to obtain the coordinate points of the moving target moving with time, and the motion path diagram is drawn by connecting lines.

Further, the pins XDA and XCL of the MPU6050 are not connected to the MSP430F169, the pin GND shares the ground with the MSP430F169, the pin VCC is connected to the 3.3V pin of the MSP430F169, the SDA pin is connected to the P2.1 port of the MSP430F169, and the SCL pin is connected P2.0 side of MSP430F169.

Further, the +5V of the PL2303HX chip is connected to the +5V pin of the MSP430F169, and the ground is common; the RXD pin of the PL2303HX chip is connected to the TXD of the MSP430F169, that is, the P3.4 pin, and the TXD pin is connected to the RXD of the MSP430F169, that is, the P3.4 pin. 5 pins.

Further, MSP430F169 is connected with MPU6050 six-axis sensor through IIC bus.

A method for tracking a moving target trajectory, comprising the following steps:

1) Obtain each parameter information transmitted to the PC;

2) The MPU6050 sensor outputs discrete data, so the curve can be simplified into a straight line when the sampling time interval Δt is small enough; in this way, the curve can be split into countless right-angled trapezoids, and the integration of v(t) is the same as The area of the split right-angled trapezoid is solved;

3) Apply the derivation results of v(n) and s(n) to the acceleration sensor to obtain the three-dimensional spatial displacement formula;

4) Calculating a motion trajectory requires 2*3*n integral operations, which is simplified to 2*3*n additions according to the actual displacement.

Further, in the step 2), let the initial condition s(t ₀ )=0, that is:

Δt=t ₁ -t ₀ =t ₂ -t ₁ =...=t _n -t _n-1 , t ₀ is the starting time, t ₁ , t ₂ ... t _n are time series at equal intervals, let Δt be time interval;

When n>1 in the discrete domain,

From Equation 2 and Equation 3, we can get:

It can be seen from Equation 4 and Equation 5 that to find the current instantaneous motion speed v(n) and motion displacement s(n), s(n-1), v(n-1), a(n-1) and the current a(n) is calculated.

Further, in the step 3), the derivation results of v(n) and s(n) in formula 4 and formula 5 are applied to the acceleration sensor, and the three-dimensional space displacement formula can be obtained as:

The Kalman filtering method includes the following steps:

The prediction stage is: Assuming that it is time k, first, according to the state system model, the current state equation can be predicted based on the state of the system at the previous moment:

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

In formula (9), X(k|k-1) is the current state estimated by the state at the previous moment, X(k-1|k-1) is the optimal predicted value at the previous moment, U(k ) is the amount of control over the system at the moment.

The forecast stage also predicts how accurate the estimates will be. The covariance of the system state is also the expected value, as shown in equation (10):

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

In formula (10), P(k-1|k-1) is the covariance of X(k|k-1), P(k-1|k-1) is X(k-1|k-1) covariance of .

The update stage is: the estimated value of the current situation is known, and then combined with the observed value of the current situation, the optimal estimated value of the current situation can be obtained:

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

Among them, Kg is the Kalman Gain, which can continuously change its own value due to different moments:

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

Although the optimal predicted value X(k|k) of the system state at time k has been obtained, P(k|k) needs to be updated to ensure that the Kalman filtering process can work cyclically to the end.

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

I is the identity matrix. The basic steps of Kalman filtering of random linear discrete systems are completed by relying on the above five formulas.

Beneficial effect: The utility model is compared with the prior art:

The system uses MSP430F169 as the main control chip, and detects the acceleration, angular velocity and inclination of the moving target through the MPU6050 six-axis sensor. After static error calibration, it is transmitted to the PC through the serial port. In the PC terminal, Kalman filtering is performed on the motion parameters to eliminate random noise, and then the integral operation is performed to obtain the trajectory coordinate position of the target moving in the three-dimensional space, and finally the motion trajectory line is drawn and the displacement value is calculated. The system realizes the measurement and tracking of the motion trajectory, and has strong practical significance and practical value.

Description of drawings

Fig. 1 is the utility model time-speed curve;

Fig. 2 is the system block diagram of the utility model;

Fig. 3 is the circuit diagram of the utility model MSP430F169 single-chip microcomputer;

Fig. 4 is the circuit diagram of the utility model MPU6050 sensor;

Fig. 5 is the LCD display circuit diagram of the utility model;

Fig. 6 is the utility model PL2303HX serial port module circuit diagram;

7 is a flow chart of the main program of the single-chip microcomputer of the present invention;

Fig. 8 is the data processing algorithm flow chart of the utility model;

9 is a comparison diagram of the utility model before and after the acceleration data filtering;

FIG. 10 is a circuit diagram of a moving target of the present invention.

Detailed ways

The present utility model will be further described below in conjunction with the accompanying drawings and embodiments.

The utility model relates to a method and system for tracking the trajectory of a moving target based on MPU6050, and the specific implementation method is as follows:

hardware circuit design

1.1 MCU module

The system adopts the 16-bit ultra-low power consumption and high processing capability microcontroller MSP430F169 produced by TI to control the hardware system. The chip adopts "von Neumann" structure, its random access memory (RAM), read only memory (ROM) and all external modules are in the same address space, the addressing range and clock frequency can reach 62KB and 8MHz respectively . And it contains 2 timers, 12-bit ADC, 2 UART ports and 48 I/O ports, etc., so it can quickly complete the initialization and application of external sensors.

The data collected by the sensor of this system is transmitted through the IIC bus. The IIC bus is a serial data bus. It has only two bidirectional signal lines, which are the bidirectional data line SDA and the bidirectional clock line SCL. The data line is used to transmit data. The clock line is used to transmit clock pulses. Microcontroller is the core part of the whole design, which is composed of crystal oscillator circuit (this system adopts 8MHZ) and reset circuit. The MSP430F169 minimum system is shown in Figure 3.

1.2 MPU-6050 detection module

As the world's first 9-axis motion processing sensor, the MPU6050 is highly integrated. It contains a MEMS gyroscope and an accelerometer, and the measured motion data is directly output in digital form. The measurement range of the gyroscope integrated in this sensor is ±250, ±500, ±1000, ±2000°/second (dps), and the measurement range of the accelerometer is ±2, ±4, ±8, ±16g, which can be selected according to actual needs. scope.

The pins XDA and XCL of MPU6050 are not connected to the microcontroller, the pin GND shares the ground with the microcontroller MSP430F169, the pin VCC is connected to the 3.3V pin of the microcontroller, the SDA pin is connected to the microcontroller P2.1 port, and the SCL pin is connected to the microcontroller P2. 0 terminal. MSP430F169 communicates with MPU-6050 serially through IIC bus, and transmits the collected motion data to PC. The sensor circuit of MPU6050 is shown in Figure 4. When the MPU6050 is working normally, the LED will emit yellow-green light.

1.3 LCD display circuit

The 12864 LCD screen is a dot-matrix graphic LCD module realized by low-power CMOS technology. It has the characteristics of zero radiation display, low power consumption, convenient screen adjustment, and stable picture. The 12864 LCD screen contains the KS0108B/HD61202 controller, which can display 128 points and 64 points through the internal 128×64-bit mapping DDRAM, and has a built-in international simplified Chinese character library. When the system is working, the 12864 LCD screen displays X, Y, Z direction acceleration and angle information in real time. The 12864 LCD display circuit is shown in Figure 5.

1.4 PL2303 serial port module

The serial communication module adopts PL2303HX chip, namely PL2303USB TO TTL module, which is a kind of RS232-USB interface converter, which enables RS232 full-duplex asynchronous serial communication equipment to be conveniently connected with the USB function interface. The module contains USB function controller, transceiver and oscillator, which can realize the conversion of USB signal and RS232 signal. As a USB/RS232 bidirectional converter, the device can exchange USB data format and RS232 information flow format. The communication baud rate of PL2303 is as high as 6Mb/s, and it has a powerful and highly compatible drive capability. It can be simulated as a traditional COM port on many operating systems, so that it can be easily converted into a USB interface application.

The system connects the +5V of the PL2303HX chip to the +5V pin of the single-chip microcomputer, and shares the ground. The RXD pin of the PL2303HX chip is connected to the TXD pin of the MSP430F169, that is, the P3.4 pin, and the TXD pin is connected to the RXD pin of the MSP430F169, that is, the P3.5 pin. When the system transmits data, the collected motion data is transmitted in real time by the PL2303 serial port to the serial port debugging assistant on the PC side and saved. The PL2303HX serial port module circuit is shown in Figure 6.

software design

The operating program of the single-chip microcomputer of the system is written in C language, including the main program, the MPU6050 initialization data reading program, the zero offset calibration program, the LCD display program, and the serial port communication program. The execution algorithm of the host computer is a MATLAB program, including the Kalman algorithm to eliminate random errors, the acceleration integration algorithm, and the trajectory drawing.

According to the register read and write function of MPU6050, initialize the sensor, mainly parameter configuration, such as sampling rate, filter frequency, etc. If there is no special requirement, use the typical value. In the program, the motion data information is obtained by calling the MPU6050_Get_Data() function. For example, MPU6050_Get_Data(GYRO_ZOUT_H) means reading 16-bit Z-axis angular velocity data.

In order to eliminate static errors, the acceleration values were read 200 times, and the average value was taken to correct the original data. Then, by calling RS232_jiao(), RS232_jia() and Display10BitData() functions, the motion data is displayed on the 12864 display and transmitted to the PC. The main program flow of the single-chip microcomputer is shown in Figure 7.

The main functions of the PC-side data processing software are: perform Kalman filtering on the received motion data, obtain real-time position coordinate points during the motion process by integrating the acceleration value twice, and use the program to draw the motion trajectory. The PC-side data processing algorithm is written in MATLAB program, and the data processing algorithm flow is shown in Figure 8.

The Kalman filtering method includes the following steps:

The prediction stage is: Assuming that it is time k, first, according to the state system model, the current state equation can be predicted based on the state of the system at the previous moment:

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

In formula (9), X(k|k-1) is the current state estimated by the state at the previous moment, X(k-1|k-1) is the optimal predicted value at the previous moment, U(k ) is the amount of control over the system at the moment.

The forecast stage also predicts how accurate the estimates will be. The covariance of the system state is also the expected value, as shown in equation (10):

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

In formula (10), P(k-1|k-1) is the covariance of X(k|k-1), P(k-1|k-1) is X(k-1|k-1) covariance of .

The update stage is: the estimated value of the current situation is known, and then combined with the observed value of the current situation, the optimal estimated value of the current situation can be obtained:

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

Among them, Kg is the Kalman Gain, which can continuously change its own value due to different moments:

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

Although the optimal predicted value X(k|k) of the system state at time k has been obtained, P(k|k) needs to be updated to ensure that the Kalman filtering process can work cyclically to the end.

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

I is the identity matrix. The basic steps of Kalman filtering of random linear discrete systems are completed by relying on the above five formulas.

When the detection module completes the collection of object motion data, the serial port module is used to transmit the data to the serial port debugging assistant on the PC side to obtain the real-time measured acceleration and angular velocity parameters, and save them as "AccDataok.txt" file. The MATLAB program can be executed on the PC side to process the data and draw the target motion trajectory curve.

Experimental results and analysis

During the test experiment, the MPU6050 sensor is placed horizontally, the Z axis is perpendicular to the horizontal plane, and the X and Y axes are parallel to the horizontal plane. MSP430F169 is connected to the PC through the PL2303HX serial port, open the serial port debugging assistant, set the serial port baud rate to 9600, and the serial port selection is consistent with the serial port number in the computer device manager. The hand-held sensor moves in a curve within 1m ² of the horizontal plane. The 13 sets of motion data collected by the system with a sampling frequency of 125Hz are shown in Table 1. The first, second, and third columns are the acceleration values in the X, Y, and Z directions, and the unit is g; the fourth, fifth, and sixth columns are X, respectively. , Y, Z direction angular velocity value, the unit is dps.

Table 1

There are some noise interferences in the collected motion data, such as sensor error, system random error, etc., which reduces the accuracy of the data. Therefore, the interference in the acceleration data is eliminated by Kalman filtering to obtain more accurate data. The comparison of acceleration data before and after Kalman filtering is shown in Figure 9.

The filtered acceleration data is integrated twice, and the motion path of the three-dimensional space target is obtained as shown in Figure 10. The target moves in a curve in the horizontal plane, and the tracking trajectory is basically the same as the actual motion trajectory.

In order to detect the accuracy of the displacement measurement of the moving object by the system, the displacements of 20cm, 50cm, 80cm and 110cm were measured respectively, and each distance was measured in 15 groups, and the average measurement displacement and error were calculated as shown in Table 2.

Table 2

It can be seen from Table 2 that in the measurement of the displacement of the moving object from 20cm to 110cm, the average error does not exceed 7%, and the displacement of the moving object can be measured more accurately.

Claims (5)

1. a moving target trajectory tracking system, is characterized in that: comprise motion detection module, control module, display module, serial port communication module and PC end processing module, motion detection module, display module and serial port communication module are all connected to control module, The serial communication module is connected to the PC-side processing module, wherein: Motion detection module: MPU6050 six-axis sensor; Control module: MSP430F169 is the main control chip; Serial communication module: using PL2303HX chip. 2. moving target track tracking system according to claim 1, is characterized in that: the pin XDA, XCL of MPU6050 are not connected with MSP430F169, pin GND and MSP430F169 share the ground, and pin VCC is connected to the 3.3V pin of MSP430F169 , the SDA pin is connected to the P2.1 port of the MSP430F169, and the SCL pin is connected to the P2.0 end of the MSP430F169. 3. moving target trajectory tracking system according to claim 1, is characterized in that: the +5V pin of PL2303HX chip is connected to the +5V pin of MSP430F169, and common ground; The RXD pin of PL2303HX chip is connected to the TXD of MSP430F169 that is P3 .4 pin, the TXD pin is connected to the RXD of the MSP430F169, that is, the P3.5 pin. 4 . The moving target trajectory tracking system according to claim 1 , wherein the MSP430F169 is connected to the MPU6050 six-axis sensor through the IIC bus. 5 . 5 . The moving target trajectory tracking system according to claim 1 , wherein the display module adopts a display screen. 6 .