CN101339240B - Wireless sensor network object tracking method based on double layer forecast mechanism - Google Patents

Abstract

The invention relates to a target tracking method based on a double-layer forecasting mechanism in a sensor network, wherein, the sensor network is based on a hierarchical wireless sensor network and consists of an SN node and a CH node; the detailed steps of the invention are that the SN node adopts a Bayes estimation method to carry out the micro-forecast of a target location; the CN node obtains the observed value of the target location by combining the result of the SN node; based on a curvilinear motion equation, the CH node carries out the macro-forecast of the target location so as to obtain the estimated value of the target location; the CH node carries out the linear fitting of the observed value and the estimated value of the target location to solve a final target tracking location. The invention is technically characterized in that a WSN is fully utilized to realize the target tracking; the tracking method fully considers the needs of low complexity, low communication traffic and low energy consumption of the WSN target tracking method and passes a theoretical analysis and a simulated verification. The locating and tracking method in the invention is capable of accurately and quickly solving the motion parameters of the target by programmed location software.

Description

Wireless sensor network target tracking method based on double-deck forecasting mechanism

Technical field

The invention belongs to wireless sensor network target tracking technique field, particularly a kind of sensor network target tracking method based on double-deck forecasting mechanism.

Background technology

Along with microelectromechanical systems, computing machine, communication, the develop rapidly of subject such as control and artificial intelligence and increasingly mature automatically, a kind of novel measurement and control network has appearred---and wireless sensor network (Wireless Sensor Network, WSN).WSN combines sensor technology, embedded computing technique, distributed information processing and radio communication and network technology, form a multihop self-organizing network system by modes such as Ad-hoc between the node, monitoring target and various environmental information in perception collaboratively, collection and the processing network's coverage area, and be sent to the user who needs these information.

Compare with the ground-based radar tracking system with satellite independently, WSN has the advantages such as efficient and convenient, that system robustness good, it is more effective to follow the tracks of, tracking is more hidden of disposing at target localization and track side's mask, some challenges have also been brought simultaneously, as, do not have feature requests such as center mechanism, node resource be limited must adopt the track algorithm of distributed, low time and space complexity, low traffic and low energy consumption, traditional target tracking algorism is not suitable for the WSN target following and uses.

At present, target tracking algorism to WSN mainly concentrates on Bayes filtering, Kalman filtering, expansion Kalman filtering and particle filter scheduling algorithm in the world, and these algorithms are attempted to estimate the following possible state of target by target past and current status information.

Kalman filtering (Kalman Filter, KF) by setting up target movement model and systematic survey model, to the prediction of target with upgrade the position computing that iterates and realize target following.Non-linear and the also non-Gauss of noise of the dynamic model of target among the WSN, and KF is a kind of centralized algorithm, so KF is not suitable for WSN.

(Extended Kalman Filter EKF) attempts the nonlinear system linearization EKF, and it is approximate to carry out linearization to the measurement equation, can't avoid the linearization error.In addition, EKF needs comparatively accurate target motion model, and model is accurate more, and accuracy of predicting is just high more, but in fact, model always is similar to, and this approximate result not only causes the loss on the precision, and evaluated error accumulative total is amplified.EKF centralized computing of needs simultaneously and speed are slower.

Particle filter method can solve non-linear non-gaussian filtering problem well.But particle filter needs complicated iterative process, and calculated amount is big and require a large amount of particle data of storage, is suitable for integrated system, moves in the distributed WSN system that is not suitable at the individual node resource-constrained.

The main task of Bayesian Estimation is to attempt the posterior probability distribution density function (PDF) of structural regime, and forecast period using system model is predicted the more following state PDF of the Measuring Time in interval; Update stage then utilizes up-to-date measured value to remove to revise the PDF of prediction.The processing mode of Bayesian Estimation sequential-type is fit to the low computation complexity demand of WSN.But this method depends on prior distribution and measured value simultaneously, therefore will approach reality on prior distribution is chosen, and could obtain more satisfactory estimation like this.

2 estimation and Forecasting Methodologies that extrapolation is a kind of simple dbjective state, its basic thought is as the target current location with the current Targets Dots of extracting, utilize two some mark data of current and previous moment target, determine the state of target and predict the dbjective state of any down.The precision of this method is only relevant with precision current and previous moment point mark data, is the lower method of a kind of precision.But this method has no requirement to the statistical property of dbjective state noise and systematic survey noise, calculates simply, does not need to store a large amount of historical datas, and no cumulative errors have energy efficiency preferably.

As can be seen from the above analysis: although simple 2 Extrapolation method precision are relatively poor, calculated amount and memory space are little, calculate simple and do not have cumulative errors.Bayes estimate also the to can yet be regarded as a kind of feasible method of WSN target following, but because the error of target movement model and systematic survey model, and inaccurate its tracking accuracy that influenced of prior distribution.

Summary of the invention

At the problem that exists in the background technology and WSN characteristics, the object of the present invention is to provide a kind of sensor network target tracking method based on double-deck forecasting mechanism to target following among the application requirements of distributed, low complex degree, low traffic and the low energy consumption of target tracking algorism and the WSN.

To achieve these goals, the present invention is based on level type wireless sensor network, this sensor network is made up of SN node and CH node, and the technical scheme that is adopted comprises the steps:

(1), SN carries out target location microcosmic prediction and the update calculation estimated based on Bayes, obtains the microcosmic predicted value of target location;

(2), by CH the microcosmic predicted value of this bunch interior nodes is merged the observed reading that the back forms the target location;

(3), CH carries out curve fitting according to three nearest Targets Dots data, obtain the target travel curvilinear equation, put the mark data according to this curvilinear equation and last then, calculate the size and Orientation of target speed, and adopting simple 2 further target of prediction of Extrapolation method in view of the above in next position (macro-forecast) constantly, this predicted value is as the estimated value of target location;

(4), the estimated value of the target location that calculates with the estimated value of (2) target location that calculates of step and (3) step carries out linear fit, the result is as the net result of target location.

The beneficial effect that the present invention has is:

(1), respectively the target location is carried out having improved tracking accuracy based on the microcosmic prediction of Bayes estimation and the macro-forecast of curvilinear motion equation by SN node and CH node.

(2), based on the track algorithm of curvilinear motion equation, on the basis of 2 extrapolations, only increase historical some mark data, replaced the dog-leg path of 2 extrapolations with curve, make prediction locus more level and smooth; With tangential direction target of prediction direction of motion along current time point mark, and ask targeted rate by curvilinear integral, these measures have at 2, and the extrapolation traffic is few, energy consumption is low, no cumulative errors, without any need for statistical information, do not need to set up advantage such as sensor measurement model, improved tracking accuracy simultaneously.

The double-deck forecasting mechanism of the present invention calculates simple, and the traffic is few, has energy efficiency preferably, has effectively improved the tracking accuracy of system simultaneously, is fit to the WSN target following and uses.

Description of drawings

Fig. 1 is a tracking process flow diagram of the present invention.

Fig. 2 is a level type WSN system assumption diagram of the present invention.

Open circles representative sensor node (SN) wherein, filled circles representative bunch first node (CH), dotted ellipse representative sensor node cluster.

Fig. 3 finds the solution synoptic diagram for arc length of the present invention.

Embodiment

The present invention is described in further detail below in conjunction with accompanying drawing.

Node among the level type WSN of the present invention can be divided into common sensor node, and (Sensor Node is SN) with bunch first node (Cluster Head, CH) (structure of level type WSN is referring to Fig. 1).

Specific implementation process of the present invention can be divided into 4 steps (referring to Fig. 2):

