CN109631887A - Inertial navigation high-precision locating method based on binocular, acceleration and gyroscope - Google Patents
Inertial navigation high-precision locating method based on binocular, acceleration and gyroscope Download PDFInfo
- Publication number
- CN109631887A CN109631887A CN201811632825.XA CN201811632825A CN109631887A CN 109631887 A CN109631887 A CN 109631887A CN 201811632825 A CN201811632825 A CN 201811632825A CN 109631887 A CN109631887 A CN 109631887A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- inertial navigation
- binocular
- point
- vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- 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
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
Abstract
A kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope is claimed in the present invention; present invention design determines inertial navigation reference position characteristic point three-dimensional coordinate based on binocular positioning+high-precision GPS information; it is as a reference point come real-time update inertial navigation three-dimensional coordinate by characteristic point coordinate value; to eliminate the cumulative errors that inertial navigation is generated due to integral, high precision positioning of the realization based on inertial navigation and binocular positioning combination auxiliary parking garage vehicle.Structure of the invention block diagram includes following components: one, High Accuracy Inertial Navigation System composition and reference coordinate architecture；Two, the acquisition methods of inertial navigation navigation origin coordinates；Three, the selection method of the high-precision fixed potential reference point of binocular；Four, the selection and design of high-precision inertial navigation coordinate system；Five, the conversion method of high-precision inertial navigation coordinate system；Six, the removing method of inertial navigation cumulative errors.
Description
Technical field
The invention belongs to integrated navigation high accuracy positioning fields, and in particular to a kind of vision guided navigation based on indoor parking is auxiliary
The method for helping inertial navigation high accuracy positioning.
Background technique
A kind of localization method of the GPS as prevalence is widely used in land vehicle positioning, but for interior
The signal of parking GPS is often blocked.Indoor high accuracy positioning quickly passes through navigation routine conducive to vehicle and finds conjunction
Suitable parking stall, in the case of in present parking garage, distribution is numerous, this technology is necessary.
Inertial navigation system (Inertial Navigation System, INS) using Newton mechanics law as working principle,
The angle speed of carrier is measured using the inertial measurement component (Inertial Mcasurement Unit, IMU) being mounted on carrier
Degree and acceleration information, obtain the navigational parameters such as position, speed and the posture of carrier by integral operation.Inertial navigation has had
Full independence and anti-interference has high-precision in the short time, but due to integral principle, error is accumulated at any time, because
The precision that this works long hours is poor.In order to reduce error accumulation, external observation value is introduced to be modified to inertial navigation system.
The common form of vision aided inertial navigation system (INS) is by monocular or binocular camera shooting system and low cost
IMU is combined, and the latter is widely used to robot technology and autonomous vehicle navigation.This method largely can reduce
Position error caused by the uncertainty of Visual Feature Retrieval Process in space reduces drift phenomenon, overcomes in inertia measurement positioning
The error of accumulation.Vision aided inertial navigation method mainly utilizes forward looking camera to capture prominent features, utilizes various features
Assist the navigation of man-made structures.The three-dimensional feature obtained herein using binocular vision system；Therefore, by binocular vision system and used
The method that property navigation system combines can be applied in the more universal environment of corner characteristics.
Nowadays there is the data anastomosing algorithm much about vision guided navigation and inertial navigation, but these blending algorithms are always
The cumulative errors of inertial navigation cannot be eliminated well, and related algorithm is more complicated.
Summary of the invention
Present invention seek to address that the above problem of the prior art.Propose it is a kind of elimination inertial navigation due to integral generate
Cumulative errors greatly improve matched precision, speed and accuracy rate, and matched stability and robustness also have to be mentioned greatly very much
Height, the good inertial navigation high-precision based on binocular, acceleration and gyroscope of real-time when updating inertial navigation location information
Localization method.Technical scheme is as follows:
A kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope comprising following steps:
Step 1 establishes the high-precision inertia system based on inertial navigation mould group, Binocular Stereo Vision System and vehicle, and
Establish reference coordinate architecture；
The acquisition of step 2, inertial navigation origin coordinates: vehicle carries out inertial navigation using origin coordinates point as starting point,
And the improvement stereo matching method based on SIFT algorithm is used, the positional relationship significantly put according to corner feature in the picture is selected
Interested region, vehicle complete positioning and tracking to area-of-interest by the accumulation of continuous position in the process of moving,
Hereafter images match only can be carried out to interested region during, the characteristic point for completing initial time binocular vision mentions
It takes, reaches matching precision height, fireballing purpose；
The selection of the high-precision fixed potential reference point of step 3, binocular: the significant corner of corner feature in parking lot indoors is selected
Or reference frame is established by origin of reference point as the reference point of binocular high accuracy positioning in turning；
The selection and design of step 4, high-precision inertial navigation coordinate system: selection includes terrestrial coordinates during the navigation process
Coordinate system including system, navigational coordinate system, carrier coordinate system, camera coordinate system, world coordinate system meets and finally sits to correlation
Target resolves；
The conversion of step 5, high-precision inertial navigation coordinate system: characteristic point is obtained in the location information of world coordinate system, vehicle
Turn between location information and world coordinate system in inertial navigation navigational coordinate system and inertial navigation navigational coordinate system
Change relationship；
The elimination of step 6, inertial navigation cumulative errors: inertial navigation is determined based on binocular positioning and high-precision GPS information
Reference position characteristic point three-dimensional coordinate, it is as a reference point come real-time update inertial navigation three-dimensional coordinate by characteristic point coordinate value,
To eliminate the cumulative errors that inertial navigation is generated due to integral, realizes and be based on stopping in inertial navigation and binocular positioning combination ancillary chamber
The accurate positioning of parking lot vehicle.
Further, the inertial navigation mould group of the step 1 is by 3-axis acceleration sensor, three-axis gyroscope and three axis magnetic
Power meter composition, measurement obtain the parameter of each sensor, resolve to obtain the coordinate system in inertial navigation system by Quaternion Algorithm
Conversion is integrated to obtain the location parameter for carrying inertial navigation mould group carrier by rate integrating and position, utilizes binocular vision
Image information determines inertial navigation reference position characteristic point three-dimensional coordinate by visual correlation algorithm, by by current time with
Last moment characteristic point coordinate value difference it is as a reference point come real-time update inertial navigation three-dimensional coordinate, come eliminate inertial navigation by
Integrate the cumulative errors generated.
Further, the acquisition of the step 2 origin coordinates point specifically includes: into no GPS signal or GPS signal
When very faint indoor garage, vehicle judges the binocular on the vehicle of starting point with the effective GPS information basis finally obtained
Whether stereo visual system can get the location information of feature angle point, by the speed of the delay time of this process and vehicle,
Course angle information etc. carries out location information interpolation, obtains inertial navigation navigation origin coordinates, vehicle is using origin coordinates as initial bit
Set carry out inertial navigation.
Further, the step 2 is specifically included based on the improvement stereo matching method step of SIFT algorithm: first by pair
The left images of acquisition carry out epipolar-line constraint, and the region ROI (Region ofInterest) is secondly selected from left figure, is being reduced
Feature vector dimension reduces time-consuming, accelerates matching speed using BBF (the Best Bin First) algorithm based on KD tree, finally
Error hiding is removed with RANSAC (Random Sample Consensus) algorithm.
Further, the local maximum that the feature of the step 3 utilization angle point, i.e. angle point are curvature on objective contour
Point is the determinant attribute of contour of object, and angle point also has rotational invariance, when vehicle enters the room parking lot from garage
Afterwards, the binocular camera on vehicle chooses the nearest significant object of a corner feature on vehicle heading as binocular
The references object of characteristic point is extracted in positioning, and the angle point that will acquire is as the high-precision fixed potential reference point of binocular.
Further, the step 5 further includes the conversion between following coordinate system:
The first, carrier coordinate system and navigational coordinate system is converted: by posture transformational analogy of the carrier in space into carrier
Coordinate system b to navigational coordinate system n is obtained around pitch axis, roll axis and course axis by 3 basic rotation transformations:
Second, the conversion between camera coordinate system and world coordinate system: selection world coordinate system and navigational coordinate system weight
It closes, coordinate (x of the space characteristics point in camera coordinate systemc,yc,zc) by being converted to space characteristics point in world coordinates
Coordinate (x in systemw,yw,zw) to get having arrived coordinate points (x of the space characteristics point in navigational coordinate systemn,yn,zn):
Third, the conversion between terrestrial coordinate system and navigational coordinate system: first by terrestrial coordinate system OexeyezeAround zeAxis rotation
Turn the angle λ, obtains Oe1-xe1ye1ze1.Then around ye1Axis is rotated by 90 ° the angle-L, obtains Oe2-xe2ye2ze2.Finally around ze2Axis revolves 90 °, warp
Crossing above step can be by terrestrial coordinate system Oe-xeyeze, it is transformed into navigational coordinate system On-xnynzn, phase between Two coordinate system
Mutual transformation can pass through matrixTo indicate.
Further, coordinate (x of the space characteristics point in camera coordinate systemc,yc,zc) by being converted to sky
Between coordinate (x of the characteristic point in world coordinate systemw, yw,zw), it specifically includes:
Homogeneous coordinates of the space characteristics point under world coordinate system and camera coordinate system are respectively (xw, yw, zw, 1) and
(xc, yc, zc, 1), wherein homogeneous coordinate system is exactly that the vector that dimension is n is indicated with the vector that n+1 is tieed up, world coordinate system
Coordinate value of the origin in camera coordinate system is t=(tx, ty, tz)T, the vector of this 3 dimension is translation vector, direction vector R
It is used to describe the direction vector of the relationship of the two in world coordinate system for camera coordinate system, is closed by the sine and cosine of three attitude angles
System combine 3 × 3 orthogonal matrix, then the relationship between world coordinate system and camera coordinate system beWherein R indicates 3 × 3 spin matrix, OT=(0,0,0).
Further, the elimination of the step 6 inertial navigation cumulative errors: by binocular stereo vision positioning and high-precision
GPS information determines inertial navigation starting point coordinate, vehicle when into no GPS signal or GPS signal very faint region,
Inertial navigation mode is opened, is believed using the position that Binocular Stereo Vision System obtains the significant corner of corner feature or inflection point
Breath, and three-dimensional reference frame is established by the location information of characteristic point is as a reference point, according to different moments vehicle in different positions
The same reference point locations information of setting acquisition, the difference to adjacent moment vehicle location converted by coordinate are updated with this
Vehicle location.Vehicle is selected by the distance and the reference point minimum threshold of distance of setting that judge reference point in the process of moving
Whether object of reference and reference point are replaced.Final realize assists parking garage vehicle based on inertial navigation and binocular positioning combination
It is accurately positioned.
It advantages of the present invention and has the beneficial effect that:
The present invention is a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope, utilizes binocular
Vision navigation system obtains the navigation information of the updating location information inertial navigation with reference to angle point, final to realize to indoor vehicle
High accuracy positioning.The error that system generates in the present invention is related with the precision that binocular positions, not related with inertial navigation, and vehicle
During inertial navigation, distance reference point is closer, and the precision of binocular positioning is higher, i.e. the precision of whole system is got over
Height, by set distance threshold value constantly update reference point, with this come eliminate inertial navigation due to integral generate cumulative errors.
Second, the method for the present invention is simple and reliable, simple using tool and process, and binocular vision navigation system is based on
The improvement stereo matching method of SIFT algorithm greatly improves matched precision, speed and accuracy rate, matched stability and Shandong
Stick is greatly enhanced, and when updating inertial navigation location information, real-time is good.
In conclusion the present invention obtains inertial navigation origin coordinates using position interpolation algorithm, pass through acceleration and gyro
The inertial navigation of instrument obtains position vector indoors, updates the position that inertial navigation navigates indoors using binocular vision data and believes
Breath, the final reconstruction realized to the motion profile of indoor vehicle, so that parking garage vehicle be made to be accurate to by navigation information
Up to parking position.
Detailed description of the invention
Fig. 1 is that the present invention provides the inertial navigation high-precision locating method overall structure block diagram of preferred embodiment.
Fig. 2 is High Accuracy Inertial Navigation System composition and reference frame structural schematic diagram of the invention.
Fig. 3 is the structural block diagram that the inertial navigation coordinate of the invention based on binocular vision updates.
Fig. 4 is that the dynamic coordinate system of High Accuracy Inertial Navigation System reference point of the invention converts schematic diagram.
Fig. 5 is vehicle of the invention Parking floor map indoors.
Fig. 6 is the improvement stereo matching method step schematic diagram based on SIFT algorithm.
Description of symbols:
1. the measurement vehicle of nine axis inertial navigation measurement devices of-carrying, binocular camera and computer；
2.-navigation coordinate N；3.-indoor garage entrance；
4.-carrier coordinate system；5.-camera coordinate system；
6. the position of-subsequent time vehicle；7. the significant object of-parking garage corner feature；
8.-refer to angle point；
⑨—t0Moment binocular camera obtains the position of first characteristic point；
⑩—t1Moment binocular camera obtains the position of first characteristic point；
The measurement vehicle of nine axis inertial navigation measurement device of a-carrying, binocular camera and computer；
B-reference point corner feature significant building angle point；
C-chooses behind parking stall the navigation routine shown on garage high-precision map indoors by car owner；
D-car owner's selection parking position.
Specific embodiment
Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, detailed
Carefully describe.Described embodiment is only a part of the embodiments of the present invention.
The technical solution that the present invention solves above-mentioned technical problem is:
The purpose of the present invention can be achieved through the following technical solutions:
(1) High Accuracy Inertial Navigation System composition and reference coordinate architecture
Fig. 2 is High Accuracy Inertial Navigation System composition and reference frame structural schematic diagram of the invention, is 1. to take in figure
The vehicle for carrying the measurement of nine axis inertial navigation measurement devices, binocular camera and computer, it is aobvious being dispersed with numerous corner features
The parking garage scene of the object of work 7., wherein the high-precision map of parking garage has obtained.Coordinate system Oe-
xeyezeFor terrestrial coordinate system, in order to illustrate the effect that thinking of the invention is described in Fig. 2, not terrestrial coordinates actual bit
It sets.Coordinate (λ in figure0, L0, H0) it is the vehicle coordinate information that indoors 3. garage port point is obtained by high-precision GPS.It establishes and sits
2., i.e., system mark O is sat in navigation for mark systemn-xnynznWith world coordinate system Ow-xwywzw.After vehicle enters the room parking lot, binocular solid
Vision system passes through 8. angle point that binocular camera is the 1st significant building of corner feature to reference point and carries out mentioning for characteristic point
It takes and positions, obtain the location parameter 9. (x with reference to angle point initial time in camera coordinate systemw0yw0zw0), meanwhile, inertia
4. navigation measurement device starts to obtain vehicle in the location information of navigational coordinate system, be 5. carrier coordinate system Ob-xbybzb.Under
At the position of one moment vehicle 6., Binocular Stereo Vision System obtains the location information of this moment characteristic point 10. (xw1yw1zw1),
By the way that two moment characteristic point coordinate value differences are as a reference point, come position of the real-time update inertial navigation in navigational coordinate system
Confidence breath, and believed by the position in the map of high-precision parking garage that is converted to of terrestrial coordinate system and navigational coordinate system
Breath.Binocular stereo vision positioning coordinate is utilized to update inertial navigation system navigation position parameter elimination inertial navigation system to reach
Location error purpose.
Fig. 3 is the structural block diagram that the inertial navigation coordinate based on binocular vision updates, wherein inertial navigation mould group is by three axis
Acceleration transducer, three-axis gyroscope and three axle magnetometer composition, measurement obtain the parameter of each sensor, pass through Quaternion Algorithm
The coordinate system conversion obtained in inertial navigation system is resolved, integrates to obtain by rate integrating and position and carries inertial navigation mould group
The location parameter of carrier.Using the image information of binocular vision, inertial navigation reference position spy is determined by visual correlation algorithm
A sign point three-dimensional coordinate, by the way that current time characteristic point coordinate value is as a reference point come real-time update inertial navigation three-dimensional coordinate,
To eliminate the cumulative errors that inertial navigation is generated by integral.
(2) acquisition methods of inertial navigation navigation origin coordinates
Objects scene according to the present invention obtains the effective GPS information that can also be received before vehicle enters garage,
Including three-dimensional coordinate of the vehicle in the terrestrial coordinate system of garage port position, i.e., worked as using the acquisition of high-precision GPS mould group
Preceding longitude, latitude, height position information.The judgment method of starting point is required according to invention, and inertial navigation is led in the present invention
It is that Binocular Stereo Vision System can get the location information of characteristic point on vehicle that the starting point of boat, which needs the essential condition met,
I.e. vehicle can acquire the location information of the significant building angle point of first corner feature after garage port entrance.?
When into the very faint indoor garage of no GPS signal or GPS signal, vehicle using effective GPS information for finally obtaining as
Initial position carries out inertial navigation.But actually due to obtaining last effective GPS information after, due to environmental factor and binocular vision
Feel system may not be able to obtain the location information of characteristic point in time, and there are one section of delay times.In this process vehicle after
It continues and sails, need to utilize the effective GPS information (λ ' finally obtained0,L′0,H′0) based on, according to the vehicle of delay time and vehicle
Speed, course angle etc. calculate vehicle inertia navigation starting position coordinates (λ0,L0,H0) carry out interpolation.Assuming that delay time TX, according to
Speed, the course angle of vehicle calculate the accurate location of vehicle, i.e. interpolation delay time TXError position (Δ λ, the Δ of interior vehicle
L, Δ H), the inertial navigation starting position coordinates finally obtained are (λ0, L0, H0)=(λ '0+ Δ λ, L '0+ Δ L, H '0+ΔH).It examines
Consider in practical parking garage, vehicle Binocular Stereo Vision System after entering parking lot obtains the process time one of characteristic point
As be not it is very long, i.e., corresponding delay time is very short, and car speed is very low, then the inertial navigation obtained by interpolating method rises
Beginning position coordinates error very little.
(3) selection method of the high-precision fixed potential reference point of binocular
Angle point is image important feature, is played a very important role to the understanding and analysis of image graphics.Angle point is being protected
While staying image graphics important feature, the data volume of information can be effectively reduced, keep the content of its information very high, effectively
The speed for improving calculating is conducive to the reliable matching of image, so that being treated as possibility in real time.Angle point is in 3 D scene rebuilding
Estimation, the computer vision fields such as target following, target identification, image registration and matching play very important effect.
The Choice of the object of reference of parking garage vehicle binocular positioning has very much, and the present invention selects parking lot indoors
It is generally existing and easy distinguish significant with the corner feature of feature extraction corner or turning as binocular high accuracy positioning
Reference point.After vehicle enters the room parking lot from garage, the binocular camera on vehicle is chosen on vehicle heading most
The close significant object of a corner feature positions the references object for extracting characteristic point as binocular, and passes through base shown in fig. 6
Feature point extraction is carried out in the improvement stereo matching method of SIFT algorithm, nearest a little positioning as binocular in characteristic point is selected to join
Examination point, and obtain three-dimensional coordinate of this point in camera coordinate system.Utilize the coordinate information of this point to inertia below
Navigation carries out error correction.
(4) selection and design of high-precision inertial navigation coordinate system
The present invention, navigation coordinate in a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope
System, such as Fig. 2 consist of the following components:
First, terrestrial coordinate system Oe-xeyeze, terrestrial coordinate system and the earth keep synchronous rotary, zeAxis is directed toward regional rotation
Axis, xeAxis is directed toward the intersection point of the first meridian and equator, yeAxis is under the line in plane and xeAxis and zeAxis constitutes right-handed coordinate system；
Second, system mark O is sat in navigationn-xnynzn, origin O is overlapped with carrier, that is, vehicle center of gravity, and reference axis is respectively directed to
North orientation, east orientation and day to；
Third, carrier coordinate system Ob-xbybzb, using the coordinate system that vehicle's center of gravity is created as origin, wherein xbAxis is directed toward carrier
Front, ybAxis is along carrier longitudinal direction, zbThe plotted of axis direction carrier；
4th, camera coordinate system Oc-xcyczc, using the optical center of camera as coordinate origin, zcAxis and image coordinate system plane
Vertically, ycAxis is vertically downward；
5th, world coordinate system Ow-xwywzw, world coordinate system describes the relative positional relationship of object in actual environment, it
Image pixel coordinates system and physical coordinates system, camera coordinate system and space object are associated together, the world in the present invention
Coordinate system selection is overlapped with navigational coordinate system in inertial navigation system；
It additionally include image pixel coordinates system and image physical coordinates system.
(5) conversion method of high-precision inertial navigation coordinate system
Inertial navigation coordinate transform in a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope
Method enables rigid body surround fixed axle center and does the referred to as basic rotation of the movement once rotated, if space coordinates analogized to just
Body, then Angle Position relationship between two coordinate systems can be indicated by repeatedly basic rotation, then the conversion square between coordinate system
Battle array can be arrived by the way that the company of the basic spin matrix of rigid body is multiplied.
First, carrier coordinate system and navigational coordinate system are converted: by posture transformational analogy of the carrier in space into carrier
Coordinate system b to navigational coordinate system n is obtained around pitch axis, roll axis and course axis by 3 basic rotation transformations:
Second, the conversion between camera coordinate system and world coordinate system: according to invention demand and facilitating calculating, at this
World coordinate system is selected to be overlapped with navigational coordinate system in invention, as shown in Figure 2.Seat of the space characteristics point in camera coordinate system
Mark (xc,yc,zc) by being converted to coordinate (x of the space characteristics point in world coordinate systemw,yw,zw) special to get space has been arrived
Coordinate points (x of the sign point in navigational coordinate systemn,yn,zn), conversion process are as follows: space characteristics point is in world coordinate system and camera shooting
Homogeneous coordinates under machine coordinate system are respectively (xw,yw,zw, 1) and (xc,yc,zc, 1), wherein homogeneous coordinate system is exactly to be dimension
The vector of n indicates that coordinate value of the origin of world coordinate system in camera coordinate system is t=with the vector that (n+1) is tieed up
(tx,ty,tz)T, the vector of this 3 dimension is translation vector, and direction vector R is that camera coordinate system is used in world coordinate system
The direction vector for describing the relationship of the two is got up 3 × 3 orthogonal matrix, then the world by the sine and cosine composition of relations of three attitude angles
Relationship between coordinate system and camera coordinate system isWherein OT=(0,0,0)；
Third, the conversion between terrestrial coordinate system and navigational coordinate system: first by terrestrial coordinate system OexeyezeAround zeAxis rotation
Turn the angle λ, obtains Oe1-xe1ye1ze1.Then around ye1Axis is rotated by 90 ° the angle-L, obtains Oe2-xe2ye2ze2.Finally around ze2Axis revolves 90 °.Through
Crossing above step can be by terrestrial coordinate system Oe-xeyeze, it is transformed into navigational coordinate system On-xnynzn, phase between Two coordinate system
Mutual transformation can pass through matrixTo indicate.
(6) removing method of inertial navigation cumulative errors:
Premise based on air navigation aid of the invention is the high-precision map of existing indoor garage, indoors garage high-precision
The relevant information that each parking stall can be accurately shown in map, location information including parking stall and whether be empty parking space, car owner can lead to
Crossing service system selects suitable empty parking space to park the vehicle of oneself.After car owner chooses parking stall, garage is accurately indoors
Navigation routine is shown on figure.The process such as Fig. 5, Fig. 5 for wherein simulating parking are parking garage parking floor map.Wherein a
For the vehicle for being equipped with inertial navigation sensors and binocular camera, b is that the corner feature that is distributed in parking lot indoors is significant
Building, c are that the navigation routine shown on garage high-precision map indoors is chosen behind parking stall by car owner, and d is car owner's selection
Parking position.
Fig. 2 is High Accuracy Inertial Navigation System composition and reference frame structural schematic diagram, with reference angle point 8. for coordinate
Origin establishes dynamic coordinate system.Assuming that in t0=0 moment, vehicle are by the location information that high-precision GPS obtains in garage port
(λ0, L0, H0), and the feature angle point of the significant building of first corner feature is detected by binocular vision system at this moment
8. position, while calculating coordinate (x of the angle point under world coordinate systemw0, yw0, zw0), then it is built using feature angle point as coordinate
In vertical coordinate system, the current position of vehicle is (- xw0,-yw0,-zw0), under current time in inertial navigation system, vehicle is being led
The coordinate to navigate under coordinate system is (xn0, yn0,zn0).After a period of time has passed, t1When inscribe, binocular vision system detects this
The position of a feature angle point 8., and calculate t1Coordinate (x under moment under the alive boundary's coordinate system of angle pointw1, yw1, zw1), in Yi Te
It levies in the coordinate system that angle point is coordinate foundation, the current position of vehicle is (- xw1,-yw1,-zw1), inertial navigation is descended at this moment
System obtains the position (x of vehiclen1,yn1,zn1).Obtain front t0、t1The location information at two moment, then by binocular tri-dimensional
The location information that feel system obtains obtains t1Moment, vehicle was in t0Location information under moment world coordinate system, i.e. (xw1-xw0,yw1-
yw0,zw1-zw0).In the present invention, the world coordinate system of selection and navigational coordinate system are the same coordinate systems, then by binocular vision system
It is (x that unified test, which obtains coordinate of the vehicle in navigational coordinate system,n1′,yn1′,zn1')=(xw1-xw0,yw1-yw0, zw1-zw0).Using leading
Location information (x under boat coordinate systemn1', yn1', zn1') replace inertial navigation system vehicle in t1Position in moment navigational coordinate system
Confidence ceases (xn1, yn1, zn1), that is, it has updated location information of the inertial navigation system vehicle in navigational coordinate system, is eliminated with this
From t0Moment t1The cumulative errors of inertial navigation.By the conversion of terrestrial coordinate system and navigational coordinate system, obtains vehicle and stop indoors
Location information (λ in the high-precision map of parking lot1,L1, H1).Vehicle continues to travel, in t2Moment, with parking garage high-precision
Position (λ in map1, L1, H1) it is starting point, and with (λ1, L1,H1) it is that origin establishes opposite terrestrial coordinate system, same side
Method is eliminated in t2The cumulative errors of moment inertial navigation system, are similarly obtained in t2Parking lot is high-precision indoors for moment vehicle
Spend the position (λ in map2,L2,H2).The removing method of the cumulative errors of later point inertial navigation system is with previous instant
Method is identical, wherein t0、t1、t2The time at intervals time is equal.
When in tnMoment Binocular Stereo Vision System detects that the corner feature of position from first of vehicle is significantly built
The distance s of objectn< p, wherein p is the threshold value set according to actual requirement, as shown in figure 4, tnMoment binocular stereo vision system
System detects that the location information of first angle point is (xwn,ywn,zwn), and the nearest angle point of next distance is obtained at this moment
The corner location information of the building of characteristic remarkable, the i.e. location information (x of the significant building of second corner featurewn′,
ywn′,zwn′).The location information in high-precision map is by previous moment t indoorsn-1The vehicle position information at moment is updated
It obtains, is (λn,Ln, Hn).In tn+1The significant building of second corner feature that moment Binocular Stereo Vision System obtains
Corner location information (xwn+1, ywn+1, zwn+1) and last moment obtain location information (xwn′,ywn′,zwn'), update tn+1Moment
Location information (the λ of vehiclen+1, Ln+1, Hn+1).In later point, when vehicle driving is less than to the significant building of digression point feature
When the threshold value of one setting, replacement is carried out with reference to angle point by algorithm above.Error and the binocular positioning that system generates
Precision is related, not related with inertial navigation, and vehicle, during inertial navigation, distance reference point is closer, binocular positioning
Precision is higher, i.e. the precision of whole system is higher, with this come eliminate inertial navigation due to integral generate cumulative errors.
The purpose of the present invention is determine inertial navigation reference position characteristic point three based on binocular positioning+high-precision GPS information
Tie up coordinate, it is as a reference point come real-time update inertial navigation three-dimensional coordinate by characteristic point coordinate value, come eliminate inertial navigation by
In the cumulative errors that integral generates, accurate the determining based on inertial navigation and binocular positioning combination auxiliary parking garage vehicle is realized
Position.To reach this purpose, specific embodiment is as follows:
According to the actual situation, vehicle is entering parking lot and is completing the entire action process to stop, present invention firstly provides
The acquisition methods of inertial navigation navigation origin coordinates.The starting point needs of inertial navigation navigation in the present invention meet important
Condition is that Binocular Stereo Vision System can get the location information of characteristic point on vehicle, i.e. vehicle is from energy after garage port entrance
Enough acquire the location information of first angle point.In the indoor garage very faint into no GPS signal or GPS signal
When, vehicle carries out inertial navigation as initial position using the effective GPS information finally obtained.But it is actually last due to obtaining
After effective GPS information, since environmental factor and binocular vision system may not be able to obtain the location information of characteristic point in time, exist
One section of delay time.Vehicle is continuing to travel in this process, needs to utilize the effective GPS information (λ ' finally obtained0, L '0,
H′0) based on, vehicle inertia navigation starting position coordinates (λ is calculated according to the speed of delay time and vehicle, course angle etc.0,
L0,H0) carry out interpolation.Assuming that delay time TX, the accurate location of vehicle, i.e. interpolation are calculated according to the speed of vehicle, course angle
Delay time TXThe error position (Δ λ, Δ L, Δ H) of interior vehicle, the inertial navigation starting position coordinates finally obtained are (λ0,
L0,H0)=(λ '0+Δλ,L′0+ΔL,H′0+ΔH)。
After vehicle obtains navigation starting point, selected for needing to utilize in inertial navigation high-precision locating method of the invention
Reference point establishes relative coordinate system to realize binocular high accuracy positioning, and the present invention provides the selection of the high-precision fixed potential reference point of binocular
Method.
Angle point can be effectively reduced the data volume of information, make its information while retaining image graphics important feature
Content it is very high, effectively improve the speed of calculating, be conducive to the reliable matching of image, so that being treated as possibility in real time.
The Choice of the object of reference of parking garage vehicle binocular positioning has very much, and parking lot is generally existing indoors for present invention selection
And it is easy to distinguish the reference point of corner significant with the corner feature of feature extraction or turning as binocular high accuracy positioning.When
After vehicle enters the room parking lot from garage, the binocular camera on vehicle chooses the nearest angle on vehicle heading
The significant object of point feature positions the references object for extracting characteristic point as binocular, and is based on SIFT algorithm by shown in fig. 6
Improvement stereo matching method carry out feature point extraction, select in characteristic point a bit nearest as binocular location reference point, and obtain
Take three-dimensional coordinate of this point in camera coordinate system.Inertial navigation is missed using the coordinate information of this point below
Difference correction.
Followed by Binocular Stereo Vision System to the feature point extraction of ambient enviroment and as the essence of the binocular in the present invention
Spend the reference point of positioning, the extraction of binocular location feature point and localization method.The present invention applies a kind of changing based on indoor parking
Into SIFT matching process, i.e., according to corner positional relationship in the picture independently to limit ROI (Region of
Interest) region is matched in a manner of area-of-interest, accumulated when vehicle driving by position, can be completed pair
The positioning and tracking of ROI region.Hereafter, only the image in ROI region can be matched, greatly improves matched essence
Degree, speed and accuracy rate, matched stability and robustness are greatly enhanced.It is vertical using a kind of improvement based on SIFT algorithm
Body matching method is applied to the feature point extraction and positioning of indoor parking navigation, and Fig. 6 is the improvement Stereo matching based on SIFT algorithm
Method step schematic diagram, first by carrying out epipolar-line constraint to the left images of acquisition, next selection target region from left figure
ROI, then feature vector dimension is reduced to reduce time-consuming, matching speed is accelerated using the BBF algorithm based on KD tree, is finally used
RANSAC algorithm removes error hiding.
The update selection that the high-precision fixed potential reference point of binocular during the entire process of parking is completed for vehicle, with reference to Fig. 4 institute
Show, provides mode in detail below: in t0=0 moment, binocular vision system detect the significant object of nearest corner feature, i.e.,
Location information (x of the angle point of the significant object of 1 corner feature in camera coordinate systemc0,yc0,zc0)；In t0Moment adds
The inertial navigation system that speedometer, gyroscope form is with initial position (λ0, L0, H0) it is that starting point starts to navigate, in t1Moment obtains
Obtain the acceleration a of each axis under carrier coordinate systemx、ay、azWith attitude angle γ, θ,In t1Moment, binocular vision system detection
To location information (x of the angle point in camera coordinate system of the 1st significant object of corner featurec1,yc1,zc1)；In t2When
It carves, the vehicle that inertial navigation high-accuracy position system has last moment the to resolve position in parking lot high-precision map indoors
Confidence ceases (λ1, L1, H1) it is starting point, the data acquisition of above step is continued to complete, when in tnThe inspection of moment Binocular Stereo Vision System
The position of vehicle is measured from the significant object distance s of the 1st corner featuren< p, wherein p is one set according to actual requirement
A threshold value, tnMoment Binocular Stereo Vision System detect the significant object angle point of the 1st corner feature in camera coordinates
Location information in system is (xcn,ycn,zcn), and the significant object of the nearest corner feature of next distance is obtained at this moment
Corner location information, i.e., the location information (x of the 2nd significant object of corner featurecn′,ycn′,zcn′).High-precision indoors
Location information in map is by previous moment tn-1The vehicle position information at moment is updated to obtain, and is (λn, Ln, Hn).In tn+1
The significant object corner location information (x of second corner feature that moment Binocular Stereo Vision System obtainscn+1,ycn+1,zcn+1)
Location information (the x obtained with last momentcn′,ycn′,zcn'), by the conversion between coordinate system, it is alive to obtain each characteristic point
Location information in boundary's coordinate system (navigational coordinate system), and by data processing, update tn+1The location information of moment vehicle
(λn+1,Ln+1,Hn+1).In later point, when the threshold value that vehicle driving to the significant object of digression point feature is set less than one
When, the significant object of replacement corner feature is carried out with reference to angle point, until vehicle reaches target parking stall by algorithm above.
Referring to shown in Fig. 2, the selection of high-precision inertial navigation coordinate system consists of the following components with design in the present invention:
First, terrestrial coordinate system Oe-xeyeze, terrestrial coordinate system and the earth keep synchronous rotary, zeAxis is directed toward regional rotation
Axis, xeAxis is directed toward the intersection point of the first meridian and equator, yeAxis is under the line in plane and xeAxis and zeAxis constitutes right-handed coordinate system；
Second, system mark O is sat in navigationn-xnynzn, origin O is overlapped with carrier, that is, vehicle center of gravity, and reference axis is respectively directed to
North orientation, east orientation and day to；
Third, carrier coordinate system Ob-xbybzb, using the coordinate system that vehicle's center of gravity is created as origin, wherein xbAxis is directed toward carrier
Front, ybAxis is along carrier longitudinal direction, zbThe plotted of axis direction carrier；
4th, camera coordinate system Oc-xcyczc, using the optical center of camera as coordinate origin, zcAxis and image coordinate system plane
Vertically, ycAxis is vertically downward；
5th, world coordinate system Ow-xwywzw, world coordinate system describes the relative positional relationship of object in actual environment, it
Image pixel coordinates system and physical coordinates system, camera coordinate system and space object are associated together, the world in the present invention
Coordinate system selection is overlapped with navigational coordinate system in inertial navigation system.
For in the present invention, conversion between each coordinate system provides the conversion method of high-precision inertial navigation coordinate system,
Mainly include: inertial navigation system coordinate transform carries out attitude algorithm, speed update and position by collecting acceleration and attitude angle
It sets update: acquiring the instantaneous velocity v of carrier movement respectively using integralx、vyAnd vzAnd instantaneous position x, y and z, the appearance of carrier
Three rotation angles when state is exactly that carrier is under body coordinate system, relative to navigational coordinate systemSize.This hair
The bright update that posture is carried out using Quaternion Method, by solving transition matrixCarrier coordinate system and navigational coordinate system can be realized
Conversion, obtain in tiLocation information (x of (i=0,1,2 ...) the moment vehicle under navigational coordinate systemni, yni, zni)；Binocular vision
The conversion for feeling system video cameras coordinate system and world coordinate system, by ti(i=0,1,2 ...) moment binocular vision system detects
Location information (x of the significant object angle point of corner feature in camera coordinate systemci,yci,zci), it obtains in world coordinate system
Ow-xwywzwPosition (the x of lower angle pointwi, ywi, zwi).Boundary's coordinate system describes the relative positional relationship of object in actual environment, it will
Image pixel coordinates system and physical coordinates system, camera coordinate system and space object are associated together.Space characteristics point is alive
Homogeneous coordinates under boundary's coordinate system and camera coordinate system are respectively (xw, yw, zw, 1) and (xc, yc, zc, 1), wherein homogeneous coordinates
System is exactly that the vector that dimension is n is indicated that the origin of world coordinate system is in camera coordinate system with the vector that (n+1) is tieed up
Coordinate value is t=(tx, ty, tz)T, the vector of this 3 dimension is translation vector, and direction vector R is that camera coordinate system is sat in the world
It is used to describe the direction vector of the relationship of the two in mark system, is got up 3 × 3 orthogonal moment by the sine and cosine composition of relations of three attitude angles
Battle array, then the relationship between world coordinate system and camera coordinate system isWherein OT=(0,0,0)；Ground
The conversion of spherical coordinate system and navigational coordinate system, first by terrestrial coordinate system OexeyezeAround zeAxis rotates the angle λ, obtains Oe1-
xe1ye1ze1.Then around ye1Axis is rotated by 90 ° the angle-L, obtains Oe2-xe2ye2ze2.Finally around ze2Axis revolves 90 °.It can by above step
To realize terrestrial coordinate system Oe-xeyezeWith navigational coordinate system On-xnynzn, the transformation between Two coordinate system can pass through matrixIt indicates, L in formula, λ respectively represent local latitude and local longitude.
The location error for utilizing inertial navigation system to obtain indoors for vehicle, the present invention provide inertial navigation accumulative mistake
Difference removing method, embodiment the following steps are included:
First, referring to Fig. 2 High Accuracy Inertial Navigation System composition and reference frame structural schematic diagram, 8. with reference angle point
Dynamic coordinate system is established for coordinate origin.Assuming that in t0At=0 moment, vehicle is in the position that garage port is obtained by high-precision GPS
Information is (λ0,L0,H0), and the significant object features angle of first corner feature is detected by binocular vision system at this moment
The position of point 8., while calculating coordinate (x of the significant object angle point of corner feature under world coordinate systemw0,yw0,zw0), then
In the coordinate system established using feature angle point as coordinate, the current position of vehicle is (- xw0,-yw0,-zw0), it is used under current time
Property navigation system in, coordinate of the vehicle under navigational coordinate system be (xn0,yn0,zn0).After a period of time has passed, t1When inscribe,
Binocular vision system detects the position of the significant object features angle point of this corner feature 8., and calculates t1Angle point under moment
Coordinate (x of the object angle point of characteristic remarkable under world coordinate systemw1,yw1,zw1), in the seat established using feature angle point as coordinate
In mark system, the current position of vehicle is (- xw1,-yw1,-zw1), descend inertial navigation system to obtain the position of vehicle at this moment
(xn1, yn1, zn1).Obtain front t0、t1The location information at two moment is then believed by the position that Binocular Stereo Vision System obtains
Breath obtains t1Moment, vehicle was in t0Location information under moment world coordinate system, i.e. (xw1-xw0, yw1-yw0,zw1-zw0).In this hair
In bright, the world coordinate system and navigational coordinate system of selection are the same coordinate systems, then measure vehicle by binocular vision system and navigating
Coordinate in coordinate system is (xn1′,yn1′,zn1')=(xw1-xw0,yw1-yw0,zw1-zw0).Utilize navigational coordinate system bottom confidence
Cease (xn1′,yn1′,zn1') replace inertial navigation system vehicle in t1Location information (x in moment navigational coordinate systemn1, yn1,
zn1), that is, it has updated location information of the inertial navigation system vehicle in navigational coordinate system, is eliminated with this from t0Moment t1Inertia
The cumulative errors of navigation.By the conversion of terrestrial coordinate system and navigational coordinate system, obtaining vehicle, parking lot is accurately indoors
Location information (λ in figure1, L1, H1)。
Second, vehicle continues to travel, in t2Moment, with the position (λ in the high-precision map of parking garage1, L1, H1) be
Starting point, and with (λ1, L1,H1) it is that origin establishes opposite terrestrial coordinate system, same method is eliminated in t2Moment inertia is led
The cumulative errors of boat system, are similarly obtained in t2The moment vehicle position (λ in parking lot high-precision map indoors2,L2,H2)。
The removing method of the cumulative errors of later point inertial navigation system is identical with the method for previous instant, wherein t0、t1、t2Moment
Interval time is equal.
Third, when in tnMoment Binocular Stereo Vision System detects that the corner feature of position from first of vehicle is significant
The distance s of objectn< p, wherein p is the threshold value set according to actual requirement, as Fig. 4 High Accuracy Inertial Navigation System refers to
Shown in the dynamic coordinate system transformation schematic diagram of point, tnMoment Binocular Stereo Vision System detects that first corner feature is significant
Object angle point location information be (xwn,ywn, zwn), and it is significant to obtain the nearest corner feature of next distance at this moment
Object corner location information, i.e. location information (the x of the significant object of second corner featurewn', ywn', zwn′).In room
Location information in interior high-precision map is by previous moment tn-1The vehicle position information at moment is updated to obtain, and is (λn, Ln,
Hn).In tn+1The significant object corner location information (x of second corner feature that moment Binocular Stereo Vision System obtainswn+1,
ywn+1,zwn+1) and last moment obtain location information (xwn′,ywn′,zwn'), update tn+1The location information of moment vehicle
(λn+1, Ln+1, Hn+1).In later point, when the threshold value that vehicle driving to the significant object of digression point feature is set less than one
When, replacement object of reference is carried out by algorithm above and with reference to angle point.
Constantly update ti(i=0,1,2 ...) moment vehicle position in high-precision map indoors, until vehicle reaches mesh
Mark parking stall.
The above embodiment is interpreted as being merely to illustrate the present invention rather than limit the scope of the invention.?
After the content for having read record of the invention, technical staff can be made various changes or modifications the present invention, these equivalent changes
Change and modification equally falls into the scope of the claims in the present invention.
Claims (8)
1. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope, which is characterized in that including following
Step:
Step 1 is established the high-precision inertia system based on inertial navigation mould group, Binocular Stereo Vision System and vehicle, and is established
Reference coordinate architecture；
The acquisition of step 2, inertial navigation origin coordinates: vehicle carries out inertial navigation using origin coordinates point as starting point, and adopts
With the improvement stereo matching method based on SIFT algorithm, the selected sense of positional relationship significantly put according to corner feature in the picture is emerging
The region of interest, vehicle complete positioning and tracking to area-of-interest by the accumulation of continuous position in the process of moving, hereafter
During can only to interested region carry out images match, complete initial time binocular vision feature point extraction, reach
High, the fireballing purpose to matching precision；
The selection of the high-precision fixed potential reference point of step 3, binocular: selection indoors the significant corner of corner feature in parking lot or
Reference frame is established by origin of reference point as the reference point of binocular high accuracy positioning in turning；
The selection and design of step 4, high-precision inertial navigation coordinate system: selection includes during the navigation process terrestrial coordinate system, leads
Coordinate system including boat coordinate system, carrier coordinate system, camera coordinate system, world coordinate system, meets finally to dependent coordinate
It resolves；
The conversion of step 5, high-precision inertial navigation coordinate system: characteristic point is obtained in the location information of world coordinate system, vehicle exists
Conversion between location information and world coordinate system in inertial navigation navigational coordinate system and inertial navigation navigational coordinate system is closed
System；
The elimination of step 6, inertial navigation cumulative errors: determine that inertial navigation refers to based on binocular positioning and high-precision GPS information
Position feature point three-dimensional coordinate, it is as a reference point come real-time update inertial navigation three-dimensional coordinate by characteristic point coordinate value, to disappear
Except the cumulative errors that inertial navigation is generated due to integral, realizes and parking garage is assisted based on inertial navigation and binocular positioning combination
The accurate positioning of vehicle.
2. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 1,
It is characterized in that, the inertial navigation mould group of the step 1 is by 3-axis acceleration sensor, three-axis gyroscope and three axle magnetometer group
At measurement obtains the parameter of each sensor, resolves to obtain the conversion of the coordinate system in inertial navigation system by Quaternion Algorithm, leads to
It crosses rate integrating and position integrates to obtain the location parameter for carrying inertial navigation mould group carrier, believed using the image of binocular vision
Breath, determines inertial navigation reference position characteristic point three-dimensional coordinate by visual correlation algorithm, by by current time and upper a period of time
Carve characteristic point coordinate value difference it is as a reference point come real-time update inertial navigation three-dimensional coordinate, come eliminate inertial navigation by integral produce
Raw cumulative errors.
3. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 1,
It is characterized in that, the acquisition of the step 2 origin coordinates point specifically includes: very micro- into no GPS signal or GPS signal
When weak indoor garage, vehicle judges the binocular tri-dimensional on the vehicle of starting point with the effective GPS information basis finally obtained
Whether feel system can get the location information of feature angle point, by the speed of the delay time of this process and vehicle, course angle
Information etc. carries out location information interpolation, obtains inertial navigation navigation origin coordinates, and vehicle is carried out by initial position of origin coordinates
Inertial navigation.
4. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 3,
It is characterized in that, the step 2 is specifically included based on the improvement stereo matching method step of SIFT algorithm: first by acquisition
Left images carry out epipolar-line constraint, and ROI region is secondly selected from left figure, reduce time-consuming reducing feature vector dimension, adopt
Accelerate matching speed with the BBF algorithm based on KD tree, finally removes error hiding with RANSAC algorithm.
5. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 4,
It is characterized in that, the Local modulus maxima that feature of the step 3 using angle point, i.e. angle point are curvature on objective contour, is object
The determinant attribute of body profile, and angle point also has rotational invariance, after vehicle enters the room parking lot from garage, vehicle
On binocular camera choose the nearest significant object of a corner feature on vehicle heading and positioned as binocular and mention
The references object for taking characteristic point, the angle point that will acquire is as the high-precision fixed potential reference point of binocular.
6. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 1,
It is characterized in that, the step 5 further includes the conversion between following coordinate system:
The first, carrier coordinate system and navigational coordinate system is converted: by posture transformational analogy of the carrier in space into carrier coordinate
It is that b to navigational coordinate system n is obtained around pitch axis, roll axis and course axis by 3 basic rotation transformations:
Second, the conversion between camera coordinate system and world coordinate system: selection world coordinate system is overlapped with navigational coordinate system, empty
Between coordinate (x of the characteristic point in camera coordinate systemc,yc,zc) by being converted to space characteristics point in world coordinate system
Coordinate (xw,yw,zw) to get having arrived coordinate points (x of the space characteristics point in navigational coordinate systemn,yn,zn):
Third, the conversion between terrestrial coordinate system and navigational coordinate system: first by terrestrial coordinate system OexeyezeAround zeAxis rotates λ
Angle obtains Oe1-xe1ye1ze1.Then around ye1Axis is rotated by 90 ° the angle-L, obtains Oe2-xe2ye2ze2.Finally around ze2Axis revolves 90 °, passes through
Above step can be by terrestrial coordinate system Oe-xeyeze, it is transformed into navigational coordinate system On-xnynzn, mutual between Two coordinate system
Transformation can pass through matrixTo indicate.
7. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 6,
It is characterized in that, coordinate (x of the space characteristics point in camera coordinate systemc,yc,zc) by being converted to space characteristics
Coordinate (x of the point in world coordinate systemw,yw,zw), it specifically includes:
Homogeneous coordinates of the space characteristics point under world coordinate system and camera coordinate system are respectively (xw,yw,zw, 1) and (xc,yc,
zc, 1), wherein homogeneous coordinate system is exactly that the vector that dimension is n is indicated that the origin of world coordinate system exists with the vector that n+1 is tieed up
Coordinate value in camera coordinate system is t=(tx,ty,tz)T, the vector of this 3 dimension is translation vector, and direction vector R is camera shooting
Machine coordinate system is used to describe the direction vector of the relationship of the two in world coordinate system, by the sine and cosine composition of relations of three attitude angles
Get up 3 × 3 orthogonal matrix, then the relationship between world coordinate system and camera coordinate system be
Wherein R indicates 3 × 3 spin matrix, OT=(0,0,0).
8. a kind of inertial navigation high-precision locating method based on binocular, acceleration and gyroscope according to claim 1,
It is characterized in that, the elimination of the step 6 inertial navigation cumulative errors: by binocular stereo vision positioning and high-precision GPS information
Determine inertial navigation starting point coordinate, vehicle is opened used when into no GPS signal or GPS signal very faint region
Property navigation mode, the location information of the significant corner of corner feature or inflection point is obtained using Binocular Stereo Vision System, and will
The location information of characteristic point is as a reference point to establish three-dimensional reference frame, is obtained according to different moments vehicle in different location
Same reference point locations information, the difference to adjacent moment vehicle location converted by coordinate update vehicle location with this.
Vehicle chooses whether to replace by the distance and the reference point minimum threshold of distance of setting that judge reference point in the process of moving
Object of reference and reference point, final accurate the determining realized based on inertial navigation and binocular positioning combination auxiliary parking garage vehicle
Position.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811632825.XA CN109631887B (en) | 2018-12-29 | 2018-12-29 | Inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811632825.XA CN109631887B (en) | 2018-12-29 | 2018-12-29 | Inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109631887A true CN109631887A (en) | 2019-04-16 |
| CN109631887B CN109631887B (en) | 2022-10-18 |
Family
ID=66079243
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811632825.XA Active CN109631887B (en) | 2018-12-29 | 2018-12-29 | Inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109631887B (en) |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110118987A (en) * | 2019-04-28 | 2019-08-13 | 桂林电子科技大学 | A kind of positioning navigation method, device and storage medium |
| CN110823225A (en) * | 2019-10-29 | 2020-02-21 | 北京影谱科技股份有限公司 | Positioning method and device under indoor dynamic situation |
| CN111121768A (en) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | Robot pose estimation method and device, readable storage medium and robot |
| CN111721282A (en) * | 2020-05-09 | 2020-09-29 | 中国人民解放军63686部队 | Strapdown inertial navigation coordinate system dynamic alignment method based on astronomical navigation principle |
| CN111795695A (en) * | 2020-05-15 | 2020-10-20 | 北京百度网讯科技有限公司 | Position information determining method, device and equipment |
| CN112097767A (en) * | 2020-10-15 | 2020-12-18 | 杭州知路科技有限公司 | Pre-integration auxiliary assembly for inertial navigation and data processing method |
| CN112179336A (en) * | 2019-07-02 | 2021-01-05 | 南京理工大学 | Automatic luggage transportation method based on binocular vision and inertial navigation combined positioning |
| CN112249089A (en) * | 2020-09-27 | 2021-01-22 | 卡斯柯信号有限公司 | Rail transit emergency positioning system and method |
| CN112304302A (en) * | 2019-07-26 | 2021-02-02 | 北京初速度科技有限公司 | Multi-scene high-precision vehicle positioning method and device and vehicle-mounted terminal |
| CN111220118B (en) * | 2020-03-09 | 2021-03-02 | 燕山大学 | Laser range finder based on visual inertial navigation system and range finding method |
| CN112550377A (en) * | 2020-12-18 | 2021-03-26 | 卡斯柯信号有限公司 | Rail transit emergency positioning method and system based on video identification and IMU (inertial measurement Unit) equipment |
| CN112556720A (en) * | 2019-09-25 | 2021-03-26 | 上海汽车集团股份有限公司 | Vehicle inertial navigation calibration method and system and vehicle |
| CN112598705A (en) * | 2020-12-17 | 2021-04-02 | 太原理工大学 | Vehicle body posture detection method based on binocular vision |
| CN112614219A (en) * | 2020-12-07 | 2021-04-06 | 灵鹿科技(嘉兴)股份有限公司 | Spatial coordinate conversion method based on identification points for map navigation positioning |
| CN112698051A (en) * | 2021-03-23 | 2021-04-23 | 天津所托瑞安汽车科技有限公司 | Vehicle speed determination method and device, equipment and medium |
| CN112697131A (en) * | 2020-12-17 | 2021-04-23 | 中国矿业大学 | Underground mobile equipment positioning method and system based on vision and inertial navigation system |
| CN112598705B (en) * | 2020-12-17 | 2024-05-03 | 太原理工大学 | Binocular vision-based vehicle body posture detection method |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5128874A (en) * | 1990-01-02 | 1992-07-07 | Honeywell Inc. | Inertial navigation sensor integrated obstacle detection system |
| CN102435172A (en) * | 2011-09-02 | 2012-05-02 | 北京邮电大学 | Visual locating system of spherical robot and visual locating method thereof |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN102768042A (en) * | 2012-07-11 | 2012-11-07 | 清华大学 | Visual-inertial combined navigation method |
| CN103278177A (en) * | 2013-04-27 | 2013-09-04 | 中国人民解放军国防科学技术大学 | Calibration method of inertial measurement unit based on camera network measurement |
| CN103438904A (en) * | 2013-08-29 | 2013-12-11 | 深圳市宇恒互动科技开发有限公司 | Inertial positioning method and system using vision-aided correction |
| CN104848858A (en) * | 2015-06-01 | 2015-08-19 | 北京极智嘉科技有限公司 | Two-dimensional code and vision-inert combined navigation system and method for robot |
| WO2016032886A1 (en) * | 2014-08-25 | 2016-03-03 | Daqri, Llc | Distributed aperture visual inertia navigation |
| CN105652305A (en) * | 2016-01-08 | 2016-06-08 | 深圳大学 | Three-dimensional positioning and attitude-determining method and system for track detection platform in dynamic environment |
| CN106525049A (en) * | 2016-11-08 | 2017-03-22 | 山东大学 | Quadruped robot body posture tracking method based on computer vision |
| CN107590827A (en) * | 2017-09-15 | 2018-01-16 | 重庆邮电大学 | A kind of indoor mobile robot vision SLAM methods based on Kinect |
| CN107600067A (en) * | 2017-09-08 | 2018-01-19 | 中山大学 | A kind of autonomous parking system and method based on more vision inertial navigation fusions |
| CN108481327A (en) * | 2018-05-31 | 2018-09-04 | 珠海市微半导体有限公司 | A kind of positioning device, localization method and the robot of enhancing vision |
| US20180365900A1 (en) * | 2017-06-20 | 2018-12-20 | Immerex Inc. | Mixed Reality Head Mounted Display Device |
-
2018
- 2018-12-29 CN CN201811632825.XA patent/CN109631887B/en active Active
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5128874A (en) * | 1990-01-02 | 1992-07-07 | Honeywell Inc. | Inertial navigation sensor integrated obstacle detection system |
| CN102435172A (en) * | 2011-09-02 | 2012-05-02 | 北京邮电大学 | Visual locating system of spherical robot and visual locating method thereof |
| CN102538781A (en) * | 2011-12-14 | 2012-07-04 | 浙江大学 | Machine vision and inertial navigation fusion-based mobile robot motion attitude estimation method |
| CN102768042A (en) * | 2012-07-11 | 2012-11-07 | 清华大学 | Visual-inertial combined navigation method |
| CN103278177A (en) * | 2013-04-27 | 2013-09-04 | 中国人民解放军国防科学技术大学 | Calibration method of inertial measurement unit based on camera network measurement |
| CN103438904A (en) * | 2013-08-29 | 2013-12-11 | 深圳市宇恒互动科技开发有限公司 | Inertial positioning method and system using vision-aided correction |
| WO2016032886A1 (en) * | 2014-08-25 | 2016-03-03 | Daqri, Llc | Distributed aperture visual inertia navigation |
| CN104848858A (en) * | 2015-06-01 | 2015-08-19 | 北京极智嘉科技有限公司 | Two-dimensional code and vision-inert combined navigation system and method for robot |
| CN105652305A (en) * | 2016-01-08 | 2016-06-08 | 深圳大学 | Three-dimensional positioning and attitude-determining method and system for track detection platform in dynamic environment |
| CN106525049A (en) * | 2016-11-08 | 2017-03-22 | 山东大学 | Quadruped robot body posture tracking method based on computer vision |
| US20180365900A1 (en) * | 2017-06-20 | 2018-12-20 | Immerex Inc. | Mixed Reality Head Mounted Display Device |
| CN107600067A (en) * | 2017-09-08 | 2018-01-19 | 中山大学 | A kind of autonomous parking system and method based on more vision inertial navigation fusions |
| CN107590827A (en) * | 2017-09-15 | 2018-01-16 | 重庆邮电大学 | A kind of indoor mobile robot vision SLAM methods based on Kinect |
| CN108481327A (en) * | 2018-05-31 | 2018-09-04 | 珠海市微半导体有限公司 | A kind of positioning device, localization method and the robot of enhancing vision |
Non-Patent Citations (2)
| Title |
|---|
| DAI YATING 等: "Asynchronous fusion in positioning of mobile robot based on vision-aided inertial navigation", 《2017 36TH CHINESE CONTROL CONFERENCE (CCC)》 * |
| 薛伟罗晨 等: "视觉辅助的INS/GNSS导航系统在动态场景下的特性分析", 《第九届中国卫星导航学术年会论文集》 * |
Cited By (22)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110118987A (en) * | 2019-04-28 | 2019-08-13 | 桂林电子科技大学 | A kind of positioning navigation method, device and storage medium |
| CN112179336B (en) * | 2019-07-02 | 2023-08-18 | 南京理工大学 | Automatic luggage transportation method based on binocular vision and inertial navigation combined positioning |
| CN112179336A (en) * | 2019-07-02 | 2021-01-05 | 南京理工大学 | Automatic luggage transportation method based on binocular vision and inertial navigation combined positioning |
| WO2021017212A1 (en) * | 2019-07-26 | 2021-02-04 | 魔门塔(苏州)科技有限公司 | Multi-scene high-precision vehicle positioning method and apparatus, and vehicle-mounted terminal |
| CN112304302A (en) * | 2019-07-26 | 2021-02-02 | 北京初速度科技有限公司 | Multi-scene high-precision vehicle positioning method and device and vehicle-mounted terminal |
| CN112304302B (en) * | 2019-07-26 | 2023-05-12 | 北京魔门塔科技有限公司 | Multi-scene high-precision vehicle positioning method and device and vehicle-mounted terminal |
| CN112556720B (en) * | 2019-09-25 | 2023-08-18 | 上海汽车集团股份有限公司 | Vehicle inertial navigation calibration method and system and vehicle |
| CN112556720A (en) * | 2019-09-25 | 2021-03-26 | 上海汽车集团股份有限公司 | Vehicle inertial navigation calibration method and system and vehicle |
| CN110823225A (en) * | 2019-10-29 | 2020-02-21 | 北京影谱科技股份有限公司 | Positioning method and device under indoor dynamic situation |
| CN111121768A (en) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | Robot pose estimation method and device, readable storage medium and robot |
| CN111220118B (en) * | 2020-03-09 | 2021-03-02 | 燕山大学 | Laser range finder based on visual inertial navigation system and range finding method |
| CN111721282A (en) * | 2020-05-09 | 2020-09-29 | 中国人民解放军63686部队 | Strapdown inertial navigation coordinate system dynamic alignment method based on astronomical navigation principle |
| CN111795695A (en) * | 2020-05-15 | 2020-10-20 | 北京百度网讯科技有限公司 | Position information determining method, device and equipment |
| CN111795695B (en) * | 2020-05-15 | 2022-06-03 | 阿波罗智联(北京)科技有限公司 | Position information determining method, device and equipment |
| CN112249089A (en) * | 2020-09-27 | 2021-01-22 | 卡斯柯信号有限公司 | Rail transit emergency positioning system and method |
| CN112097767A (en) * | 2020-10-15 | 2020-12-18 | 杭州知路科技有限公司 | Pre-integration auxiliary assembly for inertial navigation and data processing method |
| CN112614219A (en) * | 2020-12-07 | 2021-04-06 | 灵鹿科技(嘉兴)股份有限公司 | Spatial coordinate conversion method based on identification points for map navigation positioning |
| CN112697131A (en) * | 2020-12-17 | 2021-04-23 | 中国矿业大学 | Underground mobile equipment positioning method and system based on vision and inertial navigation system |
| CN112598705A (en) * | 2020-12-17 | 2021-04-02 | 太原理工大学 | Vehicle body posture detection method based on binocular vision |
| CN112598705B (en) * | 2020-12-17 | 2024-05-03 | 太原理工大学 | Binocular vision-based vehicle body posture detection method |
| CN112550377A (en) * | 2020-12-18 | 2021-03-26 | 卡斯柯信号有限公司 | Rail transit emergency positioning method and system based on video identification and IMU (inertial measurement Unit) equipment |
| CN112698051A (en) * | 2021-03-23 | 2021-04-23 | 天津所托瑞安汽车科技有限公司 | Vehicle speed determination method and device, equipment and medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109631887B (en) | 2022-10-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109631887A (en) | Inertial navigation high-precision locating method based on binocular, acceleration and gyroscope | |
| CN108375370B (en) | A kind of complex navigation system towards intelligent patrol unmanned plane | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| CN106780699B (en) | Visual SLAM method based on SINS/GPS and odometer assistance | |
| CN110243358A (en) | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion | |
| CN110095116A (en) | A kind of localization method of vision positioning and inertial navigation combination based on LIFT | |
| CN109166149A (en) | A kind of positioning and three-dimensional wire-frame method for reconstructing and system of fusion binocular camera and IMU | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN100504299C (en) | Method for obtaining three-dimensional information of space non-cooperative object | |
| CN109141433A (en) | A kind of robot indoor locating system and localization method | |
| CN104180818B (en) | A kind of monocular vision mileage calculation device | |
| CN110361027A (en) | Robot path planning method based on single line laser radar Yu binocular camera data fusion | |
| CN106679648A (en) | Vision-inertia integrated SLAM (Simultaneous Localization and Mapping) method based on genetic algorithm | |
| CN107450577A (en) | UAV Intelligent sensory perceptual system and method based on multisensor | |
| CN106056664A (en) | Real-time three-dimensional scene reconstruction system and method based on inertia and depth vision | |
| CN107478223A (en) | A kind of human body attitude calculation method based on quaternary number and Kalman filtering | |
| CN108052103A (en) | The crusing robot underground space based on depth inertia odometer positions simultaneously and map constructing method | |
| CN106017463A (en) | Aircraft positioning method based on positioning and sensing device | |
| CN106767785B (en) | Navigation method and device of double-loop unmanned aerial vehicle | |
| CN107909614A (en) | Crusing robot localization method under a kind of GPS failures environment | |
| CN103791902B (en) | It is applicable to the star sensor autonomous navigation method of high motor-driven carrier | |
| CN108253963A (en) | A kind of robot active disturbance rejection localization method and alignment system based on Multi-sensor Fusion | |
| CN107688184A (en) | A kind of localization method and system | |
| CN109443348A (en) | It is a kind of based on the underground garage warehouse compartment tracking for looking around vision and inertial navigation fusion | |
| CN110412596A (en) | A kind of robot localization method based on image information and laser point cloud |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |