CN116086453A - Inertial navigation and map combined positioning method based on probability optimization calculation - Google Patents
Inertial navigation and map combined positioning method based on probability optimization calculation Download PDFInfo
- Publication number
- CN116086453A CN116086453A CN202211588179.8A CN202211588179A CN116086453A CN 116086453 A CN116086453 A CN 116086453A CN 202211588179 A CN202211588179 A CN 202211588179A CN 116086453 A CN116086453 A CN 116086453A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- point
- candidate
- candidate road
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 48
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000005457 optimization Methods 0.000 title claims abstract description 17
- 239000013307 optical fiber Substances 0.000 claims abstract description 7
- 238000012216 screening Methods 0.000 claims description 16
- 238000009826 distribution Methods 0.000 claims description 8
- 238000005315 distribution function Methods 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 7
- 206010034701 Peroneal nerve palsy Diseases 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 239000000835 fiber Substances 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000004519 manufacturing process 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
- 230000008447 perception Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram 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/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
- G01C19/58—Turn-sensitive devices without moving masses
- G01C19/64—Gyrometers using the Sagnac effect, i.e. rotation-induced shifts between counter-rotating electromagnetic beams
- G01C19/72—Gyrometers using the Sagnac effect, i.e. rotation-induced shifts between counter-rotating electromagnetic beams with counter-rotating light beams in a passive ring, e.g. fibre laser gyrometers
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Power Engineering (AREA)
- Navigation (AREA)
Abstract
The invention discloses an inertial navigation and map combined positioning method based on probability optimization calculation, which relates to the field of navigation positioning, and is characterized in that a local geographic coordinate system is defined as a navigation system, denoted as an n system, x, y and z axes of the navigation system point to the east and north respectively, an inertial navigation coordinate system is defined as a body system, denoted as a b system, and map matching is performed by using an optical fiber inertial navigation position updating result and a vector map, so that lane-level high-precision positioning of an automatic driving automobile under the condition that satellite signals are unavailable can be realized, and candidate road sections are pre-screened by estimating positioning errors of inertial navigation, thereby effectively improving matching efficiency.
Description
Technical Field
The invention relates to the technical field of navigation positioning, in particular to a combined navigation and map positioning method based on probability optimization calculation.
Background
With the development and mass production of vehicle-mounted sensors, unmanned technology has become a research hotspot, which means that vehicles can perform motion control by themselves according to their own perception and understanding of ambient conditions, and reach the level of human drivers. In order to determine the position of a vehicle in the environment, a positioning module plays a very important role in autonomous driving. One of the typical combinations of conventional inertial navigation methods is an IMU (inertial measurement unit) and an odometer, which can achieve high-precision positioning in GNSS (global navigation satellite system) reject environments, but long-term operation can lead to error accumulation. To solve this problem, a comprehensive positioning method using other information to assist inertia is sequentially proposed. Common sensors include lidar, cameras and millimeter wave radar. These solutions are very susceptible to environmental influences, resulting in a large error in the positioning results.
Disclosure of Invention
Aiming at the defects existing in the prior art, the invention aims to provide the inertial navigation and map combined positioning method based on probability optimization calculation, which solves the problem that the positioning error is large under the condition that the satellite signal is unavailable in the existing combined positioning scheme. The invention provides a combined positioning method of inertial navigation and a vector map based on probability optimization calculation, which utilizes an optical fiber inertial navigation position updating result to carry out map matching with the vector map, and can realize the lane-level high-precision positioning of an automatic driving automobile under the condition that satellite signals are unavailable.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a combined inertial navigation and map positioning method based on probability optimization calculation comprises the following steps:
step one: matching and positioning are carried out through the output track of inertial navigation and a local vector map, a local geographic coordinate system is marked as a navigation system, the navigation system is marked as an n system, and the inertial navigation coordinate system is marked as a body system and is marked as a b system;
step two: marking the trace of inertial navigation output as a set of points as O { O1, O2, O3 … on }, wherein O1, O2 … … on are single points;
step three: the marking vector map is that the set of road segments is Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn is a single road segment;
step four: screening candidate road segments according to track points output by inertial navigation, assuming the output point of the inertial navigation at the moment t as ot, drawing a circle by taking the output point as a circle center and the radius r, and selecting the road segments in the circle or a part of the road segments in the circle as candidate road segments;
step five: calculating the distance from a locating point to each candidate road section, wherein the calculating method is to solve the distance from the locating point to two endpoints of the candidate road section and the distance from the locating point to the drop foot, if the drop foot is on an extension line of the candidate road section, the calculation is not performed, the shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the locating point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i＝1,2…n)；
Step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is: when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3, selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two end points of the road section with the distance from the foot drop to the inertial navigation track output point, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Furthermore, the inertial navigation is composed of a triaxial fiber optic gyroscope and a triaxial accelerometer, and all axes of the triaxial fiber optic gyroscope and the triaxial accelerometer have the same direction.
Further, the x, y, z axes of the n-series point to the east, north, and upward in the opposite direction of gravity, respectively.
Further, the x, y, z axes of the b system form a right hand coordinate system.
Further, in the fourth step, the value of the radius r is determined according to the average value of the estimated inertial navigation errors.
Further, F1 represents the probability that the anchor point is on the i-th candidate segment.
Further, wherein D mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent locating point observations, wherein the locating point observations are matching points on the candidate road section corresponding to the points outputted by inertial navigation, namely the end points or the feet of the candidate road section.
Compared with the prior art, the invention has the following beneficial effects:
the invention provides a combined positioning method of inertial navigation and a vector map based on probability optimization calculation, which utilizes an optical fiber inertial navigation position updating result to carry out map matching with the vector map, can realize the high-precision positioning of a lane level of an automatic driving automobile under the condition that satellite signals are unavailable, can realize the high-precision positioning of an unmanned automobile under the GPS rejection condition, preselects candidate road sections by estimating the positioning error of the inertial navigation, and effectively improves the screening efficiency.
Drawings
FIG. 1 is a flow chart of the present invention;
fig. 2 is a schematic diagram of screening candidate road segments according to the present invention.
Detailed Description
The invention provides a combined inertial navigation and map positioning method based on probability optimization calculation, and the unmanned technology becomes a research hot spot along with development and mass production of vehicle-mounted sensors at present, which means that a vehicle can automatically perform motion control according to the perception and understanding of surrounding environment conditions and reach the level of human drivers. In order to determine the position of a vehicle in the environment, a positioning module plays a very important role in autonomous driving. One of the typical combinations of conventional inertial navigation methods is an IMU (inertial measurement unit) and an odometer, which can achieve high-precision positioning in GNSS (global navigation satellite system) reject environments, but long-term operation can lead to error accumulation. To solve this problem, a comprehensive positioning method using other information to assist inertia is sequentially proposed. Common sensors include lidar, cameras and millimeter wave radar. These solutions are very susceptible to environmental influences, resulting in a large error in the positioning results. The invention solves the problem of large positioning error of the existing combined positioning scheme under the condition that satellite signals are unavailable. The invention provides a combined positioning method of inertial navigation and a vector map based on probability optimization calculation, which utilizes an optical fiber inertial navigation position updating result to carry out map matching with the vector map, and can realize the lane-level high-precision positioning of an automatic driving automobile under the condition that satellite signals are unavailable.
Example 1
Referring to fig. 1 to 2, a combined inertial navigation and map positioning method based on probability optimization calculation defines a local geographic coordinate system as a navigation system, denoted as an n-system, with x, y and z axes pointing to east, north and sky along the opposite direction of gravity, respectively; the inertial navigation consists of a triaxial fiber optic gyroscope and a triaxial accelerometer; defining an inertial navigation coordinate system as a body system, and marking as a b system, wherein x, y and z axes of the b system form a right-hand coordinate system; the three-axis optical fiber gyroscope and the three-axis accelerometer have the same direction. The invention is further illustrated below with reference to examples.
The method specifically comprises the following steps:
step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the locating point to two endpoints of the candidate road section and the drop foot distance. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observed value is a matching point of the point output by inertial navigation corresponding to the candidate road section, namely the end point of the candidate road section or the drop foot (the drop foot is applicable in the case of the candidate road section);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two endpoints of the road section and the distance from the drop foot (if the drop foot is applicable to the candidate road section) to the inertial navigation track output point, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Example 2
The method specifically comprises the following steps:
step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the solving point to the foot drop of the candidate road section. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observed value is a matching point of the point output by inertial navigation corresponding to the candidate road section, namely the end point of the candidate road section or the drop foot (the drop foot is applicable in the case of the candidate road section);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
wherein N is as followsShowing the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two endpoints of the road section and the distance from the drop foot (if the drop foot is applicable to the candidate road section) to the inertial navigation track output point, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Example 3
The method comprises the following steps:
step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the locating point to two endpoints of the candidate road section and the drop foot distance. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: definition transferThe probability is that the automatic driving automobile is from the road section q from the time t to the time t+1 i Move to road segment q j Denoted as F2, the calculation method is:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observed value is a matching point of the point output by inertial navigation corresponding to the candidate road section, namely the end point of the candidate road section or the drop foot (the drop foot is applicable in the case of the candidate road section);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two endpoints of the road section and the distance from the drop foot (if the drop foot is applicable to the candidate road section) to the inertial navigation track output point, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Example 4
The method comprises the following steps:
step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the solving point to the foot drop of the candidate road section. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observations are matches of points of inertial navigation output to candidate road segmentsPoints, namely the end points of the candidate road segments or the drop feet (the drop feet are applicable in the case of the candidate road segments);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two endpoints of the road section and the distance from the drop foot (if the drop foot is applicable to the candidate road section) to the inertial navigation track output point, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Example 5
Step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the locating point to two endpoints of the candidate road section and the drop foot distance. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observed value is a matching point of the point output by inertial navigation corresponding to the candidate road section, namely the end point of the candidate road section or the drop foot (the drop foot is applicable in the case of the candidate road section);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the distances from two endpoints of the road section to the output point of the inertial navigation track, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
Example 6
Step one: matching and positioning are carried out through the inertial navigation output track and the map of the environment, and the flow is shown in figure 1;
step two: recording the locus output by inertial navigation as a set O { O1, O2, O3 … on }, and O1, O2 … … on as a single point;
step three: the vector map is a set Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn of road segments is a single road segment;
step four: firstly, screening candidate road segments according to track points output by inertial navigation, wherein the screening method is shown in figure 2, the output point of the inertial navigation at the moment t is assumed to be ot, a circle is drawn by taking the output point as a circle center, and the road segments in the circle or a part of road segments in the circle are selected as candidate road segments;
step five: the distance from the locating point to each candidate road section is calculated, and the calculation method is to calculate the distance from the locating point to two endpoints of the candidate road section and the drop foot distance. If the drop foot is on the extension of the candidate segment, it is not calculated. The shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the candidate point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i=1, 2 … n) representing the probability of the anchor point being on the i-th candidate road segment;
step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i The probability of moving to road segment qj, denoted as F2, is calculated by:
when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
wherein D is mean Represents the average length, d (o) t →o t+1 ) mean Representing the average distance between two adjacent setpoint observations. The setpoint observed value is a matching point of the point output by inertial navigation corresponding to the candidate road section, namely the end point of the candidate road section or the drop foot (the drop foot is applicable in the case of the candidate road section);
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3; calculating F, and selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the distance from the foot drop to the inertial navigation track output point on the candidate road section, and selecting the point with the shortest distance as the final position estimation of the unmanned vehicle.
The above description is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above examples, and all technical solutions belonging to the concept of the present invention belong to the protection scope of the present invention. It should be noted that modifications and adaptations to those skilled in the art without departing from the principles of the present invention are intended to be considered as protecting the scope of the present template.
In the description of the present invention, it should be understood that the terms "upper," "lower," "left," "right," and the like indicate an orientation or a positional relationship based on that shown in the drawings, and are merely for convenience of description and for simplifying the description, and do not indicate or imply that the apparatus or element in question must have a specific orientation, as well as a specific orientation configuration and operation, and thus should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include one or more such feature. In the description of the present invention, unless otherwise indicated, the meaning of "a plurality" is two or more.
In the description of the present invention, it should be noted that, unless explicitly specified and limited otherwise, the terms "mounted," "connected," and the like are to be construed broadly and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood in specific cases by those of ordinary skill in the art.
The foregoing describes one embodiment of the present invention in detail, but the description is only a preferred embodiment of the present invention and should not be construed as limiting the scope of the invention. All equivalent changes and modifications within the scope of the present invention are intended to be covered by the present invention.
Claims (7)
1. The inertial navigation and map combined positioning method based on probability optimization calculation is characterized by comprising the following steps of:
step one: matching and positioning are carried out through the output track of inertial navigation and a local vector map, a local geographic coordinate system is marked as a navigation system, the navigation system is marked as an n system, and the inertial navigation coordinate system is marked as a body system and is marked as a b system;
step two: marking the trace of inertial navigation output as a set of points as O { O1, O2, O3 … on }, wherein O1, O2 … … on are single points;
step three: the marking vector map is that the set of road segments is Q { Q1, Q2, Q3 … … qn }, Q1, Q2 … … qn is a single road segment;
step four: screening candidate road segments according to track points output by inertial navigation, assuming the output point of the inertial navigation at the moment t as ot, drawing a circle by taking the output point as a circle center and the radius r, and selecting the road segments in the circle or a part of the road segments in the circle as candidate road segments;
step five: calculating the distance from a locating point to each candidate road section, wherein the calculating method is to solve the distance from the locating point to two endpoints of the candidate road section and the distance from the locating point to the drop foot, if the drop foot is on an extension line of the candidate road section, the calculation is not performed, the shortest distance from the locating point to each candidate road section is recorded as a set D { D1, D2 … … dn }, and n is the number of candidate road sections corresponding to the locating point;
step six: defining initial distribution probability as F1, and calculating method as F1=1/d i (i＝1,2…n)；
Step seven: defining transition probability as distance q from time t to time t+1 of automatic driving automobile i Move to road segment q j Denoted as F2, the calculation method is: when two locating point observed values correspond to the same matched road section:
when two anchor point observations are directly adjacent to a road segment:
step eight: defining the observation probability as t time of automatic driving automobile at candidate road section q j The locating point o is observed in t Denoted as F3, the calculation method is:
where N represents the number of segments in the candidate segment set, d (o t →q j ) Refers to the setpoint observed value o t To candidate road segment q j Is a Euclidean distance of (2);
step nine: defining a joint density distribution function F, wherein the calculation method comprises the following steps: f=f1×f2×f3, selecting a road segment set corresponding to the maximum value of F as an optimal path;
step ten: estimating the real position of the unmanned vehicle on the selected optimal path, comparing the two endpoints of the road section with the distance from the foot drop to the inertial navigation track output point, and selecting the shortest distance point as the final position estimation of the unmanned vehicle.
2. The combined inertial navigation and map positioning method based on probability optimization calculation according to claim 1, wherein the inertial navigation is composed of a three-axis optical fiber gyroscope and a three-axis accelerometer, and all axes of the three-axis optical fiber gyroscope and the three-axis accelerometer are identical in pointing direction.
3. The combined inertial navigation and map positioning method according to claim 2, wherein the x, y, z axes of the n-series are directed to the east, north, and the opposite direction of gravity, respectively.
4. A combined inertial navigation and map positioning method based on probability optimization calculation according to claim 3, wherein x, y and z axes of the b-system form a right-hand coordinate system.
5. The combined inertial navigation and map positioning method based on probability optimization calculation as set forth in claim 4, wherein the magnitude of the radius r in the fourth step is determined according to an average value of estimated inertial navigation errors.
6. The combined inertial navigation and map positioning method based on probability optimization calculation of claim 5, wherein F1 represents a probability of the positioning point on the i-th candidate road segment.
7. The combined inertial navigation and map positioning method based on probability optimization calculation of claim 6, wherein D mean Represents the average length, d (o) t →o t+1 ) mean Representing two adjacentThe average distance between the locating point observation values is that the point output by inertial navigation corresponds to the matching point on the candidate road section, namely the end point or the drop foot of the candidate road section.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211588179.8A CN116086453B (en) | 2022-12-12 | 2022-12-12 | Inertial navigation and map combined positioning method based on probability optimization calculation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211588179.8A CN116086453B (en) | 2022-12-12 | 2022-12-12 | Inertial navigation and map combined positioning method based on probability optimization calculation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116086453A true CN116086453A (en) | 2023-05-09 |
| CN116086453B CN116086453B (en) | 2024-03-12 |
Family
ID=86201634
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211588179.8A Active CN116086453B (en) | 2022-12-12 | 2022-12-12 | Inertial navigation and map combined positioning method based on probability optimization calculation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116086453B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104515528A (en) * | 2014-12-22 | 2015-04-15 | 厦门雅迅网络股份有限公司 | Single-point map matching method based on road section accumulation probability |
| CN106595680A (en) * | 2016-12-15 | 2017-04-26 | 福州大学 | Vehicle GPS data map matching method based on hidden markov model |
| WO2018090773A1 (en) * | 2016-11-15 | 2018-05-24 | 北京京东尚科信息技术有限公司 | Method and device thereof for matching track points captured by positioning system to map |
| US20190360819A1 (en) * | 2018-05-25 | 2019-11-28 | Here Global B.V. | Method and apparatus for path based map matching |
| CN111121791A (en) * | 2019-11-29 | 2020-05-08 | 上饶市中科院云计算中心大数据研究院 | Optimization method of hidden Markov model in map matching and GPS positioning method |
| WO2020243937A1 (en) * | 2019-06-04 | 2020-12-10 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for map-matching |
| CN113934775A (en) * | 2021-12-16 | 2022-01-14 | 禾多科技（北京）有限公司 | Vehicle track map matching method, device, equipment and computer readable medium |
| CN114184200A (en) * | 2022-02-14 | 2022-03-15 | 南京航空航天大学 | Multi-source fusion navigation method combined with dynamic mapping |
-
2022
- 2022-12-12 CN CN202211588179.8A patent/CN116086453B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104515528A (en) * | 2014-12-22 | 2015-04-15 | 厦门雅迅网络股份有限公司 | Single-point map matching method based on road section accumulation probability |
| WO2018090773A1 (en) * | 2016-11-15 | 2018-05-24 | 北京京东尚科信息技术有限公司 | Method and device thereof for matching track points captured by positioning system to map |
| CN106595680A (en) * | 2016-12-15 | 2017-04-26 | 福州大学 | Vehicle GPS data map matching method based on hidden markov model |
| US20190360819A1 (en) * | 2018-05-25 | 2019-11-28 | Here Global B.V. | Method and apparatus for path based map matching |
| WO2020243937A1 (en) * | 2019-06-04 | 2020-12-10 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for map-matching |
| CN111121791A (en) * | 2019-11-29 | 2020-05-08 | 上饶市中科院云计算中心大数据研究院 | Optimization method of hidden Markov model in map matching and GPS positioning method |
| CN113934775A (en) * | 2021-12-16 | 2022-01-14 | 禾多科技（北京）有限公司 | Vehicle track map matching method, device, equipment and computer readable medium |
| CN114184200A (en) * | 2022-02-14 | 2022-03-15 | 南京航空航天大学 | Multi-source fusion navigation method combined with dynamic mapping |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116086453B (en) | 2024-03-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20210025713A1 (en) | Lane line positioning method and apparatus, and storage medium thereof | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN101819043B (en) | Navigation device and navigation method | |
| CN107389064A (en) | A kind of unmanned vehicle based on inertial navigation becomes channel control method | |
| CN110208842A (en) | Vehicle high-precision locating method under a kind of car networking environment | |
| CN109934920A (en) | High-precision three-dimensional point cloud map constructing method based on low-cost equipment | |
| CN108645420B (en) | Method for creating multipath map of automatic driving vehicle based on differential navigation | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN110221328A (en) | A kind of Combinated navigation method and device | |
| CN101201255A (en) | Vehicle combined navigation system based on intelligent navigation algorithm | |
| CN101473195A (en) | Positioning device | |
| CN101750086B (en) | Navigation information correcting method and navigation device thereof | |
| CN104007459B (en) | A kind of vehicle-mounted integrated positioning device | |
| CN111637888B (en) | Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement | |
| CN109813306A (en) | A kind of unmanned vehicle planned trajectory satellite location data confidence level calculation method | |
| CN112327842B (en) | Method and system for positioning charging pile by robot | |
| CN110398251B (en) | Trackless navigation AGV positioning system based on multi-sensor fusion and positioning method thereof | |
| CN107607093A (en) | A kind of monitoring method and device of the lake dynamic storage capacity based on unmanned boat | |
| CN110006421A (en) | A kind of navigation method and system based on MEMS and GPS | |
| CN111221020A (en) | Indoor and outdoor positioning method, device and system | |
| CN110388939A (en) | One kind being based on the matched vehicle-mounted inertial navigation position error modification method of Aerial Images | |
| CN106093992A (en) | A kind of sub-meter grade combined positioning and navigating system based on CORS and air navigation aid | |
| CN115503694A (en) | Autonomous learning-based memory parking path generation method and device and electronic equipment | |
| CN112859107A (en) | Vehicle navigation switching equipment of golf course self-driving vehicle | |
| CN113281797B (en) | Maneuvering detection and correction radar system based on inertial navigation |
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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Country or region after: China Address after: Room 1008, building 3, 311 Yanxin Road, Huishan Economic Development Zone, Wuxi City, Jiangsu Province, 214000 Applicant after: Yunlai Intelligent Equipment (Wuxi) Co.,Ltd. Address before: Room 1008, Building 3, No. 311, Yanxin Road, Huishan Economic Development Zone, Wuxi, Jiangsu 214000 Applicant before: WUXI A-CARRIER ROBOT CO.,LTD. Country or region before: China |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |