CN109946731B - Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering - Google Patents

Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering Download PDF

Info

Publication number
CN109946731B
CN109946731B CN201910168995.5A CN201910168995A CN109946731B CN 109946731 B CN109946731 B CN 109946731B CN 201910168995 A CN201910168995 A CN 201910168995A CN 109946731 B CN109946731 B CN 109946731B
Authority
CN
China
Prior art keywords
vehicle
uwb
observation
distance
positioning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910168995.5A
Other languages
Chinese (zh)
Other versions
CN109946731A (en
Inventor
李旭
高怀堃
胡玮明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201910168995.5A priority Critical patent/CN109946731B/en
Publication of CN109946731A publication Critical patent/CN109946731A/en
Application granted granted Critical
Publication of CN109946731B publication Critical patent/CN109946731B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

The invention discloses a vehicle high-reliability fusion positioning method facing a typical urban environment, aiming at the problems that the use of vehicle-mounted satellite navigation is limited easily, the positioning accuracy and the reliability are not high and the like under the urban environment, an Ultra-wide band (UWB) positioning technology is introduced on the basis of the traditional vehicle-mounted satellite and inertial integrated navigation, and the UWB observation precision is classified by utilizing a fuzzy algorithm, so that the observation noise variance matrix of the UWB is adjusted in an adaptive manner, and on the basis, the fusion positioning of a vehicle is realized based on an unscented Kalman filtering algorithm. Compared with the traditional vehicle-mounted satellite and inertial integrated navigation, the method provided by the invention has higher reliability in urban environments, especially in complex environments (such as urban canyons, intersections and the like) in which satellite signals are seriously shielded, and is beneficial to realizing continuous, complete, reliable and real-time positioning of vehicles.

Description

Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering
Technical Field
The invention relates to the field of vehicle navigation positioning, in particular to a high-reliability fusion positioning method for vehicles facing to an urban environment.
Background
With the development and progress of economic society, the quantity of motor vehicles in China is rapidly increased, road traffic faces huge challenges, and in order to solve the increasingly serious problem of urban traffic, Intelligent Vehicle Infrastructure Cooperative Systems (IVICS) are brought forward and gradually become the latest development direction of the research of Intelligent Traffic Systems (ITS). No matter the application of vehicle-road cooperation or the realization of intelligent transportation, the high-precision vehicle positioning technology cannot be used: only under the premise of realizing accurate and reliable vehicle positioning, vehicle-vehicle and vehicle-road dynamic real-time information interaction is implemented in all directions, vehicle active safety control and road cooperative management are developed on the basis of full-time dynamic traffic information acquisition and fusion, effective cooperation of human and vehicle roads is fully realized, vehicles can be effectively commanded and dispatched, urban traffic is improved, and safe driving of the vehicles is guaranteed. Therefore, the vehicle positioning technology is one of the basic and core contents of research on vehicle-road cooperation and even intelligent transportation.
The current common vehicle navigation positioning technology comprises: dead Reckoning (DR), Inertial Navigation System (INS), Satellite Navigation (GNSS), and the like. Because of the respective defects of the single positioning technology, in order to realize accurate, reliable, continuous and complete positioning under urban environment, two or more positioning technologies are combined, wherein the application of GNSS/INS fusion positioning is the most extensive: the GNSS/INS fusion positioning can solve the positioning problem under the condition of short-time interruption of satellite signals, compensates accumulated errors of the INS to a certain extent, and can meet the vehicle positioning requirement under a relatively open environment, but in the environment (such as urban canyons, tunnels and the like) with long-time interruption of the satellite signals, if the GNSS cannot work normally, the GNSS/INS fusion positioning only depends on the autonomous calculation of the INS, and still can cause larger positioning errors.
In recent years, the rise and rapid development of Ultra-Wideband (UWB) -based wireless positioning technology provides a new idea for realizing reliable positioning of vehicles in GNSS-limited environments, and at present, UWB positioning technology is mainly used in the field of indoor positioning such as personnel, intelligent vehicles, robot positioning, and the like. Because the UWB has the technical advantages of extremely wide bandwidth, strong pulse signal penetrating power, good multi-path resolution capability and the like, the realization of vehicle positioning under outdoor environment based on the UWB also has certain feasibility, if a relatively reliable UWB information source is introduced into a sensor layer, the robustness of a fusion positioning system can be effectively improved, but under a complex urban environment, a UWB signal is easily interfered by surrounding vehicles, trees at two sides of a road and buildings to generate multi-path and non-line-of-sight errors, if UWB observed quantity with poor signal quality is used for fusion positioning, the fusion precision is liable to be reduced, therefore, how to realize dynamic interaction of human-vehicle information under the cooperation of the vehicle and the road aiming at relatively complex traffic scenes (such as crossroads, roundabout intersections and the like) by combining road traffic facilities and reasonably distributing UWB nodes, and further selecting a proper method, the self-adaptive adjustment of the dependence degree of the fusion algorithm on UWB observation information with different qualities is a key problem to be solved urgently for realizing the fusion positioning of vehicles based on UWB.
Disclosure of Invention
The technical problem is as follows: aiming at the problems that vehicle-mounted satellite navigation is easy to use and limited, and the positioning accuracy and reliability are not high and the like in an urban environment, an Ultra-Wideband (UWB) positioning technology is introduced on the basis of the traditional vehicle-mounted satellite and inertial integrated navigation, and UWB observation precision is classified by using a fuzzy algorithm, so that the observation noise variance matrix of the UWB is adaptively adjusted, and on the basis, the fusion positioning of a vehicle is realized based on an unscented Kalman filtering algorithm.
The technical scheme is as follows: in order to achieve the purpose, the invention adopts the following technical scheme: firstly, arranging UWB fixed nodes at two sides of a road, and acquiring position coordinates of the UWB fixed nodes through a high-precision differential GPS; secondly, arranging a UWB mobile node at the top of the vehicle, and obtaining the distance from the UWB mobile node to each UWB fixed node by adopting a time difference of arrival method; then, acquiring rough position coordinates of the vehicle based on a ranging positioning principle; then, according to the distance and azimuth angle characteristics of the UWB signals, classifying the UWB observation precision based on a fuzzy logic classification algorithm; further adaptively adjusting the UWB observation noise array according to the fuzzy classification result; and finally, realizing the fusion positioning of the vehicle based on unscented Kalman filtering to obtain the accurate position of the vehicle.
The idea of the present invention is further explained below with reference to the attached drawings:
the process of the present invention is shown in FIG. 1.
A vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering is characterized in that in an urban environment, UWB positioning is introduced on the basis of vehicle-mounted satellite and inertial integrated navigation, and UWB observation precision classification is carried out by combining a fuzzy algorithm, so that an observation noise variance matrix of the UWB is self-adaptively adjusted, and on the basis, the vehicle high-reliability fusion positioning is realized based on the unscented Kalman filtering algorithm, and the method comprises the following steps:
step 1) arranging UWB fixed nodes at two sides of a road, and measuring position coordinates of the UWB fixed nodes;
step 2) arranging a UWB mobile node at the top of the vehicle, obtaining the distance from the UWB mobile node to each UWB fixed node by adopting a Time Difference of Arrival (TDOA), and performing data preprocessing on a distance observation value;
step 3) establishing a coordinate equation based on a ranging and positioning principle according to the preprocessed distance observation value, calculating the coordinate of the UWB mobile node by adopting a least square method, and acquiring the rough position coordinate of the vehicle;
step 4) measuring the current course angle of the vehicle through the vehicle-mounted electronic compass, and calculating the azimuth angle of each UWB fixed node relative to the current running direction of the vehicle according to the rough position coordinate of the vehicle and the position coordinate of the UWB fixed node;
Step 5) classifying the observation precision of each UWB fixed node by adopting a fuzzy logic classification algorithm according to two characteristics of distance and azimuth angle, wherein the classification is divided into five grades of A, B, C, D and E, the precision of the distance observation value corresponding to the UWB is from high to low, and the fuzzy classification algorithm specifically comprises the following steps:
51) determining three membership functions of the distance characteristics, and classifying the distances, wherein the three membership functions are respectively N, M and R and represent 'near', 'middle' and 'far' of the distances;
52) determining two membership functions of azimuth features, classifying azimuth angles, namely L and S respectively, representing large and small of the azimuth angle;
53) determining 5 precision levels A, B, C, D and E of fuzzy logic classification output according to fuzzy classification rules as follows:
1. when the distance is N and the azimuth angle is L, the precision grade is A;
2. when the distance is N and the azimuth angle is S, the precision grade is B;
3. when the distance is M and the azimuth angle is L, the precision grade is C;
4. when the distance is M and the azimuth angle is S, the precision grade is D;
5. when the distance is R and the azimuth angle is L, the precision grade is E;
6. when the distance is R and the azimuth angle is S, the precision grade is E;
and 6) combining observation information output by the vehicle-mounted GNSS, the gyroscope and the wheel speed sensor, and realizing accurate positioning of the vehicle by using an unscented Kalman filtering algorithm, wherein the method specifically comprises the following steps:
61) Firstly, establishing an unscented Kalman filtering state model of a vehicle positioning system
The system state vector is
Figure BDA0001987299470000041
Wherein the content of the first and second substances,
Figure BDA0001987299470000042
the vehicle latitude information is obtained, the lambda is the vehicle longitude information, and the h is the vehicle height information; v. ofeEast-direction speed, v, of the vehiclenIs the north speed, v, of the vehicleuIs the vertical velocity of the vehicle; p is the pitch angle of the vehicle, r is the roll angle of the vehicle, and A is the course angle of the vehicle; sfodError of wheel speed accuracy factor, bωFor the gyroscope to drift randomly, bGNSSFor GNSS receiver clock error, dGNSSFor GNSS receiver clock drift rate, bUWBOffset error for UWB;
the external input vector of the system is U ═ vod aod fx fyωz]
Wherein v isodVehicle speed measured for wheel speed sensor, aodIs the longitudinal acceleration of the vehicle, fxAs a measure of the longitudinal acceleration of the vehicle, fyAs a measure of the lateral acceleration of the vehicle, ωzYaw rate output for the gyroscope;
the system state equation is:
Figure BDA0001987299470000051
wherein X (k) is a system state vector at discrete time k, X (k-1), U (k-1), W (k-1) and T (k-1) are respectively a system state vector, an external input vector and a system noise vector at discrete time (k-1), andinputting a noise vector; rMIs the radius of the earth meridian, RNThe radius of the earth-unitary fourth of twelve earthly branches is shown, and delta t is a sampling interval; gamma ray wError, σ, of wheel speed sensor with respect to timewIs its corresponding noise variance; beta is a betazFor random drift of the gyroscope, sigmazIts corresponding noise variance;
62) then, an unscented Kalman filtering observation model of the vehicle positioning system is established
The system observation vector is
Figure BDA0001987299470000052
Wherein the content of the first and second substances,
Figure BDA0001987299470000053
pseudorange observations for m GNSS satellites respectively,
Figure BDA0001987299470000054
distance observations for n UWB fixed nodes;
the system observation equation is as follows:
Z(k)＝H(k)·X(k)+V(k)
wherein, z (k), h (k) are system observation vectors and system observation matrices at discrete time k, and v (k) are observation noise vectors at discrete time k;
the observation noise variance matrix corresponding to the observation noise vector v (k) is r (k):
R(k)＝diag[RGNSS RUWB]
wherein R isGNSSIs an observed noise variance matrix, R, of the GNSSUWBAn observed noise variance matrix for UWB;
63) classifying the observation precision of the n UWB fixed nodes according to the fuzzy classification algorithm in the step 5), and further adaptively adjusting the noise variance array R of the UWB according to the classification resultUWBAnd obtaining a final observation noise variance matrix R (k) by the following specific method:
1. calculating observed noise for the ith UWB fixed node
Figure BDA0001987299470000061
Wherein R is0A noise variance basis for the UWB distance observation; mu is an adjusting coefficient determined by the fuzzy classification result, and the adjusting coefficients corresponding to the five different precision levels A, B, C, D and E are respectively mu a，μb，μc，μd，μe；
2. Calculating an observed noise variance matrix for UWB
Figure BDA0001987299470000062
3. Updating an observation noise variance matrix R (k), R (k) diag [ R [ ]GNSS RUWB]；
64) And carrying out the time updating and measurement updating processes of unscented Kalman filtering according to the system state model in the step 61), the observation model in the step 62) and the noise variance matrix subjected to the self-adaptive adjustment in the step 63) to obtain accurate position information of the vehicle.
1. Sigma point set xi for constructing unscented Kalman filteringi(k-1) and determining a weight coefficient thereof
Figure BDA0001987299470000063
Figure BDA0001987299470000064
Figure BDA0001987299470000065
Figure BDA0001987299470000066
Wherein eta is a scale parameter, the approximation precision can be improved by adjusting eta,
Figure BDA0001987299470000067
α1determining the distribution of sigma points, and setting the distribution to a smaller positive number(1e-4≤α1＜1)，α3A second scale parameter, set to 0, α2For the state distribution parameter, for the Gaussian distribution, setting the state distribution parameter as 2, wherein n is the dimensionality of the system state vector;
2. performing unscented Kalman filtering time updating process, and calculating one-step predicted state quantity by the following equation
Figure BDA0001987299470000071
And a one-step prediction error covariance matrix P (k, k-1):
ξi(k,k-1)＝f(ξi(k-1))i＝0,1,...,2n
Figure BDA0001987299470000072
Figure BDA0001987299470000073
ζi(k,k-1)＝h(ξi(k,k-1))
Figure BDA0001987299470000074
3. updating the observation noise variance matrix R (k) according to the method of step 63);
4. the unscented kalman filter gain matrix k (k) is calculated by the following equation:
Figure BDA0001987299470000075
Figure BDA0001987299470000076
K(k)＝PXZ·PZZ -1
5. performing unscented Kalman Filter measurement update procedure, calculating the shape by the following equation State estimation
Figure BDA0001987299470000077
And the estimation error covariance matrix p (k):
Figure BDA0001987299470000078
P(k)＝P(k,k-1)-K(k)PZZK(k)T
the unscented Kalman filtering state estimation
Figure BDA0001987299470000079
I.e. the estimated value of the vehicle state vector at discrete time k, from which accurate position information of the vehicle can be determined.
The invention has the beneficial effects that:
1. the method solves the problem that the UWB positioning technology is applied to the field of vehicle navigation in the actual traffic environment to realize the high-reliability fusion positioning of the urban vehicles;
2. according to the invention, the UWB observation precision is classified by using a fuzzy algorithm, so that the observation noise variance matrix of the UWB is adaptively adjusted, the influence of low-precision UWB observation quantity caused by non-line-of-sight or multipath on the system fusion precision can be effectively eliminated, and the accuracy and reliability of fusion positioning can be improved;
3. the invention realizes the fusion positioning of the vehicle based on unscented Kalman filtering, and overcomes the defects of low precision, poor stability, slow response to target maneuvering and the like of the traditional nonlinear filtering method such as extended Kalman filtering.
Drawings
FIG. 1 is a flow chart of the present invention.
Fig. 2 is a schematic diagram of a UWB node layout in a typical urban crossroad environment.
FIG. 3 is a graph of membership functions for a fuzzy logic classification algorithm.
Fig. 4 is a comparison diagram of the vehicle localization trajectory before and after the fuzzy adaptive algorithm is applied.
FIG. 5 is a comparison graph of vehicle positioning errors before and after the fuzzy adaptive algorithm is used.
Detailed Description
The present application will be described in further detail with reference to the following drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the relevant invention and not restrictive of the invention.
With the development and progress of economic society, the quantity of motor vehicles in China is rapidly increased, and urban traffic faces increasingly severe problems, so that an Intelligent Transport System (ITS) is produced and gradually becomes a focus of research. The implementation of ITS and the research in the field of active safety of vehicles do not leave the technology of vehicle positioning: only on the premise of realizing accurate and reliable positioning, the dispatching vehicle can be effectively commanded, the urban traffic is improved, and the safe driving of the vehicle is ensured. Therefore, vehicle positioning technology is one of the basic and core contents of ITS and active vehicle safety research.
The current common vehicle navigation positioning technology comprises: dead Reckoning (DR), Inertial Navigation System (INS), Satellite Navigation (GNSS), and the like. Because of the respective defects of the single positioning technology, in order to realize accurate, reliable, continuous and complete positioning under urban environment, two or more positioning technologies are combined, wherein the application of GNSS/INS fusion positioning is the most extensive: the GNSS/INS fusion positioning can solve the positioning problem under the condition of short-time interruption of satellite signals, compensates accumulated errors of the INS to a certain extent, and can meet the vehicle positioning requirement under a relatively open environment, but in the environment (such as urban canyons, tunnels and the like) with long-time interruption of the satellite signals, if the GNSS cannot work normally, the GNSS/INS fusion positioning only depends on the autonomous calculation of the INS, and still can cause larger positioning errors.
In recent years, the rise and rapid development of UWB-based wireless positioning technology provides a new idea for realizing reliable positioning of vehicles in GNSS-limited environments, and at present, UWB positioning technology is mainly used in indoor positioning fields such as personnel, smart cars, robot positioning, and the like. Because the UWB has the technical advantages of extremely wide bandwidth, strong pulse signal penetrating power, good multi-path resolution capability and the like, the realization of vehicle positioning under outdoor environment based on the UWB also has certain feasibility, if a relatively reliable UWB information source is introduced into a sensor layer, the robustness of a fusion positioning system can be effectively improved, but under a complex urban environment, a UWB signal is easily interfered by surrounding vehicles, trees at two sides of a road and buildings to generate multi-path and non-line-of-sight errors, if UWB observed quantity with poor signal quality is used for fusion positioning, the fusion precision is liable to be reduced, therefore, how to realize dynamic interaction of human-vehicle information under the cooperation of the vehicle and the road aiming at relatively complex traffic scenes (such as crossroads, roundabout intersections and the like) by combining road traffic facilities and reasonably distributing UWB nodes, and further selecting a proper method, the self-adaptive adjustment of the dependence degree of a fusion algorithm on UWB observation information with different qualities is a key problem to be solved urgently in realizing the fusion positioning of vehicles based on UWB.
Aiming at the problems that vehicle-mounted satellite navigation is easy to use and limited, and the positioning accuracy and reliability are not high and the like in an urban environment, an Ultra-Wideband (UWB) positioning technology is introduced on the basis of the traditional vehicle-mounted satellite and inertial integrated navigation, and UWB observation precision is classified by using a fuzzy algorithm, so that the observation noise variance matrix of the UWB is adaptively adjusted, and on the basis, the fusion positioning of a vehicle is realized based on an unscented Kalman filtering algorithm.
Referring to fig. 1, it shows a flow of implementing an embodiment of a vehicle high-precision positioning method according to the method of the present invention:
firstly, arranging UWB fixed nodes at two sides of a road, and acquiring position coordinates of the UWB fixed nodes through a high-precision differential GPS; secondly, arranging a UWB mobile node at the top of the vehicle, and obtaining the distance from the UWB mobile node to each UWB fixed node by adopting a time difference of arrival method; then, acquiring rough position coordinates of the vehicle based on a distance measurement positioning principle; then, according to the distance and azimuth angle characteristics of the UWB signals, classifying the UWB observation precision based on a fuzzy logic classification algorithm; further adaptively adjusting the UWB observation noise array according to the fuzzy classification result; and finally, realizing the fusion positioning of the vehicle based on unscented Kalman filtering to obtain the accurate position of the vehicle.
The idea of the invention is further explained below with reference to the drawings:
the process of the present invention is shown in FIG. 1.
A vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering is characterized in that in an urban environment, UWB positioning is introduced on the basis of vehicle-mounted satellite and inertial integrated navigation, and UWB observation precision classification is carried out by combining a fuzzy algorithm, so that an observation noise variance matrix of the UWB is self-adaptively adjusted, and on the basis, the vehicle high-reliability fusion positioning is realized based on the unscented Kalman filtering algorithm, and the method comprises the following steps:
step 1) arranging UWB fixed nodes at two sides of a road, and measuring position coordinates of the UWB fixed nodes;
referring to fig. 2, the present invention provides a layout scheme for a typical urban intersection area, in which the number and the installation position of UWB fixed nodes are determined according to an actual traffic scene, and the cost and the positioning accuracy requirements are comprehensively considered to determine a reasonable layout scheme: the layout mode can cover the whole intersection to the maximum extent, complete positioning of vehicles in the intersection area is realized, and the UWB nodes arranged in the roundabout center are not easy to be shielded and interfered, thereby being beneficial to improving the positioning precision;
Step 2) arranging a UWB mobile node at the top of the vehicle, obtaining the distance from the UWB mobile node to each UWB fixed node by adopting a Time Difference of Arrival (TDOA), and performing data preprocessing on the distance observation value;
step 3) establishing a coordinate equation based on a ranging and positioning principle according to the preprocessed distance observation value, calculating the coordinate of the UWB mobile node by adopting a least square method, and acquiring the rough position coordinate of the vehicle;
step 4) measuring the current course angle of the vehicle through the vehicle-mounted electronic compass, and calculating the azimuth angle of each UWB fixed node relative to the current running direction of the vehicle according to the rough position coordinate of the vehicle and the position coordinate of the UWB fixed node;
step 5) classifying the observation precision of each UWB fixed node by adopting a fuzzy logic classification algorithm according to two characteristics of distance and azimuth angle, wherein the classification is divided into five grades of A, B, C, D and E, the precision of the distance observation value corresponding to the UWB is from high to low, and the fuzzy classification algorithm specifically comprises the following steps:
51) determining three membership functions of the distance characteristics, and classifying the distances, wherein the three membership functions are respectively N, M and R and represent 'near', 'middle' and 'far' of the distances;
52) determining two membership functions of azimuth features, classifying azimuth angles, namely L and S respectively, representing large and small of the azimuth angle;
53) And determining 5 precision levels A, B, C, D and E output by fuzzy logic classification according to a fuzzy classification rule as follows:
1. when the distance is N and the azimuth angle is L, the precision grade is A;
2. when the distance is N and the azimuth angle is S, the precision grade is B;
3. when the distance is M and the azimuth angle is L, the precision grade is C;
4. when the distance is M and the azimuth angle is S, the precision grade is D;
5. when the distance is R and the azimuth angle is L, the precision grade is E;
6. when the distance is R and the azimuth angle is S, the precision grade is E;
in this embodiment, please refer to fig. 3 for the membership function of the fuzzy classification algorithm;
and 6) combining observation information output by the vehicle-mounted GNSS, the gyroscope and the wheel speed sensor, and realizing accurate positioning of the vehicle by using an unscented Kalman filtering algorithm, wherein the method specifically comprises the following steps:
61) firstly, establishing an unscented Kalman filtering state model of a vehicle positioning system
The system state vector is
Figure BDA0001987299470000111
Wherein the content of the first and second substances,
Figure BDA0001987299470000112
the vehicle latitude information is obtained, lambda is the vehicle longitude information, and h is the vehicle height information; v. ofeEast-direction speed, v, of the vehiclenIs the north speed, v, of the vehicleuIs the vertical velocity of the vehicle; p is the pitch angle of the vehicle, r is the roll angle of the vehicle, and A is the course angle of the vehicle; sf odError of wheel speed accuracy factor, bωFor the gyroscope to drift randomly, bGNSSFor GNSS receiver clock error, dGNSSFor GNSS receiver clock drift rate, bUWBOffset error for UWB;
the external input vector of the system is U ═ vod aod fx fyωz]
Wherein v isodVehicle speed measured for wheel speed sensor, aodIs the longitudinal acceleration of the vehicle, fxAs a measure of the longitudinal acceleration of the vehicle, fyAs a measure of the lateral acceleration of the vehicle, ωzYaw rate output for the gyroscope;
the system state equation is:
Figure BDA0001987299470000121
wherein, X (k) is a system state vector at a discrete time k, X (k-1), U (k-1), W (k-1), and T (k-1) are respectively a system state vector, an external input vector, a system noise vector, and an external input noise vector at the discrete time (k-1); rMIs the radius of the earth meridian, RNThe radius of the earth-unitary fourth of twelve earthly branches is shown, and delta t is a sampling interval; gamma raywError, σ, of wheel speed sensor with respect to timewIts corresponding noise variance; beta is azFor random drift of the gyroscope, σzIts corresponding noise variance;
62) then, an unscented Kalman filtering observation model of the vehicle positioning system is established
The system observation vector is
Figure BDA0001987299470000122
Wherein the content of the first and second substances,
Figure BDA0001987299470000123
pseudorange observations for m GNSS satellites respectively,
Figure BDA0001987299470000124
distance observations for n UWB fixed nodes;
The system observation equation is:
Z(k)＝H(k)·X(k)+V(k)
wherein, z (k), h (k) are system observation vectors and system observation matrices at discrete time k, and v (k) are observation noise vectors at discrete time k;
the observation noise variance matrix corresponding to the observation noise vector v (k) is r (k):
R(k)＝diag[RGNSS RUWB]
wherein R isGNSSIs an observed noise variance matrix, R, of the GNSSUWBAn observed noise variance matrix for UWB;
63) classifying the observation precision of the n UWB fixed nodes according to the fuzzy classification algorithm in the step 5), and further adaptively adjusting the noise variance array R of the UWB according to the classification resultUWBAnd obtaining a final observation noise variance matrix R (k) by the following specific method:
1. calculating observed noise for the ith UWB fixed node
Figure BDA0001987299470000131
Wherein R is0A noise variance basis for the UWB distance observation; mu is an adjusting coefficient determined by the fuzzy classification result, and the adjusting coefficients corresponding to the five different precision levels A, B, C, D and E are respectively mua，μb，μc，μd，μe；
2. Calculating observed noise floor for UWBDifferential matrix
Figure BDA0001987299470000132
3. Updating an observation noise variance matrix R (k), R (k) diag [ R [ ]GNSS RUWB]；
64) And carrying out the time updating and measurement updating processes of unscented Kalman filtering according to the system state model in the step 61), the observation model in the step 62) and the noise variance matrix subjected to the self-adaptive adjustment in the step 63) to obtain accurate position information of the vehicle.
1. Sigma point set xi for constructing unscented Kalman filteringi(k-1) and determining a weight coefficient thereof
Figure BDA0001987299470000133
Figure BDA0001987299470000134
Figure BDA0001987299470000135
Figure BDA0001987299470000136
Wherein eta is a scale parameter, the approximation precision can be improved by adjusting eta,
Figure BDA0001987299470000141
α1the distribution of sigma points is determined and set to a small positive number (1 e)-4≤α1＜1)，α3A second scale parameter, set to 0, α2For the state distribution parameter, for the Gaussian distribution, setting the state distribution parameter as 2, wherein n is the dimensionality of the system state vector;
2. performing unscented Kalman filtering time update process, and calculating one-step predicted state quantity by the following equation
Figure BDA0001987299470000142
And a one-step prediction error covariance matrix P (k, k-1):
ξi(k,k-1)＝f(ξi(k-1))i＝0,1,...,2n
Figure BDA0001987299470000143
Figure BDA0001987299470000144
ζi(k,k-1)＝h(ξi(k,k-1))
Figure BDA0001987299470000145
3. updating the observation noise variance matrix R (k) according to the method of step 63);
4. the unscented kalman filter gain matrix k (k) is calculated by the following equation:
Figure BDA0001987299470000146
Figure BDA0001987299470000147
K(k)＝PXZ·PZZ -1
5. performing unscented Kalman Filter measurement update procedure, calculating state estimation by the following equation
Figure BDA0001987299470000148
And the estimation error covariance matrix p (k):
Figure BDA0001987299470000149
P(k)＝P(k,k-1)-K(k)PZZK(k)T
the unscented Kalman filtering state estimation
Figure BDA00019872994700001410
I.e. the estimated value of the vehicle state vector at discrete time k, from which accurate position information of the vehicle can be determined.
In the embodiment, in order to check the beneficial effect of the proposed vehicle fusion positioning method based on the fuzzy adaptive unscented kalman filter on the improvement of the positioning precision, a real vehicle test is carried out, and a comparison test is carried out on the positioning results of the vehicle before and after the fuzzy adaptive algorithm is adopted. In addition, it should be noted that: in the experiment, the method before the fuzzy algorithm processing is adopted refers to the traditional unscented Kalman filtering fusion method, and the method after the fuzzy algorithm processing is adopted refers to the method in the invention.
Fig. 4 is a comparison graph of a set of test results, and it is obvious from the graph that the track of the vehicle positioning is closer to the true value after the fuzzy adaptive algorithm is adopted. FIG. 5 is a comparison graph of vehicle positioning errors before and after a group of data adopts a fuzzy classification algorithm in the embodiment, and the positioning errors in the graph can be obviously seen.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (1)

