CN208621076U - A kind of movement objective orbit tracing 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 kind of movement objective orbit tracing system

Technical field

The utility model relates to the measurements of the target trajectory of human-computer interaction and tracking technique field more particularly to a kind of movement mesh Mark trajectory track system.

Background technique

Object of which movement is a ubiquitous phenomenon.With the development of science and technology the mankind just using various technical monitorings with Track movement locus of object.In field of human-computer interaction, laser traces system, magnetic spatial pursuit system and based on space machine vision Positioning system use it is most extensive, measurement accuracy with higher, there is also limited by extraneous referential, overhead is big, sets Received shipment makees the defects of complicated.Magnetic spatial pursuit system is often subject to field source limitation, is easy to be influenced by various external environments, such as iron The barrier actions of product, the interference in other magnetic fields；Laser traces system will receive the restriction of laser source illumination range, and laser light source It can often be damaged, cause unnecessary loss；Positioning system based on three-dimensional machine vision is usually because the placement of camera The influence of point and the scene domain of shooting has certain limitation, and data calculation process is many and diverse, overhead is big.So When above-mentioned trajectory track system is for motion target tracking, positioning, respectively there are advantage and disadvantage.Therefore, design that a kind of measurement accuracy is high, answers There is stronger realistic meaning and practical value with the wide trajectory track system of range.

Utility model content

Purpose of utility model: utility model aims to solve existing laser traces systems, magnetic spatial pursuit system The defects of system and the positioning system based on space machine vision are limited by extraneous referential, overhead is big, equipment running is complicated, Magnetic spatial pursuit system be often subject to field source limitation, be easy influenced by various external environments, as ironwork barrier action, other The interference in magnetic field；Laser traces system will receive the restriction of laser source illumination range, and laser light source can be often damaged, and be caused not Necessary loss；Positioning system based on three-dimensional machine vision is usually because the placement location of camera and the scene domain of shooting Influence, there is certain limitation, and the problem that data calculation process is many and diverse, overhead is big.

Technical solution: to achieve the above object, the utility model uses following technical scheme:

A kind of movement objective orbit tracing system, including motion detection block, control module, display module, serial communication Module and the end PC processing module, in which:

Motion detection block: six axle sensor of MPU6050 is used, the acceleration and angular speed parameter of moving target is detected；

Control module: using MSP430F169 as main control chip, be mainly responsible for acquisition exercise data, control serial communication and LCD is shown, carries out zero bias calibration by acceleration information of the program to acquisition, calibration data is then transferred to the processing of the end PC；

Display module: 3 axle accelerations of MPU6050 sensor, angular velocity data are shown on LCD；

Serial communication modular: PL2303HX chip is used, exercise data is transferred to the end PC；

The end PC processing module: in MATLAB environment, Kalman filter is carried out to exercise data, then accumulated twice to it Point, the coordinate points that moving target moves at any time are obtained, line draws kinematic roadmap.

Further, pin XDA, XCL of MPU6050 is not connected with MSP430F169, pin GND and MSP430F169 Altogether, pin VCC is connected to the 3.3V pin of MSP430F169, and SDA pin connects the P2.1 mouth of MSP430F169, SCL pin Connect the end P2.0 of MSP430F169.

Further ,+the 5V of PL2303HX chip is connected to the+5V pin of MSP430F169, and altogether；PL2303HX The RXD pin of chip is connected to the TXD i.e. P3.4 pin of MSP430F169, and TXD pin is connected to the RXD of MSP430F169 i.e. P3.5 pin.

Further, MSP430F169 is connect by iic bus with six axle sensor of MPU6050.

A kind of movement objective orbit method for tracing, comprising the following steps:

1) each parameter information for being transmitted to the end PC is obtained；

2) MPU6050 sensor exports discrete data, so curve is in the case where sampling time interval Δ t is sufficiently small, It can be reduced to straight line；Curve can thus be split into numerous right-angled trapezium, being integrated to v (t) is also to fractionation Right-angled trapezium area solved；

3) derivation result of v (n), s (n) are applied in acceleration transducer, obtains three-dimensional space displacement formula；

4) it calculates a motion profile to need to carry out 2*3*n integral operation, is reduced to 2*3*n times according to actual displacement and adds Method calculation.

Further, in the step 2), primary condition s (t is enabled₀)=0, it may be assumed that

Δ t=t₁-t₀=t₂-t₁=...=t_n-t_n-1, t₀For initial time, t₁、t₂…t_nFor time series at equal intervals, enable Δ t is time interval；

In discrete domain when n > 1,

It can be obtained by formula 2, formula 3:

It can be seen that by formula 4, formula 5, ask current transient motion speed v (n) and moving displacement s (n), it can be by s (n-1), v (n- 1), a (n-1) and current a (n) calculate.

Further, the derivation result of v (n) in formula 4, formula 5, s (n) are applied into acceleration transducer in the step 3) In, available three-dimensional space displacement formula are as follows:

Kalman filter method, comprising the following steps:

Forecast period are as follows: assuming that system last moment can be based on first according to status system model at this time for the k moment Situation and predict state equation instantly:

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

In formula (9), X (k | k-1) is the state instantly obtained by the state estimation of last moment, X (k-1 | k-1) be Last moment optimal predicted value, U (k) are the control amounts to system this moment.

Forecast period also expects estimated value levels of precision.The covariance i.e. predicted value of system mode, such as formula (10) shown in:

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), and P (k-1 | k-1) is the covariance of X (k-1 | k-1).

The more new stage are as follows: the estimated value of known As-Is can obtain the present situation in conjunction with the observation of the present situation Optimum estimation value:

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

Kg therein is exactly Kalman gain (Kalman Gain), it can because of it is different at the time of and constantly change from The value of body:

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

Although having obtained the optimal predicted value X of k moment system mode (k | k) now, also need to update P (k | k) To guarantee Kalman filter process energy cycle operation to finally.

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

I is unit matrix.The basic step of stochastic discrete system Kalman filter is exactly complete by above 5 formula At.

The utility model has the advantages that the utility model is compared with prior art:

System detects the acceleration of moving target by six axle sensor of MPU6050 using MSP430F169 as main control chip The end PC is transferred to by serial ports after carrying out static error calibration with parameters such as angular speed and inclination angles.Movement is joined in the end PC Number carries out Kalman filter Removing Random No, then carries out integral operation, to obtain target in the track of three-dimensional space motion Coordinate position finally draws out motion profile route and calculates shift value.The system realizes measurement to motion profile and chases after Track has stronger realistic meaning and practical value.

Detailed description of the invention

Fig. 1 m- rate curve when being the utility model；

Fig. 2 is Tthe utility model system block diagram；

Fig. 3 is the utility model MSP430F169 single chip circuit figure；

Fig. 4 is the utility model MPU6050 sensor circuit figure；

Fig. 5 is the utility model LCD display circuit figure；

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

Fig. 7 is the utility model singlechip main program flow chart；

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

Fig. 9 is that the utility model acceleration information filters front and back comparison diagram；

Figure 10 is the utility model moving target line map.

Specific embodiment

The utility model is further detailed with reference to the accompanying drawings and examples.

The utility model is a kind of movement objective orbit method for tracing and system based on MPU6050, concrete methods of realizing It is as follows:

Hardware circuit design

1.1 one-chip computer module

System is controlled using 16 super low-power consumptions and high-strength processing capacity microcontroller MSP430F169 of TI company production Hardware system.The chip uses " von Neumann " structure, its random access memory (RAM), read-only memory (ROM) and owns For external module all in identical address space, addressing range and clock frequency respectively reach 62KB and 8MHz.And it Inside includes 2 timers, 12 ADC and 2 UART ports and 48 I/O mouthfuls etc., so can quickly complete to external sense The initialization and application of device.

The sensor acquisition data of this system are transmitted by iic bus, and iic bus is a kind of serial data bus, it is only Have two bidirectional signal lines, be two-way data line SDA and bidirectional clock lines SCL respectively, data line be for transmitting data, when Clock line is for transmitting clock pulses.Microcontroller is the most crucial part in entire design, is by crystal oscillating circuit (this system Using 8MHZ) and reset circuit composition.MSP430F169 minimum system is as shown in Figure 3.

1.2 MPU-6050 detection modules

