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
data
Prior art date
Application number
CN201410022601.2A
Other languages
Chinese (zh)
Other versions
CN103777220B (en
Inventor
杜少毅
宋晔
刘娟
薛建儒
张春家
祝继华
Original Assignee
西安交通大学
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 西安交通大学 filed Critical 西安交通大学
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

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining 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/42Determining position
    • G01S19/45Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47Determining 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in preceding groups G01C1/00-G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in preceding 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/165Navigation; Navigational instruments not provided for in preceding 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

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

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 rotation matrix and translation matrix in point set transformation relation, 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 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 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 (4)

* 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
CN108307314A (en) * 2016-08-26 2018-07-20 法乐第(北京)网络科技有限公司 A kind of vehicle locating device

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 (4)

* 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
CN108307314A (en) * 2016-08-26 2018-07-20 法乐第(北京)网络科技有限公司 A kind of vehicle locating device
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

Also Published As

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

Similar Documents

Publication Publication Date Title
Dissanayake et al. The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications
Rezaei et al. Kalman filter-based integration of DGPS and vehicle sensors for localization
Bevly Global positioning system (GPS): A low-cost velocity sensor for correcting inertial sensor errors on ground vehicles
Syed et al. Fuzzy logic-based map matching algorithm for vehicle navigation system in urban canyons
EP1913235B1 (en) Guidance, navigation, and control system for a vehicle
Ochieng et al. Map-matching in complex urban road networks
Clanton et al. A low-cost solution for an integrated multisensor lane departure warning system
KR100580628B1 (en) A heading direction estimation method and system for a mobile body
CN101576384B (en) Indoor movable robot real-time navigation method based on visual information correction
Skog et al. In-car positioning and navigation technologies—A survey
El Najjar et al. A road-matching method for precise vehicle localization using belief theory and kalman filtering
US7286933B2 (en) GPS/dead-reckoning combination system and operating method thereof
US7626533B2 (en) Road curvature estimation system
Goel et al. Robust localization using relative and absolute position estimates
Quddus et al. A high accuracy fuzzy logic based map matching algorithm for road transport
Thrapp et al. Robust localization algorithms for an autonomous campus tour guide
Durrant-Whyte et al. Localization of autonomous guided vehicles
Schroedl et al. Mining GPS traces for map refinement
Georgy et al. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter
Huang et al. A low-order DGPS-based vehicle positioning system under urban environment
US20080033645A1 (en) Pobabilistic methods for mapping and localization in arbitrary outdoor environments
DE102013107861A1 (en) Tracking of road vehicles with sensors of different types
US8301374B2 (en) Position estimation for ground vehicle navigation based on landmark identification/yaw rate and perception of landmarks
DE102014211450A1 (en) Track monitoring with electronic horizon
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20160427

Termination date: 20190117