CN113008222B - Track constraint target tracking method based on continuous time track function - Google Patents

Track constraint target tracking method based on continuous time track function Download PDF

Info

Publication number
CN113008222B
CN113008222B CN202110194528.7A CN202110194528A CN113008222B CN 113008222 B CN113008222 B CN 113008222B CN 202110194528 A CN202110194528 A CN 202110194528A CN 113008222 B CN113008222 B CN 113008222B
Authority
CN
China
Prior art keywords
target
function
curve
track
constraint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110194528.7A
Other languages
Chinese (zh)
Other versions
CN113008222A (en
Inventor
李天成
周金阳
王小旭
吴自高
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110194528.7A priority Critical patent/CN113008222B/en
Publication of CN113008222A publication Critical patent/CN113008222A/en
Application granted granted Critical
Publication of CN113008222B publication Critical patent/CN113008222B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention relates to a track constraint target tracking method based on a continuous time track function, which considers that the motion state of a target is subjected to an inequality constraint condition, and searches a continuous time track function with the minimum mean square error from continuous measurement so as to estimate the real state of the target. By adopting the mode, the method skillfully applies the retardation factor and the barrier function, embeds the inequality constraint condition into the unconstrained continuous time track function, and fits the measured data in a track fitting mode to obtain the state of the target. The method can realize higher-precision estimation and prediction on the target, considers inequality constraint conditions, does not need to know priori information such as a dynamic model, noise covariance and the like of the target in advance, and solves the problem of low tracking precision of the traditional equality constraint target; and the target state is accurately tracked and predicted by adding the track road width inequality constraint prior information.

Description

Track constraint target tracking method based on continuous time track function
Technical Field
The invention relates to the field of target tracking and information fusion, in particular to a track constraint target tracking method based on a continuous time track function. The method is suitable for targets with high-speed change of unmanned planes, missiles and on-orbit spacecrafts.
Background
Constraint target tracking is a vital technology in the field of aerospace, and researchers at home and abroad have carried out deeper research on the constraint target tracking and put forward a large number of related theories and methods. However, most of the current constrained target tracking methods are based on markov transfer models, that is, a motion model of a target needs to be established in advance, and then constraint conditions are considered. This makes the existing method difficult to adapt to situations where the object motion model is unknown.
The document "Single-Road-Constrained Positioning Based on Deterministic tracking Geometry, IEEE COMMUNICATIONS LETTERS, vol.23, pp.80-83" discloses an equality constraint target tracking method Based on a dimension-reduced continuous time Trajectory function. According to the method, a motion model of the target is not required to be established in advance, and the state of the target can be estimated and predicted only by knowing measurement information and constraint conditions. However, in real life, a moving object is not only constrained by an equality, but also constrained by an inequality. The method described in the literature only considers the equality constraint condition and will not be applicable to the problem of considering the inequality constraint condition and the track road width. Meanwhile, in the method disclosed by the literature, in the constraint target tracking modeling, the dimensionality of a target dynamic model is reduced through constraint conditions, and the equivalence of a dimensionality reduction system and an original system is neglected.
Disclosure of Invention
Technical problem to be solved
The invention provides a track constraint target tracking method based on a continuous time track function, aiming at solving the problem that the existing method only can consider equality constraint information and neglect the equivalent relation of a model. And solving the constraint state of the moving target by considering the inequality constraint condition borne by the target, thereby improving the target tracking precision.
Technical scheme
A track constraint target tracking method based on a continuous time track function is characterized by comprising the following steps:
step 1: unconstrained continuous-time trajectory function modeling
Firstly, establishing a target unconstrained continuous time trajectory function curve:
x t+1 ＝f(x t ；δ)+μ t (1)
in the formula, x t Represents the state of the target at time t, δ represents the fitting coefficient, f (x) t (ii) a Delta) represents the unconstrained continuous-time trajectory function curve, mu t Representing an environmental error;
the measurement model of the sensor to the target is established as follows:
Figure BDA0002946017620000022
in the formula, z t Zeta (x) which represents the observed value of the sensor to the target state at time t t ) An observation equation is expressed by expressing,
Figure BDA0002946017620000023
representing sensor observation noise;
and 2, step: continuous time trajectory function polynomial fit order determination
In order to find an inequality constraint curve with the minimum mean square error from the measurement information, a polynomial fitting order needs to be determined; assuming that the states of the moving target in each dimension are mutually independent, and known by formula (3), for a constant velocity motion model CV, the order of a curve polynomial can be 1; for the constant acceleration motion model CA, the curve polynomial may take the order of 2; meanwhile, the second-order curve is a parabola, and the polynomial curve order of the circular motion or turning model can also be 2 orders;
Figure BDA0002946017620000021
wherein a and b are constants known for determination;
and 3, step 3: considering inequality constraint conditions
Aiming at the condition that the target motion state is subjected to inequality constraint, adding a retardation factor and a barrier function into an unconstrained continuous time trajectory function is considered; the inequality constraint conditions and the barrier function expressions are as follows:
|C(x t )|＜L (4)
Figure BDA0002946017620000031
in the formula, C (x) t ) Is an inequality constraint function relational expression borne by a target state, L is an inequality constraint upper limit,
Figure BDA0002946017620000035
for the retardation factor psi is the barrier function;
and 4, step 4: inequality constrained continuous time trajectory function target tracking
In order to solve the most suitable constraint time trajectory function curve, the fitting coefficient delta is solved through the minimum fitting residual error, and the fitting width [ t ] of the time series is set 1 t 2 ](ii) a Then, obtaining an optimal constraint estimation track curve by adopting a non-deviation least square fitting technology; the inequality constraint fitting expression is:
Figure BDA0002946017620000032
in the formula (I), the compound is shown in the specification,
Figure BDA0002946017620000033
representing an inequality constraint continuous time function trajectory curve;
by the method, an optimal inequality constraint continuous time trajectory function curve can be obtained, and the state of any time on the curve is shown as the following formula:
Figure BDA0002946017620000034
advantageous effects
The invention provides a track constraint target tracking method based on a continuous time track function, which aims to consider that the motion state of a target is subjected to an inequality constraint condition, and search a continuous time track function with the minimum mean square error from continuous measurement so as to estimate the real state of the target. By adopting the mode, the method skillfully applies the retardation factor and the barrier function, embeds the inequality constraint condition into the unconstrained continuous time trajectory function, and fits the measured data in a track fitting mode to obtain the state of the target. The method can realize higher-precision estimation and prediction on the target, considers inequality constraint conditions, does not need to know priori information such as a dynamic model, noise covariance and the like of the target in advance, and solves the problem of low tracking precision of the traditional equality constraint target; and accurate tracking and prediction of a target state are realized by adding the prior information constrained by the inequality of the track road width. The method solves the problem that the target inequality constraint model is unknown or difficult to establish, and better accords with the real situation.
Drawings
FIG. 1 is a structural framework of the method of the present invention.
FIG. 2 is a linear inequality constraint simulation scenario of the method of the present invention.
FIG. 3 is a result of the linear inequality constraint simulation accuracy of the method of the present invention.
FIG. 4 is a non-linear inequality constraint simulation scenario of the method of the present invention.
FIG. 5 shows the result of the nonlinear inequality constraint simulation accuracy of the method of the present invention.
Detailed Description
The invention will now be further described with reference to the following examples and drawings:
reference is made to fig. 1-5. The invention relates to a track constraint target tracking method based on a continuous time track function, which comprises the following specific steps of:
step 1, simulating a scene.
The simulation scenes are arranged on a two-dimensional coordinate plane, and two simulation scenes are arranged. In the scene 1, a target moves linearly at a constant speed, and the motion state of the target is constrained by a linear inequality; the scene 2 target makes circular motion, and the target motion state is restrained by a nonlinear inequality. The motion models are respectively as follows:
Figure BDA0002946017620000041
Figure BDA0002946017620000042
in the formula, x 1 t And x 2 t Respectively representing the motion states of the first model and the second model at the time T, T representing the sampling interval of simulation time, r representing the radius of circular motion, theta 0 Represents the initial included angle between the moving object of the scene 2 and the X axis, delta theta represents the change of the sampling angle of the sensor each time, the value is a fixed value,
Figure BDA0002946017620000043
and &>
Figure BDA0002946017620000044
Representing the ambient noise of model one and model two, respectively.
The expressions of the sensor observation model are respectively as follows:
Figure BDA0002946017620000051
Figure BDA0002946017620000052
in the formula (I), the compound is shown in the specification,
Figure BDA0002946017620000053
and &>
Figure BDA0002946017620000054
Represents the measurement noise at time t in scene 1 and scene 2, respectively>
Figure BDA0002946017620000055
And &>
Figure BDA0002946017620000056
Indicating the measurement of the sensor at time t.
In a simulation scene, the simulation time length of scene 1 is set as 100s, and a target makes uniform linear motion; the simulation time length of the scene 2 is set to be 90s, the target moves circularly and changes 1 radian per second.
Setting the environmental noise and the measurement noise of the uniform linear motion model as follows:
q 1 ＝1(m/s 2 ) 2 (5)
r 1 ＝4m 2 (6)
then discretizing to obtain:
Figure BDA0002946017620000057
Figure BDA0002946017620000058
for the circular motion model, the environmental noise and the measurement noise are respectively set as follows:
Figure BDA0002946017620000059
Figure BDA00029460176200000510
and 2, determining the order of the continuous time estimation function by using an inequality constraint.
For a scene 1 linear motion model, the order of a polynomial fitting curve is set to 1 order; for the scene 2 circular motion model, the polynomial fit curve order is set to 2.
And 3, increasing linear/nonlinear inequality constraint conditions.
If the track road width of scene 1 is 5 meters, the track road width of scene 2 is 10 meters, the turning radius is 100 meters, and the motion track of the target does not break away from the road, the inequality constraint conditions are respectively as follows:
Figure BDA0002946017620000061
Figure BDA0002946017620000062
in the formula, x 1 And y 1 Representing the size of the position of the scene 1 object in the X and Y axes respectively,
Figure BDA0002946017620000063
and &>
Figure BDA0002946017620000064
Representing the velocity magnitudes, X, of scene 1 objects on the X-axis and Y-axis, respectively 2 And y 2 Representing the size of the position of the scene 2 object in the X and Y axes, respectively. Substituting and simplifying specific numerical values to obtainTo:
Figure BDA0002946017620000065
Figure BDA0002946017620000066
if the retardation factor is 4, the corresponding linear inequality constraint and nonlinear inequality constraint barrier function are respectively:
Figure BDA0002946017620000067
/>
Figure BDA0002946017620000068
substituting the inequality constraint continuous time trajectory function to obtain:
Figure BDA0002946017620000069
Figure BDA00029460176200000610
and 4, estimating a continuous time trajectory function curve by inequality constraint.
And (3) finding a constraint curve with the minimum mean square error by fitting the measurement data of the sensor to estimate the motion state of the target. Since the X-axis and Y-axis of the moving object are independent, the X-axis and Y-axis sensor measurement data are fitted in scene 1 and scene 2, respectively. And finally substituting the current time and the time of the next time according to the obtained constraint continuous time track function curve to carry out constraint estimation and prediction on the target state.
And 5, comparing the evaluation indexes.
To illustrate the rationality of the proposed method, the Root Mean Square Error (RMSE) was chosen as the evaluation index. The contrast methods are enhanced linear smooth variant structure filtering and nonlinear smooth variant structure filtering, respectively. Define the target location RMSE as:
Figure BDA0002946017620000071
wherein n is the Monte Carlo random simulation times, X and Y are the measured data of the sensor on the X axis and the Y axis,
Figure BDA0002946017620000072
and
Figure BDA0002946017620000073
is the position information estimated based on the constrained continuous-time trajectory function.
This can be obtained from figures 3 and 5: the enhanced linear smooth variational filtering and nonlinear smooth variational filtering algorithm has larger position root mean square error, and the method provided by the invention is superior to the method and can better solve the inequality constraint target tracking problem.

Claims (1)

1. A track constraint target tracking method based on a continuous time track function is characterized by comprising the following steps:
step 1: unconstrained continuous-time trajectory function modeling
Firstly, establishing a target unconstrained continuous time trajectory function curve:
x t+1 ＝f(x t ；δ)+μ t (1)
in the formula, x t Represents the state of the target at time t, δ represents the fitting coefficient, f (x) t (ii) a Delta) represents the unconstrained continuous-time trajectory function curve, mu t Representing an environmental error;
the measurement model of the sensor to the target is established as follows:
Figure FDA0003742536680000012
in the formula, z t An observed value, ζ (x), representing the state of the target at time t t ) An observation equation is expressed by expressing,
Figure FDA0003742536680000013
representing sensor observation noise;
step 2: continuous time trajectory function polynomial fit order determination
In order to find an inequality constraint curve with minimum mean square error from the measurement information, a polynomial fitting order needs to be determined; assuming that the states of the moving target in each dimension are mutually independent, known by formula (3), for a constant velocity motion model CV, the order of curve polynomial is 1; for a constant acceleration motion model CA, the polynomial order of a curve is 2; meanwhile, the second-order curve is a parabola, and the polynomial order of the curve of the circular motion or turning model is also 2 orders;
Figure FDA0003742536680000011
wherein a and b are constants known for determination;
and step 3: considering inequality constraint conditions
Aiming at the condition that the target motion state is subjected to inequality constraint, adding a retardation factor and a barrier function into an unconstrained continuous time trajectory function is considered; the inequality constraint conditions and the barrier function expressions are as follows:
|C(x t )|＜L (4)
Figure FDA0003742536680000021
in the formula, C (x) t ) Is an inequality constraint function relational expression borne by a target state, L is an inequality constraint upper limit,
Figure FDA0003742536680000025
psi is barrier function;
and 4, step 4: inequality constrained continuous time trajectory function target tracking
In order to solve the most suitable constraint time trajectory function curve, the fitting coefficient delta is solved through the minimum fitting residual error, and the fitting width [ t ] of the time series is set 1 t 2 ](ii) a Then, obtaining an optimal constrained estimated track curve by adopting a non-biased least square fitting technology; the inequality constraint fitting expression is:
Figure FDA0003742536680000022
in the formula (I), the compound is shown in the specification,
Figure FDA0003742536680000023
representing an inequality constraint continuous time function trajectory curve;
by the method, an optimal inequality constraint continuous time trajectory function curve can be obtained, and the state of any time on the curve is shown as the following formula:
Figure FDA0003742536680000024
/>
CN202110194528.7A 2021-02-20 2021-02-20 Track constraint target tracking method based on continuous time track function Active CN113008222B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110194528.7A CN113008222B (en) 2021-02-20 2021-02-20 Track constraint target tracking method based on continuous time track function

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110194528.7A CN113008222B (en) 2021-02-20 2021-02-20 Track constraint target tracking method based on continuous time track function

