CN109725339A - A kind of tightly coupled automatic Pilot cognitive method and system - Google Patents
A kind of tightly coupled automatic Pilot cognitive method and system Download PDFInfo
- Publication number
- CN109725339A CN109725339A CN201811565306.6A CN201811565306A CN109725339A CN 109725339 A CN109725339 A CN 109725339A CN 201811565306 A CN201811565306 A CN 201811565306A CN 109725339 A CN109725339 A CN 109725339A
- Authority
- CN
- China
- Prior art keywords
- module
- satellite
- measurement data
- inertial navigation
- automatic pilot
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000001149 cognitive effect Effects 0.000 title claims abstract description 21
- 238000005259 measurement Methods 0.000 claims abstract description 72
- 230000008878 coupling Effects 0.000 claims abstract description 35
- 238000010168 coupling process Methods 0.000 claims abstract description 35
- 238000005859 coupling reaction Methods 0.000 claims abstract description 35
- 230000001953 sensory effect Effects 0.000 claims abstract description 10
- 238000012545 processing Methods 0.000 claims description 22
- 230000000007 visual effect Effects 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000003068 static effect Effects 0.000 claims description 6
- 239000007787 solid Substances 0.000 claims description 3
- 230000006870 function Effects 0.000 description 12
- 239000011159 matrix material Substances 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000006073 displacement reaction Methods 0.000 description 4
- 239000005433 ionosphere Substances 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 238000005375 photometry Methods 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 239000005436 troposphere Substances 0.000 description 2
- 241000208340 Araliaceae Species 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000004883 computer application Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000003912 environmental pollution Methods 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000002955 isolation Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of tightly coupled automatic Pilot cognitive methods, this method comprises: obtaining inertial navigation module measurement data, the image data of stereo vision module and the satellite navigation raw measurement data of automatic driving vehicle；The image data of the stereo vision module, the satellite navigation raw measurement data and the inertial navigation module measurement data are subjected to close coupling, limit the increase of the drift error of inertial navigation module, guarantees the precision of positioning.Based on above-mentioned tightly coupled automatic Pilot cognitive method, the invention also discloses a kind of tightly coupled automatic Pilot sensory perceptual systems.Inertial navigation module measurement data, the image data of stereo vision module and satellite navigation raw measurement data three are carried out close coupling by the present invention, limit the increase of the drift error of inertial navigation module, to improve positioning accuracy, no longer by means of expensive scanning laser radar, to reduce the cost of autonomous driving vehicle.
Description
Technical field
The present invention relates to the technical field of computer application, in particular to a kind of tightly coupled automatic Pilot cognitive method and
System.
Background technique
In recent years, with the development of most information technology, automatic Pilot field is got over for the enhancing realized with people to automotive safety
Come it is more concerned, in the world many companies and scientific research institution all start investment research and development automatic Pilot Related product, it is contemplated that 2021
Automatic driving vehicle will enter market, bring huge change to automobile industry.Correlative study shows the hair of automatic Pilot technology
Exhibition will bring subversive development in multiple fields, such as the traffic safety of highway can be enhanced in its development, alleviation traffic is gathered around
It stifled situation and reduces environmental pollution etc., while automatic Pilot technology is also to measure a national research strength and industrial level
An important symbol, have broad application prospects in national defence and national economy field.
Automatic Pilot refers to automobile by vehicle-mounted sensor-based system to perceive to road environment, and is obtained according to perception
The control vehicle such as road, vehicle location and obstacle information steering and speed, and then automatic planning travelling line and control
The technology of vehicle arrival predeterminated target.
Nowadays in terms of automatic Pilot, there is the technique direction of oneself in each major company, and it is directly square that the prior art has binocular
The vision system of method and the combined system of inertial navigation module, but vision system and inertial navigation module generate in combined system
Error cannot effectively limit, combined system when long-time is without image gradient error can without limitation increase, cause to combine
System senses failure.
The prior art also has vision system, inertial navigation module and the close coupling of satellite navigation of monocular feature point methods certainly
It is dynamic to drive sensory perceptual system, but monocular cam can not detect no feature barrier, such as the isolation guardrail, voluntarily of expressway
Vehicle or animal etc..And existing vision system is also had and is coupled using Binocular Stereo Vision System, but still use characteristic point
Method, it is computationally intensive, and to hardware performance requirements height.
For this purpose, we have proposed a kind of 3 D visual image processing modules based on binocular direct method, inertial navigation mould
Block and the tightly coupled automatic Pilot cognitive method of satellite navigation module and system.
Summary of the invention
The main purpose of the present invention is to provide a kind of tightly coupled automatic Pilot cognitive method and systems, have maximum
It improves to limit the positioning accuracy of automatic Pilot sensory perceptual system, improve the advantages of computational efficiency and reliability.
To achieve the above object, the present invention provides a kind of tightly coupled automatic Pilot cognitive methods, which comprises
Step S1, the image data, the measurement data of inertial navigation module and satellite navigation for obtaining stereo vision module are former
Beginning measurement data；
Step S2, by the image data of the stereo vision module, the satellite navigation raw measurement data and described used
Property navigation module measurement data carry out close coupling, the drift error of the inertial navigation module is modified.
Preferably, the stereo vision module is handled using binocular camera direct method, described image data packet
Include the static measurement data and dynamic measuring data of binocular camera.
Preferably, the close coupling is led by the weighting re-projection error of stereoscopic vision, satellite navigation error and from inertia
The time error of boat constitutes cost function.
Preferably, the step S1 is specifically included:
Step S21, the image data of stereo vision module is obtained using binocular camera；
Step S22, the measurement data of inertial navigation module is obtained using inertial sensor；
Step S23, satellite navigation raw measurement data is obtained by receiver；
Step S24, by the inertial navigation module measurement data, described image data and the satellite navigation original measurement
Data carry out close coupling processing.
Preferably, the step S22 is specifically included:
Step S221,3 axle accelerations and 3 axis using inertial sensor measurement automatic driving vehicle under fixed coordinate system
Angular speed；
Step S222, the acceleration and the angular speed are turned under navigational coordinate system, it is mechanical solves inertial navigation
Layout equation and position and the attitude angle for calculating automatic driving vehicle.
Preferably, the step S23 is specifically included:
Step S231, the signal of navigation satellite is received with receiver；
Step S232, the ephemeris information for parsing each satellite calculates each satellite according to the ephemeris information
Satellite position and satellite velocities；
Step S233, and calculate the pseudo-distance of the satellite, Doppler's frequency floats and carrier phase.
Preferably, the step S24 specifically includes: by the satellite navigation raw measurement data in conjunction with the stereopsis
Feel that the image data of module is corrected the drift error of the inertial navigation module.
A kind of tightly coupled automatic Pilot sensory perceptual system, the system comprises: the 3 D visual image of binocular direct method
Processing module, satellite navigation module, inertial navigation module and system close coupling module, inertial navigation module, for using inertia
The measurement data of sensor acquisition inertial navigation module；3 D visual image processing module is obtained three-dimensional using binocular camera
The image data of vision module；Satellite navigation module, for obtaining satellite navigation raw measurement data by receiver；System is tight
Coupling module, for by the image data of the inertial navigation module measurement data, the stereo vision module and the satellite
The raw measurement data that navigates carries out close coupling processing；
The 3 D visual image processing module, the satellite navigation module and the inertial navigation module with the system
Close coupling module of uniting connection.
Preferably, the 3 D visual image processing module includes binocular camera.
Preferably, the inertial navigation module includes inertial sensor, and the inertial sensor is fixed on described to be driven automatically
It sails on vehicle.
Compared with prior art, the invention has the following beneficial effects: the present invention by inertial navigation module measurement data, stands
The image data of body vision module and satellite navigation raw measurement data three carry out close coupling, measure number to inertial navigation module
According to error be modified, to improve positioning accuracy, no longer by means of expensive scanning laser radar, to reduce automatic
The cost of driving.
Detailed description of the invention
Fig. 1 is the flow chart of the tightly coupled automatic Pilot cognitive method of the embodiment of the present invention.
Fig. 2 is the flow chart of system of embodiment of the present invention close coupling module.
Fig. 3 is the structural schematic diagram of the tightly coupled automatic Pilot sensory perceptual system of the embodiment of the present invention.
Specific embodiment
To be easy to understand the technical means, the creative features, the aims and the efficiencies achieved by the present invention, below with reference to
Specific embodiment, the present invention is further explained.
It is tightly coupled certainly based on stereo vision module, inertial navigation module and satellite navigation module that the present invention provides one kind
It is dynamic to drive cognitive method.Inertial navigation can continuously provide information, and short time precision is high, but position error can accumulate at any time；
Satellite navigation long-time stability are good, but vulnerable to interference, data renewal frequency is low；Stereoscopic vision is according to the views of different cameras
Difference, the distance of calculating barrier to camera, but vision system are unable to effective position and spy in the environment of image lacks gradient
Survey obstacle distance.Stereoscopic vision, inertial navigation and satellite navigation are constituted into integrated navigation system, so as to mutually assist to perceive
The state and ambient condition of vehicle, it is complementary under various circumstances, reliability and navigation accuracy are improved, distance can be accurately extracted,
Existing scanning laser radar can be substituted, cost is reduced.
Fig. 1 is the flow chart of the tightly coupled automatic Pilot cognitive method of the embodiment of the present invention, as shown in Figure 1, the close coupling
Automatic Pilot cognitive method specifically include:
S1, the inertial navigation module measurement data of automatic driving vehicle, the image data of stereo vision module are obtained and is defended
Star navigation raw measurement data；S2, during close coupling, by the image data of stereo vision module and the original survey of satellite navigation
It measures data and inertial navigation module measurement data carries out close coupling, the error of inertial navigation module measurement data is modified,
To perceive the state of vehicle and the state of environment.The process of specific automatic Pilot cognitive method is retouched in detail below
It states:
(1) image data of stereo vision module is obtained by the 3 D visual image processing module of binocular direct method,
The detailed process for the image data that stereo vision module is obtained using binocular camera is described below:
Direct method is based on the constant hypothesis of gray scale, the pixel grey scale of the same spatial point, is solid in each image
Fixed constant, direct method does not need to extract feature, while lacking angle point and edge or the unconspicuous environment of light variation,
There are better effects；And the data that direct method need to be handled are less, it can be achieved that Computationally efficient.
Detailed process is as follows for the image taken using binocular direct method processing cam stereo vision:
1. being recorded using binocular camera from two width of different position acquisition testees or the gray level image of multiple image
Binocular camera is I in the image that the t and t+1 moment obtainsiAnd Ij。
2. carrying out camera calibration by matlab or opencv, the internal reference of camera is obtained.
3. pair image obtained carries out distortion processing.
4. above-mentioned image data is input in system close coupling module, photometric measurement is calculated according to shooting image
Energy.Specifically, the spatial point Pi in image Ii is appeared in another frame image Ij, one occurred in image Ii and Ij is chosen
Spatial point Pi, the constant hypothesis principle of gray value measured under each visual angle according to the same space three-dimensional point, the point in image Ii
P is calculated in the projection p ' of image Ij by following formula:
Wherein, ПKWithIt is projection and the back projection's function of camera image frame point, dpIt is the inverse depth of p point, TjiIt is figure
As the transformational relation between frame:
Wherein, RjiIt is the spin matrix of a 3*3, t is translation vector.
Its luminosity error energy function are as follows:
Wherein, | | | |γIt is Huber norm, is that error energy function is too fast with the growth of luminosity error in order to prevent；ωp
It is weight, in order to reduce the corresponding weight of the big pixel of gradient in image, energy function final in this way is able to reflect greatly
The luminosity error of most pixels, rather than the error of the big pixel of individual gradients；Ii[p] and Ij[p'] is respectively PiScheming
As the gray value of corresponding points.
It optimizes to obtain camera motion by minimizing photometric measurement energy, when photometric measurement energy is minimum
Value, solution obtain the image data of stereo vision module.Obtain objective function are as follows:
Above-mentioned formula (1-3) is the luminosity error energy function of monocular cam, when camera is binocular camera,
Stereo vision module is handled using binocular camera direct method, and the image data of stereo vision module includes binocular camera shooting
Head in it is static when static measurement data and dynamic measuring data when in dynamic.Binocular direct method by introduce coupling because
Sub- λ comes the relative weighting of each picture frame and static stereoscopic vision of camera in balance exercise, error energy function are as follows:
Wherein, obst(p) point of observation in all picture frames is indicated,It is the error energy letter in static stereoscopic vision
Number.Then, according to formula (1-4) and formula (1-5) it can be concluded that the objective function of new binocular camera.Above-mentioned two picture frame
Between conversion in the R that occursjiAnd t, it is the pose of camera, solves mesh by using gradient descent method or gauss-newton method
Scalar functions, to obtain the pose of camera.And then the inertia of automatic driving vehicle in inertial navigation is led by the camera pose
The error of model plane block measurement data is modified, and the camera pose which obtains does not need to extract feature, to road
Road and environmental change respond significantly improve.
(2) inertial navigation module measurement data is obtained by inertial navigation module, and inertial navigation module measurement data includes
The speed and displacement angular speed of automatic driving vehicle, below retouch the detailed process for obtaining inertial navigation module measurement data
It states:
Firstly, 3 axle accelerations and 3 shaft angles using inertial sensor measurement automatic driving vehicle under fixed coordinate system
Speed.Preferably, inertial sensor is mounted on vehicle chassis, and inertial sensor is accelerometer and gyroscope, wherein acceleration
Meter is for measuring vehicle acceleration, and gyroscope is for measuring vehicle angular speed.But there are zero measurements such as float to miss for measurement sensor
Difference, these measurement errors increase position error with the time square, attitude error is grown proportionately with the time, if be not added
With limitation, navigation system loses ability quickly, then, carries out error compensation by modeling, can reduce ascertainment error and random drift
Shift error.
Further, acceleration and angular speed is turned under navigational coordinate system, solves inertial navigation mechanization equation
And calculate position and the attitude angle of automatic driving vehicle.Specifically, carrying out selected when pose numerical value is updated with error compensation
Navigation system be n system, usually northeast day.
Under the coordinate system of northeast day, the location update formula of vehicle is as follows:
In above-mentioned formula,For projection of the bearer rate in n system；λ, L and h are respectively indicated
Longitude, latitude and the height of carrier；A indicates that parameter-spheroid major semiaxis is long substantially bigly under WGS-84 coordinate system, and e is ellipse
The eccentricity of sphere.The differential equation about λ, L, h is finally obtained, according to the value for solving available λ, L, h, so as to count
Calculate the position of automatic driving vehicle, RNFor prime plane curvature, RMFor radius of curvature of meridian.
Under the coordinate system of northeast day, the speed of vehicle more new formula is as follows:
In above-mentioned formula,For quaternary number direction cosine matrixTransposition；vnFor throwing of the speed in n system of carrier
Shadow；The displacement angular speed being calculated for the relative velocity by carrier；What is indicated is the rotation angular speed of the earth in n
Under projection；gnFor projection of the local acceleration of gravity on n；fbFor the measurement output valve of accelerometer.Symbol "×" represents
Vector multiplication cross.It finally obtains about vn、The differential equation, according to solving available vn、Value, so as to calculate
The speed of automatic driving vehicle and displacement angular speed out.
Furthermore inertial navigation kinematics and simple dynamic deviation models coupling are got up, following equation group is obtained:
Wherein,Element be each incoherent zero mean Gaussian white noise process.
It is accelerometer measures, gWIt is the acceleration of gravity vector of the earth.And the gyroscope deviation modeled as random walk is compared, I
Using timeconstantτ > 0 come by accelerometer bias modeling be bounded random walk.Matrix Ω is by estimating along with gyro
Instrument measurementAngular speedComposition:
Finally we draw the prediction error e of inertial navigation part in integrated navigations, esIt is missed for the time of inertial navigation
Poor item:
Wherein three, the right is 1,2,3 components of quaternary number.
(3) satellite navigation raw measurement data is obtained by satellite navigation module, the satellite navigation raw measurement data packet
It includes satellite position, satellite velocities, pseudo-distance, Doppler's frequency to float and carrier phase, below to obtaining satellite navigation original measurement number
According to detailed process be described:
Global Navigation Satellite System (GNSS) navigation accuracy with higher, the GNSS on vehicle are defended to in-orbit in the air
Star emits signal, and the signal of navigation satellite is received with receiver；Satellite-signal parses the ephemeris of each satellite based on the received
Information calculates the satellite position and satellite velocities of each satellite according to the ephemeris information.Meanwhile according to ephemeris information
The pseudo-distance of satellite can also be calculated, Doppler's frequency floats and carrier phase.
Specifically, satellite navigation receiver measures pseudo-distance using one-point positioning method, pseudo-distance is certain satellite
With the relative distance of user.In satellite navigation, pseudorange ρ(n)(t) calculating is the time t being received with n satellite-signalu(t)
With launch time ts (n)Time difference between (t- τ) is multiplied with the speed c of true radio wave in air, and expression formula is as follows:
Wherein, symbol tau represents GNSS signal from being emitted to by the received real time interval of user.But due to GNSS satellite
Usually will not be synchronous with GNSS time t with the clock of receiver user, the advanced GNSS time of satellite time is usedTable
Show, by receiver time advanced GNSS time δ tu(t) it indicates, it may be assumed that
tu(t)=t+ δ tu(t) (3-3)
And the structures such as ionosphere, troposphere can cause delay to a certain extent to electromagnetic wave propagation, therefore τ needs
Subtract ionosphere delay I(n)(t) be delayed T with troposphere(n)(t) it is several from satellite position to receiver location to be only satellite-signal
What distance r(n)Propagation time, it may be assumed that
τ=r(n)/c+I(n)(t)+T(n)(t) (3-4)
(3-2), (3-3), (3-4) are updated in (3-1), final pseudorange ρ is obtained(n)(t) calculation formula, it may be assumed that
dtrop=cT(n)(t)
diono=cI(n)(t)
Wherein,For satellite clock correction, dionoIonosphere delay and dtropTropospheric delay is known quantity, ε(n)
That indicate is unknown pseudo range measurement noise, r(n)It is receiver in physical space (x, y, z) to n-th satellite (x(n), y(n), z(n)) geometric distance, the coordinate (x of each position(n), y(n), z(n)) can resolve to obtain from the ephemeris that each satellite is broadcast.
When calculating pseudorange ρ(n)(t) after, the observational equation of the carrier phase in available satellite navigation is as follows:
φ λ=ρ (ts,tr)+c(dtr-dts)+dtrop-diono+drel+dSA+dmulti+Nλ+ε (3-6)
Wherein,For carrier phase observation data, λ is carrier wavelength, and N is integer ambiguity, tsWhen for satellite emission signal
It carves, trReceive signal moment, dt for receiversAnd dtrThe respectively clock deviation of satellite and receiver, c are the light velocity, dionoFor ionization
Layer delay, dtropFor tropospheric delay, dSAFor SA influence, dmultiFor multipath effect, ε is carrier observations noise, ρ (ts,tr)
For tsMoment satellite and trGeometric distance between reception machine antenna, it contains survey station coordinate, satellite orbit and earth rotation
Parameter etc..
Since Doppler frequency shift observed quantity is the INSTANTANEOUS OBSERVATION value of carrier phase rate, ignore ionosphere, tropospheric delay pair
The variation of time,
Differential is carried out to carrier phase observational equation:
Wherein,Then, Doppler shift measurement equation can be obtained are as follows:
Wherein, λ is the corresponding wavelength of carrier wave L1 (f1=1575.42MHz),It is the Doppler of user's u relative satellite i
Frequency displacement, v(i)It is the movement speed of satellite i,It is the movement speed of user u, au (i)Be user u be directed toward satellite i unit to
Amount, c is the light velocity in vacuum, δ fuIt is the clock drift of user u, δ f(i)It is the clock drift of satellite i,It is that user u is opposite
The doppler frequency measurement noise of satellite i.
(4) data of 3 D visual image processing module, inertial navigation module and satellite navigation module three parts are inputted
Close coupling is carried out, data include that inertial navigation module measurement data, the image data of stereo vision module and satellite navigation are original
Measurement data, the drift by the image data of satellite navigation raw measurement data combination stereo vision module to inertial navigation module
Shift error is corrected, and limits drift error so as to aided inertial navigation module, Fig. 2 is the specific of system close coupling module
Flow chart.In the figure, inertial sensor is defeated for obtaining 3 axle accelerations and 3 axis angular rates of the vehicle under fixed coordinate system
Enter into inertial navigation module；Binocular camera acquires the input that picture is used for direct method；GPS receiver is for obtaining satellite
Raw measurement data.By the re-projection error of binocular camera direct method, defended according to what satellite raw measurement data obtained
Star navigation error and the time error of inertial navigation combine, and carry out overall optimum estimation to correct the drift of inertial navigation module
Shift error finally exports optimal pose.
When carrying out close coupling, close coupling is by the weighting re-projection error of stereoscopic vision, satellite navigation error and from used
Property navigation time error constitute cost function and expressed, expression formula is as follows:
Wherein, erFor the weighting re-projection error of stereoscopic vision, egFor satellite navigation error, esFor the time of inertial navigation
Error term, i indicate that the label of camera, k indicate that camera frame label, j indicate terrestrial reference label.It can in kth frame and i-th of camera
The terrestrial reference label seen is written as set J (i, k).In addition,Indicate the information matrix of corresponding subscript measurement, andTable
Inertial navigation control information when showing corresponding kth frame picture,When indicating s satellite of corresponding t moment, the corresponding mistake of satellite
Poor information matrix.
Furthermore erFor the weighting re-projection error of stereoscopic vision, the form of expression of the re-projection error of stereoscopic vision are as follows:
Wherein, hiIndicate the projection model of camera, zI, j, kIndicate the image coordinate of feature.Indicate the appearance of optimization system
State,Indicate the outer ginseng of inertial navigation and camera,Indicate characteristic coordinates.
Further, egFor satellite navigation error, the error of each navigation satellite s of each moment t includes three portions
The error divided, its form of expression are as follows:
Wherein, epIndicate pseudorange error, edIndicate Doppler error, ecIndicate carrier phase error.
Furthermore corresponding control information matrix WgForm reformed into following form:
The value of cost function J (x) is made to reach minimum by the method linearly or nonlinearly optimized, to complete stereopsis
Feel the close coupling between image processing module, inertial navigation module and satellite navigation module three, passes through the original survey of satellite navigation
The image data of amount data combination stereo vision module is corrected the drift error of inertial navigation module.When satellite number is less than
When setting quantity, receiver can not be positioned, pass through the figure of satellite navigation raw measurement data combination stereo vision module
As systematic error of the data to inertial navigation module is corrected, to improve the precision of navigation, navigation system is greatly strengthened
The robustness of system.The tightly coupled automatic Pilot sensory perceptual system can help to provide environment high-acruracy survey and establish map,
No longer by means of expensive scanning laser radar, the cost of autonomous driving vehicle is reduced.
Based on above-mentioned tightly coupled automatic Pilot cognitive method, the present invention also provides a kind of tightly coupled automatic Pilot senses
Know system 100, Fig. 3 is the structural schematic diagram of the tightly coupled automatic Pilot sensory perceptual system 100 of the embodiment of the present invention, such as Fig. 3 institute
Show, which includes the 3 D visual image processing module 10 of binocular direct method, satellite
Navigation module 20, inertial navigation module 30 and system close coupling module 40.
Specifically, inertial navigation module 30 is used to obtain inertial navigation module measurement data using inertial sensor；Specifically
As above.3 D visual image processing module 10 obtains the image data of stereo vision module using binocular direct method；Specifically such as
On.Satellite navigation module 20 is used to obtain the satellite navigation raw measurement data of navigation satellite by receiver；It is specific as above.
System close coupling module 40 is used for inertial navigation module measurement data, the image data of stereo vision module and satellite navigation is former
Beginning measurement data carries out close coupling processing；It is specific as above.Connection relationship between modules are as follows: 3 D visual image handles mould
Block 10, satellite navigation module 20 and inertial navigation module 30 are connect with system close coupling module 40.
Wherein, 3 D visual image processing module 10 includes binocular camera, and binocular camera is installed on automatic Pilot vehicle
On；As detailed above.Furthermore inertial navigation module 30 includes inertial sensor, and inertial sensor is fixed on automatic Pilot
On vehicle；As detailed above.
Compared with prior art, the invention has the following beneficial effects: the present invention by inertial navigation module measurement data, stands
The image data of body vision module and satellite navigation raw measurement data three carry out close coupling, measure number to inertial navigation module
According to error be modified, to improve positioning accuracy, no longer by means of expensive scanning laser radar, to reduce automatic
The cost of driving.
The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention.The technology of the industry
Personnel are it should be appreciated that the present invention is not limited to the above embodiments, and the above embodiments and description only describe this
The principle of invention, without departing from the spirit and scope of the present invention, various changes and improvements may be made to the invention, these changes
Change and improvement all fall within the protetion scope of the claimed invention.The claimed scope of the invention by appended claims and its
Equivalent thereof.
Claims (10)
1. a kind of tightly coupled automatic Pilot cognitive method, which is characterized in that the described method includes:
Step S1, image data, the measurement data of inertial navigation module and the original survey of satellite navigation of stereo vision module are obtained
Measure data；
Step S2, the image data of the stereo vision module, the satellite navigation raw measurement data and the inertia are led
The measurement data of model plane block carries out close coupling, is modified to the drift error of the inertial navigation module.
2. tightly coupled automatic Pilot cognitive method according to claim 1, which is characterized in that the stereo vision module
It is handled using binocular camera direct method, described image data include the static measurement data and dynamic of binocular camera
Measurement data.
3. tightly coupled automatic Pilot cognitive method according to claim 1, which is characterized in that the close coupling is by solid
Weighting re-projection error, satellite navigation error and the time error from inertial navigation of vision constitute cost function.
4. tightly coupled automatic Pilot cognitive method according to claim 1, which is characterized in that the step S1 is specifically wrapped
It includes:
Step S21, the image data of stereo vision module is obtained using binocular camera；
Step S22, the measurement data of inertial navigation module is obtained using inertial sensor；
Step S23, satellite navigation raw measurement data is obtained by receiver；
Step S24, by the inertial navigation module measurement data, described image data and the satellite navigation raw measurement data
Carry out close coupling processing.
5. tightly coupled automatic Pilot cognitive method according to claim 4, which is characterized in that the step S22 is specific
Include:
Step S221,3 axle accelerations and 3 shaft angles speed using inertial sensor measurement automatic driving vehicle under fixed coordinate system
Degree；
Step S222, the acceleration and the angular speed are turned under navigational coordinate system, solves inertial navigation mechanization
Equation and position and the attitude angle for calculating automatic driving vehicle.
6. tightly coupled automatic Pilot cognitive method according to claim 4, which is characterized in that the step S23 is specific
Include:
Step S231, the signal of navigation satellite is received with receiver；
Step S232, the ephemeris information for parsing each satellite calculates defending for each satellite according to the ephemeris information
Championship is set and satellite velocities；
Step S233, and calculate the pseudo-distance of the satellite, Doppler's frequency floats and carrier phase.
7. tightly coupled automatic Pilot cognitive method according to claim 4, which is characterized in that the step S24 is specific
Include: by the satellite navigation raw measurement data in conjunction with the stereo vision module image data to the inertial navigation
The drift error of module is corrected.
8. a kind of tightly coupled automatic Pilot sensory perceptual system, which is characterized in that the system comprises: the solid of binocular direct method
Visual pattern processing module, satellite navigation module, inertial navigation module and system close coupling module, inertial navigation module are used for
The measurement data of inertial navigation module is obtained using inertial sensor；3 D visual image processing module, using binocular camera
Obtain the image data of stereo vision module；Satellite navigation module, for obtaining satellite navigation original measurement number by receiver
According to；System close coupling module, for by the image data of the inertial navigation module measurement data, the stereo vision module and
The satellite navigation raw measurement data carries out close coupling processing；
The 3 D visual image processing module, the satellite navigation module and the inertial navigation module are tight with the system
Coupling module connection.
9. tightly coupled automatic Pilot sensory perceptual system according to claim 8, which is characterized in that the 3 D visual image
Processing module includes binocular camera.
10. tightly coupled automatic Pilot sensory perceptual system according to claim 8, which is characterized in that the inertial navigation mould
Block includes inertial sensor, and the inertial sensor is fixed on the automatic driving vehicle.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811565306.6A CN109725339A (en) | 2018-12-20 | 2018-12-20 | A kind of tightly coupled automatic Pilot cognitive method and system |
| PCT/CN2018/123652 WO2020124624A1 (en) | 2018-12-20 | 2018-12-25 | Autonomous driving sensing method and system employing close coupling |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811565306.6A CN109725339A (en) | 2018-12-20 | 2018-12-20 | A kind of tightly coupled automatic Pilot cognitive method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109725339A true CN109725339A (en) | 2019-05-07 |
Family
ID=66297006
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811565306.6A Pending CN109725339A (en) | 2018-12-20 | 2018-12-20 | A kind of tightly coupled automatic Pilot cognitive method and system |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN109725339A (en) |
| WO (1) | WO2020124624A1 (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110502005A (en) * | 2019-07-12 | 2019-11-26 | 北京合众思壮科技股份有限公司 | Automatic Pilot method and system |
| CN110906923A (en) * | 2019-11-28 | 2020-03-24 | 重庆长安汽车股份有限公司 | Vehicle-mounted multi-sensor tight coupling fusion positioning method and system, storage medium and vehicle |
| CN111829522A (en) * | 2020-07-02 | 2020-10-27 | 浙江大华技术股份有限公司 | Instant positioning and map construction method, computer equipment and device |
| CN111929718A (en) * | 2020-06-12 | 2020-11-13 | 东莞市普灵思智能电子有限公司 | Automatic driving object detection and positioning system and method |
| CN115790282A (en) * | 2022-10-11 | 2023-03-14 | 西安岳恒机电工程有限责任公司 | Direction control system and control method for unmanned target vehicle |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11875519B2 (en) * | 2020-08-13 | 2024-01-16 | Medhat Omr | Method and system for positioning using optical sensor and motion sensors |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103308925A (en) * | 2013-05-31 | 2013-09-18 | 中国科学院合肥物质科学研究院 | Integral three-dimensional color laser radar data point cloud generating method and device thereof |
| CN103635937A (en) * | 2011-05-30 | 2014-03-12 | 原子能和辅助替代能源委员会 | Method for locating a camera and for 3d reconstruction in a partially known environment |
| WO2014130854A1 (en) * | 2013-02-21 | 2014-08-28 | Regents Of The Univesity Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
| CN104316947A (en) * | 2014-08-26 | 2015-01-28 | 南京航空航天大学 | GNSS/INS ultra-tight combination navigation apparatus and relative navigation system thereof |
| CN107389064A (en) * | 2017-07-27 | 2017-11-24 | 长安大学 | A kind of unmanned vehicle based on inertial navigation becomes channel control method |
| CN108444468A (en) * | 2018-02-06 | 2018-08-24 | 浙江大学 | The bearing compass of vision and inertial navigation information is regarded under a kind of fusion |
| CN108802786A (en) * | 2018-07-20 | 2018-11-13 | 北斗星通（重庆）汽车电子有限公司 | A kind of vehicle positioning method |
| CN210038170U (en) * | 2018-12-20 | 2020-02-07 | 东莞市普灵思智能电子有限公司 | Tightly-coupled automatic driving sensing system |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103969672B (en) * | 2014-05-14 | 2016-11-02 | 东南大学 | A kind of multi-satellite system and strapdown inertial navigation system tight integration air navigation aid |
| US9435651B2 (en) * | 2014-06-04 | 2016-09-06 | Hexagon Technology Center Gmbh | System and method for augmenting a GNSS/INS navigation system in a cargo port environment |
| US10579068B2 (en) * | 2016-10-03 | 2020-03-03 | Agjunction Llc | Using optical sensors to resolve vehicle heading issues |
| CN106767787A (en) * | 2016-12-29 | 2017-05-31 | 北京时代民芯科技有限公司 | A kind of close coupling GNSS/INS combined navigation devices |
| CN107037469A (en) * | 2017-04-11 | 2017-08-11 | 北京七维航测科技股份有限公司 | Based on the self-alignment double antenna combined inertial nevigation apparatus of installation parameter |
| CN107478221A (en) * | 2017-08-11 | 2017-12-15 | 黄润芳 | A kind of high-precision locating method for mobile terminal |
| CN107633695B (en) * | 2017-09-30 | 2020-10-23 | 深圳市德赛微电子技术有限公司 | Perception type intelligent transportation terminal device |
-
2018
- 2018-12-20 CN CN201811565306.6A patent/CN109725339A/en active Pending
- 2018-12-25 WO PCT/CN2018/123652 patent/WO2020124624A1/en active Application Filing
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103635937A (en) * | 2011-05-30 | 2014-03-12 | 原子能和辅助替代能源委员会 | Method for locating a camera and for 3d reconstruction in a partially known environment |
| WO2014130854A1 (en) * | 2013-02-21 | 2014-08-28 | Regents Of The Univesity Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
| CN103308925A (en) * | 2013-05-31 | 2013-09-18 | 中国科学院合肥物质科学研究院 | Integral three-dimensional color laser radar data point cloud generating method and device thereof |
| CN104316947A (en) * | 2014-08-26 | 2015-01-28 | 南京航空航天大学 | GNSS/INS ultra-tight combination navigation apparatus and relative navigation system thereof |
| CN107389064A (en) * | 2017-07-27 | 2017-11-24 | 长安大学 | A kind of unmanned vehicle based on inertial navigation becomes channel control method |
| CN108444468A (en) * | 2018-02-06 | 2018-08-24 | 浙江大学 | The bearing compass of vision and inertial navigation information is regarded under a kind of fusion |
| CN108802786A (en) * | 2018-07-20 | 2018-11-13 | 北斗星通（重庆）汽车电子有限公司 | A kind of vehicle positioning method |
| CN210038170U (en) * | 2018-12-20 | 2020-02-07 | 东莞市普灵思智能电子有限公司 | Tightly-coupled automatic driving sensing system |
Non-Patent Citations (1)
| Title |
|---|
| TARAGAY OSKIPER 等: "Multi-Sensor Navigation Algorithm Using Monocular Camera, IMU and GPS for Large Scale Augmented Reality", 2012 IEEE INTERNATIONAL SYMPOSIUM ON MIXED AND AUGMENTED REALITY (ISMAR) - SCIENCE AND TECHNOLOGY, 10 July 2013 (2013-07-10), pages 71 - 80 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110502005A (en) * | 2019-07-12 | 2019-11-26 | 北京合众思壮科技股份有限公司 | Automatic Pilot method and system |
| CN110906923A (en) * | 2019-11-28 | 2020-03-24 | 重庆长安汽车股份有限公司 | Vehicle-mounted multi-sensor tight coupling fusion positioning method and system, storage medium and vehicle |
| CN110906923B (en) * | 2019-11-28 | 2023-03-14 | 重庆长安汽车股份有限公司 | Vehicle-mounted multi-sensor tight coupling fusion positioning method and system, storage medium and vehicle |
| CN111929718A (en) * | 2020-06-12 | 2020-11-13 | 东莞市普灵思智能电子有限公司 | Automatic driving object detection and positioning system and method |
| WO2021248636A1 (en) * | 2020-06-12 | 2021-12-16 | 东莞市普灵思智能电子有限公司 | System and method for detecting and positioning autonomous driving object |
| CN111829522A (en) * | 2020-07-02 | 2020-10-27 | 浙江大华技术股份有限公司 | Instant positioning and map construction method, computer equipment and device |
| CN115790282A (en) * | 2022-10-11 | 2023-03-14 | 西安岳恒机电工程有限责任公司 | Direction control system and control method for unmanned target vehicle |
| CN115790282B (en) * | 2022-10-11 | 2023-08-22 | 西安岳恒机电工程有限责任公司 | Unmanned target vehicle direction control system and control method |
Also Published As
| Publication number | Publication date |
|---|---|
| WO2020124624A1 (en) | 2020-06-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2021248636A1 (en) | System and method for detecting and positioning autonomous driving object | |
| CN109725339A (en) | A kind of tightly coupled automatic Pilot cognitive method and system | |
| Schreiber et al. | Vehicle localization with tightly coupled GNSS and visual odometry | |
| CN101743453B (en) | Post-mission high accuracy position and orientation system | |
| CN110100151A (en) | The system and method for global positioning system speed is used in vision inertia ranging | |
| CN110100190A (en) | System and method for using the sliding window of global location epoch in vision inertia ranging | |
| CN106643709B (en) | Combined navigation method and device for offshore carrier | |
| CN107850673A (en) | Vision inertia ranging attitude drift is calibrated | |
| CN110274588A (en) | Double-layer nested factor graph multi-source fusion air navigation aid based on unmanned plane cluster information | |
| CN111077556A (en) | Airport luggage tractor positioning device and method integrating Beidou and multiple sensors | |
| CN114488164B (en) | Synchronous positioning and mapping method for underwater vehicle and underwater vehicle | |
| CN108344415A (en) | A kind of integrated navigation information fusion method | |
| CN109983361A (en) | Opportunity signal aided inertial navigation | |
| CN110488292A (en) | A kind of remote sensing system based on satellites formation | |
| CN210038170U (en) | Tightly-coupled automatic driving sensing system | |
| Chi et al. | Enabling robust and accurate navigation for UAVs using real-time GNSS precise point positioning and IMU integration | |
| CN110887476A (en) | Autonomous course and attitude determination method based on polarization-astronomical included angle information observation | |
| Mostafa et al. | Optical flow based approach for vision aided inertial navigation using regression trees | |
| CN115523920B (en) | Seamless positioning method based on visual inertial GNSS tight coupling | |
| Amt et al. | Positioning for range-based land navigation systems using surface topography | |
| Wen et al. | Factor graph optimization for tightly-coupled GNSS pseudorange/Doppler/carrier phase/INS integration: Performance in urban canyons of Hong Kong | |
| CN115900732A (en) | Combined navigation method and system based on roadside camera and vehicle-mounted unit | |
| Iqbal et al. | A review of sensor system schemes for integrated navigation | |
| Wei | Multi-sources fusion based vehicle localization in urban environments under a loosely coupled probabilistic framework | |
| Aboutaleb et al. | Examining the Benefits of LiDAR Odometry Integrated with GNSS and INS in Urban Areas |
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 |