(1) SN node calculated target positions.Clocking and carving the t target location is X _t, X _t=(x _t, y _t), measurement value sensor is z _t, Z _t={ z ₀, z ₁..., z _tThe expression sensor local historical measuring value.The dbjective state dynamic model is by state transition probability p (X _T+1| X _t) provide; The sensor measurement model is by dbjective state-measuring value transition probability p (z _t| X _t) provide.Prediction is as follows with the update calculation formula:

Prediction: t state estimation p (X constantly _t| Z _t) be used to predict t+1 state constantly:

p(X _t+1|Z _t)＝(X _t+1|X _t)p(X _t|Z _t)dX _t (1)

In the actual scene because movement velocity and direction the unknown of target, suppose speed be evenly distributed on [0, v _Max] interval (v wherein _MaxBe the target maximum movement speed), the target travel direction is evenly distributed on [0,2 π] interval, therefore, p (X _T+1| X _t) distribution just formed with X _tBe the center of circle, radius is v _MaxDisk.Adopt this model, letter distribution p (X is put in prediction _T+1| Z _t) can be just by using posteriority degree of confidence p (X _t| Z _t) and evenly the nuclear of disk carry out convolution and obtain, this convolution has just in time reflected because probabilistic expansion of the dbjective state that the uncertainty of target speed and direction is brought.

Upgrade: update stage adopts new metric data that prior distribution density is revised, to obtain required posteriority distribution density, as shown in Equation 2 at current state:

$p\left(X_{t+1}|Z_t\∪\left\{z_{t+1}\right\}\right)=\frac{p\left(Z_{t+1}|X_{t+1}\right)p\left(X_{t+1}|Z_t\right)}{p\left(z_{t+1}|Z_t\right)}$

$=C^{-1}p\left(z_{t+1}|X_{t+1}\right)\∫p\left(X_{t+1}|X_t\right)p\left(X_t|Z_t\right)\mathrm{dX}$

$\∝p\left(z_{t+1}|X_{t+1}\right)\∫p\left(X_{t+1}|X_t\right)p\left(X_t|Z_t\right)\mathrm{dX}$

(2)

Wherein C is a normalization constant, C=p (z _T+1| X _T+1) p (x _T+1| Z _t) dX _t

Suppose initial p (X ₀| Z ₀)=p (X ₀), just the prior distribution of state is known, then probability distribution function p (X _T+1| Z _T+1) can recursively predict and the acquisition of more newly arriving dbjective state with crossing formula 2 and formula 3 in principle.

(2) measured value of CH node calculated target positions.Detect target if be carved with N node during t+1, these nodes calculate the relative posteriority distribution value of target location separately according to formula 2, and send to CH, and CH tries to achieve the measuring position of target according to formula 3 $\stackrel{\‾}{X_{t+1}},\stackrel{\‾}{X_{t+1}},=\left(\stackrel{\‾}{x_{t+1,}}\stackrel{\‾}{y_{t+1}}\right),\stackrel{\‾}{X_{t+1}}$ Represent that this positional value is by this N the target location that the SN node measurement obtains.

$\stackrel{\‾}{X_{t+1}}=\arg \max \{\underset{X_{t+1}^{\left(j\right)}}{p(}{X}_{t+1}^{\left(j\right)}|Z_{t+1}^{\left(j\right)})\}---\left(3\right)$

Wherein

Represent that j sensor node is according to local historical measurements

Drawing t+1 moment target is positioned at

Posterior probability values, this value representation target be in

Degree of confidence.

(3) CH node calculated target positions estimated value.Hypothetical target is through 3 points: (x _T-2, y _T-2), (x _T-1, y _T-1), (x _t, y _t) (t 〉=3), then used the movement locus of this conic fitting target of 3, the equation of locus of match is as follows:

$y=\underset{k=t-2}{\overset{t}{\mathrm{\Σ}}}{l}_k\left(x\right)y_k$

Wherein,

$l_k\left(x\right)=\underset{\stackrel{j=t-2,}{j\≠k}}{\overset{t}{\mathrm{\Π}}}\frac{x-x_j}{x_k-x_j}$

If cross (x _t, y _t) point tangent slope be k, then

$k=\frac{\mathrm{dy}}{\mathrm{dx}}{|}_{x=x_t}=\underset{k=t-2}{\overset{t}{\mathrm{\Σ}}}\left(\frac{\underset{\stackrel{j=t-2}{j\≠k}}{\overset{t}{\mathrm{\Σ}}}(x_t-x_j)}{\underset{\stackrel{j=t-2}{j\≠k}}{\overset{t}{\mathrm{\Π}}}(x_k-x_j)}{y}_k\right)---\left(4\right)$

Curve is at (x _t, y _t) tangent slope of point is that k is exactly a target in t direction of motion constantly.Note is from point (x _I-2, y _T-2) to point (x _t, y _t) curve arc long be s, then

$s={\∫}_L\mathrm{ds}$

$=\frac{1}{2\left|a\right|b}\left[\begin{array}{c}(b{x}_t+c)\sqrt{{(b{x}_t+c)}^2+a^2}-(b{x}_{t-2}+c)\sqrt{{(b{x}_{t-2}+c)}^2+a^2}\\ +a^2\ln \frac{(b{x}_t+c)\sqrt{{(b{x}_t+c)}^2+a^2}}{(b{x}_{t-2}+c)\sqrt{{(b{x}_{t-2}+c)}^2+a^2}}\end{array}\right]---\left(5\right)$

Wherein, α=(x _T-2-x _T-1) (x _T-1-x _t) (x _t-x _T-2)

b＝2[(y _t-1-y _t)x _t-2+(y _t-y _t-2)x _t-1+(y _t-2-y _t-1)x _t]

$c=(y_t-y_{t-1})x_{t-2}^2+(y_{t-2}-y_t)x_{t-1}^2+(y_{t-1}-y_{t-2})x_t^2$

The note target is at point (x _T-2, y _T-2), (x _T-2, y _T-1), (x _t, y _t) the moment be respectively t _T-2, t _T-1, tt ₍T 〉=3), then the mean speed v of target travel is: $\stackrel{\‾}{v}=\frac{s}{\mathrm{\Δt}}=\frac{s}{t_t-t_{t-2}},$ Wherein s as previously mentioned.

