CN112362052A - Fusion positioning method and system - Google Patents
Fusion positioning method and system Download PDFInfo
- Publication number
- CN112362052A CN112362052A CN202011163752.1A CN202011163752A CN112362052A CN 112362052 A CN112362052 A CN 112362052A CN 202011163752 A CN202011163752 A CN 202011163752A CN 112362052 A CN112362052 A CN 112362052A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- module
- noise
- target
- navigation module
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 230000004927 fusion Effects 0.000 title claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims abstract description 48
- 238000001914 filtration Methods 0.000 claims abstract description 41
- 238000005259 measurement Methods 0.000 claims abstract description 19
- 230000001133 acceleration Effects 0.000 claims description 27
- 230000008569 process Effects 0.000 claims description 19
- 230000004807 localization Effects 0.000 claims description 14
- 230000007704 transition Effects 0.000 claims description 4
- 238000004590 computer program Methods 0.000 claims description 2
- 230000003068 static effect Effects 0.000 abstract description 9
- 238000004364 calculation method Methods 0.000 description 10
- 238000012937 correction Methods 0.000 description 9
- 238000004422 calculation algorithm Methods 0.000 description 8
- 238000010276 construction Methods 0.000 description 7
- 238000005070 sampling Methods 0.000 description 7
- 238000004088 simulation Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000010354 integration Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention provides a fusion positioning method and a fusion positioning system. The method of the invention is used for locating a target body with a travel wheel, the target body is provided with an inertial navigation module and a travel wheel-based speed measurement module, and the method comprises the following steps: determining the noise variance of an inertial navigation module and a speed measurement module under the condition that the target body is kept static and a travelling wheel rotates; constructing Kalman filtering state quantities and observed quantities related to the position of the target main body; constructing a noise matrix representing position prediction uncertainty based on the noise variance and the state quantity and the observed quantity; and measuring the observed quantity in real time, and predicting the position of the target main body based on Kalman filtering based on the measured observed quantity.
Description
Technical Field
The invention relates to the technical field of navigation and positioning, in particular to the technical field of integrated navigation. The invention can be applied to systems needing mobile body positioning information, such as an outdoor unmanned system, an indoor intelligent trolley system and the like.
Background
Vehicle navigation positioning is one of the key technologies of the unmanned system, and the precise and safe unmanned system needs to obtain high-precision vehicle positioning information at any time and any space. Because an accelerometer measurement result of an Inertial Navigation module (INS) based on a Micro-Electro-Mechanical System (MEMS) technology is influenced by various noises, vehicle positioning result errors based on dead reckoning of the Inertial Navigation module are rapidly accumulated along with time, and cannot be positioned for a long time, so that the vehicle positioning result errors need to be combined with other sensors for use. Currently, the integration of an inertial navigation module with a speedometer (odometer) and a Radio Frequency Identification (RFID) technology is a possible positioning scheme. Taking the positioning scene shown in fig. 1 as an example, an RFID tag is arranged on a road surface, a vehicle-mounted RFID reader is mounted on a vehicle, and an inertial navigation module and a speedometer are mounted on the vehicle. The RFID label provides discrete absolute position coordinates, and the inertial navigation module and the speedometer finish relative position calculation. The RFID tag can correct the inertial navigation module and the speedometer, so that the difficulty in long-distance positioning of the inertial navigation module and the speedometer is reduced on the whole, and the method is a feasible high-precision positioning method. However, for the unmanned system scenario, the solution-related algorithm lacks a construction method that can be realized in this scenario, and there is room for further improving the positioning accuracy.
At present, a large number of existing schemes are based on a kalman filtering fusion positioning algorithm of sensor errors, system errors of an inertial navigation module, a speedometer and an RFID sensor in all directions are used as state quantities of kalman filtering, and a positioning result is obtained by performing kalman filtering by using a difference value between a position calculated by the inertial navigation module and the RFID position and a difference value between a speed calculated by the inertial navigation module and the speedometer as observed quantities. For example, "a high-precision combined navigation method based on inertia/odometer/RFID" disclosed IN Chinese patent CN107402005A by Sunwei et al, and "Positioning associated and method combining RFID, GPS and INS" disclosed IN U.S. patent US2005143916, such as KIM IN-JUN (KR), are all based on the above methods.
However, the existing navigation positioning scheme mainly has three problems:
(1) the observed quantity uses the position and the speed calculated by the inertial navigation module, namely, the observed quantity construction mode directly adopts the position and the speed calculated by the inertial navigation module independently to subtract the corresponding quantity of other sensors, and the error accumulation speed of the inertial navigation module is very high, so that the observed quantity is seriously far away from the actual error, and the high-precision positioning is difficult to achieve;
(2) the noise covariance matrix is not constructed, and the precision and convergence of Kalman filtering depend on process noise and observation noise covariance matrix, so that how to design is very important, but in the prior art, parameters are adjusted according to experience and actual effects, and how to construct the noise covariance matrix according to the physical significance of the noise covariance matrix is not given so as to avoid the problem of blind parameter adjustment;
(3) at the RFID tag, other sensors such as an inertial navigation module and the like are directly corrected by absolute position information stored in the RFID. In practice, the antenna of the RFID reader has a certain coverage area, when a vehicle reads the RFID tag, a certain position deviation exists between the vehicle and the RFID tag, the absolute position information stored in the RFID tag is directly regarded as the current position of the vehicle, the error can reach the maximum communication distance between the RFID tag and the reader to the maximum extent, and the position correction precision is greatly reduced.
Therefore, the existing kalman filter fusion positioning methods based on sensor errors have at least one of the above problems, and there is a need for further improvement in positioning accuracy.
Disclosure of Invention
The applicant first briefly summarized the principles of the fusion localization method of the present invention, in the form of kalman filter state quantities and observations, in theory, and summarized how the solution of the present invention solves the three problems mentioned in the background art.
In view of the above problem (1):
in the prior art, an inertial navigation module can measure the three-axis acceleration of a vehicle, in each axis direction, the acceleration is integrated for the first time to obtain the speed, the acceleration is integrated for the second time to obtain the position, and the position calculation equation is as follows
Where Δ t is the inertial navigation data update period, pkIs the position of time k, which is defined by time k-1 position pk-1Velocity vk-1And acceleration ak-1And (4) calculating. The velocity and position errors deduced therefrom accumulate over time due to the presence of accelerometer noise. A speedometer is introduced, which can measure the linear velocity of the vehicle, and the speed in each direction can be obtained by combining the yaw angle decomposition measured by the inertial navigation module, and the speedometer can perform Kalman filtering fusion on the primary integration result, namely the speed, of the inertial navigation module; the RFID tag is introduced, absolute position coordinate information is stored in the RFID tag, meanwhile, the position of the vehicle relative to the tag is calculated by combining the received Signal Strength indication RSSI (received Signal Strength indicator), and the secondary integration result, namely the position, of the inertial navigation module is corrected.
The invention designs a method for combining a speedometer, inertial navigation Kalman filtering and discrete RFID correction according to the data output frequency of the speedometer and the RFID label. The Kalman filtering state quantity is the position, the speed and the acceleration of each axis direction; the observed quantity is the discrete RFID position in each axial direction, the linear velocity of a speedometer and the inertial navigation acceleration; the constructed state quantity and the observation quantity directly use the acceleration original value of inertial navigation, an integral result is not directly obtained, and the yaw angle of an inertial navigation module with higher precision is combined to decompose the linear velocity, so that the problem (1) is solved.
For problem (2):
there are two prior noise quantities in kalman filtering, process noise and observation noise. The process noise refers to the error of the state quantity presumed from the k-1 moment at the k moment; the observation noise refers to noise of observation data of a sensor (an inertial navigation module, a speedometer). These two noise quantities are given in the method of the invention as covariance matrices. For observation noise, statistics can be carried out according to the static data of the actually measured inertial navigation module and the speedometer to obtain the variance condition of the noise, and the noise of different sensors in different directions is independent, so that an observation noise covariance matrix can be constructed; for process noise, besides autocovariance, coupling among position, speed and acceleration exists, which represents uncertainty of the acceleration to uncertainty measurement of speed prediction and position prediction, and the method deduces the process noise according to the noise of the accelerometer to obtain a noise matrix close to reality, so that Kalman filtering has high output precision and fast convergence, and the problem (2) is solved. For problem (3):
in the prior art, there are many schemes for positioning a trilateral RFID tag based on received signal strength indication in an indoor static scene. However, the inventor notices that in the positioning of a mobile body, for example, in the positioning of a vehicle, the RFID tag reader usually moves in a single direction relative to the RFID tag, and based on the characteristics, the invention adopts an innovative RFID tag positioning method when the positioning is carried out based on the RFID tag, namely, a double-reader positioning method based on speed judgment is provided, two feasible solutions are obtained by using the signal strength indication received by the double-reader, and the unique solution is determined by combining the speed. And finally, vehicle position correction with higher precision is obtained at the RFID label, and the problem (3) is solved.
In view of the above problems, the inventor of the present application proposes a fusion positioning method and system, which can realize high-precision positioning based on inertial navigation/RFID/speedometer fusion.
According to one aspect of the present invention, there is provided a fusion localization method for localizing a target subject having a travel wheel, the target subject having an inertial navigation module and a travel wheel-based speed measurement module, characterized in that the method comprises:
step 1: measuring the acceleration and the speed of the target body in real time by using the inertial navigation module and the speed measurement module;
step 2: and substituting the measured speed and acceleration serving as filtering observed quantities into a Kalman filtering equation set to predict the position of the target main body, wherein the filtering state quantities are corrected by using a noise covariance matrix among the filtering state quantities in the Kalman filtering equation set, and the noise covariance matrix is determined based on the noise variances of the inertial navigation module and the speed measurement module.
In a preferred implementation, a location module based on a location tag is provided on the target body and the travel path thereof, and the method further includes step 3: and acquiring the position information of the target main body by using the positioning module, and correcting the position prediction result based on the position information.
In another preferred implementation, the noise variance of the inertial navigation module and the velocity measurement module is determined with the target body stationary and the travel wheel rotating.
In another preferred implementation manner, the speed measuring module is a speedometer, the positioning module includes an RFID reader disposed on the target body and an RFID tag disposed in a road surface, and the target body is a vehicle.
In another preferred implementation, the noise covariance matrix comprises:
(1) the process noise covariance matrix Q is then calculated,
and (2) an observed noise covariance matrix R, R ═ diag (D)v,Dv,Da,Da) Wherein D isvThe noise variance of the velocity measurement module.
In another preferred implementation, the kalman filtering equation set is:
wherein A is a state transition matrix, H is an observation matrix,
and
for predicted filtering state quantity and error covariance at K time, KkIn order to obtain the gain of the kalman filter,
for the modified k-time filter state quantity and error covariance, I is an identity matrix of 6 × 6.
In another preferred implementation, the filter state quantity is X ═ px,py,vx,vy,ax,ay]TWherein p isx,pyIs the two-dimensional position coordinate, v, of the target bodyx,vyIs the two-dimensional velocity of the target body, ax,ayIs a two-dimensional acceleration of the target body; the observed quantity is
Wherein,
vl,kis the linear velocity of the target body, thetakIs the yaw angle of the target body.
In another preferred implementation, two RFID readers are arranged transversely in the target body along the direction perpendicular to the traveling direction of the target body
The step 3 comprises the following steps:
3.1, solving two possible relative positions of the RFID tag and the target body by a path loss model based on the signal intensity indicated values of the RFID tag obtained by the two RFID readers;
3.2, determining the relative position of the RFID tag based on the identification time of the two RFID readers to the RFID tag and the vehicle speed;
3.3, reading the absolute position information of the RFID label stored in the RFID label,
and calculating the position information of the target body.
According to another aspect of the invention, a fusion positioning system for positioning a target subject with a travel wheel is provided, the fusion positioning system comprising an inertial navigation module, a travel wheel-based speed measurement module and a fusion positioning calculation module, wherein the fusion positioning calculation module performs positioning calculation by using the method.
According to another aspect of the invention, a computer-readable storage medium is provided, on which a computer program is stored, wherein the program, when executed by a processor, implements a method according to the above.
Technical effects
According to the invention, through redesigning Kalman filtering parameters, state quantity, observed quantity, process noise covariance matrix and observation noise covariance in the Kalman filtering algorithm are innovatively constructed, positioning results can be given at any time and space, high-precision positioning output is realized, and the actual application requirements are met.
In addition, the present invention achieves equivalent, or even more accurate, position correction in the RFID section using only two RFID readers than using three RFID readers. The invention introduces a double-reader RFID label positioning algorithm based on speed judgment to further improve the position correction precision.
Drawings
The invention is illustrated and described only by way of example and not by way of limitation in the scope of the invention as set forth in the following drawings, in which:
FIG. 1 is a scenario diagram of a prior art inertial navigation module/RFID/speedometer positioning scheme;
FIG. 2 is a schematic flow chart of inertial navigation module/RFID/speedometer fusion positioning in an embodiment of the present invention;
FIG. 3 is a schematic diagram of an embodiment of the present invention for positioning using RFID tags;
fig. 4 is a graph comparing the simulated performance of the positioning scheme in the embodiment of the present invention and the prior art scheme.
Detailed Description
In order to make the objects, technical solutions, design methods, and advantages of the present invention more apparent, the present invention will be further described in detail by specific embodiments with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The principles and embodiments of the present invention will be described in detail below with reference to the accompanying drawings.
In this embodiment, the fusion positioning system includes an inertial navigation module, an RFID tag, a reader, and a speedometer. The inertial navigation module, RFID tag and reader, speedometer are arranged in a similar manner to the prior arrangement shown in fig. 1, with a number of RFID tags arranged within the pavement, preferably along the centerline of the pavement, and the inertial navigation module, RFID reader and speedometer arranged within the target vehicle or other moving body.
Fig. 2 is a schematic flow chart of the inertial navigation module/RFID/speedometer fusion positioning method in the present embodiment. When the vehicle moves in a two-dimensional plane, firstly, an inertial navigation module installed on the vehicle is used for measuring and calculating three-axis acceleration and three attitude angles in the vehicle movement in real time, information of the three-axis acceleration and the three attitude angles obtained through calculation is provided for a fusion positioning system (such as a fusion positioning module or other operation units), a speedometer measures the linear speed of the vehicle, and an RFID reader reads position information stored in an RFID tag of a road surface, so that absolute position correction is provided.
In this embodiment, the process of positioning by using the fusion positioning system includes two parts, namely, model pre-construction, real-time data acquisition and position prediction.
Wherein, the model pre-construction part comprises:
step 1: pre-estimating noise;
step 2: constructing Kalman filtering state quantity and observation quantity;
and step 3: constructing a noise matrix;
the real-time data acquisition and position prediction part comprises:
and 4, step 4: real-time data acquisition and Kalman filtering based position prediction;
and 5: and correcting the position prediction result based on the RFID label positioning.
The following is a description of the above five steps.
1. Noise pre-estimation
Data sampling is required before noise estimation, and when the data sampling is carried out, static data of an accelerometer and a speedometer are measured in a state that a vehicle body of a vehicle is static but wheels of the vehicle move. The method can suspend the rear wheel of the vehicle in the air, set a certain rotating speed, measure data of a speedometer and an accelerometer in an inertial navigation module, use the data as static data, and estimate the noise variance D of the accelerometer after low-order mean filteringaSum velocity meter noise variance Dv。
Since there are many non-gaussian noise components such as inertial noise, etc. in the measurement values of the inertial navigation module and the speedometer, which are generally high-frequency noise with large amplitude at local sampling points, under the condition of a sampling frequency of 100Hz, in this embodiment, the influence of these noises is filtered by using low-order mean filtering with an order of 10 to 20 for the raw data provided by the inertial navigation module and the speedometer, which is specifically expressed as
Xi (k) is the sampling value at the moment k, and omega is the order.
Estimating accelerometer noise variance D using low-order mean filtered dataaAnd speedometer noise variance Dv. For example, according to
And carrying out noise estimation, wherein K is the total number of sampling points and mu is the mean value of the sampling values.
2. Kalman filtering state quantity and observation quantity construction
The quantity of filter state of Ichman is X ═ px,py,vx,vy,ax,ay]TWherein p isx,pyFor navigating two-dimensional position coordinates (i.e. plane coordinates, where the z-direction is not taken into account) in a coordinate system)，vx,vyIs a two-dimensional velocity, ax,ayIs a two-dimensional acceleration. The designed state prediction equation is
In the formula XkA filtering state quantity at the moment k, A represents a state transition matrix which represents a state calculation relation at two adjacent moments, delta t is an inertial navigation module updating period, Wk-1Representing the process noise value of the filter state quantity predicted at the k-th instant from the k-1-th instant, W ═ n1 n2 n3 n4 n5 n6]TObedient mean vector of 06×1Of 6-dimensional joint normal distribution, i.e.
Covariance matrix where Q is 6 x 6, n1 n2 n3 n4 n5 n6Process noise for each state quantity. Of course, it should be understood by those skilled in the art that when the state quantity is additionally increased on the basis of the state quantity of the present embodiment, the matrix order is also increased accordingly, and the 6 rows and 6 columns in the present embodiment are only an example.
In the scheme of the embodiment, two-dimensional acceleration and speed are observable, wherein the acceleration value is measured by the inertial navigation module, and the speed is measured by the speedometer. The observation equation is
Wherein ZkIn order to observe the amount of state,
the speed and acceleration in XY direction measured by speedometer and inertial navigation module, and H is observation matrix representing observationThe relationship between the amount and the actual amount; vkAnd the noise is observed at the moment k, the noise represents the noise conditions of the inertial navigation module and the speedometer sensor, namely the difference between an observed value and a true value, and the statistical characteristic of the noise is measured by using a covariance matrix R.
The speed measured by the speedometer is the linear velocity v of the vehiclel,kAccording to the yaw angle theta output by the inertial navigation modulekDecomposed into XY directions, i.e.
The current inertial navigation module has high yaw angle precision, the error is less than 1 degree, the stability is within 0.1 degree, and the angle value can be smoothly output, so the influence of the angle value on speed noise can be ignored.
3. Noise covariance matrix construction
For the process noise covariance matrix, since both velocity and position are calculated from acceleration in the state prediction equation, the covariance matrix Q represents the measure of uncertainty brought to the velocity prediction and position prediction by the uncertainty of acceleration in the prediction process.
In the present embodiment, the variables represented from the first row to the sixth row and from the first column to the sixth column in the matrix Q are consistent with the filter state quantities, i.e., from p, according to the definition of the covariancexTo ayEach element being the covariance between the respective filter state quantities represented by the row and column in which it is located. For example, the X-direction position pxProcess noise n1The first behavior of Q is due to the uncertainty of the six components of the XY direction acceleration, XY direction velocity and XY direction position at the time k estimated from the time k-1
[Cov(px,px) Cov(px,py) Cov(px,vx) Cov(px,vy) Cov(px,ax) Cov(px,ay)]
Second behavior in the same way
[Cov(py,px) Cov(py,py) Cov(py,vx) Cov(py,vy) Cov(py,ax) Cov(py,ay)]
And so on, where uncertainty Cov (p) derived from positionx,px) Can be calculated by the formula (4)
Wherein DaRepresents the accelerometer noise variance, which can be estimated from pre-collected data. According to the above construction method, each element value in the Q matrix of 6 × 6 can be obtained. In particular, due to the acceleration a at two momentsx,k-1、ay,k-1And ax,k、ay,kThere is no reckoning relationship, so Cov (a)x,ax)＝0,Cov(ay,ay) 0; since the parameters (acceleration, velocity, and position) in the two orthogonal directions XY are independent, there is no estimation relationship. The final covariance matrix Q is
Having obtained the covariance matrix, the probability density function of the process noise value W, i.e., equation (3), may be determined, from which the process noise value may be generated.
For the predictive noise covariance matrix, since the speedometer and accelerometer are independent, and the XY directions are also independent, the matrix R is a diagonal matrix with diagonal elements in terms of ZkThe represented parameter sequence is X direction speedometer noise variance, Y direction speedometer noise variance, X direction accelerometer noise variance and Y direction accelerometer noise variance from top to bottom, namely VkThe covariance matrix R is
R＝diag(Dv,Dv,Da,Da) (7)
Wherein DvIs the velocimeter noise variance, which can be estimated from previously acquired static data. Accordingly, it is able to obtain
The normal distribution noise value at time k is 4 × 1.
After the above equations are constructed, the Kalman filtering equation set can be applied to obtain the filtering state quantity XkIs estimated by
Wherein A is a state transition matrix, H is an observation matrix,
and
for predicted K-time filter state quantities and error covariance of filter state quantities, KkIn order to obtain the gain of the kalman filter,
and PkI is an identity matrix of 6 x 6, which is the corrected filter state quantity at the k moment and the error covariance of the filter state quantity respectively.
4. Real-time data acquisition and location prediction
During formal measurement, data measured by the inertial navigation module and the speedometer are fused by using a Kalman filtering algorithm, and a process noise covariance matrix and an observation noise covariance matrix are introduced, wherein the process noise covariance matrix and the observation noise covariance matrix are derived from the estimated noise variance values. And after Kalman filtering is carried out on the data measured by the inertial navigation module and the speedometer according to a formula (8), outputting a positioning result based on the updating frequency of the inertial navigation module and outputting the positioning result.
5. And correcting the Kalman filtering result based on the RFID label positioning.
The method comprises the steps of conducting position prediction by using Kalman filtering based on an inertial navigation module and a speedometer, and meanwhile conducting detection on RFID labels by using an RFID reader in real time. When the RFID reader detects that the RFID tag exists, the RFID tag positioning is executed, the relative position between the vehicle and the tag and the absolute position stored in the RFID tag are calculated by combining the received signal strength indicating value and the vehicle speed, the Kalman filtering result is corrected, and the position calculation is continued according to the position. The RFID location process may be performed by an existing method or by an improved method described below in this embodiment.
Aiming at the defects of the existing RFID label positioning, the inventor of the application improves.
The inventor noted in the development and design process that in vehicle positioning, the reader tends to move in one direction relative to the tag. Therefore, in the solution of the present embodiment, the inventor skillfully utilizes the feature and proposes an innovative positioning solution based on positioning by RFID tag to make up for the defects of the prior art.
According to the existing RFID tag using method, in a static RFID tag location scenario, a unique location solution can be determined using three readers, so that it is necessary to use three readers for RFID tag-based location.
However, in a preferred implementation of the present embodiment, according to the dynamic scenario of vehicle positioning, the inventor reduces one RFID reader, i.e., sets two RFID readers, and obtains a unique positioning solution, because the vehicle movement has a unidirectional characteristic and the speed is available. Because the RFID tags are arranged along the center line of the lane and appear in the bottom area of the vehicle, the embodiment provides a reader installation mode as shown in fig. 3, two RFID readers are transversely arranged on two sides of the vehicle body, and the distance between the two readers is LR1,R2Distance H from the groundc. In the automatic driving process, lane keeping is basically performed by taking the center of a lane as a reference, so that the occurrence position of the RFID tag is mainly between the readers on the left side and the readers on the right side, and if two readers are used, the two RFID tag coordinate solutions are respectively positioned in front of and behind the reader connecting line. Then, vehicle speed information and R are acquiredAnd the FID identification time can obtain a unique solution based on the vehicle speed information and the RFID identification time.
The RFID reader on the vehicle monitors whether the RFID label exists on the road surface in real time, detects the label at a certain moment and successfully receives the absolute position information x stored by the RFIDtAnd ytAnd the respective received signal strength indication measurements of the two readers are RSS1And RSS2. According to the geometric relationship shown in the figure, the reflected power P of the label is set to be in accordance with a free space path loss modeltIf the operating frequency is f, for example, R1, the candidate position formed on the road surface is a circle with a radius R1Is composed of
r2The same is calculated. From the intersection formula of two circles, two coordinate solutions can be obtained
Setting the label identification time as tidentThe reading area is circular, the radius is r, and the speed of the vehicle during the identification period is vl,kIf it satisfies
Then get
Otherwise, get
I.e. the current position of the vehicle can be determined by the formula.
Simulation example:
(1) simulation parameter setting
The vehicle starts from the origin in a static state, travels in a straight line for about 600m in the positive x direction, then turns to the positive y direction, travels in a straight line for about 120m, turns to the negative x direction, travels in a straight line for about 600m, enters a curve section with the curvature of about 1/60, and stops at the origin. The values of the parameters involved in the whole process are shown in table 1.
Table 1 simulation parameter set-up and description
The simulation selects: (1) the inertial navigation module works independently, (2) the inertial navigation module/speedometer works together and utilizes Kalman filtering, (3) the inertial navigation module/speedometer/RFID direct correction and (4) the inertial navigation module/speedometer/RFID tag positioning algorithm in the embodiment of the invention to compare, so as to explain the effectiveness of scheme design.
Figure 4 shows the positioning error versus time for the four schemes.
(1) When the inertial navigation module works independently, the positioning error is rapidly accumulated along with the time due to the existence of the acceleration noise measured by the inertial navigation module, and finally the error of 4.5m is accumulated. (2) The inertial navigation module/speedometer Kalman filtering optimizes the speed, so that the calculation error of the inertial navigation module can be greatly reduced, but during long-distance positioning, due to the existence of noise of the inertial navigation module and the speedometer, the positioning error still accumulates along with time, and finally reaches an error of about 0.5 m. (3) After direct correction of the RFID is introduced, the position is corrected at a certain distance, and the overall error accumulation trend can be eliminated, but because the scheme directly uses the absolute position information stored by the RFID tag as the current vehicle position, the error during correction is uncertain, the maximum communication distance of the RFID tag can be reached, and the local positioning error in simulation can reach about 0.3 m. (4) According to the scheme in the embodiment of the invention, not only Kalman filtering is improved, but also the RFID positioning algorithm is improved, a new RFID label positioning algorithm is introduced, the positioning precision of the RFID label can be effectively improved, and meanwhile, the subsequent calculation result of the position without the RFID label is more accurate, so that the positioning method can integrally and locally achieve the high-precision positioning result with the error less than 0.1 m.
Having described embodiments of the present invention, the foregoing description is intended to be exemplary, not exhaustive, and not limited to the embodiments disclosed. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The terminology used herein is chosen in order to best explain the principles of the embodiments, the practical application, or improvements made to the technology in the marketplace, or to enable others of ordinary skill in the art to understand the embodiments disclosed herein.
Claims (10)
1. A fusion localization method for localizing a target subject having a travel wheel, the target subject having an inertial navigation module and a travel wheel-based velocity measurement module, the method comprising:
step 1: measuring the acceleration and the speed of the target body in real time by using the inertial navigation module and the speed measurement module;
step 2: and substituting the measured speed and acceleration serving as filtering observed quantities into a Kalman filtering equation set to predict the position of the target main body, wherein the filtering state quantities are corrected by using a noise covariance matrix among the filtering state quantities in the Kalman filtering equation set, and the noise covariance matrix is determined based on the noise variances of the inertial navigation module and the speed measurement module.
2. The fusion localization method according to claim 1, wherein a location tag-based localization module is provided on the target subject and its travel path, the method further comprising step 3: and acquiring the position information of the target main body by using the positioning module, and correcting the position prediction result based on the position information.
3. The fusion localization method of claim 1, wherein the noise variance of the inertial navigation module and the velocity measurement module is determined while the target subject remains stationary and the road wheel is rotating.
4. The fusion positioning method according to claim 2, wherein the speed measuring module is a speedometer, the positioning module comprises an RFID reader provided to the target subject and an RFID tag provided in a road surface, and the target subject is a vehicle.
5. The fusion localization method according to claim 1 or 2, wherein the noise covariance matrix comprises:
(1) the process noise covariance matrix Q is then calculated,
wherein DaThe noise variance of an accelerometer in the inertial navigation module is shown, and delta t is the update period of the inertial navigation module;
and (2) an observed noise covariance matrix R, R ═ diag (D)v,Dv,Da,Da) Wherein D isvThe noise variance of the velocity measurement module.
6. The fusion localization method of claim 2, wherein the kalman filter equation set is:
wherein A is a state transition matrix, H is an observation matrix,
and
error covariance of the predicted K-time filter state quantity and filter state quantity, KkIn order to obtain the gain of the kalman filter,
and PkI is an identity matrix of 6 x 6, I is the corrected filter state quantity at the k time and the error covariance of the filter state quantity.
7. The fusion localization method of claim 1, wherein the filter state quantity is X ═ px,py,vx,vy,ax,ay]TWherein p isx,pyIs the two-dimensional position coordinate, v, of the target bodyx,vyIs the two-dimensional velocity of the target body, ax,ayIs a two-dimensional acceleration of the target body; the observed quantity is
Wherein,
vl,kis the linear velocity of the target body, thetakIs the yaw angle of the target body.
8. The fusion localization method of claim 4, wherein two RFID readers are arranged laterally within the target body perpendicular to its direction of travel, characterized in that:
the step 3 further comprises:
3.1, solving two possible relative positions of the RFID tag and the target body by a path loss model based on the signal intensity indicated values of the RFID tag obtained by the two RFID readers;
3.2, determining the relative position of the RFID tag based on the identification time of the two RFID readers to the RFID tag and the vehicle speed;
and 3.3, reading the absolute position information of the RFID tag stored in the RFID tag, and calculating the position information of the target main body.
9. A fused localization system for localizing a target subject having a travel wheel, comprising an inertial navigation module, a travel wheel based speed measurement module and a fused localization solution module, characterized in that the fused localization solution module employs the method of any one of claims 1 to 8 for localization solution.
10. A computer-readable storage medium, on which a computer program is stored, which program, when being executed by a processor, carries out the method according to any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011163752.1A CN112362052B (en) | 2020-10-27 | 2020-10-27 | Fusion positioning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011163752.1A CN112362052B (en) | 2020-10-27 | 2020-10-27 | Fusion positioning method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112362052A true CN112362052A (en) | 2021-02-12 |
| CN112362052B CN112362052B (en) | 2022-09-16 |
Family
ID=74510712
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011163752.1A Active CN112362052B (en) | 2020-10-27 | 2020-10-27 | Fusion positioning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112362052B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113257044A (en) * | 2021-07-09 | 2021-08-13 | 中航信移动科技有限公司 | Filtering method and device of ADS-B data, computer equipment and storage medium |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120086598A1 (en) * | 2010-10-08 | 2012-04-12 | Canadian Space Agency | Apparatus and methods for driftless attitude determination and reliable localization of vehicles |
| CN104007459A (en) * | 2014-05-30 | 2014-08-27 | 北京融智利达科技有限公司 | Vehicle-mounted integrated positioning device |
| CN105467382A (en) * | 2015-12-31 | 2016-04-06 | 南京信息工程大学 | SVM (Support Vector Machine)-based multi-sensor target tracking data fusion algorithm and system thereof |
| CN106767783A (en) * | 2016-12-15 | 2017-05-31 | 东软集团股份有限公司 | Positioning correction method and device based on vehicle-carrying communication |
| CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
| CN107343052A (en) * | 2017-07-25 | 2017-11-10 | 苏州大学 | The physical distribution monitoring system that a kind of WSN is combined with RFID |
| CN107402005A (en) * | 2016-05-20 | 2017-11-28 | 北京自动化控制设备研究所 | One kind is based on inertia/odometer/RFID high-precision integrated navigation method |
| CN109916407A (en) * | 2019-02-03 | 2019-06-21 | 河南科技大学 | Indoor mobile robot combined positioning method based on adaptive Kalman filter |
| CN110243363A (en) * | 2019-07-03 | 2019-09-17 | 西南交通大学 | A kind of AGV real-time location method based on inexpensive IMU in conjunction with RFID technique |
| CN111272174A (en) * | 2020-02-27 | 2020-06-12 | 中国科学院计算技术研究所 | Combined navigation method and system |
-
2020
- 2020-10-27 CN CN202011163752.1A patent/CN112362052B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120086598A1 (en) * | 2010-10-08 | 2012-04-12 | Canadian Space Agency | Apparatus and methods for driftless attitude determination and reliable localization of vehicles |
| CN104007459A (en) * | 2014-05-30 | 2014-08-27 | 北京融智利达科技有限公司 | Vehicle-mounted integrated positioning device |
| CN105467382A (en) * | 2015-12-31 | 2016-04-06 | 南京信息工程大学 | SVM (Support Vector Machine)-based multi-sensor target tracking data fusion algorithm and system thereof |
| CN107402005A (en) * | 2016-05-20 | 2017-11-28 | 北京自动化控制设备研究所 | One kind is based on inertia/odometer/RFID high-precision integrated navigation method |
| CN106767783A (en) * | 2016-12-15 | 2017-05-31 | 东软集团股份有限公司 | Positioning correction method and device based on vehicle-carrying communication |
| CN107289933A (en) * | 2017-06-28 | 2017-10-24 | 东南大学 | Double card Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions |
| CN107343052A (en) * | 2017-07-25 | 2017-11-10 | 苏州大学 | The physical distribution monitoring system that a kind of WSN is combined with RFID |
| CN109916407A (en) * | 2019-02-03 | 2019-06-21 | 河南科技大学 | Indoor mobile robot combined positioning method based on adaptive Kalman filter |
| CN110243363A (en) * | 2019-07-03 | 2019-09-17 | 西南交通大学 | A kind of AGV real-time location method based on inexpensive IMU in conjunction with RFID technique |
| CN111272174A (en) * | 2020-02-27 | 2020-06-12 | 中国科学院计算技术研究所 | Combined navigation method and system |
Non-Patent Citations (2)
| Title |
|---|
| MINGHONG ZHU 等: "An_Improved_Posteriori_Variance-Covariance_Components_Estimation_Applied_to_Unconventional_GPS_and_Multiple_Low-Cost_Imus_Integration_Strategy", 《SPECIAL SECTION ON EMERGING TRENDS, ISSUES AND CHALLENGES FOR ARRAY SIGNAL PROCEAPPLICATIONS IN SMART CITY》 * |
| 蔡煊 等: "基于改进UKF的BDS_IMU组合列车定位方法", 《西南交通大学学报》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113257044A (en) * | 2021-07-09 | 2021-08-13 | 中航信移动科技有限公司 | Filtering method and device of ADS-B data, computer equipment and storage medium |
| CN113257044B (en) * | 2021-07-09 | 2022-02-11 | 中航信移动科技有限公司 | Filtering method and device of ADS-B data, computer equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112362052B (en) | 2022-09-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
| CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
| JP2022113746A (en) | Determination device | |
| CN101793528B (en) | System and method of lane path estimation using sensor fusion | |
| CN103777220B (en) | Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS | |
| Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
| CN107132563B (en) | Combined navigation method combining odometer and dual-antenna differential GNSS | |
| CN105628026A (en) | Positioning and posture determining method and system of mobile object | |
| CN109752725A (en) | Low-speed commercial robot, positioning and navigation method and positioning and navigation system | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN103487050A (en) | Positioning method for indoor mobile robot | |
| CN112198503A (en) | Target track prediction optimization method and device and radar system | |
| CN114623823B (en) | UWB (ultra wide band) multi-mode positioning system, method and device integrating odometer | |
| WO2016203744A1 (en) | Positioning device | |
| CN112147651B (en) | Asynchronous multi-vehicle cooperative target state robust estimation method | |
| CN113342059B (en) | Multi-unmanned aerial vehicle tracking mobile radiation source method based on position and speed errors | |
| CN112362052B (en) | Fusion positioning method and system | |
| CN118129733A (en) | Mobile robot positioning method under typical laser radar degradation scene | |
| Anousaki et al. | Simultaneous localization and map building of skid-steered robots | |
| JP2021085880A (en) | Analysis of localization error in mobile object | |
| CN113063441A (en) | Data source correction method and device for accumulated calculation error of odometer | |
| Song et al. | RFID/in-vehicle sensors-integrated vehicle positioning strategy utilising LSSVM and federated UKF in a tunnel | |
| CN116047480A (en) | External parameter calibration method from laser radar to attitude sensor | |
| CN114666732B (en) | Moving target positioning calculation and error evaluation method under noisy network | |
| Pereira et al. | Backward motion for estimation enhancement in sparse visual odometry |
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 |