CN102410837A - Combined locating navigation system and method for vehicles - Google Patents

Combined locating navigation system and method for vehicles Download PDF

Info

Publication number
CN102410837A
CN102410837A CN2011102154373A CN201110215437A CN102410837A CN 102410837 A CN102410837 A CN 102410837A CN 2011102154373 A CN2011102154373 A CN 2011102154373A CN 201110215437 A CN201110215437 A CN 201110215437A CN 102410837 A CN102410837 A CN 102410837A
Authority
CN
China
Prior art keywords
wave filter
equation
gps
state
noise
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
Application number
CN2011102154373A
Other languages
Chinese (zh)
Other versions
CN102410837B (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.)
Jiangsu University
Original Assignee
Jiangsu 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 Jiangsu University filed Critical Jiangsu University
Priority to CN201110215437.3A priority Critical patent/CN102410837B/en
Publication of CN102410837A publication Critical patent/CN102410837A/en
Application granted granted Critical
Publication of CN102410837B publication Critical patent/CN102410837B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

The invention provides a combined navigation method for locating vehicles. A dual-sensor combined locating navigation filter system taking a navigation computer as the core is formed by reconfiguring an angular speed gyroscope, a low-pass filter, an A/D converter, a millimeter, a GPS, a microprocessor, a level switch and a digital processor. The dual-sensor data is filtered and optimized by the information fusion technology. The shortcoming that the GPS locating method cannot normally locate vehicles caused by blocked signals is overcome by the function provided by DR system. The error of the DR system is corrected and compensated by the vehicle position and speed information received by GPS. Therefore, the problem that the DR locating technology cannot be used singly after accumulating errors for a long time is solved. Meanwhile, data share is realized by the embedded CAN control transceiver.

Description

