CN103777220B - Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS - Google Patents

Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS Download PDF

Info

Publication number
CN103777220B
CN103777220B CN201410022601.2A CN201410022601A CN103777220B CN 103777220 B CN103777220 B CN 103777220B CN 201410022601 A CN201410022601 A CN 201410022601A CN 103777220 B CN103777220 B CN 103777220B
Authority
CN
China
Prior art keywords
pose
gps
local
overall
data
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.)
Expired - Fee Related
Application number
CN201410022601.2A
Other languages
Chinese (zh)
Other versions
CN103777220A (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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong 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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN201410022601.2A priority Critical patent/CN103777220B/en
Publication of CN103777220A publication Critical patent/CN103777220A/en
Application granted granted Critical
Publication of CN103777220B publication Critical patent/CN103777220B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention provides a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS, wherein based on the unmanned vehicle local positioning algorithm of body movement model mainly based on the reckoning principle of two dimensional surface, the local pose that car body is current is estimated by the speed of the car body sensor of previous moment, course information, and utilize linear model to compensate the cumulative errors of gyro-compass course, improve the precision of local positioning system; And utilize the position and orientation estimation method of iterative closest point algorithms registration GPS drawn game portion pose measurement system can correct the cumulative errors of local positioning, the random noise of effective elimination GPS, and the precision of pose is kept in GPS complete failure or when breaking down, preventing test result shows, the method that the present invention proposes can be good at merging many heat transfer agents, reliable continuous print locating information can be provided for unmanned vehicle in the urban area circumstance of woody, many buildingss, in addition, the method also has good real-time.

Description

Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS
Technical field
The present invention relates to mobile robot, navigator fix and domain of data fusion, be specifically related to a kind ofly utilize the sensor such as the odometer of optical fibre gyro and intelligent vehicle to calculate unmanned vehicle local pose information, utilize GPS to obtain overall pose, unmanned vehicle local pose and GPS location are carried out the method that registration obtains the overall pose revised.
Background technology
Along with the development of science and technology, automatic driving vehicle (unmanned vehicle) in mining industry, freight transportation, agricultural automation and military field in an increasingly wide range of applications.Navigation positioning system provides real-time position and attitude information for unmanned vehicle, and ensureing that unmanned vehicle completes independent navigation according to correct line and accurately controls, is the pith of unmanned vehicle system.Current unmanned vehicle locator meams mainly contains four classes, gps system, dead reckoning system (DeadReckoningSystem, DRS), the integrated navigation system both combined, and also has the positioning system of view-based access control model/laser.
The first kind, GPS.Gps system can, in the world for user provides accurate, real-time position and velocity information, be current the most widely used navigation positioning system.Based on satellite ranging principle, there is not cumulative errors, the superior performance in Long time scale in GPS location.But in city, valley, tunnel and woody environment, satellite-signal is easily interfered, GPS positioning performance declines, and even loses positioning function.
Equations of The Second Kind, dead reckoning system.DRS belongs to local positioning system, is land wheeled vehicle distinctive low cost local positioning technology, calculates position and the attitude of car body mainly through the velocity information of odometer and the course information of gyro.DRS has high degree of autonomy, and hardly by external environmental interference, renewal frequency is fast, and short time inner position precision is high.But by the impact of sensor intrinsic gauging error, the error of DRS is accumulated rapidly in time, cannot ensure long positioning precision.
3rd class, integrated navigation system.Unmanned vehicle requires that navigational system provides continuous continual hi-Fix information, and this positioning system being current any one is single cannot meet, and the Combinated navigation method merging GPS and other positioning systems has become the focus of Study of location.
Because cost is low, in the short time, precision is high, the normal and GPS of dead reckoning system forms integrated navigation system, improves the performance of original positioning system.Kalman filtering and EKF are the main blending algorithms that GPS/DR integrated navigation adopts.Such as, RezaeiS and SenguptaR proposes a kind of Integrated Navigation Algorithm merging GPS, odometer and gyro based on EKF, improves the noise estimated accuracy to GPS by the constraint adding body movement model; LuWC and ZhangW then proposes to use commonwealth filter technique to merge the information of GPS and DR, use KF and EKF algorithm design GPS and DR local filter respectively, senior filter only does information fusion, compares centralized filter structure, improves the fault-tolerance of counting yield and system; ObradovicD, LenzH and SchupfnerM describe the GPS/DR onboard combined navigation system of Siemens Company's design, and this system adopts distributed kalman filter algorithm to merge, and rely on cartographic information to improve the positioning performance of DR when GPS lost efficacy.But filtering method can only add single Gaussian noise model when processing random noise, is difficult to carry out many-sided estimation and process to noise.
In addition, also there is GPS/INS integral type sealed in unit available, there are tight coupling and loose coupling two kinds of array modes.Integral type is all eaily in installation, operation etc.But at present, in the most fetch long price of high-precision GPS/INS integrated navigation system, allow user hang back.
4th class, utilizes the sensor such as vision and laser also can estimate the position of carrier and attitude information.The positioning system cost of view-based access control model/laser is lower, contain much information, and become a focus of artificial intelligence and positioning and navigation field research.But laser/vision positioning system is easily by the interference of external environment, and locating effect is unstable, be mainly used at present in the comparatively simple Indoor Robot location of environment and the location technology such as assistant GPS, DRS.
Summary of the invention
The object of the invention is to the shortcoming overcoming existing GPS localization method, a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS is provided.
For achieving the above object, present invention employs following technical scheme.
1) the local pose of car body is obtained
Based on the reckoning principle of two dimensional surface, estimated the local pose of car body current time by the information obtained by speed pickup and the course information obtained by optical fibre gyro;
2) the overall pose of car body is obtained from GPS
Real time parsing goes out position and the three-dimensional velocity of WGS84 coordinate system lower body, and under the position parsed and three-dimensional velocity being transformed into sky, northeast coordinate system, the three-dimensional velocity after then utilizing conversion calculates and filtering obtains course information;
3) overall pose is revised
The overall track (data point set of overall pose) of historical juncture and local path (data point set of local pose) are carried out registration, then by the local pose data projection of current time under the coordinate system at overall pose place, obtain the predicted value of the overall pose of current time car body, utilize predicted value to complete local pose data to the correction of overall pose data.
The concrete steps of described step 1) are: the local course angle θ that Real-time Obtaining optical fibre gyro exports; Wheel speed and front wheel angle before real-time from speed pickup acquisition, velocity information according to front and back two frame calculates relative displacement, utilize described relative displacement to carry out the local location information of real-time reckoning acquisition car body, described velocity information is front wheel speed and front wheel angle.
Described local location information is cumulative integration acquisition in real time after averaging according to the velocity information of front and back two frame and local course angle θ.
The formula of described reckoning is:
X k / k - 1 = f ( X k - 1 , θ k ) = x k - 1 + Δ D k cos ( θ k - 1 + θ k 2 ) y k - 1 + Δ D k sin ( θ k - 1 + θ k 2 ) θ k
Wherein, X represents local pose, and k-1 represents former frame, k represents present frame, and x represents the position coordinates of local pose in x direction, and y represents the position coordinates of local pose in y direction, Δ D krepresent [k-1, k] relative displacement in the time, θ represents local course angle.
ΔD k=ΔT [k-1,k](v k-1cosα k-1+v kcosα k)/2
Wherein, Δ T [k-1, k]for the mistiming of front and back two frame data of acquisition, v k-1, v kfor front wheel speed, α k-1, α kfor front wheel angle.
In described step 3), known effective overall track and the registration of local path is completed by iterative closest point algorithms, thus obtain the transformation relation of overall track and local path, overall pose prediction is carried out by described transformation relation, whether the gps data of current acquisition is effective to utilize the overall pose of prediction to judge, if effectively, then uses the overall pose determined according to the gps data of current acquisition, if invalid, then the overall pose of usage forecastings.
The concrete steps of described step 3) are:
A) overall track and local path registration
Be that starting point selects one section of corresponding overall track and local path in the track produced with current time, after determining overall track and local path, use iterative closest point algorithms to calculate the rotation translational movement that local path is overlapped with overall track through rotating translation, this rotates the benchmark that translational movement is the prediction of overall pose;
B) overall pose prediction
Use the local pose data of rotation translational movement to current time to carry out rotation translation, obtain the predicted value of current time overall situation pose;
C) gps data Effective judgement and process
Calculate the range deviation of predicted value described in the overall pose and step b) determined according to current time gps data, if range deviation is less than threshold value, then using the described overall pose determined according to current time gps data as be used for calculating rotate translational movement overall track in a bit (i.e. effective overall track); If range deviation is more than or equal to threshold value, be then noise signal by the local pose data markers of the overall pose determined according to current time gps data and current time, use predicted value described in step b).
Described threshold value is determined by the GPS precision adopted.
If appearance distance deviation is more than or equal to the situation of threshold value continuously, then noise signal is used to re-start the calculating rotating translational movement.
The present invention is by merging the data of optical fibre gyro, speed pickup and GPS, local pose and overall pose can be combined effectively, the unmanned vehicle local positioning algorithm that wherein local pose have employed based on body movement model obtains, this algorithm is mainly based on the reckoning principle of two dimensional surface, estimate to improve the precision of local positioning system by the local pose that car body is current by the speed of the car body sensor of previous moment, course information; Utilize iterative closest point (ICP) algorithm registration overall situation drawn game position appearance, the cumulative errors of local positioning can be corrected, effectively eliminate the random noise of GPS, and keep the precision of pose in GPS complete failure or when breaking down.Preventing test result shows, the algorithm that the present invention proposes can be good at merging many heat transfer agents, based on reckoning and ICP registration, the pose data that GPS location obtains are revised, the overall posture information of continuously smooth can be obtained in real time, reliable continuous print locating information is provided in the urban area circumstance of woody, many buildingss.
The present invention has following feature:
1) according to body movement model, the car speed provided by speed pickup and front wheel angle, obtain the speed (vcos α, v represent front wheel speed, and α represents front wheel angle) of direction of vehicle movement, calculates relative displacement; Use based on the reckoning principle of two dimensional surface, calculate the current local pose of car body by the speed of the car body sensor of previous moment, course information;
2) adopt iterative closest point algorithms, calculate rotation amount and translational movement, position appearance of playing a game and overall pose data carry out registration, by data fusion; The accumulation of gps data to local positioning corrects, and local message can eliminate the random noise of GPS, and both make up promotion mutually, and the pose that this position and orientation estimation method is still remained valid when complex environment and GPS losing lock, fault exports;
3) optical fibre gyro is affected by environment little, and antijamming capability is strong; Speed pickup is odometer, additionally need not increase equipment; Therefore this system has good stability, and reliability is high, the feature that cost is low.
Accompanying drawing explanation
Fig. 1 is schematic flow sheet of the present invention;
Fig. 2 calculates the body movement model of local location information and two-dimentional reckoning schematic diagram, and wherein, (a) is body movement model; B () is two-dimentional reckoning principle;
Fig. 3 is the ICP(iterative closest point of local pose and overall pose data fusion) algorithm flow block diagram;
Fig. 4 is the Data Comparison figure of practical application of the present invention, and wherein, (a) compares for the entirety of urban area circumstance sport car data; B () and (c) is the partial enlarged drawing at two places in (a), displaying has the correction result in saltus step and burr situation without gps data and gps data respectively.
Embodiment
Below in conjunction with accompanying drawing, the invention will be further described.
See Fig. 1, based on optical fibre gyro, speed pickup and GPS in real time accurately position and orientation estimation method be divided into three parts, each step comprised is as follows:
1) obtain local pose by optical fibre gyro and speed pickup, concrete steps are as follows:
(1a) gyro exports course angle θ information;
(1b) from UDP(User Data Protocol) parse real-time front wheel speed and front wheel angle bag, thus obtain velocity information;
(1c) according to the velocity information of front and back two frame, relative displacement is calculated;
(1d) reckoning in real time obtains local location information;
According to Fig. 2 (a), the real-time trailing wheel speed of a motor vehicle (speed of direction of vehicle movement) can be obtained: what speed pickup read is car body front wheel rotation speed information, choose bodywork reference frame initial point and be positioned at car body rear shaft center, after when car is turned, wheel speed is the component of front wheel speed along car body direction; Car front wheel angle α can be read by sensor information
v m=[vsinαvcosα]
Wherein, v mrepresent the two-dimension speed vector of car body front-wheel, vsin α represents the cross component of front wheel speed, and vcos α represents the longitudinal component of front wheel speed, namely real-time trailing wheel vehicle speed data.The local pose of intelligent vehicle can be calculated: the course information being obtained current time by gyro according to Fig. 2 (b); Car body real-time position information is cumulative integration acquisition in real time after averaging according to the velocity information of front and back two frame and course angle θ.[k-1, k] in the time relative displacement be expressed as:
ΔD k=ΔT [k-1,k](v k-1cosα k-1+v kcosα k)/2
Wherein, Δ T [k-1, k]obtained by the mistiming obtaining front and back two frame data, front-wheel speed v k-1, v kand front wheel angle α k-1, α kprovided by speed pickup.Course angle θ is provided in real time by gyro, and reckoning formula is:
X k / k - 1 = f ( X k - 1 , θ k ) = x k - 1 + Δ D k cos ( θ k - 1 + θ k 2 ) y k - 1 + Δ D k sin ( θ k - 1 + θ k 2 ) θ k
Wherein, X represents local pose, and k-1 represents former frame, k represents present frame, and x represents the position coordinates of local pose in x direction, and y represents the position coordinates of local pose in y direction, Δ D krepresent [k-1, k] relative displacement in the time, θ represents local course angle.
2) obtain overall pose from GPS, concrete steps are as follows:
(2a) position and the three-dimensional velocity of the car body under WGS84 coordinate system is gone out by UDP bag real time parsing;
(2b) under the position parsed and three-dimensional velocity being transformed into sky, northeast coordinate system;
(2c) carry out tangent calculating by three-dimensional velocity and mean filter obtains course information, binding site obtains GPS overall situation pose.
3) local pose and GPS overall situation pose are merged, obtain the overall pose revised, concrete steps are as follows:
(3a) overall track and local path registration
The first step take current time as starting point produces in track and selects one section of corresponding overall track and local path;
Second step, after determining overall situation and partial situation's track, then uses ICP(iterative closest point) registration Algorithm calculates the rotation translational movement that local path can be made to overlap with overall track through rotating translation, and this rotation translational movement is the benchmark that overall pose is predicted.
ICP registration is the cyclic process of a progressive alternate, and as shown in Figure 3, ICP algorithm mainly comprises following two basic steps:
First, the corresponding relation between the first two point set (local pose point set P and overall pose point set M) is set up according to the conversion of second step:
c k ( i ) = arg min j ∈ { 1,2 , . . . , N m } ( | | ( R k - 1 p → i + t → k - 1 ) - m → j | | 2 2 ) , i = 1,2 , . . . , N p
Secondly, two point sets are calculated with between new conversion:
( R * , t → * ) = arg min R T R = I m , det ( R ) = 1 , t → ( Σ i = 1 N p | | R ( R k - 1 p → i + t → k - 1 ) + t → - m → c k ( i ) | | 2 2 )
Upgrade the conversion R of kth step kwith
R k = R * R k - 1 , t k = R * t → k - 1 + t → *
Wherein, N p, N mbe respectively the quantity of local pose point set P and overall pose point set M mid point, R, be respectively the rotation matrix in point set transformation relation and translation matrix, for the data matrix of point set P and M.
In Fig. 3, T is the rotation translation relation of local pose point set P and overall pose point set M, when point set P be T conversion result and point set M similarity degree reach threshold requirement, or iterations is when exceeding the set limit, stop iteration, obtain optimal T, now can predict overall pose with local pose according to this transformation relation T.
(3b) overall pose prediction
Local path can be obtained by (3a) and transform to rotation translational movement on overall track, use this rotation translational movement to carry out rotation translation to the local data of current time, as the predicted value of current overall pose.
(3c) gps data Effective judgement and process
The first step, according to the current overall pose that (3b) predicts, now needs the range deviation of overall pose and the predicted value determining that current time gps data is determined, judges that whether gps data is effective with this;
Second step, the overall pose determined according to current time gps data if described and the range deviation of current predicted value are less than threshold value, then allow its rotation translation gauge participated in (3a) calculate; If the overall pose determined according to current time gps data described and the range deviation of current predicted value are more than or equal to threshold value, be then noise by the overall pose determined according to current time gps data and the local pose data markers obtained by optical fibre gyro and speed pickup.Noise processed is extremely important for the timely error correction of system, once overall pose predicted value is made mistakes continuously, then uses noise signal to re-start the calculating rotating translational movement.Described threshold value is determined by the GPS precision adopted, and such as, if its precision is 2 meters, in the circle that radius is 2 meters, maximum deviation is 4 meters, and therefore this threshold value is 4.
Based on the unmanned vehicle local positioning algorithm of body movement model in the present invention, mainly based on the reckoning (DeadReckoning of two dimensional surface, DR) principle, estimates to improve the precision of local positioning system by the local pose that car body is current by the speed of previous moment sensor, course information; Simultaneously, utilize iterative closest point (IterativeClosestPoint, ICP) position and orientation estimation method of algorithm registration GPS drawn game portion pose measurement system, the cumulative errors of local positioning can be corrected, the random noise of effective elimination GPS, and the precision of pose is kept in GPS complete failure or when breaking down.Preventing test result shows, the algorithm that the present invention proposes can be good at merging many heat transfer agents, for unmanned vehicle provides reliable continuous print locating information in the urban area circumstance of woody, many buildingss.In addition, the present invention also has good real-time.
According to Fig. 4, the present invention's effect in actual applications can be found out.These data are unmanned vehicle sport car results in urban area circumstance, and environment has more serious trees, high building shield.The entirety that Fig. 4 (a) is data compares, and GPS raw data has disappearance, saltus step and burr, revised overall pose data then very continuously smooth.Fig. 4 (b) is left data partial enlargement in Fig. 4 (a), and in this section, GPS raw data lacks; Fig. 4 (c) is bottom data partial enlargement in Fig. 4 (a), and in this section, saltus step and burr appear in GPS raw data; But after the correction of algorithm in the present invention, can provide correct traffic route, eliminating the deviation that gps data disappearance, saltus step and burr cause to a great extent, is very reliable.

Claims (7)

1., based on an accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS, it is characterized in that: comprise the following steps:
1) the local pose of car body is obtained
Based on the reckoning principle of two dimensional surface, estimated the local pose of car body current time by the information obtained by speed pickup and the course information obtained by optical fibre gyro;
2) the overall pose of car body is obtained from GPS
Real time parsing goes out position and the three-dimensional velocity of WGS84 coordinate system lower body, and under the position parsed and three-dimensional velocity being transformed into sky, northeast coordinate system, the three-dimensional velocity after then utilizing conversion calculates and filtering obtains course information;
3) overall pose is revised
The overall track of historical juncture and local path are carried out registration, then by the local pose data projection of current time under the coordinate system at overall pose place, obtain the predicted value of the overall pose of current time car body, utilize predicted value to complete local pose data to the correction of overall pose data; Judge that whether Current GPS data are effective by the range deviation of the gps data of predicted value and Current observation, valid data are preserved for the accurate prediction of subsequent time;
Described step 3) concrete steps be:
A) overall track and local path registration
Be that starting point selects one section of corresponding overall track and local path in the track produced with current time, after determining overall track and local path, use iterative closest point algorithms to calculate the rotation translational movement that local path is overlapped with overall track through rotating translation, this rotates the benchmark that translational movement is the prediction of overall pose;
B) overall pose prediction
Use the local pose data of rotation translational movement to current time to carry out rotation translation, obtain the predicted value of current time overall situation pose;
C) gps data Effective judgement and process
Calculate the overall pose and step b determined according to current time gps data) described in the range deviation of predicted value, if range deviation is less than threshold value, then using the described overall pose determined according to current time gps data as be used for calculating rotate translational movement overall track in a bit; If range deviation is more than or equal to threshold value, be then noise signal by the local pose data markers of the overall pose determined according to current time gps data and current time.
2. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 1, is characterized in that: described step 1) concrete steps be: the local course angle θ that Real-time Obtaining optical fibre gyro exports; Wheel speed and front wheel angle before real-time from speed pickup acquisition, velocity information according to front and back two frame calculates relative displacement, utilize described relative displacement to carry out the local location information of real-time reckoning acquisition car body, described velocity information is front wheel speed and front wheel angle.
3. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 2, is characterized in that: described local location information after averaging according to the velocity information of front and back two frame and local course angle θ in real time cumulative integration obtain.
4. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 1, is characterized in that: the formula of described reckoning is:
X k / k - 1 = f ( X k - 1 , θ k ) = x k - 1 + Δ D k c o s ( θ k - 1 + θ k 2 ) y k - 1 + Δ D k s i n ( θ k - 1 + θ k 2 ) θ k
Wherein, X represents local pose, and k-1 represents former frame, k represents present frame, and x represents the position coordinates of local pose in x direction, and y represents the position coordinates of local pose in y direction, Δ D krepresent [k-1, k] relative displacement in the time, θ represents local course angle.
5. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 4, is characterized in that:
ΔD k＝ΔT [k-1,k](v k-1cosα k-1+v kcosα k)/2
Wherein, Δ T [k-1, k]for the mistiming of front and back two frame data of acquisition, v k-1, v kfor front wheel speed, α k-1, α kfor front wheel angle.
6. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 1, is characterized in that: described threshold value is determined by the GPS precision adopted.
7. a kind of accurate position and orientation estimation method in real time based on optical fibre gyro, speed pickup and GPS according to claim 1, it is characterized in that: if appearance distance deviation is more than or equal to the situation of threshold value continuously, then use noise signal to re-start the calculating rotating translational movement.
CN201410022601.2A 2014-01-17 2014-01-17 Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS Expired - Fee Related CN103777220B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410022601.2A CN103777220B (en) 2014-01-17 2014-01-17 Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410022601.2A CN103777220B (en) 2014-01-17 2014-01-17 Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS

Publications (2)

Publication Number Publication Date
CN103777220A CN103777220A (en) 2014-05-07
CN103777220B true CN103777220B (en) 2016-04-27

Family

ID=50569682

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410022601.2A Expired - Fee Related CN103777220B (en) 2014-01-17 2014-01-17 Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS

Country Status (1)

Country Link
CN (1) CN103777220B (en)

Families Citing this family (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104121905B (en) * 2014-07-28 2017-02-22 东南大学 Course angle obtaining method based on inertial sensor
CN108307314A (en) * 2016-08-26 2018-07-20 法乐第(北京)网络科技有限公司 A kind of vehicle locating device
CN107782320A (en) * 2016-08-26 2018-03-09 法乐第(北京)网络科技有限公司 A kind of vehicle positioning method
EP3339908B1 (en) * 2016-12-23 2019-10-02 u-blox AG Distributed kalman filter architecture for carrier range ambiguity estimation
CN106525053A (en) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 Indoor positioning method for mobile robot based on multi-sensor fusion
CN107015238A (en) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 Unmanned vehicle autonomic positioning method based on three-dimensional laser radar
CN107273831A (en) * 2017-06-05 2017-10-20 苏州大学 A kind of Three-dimensional target recognition method based on spherical space
US11042155B2 (en) * 2017-06-06 2021-06-22 Plusai Limited Method and system for closed loop perception in autonomous driving vehicles
US11392133B2 (en) 2017-06-06 2022-07-19 Plusai, Inc. Method and system for object centric stereo in autonomous driving vehicles
CN107328410B (en) * 2017-06-30 2020-07-28 百度在线网络技术（北京）有限公司 Method for locating an autonomous vehicle and vehicle computer
CN107328411B (en) * 2017-06-30 2020-07-28 百度在线网络技术（北京）有限公司 Vehicle-mounted positioning system and automatic driving vehicle
CN109143305A (en) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 Automobile navigation method and device
CN109947103B (en) * 2019-03-18 2022-06-28 深圳一清创新科技有限公司 Unmanned control method, device and system and bearing equipment
CN110081881B (en) * 2019-04-19 2022-05-10 成都飞机工业（集团）有限责任公司 Carrier landing guiding method based on unmanned aerial vehicle multi-sensor information fusion technology
CN110068338A (en) * 2019-04-30 2019-07-30 杭州视氪科技有限公司 A kind of route memory system and method
CN111311679B (en) * 2020-01-31 2022-04-01 武汉大学 Free floating target pose estimation method based on depth camera
CN111366139A (en) * 2020-04-03 2020-07-03 深圳市赛为智能股份有限公司 Indoor mapping point positioning method and device, computer equipment and storage medium
CN111693019B (en) * 2020-05-20 2021-04-20 西安交通大学 Attitude sensing device and data fusion and attitude calculation method
CN111949030B (en) * 2020-08-17 2023-01-10 江苏常发农业装备股份有限公司 Agricultural machinery positioning method, agricultural machinery vehicle and storage medium
US11718320B1 (en) 2020-08-21 2023-08-08 Aurora Operations, Inc. Using transmission sensor(s) in localization of an autonomous vehicle
CN112050809B (en) * 2020-10-08 2022-06-17 吉林大学 Wheel type odometer and gyroscope information fusion unmanned vehicle directional positioning method
CN112327312A (en) * 2020-10-28 2021-02-05 上海高仙自动化科技发展有限公司 Vehicle pose determining method and device, vehicle and storage medium
US11859994B1 (en) 2021-02-18 2024-01-02 Aurora Innovation, Inc. Landmark-based localization methods and architectures for an autonomous vehicle
CN113899363B (en) 2021-09-29 2022-10-21 北京百度网讯科技有限公司 Vehicle positioning method and device and automatic driving vehicle
CN115990880A (en) * 2023-01-04 2023-04-21 佛山市顺德区一拓电气有限公司 Robot course adjustment method, robot, device and computer storage medium

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1869630A (en) * 2006-04-19 2006-11-29 吉林大学 Testing system for integral vehicle running station
CN2901249Y (en) * 2005-05-12 2007-05-16 吉林大学 Meassuring system for automobile moving state

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3075889B2 (en) * 1993-05-31 2000-08-14 株式会社日立製作所 Navigation device
WO2007043398A1 (en) * 2005-10-07 2007-04-19 Pioneer Corporation Route guiding device, time modification reporting device, route guiding method, time modification reporting method, route guiding program, time modification reporting program, and recording medium
US20090112393A1 (en) * 2007-10-25 2009-04-30 Maten Michael A Generating vehicle trip expenses and projected maintenance needs
CN201867196U (en) * 2010-11-15 2011-06-15 中交一航局第二工程有限公司 Underwater leveling machine measuring and positioning system
CN102364550B (en) * 2011-10-12 2013-09-18 北京世纪高通科技有限公司 Method and device for selecting information trigger position

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN2901249Y (en) * 2005-05-12 2007-05-16 吉林大学 Meassuring system for automobile moving state
CN1869630A (en) * 2006-04-19 2006-11-29 吉林大学 Testing system for integral vehicle running station

Also Published As

Publication number Publication date
CN103777220A (en) 2014-05-07

Similar Documents

Publication Publication Date Title
CN103777220B (en) Based on the accurate position and orientation estimation method in real time of optical fibre gyro, speed pickup and GPS
CN109946732B (en) Unmanned vehicle positioning method based on multi-sensor data fusion
CN107246868B (en) Collaborative navigation positioning system and navigation positioning method
Bétaille et al. Creating enhanced maps for lane-level vehicle navigation
CN104061899B (en) A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation
CN102538781B (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
CN103453913B (en) The combined positioning method of vehicle in parking lot
US8775063B2 (en) System and method of lane path estimation using sensor fusion
CN102372000B (en) Device and system for acquiring parking lot parameters
US9482536B2 (en) Pose estimation
CN107132563B (en) Combined navigation method combining odometer and dual-antenna differential GNSS
Pfaff et al. Towards mapping of cities
Wang et al. Vehicle localization at an intersection using a traffic light map
CN103207634A (en) Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle
CN101201255A (en) Vehicle combined navigation system based on intelligent navigation algorithm
TWI522258B (en) Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method
Heirich et al. Bayesian train localization method extended by 3D geometric railway track observations from inertial sensors
Li et al. Constrained unscented Kalman filter based fusion of GPS/INS/digital map for vehicle localization
JP7203805B2 (en) Analysis of localization errors of moving objects
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
Dean et al. Terrain-based road vehicle localization on multi-lane highways
Moussa et al. Ultrasonic wheel based aiding for land vehicle navigation in GNSS denied environment
Bevly et al. Navigation in Advanced Driver Assistance Systems and Automated Driving
Date et al. Real world experiments of an autonomous mobile robot in the pedestrian environment
Selloum et al. Lane level positioning using particle filtering

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: 20160427

Termination date: 20190117

CF01 Termination of patent right due to non-payment of annual fee