CN114543793A - Multi-sensor fusion positioning method of vehicle navigation system - Google Patents
Multi-sensor fusion positioning method of vehicle navigation system Download PDFInfo
- Publication number
- CN114543793A CN114543793A CN202011330616.7A CN202011330616A CN114543793A CN 114543793 A CN114543793 A CN 114543793A CN 202011330616 A CN202011330616 A CN 202011330616A CN 114543793 A CN114543793 A CN 114543793A
- Authority
- CN
- China
- Prior art keywords
- particle
- navigation system
- vehicle
- initial
- particle swarm
- 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 70
- 230000004927 fusion Effects 0.000 title claims abstract description 59
- 239000002245 particle Substances 0.000 claims abstract description 448
- 238000001914 filtration Methods 0.000 claims abstract description 73
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 43
- 238000005457 optimization Methods 0.000 claims abstract description 34
- 230000001133 acceleration Effects 0.000 claims description 25
- 230000006870 function Effects 0.000 claims description 21
- 230000014509 gene expression Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 15
- 238000012952 Resampling Methods 0.000 claims description 11
- 230000033001 locomotion Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 7
- 238000005070 sampling Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 101100134058 Caenorhabditis elegans nth-1 gene Proteins 0.000 description 1
- 230000003416 augmentation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation 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
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention provides a multi-sensor fusion positioning method of a vehicle navigation system, wherein the vehicle navigation system comprises a strapdown inertial navigation system and a global navigation satellite positioning system, and the method comprises the following steps: judging whether the signal of the global navigation satellite positioning system is lost; if the vehicle navigation information is lost, the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle; the navigation information of the vehicle comprises vehicle position information and vehicle speed information; and if the vehicle navigation information is not lost, performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by using a particle swarm optimization and particle filtering algorithm, and taking the fused information as the navigation information of the vehicle. The vehicle navigation system combines the global navigation satellite positioning system and the low-cost strapdown inertial navigation system to jointly position the vehicle, so that the positioning function of the system under the condition that satellite signals are lost when the system enters a culvert and the like is ensured, the precision of the vehicle navigation system is improved, and the cost of the vehicle navigation system is also reduced.
Description
Technical Field
The invention relates to the technical field of vehicle navigation, in particular to a multi-sensor fusion positioning method of a vehicle navigation system.
Background
The AR automobile live-action navigation technology has received extensive attention from academic circles and industrial circles in recent years, and the AR navigation helps a driver to make a navigation judgment decision timely and correctly by using a virtual augmentation technology, on the premise that position and posture information of a vehicle can be fed back to a navigation engine in time. However, in the current market, the position and pose analysis of a common driving vehicle depends on GNSS satellite signals, the precision error is about 10 meters, and the precision requirement of navigation products under complex road conditions (viaducts, multiple intersections and the like) cannot be met. Especially in the culvert and tunnel scene, the loss of satellite signals is still the root cause of the failure of the AR navigation product. In the market, the pose analysis of the vehicle with special functions can realize centimeter-level precision error by fusing an RTK differential signal enhancement technology and a professional inertial navigation assembly, but also greatly improves the hardware cost and limits the application of the technology to common driving vehicles.
Therefore, the navigation system of the vehicle in the prior art has the problems of low precision and high navigation cost by using a differential signal enhancement technology.
Disclosure of Invention
The invention aims to solve the problems that in the prior art, a vehicle navigation system is low in precision and high in navigation cost by using a differential signal enhancement technology.
In order to solve the problems, the embodiment of the invention discloses a multi-sensor fusion positioning method of a vehicle navigation system, wherein the vehicle navigation system comprises a strapdown inertial navigation system and a global navigation satellite positioning system; the multi-sensor fusion positioning method of the vehicle navigation system comprises the following steps:
s0: judging whether the signal of the global navigation satellite positioning system is lost; wherein
If the signal of the global navigation satellite positioning system is lost, the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle; the navigation information of the vehicle comprises vehicle position information and vehicle speed information; or
And if the signal of the global navigation satellite positioning system is not lost, performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by utilizing a particle swarm optimization and particle filtering algorithm, and taking the fused information as the navigation information of the vehicle.
By adopting the scheme, the global navigation satellite positioning system and the low-cost strapdown inertial navigation system are combined to jointly position the vehicle, so that the positioning function of the system under the condition that satellite signals entering culverts, tunnels and the like are lost is ensured, the precision of the navigation system of the vehicle is improved, and the cost of the navigation system of the vehicle is also reduced.
According to another specific embodiment of the present invention, the multi-sensor fusion positioning method for a vehicle navigation system disclosed in the embodiment of the present invention performs multi-sensor information fusion on output information of a strapdown inertial navigation system and output information of a global navigation satellite positioning system by using a particle swarm optimization and a particle filtering algorithm, and uses the fused information as navigation information of a vehicle, including:
s1: establishing an initial particle swarm for a strapdown inertial navigation system and a global navigation satellite positioning system, and determining particle swarm parameters of the initial particle swarm;
s2: updating the initial particle swarm according to the output information of the strapdown inertial navigation system to form an updated particle swarm, and acquiring the output information of the strapdown inertial navigation system corresponding to the updated particle swarm;
s3: driving and updating the particles in the particle swarm to move to the high-likelihood region by utilizing a particle swarm optimization algorithm; the high-likelihood region is used for updating the optimal value of the iterative population of the particle swarm and updating the optimal value of the iterative particles of each particle in the particle swarm;
s4: calculating and updating the particle weight of each particle in the particle swarm by using a particle filter algorithm, and performing normalization processing on the particle weight;
s5: resampling each particle in the update particle swarm to form a resampled particle swarm;
s6: and determining output information of the strapdown inertial navigation system and output information of the global navigation satellite positioning system according to the dimensionality of the central particles of the resampling particle group, and bringing the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system into an updated attitude conversion matrix of the strapdown inertial navigation system to obtain an updated attitude of the central particles, wherein the updated attitude is used as navigation information of the vehicle.
By adopting the scheme, the common global navigation satellite positioning system and the low-cost strapdown inertial navigation system are combined for positioning, the function of the positioning system is guaranteed when satellite signals are lost, the particle weight of each particle in the particle swarm is calculated and updated by using a particle filter algorithm, the particle weight is normalized, the requirement of vehicle navigation on a vehicle positioning precision system is met, and the hardware cost of the vehicle positioning system is reduced. Compared with the traditional multi-sensor fusion combined positioning algorithm based on Kalman filtering, the method considers the nonlinear characteristic of a joint inertial navigation system model, adopts a particle filtering algorithm and a particle swarm optimization algorithm to enable the distribution of particles to move to a high-likelihood probability density distribution area, finally reduces the number of filtering particles on the basis of improving the precision, solves the problem of particle depletion, improves the real-time property of the positioning system, and ensures the robustness and the stability of the positioning system. Meanwhile, each filtering time based on a small amount of particles, namely the period of outputting the positioning result in real time can meet the frequency requirement of information fusion of the current low-cost global navigation satellite positioning system and the strapdown inertial navigation system on the output of the positioning result, reduce the computing resources and improve the computing precision.
According to another specific embodiment of the present invention, in the multi-sensor fusion positioning method for a vehicle navigation system disclosed in the embodiment of the present invention, in step S1, the particle swarm parameters of the initial particle swarm include the initial dimension of each particle, the initial mean of the particle swarm, and the initial distribution variance of the particle swarm; and, determining particle population parameters for the initial particle population comprises:
s11: each particle of the initial population of particles is defined according to the following formula:
X＝(vn,ve,vd,lat,lon,h)
wherein X is the dimension of each particle; v. ofnVehicle speed in geographic north direction; v. ofeIs the speed of the vehicle in the geodetic direction; v. ofdVehicle speed in a ground direction perpendicular to the ground surface; lat is longitude; lon is latitude; h is the height;
s12: acquiring the speed of a vehicle in the north direction, the speed of a vehicle in the east direction, the speed of a vehicle in the ground direction, longitude, latitude and altitude at the previous moment by using an inertial navigation recursion algorithm, and taking the obtained speed, longitude, latitude and altitude as an initial average value of the particle swarm;
s13: acquiring the observation precision distribution variance of a global navigation satellite positioning system, and taking the observation precision distribution variance as the initial distribution variance of the particle swarm;
s14: and determining the specific numerical value of the dimension of each particle according to the initial mean value of the particle swarm and the initial distribution variance of the particle swarm.
According to another specific embodiment of the invention, in the multi-sensor fusion positioning method of the vehicle navigation system disclosed by the embodiment of the invention, the output information of the strapdown inertial navigation system comprises the angular velocity of the strapdown inertial navigation system and the acceleration of the strapdown inertial navigation system; step S2 includes:
s21: performing strapdown inertial navigation resolving on each particle in the initial particle swarm according to the angular velocity of the strapdown inertial navigation system at the current moment and the acceleration of the strapdown inertial navigation system to obtain the updated angular velocity and acceleration of the strapdown inertial navigation system;
s22: determining the state noise of a filtering process according to the random walk error of an accelerometer of the strapdown inertial navigation system;
s23: predicting the angular velocity and the acceleration of the strapdown inertial navigation system by using a Gaussian model according to the state noise of the filtering process and the updated angular velocity and acceleration of the strapdown inertial navigation system;
s24: and taking the angular velocity and the acceleration of the strapdown inertial navigation system obtained by prediction as output information of the strapdown inertial navigation system corresponding to the updated particle swarm.
By adopting the scheme, the angular velocity and the acceleration of the strapdown inertial navigation system are predicted by utilizing the Gaussian model according to the state noise of the filtering process and the updated angular velocity and acceleration of the strapdown inertial navigation system, so that the predicted angular velocity and acceleration of the strapdown inertial navigation system are closer to the true values.
According to another specific embodiment of the invention, in the multi-sensor fusion positioning method of the vehicle navigation system disclosed by the embodiment of the invention, the particle swarm parameters of the initial particle swarm further comprise preset iteration steps and initial movement speeds of the particles; and the number of the first and second electrodes,
step S3 includes:
s31: constructing a fitness function expression of the particle swarm optimization algorithm;
s32: performing iterative processing on the initial particle swarm, and setting the initial movement speed of each particle to be one thousandth to ten thousandth of the distance between the initial value of each particle and the observed quantity of the global navigation satellite positioning system;
s33: calculating and recording initial particle optimal values of all particles in all initial particle swarms and initial particle swarm optimal values of all initial particle swarms according to fitness function expressions of the particle swarm optimization algorithm;
s34: enabling each particle in the updated particle swarm to move towards the initial particle optimal value and the initial group optimal value according to the self-learning rate, the group learning rate, the speed and the speed inertia weight coefficient;
s35: calculating and recording the particle history optimal value of each particle in all the updated particle swarms, the particle corresponding to the particle history optimal value, and the global population optimal value of all the updated particle swarms and the particle corresponding to the global population optimal value according to the fitness function expression of the particle swarms optimization algorithm;
s36: judging whether the current iteration step number is equal to a preset iteration step number or not;
if so, ending the iteration and outputting the optimized particle swarm;
if not, entering the next iteration.
By adopting the scheme, the particles can be rapidly moved to the optimal value, and the real-time performance of the system is ensured.
According to another specific embodiment of the present invention, in the multi-sensor fusion positioning method for a vehicle navigation system disclosed in the embodiment of the present invention, a fitness function expression of the particle swarm optimization algorithm is:
wherein, the fitness is a fitness function expression; r istA measured noise covariance matrix of the global navigation satellite positioning system; and is
Moving each particle in the updated population of particles to the initial particle optimum and the initial population optimum according to the following formula:
Vk+1＝C0*Vk+C1*rand*(Yk-Xk)+C2*rand*(Pgbest-Xk)
Xk+1＝Xk+Vk+1
wherein the subscript k represents the current iteration step number; v is the velocity vector of the particle; x is the value of the current particle; y is the particle history optimal value of each particle; pgbestThe particles are corresponding to the historical optimal values of the particles; c0Is a velocity inertial weight coefficient; c1Is the self-learning rate of the particle; c2Is the population learning rate of the particle.
By adopting the scheme, the self-learning rate of the particles and the group learning rate of the particles are considered during the movement of the particles, and the accuracy rate and the movement efficiency of the movement of the particles are improved.
According to another specific embodiment of the present invention, in the multi-sensor fusion positioning method for a vehicle navigation system, step S4 includes:
s41: calculating the optimized particle swarm by using Gaussian distribution; the mean value of the Gaussian distribution is an observed value of a global navigation satellite positioning system, and the variance of the Gaussian distribution is a measurement noise covariance of the global navigation satellite positioning system;
s42: updating the particle weight of each particle in the updated particle swarm according to the calculation result of S41; and updating the particle weight of each particle in the updated population of particles according to the following formula:
wherein the content of the first and second substances,
i is the ith particle; p is the total number of each particle in the particle swarm;
is a Gaussian distribution;
s43: normalizing the particle weight according to the updated particle weight; and, normalizing the particle weights according to the following formula:
wherein the content of the first and second substances,
is the particle weight; i is the ith particle.
By adopting the scheme, particle filtering processing of particle swarm optimization is carried out, compared with a pure particle filtering algorithm, the particle swarm optimization is adopted in the scheme, so that particles tend to high likelihood probability distribution during sampling, divergence can be inhibited, and filtering positioning precision can be improved.
According to another specific embodiment of the present invention, in the multi-sensor fusion positioning method for a vehicle navigation system, step S5 includes:
and resampling each particle in the updated particle swarm by adopting a roulette method so as to enable the resampled particles to move towards an area with a posterior probability density distribution value larger than a preset density distribution value threshold.
According to another specific embodiment of the invention, the particle swarm parameters of the initial particle swarm further comprise preset filtering times, and the preset filtering times are at least one time;
when the preset filtering times is more than one, the step S5 is followed by:
s5': repeatedly executing the step S4 and the step S5, and determining whether the filtering times are equal to the preset filtering times;
when the filtering times are equal to the preset filtering times, ending the filtering and executing the step S6;
when the filtering times are not equal to the preset filtering times, continuously and repeatedly executing the step S4 and the step S5; and the number of the first and second electrodes,
before the step S5' is repeatedly executed, establishing an initialization particle swarm;
and when the preset filtering times are more than equal and twice, updating the distribution variance of the particle swarm and determining according to the distribution variance of the corresponding particle swarm after the last filtering.
According to another specific embodiment of the present invention, in the multi-sensor fusion positioning method for a vehicle navigation system disclosed in the embodiment of the present invention, the particle swarm parameters of the initial particle swarm further include the particle number of the initial particle swarm; and the number of the first and second electrodes,
the number of particles in the initial population of particles is from 80 to 120.
By adopting the scheme, the requirement of the positioning system on real-time performance is met by setting the number of the initial particle swarm to be 80-120.
The invention has the beneficial effects that:
according to the scheme, the real-time requirement of the vehicle positioning system is met by setting the smaller particle swarm number, the particle swarm optimization algorithm effectively solves the problem of particle shortage in the filtering process, and the precision and the stability of the positioning system are improved. In addition, particle swarm optimization enables particles to tend to high likelihood probability distribution during sampling, so that divergence can be restrained, and filtering positioning accuracy can be improved.
Furthermore, the sensors for acquiring the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system adopt existing hardware in vehicle-mounted communication equipment, and do not consider positioning modes such as differential positioning and the like with expensive service cost, so that the hardware cost can be reduced to the maximum extent.
Furthermore, the combined positioning algorithm research is carried out by combining the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system, and the requirements of vehicle navigation on the positioning precision, the instantaneity and the stability of the vehicle are ensured on the basis of reducing the particle number, namely saving the computing resources.
Drawings
FIG. 1 is a schematic flowchart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 2 is another schematic flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 3 is another schematic flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 4 is another schematic flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 5 is another schematic flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 6 is another schematic flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention;
FIG. 7 is a positioning result of a multi-sensor fusion positioning method of a vehicle navigation system according to an embodiment of the present invention;
fig. 8a, fig. 8b, and fig. 8c are comparison diagrams of the trajectory error of the global navigation satellite positioning system and the trajectory error after the particle swarm optimization particle filtering fusion in the multi-sensor fusion positioning method for the vehicle navigation system according to the embodiment of the present invention;
fig. 9a, 9b, and 9c are trajectory comparison diagrams of particle group optimized particle filtering and pure particle filtering algorithms in the multi-sensor fusion positioning method of the vehicle navigation system according to the embodiment of the present invention.
Detailed Description
The following description of the embodiments of the present invention is provided for illustrative purposes, and other advantages and effects of the present invention will become apparent to those skilled in the art from the present disclosure. While the invention will be described in conjunction with the preferred embodiments, it is not intended that features of the invention be limited to these embodiments. On the contrary, the invention is described in connection with the embodiments for the purpose of covering alternatives or modifications that may be extended based on the claims of the present invention. In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention. The invention may be practiced without these particulars. Moreover, some of the specific details have been left out of the description in order to avoid obscuring or obscuring the focus of the present invention. It should be noted that the embodiments and features of the embodiments may be combined with each other without conflict.
It should be noted that in this specification, like reference numerals and letters refer to like items in the following drawings, and thus, once an item is defined in one drawing, it need not be further defined and explained in subsequent drawings.
In the description of the present embodiment, it should be noted that the terms "upper", "lower", "inner", "bottom", and the like indicate orientations or positional relationships based on orientations or positional relationships shown in the drawings or orientations or positional relationships that the product of the present invention is usually placed in when used, and are only used for convenience of description and simplification of description, but do not indicate or imply that the device or element referred to must have a specific orientation, be constructed in a specific orientation, and operate, and therefore, should not be construed as limiting the present invention.
The terms "first," "second," and the like are used solely to distinguish one from another and are not to be construed as indicating or implying relative importance.
In the description of the present embodiment, it should be further noted that, unless explicitly stated or limited otherwise, the terms "disposed," "connected," and "connected" are to be interpreted broadly, e.g., as a fixed connection, a detachable connection, or an integral connection; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present embodiment can be understood in specific cases by those of ordinary skill in the art.
In order to make the objects, technical solutions and advantages of the present invention more apparent, embodiments of the present invention will be described in detail with reference to the accompanying drawings.
In order to solve the problems that a navigation system of a vehicle in the prior art is low in precision and high in navigation cost by using a differential signal enhancement technology, the embodiment of the invention discloses a multi-sensor fusion positioning method of the vehicle navigation system. Specifically, in the multi-sensor fusion positioning method for the vehicle navigation system provided by the embodiment of the invention, the vehicle navigation system comprises a strapdown inertial navigation system and a global navigation satellite positioning system. Referring to fig. 1, a flow chart of a multi-sensor fusion positioning method for a vehicle navigation system according to an embodiment of the present invention is shown, and the multi-sensor fusion positioning method for the vehicle navigation system includes the following steps:
s0: and judging whether the signal of the global navigation satellite positioning system is lost.
If the signal of the global navigation satellite positioning system is lost, the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle; the navigation information of the vehicle comprises vehicle position information and vehicle speed information; or
And if the signal of the global navigation satellite positioning system is not lost, performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by utilizing a particle swarm optimization and particle filtering algorithm, and taking the fused information as the navigation information of the vehicle.
By adopting the scheme, the global navigation satellite positioning system and the low-cost strapdown inertial navigation system are combined to jointly position the vehicle, so that the positioning function of the system under the condition that satellite signals entering culverts, tunnels and the like are lost is ensured, the precision of the navigation system of the vehicle is improved, and the cost of the navigation system of the vehicle is also reduced.
The multi-sensor fusion positioning method of the vehicle navigation system provided by the embodiment of the invention is specifically described below with reference to fig. 1 to 9, wherein fig. 1 to 6 are schematic flow diagrams of the multi-sensor fusion positioning method of the vehicle navigation system provided by the embodiment of the invention; FIG. 7 is a positioning result of a multi-sensor fusion positioning method of a vehicle navigation system according to an embodiment of the present invention; FIG. 8 is a comparison graph of the trajectory error of the global navigation satellite positioning system and the trajectory error after particle swarm optimization particle filtering fusion in the multi-sensor fusion positioning method for the vehicle navigation system provided by the embodiment of the present invention; fig. 9 is a comparison diagram of the particle group optimized particle filtering and pure particle filtering algorithm in the multi-sensor fusion positioning method of the vehicle navigation system according to the embodiment of the invention.
Referring to fig. 1, in the multi-sensor fusion positioning method of the vehicle navigation system provided in this embodiment, step S0 is first executed to determine whether the signal of the global navigation satellite positioning system is lost.
And whether the signal of the global navigation satellite positioning system is lost or not is judged, the signal of the global navigation satellite positioning system can be indirectly judged according to the vehicle condition, namely when the vehicle enters a tunnel or a culvert, the signal of the global navigation satellite positioning system is judged to be lost, or the signal can be directly judged through the received signal of the vehicle-mounted communication equipment, namely when the vehicle-mounted communication equipment can not receive the signal of the global navigation satellite positioning system, the signal of the global navigation satellite positioning system is judged to be lost.
And if the signal of the global navigation satellite positioning system is lost, the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle. That is, in the culvert and tunnel scenario, if the signal of the global navigation satellite positioning system is lost, only the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle, and the navigation information is used for navigating the vehicle.
It should be noted that, in this embodiment, the strapdown inertial navigation system is an autonomous navigation system that does not depend on any external information and does not radiate energy to the outside. The strapdown inertial navigation system consists of three rate gyroscopes, three linear accelerometers and a microcomputer, wherein the gyroscopes and the accelerometers are respectively used for measuring angular motion information and linear motion information of the carrier. The output information of the strapdown inertial navigation system comprises the angular velocity of the strapdown inertial navigation system and the acceleration of the strapdown inertial navigation system. The angular velocity of the strapdown inertial navigation system is also the angular velocity of a rate gyro of the strapdown inertial navigation system, and the acceleration of the strapdown inertial navigation system is also the acceleration information acquired by an accelerometer of the strapdown inertial navigation system. The microcomputer calculates the heading, attitude, speed and position of the vehicle, i.e. the vehicle, according to the measurement information, and can navigate the vehicle.
And if the signal of the global navigation satellite positioning system is not lost, performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by utilizing a particle swarm optimization and particle filtering algorithm, and taking the fused information as the navigation information of the vehicle.
That is, when the vehicle can receive the signal of the global navigation satellite positioning system and the signal of the strapdown inertial navigation system, the output information of the global navigation satellite positioning system and the signal of the strapdown inertial navigation system are fused, and the fused information is used for vehicle navigation.
Hereinafter, the multi-sensor information fusion of the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by using the particle swarm optimization and the particle filter algorithm, and the detailed description will be given with the fused information as the navigation information of the vehicle.
Referring to fig. 2, in this embodiment, if the signal of the global navigation satellite positioning system is not lost, step S1 is first executed to establish an initial particle swarm for the strapdown inertial navigation system and the global navigation satellite positioning system, and determine particle swarm parameters of the initial particle swarm.
Specifically, in this embodiment, the particle swarm parameters of the initial particle swarm include an initial dimension of each particle, an initial mean of the particle swarm, and an initial distribution variance of the particle swarm.
In this embodiment, the particle group parameter of the initial particle group further includes the number of particles of the initial particle group. In order to meet the real-time requirement of the positioning system, the number of initial particle groups needs to be set as small as possible. In this embodiment, the number of the particles in the initial particle group is 80 to 120, specifically, 80, 90, 100, 110, 120, or other values within this range may be also used, which is not limited in this embodiment.
More specifically, referring to fig. 3, in the present embodiment, determining particle swarm parameters of an initial particle swarm includes the following steps:
s11: each particle of the initial population of particles is defined according to the following formula:
X＝(vn,ve,vd,lat,lon,h)
wherein X is the dimension of each particle; v. ofnVehicle speed in geographic north direction; v. ofeIs the speed of the vehicle in the geodetic direction; v. ofdVehicle speed in a ground direction perpendicular to the ground surface; lat is longitude; lon is latitude; h is the height.
S12: and acquiring the speed of the vehicle in the geographical north direction, the speed of the vehicle in the geographical east direction and the speed, longitude, latitude and altitude of the vehicle in the ground direction perpendicular to the ground surface at the previous moment by using an inertial navigation recursion algorithm, and taking the obtained speed, longitude, latitude and altitude as the initial average value of the particle swarm.
It should be noted that the inertial navigation recursive algorithm is specifically an inertial navigation attitude error dynamic calibration method based on a recursive least square method, which may specifically refer to the prior art, and this implementation is not described herein again.
S13: and acquiring the observation precision distribution variance of the global navigation satellite positioning system, and taking the observation precision distribution variance as the initial distribution variance of the particle swarm.
S14: and determining the specific numerical value of the dimension of each particle according to the initial mean value of the particle swarm and the initial distribution variance of the particle swarm.
The above is a process of defining an initial particle swarm to solve a specific numerical value of the dimension of each particle in the initial particle swarm.
After the initial particle group is established and the particle group parameters of the initial particle group are set, step S2 is executed: and updating the initial particle swarm according to the output information of the strapdown inertial navigation system to form an updated particle swarm, and acquiring the output information of the strapdown inertial navigation system corresponding to the updated particle swarm.
That is to say, in this step, the initial particle swarm needs to be updated according to the angular velocity of the strapdown inertial navigation system and the acceleration of the strapdown inertial navigation system to obtain an updated particle swarm, and then the angular velocity of the strapdown inertial navigation system and the acceleration of the strapdown inertial navigation system corresponding to the updated particle swarm are further obtained.
More specifically, referring to fig. 3, step S2 includes the steps of:
s21: and carrying out strapdown inertial navigation resolving on each particle in the initial particle swarm according to the angular velocity of the strapdown inertial navigation system at the current moment and the acceleration of the strapdown inertial navigation system so as to obtain the updated angular velocity and acceleration of the strapdown inertial navigation system.
In the step, strapdown inertial navigation calculation is carried out on each particle in the initial particle swarm, so as to obtain updated position and speed theoretical values.
S22: and determining the state noise of the filtering process according to the random walk error of the accelerometer of the strapdown inertial navigation system.
S23: and predicting the angular velocity and the acceleration of the strapdown inertial navigation system by using a Gaussian model according to the state noise of the filtering process and the updated angular velocity and acceleration of the strapdown inertial navigation system.
S24: and taking the angular velocity and the acceleration of the strapdown inertial navigation system obtained by prediction as output information of the strapdown inertial navigation system corresponding to the updated particle swarm.
This step is to superimpose the gaussian model on the updated position and velocity theoretical values to obtain the predicted true values.
After obtaining the predicted output information of the updated strapdown inertial navigation system, step S3 is executed: and driving and updating the particles in the particle swarm to move to the high-likelihood region by utilizing a particle swarm optimization algorithm.
In the present embodiment, the high-likelihood region is an iterative population optimal value of the update particle group and an iterative particle optimal value of each particle in the update particle group.
It should be explained that the optimal value of the iterative population is the optimal particle in the particle group. The specific determination method comprises the steps of determining the optimal particles of the initial particle swarm, then updating the initial particle swarm to obtain an updated particle swarm, determining the optimal particles of the updated particle swarm, comparing the optimal particles with the optimal particles of the initial particle swarm, and selecting the optimal particles of the updated particle swarm and the optimal particles of the initial particle swarm. In the subsequent updating process, selection is performed once every updating to obtain the optimal value of the iteration population. The optimal value of the iterative particle is the historical optimal value of each particle in each generation of particle swarm.
Specifically, in this embodiment, the particle swarm parameters of the initial particle swarm further include a preset iteration step number and an initial movement speed of each particle.
More specifically, referring to fig. 4, in the present embodiment, step S3 includes the following steps:
s31: and constructing a fitness function expression of the particle swarm optimization algorithm.
It should be noted that, in this embodiment, the observed quantity of the global navigation satellite positioning system at the current time is introduced into the solution of the fitness function. Embodied as the distance between each particle and the observed quantity. That is, if the distribution of the particle group tends to the true state, i.e., the true vehicle speed and the vehicle position, the fitness function value calculated for each particle is high, whereas the individual optimum value and the global optimum value of each particle in the particle group are both low.
It should be further noted that, in this embodiment, the fitness function expression of the particle swarm optimization algorithm is as follows:
wherein, the fitness is a fitness function expression; rtA measured noise covariance matrix for a global navigation satellite positioning system.
S32: and carrying out iterative processing on the initial particle swarm, and setting the initial motion speed of each particle to be one thousandth to ten thousandth of the distance between the initial value of each particle and the observed quantity of the global navigation satellite positioning system.
In this embodiment, when performing iterative processing on the initial particle swarm, a preset iteration step number needs to be set. In this embodiment, in order to meet the requirement of the real-time performance of the positioning system, a larger number of iteration steps cannot be set. Specifically, the preset iteration step number in this embodiment may be 3 to 5 generations, that is, may be 3 generations, 4 generations, or 5 generations.
S33: and calculating and recording the initial particle optimal value of each particle in each initial particle swarm and the initial swarm optimal value of each initial particle swarm according to a fitness function expression of the particle swarm optimization algorithm.
S34: and moving each particle in the updated particle swarm to the initial particle optimal value and the initial group optimal value according to the self-learning rate, the group learning rate, the speed and the speed inertia weight coefficient.
In this embodiment, each particle in the updated particle group is moved to the initial particle optimal value and the initial population optimal value according to the following formula:
Vk+1＝C0*Vk+C1*rand*(Yk-Xk)+C2*rand*(Pgbest-Xk)
Xk+1＝Xk+Vk+1
wherein the subscript k represents the current iteration step number; v is the velocity vector of the particle; x is the value of the current particle; y is the particle history optimal value of each particle; pgbestThe particles are corresponding to the historical optimal values of the particles; c0Is a velocity inertial weight coefficient; c1Is the self-learning rate of the particle; c2Is the population learning rate of the particle.
In an embodiment of the present invention, each particle in the updated set of particles is weighted according to the velocity inertia weight coefficient c00.5, self-learning rate c11.5, group learning rate c21.5, and a random rate between 0 and 1 is shifted towards the initial particle optimum and the initial population optimum.
S35: and calculating and recording the particle history optimal value of each particle in all the updating particle swarms and the particle corresponding to the particle history optimal value as well as the global group optimal value of all the updating particle swarms and the particle corresponding to the global group optimal value according to the fitness function expression of the particle swarms optimization algorithm.
S36: judging whether the current iteration step number is equal to a preset iteration step number or not;
if so, ending the iteration and outputting the optimized particle swarm;
if not, entering the next iteration.
It should be noted that, in this embodiment, after the iteration is completed, the optimized particle swarm is output, and the particle swarm distribution at this time tends to be a high-likelihood probability density distribution, thereby solving the problem of a small number of particles in the particle swarm. That is, with the above method, even if the number of particles of the initial particle group is set to be small and the number of iterations is small, the error of the result finally output by the system is not large.
The above process is a process of making the updated particle swarm approach a real posterior state. After that, step S4 is executed: and calculating and updating the particle weight of each particle in the particle swarm by using a particle filter algorithm, and normalizing the particle weight. This step is a process of performing filter processing on the particles.
Specifically, referring to fig. 5, in this embodiment, the step S4 specifically includes the following steps:
s41: calculating the optimized particle swarm by using Gaussian distribution; the mean value of the Gaussian distribution is an observed value of the global navigation satellite positioning system, and the variance of the Gaussian distribution is a measurement noise covariance of the global navigation satellite positioning system.
It should be noted that, the optimized particle swarm is calculated by using gaussian distribution, that is, a process of weight sampling is performed, the process of weight sampling is to determine the probability of occurrence of the observation value y when the particle is in the x state, and the specific implementation manner is to put the optimized particle swarm into the gaussian distribution with the observation value as the mean and the measurement noise as the variance
To perform the calculation.
S42: updating the particle weight of each particle in the particle swarm according to the calculation result of the S41; and updating the particle weight of each particle in the updated population of particles according to the following formula:
wherein the content of the first and second substances,
i is the ith particle; p is the total number of each particle in the particle swarm;
is a gaussian distribution.
Note that this step is a step of updating the weight of the particle group by using the latest observation value.
S43: normalizing the particle weight according to the updated particle weight; and, normalizing the particle weights according to the following formula:
wherein the content of the first and second substances,
is the particle weight; i is the ith particle.
In one embodiment of the present invention, the initial weights
After the filter processing is performed on the particles, step S5 is performed: resampling each particle in the updated population of particles to form a resampled population of particles.
Specifically, in this embodiment, step S5 specifically includes: and resampling each particle in the updated particle swarm by adopting a roulette method so as to enable the resampled particles to move towards an area with a posterior probability density distribution value larger than a preset density distribution value threshold.
That is, the particles are resampled in order to select the particles that are close to the true state with the highest probability.
In one embodiment of the present invention, the weight of each particle after resampling is
More specifically, referring to fig. 6, the particle swarm parameters of the initial particle swarm further include a preset filtering number, and the preset filtering number is at least one. When the preset filtering number is greater than one, the step S5 is followed by:
s5': the steps S4 and S5 are repeatedly performed, and it is determined whether the number of filtering times is equal to a preset number of filtering times.
When the number of filtering times is equal to the preset number of filtering times, the filtering is ended and step S6 is performed.
When the number of filtering times is not equal to the preset number of filtering times, the steps S4 and S5 are continuously and repeatedly performed.
Before repeatedly executing step S5', the method further includes establishing an initialization particle group. That is, the particle group initialization is performed once every time the particle filter is started.
And when the preset filtering times are more than equal and twice, updating the distribution variance of the particle swarm and determining according to the distribution variance of the corresponding particle swarm after the last filtering.
That is, in the first filtering, the average variance given by the global navigation satellite positioning system is taken as the distribution variance of the particle swarm, and in the Nth filtering, the variance is determined by the distribution variance of the particle swarm after the Nth-1 st filtering. In addition, at the first filtering, v is given0As the standard deviation of the distribution of the population of particles; v. of0Is the vehicle speed in the geographical north direction at the initial moment.
After the particle closest to the real state is selected, step S6 is performed: and determining output information of the strapdown inertial navigation system and output information of the global navigation satellite positioning system according to the dimensionality of the central particles of the resampling particle group, and bringing the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system into an updated attitude conversion matrix of the strapdown inertial navigation system to obtain an updated attitude of the central particles, wherein the updated attitude is used as navigation information of the vehicle.
In this embodiment, the particle swarm center formed after resampling is a fusion result of output information of the strapdown inertial navigation system and output information of the global navigation satellite positioning system, and six latitudes of the center particle respectively describe the position and speed information of the fused vehicle, update the attitude transformation matrix, and solve the updated attitude information.
In this embodiment, a final positioning result based on the multi-sensor fusion positioning method is shown in fig. 7, a track with more fluctuation is a track based on a global navigation satellite positioning system, and a track with more smoothness and no protrusion is a track based on a particle swarm optimization and a particle filter algorithm. It can be seen that the fused track based on the particle swarm optimization and the particle filtering algorithm is superior to the track based on the global navigation satellite positioning system, and the track based on the particle swarm optimization and the particle filtering algorithm approaches to the real track.
Referring to fig. 8a, 8b, 8c and 9a, 9b, 9c, when the output signal of the global navigation satellite positioning system is detected, the observation data of the global navigation satellite positioning system is fused with the strapdown inertial navigation system, and a track approaching to the true value is output.
Specifically, referring to fig. 8a, 8b, and 8c, when the sampling time t is 80s to 120s, the global navigation satellite positioning system has an intermittent failure, and when the output signal of the global navigation satellite positioning system is not detected, the positioning system relies on the strapdown inertial navigation system to perform state recursion, and after the output signal of the global navigation satellite positioning system is detected intermittently, information fusion continues to be performed, and the fusion trajectory does not have a large deviation, so that the positioning system is guaranteed to operate as usual, and the trajectory is smooth.
Further, referring to fig. 9a, 9b, and 9c, when the sampling time is 90s to 150s, the trajectory of the global navigation satellite positioning system deviates from the real trajectory obtained in advance, and the trajectories of the particle swarm optimization and the particle filtering algorithm are always stable and have no great difference from the real trajectory obtained in advance.
While the invention has been shown and described with reference to certain preferred embodiments thereof, it will be understood by those skilled in the art that the foregoing is a more detailed description of the invention, taken in conjunction with the specific embodiments thereof, and that no limitation of the invention is intended thereby. Various changes in form and detail, including simple deductions or substitutions, may be made by those skilled in the art without departing from the spirit and scope of the invention.
Claims (10)
1. A multi-sensor fusion positioning method of a vehicle navigation system comprises a strapdown inertial navigation system and a global navigation satellite positioning system; the method for fusing and positioning the multiple sensors of the vehicle navigation system is characterized by comprising the following steps of:
s0: judging whether the signal of the global navigation satellite positioning system is lost; wherein
If the signal of the global navigation satellite positioning system is lost, the output information of the strapdown inertial navigation system is used as the navigation information of the vehicle; wherein the navigation information of the vehicle comprises vehicle position information and vehicle speed information; or
And if the signal of the global navigation satellite positioning system is not lost, performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by utilizing a particle swarm optimization and particle filtering algorithm, and taking the fused information as navigation information of a vehicle.
2. The multi-sensor fusion positioning method for vehicle navigation system according to claim 1, wherein the performing multi-sensor information fusion on the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system by using the particle swarm optimization and particle filtering algorithm, and using the fused information as the navigation information of the vehicle comprises:
s1: establishing an initial particle swarm for the strapdown inertial navigation system and the global navigation satellite positioning system, and determining particle swarm parameters of the initial particle swarm;
s2: updating the initial particle swarm according to the output information of the strapdown inertial navigation system to form an updated particle swarm, and acquiring the output information of the strapdown inertial navigation system corresponding to the updated particle swarm;
s3: driving the particles in the updated particle swarm to move to a high-likelihood region by utilizing a particle swarm optimization algorithm; wherein the high likelihood region is an iterative population optimal value for the update particle population and an iterative particle optimal value for each particle in the update particle population;
s4: calculating the particle weight of each particle in the updated particle swarm by using a particle filtering algorithm, and performing normalization processing on the particle weight;
s5: resampling each particle in the update particle population to form a resampled particle population;
s6: and determining the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system according to the dimensionality of the central particles of the resampling particle group, and bringing the output information of the strapdown inertial navigation system and the output information of the global navigation satellite positioning system into the updated attitude conversion matrix of the strapdown inertial navigation system to obtain the updated attitude of the central particles, and taking the updated attitude as the navigation information of the vehicle.
3. The multi-sensor fusion positioning method for vehicle navigation system according to claim 2, wherein in step S1, the particle swarm parameters of the initial particle swarm comprise the initial dimension of each particle, the initial mean of the particle swarm, and the initial distribution variance of the particle swarm; and said determining particle population parameters for the initial particle population comprises:
s11: defining each particle of the initial population of particles according to the following formula:
X＝(vn,ve,vd,lat,lon,h)
wherein X is the dimension of each particle; v. ofnVehicle speed in the geographic north direction; v. ofeIs the speed of the vehicle in the geodetic direction; v. ofdVehicle speed in a ground direction perpendicular to the ground surface; lat is longitude; lon is latitude; h is the height;
s12: obtaining the vehicle speed in the north direction, the vehicle speed in the east direction, the vehicle speed in the ground direction, the longitude, the latitude and the height at the previous moment by using an inertial navigation recursion algorithm, and taking the obtained values as the initial average value of the particle swarm;
s13: acquiring the observation precision distribution variance of the global navigation satellite positioning system, and taking the observation precision distribution variance as the initial distribution variance of the particle swarm;
s14: and determining a specific numerical value of the dimension of each particle according to the initial mean value of the particle swarm and the initial distribution variance of the particle swarm.
4. The multi-sensor fusion positioning method of a vehicle navigation system according to claim 3, wherein the output information of the strapdown inertial navigation system includes an angular velocity of the strapdown inertial navigation system, an acceleration of the strapdown inertial navigation system; step S2 includes:
s21: performing strapdown inertial navigation calculation on each particle in the initial particle swarm according to the angular velocity of the strapdown inertial navigation system at the current moment and the acceleration of the strapdown inertial navigation system to obtain the updated angular velocity and acceleration of the strapdown inertial navigation system;
s22: determining the state noise of a filtering process according to the random walk error of the accelerometer of the strapdown inertial navigation system;
s23: predicting the angular velocity and the acceleration of the strapdown inertial navigation system by using a Gaussian model according to the filtering process state noise and the updated angular velocity and acceleration of the strapdown inertial navigation system;
s24: and taking the angular velocity and the acceleration of the strapdown inertial navigation system obtained by prediction as output information of the strapdown inertial navigation system corresponding to the updated particle swarm.
5. The multi-sensor fusion positioning method of vehicle navigation system according to claim 4, wherein the particle swarm parameters of the initial particle swarm further comprise a preset iteration step number, an initial motion speed of each particle; and the number of the first and second electrodes,
step S3 includes:
s31: constructing a fitness function expression of the particle swarm optimization algorithm;
s32: performing iterative processing on the initial particle swarm, and setting the initial movement speed of each particle to be one thousandth to ten thousandth of the distance between the initial value of each particle and the observed quantity of the global navigation satellite positioning system;
s33: calculating and recording an initial particle optimal value of each particle in each initial particle swarm and an initial swarm optimal value of each initial particle swarm according to a fitness function expression of the particle swarm optimization algorithm;
s34: enabling each particle in the updated particle swarm to move towards the initial particle optimal value and the initial swarm optimal value according to a self-learning rate, a swarm learning rate, a speed and a speed inertia weight coefficient;
s35: calculating and recording the particle history optimal value of each particle in all the updated particle swarms, the particle corresponding to the particle history optimal value, the global group optimal value of all the updated particle swarms and the particle corresponding to the global group optimal value according to the fitness function expression of the particle swarms optimization algorithm;
s36: judging whether the current iteration step number is equal to the preset iteration step number or not;
if so, ending the iteration and outputting the optimized particle swarm;
if not, entering the next iteration.
6. The multi-sensor fusion positioning method of the vehicle navigation system according to claim 5, wherein the fitness function expression of the particle swarm optimization algorithm is as follows:
wherein, the fitness is a fitness function expression; rtA measured noise covariance matrix for the global navigation satellite positioning system; and is
Moving each particle in the updated population of particles to the initial particle optimum and initial population optimum according to the following formula:
Vk+1＝C0*Vk+C1*rand*(Yk-Xk)+C2*rand*(Pgbest-Xk)
Xk+1＝Xk+Vk+1
wherein the subscript k represents the current iteration step number; v is the velocity vector of the particle; x is the value of the current particle; y is the particle history optimal value of each particle; pgbestThe particle is the particle corresponding to the historical optimal value of the particle; c0Is a velocity inertial weight coefficient; c1Is the self-learning rate of the particle; c2Is the population learning rate of the particle.
7. The multi-sensor fusion positioning method of vehicle navigation system according to claim 6, wherein the step S4 includes:
s41: calculating the optimized particle swarm by using Gaussian distribution; the mean value of the Gaussian distribution is an observed value of the global navigation satellite positioning system, and the variance of the Gaussian distribution is a measured noise covariance of the global navigation satellite positioning system;
s42: updating the particle weight of each particle in the updated particle swarm according to the calculation result of S41; and updating the particle weight of each particle in the updated population of particles according to the following formula:
wherein the content of the first and second substances,
i is the ith particle; p is the total number of each particle in the particle swarm;
is a Gaussian distribution;
s43: carrying out normalization processing on the particle weight according to the updated particle weight; and, normalizing the particle weights according to the following formula:
wherein the content of the first and second substances,
is the particle weight; i is the ith particle.
8. The multi-sensor fusion positioning method of vehicle navigation system according to claim 7, wherein the step S5 includes:
and resampling each particle in the updated particle swarm by adopting a roulette method so as to enable the resampled particles to move towards an area with a posterior probability density distribution value larger than a preset density distribution value threshold.
9. The multi-sensor fusion positioning method of vehicle navigation system according to claim 8, wherein the particle swarm parameters of the initial particle swarm further comprise a preset filtering number, and the preset filtering number is at least one;
when the preset filtering number is greater than one, step S5 is followed by:
s5': repeatedly executing the step S4 and the step S5, and determining whether the filtering times are equal to the preset filtering times;
when the filtering times are equal to the preset filtering times, ending the filtering and executing the step S6;
when the filtering times are not equal to the preset filtering times, continuously and repeatedly executing the steps S4 and S5; and the number of the first and second electrodes,
before the step S5' is repeatedly executed, establishing an initialization particle swarm;
and when the preset filtering times are more than equal and twice, determining the distribution variance of the updated particle swarm according to the distribution variance of the corresponding particle swarm after the last filtering.
10. The multi-sensor fusion localization method of a vehicle navigation system of claim 9, wherein the particle population parameters of the initial particle population further comprise a number of particles of the initial particle population; and the number of the first and second electrodes,
the number of particles of the initial population of particles is from 80 to 120.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011330616.7A CN114543793B (en) | 2020-11-24 | 2020-11-24 | Multi-sensor fusion positioning method of vehicle navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011330616.7A CN114543793B (en) | 2020-11-24 | 2020-11-24 | Multi-sensor fusion positioning method of vehicle navigation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114543793A true CN114543793A (en) | 2022-05-27 |
| CN114543793B CN114543793B (en) | 2024-02-09 |
Family
ID=81659967
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011330616.7A Active CN114543793B (en) | 2020-11-24 | 2020-11-24 | Multi-sensor fusion positioning method of vehicle navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114543793B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102156478A (en) * | 2010-12-28 | 2011-08-17 | 北京航空航天大学 | Integrated attitude determination method based on ant colony unscented particle filter algorithm |
| CN106932802A (en) * | 2017-03-17 | 2017-07-07 | 安科智慧城市技术（中国）有限公司 | A kind of air navigation aid and system based on spreading kalman particle filter |
| CN107402012A (en) * | 2016-05-20 | 2017-11-28 | 北京自动化控制设备研究所 | A kind of Combinated navigation method of vehicle |
| CN109084777A (en) * | 2018-09-13 | 2018-12-25 | 黎建军 | Particle filtering and map constructing method based on self-adapted genetic algorithm |
| CN109443354A (en) * | 2018-12-25 | 2019-03-08 | 中北大学 | Vision-inertia close coupling Combinated navigation method based on firefly group's optimization PF |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| US20200182995A1 (en) * | 2015-07-17 | 2020-06-11 | Origin Wireless, Inc. | Method, apparatus, and system for outdoor target tracking |
-
2020
- 2020-11-24 CN CN202011330616.7A patent/CN114543793B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102156478A (en) * | 2010-12-28 | 2011-08-17 | 北京航空航天大学 | Integrated attitude determination method based on ant colony unscented particle filter algorithm |
| US20200182995A1 (en) * | 2015-07-17 | 2020-06-11 | Origin Wireless, Inc. | Method, apparatus, and system for outdoor target tracking |
| CN107402012A (en) * | 2016-05-20 | 2017-11-28 | 北京自动化控制设备研究所 | A kind of Combinated navigation method of vehicle |
| CN106932802A (en) * | 2017-03-17 | 2017-07-07 | 安科智慧城市技术（中国）有限公司 | A kind of air navigation aid and system based on spreading kalman particle filter |
| CN109084777A (en) * | 2018-09-13 | 2018-12-25 | 黎建军 | Particle filtering and map constructing method based on self-adapted genetic algorithm |
| CN109443354A (en) * | 2018-12-25 | 2019-03-08 | 中北大学 | Vision-inertia close coupling Combinated navigation method based on firefly group's optimization PF |
| CN110780326A (en) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | Vehicle-mounted integrated navigation system and positioning method |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
Non-Patent Citations (1)
| Title |
|---|
| 宫轶松, 粒子滤波算法研究及其在GPS/DR组合导航中的应用, no. 2011, pages 79 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114543793B (en) | 2024-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106840179B (en) | Intelligent vehicle positioning method based on multi-sensor information fusion | |
| CN105509738B (en) | Vehicle positioning orientation method based on inertial navigation/Doppler radar combination | |
| CN113029137B (en) | Multi-source information self-adaptive fusion positioning method and system | |
| CN104848867B (en) | The pilotless automobile Combinated navigation method of view-based access control model screening | |
| US9551561B2 (en) | Determining location using magnetic fields from AC power lines | |
| CN104121905B (en) | Course angle obtaining method based on inertial sensor | |
| CN111272165A (en) | Intelligent vehicle positioning method based on characteristic point calibration | |
| CN101846734B (en) | Agricultural machinery navigation and position method and system and agricultural machinery industrial personal computer | |
| CN104061899B (en) | A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation | |
| CN108362288B (en) | Polarized light SLAM method based on unscented Kalman filtering | |
| CN107132563B (en) | Combined navigation method combining odometer and dual-antenna differential GNSS | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN109855617A (en) | A kind of vehicle positioning method, vehicle locating device and terminal device | |
| Pfaff et al. | Towards mapping of cities | |
| CN106643715A (en) | Indoor inertial navigation method based on bp neural network improvement | |
| CN112284376A (en) | Mobile robot indoor positioning mapping method based on multi-sensor fusion | |
| CN108426582B (en) | Indoor three-dimensional map matching method for pedestrians | |
| CN102589552A (en) | Data fusion method and device for low-cost integrated navigation system | |
| CN113252033A (en) | Positioning method, positioning system and robot based on multi-sensor fusion | |
| Zheng et al. | An optimization-based UWB-IMU fusion framework for UGV | |
| CN113375664B (en) | Autonomous mobile device positioning method based on dynamic loading of point cloud map | |
| CN112325878A (en) | Ground carrier combined navigation method based on UKF and air unmanned aerial vehicle node assistance | |
| CN114543793B (en) | Multi-sensor fusion positioning method of vehicle navigation system | |
| CN110864688A (en) | Attitude heading method for vehicle-mounted azimuth open-loop horizontal attitude angle closed loop |
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 |