A kind of vehicle combination Position Fixing Navigation System and method
Technical field
The invention discloses a kind of vehicle combination positioning navigation method, belong to the vehicle positioning and navigation technical field.
Background technology
At present on vehicle widely used GPS GPS have that locating speed is fast, precision is high, ground characteristics such as coverings continuously, but in built-up city, block with the multipath effect problem and make the bearing accuracy decline of vehicle and have a lot of blind areas.Dead reckoning system (DR) utilizes the direction of vehicle ' and the instantaneous position that range information is calculated vehicle; Do not rely on outer signals; Has autonomous location navigation performance; And the inherent defect of dead reckoning system be positioning error along with time integral, can not be independent carry out dead reckoning and location for a long time.Therefore carry out reconstruct filtering and information fusion to GPS and two kinds of systems of DR, both are learnt from other's strong points to offset one's weaknesses, reach better positioning effect.On the other hand, information such as the vehicle location how the shared group assembly system is confirmed such as the steering of vehicle, brake system, the speed of a motor vehicle are vital problems at aspects such as automotive vehicle, intelligent transportation undoubtedly.The present invention relates to a kind of vehicle combination positioning navigation method of the CAN of embedding bus functionality, in order to address the above problem.
Summary of the invention
Technical matters to be solved by this invention is to use reconfiguration technique that the vehicle combination Position Fixing Navigation System is reconstituted the two sensors system.Utilize Kalman Filter Technology that this two sensors signal is carried out Filtering Processing, the gained status information is carried out information fusion with overall wave filter, obtains global optimum's estimated value, reaches the bearing accuracy of raising vehicle combination Position Fixing Navigation System and the purpose of integrality.
Combined system according to the invention is utilized reconfiguration technique, and system is carried out reconstruct, and is specific as follows:
One, GPS (5) is reconstituted digital sensor A (22);
Two, rate-of-turn gyroscope (1), low-pass filter (2), A/D converter (3) and mileometer (4) reconstitute digital sensor B (21);
Three, microprocessor (6), level conversion (7), digital processing unit (8) reconstitute navigational computer (28);
Four, in navigational computer (28), dead reckoning device (23), wave filter A (24), wave filter 2 (25) and overall wave filter (26) are set;
Five, further digital sensor B (21) and dead reckoning device (23) are reconstituted intelligent dead reckoning sensor (27).
After carrying out above-mentioned reconstruct, thereby reconstituted dual sensor filtering combination Position Fixing Navigation System.
In dual sensor filtering combination Position Fixing Navigation System; Use wave filter A (24), wave filter 2 (25) and overall wave filter (26); The gps signal of digital sensors A (22) and the DR signal of digital sensor B (21) are carried out filtering fusion optimization process, and concrete grammar is following:
The first step; Choosing
Figure 79144DEST_PATH_IMAGE001
,
Figure 600256DEST_PATH_IMAGE002
,
Figure 787654DEST_PATH_IMAGE003
are respectively the state variable of vehicle at location components, speed component and the component of acceleration of east orientation; ,
Figure 958053DEST_PATH_IMAGE005
, are respectively the state variable of vehicle at location components, speed component and the component of acceleration of north orientation; Choosing
Figure 954620DEST_PATH_IMAGE007
, are the error of vehicle at east orientation and north orientation; Error source equivalence during with GPS and DR system location is a first-order Markov process, gets
Figure 150426DEST_PATH_IMAGE009
and is the state vector of integrated positioning system.
In second step, the continuous state equation of getting integrated positioning system does
Figure 380550DEST_PATH_IMAGE010
=
Figure 175331DEST_PATH_IMAGE011
In the formula,
Figure 807301DEST_PATH_IMAGE012
，
Figure 458862DEST_PATH_IMAGE013
，
Figure 441861DEST_PATH_IMAGE014
Where:
Figure 789402DEST_PATH_IMAGE015
,
Figure 275878DEST_PATH_IMAGE016
were
Figure 301603DEST_PATH_IMAGE017
,
Figure 506320DEST_PATH_IMAGE018
Gaussian white noise;
Figure 377324DEST_PATH_IMAGE019
, , Vehicle maneuvering east and north to the rate of change of acceleration time constant associated;
Figure 118195DEST_PATH_IMAGE021
,
Figure 479381DEST_PATH_IMAGE022
, respectively, corresponding to the time constants associated Markov;
Figure 357339DEST_PATH_IMAGE023
,
Figure 756090DEST_PATH_IMAGE024
, Vehicle east and north to maneuver acceleration component of the current average.
In the 3rd step, getting T is the sampling period, and the continuous motion model of vehicle is that the discretization model of " current " statistical model of maneuvering target does
Figure 795721DEST_PATH_IMAGE025
（1）
In the formula,
Figure 518662DEST_PATH_IMAGE027
（2）
Get
Figure 834237DEST_PATH_IMAGE028
;
Figure 372666DEST_PATH_IMAGE029
; Then
Figure 773691DEST_PATH_IMAGE030
;
Figure 55768DEST_PATH_IMAGE031
,
Figure 491429DEST_PATH_IMAGE032
is respectively
Figure 935180DEST_PATH_IMAGE033
，
Figure 89080DEST_PATH_IMAGE034
Figure 971586DEST_PATH_IMAGE035
，
Figure 258823DEST_PATH_IMAGE036
Figure 873475DEST_PATH_IMAGE037
（3）
In the formula
Figure 249093DEST_PATH_IMAGE038
，
Figure 872972DEST_PATH_IMAGE039
Figure 17646DEST_PATH_IMAGE040
，
Figure 68778DEST_PATH_IMAGE041
Figure 931692DEST_PATH_IMAGE042
，
Figure 93683DEST_PATH_IMAGE043
，
Figure 420759DEST_PATH_IMAGE044
=
Figure 634004DEST_PATH_IMAGE045
=0。
The 4th step; The state variable
Figure 718635DEST_PATH_IMAGE046
of peek word sensors A (22), wave filter A (24) subsystem; State equation is identical with the overall status equation; Select the east orientation positional information
Figure 684317DEST_PATH_IMAGE047
of subsystem and the positional information
Figure 803583DEST_PATH_IMAGE048
(unit all turns to m) of north orientation to be observed quantity; , the discretize observation equation is:
Figure 34024DEST_PATH_IMAGE050
（4）
In the formula,
Figure 537817DEST_PATH_IMAGE051
（5）
Figure 137743DEST_PATH_IMAGE053
and is GPS receiver location observation noise; Be approximately
Figure 501521DEST_PATH_IMAGE055
, the white Gaussian noise of
Figure 595379DEST_PATH_IMAGE056
, the measurement noise covariance matrix is:
Figure 330117DEST_PATH_IMAGE057
。
The 5th step; Get the state variable
Figure 876636DEST_PATH_IMAGE058
of intelligent dead reckoning sensor (27), wave filter 2 (25) subsystems; State equation is identical with the overall status equation; Selecting the role, as observed quantity, the calibration factor of odometer is taken as K=1 for the output
Figure 722232DEST_PATH_IMAGE059
of rate gyro and the distance
Figure 732913DEST_PATH_IMAGE060
that odometer was exported in a sampling period.Get
Figure 638552DEST_PATH_IMAGE061
, the observation equation that gets continuously is:
Figure 672367DEST_PATH_IMAGE062
Figure 324584DEST_PATH_IMAGE063
is the drift of gyro, is approximately the white Gaussian noise of
Figure 393034DEST_PATH_IMAGE064
;
Figure 469575DEST_PATH_IMAGE065
is the observation noise of odometer, is approximately the white Gaussian noise of
Figure 725107DEST_PATH_IMAGE066
.The observation noise covariance matrix is:
Figure 178085DEST_PATH_IMAGE067
With the observation equation discretize, the observation equation that the system that obtains disperses does
Figure 101041DEST_PATH_IMAGE068
In the formula
Figure 348483DEST_PATH_IMAGE069
Figure 91311DEST_PATH_IMAGE070
，
Figure 79471DEST_PATH_IMAGE071
Above-mentioned nonlinear observation equation is adopted expansion Kalman filtering linearization;
Figure 856934DEST_PATH_IMAGE072
located Taylor series expansion in predicted value
Figure 275277DEST_PATH_IMAGE073
; And ignore second order and above item, get
Figure 646347DEST_PATH_IMAGE074
Abbreviation gets the discrete observation equation of DR system linear
Figure 113232DEST_PATH_IMAGE075
（6）
Wherein,
Figure 25429DEST_PATH_IMAGE076
（7）
Figure 614673DEST_PATH_IMAGE077
Figure 207460DEST_PATH_IMAGE078
Figure 540352DEST_PATH_IMAGE079
Figure 862673DEST_PATH_IMAGE081
Figure 67389DEST_PATH_IMAGE082
。
The 6th step; By front formula (2) and formula (3), system state transition matrix
Figure 141655DEST_PATH_IMAGE083
, system's control vector matrix
Figure 748217DEST_PATH_IMAGE084
are:
Figure 433594DEST_PATH_IMAGE037
By the foregoing formula (5), (7)? Obtained observation matrix system
Figure 376797DEST_PATH_IMAGE085
and
Figure 837866DEST_PATH_IMAGE086
is:
Figure 119123DEST_PATH_IMAGE076
According to the statistical property of system noise, get
Figure 597508DEST_PATH_IMAGE088
.
According to the statistical property of system noise, obtain
Figure 913083DEST_PATH_IMAGE089
as follows:
Figure 914854DEST_PATH_IMAGE091
，
Figure 196931DEST_PATH_IMAGE092
Figure 364083DEST_PATH_IMAGE093
，
Figure 73413DEST_PATH_IMAGE094
Wherein:
Figure 961734DEST_PATH_IMAGE095
Figure 781923DEST_PATH_IMAGE096
Figure 72090DEST_PATH_IMAGE097
Figure 952321DEST_PATH_IMAGE098
Figure 327939DEST_PATH_IMAGE099
Figure 14135DEST_PATH_IMAGE100
In the formula:
Figure 158809DEST_PATH_IMAGE101
and
Figure 201152DEST_PATH_IMAGE102
all is symmetric matrix; Element expression in
Figure 1749DEST_PATH_IMAGE102
is similar with the element expression in
Figure 163740DEST_PATH_IMAGE101
;
Figure 447271DEST_PATH_IMAGE103
in each element expression in
Figure 428499DEST_PATH_IMAGE101
replaces with
Figure 797481DEST_PATH_IMAGE104
, can correspondingly obtain the element expression in
Figure 700846DEST_PATH_IMAGE102
.
In the 7th step, discrete model that must GPS wave filter A according to the state equation (1) of the wave filter A that is set up and observation equation (4) is:
Figure 817182DEST_PATH_IMAGE025
According to the state equation (1) and the observation equation (6) of the wave filter of being set up 2, the discrete model that gets DR wave filter 2 is:
Figure 782044DEST_PATH_IMAGE025
Figure 285838DEST_PATH_IMAGE075
According to expansion Kalman filtering recurrence equation, the filtering equations of the discrete model of GPS wave filter A and the discrete model of DR wave filter 2 is following:
1, system state one-step prediction estimate equation:
Figure 525189DEST_PATH_IMAGE106
2, system state estimation value equation:
Figure 26709DEST_PATH_IMAGE107
3, filter gain equation:
Figure 85932DEST_PATH_IMAGE108
4, one-step prediction estimation error variance equation:
5, filtering error variance equation:
In the formula, ;
Figure 646827DEST_PATH_IMAGE112
Wherein,
Figure 375541DEST_PATH_IMAGE114
=
Figure 343497DEST_PATH_IMAGE115
A state of the output filter estimate
Figure 377312DEST_PATH_IMAGE116
, the estimation error variance
Figure 26599DEST_PATH_IMAGE117
, the noise mean square value
Figure 829470DEST_PATH_IMAGE118
and filter 2 state estimation value
Figure 906011DEST_PATH_IMAGE119
, the estimation error variance
Figure 489439DEST_PATH_IMAGE120
, the noise mean square value
Figure 880100DEST_PATH_IMAGE121
.
The 7th step; Merge by the estimated information of following method wave filter A and wave filter 2; Obtain global optimum's estimated value, overall estimation error variance , global noise mean square value
Figure 847236DEST_PATH_IMAGE123
and global state estimated value:
Figure 870292DEST_PATH_IMAGE124
Figure 861381DEST_PATH_IMAGE125
。
In the 8th step, overall wave filter is given wave filter A and wave filter 2 with information feedback
Figure 615208DEST_PATH_IMAGE128
Figure 144409DEST_PATH_IMAGE129
Figure 776379DEST_PATH_IMAGE130
Figure 568885DEST_PATH_IMAGE131
Figure 283376DEST_PATH_IMAGE132
Figure 616268DEST_PATH_IMAGE133
GPS can operate as normal, when bearing accuracy is higher, get
Figure 368324DEST_PATH_IMAGE134
=
Figure 66152DEST_PATH_IMAGE135
=0.5; As blocking etc. that reason GPS positioning system can not normally be located or bearing accuracy when relatively poor; Get
Figure 270869DEST_PATH_IMAGE134
=0,
Figure 407452DEST_PATH_IMAGE135
=1.
The position of supposing surface car initial time east orientation and north orientation is zero; Vehicle is with the speed of
Figure 14014DEST_PATH_IMAGE136
; Along 45 degree course angle linear uniform motion, the concurrence time of sailing is 500s; The sampling period of system is 1s, has related parameter to get:
，
Figure 577686DEST_PATH_IMAGE138
，
Figure 314698DEST_PATH_IMAGE139
，
Figure 713449DEST_PATH_IMAGE140
，
Figure 80977DEST_PATH_IMAGE141
，
Figure 197969DEST_PATH_IMAGE142
， ，
，
Figure 324166DEST_PATH_IMAGE145
。
Beneficial effect of the present invention does; Use reconfiguration technique; Gyroscope, wave filter, A/D converter, mileometer, GPS, microprocessor, level shifting circuit and digital processing unit etc. are carried out reconstruct, GPS and two kinds of systems of DR are carried out reconstruct filtering and information fusion, both are learnt from other's strong points to offset one's weaknesses; Improved the bearing accuracy and the integrality of system, make vehicle can be round-the-clock, do not have block, high-precision location.Utilize microprocessor to gather GPS, gyroscope and mileometer data, carry out digital filtering and the optimum processing of information fusion with digital processing unit, system real time is strong.Show information such as GPS, gyroscope and mileometer through display in real time and send to the data bus of vehicle to them, realize data sharing through the CAN control transceiver that embeds.
Description of drawings
Fig. 1 is vehicle GPS of the present invention/DR integrated navigation and location system composition diagram.
The combined system figure of the dual sensor navigator fix filtering that Fig. 2 reconstitutes for the present invention.
System's east orientation Error Graph before Fig. 3 filtering is merged.
System's north orientation Error Graph before Fig. 4 filtering is merged.
The Error Graph of back system east orientation is merged in Fig. 5 filtering.
The Error Graph of back system north orientation is merged in Fig. 6 filtering.
Embodiment
Embodiment 1:
According to Fig. 1, native system by angular rate gyroscope (1) (ADRS300), GPS (5) (GM-210), microprocessor (6) (P89C669), digital processing unit (8) (TMS320vc5410), low-pass filter (2), A/D converter (3) (TLC1549), mileometer (4), level conversion (7) (74LVC1654A), CAN controller (9) (SJA1000), transmitter-receiver (10) (TJA1050) and display (11) etc. form.With GPS (5) (GM-210) through MAX232 with reconstitute digital sensor A (22) after microprocessor (6) P3.0, P3.1 (P89C669) is connected.Rate-of-turn gyroscope (1) output (ADRS300) links to each other with the input of low-pass filter (2); The output of low-pass filter (2) links to each other with A/D converter (3) input (TLC1549); A/D converter (3) output (TLC1549) is connected with microprocessor (6) P1.1 (P89C669); Microprocessor (6) P1.2, P1.0 (P89C669) is connected with A/D converter (3) CS, CLK (TLC1549) respectively; Under the control of P1.2, P1.0, A/D converter (3) (TLC1549) data after will changing is input to P89C669 with serial mode.The output of mileometer (4) is connected with microprocessor (6) P3.4 (P89C669).With rate-of-turn gyroscope (1) (ADRS300), low-pass filter (2), A/D converter (3) (TLC1549) and mileometer (4) reconstitute digital sensor B (21).Microprocessor (6) (P89C669) (74LVC1654A) (TMS320vc5410) is connected with digital processing unit (8) through level conversion (7), and with microprocessor (6) (P89C669), level conversion (7) (74LVC1654A) and digital processing unit (8) (TMS320vc5410) reconstitute navigational computer (28).In navigational computer (28), design dead reckoning device (23), wave filter A (24), wave filter 2 (25) and overall wave filter (26); Further digital sensor B (21) and dead reckoning device (23) are reconstituted intelligent dead reckoning sensor (27), thereby reconstitute the combined system of dual sensor navigator fix filtering shown in Figure 2.
Embodiment 2:
According to shown in Figure 2; The inventive method is carried out Filtering Processing with digital sensor A (22) signal with wave filter A (24); Gained state estimation value
Figure 928454DEST_PATH_IMAGE116
and estimation error variance
Figure 944951DEST_PATH_IMAGE117
are as an input signal of overall wave filter; With wave filter 2 (25) to the filtering of intelligent dead reckoning sensor signal after gained state estimation value
Figure 380612DEST_PATH_IMAGE119
and estimation error variance
Figure 152259DEST_PATH_IMAGE120
as another input signal of overall wave filter; The estimated information of overall situation wave filter fused filtering device A (24) and wave filter 2 (25); When obtaining global optimum's estimated value, feed back to wave filter A (24) and wave filter 2 (25) to information
Figure 40580DEST_PATH_IMAGE146
, respectively.Improved the bearing accuracy and the integrality of system, make vehicle can be round-the-clock, do not have block, high-precision location.
Embodiment 3:
According to Fig. 1, microprocessor (6) data bus (P89C669) is connected with CAN controller (9) data bus (SJA1000), CAN controller (9) TX0, RX0 (SJA1000) is connected with transmitter-receiver (10) TXD, RXD (TJA1050) respectively.According to Fig. 1, Fig. 2, the locating navigation information of gained vehicle, through display (11), CAN controller (9) (SJA1000) and transmitter-receiver (10) (TJA1050) show and send to the CAN bus, realize data sharing.
Fig. 3 and Fig. 4 are respectively that integrated navigation system adopts the site error curve before filtering is merged; Fig. 5 and Fig. 6 are respectively that system has adopted the site error curve after filtering is merged.East orientation error before the GPS/DR system filter merges is 14.566m, and the north orientation error is 15.594m; East orientation error after filtering is merged is 4.676m, and the north orientation error is 5.385m.Before filtering is merged, convergence is preferably arranged so the error of back system is merged in filtering,, improved locating accuracy so this filtering blending algorithm has suppressed dispersing of filtering error effectively.

Claims (3)

1. vehicle combination Position Fixing Navigation System; It is characterized in that, comprise angular rate gyroscope (1), GPS (5), microprocessor (6), digital processing unit (8), low-pass filter (2), A/D converter (3), mileometer (4), level conversion (7), CAN controller (9), transmitter-receiver (10) and display (11); Said GPS (5) with reconstitute digital sensor A (22) after microprocessor (6) is connected; The output of rate-of-turn gyroscope (1) links to each other with the input of low-pass filter (2); The output of low-pass filter (2) links to each other with the input of A/D converter (3); The output of A/D converter (3) is connected with microprocessor (6), and the data after A/D converter (3) will be changed are input to microprocessor (6) with serial mode; The output of mileometer (4) is connected with microprocessor (6); Rate-of-turn gyroscope (1), low-pass filter (2), A/D converter (3) and mileometer (4) are reconstituted digital sensor B (21); Microprocessor (6) is connected with digital processing unit (8) through level conversion (7), and microprocessor (6), level conversion (7) and digital processing unit (8) are reconstituted navigational computer (28); In navigational computer (28), dead reckoning device (23), wave filter A (24), wave filter B (25) and overall wave filter (26) are set; Digital sensor B (21) and dead reckoning device (23) are reconstituted intelligent dead reckoning sensor (27).
2. air navigation aid of utilizing the described vehicle combination Position Fixing Navigation System of claim 1; It is characterized in that two digital sensor signals are carried out Filtering Processing with wave filter, and gained state estimation value and estimation error variance are as the input signal of overall wave filter; Overall situation wave filter merges the corresponding wave filter estimated information of two digital sensors; When obtaining global optimum's estimated value, feed back to two wave filters to relevant information respectively, improve bearing accuracy and integrality.
3. vehicle combination positioning navigation method according to claim 2 is characterized in that, concrete steps are:
The first step; Choosing ,
Figure 2011102154373100001DEST_PATH_IMAGE004
,
Figure 2011102154373100001DEST_PATH_IMAGE006
are respectively the state variable of vehicle at location components, speed component and the component of acceleration of east orientation;
Figure 2011102154373100001DEST_PATH_IMAGE008
,
Figure 2011102154373100001DEST_PATH_IMAGE010
,
Figure 2011102154373100001DEST_PATH_IMAGE012
are respectively the state variable of vehicle at location components, speed component and the component of acceleration of north orientation; Choosing
Figure 2011102154373100001DEST_PATH_IMAGE014
,
Figure 2011102154373100001DEST_PATH_IMAGE016
are the error of vehicle at east orientation and north orientation; Error source equivalence during with GPS and DR system location is a first-order Markov process, gets
Figure 2011102154373100001DEST_PATH_IMAGE018
and is the state vector of integrated positioning system;
In second step, the continuous state equation of getting integrated positioning system does
Figure 2011102154373100001DEST_PATH_IMAGE020
=
Figure 2011102154373100001DEST_PATH_IMAGE022
In the formula,
Figure 2011102154373100001DEST_PATH_IMAGE024
，
Figure 2011102154373100001DEST_PATH_IMAGE026
，
Figure 2011102154373100001DEST_PATH_IMAGE028
Where:
Figure 2011102154373100001DEST_PATH_IMAGE030
, were ,
Figure 2011102154373100001DEST_PATH_IMAGE036
Gaussian white noise; ,
Figure DEST_PATH_IMAGE040
, Vehicle eastward and northward Motor acceleration time constants related to the rate of change;
Figure DEST_PATH_IMAGE042
,
Figure DEST_PATH_IMAGE044
, respectively, corresponding to the time constants associated Markov;
Figure DEST_PATH_IMAGE046
,
Figure DEST_PATH_IMAGE048
, Vehicle east and north to maneuver acceleration component of the current mean;
In the 3rd step, getting T is the sampling period, and the continuous motion model of vehicle is that the discretization model of " current " statistical model of maneuvering target does
Figure DEST_PATH_IMAGE050
（1）
In the formula,
Figure DEST_PATH_IMAGE052
Figure DEST_PATH_IMAGE054
（2）
Get
Figure DEST_PATH_IMAGE056
;
Figure DEST_PATH_IMAGE058
; Then
Figure DEST_PATH_IMAGE060
;
Figure DEST_PATH_IMAGE062
,
Figure DEST_PATH_IMAGE064
is respectively
Figure DEST_PATH_IMAGE066
，
Figure DEST_PATH_IMAGE068
Figure DEST_PATH_IMAGE070
，
Figure DEST_PATH_IMAGE072
（3）
In the formula
Figure DEST_PATH_IMAGE076
，
Figure DEST_PATH_IMAGE078
Figure DEST_PATH_IMAGE080
，
Figure DEST_PATH_IMAGE082
Figure DEST_PATH_IMAGE084
，
Figure DEST_PATH_IMAGE086
，
Figure DEST_PATH_IMAGE088
=
Figure DEST_PATH_IMAGE090
=0
The 4th step; The state variable
Figure DEST_PATH_IMAGE092
of peek word sensors A (22), wave filter A (24) subsystem; State equation is identical with the overall status equation; Select the east orientation positional information of subsystem and the positional information
Figure DEST_PATH_IMAGE096
(unit all turns to m) of north orientation to be observed quantity; , the discretize observation equation is:
Figure DEST_PATH_IMAGE100
（4）
In the formula,
（5）
Figure DEST_PATH_IMAGE104
Figure DEST_PATH_IMAGE106
and is GPS receiver location observation noise; Be approximately
Figure DEST_PATH_IMAGE110
, the white Gaussian noise of
Figure DEST_PATH_IMAGE112
, the measurement noise covariance matrix is:
Figure DEST_PATH_IMAGE114
The 5th step; Get the state variable of intelligent dead reckoning sensor (27), wave filter B (25) subsystem; State equation is identical with the overall status equation; Selecting the role, as observed quantity, the calibration factor of odometer is taken as K=1 for the output
Figure DEST_PATH_IMAGE118
of rate gyro and the distance
Figure DEST_PATH_IMAGE120
that odometer was exported in a sampling period;
Get
Figure DEST_PATH_IMAGE122
, the observation equation that gets continuously is:
Figure DEST_PATH_IMAGE124
Figure DEST_PATH_IMAGE126
is the drift of gyro, is approximately the white Gaussian noise of
Figure DEST_PATH_IMAGE128
;
Figure DEST_PATH_IMAGE130
is the observation noise of odometer, is approximately the white Gaussian noise of
Figure DEST_PATH_IMAGE132
;
The observation noise covariance matrix is:
Figure DEST_PATH_IMAGE134
With the observation equation discretize, the observation equation that the system that obtains disperses does
In the formula
Figure DEST_PATH_IMAGE138
Figure DEST_PATH_IMAGE140
，
Figure DEST_PATH_IMAGE142
Above-mentioned nonlinear observation equation is adopted expansion Kalman filtering linearization;
Figure DEST_PATH_IMAGE144
located Taylor series expansion in predicted value
Figure DEST_PATH_IMAGE146
; And ignore second order and above item, get
Abbreviation gets the discrete observation equation of DR system linear
Figure DEST_PATH_IMAGE150
（6）
Wherein,
Figure DEST_PATH_IMAGE152
（7）
Figure DEST_PATH_IMAGE154
Figure DEST_PATH_IMAGE158
Figure DEST_PATH_IMAGE160
Figure DEST_PATH_IMAGE162
Figure DEST_PATH_IMAGE164
The 6th step; By front formula (2) and formula (3), system state transition matrix
Figure DEST_PATH_IMAGE166
, system's control vector matrix
Figure DEST_PATH_IMAGE168
are:
Figure 355160DEST_PATH_IMAGE054
Figure 209983DEST_PATH_IMAGE074
By the foregoing formula (5), (7)? Obtained observation matrix system
Figure DEST_PATH_IMAGE170
and
Figure DEST_PATH_IMAGE172
is:
Figure DEST_PATH_IMAGE174
Figure 737523DEST_PATH_IMAGE152
According to the statistical property of system noise, get
Figure DEST_PATH_IMAGE176
;
According to the statistical property of system noise, obtain
Figure DEST_PATH_IMAGE178
as follows:
Figure DEST_PATH_IMAGE180
Figure DEST_PATH_IMAGE182
，
Figure DEST_PATH_IMAGE184
Figure DEST_PATH_IMAGE186
，
Figure DEST_PATH_IMAGE188
Wherein:
Figure DEST_PATH_IMAGE190
Figure DEST_PATH_IMAGE194
Figure DEST_PATH_IMAGE196
Figure DEST_PATH_IMAGE198
In the formula: and all is symmetric matrix; Element expression in is similar with the element expression in
Figure 151634DEST_PATH_IMAGE202
;
Figure DEST_PATH_IMAGE206
in each element expression in
Figure 849462DEST_PATH_IMAGE202
replaces with
Figure DEST_PATH_IMAGE208
, can correspondingly obtain the element expression in
Figure 864298DEST_PATH_IMAGE204
;
In the 7th step, discrete model that must GPS wave filter A according to the state equation (1) of the wave filter A that is set up and observation equation (4) is:
Figure 882DEST_PATH_IMAGE050
Figure DEST_PATH_IMAGE210
According to state equation (1) and the observation equation (6) of the wave filter B that is set up, the discrete model that gets DR wave filter B is:
Figure 538490DEST_PATH_IMAGE150
According to expansion Kalman filtering recurrence equation, the filtering equations of the discrete model of the discrete model of GPS wave filter A and DR wave filter B is following:
(1), system state one-step prediction estimate equation:
Figure DEST_PATH_IMAGE212
(2), system state estimation value equation:
Figure DEST_PATH_IMAGE214
(3), filter gain equation:
Figure DEST_PATH_IMAGE216
(4), one-step prediction estimation error variance equation:
Figure DEST_PATH_IMAGE218
(5), filtering error variance equation:
Figure DEST_PATH_IMAGE220
In the formula, ;
Figure DEST_PATH_IMAGE224
Figure DEST_PATH_IMAGE226
Wherein,
Figure DEST_PATH_IMAGE228
=
Figure DEST_PATH_IMAGE230
A state of the output filter estimate
Figure DEST_PATH_IMAGE232
, the estimation error variance
Figure DEST_PATH_IMAGE234
, the noise mean square value
Figure DEST_PATH_IMAGE236
and the filter B state estimation value
Figure DEST_PATH_IMAGE238
, the estimation error variance
Figure DEST_PATH_IMAGE240
, the noise mean square value
Figure DEST_PATH_IMAGE242
;
The 7th step; Merge by the estimated information of following method wave filter A and wave filter B; Obtain global optimum's estimated value, overall estimation error variance
Figure DEST_PATH_IMAGE244
, global noise mean square value
Figure DEST_PATH_IMAGE246
and global state estimated value:
Figure DEST_PATH_IMAGE250
Figure DEST_PATH_IMAGE252
In the 8th step, overall wave filter is given wave filter A and wave filter B with information feedback
Figure DEST_PATH_IMAGE254
Figure DEST_PATH_IMAGE256
Figure DEST_PATH_IMAGE258
Figure DEST_PATH_IMAGE262
Figure DEST_PATH_IMAGE264
Figure DEST_PATH_IMAGE266
GPS can operate as normal, when bearing accuracy is higher, get
Figure DEST_PATH_IMAGE268
= =0.5; As blocking etc. that reason GPS positioning system can not normally be located or bearing accuracy when relatively poor; Get
Figure 660862DEST_PATH_IMAGE268
=0,
Figure 335557DEST_PATH_IMAGE270
=1.
CN201110215437.3A 2011-07-29 2011-07-29 Combined locating navigation system and method for vehicles Expired - Fee Related CN102410837B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201110215437.3A CN102410837B (en) 2011-07-29 2011-07-29 Combined locating navigation system and method for vehicles

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201110215437.3A CN102410837B (en) 2011-07-29 2011-07-29 Combined locating navigation system and method for vehicles

Publications (2)

Publication Number Publication Date
CN102410837A true CN102410837A (en) 2012-04-11
CN102410837B CN102410837B (en) 2014-10-29

Family

ID=45913052

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201110215437.3A Expired - Fee Related CN102410837B (en) 2011-07-29 2011-07-29 Combined locating navigation system and method for vehicles

Country Status (1)

Country Link
CN (1) CN102410837B (en)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104990563A (en) * 2015-07-22 2015-10-21 广西大学 Vehicle travelled mileage calculation method
CN105190356A (en) * 2013-03-22 2015-12-23 高通股份有限公司 Heading, velocity, and position estimation with vehicle sensors, mobile device, and GNSS inputs
CN105509764A (en) * 2015-12-30 2016-04-20 北京星网宇达科技股份有限公司 Vehicle-mounted integrated terminal used for intelligent driving test
CN106772449A (en) * 2015-11-20 2017-05-31 现代自动车株式会社 The system and method for shared vehicle position information
CN108700423A (en) * 2016-03-01 2018-10-23 歌乐株式会社 Car-mounted device and presumption method
CN109115223A (en) * 2018-08-30 2019-01-01 江苏大学 A kind of full source integrated navigation system of full landform towards intelligent agricultural machinery
CN110940344A (en) * 2019-11-25 2020-03-31 奥特酷智能科技(南京)有限公司 Low-cost sensor combination positioning method for automatic driving
CN113075713A (en) * 2021-03-29 2021-07-06 北京理工大学重庆创新中心 Vehicle relative pose measuring method, system, equipment and storage medium
CN113924464A (en) * 2019-05-10 2022-01-11 萨克提斯有限责任公司 Method for measuring a structure, and a procedure for defining an optimal method for measuring said structure

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1948910A (en) * 2006-11-09 2007-04-18 复旦大学 Combined positioning method and apparatus using GPS, gyroscope, speedometer
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1948910A (en) * 2006-11-09 2007-04-18 复旦大学 Combined positioning method and apparatus using GPS, gyroscope, speedometer
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
寇艳红等: "车载GPS/DR组合导航系统的信息融合新方案", 《遥测遥控》, vol. 23, no. 1, 28 February 2002 (2002-02-28) *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105190356A (en) * 2013-03-22 2015-12-23 高通股份有限公司 Heading, velocity, and position estimation with vehicle sensors, mobile device, and GNSS inputs
CN104990563B (en) * 2015-07-22 2017-12-26 广西大学 VMT Vehicle-Miles of Travel computational methods
CN104990563A (en) * 2015-07-22 2015-10-21 广西大学 Vehicle travelled mileage calculation method
CN106772449B (en) * 2015-11-20 2021-07-16 现代自动车株式会社 System and method for sharing vehicle position information
CN106772449A (en) * 2015-11-20 2017-05-31 现代自动车株式会社 The system and method for shared vehicle position information
CN105509764A (en) * 2015-12-30 2016-04-20 北京星网宇达科技股份有限公司 Vehicle-mounted integrated terminal used for intelligent driving test
CN105509764B (en) * 2015-12-30 2019-03-12 北京星网宇达科技股份有限公司 A kind of vehicle-mounted integrated terminal for intelligent Driving Test
CN108700423A (en) * 2016-03-01 2018-10-23 歌乐株式会社 Car-mounted device and presumption method
CN108700423B (en) * 2016-03-01 2022-02-01 歌乐株式会社 In-vehicle device and estimation method
CN109115223A (en) * 2018-08-30 2019-01-01 江苏大学 A kind of full source integrated navigation system of full landform towards intelligent agricultural machinery
CN113924464A (en) * 2019-05-10 2022-01-11 萨克提斯有限责任公司 Method for measuring a structure, and a procedure for defining an optimal method for measuring said structure
CN110940344A (en) * 2019-11-25 2020-03-31 奥特酷智能科技(南京)有限公司 Low-cost sensor combination positioning method for automatic driving
CN113075713A (en) * 2021-03-29 2021-07-06 北京理工大学重庆创新中心 Vehicle relative pose measuring method, system, equipment and storage medium

Also Published As

Publication number Publication date
CN102410837B (en) 2014-10-29

Similar Documents

Publication Publication Date Title
CN102410837B (en) Combined locating navigation system and method for vehicles
US9921065B2 (en) Unit and method for improving positioning accuracy
CN103777220B (en) Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS
US20190323844A1 (en) System and method for lidar-based vehicular localization relating to autonomous navigation
CN101846734B (en) Agricultural machinery navigation and position method and system and agricultural machinery industrial personal computer
CN107132563B (en) Combined navigation method combining odometer and dual-antenna differential GNSS
CN105300395A (en) Navigation and positioning method and device
CN104061899A (en) Kalman filtering based method for estimating roll angle and pitching angle of vehicle
WO2014042710A2 (en) Pose estimation
JP2008249688A (en) System and method for sensor-fused navigation
Petrich et al. On-board wind speed estimation for uavs
CN109343095A (en) A kind of vehicle mounted guidance vehicle combination positioning device and combinations thereof localization method
Yun et al. IMU/Vision/Lidar integrated navigation system in GNSS denied environments
CN107274721A (en) Many vehicle cooperative localization methods in a kind of intelligent transportation system
Park et al. MEMS 3D DR/GPS integrated system for land vehicle application robust to GPS outages
Hossein et al. Multi-sensor data fusion for autonomous vehicle navigation through adaptive particle filter
CN112904396A (en) High-precision positioning method and system based on multi-sensor fusion
Suwandi et al. Low-cost IMU and GPS fusion strategy for apron vehicle positioning
CN114076610A (en) Error calibration and navigation method and device of GNSS/MEMS vehicle-mounted integrated navigation system
CN109084760A (en) Navigation system between a kind of building
CN203479311U (en) Vehicle-mounted combined navigation equipment
Seyr et al. Proprioceptive navigation, slip estimation and slip control for autonomous wheeled mobile robots
Choi et al. Outdoor positioning estimation of multi-GPS/INS integrated system by EKF/UPF filter conversion
Niknejad et al. Multi-sensor data fusion for autonomous vehicle navigation and localization through precise map
Zhu et al. Research on localization vehicle based on multiple sensors fusion system

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20141029

Termination date: 20150729

EXPY Termination of patent right or utility model