1. A vehicle high-reliability fusion positioning method based on fuzzy adaptive unscented Kalman filtering is characterized by comprising the following steps:
step 1) arranging UWB fixed nodes at two sides of a road, and measuring position coordinates of the UWB fixed nodes;
step 2) arranging a UWB mobile node at the top of the vehicle, obtaining the distance from the UWB mobile node to each UWB fixed node by adopting a time difference of arrival method, and performing data preprocessing on a distance observation value;
Step 3) establishing a coordinate equation based on a ranging and positioning principle according to the preprocessed distance observation value, calculating the coordinate of the UWB mobile node by adopting a least square method, and acquiring the rough position coordinate of the vehicle;
step 4) measuring the current course angle of the vehicle through the vehicle-mounted electronic compass, and calculating the azimuth angle of each UWB fixed node relative to the current running direction of the vehicle according to the rough position coordinate of the vehicle and the position coordinate of the UWB fixed node;
step 5) classifying the observation precision of each UWB fixed node by adopting a fuzzy logic classification algorithm according to two characteristics of distance and azimuth angle, wherein the classification is divided into five grades of A, B, C, D and E, the precision of the distance observation value corresponding to the UWB is from high to low, and the fuzzy classification algorithm specifically comprises the following steps:
51) determining three membership functions of the distance characteristics, and classifying the distances, wherein the three membership functions are respectively N, M and R and represent 'near', 'middle' and 'far' of the distances;
52) determining two membership functions of azimuth features, classifying azimuth angles, namely L and S respectively, representing large and small of the azimuth angle;
53) determining 5 precision levels A, B, C, D and E of fuzzy logic classification output according to fuzzy classification rules as follows:
1. When the distance is N and the azimuth angle is L, the precision grade is A;
2. when the distance is N and the azimuth angle is S, the precision grade is B;
3. when the distance is M and the azimuth angle is L, the precision grade is C;
4. when the distance is M and the azimuth angle is S, the precision grade is D;
5. when the distance is R and the azimuth angle is L, the precision grade is E;
6. when the distance is R and the azimuth angle is S, the precision grade is E;
and 6) combining observation information output by the vehicle-mounted GNSS, the gyroscope and the wheel speed sensor, and realizing accurate positioning of the vehicle by using an unscented Kalman filtering algorithm, wherein the method specifically comprises the following steps:
61) firstly, establishing an unscented Kalman filtering state model of a vehicle positioning system
The system state vector is
Figure FDA0003576166570000021
Wherein the content of the first and second substances,
Figure FDA0003576166570000022
the vehicle latitude information is obtained, the lambda is the vehicle longitude information, and the h is the vehicle height information; v. ofeEast-direction speed, v, of the vehiclenIs the north speed, v, of the vehicleuIs the vertical speed of the vehicle; p is the pitch angle of the vehicle, r is the roll angle of the vehicle, and A is the course angle of the vehicle; sfodError of wheel speed accuracy factor, bωFor the gyroscope to drift randomly, bGNSSFor GNSS receiver clock error, dGNSSFor GNSS receiver clock drift rate, bUWBOffset error for UWB;
the external input vector of the system is U ═ v od aod fx fy ωz]
Wherein v isodVehicle speed measured for wheel speed sensor, aodIs the longitudinal acceleration of the vehicle, fxAs a measure of the longitudinal acceleration of the vehicle, fyAs a measure of the lateral acceleration of the vehicle, ωzYaw rate output for the gyroscope;
the system state equation is:
Figure FDA0003576166570000023
wherein X (k) is a system state vector at discrete time k, X (k-1), U (k-1), W (k-1), and T (k-1) are respectively a system state vector, an external input vector, and system noise at discrete time (k-1)Vector, external input noise vector; rMIs the radius of the earth meridian, RNThe radius of the earth-unitary fourth of twelve earthly branches is shown, and delta t is a sampling interval; gamma raywError, σ, of wheel speed sensor with respect to timewIts corresponding noise variance; beta is azFor random drift of the gyroscope, σzIts corresponding noise variance;
62) then, an unscented Kalman filtering observation model of the vehicle positioning system is established
The system observation vector is
Figure FDA0003576166570000031
Wherein the content of the first and second substances,
Figure FDA0003576166570000032
pseudorange observations for m GNSS satellites respectively,
Figure FDA0003576166570000033
distance observations for n UWB fixed nodes;
the system observation equation is as follows:
Z(k)＝H(k)·X(k)+V(k)
wherein, z (k), h (k) are system observation vectors and system observation matrices at discrete time k, and v (k) are observation noise vectors at discrete time k;
the observation noise variance matrix corresponding to the observation noise vector v (k) is r (k):
R(k)＝diag[RGNSS RUWB]
Wherein R isGNSSArray of observed noise variances, R, for GNSSUWBAn observed noise variance matrix for UWB;
63) classifying the observation precision of the n UWB fixed nodes according to the fuzzy classification algorithm in the step 5), and further adaptively adjusting the noise variance array R of the UWB according to the classification resultUWBAnd obtaining a final observation noise variance matrix R (k) by the following specific method:
1. calculating observed noise for the ith UWB fixed node
Figure FDA0003576166570000034
Figure FDA0003576166570000035
Wherein R is0A noise variance basis for the UWB distance observation; mu is an adjusting coefficient determined by the fuzzy classification result, and the adjusting coefficients corresponding to the five different precision levels A, B, C, D and E are respectively mua，μb，μc，μd，μe；
2. Calculating an observed noise variance matrix R for UWBUWB:
Figure FDA0003576166570000036
3. Updating an observation noise variance matrix R (k), R (k) diag [ R [ ]GNSS RUWB]；
64) According to the system state model in the step 61), the observation model in the step 62) and the noise variance matrix adaptively adjusted in the step 63), performing the time updating and measurement updating process of unscented kalman filtering to obtain the accurate position information of the vehicle, and specifically, the method comprises the following steps:
1. sigma point set xi for constructing unscented Kalman filteringi(k-1) and determining a weight coefficient thereof
Figure FDA0003576166570000041
Figure FDA0003576166570000042
Figure FDA0003576166570000043
Figure FDA0003576166570000044
Wherein eta is a scale parameter, the approximation precision can be improved by adjusting eta,
Figure FDA0003576166570000045
α1The distribution of sigma points is determined and set to 1e-4≤α1＜1，α3A second scale parameter, set to 0, α2For the state distribution parameter, for the Gaussian distribution, setting the state distribution parameter as 2, wherein n is the dimensionality of the system state vector;
2. performing unscented Kalman filtering time updating process, and calculating one-step predicted state quantity by the following equation
Figure FDA0003576166570000046
And a one-step prediction error covariance matrix P (k, k-1):
ξi(k,k-1)＝f(ξi(k-1))i＝0,1,...,2n
Figure FDA0003576166570000047
Figure FDA0003576166570000048
ζi(k,k-1)＝h(ξi(k,k-1))
Figure FDA0003576166570000049
3. updating the observation noise variance matrix R (k) according to the method of step 63);
4. the unscented kalman filter gain matrix k (k) is calculated by the following equation:
Figure FDA00035761665700000410
Figure FDA00035761665700000411
K(k)＝PXZ·PZZ -1
5. performing unscented Kalman Filter measurement update procedure, calculating state estimation by the following equation
Figure FDA0003576166570000051
And the estimation error covariance matrix p (k):
Figure FDA0003576166570000052
P(k)＝P(k,k-1)-K(k)PZZK(k)T
the unscented Kalman filtering state estimation
Figure FDA0003576166570000053
I.e. the estimated value of the vehicle state vector at discrete time k, from which accurate position information of the vehicle can be determined.
CN201910168995.5A 2019-03-06 2019-03-06 Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering Active CN109946731B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910168995.5A CN109946731B (en) 2019-03-06 2019-03-06 Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910168995.5A CN109946731B (en) 2019-03-06 2019-03-06 Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering

Publications (2)

Publication Number Publication Date
CN109946731A CN109946731A (en) 2019-06-28
CN109946731B true CN109946731B (en) 2022-06-10

Family

ID=67009302

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910168995.5A Active CN109946731B (en) 2019-03-06 2019-03-06 Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering

Country Status (1)

Country Link
CN (1) CN109946731B (en)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110296706A (en) * 2019-07-11 2019-10-01 北京云迹科技有限公司 Localization method and device, robot for Indoor Robot
US11131776B2 (en) * 2019-08-19 2021-09-28 Gm Global Technology Operations, Llc Method and apparatus for Kalman filter parameter selection using map data
CN112577513A (en) * 2019-09-27 2021-03-30 北京初速度科技有限公司 State quantity error determination method and vehicle-mounted terminal
CN112577512A (en) * 2019-09-27 2021-03-30 北京初速度科技有限公司 State quantity error determination method based on wheel speed fusion and vehicle-mounted terminal
CN110940999A (en) * 2019-12-13 2020-03-31 合肥工业大学 Self-adaptive unscented Kalman filtering method based on error model
CN111551976A (en) * 2020-05-20 2020-08-18 四川万网鑫成信息科技有限公司 Method for automatically completing abnormal positioning by combining various data
CN111811503B (en) * 2020-07-15 2022-02-18 桂林电子科技大学 Unscented Kalman filtering fusion positioning method based on ultra wide band and two-dimensional code
CN111915921A (en) * 2020-08-11 2020-11-10 大陆投资(中国)有限公司 Lane-level precision positioning method using roadside equipment
CN111954153B (en) * 2020-08-12 2022-11-29 南京工程学院 Intelligent taxi calling navigation positioning method based on UWB positioning
US11718317B2 (en) 2020-10-16 2023-08-08 Ford Global Technologies, Llc Vehicle location correction using roadside devices
CN112533149B (en) * 2020-11-27 2022-06-07 桂林理工大学 Moving target positioning algorithm based on UWB mobile node
CN113029137B (en) * 2021-04-01 2023-02-28 清华大学 Multi-source information self-adaptive fusion positioning method and system
CN114440876B (en) * 2022-01-21 2024-04-02 北京自动化控制设备研究所 Positioning and guiding method and system for underground heading machine
CN114779307B (en) * 2022-06-17 2022-09-13 武汉理工大学 Port area-oriented UWB/INS/GNSS seamless positioning method
CN117148406B (en) * 2023-10-30 2024-01-30 山东大学 Indoor and outdoor seamless elastic fusion positioning method, system, medium and equipment

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102928816A (en) * 2012-11-07 2013-02-13 东南大学 High-reliably integrated positioning method for vehicles in tunnel environment
CN103323005A (en) * 2013-03-06 2013-09-25 郭雷 Multi-objective optimization anti-interference filtering method for SINS/GPS/polarized light combined navigation system
CN104154913A (en) * 2014-06-28 2014-11-19 南京诺导电子科技有限公司 Autonomous indoor positioning algorithm in all-attitude environment
CN105807301A (en) * 2016-03-03 2016-07-27 东南大学 Enhanced digital map based vehicle optimization oriented satellite selection positioning method
CN108363978A (en) * 2018-02-12 2018-08-03 华南理工大学 Using the emotion perception method based on body language of deep learning and UKF
CN108445480A (en) * 2018-02-02 2018-08-24 重庆邮电大学 Mobile platform based on laser radar adaptively extends Target Tracking System and method

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102928816A (en) * 2012-11-07 2013-02-13 东南大学 High-reliably integrated positioning method for vehicles in tunnel environment
CN103323005A (en) * 2013-03-06 2013-09-25 郭雷 Multi-objective optimization anti-interference filtering method for SINS/GPS/polarized light combined navigation system
CN104154913A (en) * 2014-06-28 2014-11-19 南京诺导电子科技有限公司 Autonomous indoor positioning algorithm in all-attitude environment
CN105807301A (en) * 2016-03-03 2016-07-27 东南大学 Enhanced digital map based vehicle optimization oriented satellite selection positioning method
CN108445480A (en) * 2018-02-02 2018-08-24 重庆邮电大学 Mobile platform based on laser radar adaptively extends Target Tracking System and method
CN108363978A (en) * 2018-02-12 2018-08-03 华南理工大学 Using the emotion perception method based on body language of deep learning and UKF

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于车内总线的车辆双模卫星融合定位技术研究;王宇;《中国优秀硕士学位论文全文数据库工程科技II辑》;20170315;第44-54页 *