Publications (2)

Publication Number Publication Date
CN113008222A CN113008222A (en) 2021-06-22
CN113008222B true CN113008222B (en) 2023-03-31

Family

ID=76404902

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110194528.7A Active CN113008222B (en) 2021-02-20 2021-02-20 Track constraint target tracking method based on continuous time track function

Country Status (1)

Country Link
CN (1) CN113008222B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114510676B (en) * 2022-02-17 2024-07-02 西北工业大学 Measurement method for evaluating performance of continuous time track tracking algorithm

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109828600A (en) * 2019-01-09 2019-05-31 北京理工大学 Time optimal quick three-dimensional obstacle-avoiding route planning method
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN112148024A (en) * 2020-08-20 2020-12-29 中国人民解放军海军航空大学 Unmanned aerial vehicle real-time online flight path planning method based on self-adaptive pseudo-spectral method

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7072723B2 (en) * 2002-10-23 2006-07-04 Clearsight Systems Inc. Method and system for optimization of general problems
FR3048148B1 (en) * 2016-02-22 2018-12-07 Universite Blaise Pascal LOCATION OF A TARGET FOR FOLLOWING VEHICLE
CN108803632B (en) * 2018-09-19 2021-03-09 哈尔滨工程大学 Water surface unmanned ship all-state constraint trajectory tracking control method based on saturation compensation technology
CN110208792B (en) * 2019-06-26 2020-02-11 哈尔滨工业大学 Arbitrary straight line constraint tracking method for simultaneously estimating target state and track parameters
CN110632941B (en) * 2019-09-25 2020-12-15 北京理工大学 Trajectory generation method for target tracking of unmanned aerial vehicle in complex environment
CN110749333B (en) * 2019-11-07 2022-02-22 中南大学 Unmanned vehicle motion planning method based on multi-objective optimization
CN110989626B (en) * 2019-12-27 2021-07-27 四川大学 Unmanned aerial vehicle path planning method based on control parameterization
CN111896946B (en) * 2020-06-29 2023-08-18 西北工业大学 Continuous time target tracking method based on track fitting

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109828600A (en) * 2019-01-09 2019-05-31 北京理工大学 Time optimal quick three-dimensional obstacle-avoiding route planning method
CN111413966A (en) * 2020-03-12 2020-07-14 天津大学 Progressive model prediction unmanned planning tracking cooperative control method
CN112148024A (en) * 2020-08-20 2020-12-29 中国人民解放军海军航空大学 Unmanned aerial vehicle real-time online flight path planning method based on self-adaptive pseudo-spectral method