MPU6050 has the characteristics that Highgrade integration, inside includes as 9 axis motion sensor of global first case MEMS gyroscope and accelerometer, and the exercise data measured is directly with digital output.The gyroscope of this sensor integration Measurement range be ± 250, ± 500, ± 1000, ± 2000 °s/sec (dps), accelerometer measures range be ± 2, ± 4, ± 8, ± 16g can select measurement range according to actual needs.

Pin XDA, XCL of MPU6050 is not connected with single-chip microcontroller, pin GND and single-chip microcontroller MSP430F169 altogether, pin VCC is connected to the 3.3V pin of single-chip microcontroller, and SDA pin connects single-chip microcontroller P2.1 mouthfuls, and SCL pin connects the end single-chip microcontroller P2.0. MSP430F169 carries out serial communication by iic bus and MPU-6050, and the exercise data of acquisition is transferred to the end PC. For MPU6050 sensor circuit as shown in figure 4, when MPU6050 is worked normally, light emitting diode can issue the light of yellow green.

1.3 LCD display circuit

12864 liquid crystal displays are a kind of dot matrix graphic LCD modules realized using Low-Power CMOS technology, have zero spoke of display It penetrates, the features such as low-power consumption, screen adjustment are convenient, frame stabilization.It is controlled inside 12864 liquid crystal displays containing KS0108B/HD61202 Device completes, 64 points of displays, built-in internationalization simplified form of Chinese Character character library at 128 points by 128 × 64 internal bit mapping DDRAM.System When system work, 12864 liquid crystal display real-time display X, Y, Z-direction acceleration, angle information.12864 liquid crystal display display circuits are as schemed Shown in 5.

1.4 PL2303 serial port modules

Serial communication modular uses PL2303HX chip, i.e. PL2303USB TO TTL module, is a kind of RS232-USB Interface convertor can be such that RS232 full duplex asynchronous serial communication equipment easily connects with USB functional interface.The inside modules Comprising USB function control device, transceiver, oscillator, the conversion of usb signal Yu RS232 signal can be realized.The device conduct USB/RS232 bidirectional transducer is interchangeable usb data format and RS232 information stream format.The communication baud of PL2303 Rate is up to 6Mb/s, has powerful high compatible driving ability, traditional COM port can be modeled in numerous operating systems, thus It can be easily swapped out to USB interface application.

+ the 5V of PL2303HX chip is connected to the+5V pin of single-chip microcontroller by system, and altogether.PL2303HX chip RXD pin is connected to the TXD i.e. P3.4 pin of MSP430F169, and the RXD i.e. P3.5 that TXD pin is connected to MSP430F169 draws Foot.When system transmits data, the exercise data of acquisition is transferred to COM Debug Assistant and the guarantor at the end PC by PL2303 serial ports in real time It deposits.PL2303HX serial port module circuit is as shown in Figure 6.

Software design

The single-chip microcontroller operation program of system is that C language is write, including main program, MPU6050 initialization data reading program, Zero bias calibration procedure, LCD show program, serial communication program etc..And it is MATLAB program that host computer, which executes algorithm, including is eliminated The Kalman algorithm of random error, integrated acceleration algorithm, track are drawn.

According to the register read/write function of MPU6050, sensor is initialized, mainly parameter configuration, is such as sampled Rate, frequency filtering etc. such as use representative value without particular/special requirement.In a program, by calling MPU6050_Get_Data () letter Number obtains exercise data information, reads 16 Z axis angular velocity datas as MPU6050_Get_Data (GYRO_ZOUT_H) is indicated.

To eliminate static error, the value of 200 acceleration is read, is averaged and initial data is corrected.Then, lead to It crosses calling RS232_jiao (), RS232_jia () and Display10BitData () function and exercise data is shown to 12864 Display screen is simultaneously transferred to the end PC.Singlechip main program process is as shown in Figure 7.

PC end data processing software major function are as follows: Kalman filter is carried out to the exercise data that receives, by adding Velocity amplitude does integral operation twice to obtain the position coordinates point in motion process in real time, draws motion profile with program.The end PC For data processing algorithm with MATLAB programming, data processing algorithm process is as shown in Figure 8.

Kalman filter method, comprising the following steps:

Forecast period are as follows: assuming that system last moment can be based on first according to status system model at this time for the k moment Situation and predict state equation instantly:

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