Also Published As

Publication number Publication date
CN109946731A (en) 2019-06-28

Similar Documents

Publication Publication Date Title
CN109946731B (en) Vehicle high-reliability fusion positioning method based on fuzzy self-adaptive unscented Kalman filtering
CN109946730B (en) Ultra-wideband-based high-reliability fusion positioning method for vehicles under cooperation of vehicle and road
CN106840179B (en) Intelligent vehicle positioning method based on multi-sensor information fusion
CN104076382B (en) A kind of vehicle seamless positioning method based on Multi-source Information Fusion
CN105509738B (en) Vehicle positioning orientation method based on inertial navigation/Doppler radar combination
CN107247275B (en) Urban GNSS vulnerability monitoring system and method based on bus
CN102928816A (en) High-reliably integrated positioning method for vehicles in tunnel environment
CN107274721B (en) Multi-vehicle cooperative positioning method in intelligent transportation system
CN112923931A (en) Feature map matching and GPS positioning information fusion method based on fixed route
Gao et al. Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system
Heirich et al. Probabilistic localization method for trains
CN109946648B (en) Ultra-wideband-based high-precision vehicle positioning method under cooperation of vehicle and road
Onyekpe et al. Learning uncertainties in wheel odometry for vehicular localisation in GNSS deprived environments
Gao et al. An integrated land vehicle navigation system based on context awareness
Dawson et al. Radar-based multisensor fusion for uninterrupted reliable positioning in GNSS-denied environments
Iqbal et al. Experimental results on an integrated GPS and multisensor system for land vehicle positioning
CN112462401B (en) Urban canyon rapid detection method and device based on floating vehicle track data
Hsu et al. Intelligent viaduct recognition and driving altitude determination using GPS data
Sirikonda et al. Integration of low-cost IMU with MEMS and NAVIC/IRNSS receiver for land vehicle navigation
Mandel et al. Particle filter-based position estimation in road networks using digital elevation models
CN116917967A (en) Synchronous localization and mapping using road surface data
EP4220079A1 (en) Radar altimeter inertial vertical loop
Wilfinger et al. Indoor position determination using location fingerprinting and vehicle sensor data
CN112985428B (en) Safety angle-based priority reference method for image layer of high-precision sensing map
Petovello et al. Development and testing of a real-time GPS/INS reference system for autonomous automobile navigation

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