Set a moment t _T+1The predicted position point of target is

Then the CH node calculates the macro-forecast position of targets according to formula 7:

X wherein _tBe t _tTarget location constantly, V is a target velocity:

$V=\left(\begin{array}{c}{v}_x\\ v_y\end{array}\right)=\left(\begin{array}{c}\stackrel{\‾}{v}\cos \mathrm{\θ}\\ \stackrel{\‾}{v}\sin \mathrm{\θ}\end{array}\right)=\left(\begin{array}{c}\stackrel{\‾}{v}\frac{1}{\sqrt{1+k^2}}\\ \stackrel{\‾}{v}\frac{k}{\sqrt{1+k^2}}\end{array}\right)---\left(8\right)$

θ is t target travel direction and x axle clamp angle constantly in the formula (8).

(4) the CH node calculates the final objective position

CH node target predicted position X _T+1Remove to upgrade target measurement position X according to formula 9 _T+1, get target following position X to the end _T+1:

Wherein α and 1-α are respectively the measured value of target location and the weights of predicted value, 0≤α≤1, and α can be by the experiment value.Measured value with the SN node only o'clock is represented as the target location after upgrading in α=1, α=0 then the expression target location that will go out according to the curvilinear motion Equation for Calculating as the target location after upgrading.The value of α can be taked several different methods, determine as the method that can take to test, carry out the several times simulated experiment, investigate the statistical property of measuring position, predicted position and the actual position of some spots mark on the target trajectory, if actual position is more near the measuring position, then get α〉0.5, otherwise get α＜0.5.

The tracking that proposes according to the present invention resolves, can be in the hope of the kinematic parameter of target among the WSN.The present invention has taken into full account the technical characterstic that utilizes WSN to realize target following, its tracking has been considered the demand of WSN method for tracking target low complex degree, low traffic and low energy consumption, The theoretical analysis and simulating, verifying, positioning and tracing method of the present invention can solve the kinematic parameter of target quickly and accurately by the positioning software of establishment.

Attached: rate of curve k (formula 4) and arc length s (formula 5) find the solution

(1) derivation of formula 4:

Hypothetical target is through 3 points: (x _T-2, y _T-2), (x _T-1, y _T-1), (x _t, y _t) (t 〉=3), then used the movement locus of this conic fitting target of 3, the equation of locus of match is as follows:

$y=\frac{(x-x_{t-1})(x-x_t)}{(x_{t-2}-x_{t-1})(x_{t-2}-x_t)}{y}_{t-2}+\frac{(x-x_{t-2})(x-x_t)}{(x_{t-1}-x_{t-2})(x_{t-1}-x_t)}{y}_{t-1}+\frac{(x-x_{t-2})(x-x_{t-1})}{(x_t-x_{t-2})(x_t-x_{t-1})}{y}_t$

Then curve is at (x ₃, y ₃) point tangent slope k be:

$k=\frac{\mathrm{dy}}{\mathrm{dx}}{|}_{x=x_t}$

$=\frac{(2x-x_{t-1}-x_t)}{(x_{t-2}-x_{t-1})(x_{t-2}-x_t)}{y}_{t-2}+\frac{(2x-x_{t-2}-x_t)}{(x_{t-1}-x_{t-2})(x_{t-1}-x_t)}{y}_{t-1}+\frac{(2x-x_{t-2}-x_{t-1})}{(x_t-x_{t-2})(x_t-x_{t-1})}{y}_t$

