CN110411462B - GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method - Google Patents
GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method Download PDFInfo
- Publication number
- CN110411462B CN110411462B CN201910659041.4A CN201910659041A CN110411462B CN 110411462 B CN110411462 B CN 110411462B CN 201910659041 A CN201910659041 A CN 201910659041A CN 110411462 B CN110411462 B CN 110411462B
- Authority
- CN
- China
- Prior art keywords
- lane line
- constraint
- gnss
- inertial navigation
- observation
- 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.)
- Active
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 15
- 238000000034 method Methods 0.000 claims abstract description 34
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 230000000007 visual effect Effects 0.000 claims abstract description 6
- 238000000605 extraction Methods 0.000 claims description 14
- 230000004927 fusion Effects 0.000 claims description 14
- 238000001914 filtration Methods 0.000 claims description 13
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 8
- 239000013598 vector Substances 0.000 claims description 8
- 230000036544 posture Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 4
- 238000007781 pre-processing Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 241000283070 Equus zebra Species 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- IAZDPXIOMUYVGZ-UHFFFAOYSA-N Dimethylsulphoxide Chemical compound CS(C)=O IAZDPXIOMUYVGZ-UHFFFAOYSA-N 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000007689 inspection Methods 0.000 claims description 2
- 230000000452 restraining effect Effects 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 6
- 238000013135 deep learning Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000013213 extrapolation Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000008520 organization Effects 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Images
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a GNSS/inertia/lane line constraint/mileometer multi-source fusion method. When the carrier carries out real-time calculation navigation positioning, a differential GNSS/INS tight combination is taken as a basis, when the GNSS blocks serious conditions, the relative relation between the carrier and a lane line is detected through a visual sensor arranged on the carrier, under the assistance of a lane line map, a lane line constraint observation equation is dynamically added for auxiliary positioning, the position divergence in the lateral direction and the elevation direction is effectively inhibited, a speed observation value in the vehicle advancing direction and observable satellites above the carrier and in the road advancing direction are provided through a wheel type odometer, and the forward error of the carrier is obviously improved. The method can effectively control the position error drift in three directions under the typical dynamic complex environment of the city, and is an effective scheme for realizing centimeter-level positioning under the complex environment of the city.
Description
Technical Field
The invention relates to the field of integrated navigation, in particular to a GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method.
Background
With the establishment and operation of the regional service System of the Beidou Satellite Navigation System in China, the modernization of the American GPS and Russian GLONASS, the construction of the European Union Galileo, Japanese QZSS and India IRNSS systems, the GNSS (Global Navigation Satellite System) is developing in the direction of multi-frequency and multi-System. Then, because GNSS has three vulnerability problems of obstruction, interference and spoofing, reliable and continuous positioning service cannot be provided in a complex environment. Therefore, the full-source positioning and navigation integrating various different navigation systems and sensors, fusing all available signal sources in a plug-and-play mode are provided, all effective information is fused, and high-availability and high-reliability seamless navigation under any platform and any environment is realized through an advanced self-adaptive navigation algorithm, so that the excessive dependence on the GNSS is eliminated.
The high-precision positioning and attitude determination technology has great demands in the fields of surveying and mapping mobile measurement and popular location service. The conventional RTK/INS post-processing combined navigation mode can basically meet the requirements of mobile measurement, but in recent years, the rise of the intelligent industry represented by automatic driving puts higher demands on positioning navigation. Currently, technology companies such as Google, Waymo, Tesla, hundredth, kyoto and the like basically have unmanned driving capability in open environments or limited scenes. However, urban environments are complex and variable, difficult to predict, and under the conditions of large uncertainty and incomplete knowledge, automatic driving faces huge risks. The key to whether the accurate positioning in the urban environment can realize large-range landing application is that automatic driving is carried out. Currently, the automatic driving positioning technology adopts multi-sensor cooperative work to complete high-precision positioning, including a Global Navigation Satellite System (GNSS) receiver, an inertial sensor, a laser radar, a vision sensor, a odometer and the like. However, GNSS signals are easily blocked by urban building groups and overhead tunnels and lose positioning capability, and even if satellite signals can be locked, the problems of low signal-to-noise ratio, multi-cycle slip, multiple gross errors and ambiguity fixing errors are often presented, so that the data processing difficulty is extremely high; the traditional inertial navigation/mileometer dead reckoning technology cannot keep the autonomous positioning capability with high precision for a long time; in addition, the laser radar/vision sensor belongs to a matching positioning technology, is interfered by dynamic objects of road vehicles and pedestrians, and is seriously influenced by environmental factors such as low illumination, weak texture, motion blur and the like, so that the difficult problems are solved, the high-precision positioning of multi-source heterogeneous data fusion is realized, and the method is one of bottleneck problems to be solved urgently in automatic driving in an urban environment.
Aiming at the problems, the invention provides a method for multi-source fusion of GNSS/inertial navigation/lane line constraint/odometer, which is based on differential GNSS/INS tight combination, provides a lane line lateral and elevation constraint observation equation under the assistance of a lane line map, and provides carrier forward constraint through a wheel type odometer and an observable satellite above a carrier and in the advancing direction of a road, thereby inhibiting inertial navigation error divergence in three directions and providing reliable and continuous high-precision pose.
Disclosure of Invention
The invention aims to solve the technical problem of providing a GNSS/inertial navigation/lane line constraint/odometer multi-source fusion method aiming at the defects in the prior art.
The technical scheme adopted by the invention for solving the technical problems is as follows:
the invention provides a GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method, in the method, images are collected through a vision sensor, and a lane line map database is generated off line; when the carrier carries out real-time calculation navigation positioning, based on the tight combination of a differential GNSS/INS, when the GNSS is shielded, the relative relation between the carrier and a lane line is detected through a visual sensor arranged on the carrier, and a lane line constraint observation equation is dynamically added to assist lateral and elevation positioning under the assistance of a lane line map; and forming a differential observation value by using observable satellites above the carrier and in the road advancing direction to participate in tight combination, and restraining forward error divergence of the carrier.
Further, the method of the present invention specifically comprises the steps of:
Step 3, according to the inertial navigation forecast position, constructing a lane line elevation constraint observation equation under the assumption that the vehicle height and the camera are subjected to external parameter correction and the vehicle is located on a lane surface;
step 4, adding PL1、PL2、PR1、PR2Four points are raised to the center of the camera, and the center of the camera and the lane line are constructed according to the assumption that the distance between the forecast values of the center of the camera and the lane line is equal to the observation distanceThe lateral constraint observation equation is converted into an inertial navigation center by utilizing a chain rule;
step 5, constructing an observation equation of forward constraint of the odometer according to the assumptions that the carrier slips less during linear motion and turns at a lower speed;
and 6, realizing multi-source fusion observation updating according to the elevation of the lane line, the equation of lateral constraint and the forward constraint observation equation of the odometer.
Further, the method for tracking the lane line in step 1 of the present invention specifically comprises:
step 1.1, pretreatment: performing monocular calibration on the monocular camera through a Matlab calibration tool box to obtain camera internal parameters, and performing distortion correction on the lane line image acquired by the monocular camera by using the camera internal parameters; according to the installation position and the visual field range of the monocular camera, an interested area is defined, the interference of other irrelevant areas is eliminated, and the lane line is restored to the overlook state by utilizing homography transformation;
step 1.2, lane line extraction: inputting a lane line orthoimage, and binarizing the lane line orthoimage by color and threshold segmentation of edge information; dividing a pixel window at the bottom of the image, performing histogram statistics on the gray value of the window from left to right, obtaining a gray mean curve graph by adopting Gaussian kernel smoothing, and extracting the peak position as the lane line position;
step 1.3, lane line tracking: tracking the lane line by adopting Kalman filtering, and establishing a relation between a front epoch and a rear epoch by using a speed model, wherein a state equation is as follows:
wherein q isxAnd q isvRespectively, the process noise of the position x and the velocity v, k being the time;
the observation equation is:
yk+1＝xk+1+ε
wherein, yk+1The position of the currently extracted lane line is obtained; epsilon represents the extraction vehicleObservation noise of the lane position;
after filtering, obtaining the position of each window i
Performing quadratic curve fitting on the positions of the continuous windows to obtain the continuous and smooth lane line extraction in the whole time period, and realizing the smooth transition of lane line dotted lines and zebra stripes; and after the lane lines are successfully extracted, obtaining the x coordinates of the left and right lane lines at the bottom under a camera coordinate system, namely the distance between the camera and the left and right lane lines.
Further, the specific method for octree-assisted lane line fast search in step 2 of the present invention is:
step 2.1, searching nodes of the nearest lane line: according to the inertial navigation prediction position, searching a sub-node voxel corresponding to the prediction point from a space octree storage structure of the lane line; after finding the voxel, traversing all the lane line coordinate points in the voxel, and finding the nearest point according to the Euclidean geometric distance method;
step 2.2, consistency judgment: obtaining the advancing direction of the carrier according to course information in the posture of the position point predicted by inertial navigation, carrying out consistency judgment according to the advancing direction and the course of the searched lane line node, and if the difference between the two courses is greater than a threshold value, indicating that the searched lane line node is wrong;
according to the topological relation inspection of the lane line nodes successfully searched at the historical moment and the currently searched lane line nodes, whether the historical nodes and the current nodes have adjacent topological relation or not is judged, and if not, the searched lane line nodes are indicated to be wrong;
otherwise, the searched lane line node is correct.
Further, the elevation constraint observation equation of the lane line constructed in the step 3 of the invention is as follows:
wherein,
to forecast the distance, dIIs constant value, HlaneFor elevation-constrained coefficient matrices, δ re、δve、φ、ab、εbRespectively representing the position, the speed and the attitude error as well as the zero offset of an accelerometer and the zero offset of a gyroscope; the elevation-constrained coefficient matrix is represented as:
wherein, (A, B and C) are plane coefficients.
Further, in step 4 of the present invention, the lateral constraint observation equation of the camera center and the lane line is constructed as follows:
wherein f isL、fRRepresenting the observed distance from the left and right lane lines,
indicating the distance between the forecast location and the searched left and right lane lines,
and the partial derivatives of the observed values to the positions and the postures are represented.
Further, the observation equation of the odometer forward constraint constructed in step 5 of the present invention is:
wherein, δ vbA difference value representing the velocity of the odometer observed value and the inertial navigation forecast under the b system,
Representation inertial navigationThe predicted ECEF is a rotation matrix relative to b,
Velocity, l under ECEF system representing inertial navigation predictionbA lever arm for indicating the odometer and the inertial navigation,
Representing the rotational angular velocity of the earth.
Further, after the GNSS/lane line constraint/odometer fusion is fused in step 6 of the present invention, the obtained observation equation is:
wherein v isPAnd vLRepresenting the carrier and phase observations,
the direction cosine vector of the inter-station inter-satellite double-difference observed value is shown,
representing the lever arm of the antenna and inertial navigation under ECEF system, deltaN is a double-difference ambiguity state parameter, epsilonP、εL、εf、εd、
The sub-tables represent the observation noise of the carrier, the phase, the lane line lateral observation value, the lane line elevation observation value and the odometer observation value.
Further, the method for performing multi-source fusion in step 6 of the present invention specifically comprises:
and dynamically selecting whether to add lane line and odometer constraint, when the GNSS meets the observation condition, only using the GNSS/INS for positioning, when the GNSS is shielded or interrupted, adding the odometer constraint, and determining whether to add the lane line constraint according to the detection condition of the lane line.
The invention has the following beneficial effects: the GNSS/inertial navigation/lane line constraint/mileometer multi-source fusion method disclosed by the invention has the advantages that 1) the information positioning complementary characteristic of each sensor is fully utilized, so that the positioning constraint in three directions is realized, and the error divergence of inertial navigation in each direction is greatly inhibited. 2) Different sensors and different data are not influenced by each other, and the data can be checked and corrected by mutual constraint, so that double constraint guarantee is provided, and the positioning reliability is high. 3) According to different characteristics of typical urban environment, whether observation constraints of the information are added or not can be dynamically selected, and the positioning means is flexible. 4) The odometer and the vision sensor are low in cost, and most of vehicle carriers have the odometer and the vision sensor, so that the fusion positioning method is convenient to apply, and the cost is not increased basically. 5) Through the constraint of three directions, the GNSS interruption time allowed by the positioning method can be extended to about five minutes, and the positioning precision can still be maintained in the decimeter to centimeter level in the interruption time of five minutes.
Drawings
The invention will be further described with reference to the accompanying drawings and examples, in which:
FIG. 1 is a general flow chart of a GNSS/inertial navigation/lane line constraint/odometry multi-source fusion algorithm of an embodiment of the present invention;
FIG. 2 is a block diagram of an exemplary GNSS/inertial navigation combination of the present invention;
FIG. 3 is a flow chart of lane line extraction according to an embodiment of the present invention;
FIG. 4 is a flowchart of a nearest neighbor lane line search according to an embodiment of the present invention;
FIG. 5 is a lane line constraint diagram of an embodiment of the present invention;
FIG. 6 is a flow chart of multi-source fusion positioning according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The integral filtering framework of the invention is GNSS/INS tightly-combined extended Kalman filtering, the tightly-combined structure is shown in figure 2, a navigation coordinate system is selected as an ECEF system, and corresponding SINS mechanical arrangement is also carried out under the ECEF system. In the tight combination, the raw observations of the GNSS and the SINS are jointly input into a Kalman filter, the navigation parameters (position, velocity and attitude), the SINS system error and the GNSS related parameters (ambiguity) are jointly estimated, and the SINS system error is feedback corrected using a closed-loop correction technique. The GNSS/SINS tightly combined state model and the observation model are respectively as follows:
δz＝HδX+η (2)
in the formula (1), δ XSINS＝(δre δve φ ab εb)T，
The state parameters of the SINS and GNSS, respectively, due to the double-difference positioning mode, the GNSS receiver clock difference is eliminated, leaving only the inter-station differential ambiguity. F is a state differential equation coefficient matrix, and w is process noise; in the formula (2), δ z is an observed value residual error, H is a design matrix, and η is an observation noise.
Estimating by adopting Kalman filtering according to the tightly combined mathematical model, wherein the general solving step comprises two parts of state prediction and observation updating, and the method comprises the following steps:
and (3) one-step state prediction:
and (3) observation updating:
in the formula (3), Xk,k-1For the forecast of the state, phik,k-1Is a state transition matrix，Qk-1Is process noise, Δ tkIs the time interval between two epochs, Pk,k-1The variance is forecasted. In the formula (4), KkIs a gain factor, RkTo observe noise, XkAnd PkThe filter state and its variance.
The above tightly combined mathematical model and Kalman filter general solution steps are the basis of the algorithm of the present invention, and the key technologies of the modules and the implementation method thereof will be described in detail below with reference to the technical route shown in fig. 1.
Lane line extraction based on Kalman filtering
The lane line extraction method can be divided into two categories, namely a lane line extraction method based on road characteristics and a road model and a lane line extraction method based on deep learning. Deep learning has a good detection effect, but relies on a large amount of sample data to perform model training. At present, the traditional method is mainly used, the lane line is separated and obtained by using information, such as lane line characteristics, colors, structures and the like, different from the road background, the sliding window Kalman filtering method is adopted for improvement, the flow is shown in fig. 3, and the method mainly comprises three modules, namely preprocessing, lane line extraction and lane line tracking.
1) Pretreatment of
Before data acquisition, the monocular camera is subjected to monocular calibration through a Matlab calibration tool box to obtain camera internal parameters including a focal length fx、fyC displacement of image principal pointx、cyAnd the radial distortion coefficient and the tangential distortion coefficient of the camera. And during preprocessing, carrying out distortion correction on the lane line image acquired by the monocular camera by using the parameters. According to the installation position and the visual field range of the monocular camera, an interested area is defined, the interference of other irrelevant areas is eliminated, and the lane line is restored to the overlooking state by utilizing a homography transformation formula (5), wherein H is a corresponding homography matrix, u and v are coordinates under a pixel coordinate system, and [ X, Y, Z]The coordinates of the corresponding points in the overlooking state can be conveniently extracted and processed by the subsequent lane lines.
2) Lane line extraction
The lane line ortho-image is input, and the lane line ortho-image is binarized by color and threshold segmentation of edge information. Since the lane lines are generally white and yellow, and the lane lines are generally high in light reflection intensity and bright in color for guiding a driver to drive, the lane lines are detected by comprehensively using a B channel of an LAB color space, a luminance channel of an HSL color space, and a luminance channel of an HSV color space, so as to separate the lane lines from a background. After binarization, dividing a pixel window at the bottom of the image, performing left-to-right histogram statistics on the gray value of the window, then obtaining a gray mean curve graph by adopting Gaussian kernel smoothing, and extracting the peak position as the lane line position.
3) Lane line tracking
Because the lane line is continuously and slowly changed in the vehicle running process, according to the property, the Kalman filtering can be adopted to dynamically, continuously and smoothly track the lane line.
The relationship between the previous epoch and the next epoch is established by using a velocity model, and the state equation is as follows, wherein q isxAnd q isvProcess noise at position x and velocity v, respectively:
the observation equation is as follows, yk+1For the currently extracted lane line position:
yk+1＝xk+1+ε (7)
after filtering, obtaining the position of each window i
And finally, performing quadratic curve fitting on the positions of the continuous windows to obtain the continuous and smooth lane line extraction in the whole time period, so as to realize the smooth transition of the lane line dotted line and the zebra crossing.
After the lane lines are successfully extracted, the obtained left and right lane lines at the bottom are arranged in a camera coordinate systemX coordinate of lower, i.e. distance l between camera and left and right lane lineLAnd lRAnd feeding back to the GNSS/INS filter.
Two-and eight-branch tree-assisted lane line fast search
The lane line map contains all the information of the lane including coordinates, heading, curvature, color, topological relations, etc. In order to improve the efficiency of lane line searching, when lane line map data are stored, an octree structure is adopted, organization and management are carried out in a coordinate domain, a spatial index is established for point cloud coordinates, and spatial clustering and division of lane line coordinates are realized. Each node of the octree represents a cubic volume element, and a parent node can be divided into eight child nodes, and the division is carried out until the division is not possible. When the lane line map is stored, the size of the minimum voxel of the space octree is set to be 10m, so that for a lane with the length of 40km, at most 12 searches are needed to know the voxel at which the lane line coordinate is located.
1) Nearest lane node search
According to the inertial navigation forecast position, points shown in FIG. 5
And searching a sub-node voxel corresponding to the forecast point from the space octree storage structure of the lane line. After finding the voxel, traversing all the lane line coordinate points in the voxel, and finding the point closest to the voxel according to the Euclidean geometric distance method.
2) Consistency determination
And obtaining the advancing direction of the carrier according to the course information in the posture of the position point predicted by inertial navigation, carrying out consistency judgment according to the advancing direction and the course of the searched lane line node, and if the difference between the two courses is greater than a threshold value, indicating that the searched lane line node is wrong.
And according to the topological relation check between the lane line node successfully searched at the historical moment and the currently searched lane line node, judging whether the historical node and the current node have an adjacent topological relation, and if not, indicating that the searched lane line node is wrong.
If the above-mentioned consistency judgment is passed,it indicates that the searched lane line node is correct. Because the lane line where the carrier is actually located is a left and right continuous curve, and only two nearest lane line nodes are searched at present, n nodes are firstly pushed forwards and backwards according to curvature information of the nodes, wherein n depends on the curvature of the nodes, and if the curvature is large, n can be properly amplified. After extrapolation, 2 nodes P of the left lane line can be obtainedL1、PL2And 2 nodes P of the right lane lineR1、PR2As shown in fig. 5.
Thirdly, lane line assisted GNSS/INS elevation positioning constraint
The nearest left lane line node P has been obtainedL1、PL2And 2 nodes P of the right lane lineR1、PR2Theoretically, if there is no error in the inertial navigation prediction position, the center of the inertial navigation prediction position should be located on the lane surface after the vehicle height and the external parameter correction with the camera, so that the elevation constraint can be established.
Assuming that the distance from the inertial navigation center to the ground is known in advance, let dIThe coordinates of the four points of the left and right lane lines are also known as (P)L1,PL2,PR1,PR2) The ground plane can be expressed as a · X + B · Y + C · Z +1, which is 0, and the plane coefficients (a, B, C) are obtained by substituting the four-point coordinates of the lane lines, and the distance from the inertial navigation center to the ground plane is obtained by the equation of the point-to-plane distance:
since the vehicle must be above the road surface, the absolute sign in the equation can be removed, and finally expressed as the observation equation of inertial navigation position error:
in the formula,
for forecasting distanceFrom, the predicted value of inertial navigation position is substituted into the formula (8), HlaneThe coefficient matrix, which is an elevation constraint, is expressed as follows:
the resulting observation equation is as follows:
dIthe calculation can be carried out in real time if the calculation is constant and is not measured in advance. Under the condition of good GNSS signals, the GNSS/SINS combination can obtain a centimeter-level positioning result, and the vertical distance from the inertial navigation center to the ground is directly calculated and stored for subsequent elevation constraint.
Fourth, lane line assisted GNSS/INS lateral positioning constraint
The lateral constraint equation needs to take the center of a camera as a medium, the horizontal distance from the center of the camera to a lane line is obtained through lane line extraction and homography transformation, and the distances between the left lane line and the right lane line are respectively recorded as dLAnd dR. Assume that the position of the center of the camera is PcIt has a height to the road surface, so to calculate the horizontal distance of the camera to the lane line, the center of the camera must be on the same plane as the road surface.
For unified calculation, when an observation equation needs to be constructed again, the road surface is lifted to the center of the camera, and the specific method is as follows: 1) obtaining the distance d from the center of the camera to the road surfacec. In step 4, the distance from the inertial navigation center to the road surface is obtained, and the space between the inertial navigation and the camera is calibrated in advance before data acquisition, and is known. Thus, the distance from the center of the camera to the road surface
The vertical distance between the two; 2) will road surface coordinate (P)L1,PL2,PR1,PR2) Elevation due to ground plane coefficient
I.e. the unit normal vector of the plane, so that the road coordinates plus the same vector increment
Obtaining a New road surface coordinate (P'L1,P′L2,P′R1,P′R2) I.e. the lane line raised to the center of the camera.
At the moment, two vectors are constructed, cross multiplication is carried out to obtain four parallel areas enclosed by the two vectors, then the four parallel areas are divided by the bottom edge, and the height is obtained through calculation, namely the horizontal distance from the center of the camera to the lane line:
(Pc-P′L1)×mLthe vector cross-product expansion is expressed as:
squaring the distance between two sides of the (12) formula to obtain a new observed quantity, and obtaining a square distance f by using an MATLAB algebra toolboxL＝(dL·||mL||)2To PcPartial derivatives of (a):
in the formula, M1、M2And M3Is represented as follows:
camera center coordinate PcAnd inertial navigation center coordinate PSINSThe relationship is as follows:
in the formula,
lbis the lever arm component of the camera center in the inertial navigation coordinate system. Thereby obtaining PcThe partial derivative of the inertial navigation pose error is as follows:
in the formula, re＝PSINSIn order to be consistent with the equation of state. According to a chain rule, obtaining a partial derivative of the square distance f to the inertial navigation pose error according to the formula (14) and the formula (17):
finally, the observation equation is obtained as:
fifthly, odometer-assisted GNSS/INS forward positioning constraint
There are two assumptions about forward constraints using a wheel odometer: 1) the moving carrier does not sideslip and moves along the advancing direction of a carrier coordinate system, namely the lateral speed is zero; 2) the carrier moves against the ground surface without fluctuation up and down, i.e. the vertical speed is zero.
The forward speed obtained from the odometer is:
in the formula,
the speed of the odometer is converted to the speed under the inertial navigation system,
is the speed of the odometer, only has the speed in the forward direction, the other two shafts are zero,
lbthe position vector of the odometer center under the inertial navigation system is corrected, namely the arm correction of the odometer.
And deducing an observation model of the odometer constraint from the formula, wherein the position and the speed state are based on the SINS center. By adopting the perturbation method, the following can be obtained:
at this time, the derived observation equation is as follows:
the above is a strictly derived observation model, but the model can be simplified according to actual situations.
When the vehicle runs straight, the inertial navigation system does not rotate relative to the ground,
and when the speed of the odometer is close to zero, the speed of the odometer is equal to the advancing speed of the inertial navigation system. When the vehicle is turning around, the vehicle is,
calculated to obtain
The speed component can appear in the direction finding (X axis) and is inconsistent with the non-integrity constraint assumption, because the vehicle turns with the rear wheel as a fixed axis and the turning speed can appear at the point which is not on the axle surface of the rear wheel, but in practice, the ground vehicle turns slowly, the speed component is small, the point is slightly amplified to observe noise, and the situation that v is directly considered to be that v is slightly amplified to observe noise can be directly consideredSINS＝vDMI. The observation equation can now be simplified to:
in the formula,
and
for the values obtained for the mechanical programming at the current moment, the index 2 indicates taking the second action:
six, multi-source fusion observation update
In the steps, the equation of lane line elevation and lateral constraint and the forward constraint observation equation of the odometer are obtained. The whole fusion positioning system dynamically adds a lane line constraint observation equation and a mileometer observation equation on the basis of a differential GNSS/SINS tight combination, as shown in FIG. 6.
When the number of GNSS satellites is large and the PDOP value is small, the GNSS positioning geometric condition meets the requirement, at the moment, the observation of lane lines and a mileometer can be omitted, and the state equation is still unchanged.
When the number of GNSS satellites is small and the lane line detection is successful, adding a lateral and elevation constraint equation of the lane line into the observation equation.
In an urban scene of a high-rise forest on two sides of a road, GNSS signals are shielded seriously, but the shielding in the front-back direction of the road is less, GNSS satellites are still visible, at the moment, a satellite above a carrier, namely the satellite with the highest altitude angle, is selected as a reference satellite, the reference satellite and the visible satellites in the front-back direction of the road form a double-difference observation value to participate in positioning and resolving, and meanwhile, forward constraint of a speedometer is added. When the vehicle is under an overhead bridge, in a tunnel and other scenes, the GNSS satellites are completely invisible, and at the moment, the forward direction is only restricted by odometer errors.
The observation equation for completely establishing the fusion of the GNSS/lane line constraint/odometer is as follows:
vPand vLRepresenting the carrier and phase observations, with epsilon being the noise of each observation.
It will be understood that modifications and variations can be made by persons skilled in the art in light of the above teachings and all such modifications and variations are intended to be included within the scope of the invention as defined in the appended claims.
Claims (8)
1. A GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method is characterized in that in the method, images are collected through a vision sensor, and a lane line map database is generated in an off-line manner; when the carrier carries out real-time calculation navigation positioning, based on the tight combination of a differential GNSS/INS, when the GNSS is shielded, the relative relation between the carrier and a lane line is detected through a visual sensor arranged on the carrier, and a lane line constraint observation equation is dynamically added to assist lateral and elevation positioning under the assistance of a lane line map; forming a differential observation value by using observable satellites above the carrier and in the road advancing direction to participate in tight combination, and restraining forward error divergence of the carrier; the method specifically comprises the following steps:
step 1, preprocessing an image collected in real time to obtain an overlook image of a lane line image, performing color space conversion on the image to separate a lane line, determining the coordinate of the lane line based on the gray histogram statistics of a sliding window, and finally tracking the lane line by using Kalman filtering based on uniform velocity hypothesis to obtain the distance between the center of a continuous and smooth camera and the left and right images;
step 2, quickly searching the minimum voxel of the inertial navigation forecasting position by using an octree data structure, finding the nearest lane line node by using an Euclidean geometric distance method, carrying out course/topology consistency check on the node, and returning the best matching P according to the curvature after passing the checkL1、PL2、PR1、PR2；
Step 3, according to the inertial navigation forecast position, constructing a lane line elevation constraint observation equation under the assumption that the vehicle height and the camera are subjected to external parameter correction and the vehicle is located on a lane surface;
step 4, adding PL1、PL2、PR1、PR2Lifting the four points to the center of the camera, constructing a lateral constraint observation equation of the center of the camera and the lane line according to the assumption that the distance between the predicted value of the center of the camera and the lane line is equal to the observation distance, and converting the equation to the inertial navigation center by utilizing a chain rule;
step 5, constructing an observation equation of forward constraint of the odometer according to the assumptions that the carrier slips less during linear motion and turns at a lower speed;
and 6, realizing multi-source fusion observation updating according to the elevation of the lane line, the equation of lateral constraint and the forward constraint observation equation of the odometer.
2. The GNSS/inertial navigation/lane line constraint/odometry multi-source fusion method according to claim 1, wherein the method for performing lane line tracking in step 1 specifically comprises:
step 1.1, pretreatment: performing monocular calibration on the monocular camera through a Matlab calibration tool box to obtain camera internal parameters, and performing distortion correction on the lane line image acquired by the monocular camera by using the camera internal parameters; according to the installation position and the visual field range of the monocular camera, an interested area is defined, the interference of other irrelevant areas is eliminated, and the lane line is restored to the overlook state by utilizing homography transformation;
step 1.2, lane line extraction: inputting a lane line orthoimage, and binarizing the lane line orthoimage by color and threshold segmentation of edge information; dividing a pixel window at the bottom of the image, performing histogram statistics on the gray value of the window from left to right, obtaining a gray mean curve graph by adopting Gaussian kernel smoothing, and extracting the peak position as the lane line position;
step 1.3, lane line tracking: tracking the lane line by adopting Kalman filtering, and establishing a relation between a front epoch and a rear epoch by using a speed model, wherein a state equation is as follows:
wherein q isxAnd q isvRespectively, the process noise of the position x and the velocity v, k being the time;
the observation equation is:
yk+1＝xk+1+ε
wherein, yk+1The position of the currently extracted lane line is obtained; epsilon represents the observation noise of the extracted lane line position;
after filtering, obtaining the position of each window i
Performing quadratic curve fitting on the positions of the continuous windows to obtain the continuous and smooth lane line extraction in the whole time period, and realizing the smooth transition of lane line dotted lines and zebra stripes; after the lane lines are successfully extracted, the x coordinates of the left and right lane lines at the bottom under the camera coordinate system, namely the distance between the camera and the left and right lane lines, are obtained.
3. The GNSS/inertial navigation/lane line constraint/odometry multi-source fusion method according to claim 2, wherein the specific method for octree-assisted lane line fast search in step 2 is as follows:
step 2.1, searching nodes of the nearest lane line: according to the inertial navigation prediction position, searching a sub-node voxel corresponding to the prediction point from a space octree storage structure of the lane line; after finding the voxel, traversing all the lane line coordinate points in the voxel, and finding the nearest point according to the Euclidean geometric distance method;
step 2.2, consistency judgment: obtaining the advancing direction of the carrier according to course information in the posture of the position point predicted by inertial navigation, carrying out consistency judgment according to the advancing direction and the course of the searched lane line node, and if the difference between the two courses is greater than a threshold value, indicating that the searched lane line node is wrong;
according to the topological relation inspection of the lane line nodes successfully searched at the historical moment and the currently searched lane line nodes, whether the historical nodes and the current nodes have adjacent topological relation or not is judged, and if not, the searched lane line nodes are indicated to be wrong;
otherwise, the searched lane line node is correct.
4. The GNSS/inertial navigation/lane line constraint/odometer multi-source fusion method according to claim 1, wherein the lane line elevation constraint observation equation constructed in step 3 is:
wherein,
to forecast the distance, dIIs constant value, HlaneFor elevation-constrained coefficient matrices, δ re、δve、φ、ab、εbRespectively representing position error, speed error, attitude error, accelerometer zero offset and gyroscope zero offset; the elevation-constrained coefficient matrix is represented as:
wherein A, B and C are plane coefficients.
5. The GNSS/inertial navigation/lane line constraint/odometry multi-source fusion method according to claim 2, wherein the lateral constraint observation equation of the camera center and the lane line is constructed in step 4 as follows:
wherein f isL、fRRepresenting the observed distance from the left and right lane lines,
indicating the distance between the forecast location and the searched left and right lane lines,
and the partial derivatives of the observed values to the positions and the postures are represented.
6. The GNSS/inertial navigation/lane line constraint/odometer multi-source fusion method according to claim 5, wherein the observation equation of the odometer forward constraint constructed in step 5 is:
wherein, δ vbA difference value representing the velocity of the odometer observed value and the inertial navigation forecast under the b system,
The ECEF system representing the inertial navigation prediction is a rotation matrix relative to the b system,
Velocity, l under ECEF system representing inertial navigation predictionbA lever arm for indicating the odometer and the inertial navigation,
Representing the earth's own movementThe angular velocity.
7. The GNSS/inertial navigation/lane line constraint/odometer multi-source fusion method according to claim 6, wherein in step 6, after the GNSS/lane line constraint/odometer fusion, the obtained observation equation is:
wherein v isPAnd vLRepresenting the carrier and phase observations,
the direction cosine vector of the inter-station inter-satellite double-difference observed value is shown,
representing the lever arm of the antenna and inertial navigation under ECEF system, deltaN is a double-difference ambiguity state parameter, epsilonP、εL、εf、εd、
Respectively representing the observation noise of the carrier wave, the observation noise of the phase, the observation noise of the lane line lateral observation value, the observation noise of the lane line elevation observation value and the observation noise of the odometer observation value.
8. The GNSS/inertial navigation/lane line constraint/odometer multi-source fusion method according to claim 1, wherein the method for performing multi-source fusion in step 6 specifically comprises:
and dynamically selecting whether to add lane line and odometer constraint, when the GNSS meets the observation condition, only using the GNSS/INS for positioning, when the GNSS is shielded or interrupted, adding the odometer constraint, and determining whether to add the lane line constraint according to the detection condition of the lane line.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910659041.4A CN110411462B (en) | 2019-07-22 | 2019-07-22 | GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910659041.4A CN110411462B (en) | 2019-07-22 | 2019-07-22 | GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110411462A CN110411462A (en) | 2019-11-05 |
| CN110411462B true CN110411462B (en) | 2021-05-18 |
Family
ID=68362292
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910659041.4A Active CN110411462B (en) | 2019-07-22 | 2019-07-22 | GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110411462B (en) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110866482B (en) * | 2019-11-08 | 2022-09-16 | 广东工业大学 | Dynamic selection method, device and equipment for odometer data source |
| CN110888440A (en) * | 2019-11-28 | 2020-03-17 | 山东三木环保工程有限公司 | Rail vehicle door alignment system and method combining GNSS satellite positioning and shielding plate |
| CN112945266B (en) * | 2019-12-10 | 2024-07-19 | 炬星科技（深圳）有限公司 | Laser navigation robot and odometer calibration method thereof |
| CN111026081B (en) * | 2019-12-10 | 2021-05-11 | 苏州智加科技有限公司 | Error calculation method, device, equipment and storage medium |
| CN111274343B (en) * | 2020-01-20 | 2023-11-24 | 阿波罗智能技术(北京)有限公司 | Vehicle positioning method and device, electronic equipment and storage medium |
| CN111380516B (en) * | 2020-02-27 | 2022-04-08 | 上海交通大学 | Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information |
| CN111272165B (en) * | 2020-02-27 | 2020-10-30 | 清华大学 | Intelligent vehicle positioning method based on characteristic point calibration |
| CN112581795B (en) * | 2020-12-16 | 2022-04-29 | 东南大学 | Video-based real-time early warning method and system for ship bridge and ship-to-ship collision |
| CN112596089B (en) * | 2021-03-02 | 2021-05-18 | 腾讯科技（深圳）有限公司 | Fusion positioning method and device, electronic equipment and storage medium |
| CN114136315B (en) * | 2021-11-30 | 2024-04-16 | 山东天星北斗信息科技有限公司 | Monocular vision-based auxiliary inertial integrated navigation method and system |
| CN114646992B (en) * | 2022-03-21 | 2024-08-02 | 腾讯科技（深圳）有限公司 | Positioning method, apparatus, computer device, storage medium and computer program product |
| TWI814480B (en) * | 2022-07-11 | 2023-09-01 | 新馳科技股份有限公司 | Vehicle positioning system and vehicle positioning method |
| CN116129389B (en) * | 2023-03-27 | 2023-07-21 | 浙江零跑科技股份有限公司 | Lane line acquisition method, computer equipment, readable storage medium and motor vehicle |
| CN116540286B (en) * | 2023-07-06 | 2023-08-29 | 中国科学院空天信息创新研究院 | Multi-source robust fusion positioning method based on non-integrity constraint |
| CN116642501B (en) * | 2023-07-25 | 2023-09-29 | 齐鲁空天信息研究院 | Multi-source fusion method for auxiliary positioning of lane lines with inertia as core |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107229063A (en) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | A kind of pilotless automobile navigation and positioning accuracy antidote merged based on GNSS and visual odometry |
| CN107728175A (en) * | 2017-09-26 | 2018-02-23 | 南京航空航天大学 | The automatic driving vehicle navigation and positioning accuracy antidote merged based on GNSS and VO |
| US10151588B1 (en) * | 2016-09-28 | 2018-12-11 | Near Earth Autonomy, Inc. | Determining position and orientation for aerial vehicle in GNSS-denied situations |
| CN109405824A (en) * | 2018-09-05 | 2019-03-01 | 武汉契友科技股份有限公司 | A kind of multi-source perceptual positioning system suitable for intelligent network connection automobile |
-
2019
- 2019-07-22 CN CN201910659041.4A patent/CN110411462B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10151588B1 (en) * | 2016-09-28 | 2018-12-11 | Near Earth Autonomy, Inc. | Determining position and orientation for aerial vehicle in GNSS-denied situations |
| CN107229063A (en) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | A kind of pilotless automobile navigation and positioning accuracy antidote merged based on GNSS and visual odometry |
| CN107728175A (en) * | 2017-09-26 | 2018-02-23 | 南京航空航天大学 | The automatic driving vehicle navigation and positioning accuracy antidote merged based on GNSS and VO |
| CN109405824A (en) * | 2018-09-05 | 2019-03-01 | 武汉契友科技股份有限公司 | A kind of multi-source perceptual positioning system suitable for intelligent network connection automobile |
Non-Patent Citations (1)
| Title |
|---|
| 融合视觉的智能车组合导航技术分析;曾庆喜等;《导航定位学报》;20170630;第2.2-2.3节 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110411462A (en) | 2019-11-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110411462B (en) | GNSS/inertial navigation/lane line constraint/milemeter multi-source fusion method | |
| CN111551958B (en) | Mining area unmanned high-precision map manufacturing method | |
| US11002859B1 (en) | Intelligent vehicle positioning method based on feature point calibration | |
| Ding et al. | LiDAR inertial odometry aided robust LiDAR localization system in changing city scenes | |
| CN110412635B (en) | GNSS/SINS/visual tight combination method under environment beacon support | |
| Rose et al. | An integrated vehicle navigation system utilizing lane-detection and lateral position estimation systems in difficult environments for GPS | |
| CN107782321B (en) | Combined navigation method based on vision and high-precision map lane line constraint | |
| JP2022019642A (en) | Positioning method and device based upon multi-sensor combination | |
| CN114199240B (en) | Two-dimensional code, laser radar and IMU fusion positioning system and method without GPS signal | |
| CN114199259B (en) | Multi-source fusion navigation positioning method based on motion state and environment perception | |
| Tao et al. | Lane marking aided vehicle localization | |
| JP5162849B2 (en) | Fixed point position recorder | |
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| CN113654555A (en) | Automatic driving vehicle high-precision positioning method based on multi-sensor data fusion | |
| CN107246868A (en) | A kind of collaborative navigation alignment system and navigation locating method | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN107229063A (en) | A kind of pilotless automobile navigation and positioning accuracy antidote merged based on GNSS and visual odometry | |
| CN115407357A (en) | Low-beam laser radar-IMU-RTK positioning mapping algorithm based on large scene | |
| CN114019552A (en) | Bayesian multi-sensor error constraint-based location reliability optimization method | |
| Xiao et al. | LIO-vehicle: A tightly-coupled vehicle dynamics extension of LiDAR inertial odometry | |
| Wang et al. | Simultaneous localization of rail vehicles and mapping of surroundings with LiDAR-inertial-GNSS integration | |
| Deusch et al. | Improving localization in digital maps with grid maps | |
| CN116929363A (en) | Mining vehicle autonomous navigation method based on passable map | |
| CN113777589B (en) | LIDAR and GPS/IMU combined calibration method based on point characteristics | |
| Huang et al. | Wheel odometry aided visual-inertial odometry for land vehicle navigation in winter urban environments |
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 |