Also Published As

Publication number Publication date
CN113008222A (en) 2021-06-22

Similar Documents

Publication Publication Date Title
CN110375730B (en) Indoor positioning navigation system based on IMU and UWB fusion
US11255677B2 (en) Intelligent device navigation method and navigation system
CN106020231B (en) Hypersonic aircraft reentry trajectory optimization method based on reentry point parameter
CN103776453B (en) A kind of multi-model scale underwater vehicle combined navigation filtering method
Wang et al. Combination of Interacting Multiple Models with the Particle Filter for Three‐Dimensional Target Tracking in Underwater Wireless Sensor Networks
CN110209180B (en) Unmanned underwater vehicle target tracking method based on HuberM-Cubasic Kalman filtering
CN113008222B (en) Track constraint target tracking method based on continuous time track function
Wang et al. Vision-based tracking control of underactuated water surface robots without direct position measurement
CN113342059B (en) Multi-unmanned aerial vehicle tracking mobile radiation source method based on position and speed errors
Haoran et al. Optimal observation configuration of UAVs based on angle and range measurements and cooperative target tracking in three-dimensional space
CN111896946B (en) Continuous time target tracking method based on track fitting
Zhao et al. An improved particle filter based on gravity measurement feature in gravity-aided inertial navigation system
Song et al. Research on Target Tracking Algorithm Using Millimeter‐Wave Radar on Curved Road
CN102707268A (en) Movable radar networking batch-processing type error register
Luo et al. A multi-AUV cooperative navigation method based on the augmented adaptive embedded cubature Kalman filter algorithm
CN117387604A (en) Positioning and mapping method and system based on 4D millimeter wave radar and IMU fusion
Cai et al. Design of Multisensor Mobile Robot Vision Based on the RBPF‐SLAM Algorithm
Chen et al. Path planning of underwater terrain-aided navigation based on improved artificial potential field method
Yan et al. Indoor location method based on UWB and inertial navigation fusion
CN106227697B (en) A kind of non-linear space target state based on manifold determines method
Liang et al. An improved UAV path optimization algorithm for target accurately and quickly localization
Zuo et al. Indoor UAV Integrated Navigation Technology Based on UWB and IMU
Luo et al. A visual-inertial navigation method for high-speed unmanned aerial vehicles
CN113804181B (en) Non-line-of-sight identification and positioning method using anchor point high-dimensional space coordinate information
Qi et al. Research on tracking algorithm of turning maneuvering target

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant