CN103777220A - Real-time and accurate pose estimation method based on fiber-optic gyroscope, speed sensor and GPS - Google Patents

Real-time and accurate pose estimation method based on fiber-optic gyroscope, speed sensor and GPS Download PDF

Info

Publication number
CN103777220A
CN103777220A CN201410022601.2A CN201410022601A CN103777220A CN 103777220 A CN103777220 A CN 103777220A CN 201410022601 A CN201410022601 A CN 201410022601A CN 103777220 A CN103777220 A CN 103777220A
Authority
CN
China
Prior art keywords
pose
local
gps
overall
estimation method
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
CN201410022601.2A
Other languages
Chinese (zh)
Other versions
CN103777220B (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

Images

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 real-time and accurate pose estimation method based on a fiber-optic gyroscope, a speed sensor and a GPS. The unmanned vehicle local locating algorithm based on a vehicle body motion model is mainly based on the two-dimensional plane dead reckoning principle, the vehicle body current local pose is estimated through the vehicle body sensor speed and course information estimation at the previous time, and the gyroscope course accumulative error is compensated through a linear model, so the accuracy of the local locating system can be improved; and by using the iterative closet point algorithm to register GPS and the local pose measuring system pose estimation method, the local locating accumulative error can be corrected, so the random noise of the GPS can be effectively eliminated, and the pose accuracy can be maintained when the GPS is in complete failure condition or failed, and the running test result shows that the method provided in the invention can be used to well fuse multi-sensor information, and provide reliable and continuous locating information for unmanned vehicles in multi-tree and multi-building urban environments. Moreover, the method has the advantage of good real-time performance.

Description

Based on the in real time accurate position and orientation estimation method 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 the sensor such as a kind of odometer that utilizes optical fibre gyro and intelligent vehicle and calculate the local attitude information of unmanned vehicle, utilize GPS to obtain overall pose, local unmanned vehicle pose and GPS position are carried out to registration and obtained the method for the overall pose of revision.
Background technology
Along with scientific and technical development, automatic driving vehicle (unmanned vehicle) is in mining industry, freight transportation, and agricultural automation and military field are in an increasingly wide range of applications.Navigation positioning system, for unmanned vehicle provides real-time position and attitude information, guarantees that unmanned vehicle completes independent navigation and accurately controls according to correct line, is the pith of unmanned vehicle system.Current unmanned vehicle locator meams mainly contains four classes, gps system, dead reckoning system (Dead Reckoning System, DRS), and the integrated navigation system that both are combined, also has the positioning system based on vision/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.GPS locates based on satellite ranging principle, does not have cumulative errors, the superior performance in Long time scale.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 the distinctive low cost local positioning of land wheeled vehicle technology, mainly calculates position and the attitude of car body by the velocity information of odometer and the course information of gyro.DRS has high degree of autonomy, is subject to hardly external environmental interference, and renewal frequency is fast, and in the short time, positioning precision is high.But, being subject to the impact of sensor intrinsic gauging error, the error of DRS is accumulation rapidly in time, cannot guarantee long positioning precision.
The 3rd class, integrated navigation system.Unmanned vehicle requires navigational system that continuous continual hi-Fix information is provided, and this is that any single positioning system cannot meet at present, and the Combinated navigation method that merges GPS and other positioning systems has become the focus of Study of location.
Because cost is low, in the short time, precision is high, dead reckoning system normal with GPS formation integrated navigation system, improve the performance of original positioning system.Kalman filtering and EKF are the main blending algorithms that GPS/DR integrated navigation adopts.For example, Rezaei S and Sengupta R have proposed a kind of Integrated Navigation Algorithm that merges GPS, odometer and gyro based on EKF, improve the noise estimated accuracy to GPS by the constraint that adds body movement to learn model; Lu WC and Zhang W propose to use the information of federated filter structure fusion GPS and DR, use respectively KF and EKF algorithm design GPS and DR local filter, senior filter only does information fusion, compares centralized filter structure, has improved the fault-tolerance of counting yield and system; Obradovic D, Lenz H and Schupfner M have introduced the GPS/DR onboard combined navigation system of Siemens Company's design, and this system adopts distributed kalman filter algorithm to merge, and the positioning performance of DR while relying on cartographic information to improve GPS to lose efficacy.But filtering method can only add single Gaussian noise model in the time processing random noise, is difficult to noise to carry out many-sided estimation and processing.
In addition, also there is GPS/INS integral type sealed in unit available, there are two kinds of array modes of tight coupling and loose coupling.Integral type is all eaily at aspects such as installation, operations.But at present, in the most fetch long price of high-precision GPS/INS integrated navigation system, allow user hang back.
The 4th class, utilizes the sensor such as vision and laser also can estimate the position of carrier and attitude information.Positioning system cost based on vision/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 subject to the interference of external environment, locating effect is unstable, is mainly used at present in the location technologies such as the comparatively simple Indoor Robot location of environment and assistant GPS, DRS.
Summary of the invention
The object of the invention is to overcome the shortcoming of existing GPS localization method, a kind of in real time accurate position and orientation estimation method based on optical fibre gyro, speed pickup and GPS is provided.
For achieving the above object, the present invention has adopted following technical scheme.
1) obtain the local pose of car body
Reckoning principle based on two dimensional surface, by the local pose of the information of being obtained by speed pickup and the course information being obtained by optical fibre gyro estimation car body current time;
2) obtain the overall pose of car body from GPS
Real time parsing goes out position and the three-dimensional velocity of WGS84 coordinate system lower body, and the position parsing and three-dimensional velocity are transformed under sky, northeast coordinate system, then utilizes three-dimensional velocity calculating filtering after conversion to obtain course information;
3) revise overall pose
The overall track of historical juncture (data point set of overall pose) and local path (data point set of local pose) are carried out to registration, then the local pose data projection of current time is arrived 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 the correction of local pose data to overall pose data.
The concrete steps of described step 1) are: the local course angle θ of Real-time Obtaining optical fibre gyro output; Obtain real-time front-wheel speed and front wheel angle from speed pickup, calculate relative displacement according to the velocity information of front and back two frames, 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 after averaging according to the velocity information of front and back two frames and local course angle θ in real time cumulative integration obtain.
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 that former frame, k represent 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 obtaining, v k-1, v kfor front-wheel speed, α k-1, α kfor front wheel angle.
In described step 3), complete known effective overall track and the registration of local path by iterative closest point algorithms, thereby obtain the transformation relation of overall track and local path, carry out overall pose prediction by described transformation relation, utilize the overall pose of prediction to judge that whether the gps data of current acquisition is effective, if effectively, use the overall pose of determining according to the gps data of current acquisition, if invalid, use the overall pose of predicting.
The concrete steps of described step 3) are:
A) overall track and local path registration
In the track having produced, select corresponding one section of overall track and local path take current time as starting point, determine after 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 rotation translational movement is the benchmark of overall pose prediction;
B) overall pose prediction
Use rotation translational movement to be rotated translation to the local pose data of current time, obtain the predicted value of current time overall situation pose;
C) judgement of gps data validity and processing
Calculate the range deviation of predicted value described in the overall pose definite according to current time gps data and step b), if range deviation is less than threshold value, using the described overall pose of determining according to current time gps data as a bit (being effective overall track) of the overall track for calculating rotation translational movement; If range deviation is more than or equal to threshold value, be noise signal by the overall pose of determining according to current time gps data and the local pose data markers of current time, use predicted value described in step b).
Described threshold value is to be determined by the GPS receiver precision adopting.
If appearance distance deviation is more than or equal to the situation of threshold value continuously, use noise signal to re-start the calculating of rotation 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, wherein local pose has adopted the unmanned vehicle local positioning algorithm based on body movement model to obtain, this algorithm is the reckoning principle based on two dimensional surface mainly, speed, the course information of the car body sensor by previous moment are estimated the current local pose of car body, have improved the precision of local positioning system; Utilize iterative closest point (ICP) algorithm registration overall situation drawn game position appearance, can proofread and correct the cumulative errors of local positioning, effectively eliminate the random noise of GPS, and keep the precision of pose in GPS complete failure or break down in the situation that.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 of GPS location acquisition are revised, can obtain in real time the overall posture information of continuously smooth, reliable continuous 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 being provided by speed pickup and front wheel angle, obtain the speed (vcos α, v represents front-wheel speed, α represents front wheel angle) of direction of vehicle movement, calculates relative displacement; Use reckoning principle based on two dimensional surface, speed, the course information of the car body sensor by previous moment are calculated the current local pose of car body;
2) adopt iterative closest point algorithms, calculate rotation amount and translational movement, the position appearance of playing a game and overall pose data are carried out registration, by data fusion; Gps data is proofreaied and correct the accumulation of local positioning, and local message can be eliminated the random noise of GPS, and both make up promotion mutually, the pose output that this position and orientation estimation method is still remained valid the in the situation that of complex environment and GPS losing lock, fault;
3) optical fibre gyro is affected by environment little, and antijamming capability is strong; Speed pickup is odometer, need not additionally 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 is body movement model and the two-dimentional reckoning schematic diagram that calculates local location information, wherein, (a) is body movement model; (b) be 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, wherein, (a) is the entirety comparison of urban area circumstance sport car data; (b) be and (c) partial enlarged drawing at two places in (a), show respectively without gps data and gps data and have the correction result in saltus step and burr situation.
Embodiment
Below in conjunction with accompanying drawing, the invention will be further described.
Referring to Fig. 1, the in real time accurate position and orientation estimation method based on optical fibre gyro, speed pickup and GPS is divided into three parts, and each step comprising is as follows:
1) obtain local pose by optical fibre gyro and speed pickup, concrete steps are as follows:
(1a) gyro output course angle θ information;
(1b) from UDP(User Data Protocol) bag parse real-time front-wheel speed and front wheel angle, thereby the velocity information of obtaining;
(1c), according to the velocity information of front and back two frames, calculate relative displacement;
(1d) reckoning in real time obtains local location information;
According to Fig. 2 (a), can obtain the real-time trailing wheel speed of a motor vehicle (speed of direction of vehicle movement): 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, when car is turned, trailing wheel speed is the component of front-wheel speed along car body direction; Can read car front wheel angle α by sensor information
v m=[vsinα vcosα]
Wherein, v mthe two-dimension speed vector that represents car body front-wheel, vsin α represents the cross component of front-wheel speed, vcos α represents the longitudinal component of front-wheel speed, i.e. real-time trailing wheel vehicle speed data.Can calculate the local pose of intelligent vehicle according to Fig. 2 (b): the course information that obtains current time by gyro; Car body real-time position information after averaging according to the velocity information of front and back two frames and course angle θ in real time cumulative integration obtain.In [k-1, the k] time, relative displacement is expressed as:
ΔD k=ΔT [k-1,k](v k-1cosα k-1+v kcosα k)/2
Wherein, Δ T [k-1, k]mistiming by two frame data before and after obtaining obtains, 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 that former frame, k represent 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) wrap real time parsing and go out position and the three-dimensional velocity of the car body under WGS84 coordinate system by UDP;
(2b) position parsing and three-dimensional velocity are transformed under 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 of revision, concrete steps are as follows:
(3a) overall track and local path registration
The first step is selected corresponding one section of overall track and local path in producing track take current time as starting point;
Second step, determines after overall situation and partial situation's track, uses ICP(iterative closest point) registration Algorithm calculates and can make local path through the rotation translational movement that overlaps with overall track of rotation translation, and this rotates translational movement is the benchmark that overall pose is predicted.
ICP registration is a progressively cyclic process for iteration, and as shown in Figure 3, ICP algorithm mainly comprises following two basic steps:
First, set up the corresponding relation between the first two point set (local pose point set P and overall pose point set M) 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, calculate two point sets with
Figure BDA0000458574320000083
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 k step kwith
Figure BDA0000458574320000085
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,
Figure BDA0000458574320000087
be respectively rotation matrix and translation matrix in point set transformation relation,
Figure BDA0000458574320000088
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, the result and the point set M similarity degree that do T conversion as point set P reach threshold value requirement, or iterations is while exceeding the set limit, stop iteration, obtain optimal T, now can predict overall pose according to this transformation relation T with local pose.
(3b) overall pose prediction
Can obtain local path by (3a) and transform to the rotation translational movement on overall track, use this rotation translational movement to be rotated translation to the local data of current time, as the predicted value of current overall pose.
(3c) judgement of gps data validity and processing
The first step, according to the current overall pose of (3b) prediction, now needs the definite overall pose of definite current time gps data and the range deviation of predicted value, judges that with this whether gps data is effective;
Second step, if the described overall pose of determining according to current time gps data and the range deviation of current predicted value are less than threshold value, allows its rotation translational movement of participating in (3a) calculate; If the described overall pose of determining according to current time gps data and the range deviation of current predicted value are more than or equal to threshold value, be noise by the overall pose of determining according to current time gps data with the local pose data markers being 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, uses noise signal to re-start the calculating of rotation translational movement.Described threshold value is to be determined by the GPS receiver precision adopting, and for example, if its precision is 2 meters, in radius is the circle of 2 meters, maximum deviation is 4 meters, and therefore this threshold value is 4.
Unmanned vehicle local positioning algorithm based on body movement model in the present invention, mainly (the Dead Reckoning of the reckoning based on two dimensional surface, DR) principle, speed, course information by previous moment sensor are estimated the current local pose of car body, have improved the precision of local positioning system; Simultaneously, utilize iterative closest point (Iterative Closest Point, ICP) position and orientation estimation method of algorithm registration GPS drawn game portion pose measurement system, can proofread and correct the cumulative errors of local positioning, effectively eliminate the random noise of GPS, and keep the precision of pose in GPS complete failure or break down in the situation that.Preventing test result shows, the algorithm that the present invention proposes can be good at merging many heat transfer agents, in the urban area circumstance of woody, many buildingss for unmanned vehicle provides reliable continuous locating information.In addition, the present invention also has good real-time.
According to Fig. 4, can find out the present invention's effect in actual applications.These data are unmanned vehicle sport car results in urban area circumstance, and environment has more serious trees, high building shield.The entirety comparison that Fig. 4 (a) is data, GPS raw data has disappearance, saltus step and burr, continuously smooth very of revised overall pose data.Fig. 4 (b) is the local amplification of left data in Fig. 4 (a), in this section, and GPS raw data disappearance; Fig. 4 (c) is data local amplification in below 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, eliminate to a great extent the deviation that gps data disappearance, saltus step and burr cause, be very reliable.

Claims (9)

1. the in real time accurate position and orientation estimation method based on optical fibre gyro, speed pickup and GPS, is characterized in that: comprise the following steps:
1) obtain the local pose of car body
Reckoning principle based on two dimensional surface, by the local pose of the information of being obtained by speed pickup and the course information being obtained by optical fibre gyro estimation car body current time;
2) obtain the overall pose of car body from GPS
Real time parsing goes out position and the three-dimensional velocity of WGS84 coordinate system lower body, and the position parsing and three-dimensional velocity are transformed under sky, northeast coordinate system, then utilizes three-dimensional velocity calculating filtering after conversion to obtain course information;
3) revise overall pose
The overall track of historical juncture and local path are carried out to registration, then the local pose data projection of current time is arrived 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 the correction of local pose data to overall pose data.
2. a kind of in real time accurately position and orientation estimation method based on optical fibre gyro, speed pickup and GPS according to claim 1, is characterized in that: the concrete steps of described step 1) are: the local course angle θ of Real-time Obtaining optical fibre gyro output; Obtain real-time front-wheel speed and front wheel angle from speed pickup, calculate relative displacement according to the velocity information of front and back two frames, 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 in real time accurately position and orientation estimation method based on optical fibre gyro, speed pickup and GPS according to claim 2, is characterized in that: described local location information average according to the velocity information of front and back two frames and local course angle θ after in real time cumulative integration obtain.
4. a kind of in real time accurately position and orientation estimation method 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 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 that former frame, k represent 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 in real time accurately position and orientation estimation method 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 obtaining, v k-1, v kfor front-wheel speed, α k-1, α kfor front wheel angle.
6. a kind of based on optical fibre gyro according to claim 1, the in real time accurate position and orientation estimation method of speed pickup and GPS, it is characterized in that: in described step 3), complete known effective overall track and the registration of local path by iterative closest point algorithms, thereby obtain the transformation relation of overall track and local path, carry out overall pose prediction by described transformation relation, utilize the overall pose of prediction to judge that whether the gps data of current acquisition is effective, if effectively, use the overall pose of determining according to the gps data of current acquisition, if invalid, use the overall pose of prediction.
7. a kind of in real time accurately position and orientation estimation method based on optical fibre gyro, speed pickup and GPS according to claim 6, is characterized in that: the concrete steps of described step 3) are:
A) overall track and local path registration
In the track having produced, select corresponding one section of overall track and local path take current time as starting point, determine after 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 rotation translational movement is the benchmark of overall pose prediction;
B) overall pose prediction
Use rotation translational movement to be rotated translation to the local pose data of current time, obtain the predicted value of current time overall situation pose;
C) judgement of gps data validity and processing
Calculate the range deviation of predicted value described in the overall pose definite according to current time gps data and step b), if range deviation is less than threshold value, using the described overall pose of determining according to current time gps data as the overall track for calculating rotation translational movement a bit; If range deviation is more than or equal to threshold value, be noise signal by the overall pose of determining according to current time gps data and the local pose data markers of current time, use predicted value described in step b).
8. a kind of in real time accurately position and orientation estimation method based on optical fibre gyro, speed pickup and GPS according to claim 7, is characterized in that: described threshold value is to be determined by the GPS receiver precision adopting.
9. a kind of in real time accurately position and orientation estimation method based on optical fibre gyro, speed pickup and GPS according to claim 7, it is characterized in that: if continuous appearance distance deviation is more than or equal to the situation of threshold value, use noise signal to re-start the calculating of rotation 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 true CN103777220A (en) 2014-05-07
CN103777220B 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)

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104121905A (en) * 2014-07-28 2014-10-29 东南大学 Course angle obtaining method based on inertial sensor
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
CN107328410A (en) * 2017-06-30 2017-11-07 百度在线网络技术（北京）有限公司 Method and automobile computer for positioning automatic driving vehicle
CN107328411A (en) * 2017-06-30 2017-11-07 百度在线网络技术（北京）有限公司 Vehicle positioning system and automatic driving vehicle
CN107782320A (en) * 2016-08-26 2018-03-09 法乐第(北京)网络科技有限公司 A kind of vehicle positioning method
CN108241161A (en) * 2016-12-23 2018-07-03 瑞士优北罗股份有限公司 For the distributed Kalman filter framework of reference carrier wave fuzziness estimation
CN108307314A (en) * 2016-08-26 2018-07-20 法乐第(北京)网络科技有限公司 A kind of vehicle locating device
CN109143305A (en) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 Automobile navigation method and device
CN109947103A (en) * 2019-03-18 2019-06-28 深圳一清创新科技有限公司 Unmanned control method, device, system and load bearing equipment
CN110068338A (en) * 2019-04-30 2019-07-30 杭州视氪科技有限公司 A kind of route memory system and method
CN110081881A (en) * 2019-04-19 2019-08-02 成都飞机工业（集团）有限责任公司 It is a kind of based on unmanned plane multi-sensor information fusion technology warship bootstrap technique
CN110785719A (en) * 2017-06-06 2020-02-11 智加科技公司 Method and system for instant object tagging via cross temporal verification in autonomous vehicles
CN111311679A (en) * 2020-01-31 2020-06-19 武汉大学 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
CN111693019A (en) * 2020-05-20 2020-09-22 西安交通大学 Attitude sensing device and data fusion and attitude calculation method
CN111949030A (en) * 2020-08-17 2020-11-17 江苏常发农业装备股份有限公司 Agricultural machinery positioning method, agricultural machinery vehicle and storage medium
CN112050809A (en) * 2020-10-08 2020-12-08 吉林大学 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
CN113899363A (en) * 2021-09-29 2022-01-07 北京百度网讯科技有限公司 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
US11718320B1 (en) 2020-08-21 2023-08-08 Aurora Operations, Inc. Using transmission sensor(s) in localization of an autonomous vehicle
US11790551B2 (en) 2017-06-06 2023-10-17 Plusai, Inc. Method and system for object centric stereo in autonomous driving vehicles
US11859994B1 (en) 2021-02-18 2024-01-02 Aurora Innovation, Inc. Landmark-based localization methods and architectures for an autonomous vehicle

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06341847A (en) * 1993-05-31 1994-12-13 Hitachi Ltd Navigation apparatus
CN1869630A (en) * 2006-04-19 2006-11-29 吉林大学 Testing system for integral vehicle running station
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
CN2901249Y (en) * 2005-05-12 2007-05-16 吉林大学 Meassuring system for automobile moving state
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
CN102364550A (en) * 2011-10-12 2012-02-29 北京世纪高通科技有限公司 Method and device for selecting information trigger position

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06341847A (en) * 1993-05-31 1994-12-13 Hitachi Ltd Navigation apparatus
CN2901249Y (en) * 2005-05-12 2007-05-16 吉林大学 Meassuring system for automobile moving state
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
CN1869630A (en) * 2006-04-19 2006-11-29 吉林大学 Testing system for integral vehicle running station
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
CN102364550A (en) * 2011-10-12 2012-02-29 北京世纪高通科技有限公司 Method and device for selecting information trigger position

Cited By (34)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104121905A (en) * 2014-07-28 2014-10-29 东南大学 Course angle obtaining method based on inertial sensor
CN107782320A (en) * 2016-08-26 2018-03-09 法乐第(北京)网络科技有限公司 A kind of vehicle positioning method
CN108307314A (en) * 2016-08-26 2018-07-20 法乐第(北京)网络科技有限公司 A kind of vehicle locating device
CN108241161A (en) * 2016-12-23 2018-07-03 瑞士优北罗股份有限公司 For the distributed Kalman filter framework of reference carrier wave fuzziness estimation
CN108241161B (en) * 2016-12-23 2023-03-10 瑞士优北罗股份有限公司 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
CN110785719A (en) * 2017-06-06 2020-02-11 智加科技公司 Method and system for instant object tagging via cross temporal verification in autonomous vehicles
US12093821B2 (en) 2017-06-06 2024-09-17 Plusai, Inc. Method and system for closed loop perception in autonomous driving vehicles
US12039445B2 (en) 2017-06-06 2024-07-16 Plusai, Inc. Method and system for on-the-fly object labeling via cross modality validation in autonomous driving vehicles
US11790551B2 (en) 2017-06-06 2023-10-17 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
CN107328410A (en) * 2017-06-30 2017-11-07 百度在线网络技术（北京）有限公司 Method and automobile computer for positioning automatic driving vehicle
CN107328411A (en) * 2017-06-30 2017-11-07 百度在线网络技术（北京）有限公司 Vehicle 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
CN109947103A (en) * 2019-03-18 2019-06-28 深圳一清创新科技有限公司 Unmanned control method, device, system and load bearing equipment
CN110081881A (en) * 2019-04-19 2019-08-02 成都飞机工业（集团）有限责任公司 It is a kind of based on unmanned plane multi-sensor information fusion technology warship bootstrap technique
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
CN111311679A (en) * 2020-01-31 2020-06-19 武汉大学 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
CN111693019A (en) * 2020-05-20 2020-09-22 西安交通大学 Attitude sensing device and data fusion and attitude calculation method
CN111693019B (en) * 2020-05-20 2021-04-20 西安交通大学 Attitude sensing device and data fusion and attitude calculation method
CN111949030A (en) * 2020-08-17 2020-11-17 江苏常发农业装备股份有限公司 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
CN112050809A (en) * 2020-10-08 2020-12-08 吉林大学 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
CN113899363A (en) * 2021-09-29 2022-01-07 北京百度网讯科技有限公司 Vehicle positioning method and device and automatic driving vehicle
US11953609B2 (en) 2021-09-29 2024-04-09 Beijing Baidu Netcom Science Technology Co., Ltd. Vehicle positioning method, apparatus and autonomous driving vehicle
CN115990880A (en) * 2023-01-04 2023-04-21 佛山市顺德区一拓电气有限公司 Robot course adjustment method, robot, device and computer storage medium

Also Published As

Publication number Publication date
CN103777220B (en) 2016-04-27

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
Heo et al. EKF-based visual inertial navigation using sliding window nonlinear optimization
CN107246868B (en) Collaborative navigation positioning system and navigation positioning method
CN110411462B (en) GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method
Bétaille et al. Creating enhanced maps for lane-level vehicle navigation
EP3109589B1 (en) A unit and method for improving positioning accuracy
CN102538781B (en) Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method
Jo et al. Road slope aided vehicle position estimation system based on sensor fusion of GPS and automotive onboard sensors
JP4897542B2 (en) Self-positioning device, self-positioning method, and self-positioning program
CN102372000B (en) Device and system for acquiring parking lot parameters
Wang et al. Vehicle localization at an intersection using a traffic light map
CN104061899B (en) A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation
CN107132563B (en) Combined navigation method combining odometer and dual-antenna differential GNSS
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
CN103207634A (en) Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle
Pfaff et al. Towards mapping of cities
CN102147258A (en) Feedback-mechanism-based vehicle navigation method and system
Li et al. Constrained unscented Kalman filter based fusion of GPS/INS/digital map for vehicle localization
Niu et al. Camera-based lane-aided multi-information integration for land vehicle navigation
JP7203805B2 (en) Analysis of localization errors of moving objects
Dean et al. Terrain-based road vehicle localization on multi-lane highways
EP4113063A1 (en) Localization of autonomous vehicles using camera, gps, and imu
Zhou et al. Road-Pulse from IMU to Enhance HD Map Matching for Intelligent Vehicle Localization

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