In formula (9), X (k | k-1) is the state instantly obtained by the state estimation of last moment, X (k-1 | k-1) be Last moment optimal predicted value, U (k) are the control amounts to system this moment.

Forecast period also expects estimated value levels of precision.The covariance i.e. predicted value of system mode, such as formula (10) shown in:

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), and P (k-1 | k-1) is the covariance of X (k-1 | k-1).

The more new stage are as follows: the estimated value of known As-Is can obtain the present situation in conjunction with the observation of the present situation Optimum estimation value:

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

Kg therein is exactly Kalman gain (Kalman Gain), it can because of it is different at the time of and constantly change from The value of body:

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

Although having obtained the optimal predicted value X of k moment system mode (k | k) now, also need to update P (k | k) To guarantee Kalman filter process energy cycle operation to finally.

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

I is unit matrix.The basic step of stochastic discrete system Kalman filter is exactly complete by above 5 formula At.

After detection module completes the acquisition to object of which movement data, the serial ports at the end PC is transmitted data to using serial port module Debugging assistant, obtains the acceleration and angular speed parameter of real-time measurement, and saves as " AccDataok.txt " file.The end PC is Executable MATLAB program handles data, and draws target trajectory curve.

Experimental result and analysis

When test experiments, MPU6050 sensor levels are placed, Z-direction is perpendicular to horizontal plane, X and Y-axis and horizontal plane In parallel.MSP430F169 is connected by PL2303HX serial ports with the end PC, and COM Debug Assistant is opened, and setting serial port baud rate is 9600, serial ports selection is consistent with the serial port in computer equipment manager.Hand-held sensor is in horizontal plane 1m²Song is done in range Line is mobile.System is as shown in table 1 with 13 groups of exercise datas that 125Hz sample frequency acquires, and the first, second and third column are respectively X, Y, Z The acceleration value in direction, unit g；Fourth, fifth, six column are respectively the magnitude of angular velocity in the direction X, Y, Z, unit dps.

Table 1

The exercise data of acquisition is reduced there are some noise jammings, such as sensor itself error, system random error The accuracy of data.Therefore, the interference in acceleration information is eliminated by Kalman filtering, obtains more accurate data, blocked Acceleration information comparison before and after Kalman Filtering is as shown in Figure 9.

Quadratic integral is carried out to filtered acceleration information, obtains motion path such as Figure 10 institute of Three dimensional Targets Show, object does curvilinear motion in horizontal plane, and tracing path and actual motion track are almost the same.

For the precision that detection system measures displacement of moving article, respectively to the position of 20cm, 50cm, 80cm and 110cm Row motion measurement, and every kind 15 groups of range measurement are moved into, average measurement displacement is calculated and error is as shown in table 2.

Table 2

As shown in Table 2, in the measurement of 20cm to 110cm displacement of moving article, mean error is no more than 7%, can compare Relatively accurately realize the measurement moved to moving target level.

Claims (5)

1. a kind of movement objective orbit tracing system, it is characterised in that: including motion detection block, control module, display module, Serial communication modular and the end PC processing module, motion detection block, display module and serial communication modular are all connected to control mould Block, serial communication modular connect the end PC processing module, in which:

Motion detection block: six axle sensor of MPU6050 is used；

Control module: using MSP430F169 as main control chip；

Serial communication modular: PL2303HX chip is used.

2. movement objective orbit tracing system according to claim 1, it is characterised in that: pin XDA, XCL of MPU6050 It is not connected with MSP430F169, altogether, pin VCC is connected to the 3.3V pin of MSP430F169 by pin GND and MSP430F169, SDA pin connects the P2.1 mouth of MSP430F169, and SCL pin connects the end P2.0 of MSP430F169.

3. movement objective orbit tracing system according to claim 1, it is characterised in that:+the 5V of PL2303HX chip connects It is connected to+5V the pin of MSP430F169, and altogether；The RXD pin of PL2303HX chip is connected to the TXD of MSP430F169 i.e. P3.4 pin, TXD pin are connected to the RXD i.e. P3.5 pin of MSP430F169.

4. movement objective orbit tracing system according to claim 1, it is characterised in that: MSP430F169 is total by IIC Line is connect with six axle sensor of MPU6050.

5. movement objective orbit tracing system according to claim 1, it is characterised in that: display module uses display screen.