$=\frac{(x_t-x_{t-1})}{(x_{t-2}-x_{t-1})(x_{t-2}-x_t)}{y}_{t-2}+\frac{(x_t-x_{t-2})}{(x_{t-1}-x_{t-2})(x_{t-1}-x_t)}{y}_{t-1}+\frac{(2x_t-x_{t-2}-x_{t-1})}{(x_t-x_{t-2})(x_t-x_{t-1})}{y}_t$

$=\underset{k=t-2}{\overset{t}{\mathrm{\Σ}}}\left(\frac{\underset{\stackrel{j=t-2}{j\≠k}}{\overset{t}{\mathrm{\Σ}}}(x_t-x_j)}{\underset{\stackrel{j=t-2}{j\≠k}}{\overset{t}{\mathrm{\Π}}}(x_k-x_j)}{y}_k\right)$

(2) derivation of formula 5:

As shown in Figure 3, curve is at (x _t, y _t) point tangential equation be: y=k (x-x _t)+y _t

Fig. 3 mid point (x _T-2, y _T-2) to point (x _t, y _t) length of curve s be:

$s={\&Integral;}_L\mathrm{ds}={\&Integral;}_{x_{t-2}}^{{\mathrm{<figure-callout\; id="1"\; label="x\; t"\; filenames="H2008100489671E00012.png"\; state="\{\{state\}\}">x</figure-callout>}}_t}\sqrt{1+{[f\&prime;\left(x\right)]}^2}\mathrm{dx}$

$={\&Integral;}_{x_{t-1}}^{{\mathrm{<figure-callout\; id="1"\; label="x\; t"\; filenames="H2008100489671E00012.png"\; state="\{\{state\}\}">x</figure-callout>}}_t}\sqrt{1+{[\frac{(2x-x_{t-1}-x_t)}{(x_{t-2}-x_{t-1})(x_{t-2}-x_t)}{y}_{t-2}+\frac{(2x-x_{t-2}-x_t)}{(x_{t-1}-x_{t-2})(x_{t-1}-x_t)}{y}_{t-1}+\frac{(2x-x_{t-2}-x_{t-1})}{(x_t-x_{t-2})(x_t-x_{t-1})}{y}_t]}^2\mathrm{dx}}$

α＝(x _t-2-x _t-1)(x _i-1-x _t)(x _t-x _t-2)

b＝2[(y _t-1-y _t)x _i-2+(y _t-y _t-2)x _t-1+(y _t-2-y _t-1)x _t]

Order: $c=(y_t-y_{t-1})x_{t-2}^2+(y_{t-2}-y_t)x_{t-1}^2+(y_{t-1}-y_{t-2})x_t^2$

Then have:

$s={\&Integral;}_L\mathrm{ds}={\&Integral;}_{x_{t-2}}^{{\mathrm{<figure-callout\; id="1"\; label="x\; t"\; filenames="H2008100489671E00012.png"\; state="\{\{state\}\}">x</figure-callout>}}_t}\sqrt{1+{\left(\frac{\mathrm{bx}+c}{a}\right)}^2}\mathrm{dx}$

$={\&Integral;}_{x_{t-2}}^{{\mathrm{<figure-callout\; id="1"\; label="x\; t"\; filenames="H2008100489671E00012.png"\; state="\{\{state\}\}">x</figure-callout>}}_t}\sqrt{1+\left(\frac{{(\mathrm{bx}+c)}^2+a^2}{a^2}\right)}\mathrm{dx}$

$=\frac{1}{\left|a\right|b}{\&Integral;}_{x_{t-2}}^{x_t}\sqrt{{(\mathrm{bx}+c)}^2+a^2}d(\mathrm{bx}+c)$

$=\frac{1}{\left|a\right|b}\left[{|\frac{\mathrm{bx}+c}{2}\sqrt{{(\mathrm{bx}+c)}^2+a^2}+\frac{a^2}{2}\ln (\mathrm{bx}+c)+\sqrt{{(\mathrm{bx}+c)}^2+a^2}|}_{x_{t-2}}^{x_t}\right]$

$=\frac{1}{2\left|a\right|b}\left[\begin{array}{c}(b{x}_t+c)\sqrt{{(b{x}_t+c)}^2+a^2}-({\mathrm{bx}}_{t-2}+c)\sqrt{{(b{x}_{t-2}+c)}^2+a^2}+\\ a^2\ln \frac{({\mathrm{bx}}_t+c)\sqrt{{({\mathrm{bx}}_t+c)}^2+a^2}}{({\mathrm{bx}}_{t-2}+c)\sqrt{{({\mathrm{bx}}_{t-2}+c)}^2+a^2}}\end{array}\right]$

The content that is not described in detail in this title book belongs to this area professional and technical personnel's known prior art.

Claims (1)

1. sensor network target tracking method based on double-deck forecasting mechanism, sensor network is based on level type wireless sensor network, and this sensor network is made up of SN node and CH node, and this method is divided into 4 steps:

(1) SN node calculated target positions, clocking and carving the t target location is X _t, X _t=(x _t, y _t), measurement value sensor is z _t, Z _t={ z ₀, z ₁..., z _tThe expression sensor local historical measuring value; The dbjective state dynamic model is by state transition probability p (X _T+1| X _t) provide; The sensor measurement model is by dbjective state-measuring value transition probability p (z _t| X _t) provide; Prediction is as follows with the update calculation formula:

Prediction: t state estimation p (X constantly _t| Z _t) be used to predict t+1 state constantly:

p(X _t+1|Z _t)＝∫p(X _t+1|X _t)p(X _t|Z _t)dX _t (1)

In the actual scene because movement velocity and direction the unknown of target, setting speed be evenly distributed on [0, v _Max] interval, wherein v _MaxBe the target maximum movement speed, the target travel direction is evenly distributed on [0,2 π] interval, therefore, and p (X _T+1| X _t) distribution just formed with X _tBe the center of circle, radius is v _MaxDisk; Adopt this model, letter distribution p (X is put in prediction _T+1| Z _t) by using posteriority degree of confidence p (X _t| Z _t) and evenly the nuclear of disk carry out convolution and obtain, this convolution has just in time reflected because probabilistic expansion of the dbjective state that the uncertainty of target speed and direction is brought;

Upgrade: update stage adopts metric data that prior distribution density is revised, to obtain required posteriority distribution density at current state:

$p\left(X_{t+1}\right|Z_t\&cup;\left\{z_{t+1}\right\})=\frac{p\left(Z_{t+1}\right|X_{t+1})p\left(X_{t+1}\right|Z_t)}{p\left(z_{t+1}\right|Z_t)}$

$=C^{-1}p\left(z_{t+1}\right|X_{t+1})\&Integral;p\left(X_{t+1}\right|X_t)p(X_t/Z_t)\mathrm{dX}$

$\&Proportional;p\left(z_{t+1}\right|X_{t+1})\&Integral;p\left(X_{t+1}\right|X_t)p\left(X_t\right|Z_t)\mathrm{dX}---\left(2\right)$

Wherein C is a normalization constant, C=∫ p (z _T+1| X _T+1) p (X _T+1| Z _t) dX _t

Set initial p (X ₀| Z ₀)=p (X ₀), just the prior distribution of state is known, then probability distribution function p (X _T+1| Z _T+1) can recursively predict and the acquisition of more newly arriving dbjective state with crossing formula (2) and formula (3) in principle;

(2) measured value of CH node calculated target positions, detect target if be carved with N node during t+1, these nodes calculate the relative posteriority distribution value of target location separately according to formula (2), and send to CH, and CH tries to achieve the measuring position of target according to formula (3)

Represent that this positional value is by this N the target location that the SN node measurement obtains:

$\stackrel{\&OverBar;}{X_{t+1}}=\underset{X_{t+1}^{\left(j\right)}}{\arg \max \{p\left(X_{t+1}^{\left(j\right)}\right|Z_{t+1}^{\left(j\right)})}---\left(3\right)$

Wherein

Represent that j sensor node is according to local historical measurements

Drawing t+1 moment target is positioned at

Posterior probability values, this value representation target be in

Degree of confidence;

(3) CH node calculated target positions estimated value, target setting are through 3 points: (x _T-2, y _T-2), (x _T-1, y _T-1), (x _t, y _t) (t 〉=3), then used the movement locus of this conic fitting target of 3, the equation of locus of match is as follows:

$y=\underset{k=t-2}{\overset{t}{\mathrm{\&Sigma;}}}{l}_k\left(x\right)y_k$

Wherein,

$l_k\left(x\right)=\underset{\begin{array}{c}j=t-2,\\ j\&NotEqual;k\end{array}}{\overset{t}{\mathrm{\&Pi;}}}\frac{x-x_j}{x_k-x_j}$

If cross (x _t, y _t) point tangent slope be k, then

$k=\frac{\mathrm{dy}}{\mathrm{dx}}{|}_{x=x_t}=\underset{k=t-2}{\overset{t}{\mathrm{\&Sigma;}}}\left(\frac{\underset{\begin{array}{c}j=t-2\\ j\&NotEqual;k\end{array}}{\overset{t}{\mathrm{\&Sigma;}}}(x_t-x_j)}{\underset{\begin{array}{c}j=t-2\\ j\&NotEqual;k\end{array}}{\overset{t}{\mathrm{\&Pi;}}}(x_k-x_j)}{y}_k\right)---\left(4\right)$

Curve is at (x _t, y _t) tangent slope of point is that k is exactly a target in t direction of motion constantly, note is from point (x _T-2, y _T-2) to point (x _t, y _t) curve arc long be s, then

$s={\&Integral;}_L\mathrm{ds}$

$=\frac{1}{2\left|a\right|b}\left[\begin{array}{c}({\mathrm{bx}}_t+c)\sqrt{{({\mathrm{bx}}_t+c)}^2+a^2}-({\mathrm{bx}}_{t-2}+c)\sqrt{{({\mathrm{bx}}_{t-2}+c)}^2+a^2}\\ +a^2\ln \frac{({\mathrm{bx}}_t+c)\sqrt{{({\mathrm{bx}}_t+c)}^2+a^2}}{({\mathrm{bx}}_{t-2}+c)\sqrt{{({\mathrm{bx}}_{t-2}+c)}^2+a^2}}\end{array}\right]---\left(5\right)$

Wherein,

a＝(x _t-2-x _t-1)(x _t-1-x _t)(x _t-x _t-2)

b＝2[(y _t-1-y _t)x _t-2+(y _t-y _t-2)x _t-1+(y _t-2-y _t-1)x _t]

$c=(y_t-y_{t-1})x_{t-2}^2+(y_{t-2}-y_t)x_{t-1}^2+(y_{t-1}-y_{t-2})x_t^2$

The note target is at point (x _T-2, y _T-2), (x _T-1, y _T-1), (x _t, y _t) the moment be respectively T _T-2, T _T-1, T _t(t 〉=3), the then mean speed of target travel

For:

Wherein s as previously mentioned;

Set a moment T _T+1The predicted position point of target is

Then the CH node calculates the macro-forecast position of target according to formula (7):

${\hat{X}}_{t+1}=X_t+V*\mathrm{\&Delta;T}---\left(7\right)$

X wherein _tBe T _tTarget location constantly, V is a target velocity:

$V=\left(\begin{array}{c}{v}_x\\ v_y\end{array}\right)=\left(\begin{array}{c}\stackrel{\&OverBar;}{v}\cos \mathrm{\&theta;}\\ \stackrel{\&OverBar;}{v}\sin \mathrm{\&theta;}\end{array}\right)=\left(\begin{array}{c}\stackrel{\&OverBar;}{v}\frac{1}{\sqrt{1+k^2}}\\ \stackrel{\&OverBar;}{v}\frac{k}{\sqrt{1+k^2}}\end{array}\right)---\left(8\right)$

θ is t target travel direction and x axle clamp angle constantly in the formula (8);

(4) the CH node calculates the final objective position

The CH node is used the target predicted position

Go to upgrade the target measurement position according to formula (9)

Get target following position X to the end _T+1:

$X_{t+1}=\mathrm{\&alpha;}\stackrel{\&OverBar;}{X_{t+1}}+(1-\mathrm{\&alpha;}){\hat{X}}_{t+1}---\left(9\right)$

Wherein α and 1-α are respectively the measured value of target location and the weights of predicted value, 0≤α≤1, and α can be by the experiment value; Measured value with the SN node only o'clock is represented as the target location after upgrading in α=1, α=0 then the expression target location that will go out according to the curvilinear motion Equation for Calculating as the target location after upgrading; The value of α is taked several different methods, the method that can take to test is determined, carry out the several times simulated experiment, investigate the statistical property of measuring position, predicted position and the actual position of some spots mark on the target trajectory, if actual position is more near the measuring position, then get α＞0.5, otherwise get α＜0.5.

Images