CN109946731A - A kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering - Google Patents
A kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering Download PDFInfo
- Publication number
- CN109946731A CN109946731A CN201910168995.5A CN201910168995A CN109946731A CN 109946731 A CN109946731 A CN 109946731A CN 201910168995 A CN201910168995 A CN 201910168995A CN 109946731 A CN109946731 A CN 109946731A
- Authority
- CN
- China
- Prior art keywords
- uwb
- vehicle
- observation
- kalman filtering
- distance
- 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
Landscapes
- Navigation (AREA)
Abstract
The invention discloses a kind of highly reliable fusion and positioning methods of the vehicle towards typical urban environment, for under urban environment, Vehicular satellite navigation is easily using limited, the problems such as positioning accuracy and not high reliability, on the basis of traditional Vehicular satellite and inertia combined navigation, introduce ultra wide band (Ultra-Wideband, UWB) location technology, and classified using fuzzy algorithmic approach to UWB accuracy of observation, to the observation noise variance matrix of automatic adjusument UWB, herein on basis, the fusion positioning of vehicle is realized based on Unscented kalman filtering algorithm.Compared to traditional Vehicular satellite and inertia combined navigation, method in the present invention, under the complex environment (such as urban canyons, intersection) that urban environment, especially satellite-signal are seriously blocked, reliability is higher, helps to realize continuous, complete, reliable, the real-time positioning of vehicle.
Description
Technical field
The present invention relates to automobile navigation positioning field, in particular to the highly reliable fusion of a kind of vehicle towards urban environment is fixed
Position method.
Background technique
With the development and progress of economic society, the vehicle guaranteeding organic quantity in China is quicklyd increase, and road traffic is faced with huge
Big challenge, in order to solve increasingly serious urban transport problems, intelligent bus or train route cooperative system (Intelligent Vehicle
Infrastructure Cooperative Systems, IVICS) it comes into being, and gradually become intelligent transportation system
The latest development direction of (Intelligent Transport Systems, ITS) research.The either application of bus or train route collaboration, also
It is the realization of intelligent transportation, all be unable to do without high-precision vehicle positioning technology: only in the premise for realizing accurate reliable vehicle positioning
Under, comprehensive implementation vehicle vehicle, bus or train route dynamic realtime information exchange, and in full space-time dynamic traffic information collection and the basis merged
Upper development vehicle active safety control and road coordinated management are sufficiently realized effective collaboration of people's bus or train route, can effectively be referred to
It waves and dispatches buses, improve urban transportation, guarantee vehicle safe driving.Therefore, vehicle positioning technology is bus or train route collaboration or even intelligence
One of the basis of the researchs such as traffic and core content.
Automobile navigation location technology common at present includes: dead reckoning (Dead Reckoning, DR), inertial navigation system
It unites (Inertial Navigation System, INS), satellite navigation (Global Navigation Satellite
System, GNSS) etc..Since the presence of single location technology is respectively insufficient, in order to realize under urban environment it is accurate, reliable,
Continuously, it completely positions, mostly uses two kinds and two or more location technologies combines, wherein GNSS/INS fusion positioning is answered
With the most extensively: GNSS/INS fusion positioning can solve the orientation problem in the case of satellite-signal short interruptions, and certain
The accumulated error that INS is compensated in degree can satisfy the vehicle location demand under opposite free environments, but when satellite-signal is long
Between interrupt environment (such as urban canyons, tunnel) in, if GNSS can not work normally, only rely on INS and independently calculate, can still lead
Cause biggish position error.
In recent years, based on ultra wide band (Ultra-Wideband, UWB) wireless location technology rise and rapid development be
Realize that reliable location of the vehicle under GNSS constrained environment provides new approaches, currently, UWB location technology is primarily used to room
Interior positioning field such as personnel, intelligent carriage, robot localization etc..Since the very bandwidth of UWB is wide, pulse signal penetration power is strong, more
The well equal technological merits of diameter resolution capability, realize that the vehicle location under outdoor environment also has certain feasibility based on UWB,
In sensor layer, if introducing relatively reliable UWB information source, can effectively improve fusion positioning system robustness, but
Under complicated urban environment, vehicle, the trees of road both sides and the interference of building that UWB signal is travelled vulnerable to surrounding and go out
Existing multipath and non-market value certainly will lead to fusion essence if being used to merge positioning for the poor UWB observed quantity of signal quality
Therefore how the decline of degree is directed to relative complex traffic scene (such as right-angled intersection, rotary island crossing), in conjunction with road
Means of transportation, rational deployment trackside UWB node are realized people's bus or train route information dynamic interaction under bus or train route collaboration, and then are chosen properly
Method, automatic adjusument blending algorithm is to realize the vehicle based on UWB to the degree of dependence of the UWB observation information of different quality
Fusion positions critical issue urgently to be resolved.
Summary of the invention
Technical problem: under urban environment, easily using being limited, positioning accuracy and reliability be not high for Vehicular satellite navigation
The problems such as, on the basis of traditional Vehicular satellite and inertia combined navigation, introduce ultra wide band (Ultra-Wideband,
UWB) location technology, and classified using fuzzy algorithmic approach to UWB accuracy of observation, thus the observation noise variance of automatic adjusument UWB
Herein on basis, the fusion positioning of vehicle is realized based on Unscented kalman filtering algorithm for battle array.
Technical solution: to achieve the above object, the present invention adopts the following technical scheme: firstly, arranging UWB in road both sides
Stationary nodes, and pass through the position coordinates of high-precision difference GPS acquisition UWB stationary nodes；Secondly, arranging one in vehicle roof
UWB mobile node, using reach time difference method, obtain the UWB mobile node to each UWB stationary nodes distance；Then,
The rough position coordinate of vehicle is obtained based on ranging localization principle；Then, according to the distance and bearing corner characteristics of UWB signal, base
Classify in accuracy of observation of the fuzzy logic classifier algorithm to UWB；And then it is made an uproar according to fuzzy classification result automatic adjusument UWB observation
Acoustic matrix；Finally, realize that the fusion positioning of vehicle obtains the exact position of vehicle based on Unscented kalman filtering.
With reference to the accompanying drawing, thinking of the invention is further described:
Process of the invention is as shown in Figure 1.
A kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering, which is characterized in that
In urban environment, on the basis of Vehicular satellite and inertia combined navigation, UWB positioning is introduced, and combine fuzzy algorithmic approach to UWB
Accuracy of observation classification, so that the observation noise variance matrix of automatic adjusument UWB, herein on basis, is filtered based on Unscented kalman
Wave algorithm realizes the highly reliable fusion positioning of vehicle, and described method includes following steps:
Step 1) arranges UWB stationary nodes in road both sides, and measures the position coordinates of UWB stationary nodes；
Step 2) arranges a UWB mobile node in vehicle roof, using arrival time difference method (Time Difference
Of Arrival, TDOA) the UWB mobile node is obtained to the distance of each UWB stationary nodes, and observation of adjusting the distance is counted
Data preprocess；
Step 3), apart from observation, establishes equation in coordinates based on ranging localization principle, using minimum according to pretreated
Square law calculates the coordinate of UWB mobile node, obtains the rough position coordinate of vehicle；
Step 4) measures the current course angle of vehicle by vehicle electronics compass, and according to the rough position coordinate of vehicle with
And the position coordinates of UWB stationary nodes, calculate azimuth of each UWB stationary nodes relative to vehicle current driving direction；
Step 5) is according to two, distance and bearing angle feature, using fuzzy logic classifier algorithm to each UWB stationary nodes
Accuracy of observation classify, one is divided into A, B, C, five grades of D, E, the precision apart from observation of corresponding UWB by height to
Low, specific step is as follows for fuzzy Classified Algorithms Applied:
51) three subordinating degree functions of distance feature, and adjust the distance classification, respectively N, M and R are determined, distance is represented
" close ", " in ", " remote "；
52) two subordinating degree functions of orientation corner characteristics are determined, and azimuthal is classified, respectively L and S represent orientation
" big " and " small " at angle；
53) according to Fuzzy classification rule, 5 accuracy classes A, B, C, D, E of fuzzy logic classifier output are determined, are obscured
Classifying rules is as follows:
1. when distance is N, when azimuth is L, accuracy class A；
2. when distance is N, when azimuth is S, accuracy class B；
3. when distance is M, when azimuth is L, accuracy class C；
4. when distance is M, when azimuth is S, accuracy class D；
5. when distance is R, when azimuth is L, accuracy class E；
6. when distance is R, when azimuth is S, accuracy class E；
Step 6) combines the observation information of vehicle-mounted GNSS, gyroscope and wheel speed sensors output, utilizes Unscented kalman
The accurate positioning of filtering algorithm realization vehicle, the specific steps are as follows:
61) firstly, establishing the Unscented kalman filtering state model of vehicle positioning system
System mode vector is
Wherein,For vehicle latitude information, λ is vehicle longitude information, and h is height of car information；veFor the east orientation speed of vehicle
Degree, vnFor the north orientation speed of vehicle, vuFor the vertical velocity of vehicle；P is the pitch angle of vehicle, and r is the angle of heel of vehicle, and A is vehicle
Course angle；sfodFor wheel speed dilution of precision error, bωFor Gyroscope Random Drift, bGNSSFor GNSS receiver clock deviation, dGNSS
For GNSS receiver clock deviation drift rate, bUWBFor the offset error of UWB；
Exterior input vector is U=[vod aod fx fyωz]
Wherein, vodFor the car speed that wheel speed sensors measure, aodFor longitudinal acceleration of the vehicle, fxFor longitudinal direction of car acceleration
Spend measured value, fyFor vehicle lateral acceleration measured value, ωzFor the yaw velocity of gyroscope output；
System state equation are as follows:
Wherein, X (k) is the system mode vector of discrete instants k, and X (k-1), U (k-1), W (k-1), T (k-1) are respectively
The system mode vector of discrete instants (k-1), external input vector, system noise vector, external input noise vector；RMFor ground
Ball meridian radius, RNFor earth prime vertical radius, Δ t is the sampling interval；γwFor the error of wheel speed sensors and time correlation,
σwFor its corresponding noise variance；βzFor Gyroscope Random Drift, σzFor its corresponding noise variance；
62) the Unscented kalman filtering observation model of vehicle positioning system then, is established
Systematic observation vector is
Wherein,The pseudo range observed quantity of respectively m GNSS satellite,It is n UWB stationary nodes apart from observation；
Systematic observation equation are as follows:
Z (k)=H (k) X (k)+V (k)
Wherein, Z (k), H (k) are the systematic observation vector and systematic observation matrix of discrete instants k, when V (k) is discrete
Carve the observation noise vector of k；
The corresponding observation noise variance matrix of observation noise vector V (k) is R (k):
R (k)=diag [RGNSS RUWB]
Wherein, RGNSSFor the observation noise variance matrix of GNSS, RUWBFor the observation noise variance matrix of UWB；
63) fuzzy Classified Algorithms Applied according to step 5) classifies to the accuracy of observation of n UWB stationary nodes,
And then according to the noise variance matrix R of classification results automatic adjusument UWBUWB, final observation noise variance matrix R (k) is obtained, is had
Body method is as follows:
1. calculating the observation noise of i-th of UWB stationary nodes
Wherein, R0Noise variance basic value for UWB apart from observation；μ is adjustment factor, true by the result of fuzzy classification
Fixed, five kinds of different corresponding adjustment factors of accuracy class A, B, C, D, E are respectively μa, μb, μc, μd, μe；
2. calculating the observation noise variance matrix of UWB
3. updating observation noise variance matrix R (k): R (k)=diag [RGNSS RUWB]；
64) System State Model according to step 61), observation model described in step 62), and through step 63)
Noise variance matrix after automatic adjusument, the time for carrying out Unscented kalman filtering updates and measurement updaue process, obtains vehicle
Accurate location information.
1. constructing the sigma point set ξ of Unscented kalman filteringi(k-1), and its weight coefficient is determined
Wherein, η is scale parameter, and approximation accuracy can be improved by adjusting η,α1It determines
The distribution of sigma point is set to a lesser positive number (1e-4≤α1< 1), α3Second scale parameter, is arranged
It is 0, α2The dimension that 2, n is system mode vector is set to for Gaussian Profile for state distribution parameter；
2. carrying out Unscented kalman filtering time renewal process, one-step prediction quantity of state is calculated by following equationsWith one-step prediction error covariance matrix P (k, k-1):
ξi(k, k-1)=f (ξi(k-1)) i=0,1 ..., 2n
ζi(k, k-1)=h (ξi(k,k-1))
3. updating observation noise variance matrix R (k) according to step 63) the method；
4. calculating Unscented kalman filtering gain matrix K (k) by following equations:
K (k)=PXZ·PZZ -1
5. carrying out Unscented kalman filtering measurement updaue process, by following equations, state estimation is calculatedAnd estimation
Error covariance matrix P (k):
P (k)=P (k, k-1)-K (k) PZZK(k)T
The Unscented kalman filtering state estimationThe vehicle-state vector estimated value of as discrete instants k, thus
It can determine the precise position information of vehicle.
The invention has the benefit that
1. the method in the present invention solves under actual traffic environment, UWB location technology is led applied to automobile navigation
The highly reliable fusion orientation problem of city vehicle is realized in domain；
2. being classified using fuzzy algorithmic approach to UWB accuracy of observation in the present invention, and then the observation noise of automatic adjusument UWB
Variance matrix can effectively exclude the low precision UWB observed quantity as caused by non line of sight or multipath to the shadow of system globe area precision
It rings, helps to improve the accuracy and reliability of fusion positioning；
3. realizing the fusion positioning of vehicle in the present invention based on Unscented kalman filtering, traditional nonlinear filtering is overcome
For example Extended Kalman filter precision is not high, stability is poor for the method for wave, to target maneuver delay of response the disadvantages of.
Detailed description of the invention
Fig. 1 is flow chart of the invention.
Fig. 2 is the UWB node layout schematic diagram under the right-angled intersection environment of typical urban.
Fig. 3 is the membership function figure of fuzzy logic classifier algorithm.
Fig. 4 is the vehicle location track comparison diagram before and after the algorithm using fuzzy self-adaption.
Fig. 5 is the vehicle location error comparison diagram before and after the algorithm using fuzzy self-adaption.
Specific embodiment
The application is described in further detail with reference to the accompanying drawings and examples.It is understood that this place is retouched
The specific embodiment stated is only used for explaining related invention, rather than the restriction to the invention.
With the development and progress of economic society, the vehicle guaranteeding organic quantity in China is quicklyd increase, and urban transportation is faced with day
Beneficial severe problem, therefore intelligent transportation system (Intelligent Transport System, ITS) is come into being and gradually
The hot spot focused as research.It is fixed all to be unable to do without vehicle for the either realization of ITS or the research in vehicle active safety field
Position technology: only under the premise of realizing accurate, reliable location, can effectively command scheduling vehicle, improve urban transportation,
Guarantee vehicle safe driving.Therefore, vehicle positioning technology is in the basis and core of the researchs such as ITS and vehicle active safety
One of hold.
Automobile navigation location technology common at present includes: dead reckoning (Dead Reckoning, DR), inertial navigation system
It unites (Inertial Navigation System, INS), satellite navigation (Global Navigation Satellite
System, GNSS) etc..Since the presence of single location technology is respectively insufficient, in order to realize under urban environment it is accurate, reliable,
Continuously, it completely positions, mostly uses two kinds and two or more location technologies combines, wherein GNSS/INS fusion positioning is answered
With the most extensively: GNSS/INS fusion positioning can solve the orientation problem in the case of satellite-signal short interruptions, and certain
The accumulated error that INS is compensated in degree can satisfy the vehicle location demand under opposite free environments, but when satellite-signal is long
Between interrupt environment (such as urban canyons, tunnel) in, if GNSS can not work normally, only rely on INS and independently calculate, can still lead
Cause biggish position error.
In recent years, the rise and rapid development of the wireless location technology based on UWB are realization vehicle in GNSS constrained environment
Under reliable location provide new approaches, currently, UWB location technology be primarily used to indoor positioning field such as personnel, intelligence it is small
Vehicle, robot localization etc..The technologies such as since the very bandwidth of UWB is wide, pulse signal penetration power is strong, and multi-path resolved ability is good are excellent
Point realizes that the vehicle location under outdoor environment also has certain feasibility based on UWB, in sensor layer, if introducing opposite
Reliable UWB information source, then can effectively improve the robustness of fusion positioning system, but under complicated urban environment, UWB signal
There is multipath and non-market value vulnerable to vehicle, the trees of road both sides and the interference of building that surrounding travels,
If being used to merge positioning for the poor UWB observed quantity of signal quality, the decline of fusion accuracy certainly will be caused, therefore, how to be directed to
Relative complex traffic scene (such as right-angled intersection, rotary island crossing), in conjunction with road traffic facility, rational deployment trackside
UWB node realizes people's bus or train route information dynamic interaction under bus or train route collaboration, and then chooses suitable method, and automatic adjusument is merged
Algorithm is to realize that the Vehicle Fusion based on UWB positions pass urgently to be resolved to the degree of dependence of the UWB observation information of different quality
Key problem.
For under urban environment, Vehicular satellite navigation is easily using limited, the problems such as positioning accuracy and not high reliability,
On the basis of traditional Vehicular satellite and inertia combined navigation, ultra wide band (Ultra-Wideband, UWB) positioning skill is introduced
Art, and classified using fuzzy algorithmic approach to UWB accuracy of observation, thus the observation noise variance matrix of automatic adjusument UWB, in this base
On plinth, the fusion positioning of vehicle is realized based on Unscented kalman filtering algorithm.
Referring to FIG. 1, it illustrates the methods according to the present invention to realize that one of vehicle high-precision localization method is implemented
The process of example:
Firstly, arranging UWB stationary nodes in road both sides, and UWB stationary nodes are obtained by high-precision difference GPS
Position coordinates；Secondly, arranging a UWB mobile node in vehicle roof, using time difference method is reached, the UWB movable joint is obtained
Point arrives the distance of each UWB stationary nodes；Then, the rough position coordinate of vehicle is obtained based on ranging localization principle；Then, root
According to the distance and bearing corner characteristics of UWB signal, classified based on accuracy of observation of the fuzzy logic classifier algorithm to UWB；And then basis
Fuzzy classification result automatic adjusument UWB observation noise battle array；Finally, the fusion positioning of vehicle is realized based on Unscented kalman filtering
Obtain the exact position of vehicle.
With reference to the accompanying drawing, thinking of the invention is further described:
Process of the invention is as shown in Figure 1.
A kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering, which is characterized in that
In urban environment, on the basis of Vehicular satellite and inertia combined navigation, UWB positioning is introduced, and combine fuzzy algorithmic approach to UWB
Accuracy of observation classification, so that the observation noise variance matrix of automatic adjusument UWB, herein on basis, is filtered based on Unscented kalman
Wave algorithm realizes the highly reliable fusion positioning of vehicle, and described method includes following steps:
Step 1) arranges UWB stationary nodes in road both sides, and measures the position coordinates of UWB stationary nodes；
Wherein the number of UWB stationary nodes and installation site will be directed to actual traffic scene, considering cost and fixed
Position accuracy requirement, determines reasonable placement scheme, please refers in Fig. 2, and the present invention is what typical urban intersection region proposed
A kind of placement scheme: in the road both sides and intersection rotary island region of four different directions of right-angled intersection
The heart has laid nine UWB stationary nodes in total, and this layout type can cover entire intersection to greatest extent, realizes
Vehicle is in the complete positioning in crossing region, and in the UWB node of rotary island center arrangement, not vulnerable to interference is blocked, it is fixed to facilitate
The raising of position precision；
Step 2) arranges a UWB mobile node in vehicle roof, using arrival time difference method (Time Difference
Of Arrival, TDOA) the UWB mobile node is obtained to the distance of each UWB stationary nodes, and observation of adjusting the distance is counted
Data preprocess；
Step 3), apart from observation, establishes equation in coordinates based on ranging localization principle, using minimum according to pretreated
Square law calculates the coordinate of UWB mobile node, obtains the rough position coordinate of vehicle；
Step 4) measures the current course angle of vehicle by vehicle electronics compass, and according to the rough position coordinate of vehicle with
And the position coordinates of UWB stationary nodes, calculate azimuth of each UWB stationary nodes relative to vehicle current driving direction；
Step 5) is according to two, distance and bearing angle feature, using fuzzy logic classifier algorithm to each UWB stationary nodes
Accuracy of observation classify, one is divided into A, B, C, five grades of D, E, the precision apart from observation of corresponding UWB by height to
Low, specific step is as follows for fuzzy Classified Algorithms Applied:
51) three subordinating degree functions of distance feature, and adjust the distance classification, respectively N, M and R are determined, distance is represented
" close ", " in ", " remote "；
52) two subordinating degree functions of orientation corner characteristics are determined, and azimuthal is classified, respectively L and S represent orientation
" big " and " small " at angle；
53) according to Fuzzy classification rule, 5 accuracy classes A, B, C, D, E of fuzzy logic classifier output are determined, are obscured
Classifying rules is as follows:
1. when distance is N, when azimuth is L, accuracy class A；
2. when distance is N, when azimuth is S, accuracy class B；
3. when distance is M, when azimuth is L, accuracy class C；
4. when distance is M, when azimuth is S, accuracy class D；
5. when distance is R, when azimuth is L, accuracy class E；
6. when distance is R, when azimuth is S, accuracy class E；
In the present embodiment, the subordinating degree function of the fuzzy Classified Algorithms Applied please refers to Fig. 3；
Step 6) combines the observation information of vehicle-mounted GNSS, gyroscope and wheel speed sensors output, utilizes Unscented kalman
The accurate positioning of filtering algorithm realization vehicle, the specific steps are as follows:
61) firstly, establishing the Unscented kalman filtering state model of vehicle positioning system
System mode vector is
Wherein,For vehicle latitude information, λ is vehicle longitude information, and h is height of car information；veFor the east orientation speed of vehicle
Degree, vnFor the north orientation speed of vehicle, vuFor the vertical velocity of vehicle；P is the pitch angle of vehicle, and r is the angle of heel of vehicle, and A is vehicle
Course angle；sfodFor wheel speed dilution of precision error, bωFor Gyroscope Random Drift, bGNSSFor GNSS receiver clock deviation, dGNSS
For GNSS receiver clock deviation drift rate, bUWBFor the offset error of UWB；
Exterior input vector is U=[vod aod fx fyωz]
Wherein, vodFor the car speed that wheel speed sensors measure, aodFor longitudinal acceleration of the vehicle, fxFor longitudinal direction of car acceleration
Spend measured value, fyFor vehicle lateral acceleration measured value, ωzFor the yaw velocity of gyroscope output；
System state equation are as follows:
Wherein, X (k) is the system mode vector of discrete instants k, and X (k-1), U (k-1), W (k-1), T (k-1) are respectively
The system mode vector of discrete instants (k-1), external input vector, system noise vector, external input noise vector；RMFor ground
Ball meridian radius, RNFor earth prime vertical radius, Δ t is the sampling interval；γwFor the error of wheel speed sensors and time correlation,
σwFor its corresponding noise variance；βzFor Gyroscope Random Drift, σzFor its corresponding noise variance；
62) the Unscented kalman filtering observation model of vehicle positioning system then, is established
Systematic observation vector is
Wherein,The pseudo range observed quantity of respectively m GNSS satellite,It is n UWB stationary nodes apart from observation；
Systematic observation equation are as follows:
Z (k)=H (k) X (k)+V (k)
Wherein, Z (k), H (k) are the systematic observation vector and systematic observation matrix of discrete instants k, when V (k) is discrete
Carve the observation noise vector of k；
The corresponding observation noise variance matrix of observation noise vector V (k) is R (k):
R (k)=diag [RGNSS RUWB]
Wherein, RGNSSFor the observation noise variance matrix of GNSS, RUWBFor the observation noise variance matrix of UWB；
63) fuzzy Classified Algorithms Applied according to step 5) classifies to the accuracy of observation of n UWB stationary nodes,
And then according to the noise variance matrix R of classification results automatic adjusument UWBUWB, final observation noise variance matrix R (k) is obtained, is had
Body method is as follows:
1. calculating the observation noise of i-th of UWB stationary nodes
Wherein, R0Noise variance basic value for UWB apart from observation；μ is adjustment factor, true by the result of fuzzy classification
Fixed, five kinds of different corresponding adjustment factors of accuracy class A, B, C, D, E are respectively μa, μb, μc, μd, μe；
2. calculating the observation noise variance matrix of UWB
3. updating observation noise variance matrix R (k): R (k)=diag [RGNSS RUWB]；
64) System State Model according to step 61), observation model described in step 62), and through step 63)
Noise variance matrix after automatic adjusument, the time for carrying out Unscented kalman filtering updates and measurement updaue process, obtains vehicle
Accurate location information.
1. constructing the sigma point set ξ of Unscented kalman filteringi(k-1), and its weight coefficient is determined
Wherein, η is scale parameter, and approximation accuracy can be improved by adjusting η,α1Determine sigma
The distribution of point, is set to a lesser positive number (1e-4≤α1< 1), α3Second scale parameter, is set to 0, α2
The dimension that 2, n is system mode vector is set to for Gaussian Profile for state distribution parameter；
2. carrying out Unscented kalman filtering time renewal process, one-step prediction quantity of state is calculated by following equationsWith one-step prediction error covariance matrix P (k, k-1):
ξi(k, k-1)=f (ξi(k-1)) i=0,1 ..., 2n
ζi(k, k-1)=h (ξi(k,k-1))
3. updating observation noise variance matrix R (k) according to step 63) the method；
4. calculating Unscented kalman filtering gain matrix K (k) by following equations:
K (k)=PXZ·PZZ -1
5. carrying out Unscented kalman filtering measurement updaue process, by following equations, state estimation is calculatedAnd estimation
Error covariance matrix P (k):
P (k)=P (k, k-1)-K (k) PZZK(k)T
The Unscented kalman filtering state estimationThe vehicle-state vector estimated value of as discrete instants k, thus
It can determine the precise position information of vehicle.
In this embodiment, for the Vehicle Fusion based on fuzzy self-adaption Unscented kalman filtering of inspection institute's proposition
The beneficial effect that localization method improves positioning accuracy, has carried out real train test, forward and backward to the algorithm for using fuzzy self-adaption
Vehicle location result carried out comparative test.In addition, it is necessary to be pointed out that: herein in test " before fuzzy algorithmic approach processing "
Method refer to traditional Unscented kalman filtering fusion method, " using fuzzy algorithmic approach processing after " method refer to this hair
Bright middle method.
Fig. 4 is battery of tests comparative result figure, after You Tuzhong's, it is apparent that use fuzzy adaptive algorithm,
The track of vehicle location is closer to true value.Fig. 5 is the front and back that one group of data uses fuzzy Classified Algorithms Applied in embodiment
Vehicle location error comparison diagram, the position error of You Tuzhong is, it is apparent that use fuzzy Classified Algorithms Applied automatic adjusument
After the observation noise matrix of Unscented kalman filtering, Euclidean distance position error reduces, and vehicle location precision significantly improves.
The foregoing is only a preferred embodiment of the present invention, is not intended to restrict the invention, for the skill of this field
For art personnel, the invention may be variously modified and varied.All within the spirits and principles of the present invention, made any to repair
Change, equivalent replacement, improvement etc., should all be included in the protection scope of the present invention.
Claims (1)
1. a kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering, which is characterized in that institute
The method of stating includes the following steps:
Step 1) arranges UWB stationary nodes in road both sides, and measures the position coordinates of UWB stationary nodes；
Step 2) arranges a UWB mobile node in vehicle roof, obtains the UWB mobile node to respectively using time difference method is reached
The distance of a UWB stationary nodes, and observation of adjusting the distance carries out data prediction；
Step 3), apart from observation, establishes equation in coordinates based on ranging localization principle, using least square according to pretreated
Method calculates the coordinate of UWB mobile node, obtains the rough position coordinate of vehicle；
Step 4) measures the current course angle of vehicle by vehicle electronics compass, and according to the rough position coordinate of vehicle and
The position coordinates of UWB stationary nodes calculate azimuth of each UWB stationary nodes relative to vehicle current driving direction；
Sight of the step 5) according to two, distance and bearing angle feature, using fuzzy logic classifier algorithm to each UWB stationary nodes
Precision is surveyed to classify, one is divided into A, B, C, five grades of D, E, the precision apart from observation of corresponding UWB from high to low, mould
Pasting sorting algorithm, specific step is as follows:
51) three subordinating degree functions of distance feature, and adjust the distance classification, respectively N, M and R are determined, represent distance " close ",
" in ", " remote "；
52) two subordinating degree functions of orientation corner characteristics are determined, and azimuthal is classified, respectively L and S are represented azimuthal
" big " and " small "；
53) according to Fuzzy classification rule, 5 accuracy classes A, B, C, D, E of fuzzy logic classifier output, fuzzy classification rule are determined
It is then as follows:
<1>is when distance is N, and azimuth is L, accuracy class A；
<2>is when distance is N, and azimuth is S, accuracy class B；
<3>is when distance is M, and azimuth is L, accuracy class C；
<4>is when distance is M, and azimuth is S, accuracy class D；
<5>is when distance is R, and azimuth is L, accuracy class E；
<6>is when distance is R, and azimuth is S, accuracy class E；
Step 6) combines the observation information of vehicle-mounted GNSS, gyroscope and wheel speed sensors output, utilizes Unscented kalman filtering
The accurate positioning of algorithm realization vehicle, the specific steps are as follows:
61) firstly, establishing the Unscented kalman filtering state model of vehicle positioning system
System mode vector is
Wherein,For vehicle latitude information, λ is vehicle longitude information, and h is height of car information；veFor the east orientation speed of vehicle,
vnFor the north orientation speed of vehicle, vuFor the vertical velocity of vehicle；P is the pitch angle of vehicle, and r is the angle of heel of vehicle, and A is vehicle
Course angle；sfodFor wheel speed dilution of precision error, bωFor Gyroscope Random Drift, bGNSSFor GNSS receiver clock deviation, dGNSSFor
GNSS receiver clock deviation drift rate, bUWBFor the offset error of UWB；
Exterior input vector is U=[vod aod fx fy ωz]
Wherein, vodFor the car speed that wheel speed sensors measure, aodFor longitudinal acceleration of the vehicle, fxFor longitudinal acceleration of the vehicle survey
Magnitude, fyFor vehicle lateral acceleration measured value, ωzFor the yaw velocity of gyroscope output；
System state equation are as follows:
Wherein, X (k) is the system mode vector of discrete instants k, and X (k-1), U (k-1), W (k-1), T (k-1) are respectively discrete
The system mode vector at moment (k-1), external input vector, system noise vector, external input noise vector；RMFor earth
Noon line radius, RNFor earth prime vertical radius, Δ t is the sampling interval；γwFor the error of wheel speed sensors and time correlation, σwFor
Its corresponding noise variance；βzFor Gyroscope Random Drift, σzFor its corresponding noise variance；
62) the Unscented kalman filtering observation model of vehicle positioning system then, is established
Systematic observation vector is
Wherein,The pseudo range observed quantity of respectively m GNSS satellite,For
N UWB stationary nodes apart from observation；
Systematic observation equation are as follows:
Z (k)=H (k) X (k)+V (k)
Wherein, Z (k), H (k) are the systematic observation vector and systematic observation matrix of discrete instants k, and V (k) is discrete instants k's
Observation noise vector；
The corresponding observation noise variance matrix of observation noise vector V (k) is R (k):
R (k)=diag [RGNSS RUWB]
Wherein, RGNSSFor the observation noise variance matrix of GNSS, RUWBFor the observation noise variance matrix of UWB；
63) fuzzy Classified Algorithms Applied according to step 5) classifies to the accuracy of observation of n UWB stationary nodes, in turn
According to the noise variance matrix R of classification results automatic adjusument UWBUWB, final observation noise variance matrix R (k) is obtained, specific side
Method is as follows:
<1>calculates the observation noise of i-th of UWB stationary nodes
Wherein, R0Noise variance basic value for UWB apart from observation；μ is adjustment factor, is determined by the result of fuzzy classification, five
The different corresponding adjustment factor of accuracy class A, B, C, D, E of kind is respectively μa, μb, μc, μd, μe；
<2>calculates the observation noise variance matrix of UWB
<3>updates observation noise variance matrix R (k): R (k)=diag [RGNSS RUWB]；
64) System State Model according to step 61), observation model described in step 62), and it is adaptive through step 63)
Noise variance matrix after should adjusting, the time for carrying out Unscented kalman filtering updates and measurement updaue process, obtains the essence of vehicle
True location information, the specific steps are as follows:
<1>constructs the sigma point set ξ of Unscented kalman filteringi(k-1), and its weight coefficient is determined
Wherein, η is scale parameter, and approximation accuracy can be improved by adjusting η,α1Determine sigma point
Distribution, is set to a lesser positive number (1e-4≤α1< 1), α3Second scale parameter, is set to 0, α2For shape
State distribution parameter is set to the dimension that 2, n is system mode vector for Gaussian Profile；
<2>carries out Unscented kalman filtering time renewal process, calculates one-step prediction quantity of state by following equations
With one-step prediction error covariance matrix P (k, k-1):
ξi(k, k-1)=f (ξi(k-1)) i=0,1 ..., 2n
ζi(k, k-1)=h (ξi(k,k-1))
<3>updates observation noise variance matrix R (k) according to step 63) the method；
<4>calculates Unscented kalman filtering gain matrix K (k) by following equations:
K (k)=PXZ·PZZ -1
<5>carries out Unscented kalman filtering measurement updaue process, by following equations, calculates state estimationIt is missed with estimation
Poor covariance matrix P (k):
P (k)=P (k, k-1)-K (k) PZZK(k)T
The Unscented kalman filtering state estimationThe vehicle-state vector estimated value of as discrete instants k, it is possible thereby to
Determine the precise position information of vehicle.
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 true CN109946731A (en) | 2019-06-28 |
| CN109946731B 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) |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110296706A (en) * | 2019-07-11 | 2019-10-01 | 北京云迹科技有限公司 | Localization method and device, robot for Indoor Robot |
| CN110940999A (en) * | 2019-12-13 | 2020-03-31 | 合肥工业大学 | Self-adaptive unscented Kalman filtering method based on error model |
| CN111090113A (en) * | 2019-12-31 | 2020-05-01 | 南京泰通科技股份有限公司 | High-precision train positioning terminal and positioning method based on Beidou, UWB and inertial navigation |
| CN111551976A (en) * | 2020-05-20 | 2020-08-18 | 四川万网鑫成信息科技有限公司 | Method for automatically completing abnormal positioning by combining various data |
| CN111811503A (en) * | 2020-07-15 | 2020-10-23 | 桂林电子科技大学 | 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 |
| CN111954153A (en) * | 2020-08-12 | 2020-11-17 | 南京工程学院 | Intelligent taxi calling navigation positioning method based on UWB positioning |
| CN112406861A (en) * | 2019-08-19 | 2021-02-26 | 通用汽车环球科技运作有限责任公司 | Method and device for Kalman filter parameter selection by using map data |
| CN112533149A (en) * | 2020-11-27 | 2021-03-19 | 桂林理工大学 | Moving target positioning algorithm based on UWB mobile node |
| CN112577512A (en) * | 2019-09-27 | 2021-03-30 | 北京初速度科技有限公司 | State quantity error determination method based on wheel speed fusion and vehicle-mounted terminal |
| CN112577513A (en) * | 2019-09-27 | 2021-03-30 | 北京初速度科技有限公司 | State quantity error determination method and vehicle-mounted terminal |
| CN113029137A (en) * | 2021-04-01 | 2021-06-25 | 清华大学 | Multi-source information self-adaptive fusion positioning method and system |
| CN114440876A (en) * | 2022-01-21 | 2022-05-06 | 北京自动化控制设备研究所 | Underground heading machine positioning and guiding method and underground heading machine positioning and guiding system |
| CN114527487A (en) * | 2022-02-24 | 2022-05-24 | 东南大学 | Active characteristic assisted self-adaptive fusion positioning method in point cloud sparse scene |
| CN114779307A (en) * | 2022-06-17 | 2022-07-22 | 武汉理工大学 | Port area-oriented UWB/INS/GNSS seamless positioning method |
| US11718317B2 (en) | 2020-10-16 | 2023-08-08 | Ford Global Technologies, Llc | Vehicle location correction using roadside devices |
| CN117148406A (en) * | 2023-10-30 | 2023-12-01 | 山东大学 | Indoor and outdoor seamless elastic fusion positioning method, system, medium and equipment |
| CN117610601A (en) * | 2023-11-08 | 2024-02-27 | 深圳市百千诚国际物流有限公司 | Logistics equipment intelligent positioning method and system based on Internet of things |
Citations (6)
| 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 |
-
2019
- 2019-03-06 CN CN201910168995.5A patent/CN109946731B/en active Active
Patent Citations (6)
| 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)
| Title |
|---|
| 王宇: "基于车内总线的车辆双模卫星融合定位技术研究", 《中国优秀硕士学位论文全文数据库工程科技II辑》 * |
Cited By (24)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110296706A (en) * | 2019-07-11 | 2019-10-01 | 北京云迹科技有限公司 | Localization method and device, robot for Indoor Robot |
| CN112406861A (en) * | 2019-08-19 | 2021-02-26 | 通用汽车环球科技运作有限责任公司 | Method and device for Kalman filter parameter selection by using map data |
| CN112406861B (en) * | 2019-08-19 | 2024-03-22 | 通用汽车环球科技运作有限责任公司 | Method and device for carrying out Kalman filter parameter selection by using map data |
| CN112577512A (en) * | 2019-09-27 | 2021-03-30 | 北京初速度科技有限公司 | State quantity error determination method based on wheel speed fusion and vehicle-mounted terminal |
| CN112577513A (en) * | 2019-09-27 | 2021-03-30 | 北京初速度科技有限公司 | State quantity error determination method and vehicle-mounted terminal |
| CN110940999A (en) * | 2019-12-13 | 2020-03-31 | 合肥工业大学 | Self-adaptive unscented Kalman filtering method based on error model |
| CN111090113A (en) * | 2019-12-31 | 2020-05-01 | 南京泰通科技股份有限公司 | High-precision train positioning terminal and positioning method based on Beidou, UWB and inertial navigation |
| CN111551976A (en) * | 2020-05-20 | 2020-08-18 | 四川万网鑫成信息科技有限公司 | Method for automatically completing abnormal positioning by combining various data |
| CN111811503A (en) * | 2020-07-15 | 2020-10-23 | 桂林电子科技大学 | 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 |
| CN111954153A (en) * | 2020-08-12 | 2020-11-17 | 南京工程学院 | 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 |
| CN112533149A (en) * | 2020-11-27 | 2021-03-19 | 桂林理工大学 | 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 |
| CN113029137A (en) * | 2021-04-01 | 2021-06-25 | 清华大学 | Multi-source information self-adaptive fusion positioning method and system |
| CN114440876A (en) * | 2022-01-21 | 2022-05-06 | 北京自动化控制设备研究所 | Underground heading machine positioning and guiding method and underground heading machine positioning and guiding system |
| CN114440876B (en) * | 2022-01-21 | 2024-04-02 | 北京自动化控制设备研究所 | Positioning and guiding method and system for underground heading machine |
| CN114527487A (en) * | 2022-02-24 | 2022-05-24 | 东南大学 | Active characteristic assisted self-adaptive fusion positioning method in point cloud sparse scene |
| CN114779307A (en) * | 2022-06-17 | 2022-07-22 | 武汉理工大学 | Port area-oriented UWB/INS/GNSS seamless positioning method |
| CN117148406A (en) * | 2023-10-30 | 2023-12-01 | 山东大学 | Indoor and outdoor seamless elastic fusion positioning method, system, medium and equipment |
| CN117148406B (en) * | 2023-10-30 | 2024-01-30 | 山东大学 | Indoor and outdoor seamless elastic fusion positioning method, system, medium and equipment |
| CN117610601A (en) * | 2023-11-08 | 2024-02-27 | 深圳市百千诚国际物流有限公司 | Logistics equipment intelligent positioning method and system based on Internet of things |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109946731B (en) | 2022-06-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109946731A (en) | A kind of highly reliable fusion and positioning method of vehicle based on fuzzy self-adaption Unscented kalman filtering | |
| CN109946730B (en) | Ultra-wideband-based high-reliability fusion positioning method for vehicles under cooperation of vehicle and road | |
| CN104076382B (en) | A kind of vehicle seamless positioning method based on Multi-source Information Fusion | |
| CN106840179B (en) | Intelligent vehicle positioning method based on multi-sensor information fusion | |
| CN103760904B (en) | A kind of voice broadcast type intelligent vehicle path planning apparatus and implementation | |
| CN105792135B (en) | A kind of method and device in positioning vehicle place lane | |
| CN108089180A (en) | Based on UWB sensors as back indicator to the localization method of GPS and inertial navigation system the suspension type rail vehicle corrected | |
| US11874366B2 (en) | High-precision vehicle positioning method based on ultra-wideband in intelligent vehicle infrastructure cooperative systems | |
| CN113706612B (en) | Underground coal mine vehicle positioning method fusing UWB and monocular vision SLAM | |
| AU2021106247A4 (en) | Vehicle fusion positioning method based on V2X and laser point cloud registration for advanced automatic driving | |
| CN108627864A (en) | Localization method and system, pilotless automobile system based on automobile key | |
| CN106469505A (en) | A kind of floating wheel paths method for correcting error and device | |
| CN111221020A (en) | Indoor and outdoor positioning method, device and system | |
| He et al. | Research on GNSS INS & GNSS/INS integrated navigation method for autonomous vehicles: A survey | |
| Cho et al. | Enhancing GNSS performance and detection of road crossing in urban area using deep learning | |
| Zhang et al. | Using UWB aided GNSS/INS integrated navigation to bridge GNSS outages based on optimal anchor distribution strategy | |
| CN115711618B (en) | Mining area high-precision positioning method based on road side guidance | |
| CN204115737U (en) | A kind of indoor positioning device based on inertial guidance and radio-frequency (RF) identification | |
| CN115061176B (en) | Vehicle GPS enhanced positioning method based on V2V instantaneous data exchange | |
| Wilfinger et al. | Indoor position determination using location fingerprinting and vehicle sensor data | |
| CN108696820A (en) | Indoor orientation method based on iBeacon ranging technologies | |
| Zhong et al. | Collaborative navigation in V2I network based on Chan-Taylor joint iterative algorithm | |
| Ke et al. | Integrated positioning method for intelligent vehicle based on GPS and UWB | |
| Sharma et al. | Real time location tracking map matching approaches for road navigation applications | |
| CN113156479B (en) | Combined positioning method for outdoor country road scene |
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 |