CN113267184B - Vehicle inertial navigation track map matching method based on curve - Google Patents
Vehicle inertial navigation track map matching method based on curve Download PDFInfo
- Publication number
- CN113267184B CN113267184B CN202110448330.7A CN202110448330A CN113267184B CN 113267184 B CN113267184 B CN 113267184B CN 202110448330 A CN202110448330 A CN 202110448330A CN 113267184 B CN113267184 B CN 113267184B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- sin
- road
- vector
- curve
- 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
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/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
Abstract
The invention discloses a vehicle inertial navigation track map matching method based on a curve, which belongs to the field of electronic map matching and comprises the following specific steps: firstly, determining a road number where a military vehicle is located from a road network map according to the current position of the military vehicle; then, extracting characteristic vectors of a curve road section and a road from an actual driving road of the vehicle by using a double-window method; secondly, extracting the characteristic vector of the corresponding numbered road section in the road network map, carrying out optimal matching on the characteristic vector of the road section of the actual curve of the vehicle according to the minimum root mean square error, using the matching result to correct the actual position of the vehicle, and projecting the corrected vehicle position onto the road on which the vehicle runs again, thereby improving the positioning precision of the vehicle; the method has small calculated amount, can directly run on embedded equipment with low calculated amount and the like in real time, has stronger matching robustness and high matching precision, and has good practical engineering application value.
Description
Technical Field
The invention belongs to the field of electronic map matching, and particularly relates to a vehicle inertial navigation track map matching method based on a curve.
Background
Military vehicles, such as remote rocket launchers, missile launchers, etc., as vehicles for special purposes must have the capability of realizing long-term autonomous high-precision positioning without depending on GPS positioning information. These vehicles are usually equipped with a high-precision inertial Navigation system ins (inertial Navigation system) and a odometer, and autonomous Navigation can be achieved by a dead reckoning method; although the high-precision INS is used, the position error is more and more serious as time is accumulated, so that the accumulated error needs to be corrected by external position information so as to ensure the accuracy of the vehicle position.
The electronic digital road network map data can provide absolute position information as an available external position information source, and the electronic digital road network map database can be constructed in advance and loaded into the equipment of the vehicle when needed.
Disclosure of Invention
Aiming at the problem that a dead reckoning method is adopted by military vehicles to realize autonomous positioning so as to generate accumulated position errors, the invention provides a curve-based vehicle inertial navigation track map matching method.
The method for matching the inertial navigation track map of the vehicle based on the curve comprises the following specific steps:
the method comprises the following steps: for a military vehicle, determining a road number where the vehicle is located at a specific time from a road network map;
the specific time is a time point or a time period which is artificially selected according to actual needs;
step two: and extracting the curve road section with obvious characteristic information from the actual driving road corresponding to the serial number by using a double-window method.
The specific extraction process is as follows:
firstly, carrying out equidistant interpolation on the actual running track of the vehicle, and respectively linearly fitting straight lines l and k by using points in double windows;
let the interpolation point set in the long window be P ═ P 1 ,p 2 ,...,p i ,...p I H, the set of interpolation points in the short window is Q ═ Q 1 ,q 2 ,...,q j ,...,q J }；
The straight line fitting criterion is that the sum of distances from points in the window to the straight line to be fitted is minimum, and specifically comprises the following steps:
d(p i l) is point p i Distance of a point fitting straight line l to the long window; d (q) j K) is a point q j The distance of a point fitting straight line k to the short window;
then, the current driving state of the vehicle is judged by utilizing the sum D of the distances from each point in the long window to the fitted straight line l;
as follows:
T d switching a threshold for a vehicle state;
finally, judging whether the vehicle is in a straight-going state, if so, detecting a starting point and an end point of the vehicle entering a curve road section by using an included angle between two fitting straight lines; otherwise, when the vehicle is in a turning state, no processing is carried out;
the method specifically comprises the following steps:
first, the calculation formula of the included angle between the two fitting straight lines l and k is as follows:
in the formula k 1 Is the slope of the line l; k is a radical of 2 The slope of the line k.
Then, when the angle theta is larger than a set threshold value theta', the angle theta is the starting point of the detected curve road section, namely the vehicle enters the curve road section; and when theta is less than or equal to the threshold theta', the curve section is the ending point of the curve when the curve is ended.
Step three: and extracting the characteristics of the extracted curve road section to obtain the characteristic vector of the road and processing the characteristic vector.
Firstly, taking the direction of a long window fitting straight line l as a reference direction, carrying out equidistant interpolation on the extracted curve road section, calculating the included angle between the road section vector between every two adjacent interpolation points and the reference direction vector, and forming a characteristic vector set F of the curve road section 0 ；
F 0 ＝[α 1 ,α 2 ,...α n ,...,α N ] T
N is a vehicle track feature vector F 0 Dimension (d);
reference direction vector V b The road section vector formed by the nth interpolation point and the (n + 1) th interpolation point is V n Then angle of included a n Satisfies the following relationship:
if the included angle is positive in the counterclockwise direction and negative in the clockwise direction, the included angle alpha is defined n Has a value range of (-pi, pi)]；
Then, the characteristic angle vector is converted into a corresponding sine value, and the converted characteristic vector is obtained as follows:
F＝[sin(α 1 ),sin(α 2 ),...,sin(α N )] T
and finally, filtering the transformed feature vectors by adopting a median filtering method.
Step four: and optimally matching the actual curve road section driven by the vehicle with the corresponding road section in the road network map according to the minimum root mean square error.
Firstly, according to the actual start and stop point position of the vehicle curve road section, extracting the corresponding candidate road section from the road network map and carrying out feature extraction to obtain the section feature vector F' of the candidate road.
F'＝[sin(α' 1 ),sin(α' 2 ),...,sin(α' n' ),...,sin(α' N' )] T ；
N 'is the dimension of the section feature vector F' of the candidate road; n' is more than or equal to N.
Then, segmenting the feature vector F of the actual curve of the vehicle into M feature sub-vectors with the same length, wherein each feature sub-vector is respectively as follows:
F 1 ＝{sin(α 1 ),sin(α 2 ),...,sin(α N/M )}
F 2 ＝{sin(α [N/M]+1 ),sin(α [N/M]+2 ),...,sin(α 2N/M )}
...
F m ＝{sin(α [(m-1)N/M]+1 ),sin(α [(m-1)N/M]+2 ),...,sin(α mN/M )}
...
F M ＝{sin(α [(M-1)N/M]+1 ),sin(α [(M-1)N/M]+2 ),...,sin(α N )}
finally, adopting a segmentation sliding window matching method to perform sliding matching on each characteristic sub-vector in the characteristic vector F' of the candidate road section;
the specific process is as follows:
firstly, dividing the section feature vectors F' of the candidate road according to the length L, and sequentially obtaining vector sets corresponding to sliding windows:
F 1 '＝{sin(α' 1 ),sin(α' 2 ),...,sin(α' N/M )}
F 2 '＝{sin(α' 2 ),sin(α' 3 ),...,sin(α' [N/M]+1 )}
...
F m '＝{sin(α' m ),sin(α' m+1 ),...,sin(α' [N/M]+(m-1) )}
...
sequentially selecting each characteristic sub-vector and aiming at the current characteristic sub-vector F m Carrying out sliding window matching with all sliding window vector sets one by one, and calculating the root mean square error between the candidate road characteristics of each sliding window and the actual track characteristics of the vehicle;
eigenvector F m The root mean square error with a single set of sliding window vectors is calculated as follows:
in the formula, sin α f As feature subvectors F m The f-th element selected in (1); sin alpha' f Is the f element in a single sliding window vector set;
feature subvector F m Respectively carrying out the calculation with each sliding window vector set to obtain the matching root mean square error corresponding to each sliding window, and selecting the minimum value from all the root mean square errors, namely the characteristic subvector F m The matching result of (1);
then, selecting the next characteristic sub-vector to repeat the matching process, selecting the sliding window vector set with the minimum root mean square error as the matching value corresponding to the current characteristic sub-vector, and obtaining a matching result set { S until all the characteristic sub-vectors are completely matched 1 ,S 2 ,...S m ,...S M }；
Step five: and correcting the actual position of the vehicle by using the root mean square error value obtained by the optimal matching to obtain a position error E.
Firstly, respectively calculating the weight value of each matching window based on the root-mean-square error value of each matching window;
weight value w of mth matching window m The calculation method of (2) is as follows:
then, the error of the actual position of the vehicle corresponding to each matching window is given to each matching window: { e 1 ,e 2 ,...e m ,...e M }；
Finally, the final vehicle position error E is calculated, as follows:
E＝w 1 e 1 +w 2 e 2 +...w m e m +...w M e M
and step six, correcting the current actual position of the vehicle by using the position error E, and projecting the corrected position of the vehicle onto the road on which the vehicle runs again, so that the positioning accuracy of the vehicle is improved.
The invention has the advantages that:
1) the characteristic angle vector of the curve road section is extracted by utilizing an equidistant interpolation method, the characteristic vector only depends on the shape of the road section, the inherent characteristics of the curve can be effectively described, and the method has the characteristic of direction invariance.
2) The vehicle inertial navigation track map matching method based on the curve has the advantages that the calculated amount is small, the method can be directly operated on embedded equipment with low calculation amount and the like in real time, the matching robustness is strong, the matching precision is high, and the method has good practical engineering application value.
Drawings
FIG. 1 is an overall schematic diagram of a curve-based vehicle inertial navigation track map matching method of the present invention;
FIG. 2 is a flow chart of a curve-based vehicle inertial navigation track map matching method of the present invention;
FIG. 3 is a flow chart of a feature trajectory extraction algorithm based on a dual-window method according to the present invention;
FIG. 4 is a schematic diagram of a characteristic road segment starting point determined by a double sliding window method according to the present invention;
FIG. 5 is a schematic diagram of determining a characteristic road segment termination point by a double sliding window method according to the present invention;
FIG. 6 is a schematic diagram of trajectory feature extraction according to the present invention;
FIG. 7 is a vehicle trajectory view in the UTM coordinate system of the present invention;
FIG. 8 is a vehicle travel path feature of the present invention;
FIG. 9 is a map data diagram of a candidate road segment according to the present invention;
FIG. 10 is a diagram of the characteristics of the driving path of the vehicle and the characteristics of the road candidate links according to the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
In the running process of the vehicle, as the straight road section has no obvious characteristic information, the matching effect with the electronic digital road network map is poor, and in order to improve the robustness and the matching precision of map matching, the road section with strong characteristics is selected from the running track of the vehicle for matching. The invention matches the actual running track of the vehicle by utilizing the curve and the like with obvious characteristics on the map data, thereby correcting the accumulated position error and improving the long-time autonomous positioning precision of the vehicle; in particular to a vehicle inertial navigation track map matching method based on a curve.
The principle of the method is shown in fig. 1, and the actual driving track of a military vehicle is determined based on an inertia combination system, a curve road section with obvious characteristic information is extracted from an actual driving road by using a double-window method, and the characteristic extraction is carried out on the extracted curve road section to obtain the characteristic vector of the road.
Meanwhile, correspondingly finding out a number corresponding to the road on which the vehicle actually runs from the electronic map database, extracting key road sections on the numbered road, and further extracting road characteristics;
and optimally matching the feature vector of the actual road with the road features corresponding to the map data according to a feature similarity matching algorithm, estimating the actual position error of the vehicle, correcting the error by combining an inertia combination system, finally outputting the current actual position of the vehicle, and projecting the corrected position of the vehicle onto the road on which the vehicle runs again, thereby improving the positioning accuracy of the vehicle.
As shown in fig. 2, the specific steps are as follows:
the method comprises the following steps: for a certain military vehicle, determining a road number of the vehicle at a specific time from a road network map according to the current position output by vehicle inertial navigation;
the specific time is a time point or a time period which is artificially selected according to actual needs;
step two: and extracting the curve road section with obvious characteristic information from the actual driving road corresponding to the serial number by using a double-window method.
As shown in fig. 3, the specific extraction process is as follows:
firstly, carrying out equidistant interpolation on the actual running track of the vehicle to judge the running state of the vehicle, if the vehicle turns, updating the sliding window, and continuing to return to the judgment, otherwise, when the vehicle runs straight, firstly carrying out straight line fitting on the long window, then carrying out fitting on the short window, judging whether a starting point is detected or not by using a fitting result, and if the starting point is detected, continuing to judge a detection stop point; and finishing the start and stop point detection of a section of curved road, otherwise, continuously judging whether the start point detection condition is met, if not, updating the sliding window, and judging again.
The equidistant interpolation point set in the long window is P ═ P 1 ,p 2 ,...,p i ,...p I H, the set of equidistant interpolation points in the short window is Q ═ Q 1 ,q 2 ,...,q j ,...,q J Linearly fitting straight lines l and k respectively by using points in the long window and the short window; the criterion for fitting a straight line is the distance of the points in the window to the straight line to be fittedAnd minimum, specifically:
d(p i l) is point p i The distance of the point fitting straight line l to the long window; d (q) j K) is a point q j The distance of a point fitting straight line k to the short window;
the long window determines the traveling direction of the vehicle in a relatively long period of time recently, the short window determines the traveling direction of the vehicle in a relatively short period of time currently, and the calculation formula of the included angle between the two fitting straight lines l and k is as follows:
in the formula k 1 Is the slope of the line l; k is a radical of 2 Being the slope of the line k, atan (-) is an arctangent function.
As shown in fig. 4, when the angle θ is greater than the set threshold θ', it is the starting point of the detected curve section, i.e., the vehicle enters the curve section;
as shown in FIG. 5, at the end of the turn, the slope k of the straight line l is fitted 1 Slope k of straight line k fitting short window track point 2 The angle theta therebetween should gradually decrease, being zero when the vehicle is traveling completely along a straight line. Thus, by setting an appropriate threshold, when the angle between two straight lines is less than the threshold, it can be considered that a curve section end point is detected.
The characteristic track starting and stopping point detection method aims to detect a switching point from straight running to turning and a switching point from turning state to straight running state of a vehicle, and can detect two switching points, but when the turning rate of the vehicle is large in the turning process, an included angle between two fitted straight lines easily meets the starting and stopping point detection condition, so that the starting and stopping point is detected mistakenly. In order to reduce the false detection rate of the start point and the stop point, the current driving state of the vehicle is judged by utilizing the sum D of the distances from each point in the long sliding window to the fitted straight line l;
as follows:
in the formula T d Switching a threshold for a vehicle state; d approaches zero when the vehicle is in a fully straight-ahead state, and the value of D increases gradually as the vehicle enters a turning process.
The method can be used for judging the current running state of the vehicle, and the detection of the start point and the stop point of the curve is meaningful only when the vehicle is in a straight running state, so that the current running state of the vehicle is estimated before the detection of the start point and the stop point of the curve, and the included angle between two fitted straight lines is used for detecting the start point and the stop point of the vehicle entering the curve section when the vehicle is in the straight running state.
Step three: and extracting the characteristics of the extracted curve road section to obtain the characteristic vector of the road and processing the characteristic vector.
As shown in fig. 6, first, a direction of a long window fitting straight line l is used as a reference direction, an extracted curve segment is interpolated at equal intervals, and an included angle between a segment vector between adjacent interpolation points and the reference direction vector is calculated to form a feature vector set F of the curve segment 0 ；
F 0 ＝[α 1 ,α 2 ,...α n ,...,α N ] T
N is a vehicle track feature vector F 0 Dimension (d);
reference direction vector V b The road section vector formed by the nth interpolation point and the (n + 1) th interpolation point is V n Then angle of included a n Satisfies the following relationship:
if the included angle is positive in the counterclockwise direction and negative in the clockwise direction, the included angle alpha is defined n Has a value range of (-pi, pi)]；
Then, since the characteristic angle generates numerical jump near ± pi, which affects the subsequent characteristic matching algorithm, the characteristic angle vector is transformed into a corresponding sine value to solve the problem of numerical jump, and the transformed characteristic vector is as follows:
F＝[sin(α 1 ),sin(α 2 ),...,sin(α N )] T
and finally, in order to further reduce the influence of the abnormal value on the feature vector and improve the robustness of a subsequent matching algorithm, filtering the transformed feature vector by adopting a median filtering method.
To further explain the method, as shown in fig. 7, a section of the vehicle driving track is extracted from the actual sports car data, as shown in fig. 8, the track feature information extracted by the method is shown, and it can be seen from the figure that the method can effectively describe the shape of the track.
Step four: and optimally matching the actual curve road section driven by the vehicle with the corresponding road section in the road network map according to the minimum root mean square error.
Firstly, extracting corresponding candidate road sections from a road network map according to the actual start and stop point positions of the road sections of the vehicle curve and extracting features;
the candidate road segments cut out from the road network map should contain the detected vehicle curve track in consideration of the error of vehicle positioning.
The link feature vector F' of the candidate road.
F'＝[sin(α' 1 ),sin(α' 2 ),...,sin(α' n' ),...,sin(α' N' )] T ；
N 'is the dimension of the section feature vector F' of the candidate road; n' is more than or equal to N.
To further explain the above method, as shown in fig. 9, the broken line is a candidate road link extracted from the electronic map, and the solid line is a vehicle travel track. As shown in fig. 10, a characteristic curve is obtained for the characteristic extraction of the trajectory of the curve traveled by the vehicle and the candidate road segment.
Then, segmenting the feature vector F of the actual curve of the vehicle into M feature sub-vectors with equal length;
the feature subvectors are respectively:
F 1 ＝{sin(α 1 ),sin(α 2 ),...,sin(α N/M )}
F 2 ＝{sin(α [N/M]+1 ),sin(α [N/M]+2 ),...,sin(α 2N/M )}
...
F m ＝{sin(α [(m-1)N/M]+1 ),sin(α [(m-1)N/M]+2 ),...,sin(α mN/M )}
...
F M ＝{sin(α [(M-1)N/M]+1 ),sin(α [(M-1)N/M]+2 ),...,sin(α N )}
in order to improve the robustness and the matching precision of feature matching, the feature vector F of the vehicle track is segmented into three feature sub-vectors; f 1 ＝F(1～[N/3])F 2 ＝F([N/3]+1～[2N/3])F 3 ＝F([2N/3]+1 to N); operator [ ·]The rounding operator.
Finally, adopting a segmented sliding window matching method to perform sliding matching on each feature sub-vector in the candidate road section feature vector F';
the matching criterion is the root mean square error between the road characteristics of the actual track of the vehicle in the sliding window and the candidate road characteristics, and the specific process is as follows:
firstly, dividing the section feature vectors F' of the candidate road according to the length L, and sequentially obtaining vector sets corresponding to sliding windows:
F 1 '＝{sin(α' 1 ),sin(α' 2 ),...,sin(α' N/M )}
F 2 '＝{sin(α' 2 ),sin(α' 3 ),...,sin(α' [N/M]+1 )}
...
F m '＝{sin(α' m ),sin(α' m+1 ),...,sin(α' [N/M]+(m-1) )}
...
sequentially selecting each characteristic subvector aiming at the current characteristic subvectorVector F m Carrying out sliding window matching with all sliding window vector sets one by one, and calculating the root mean square error between the candidate road characteristics of each sliding window and the actual track characteristics of the vehicle;
eigenvector F m The root mean square error from a single set of sliding window vectors is calculated as follows:
in the formula, sin alpha f As feature subvectors F m The f-th element of each selected feature vector; for example, feature subvector F m When f is 1, the corresponding sin α f Taking the value as sin (alpha) [(m-1)N/M]+1 )；sinα' f Is the f element in a single sliding window vector set;
feature subvector F m Respectively carrying out the calculation with each sliding window vector set to obtain the matching root mean square error corresponding to each sliding window, selecting the minimum value from all the root mean square errors, and then obtaining the root mean square error value S m I.e. the eigenvector F m The matching result of (1);
then, selecting the next characteristic sub-vector to repeat the matching process, selecting the sliding window vector set with the minimum root mean square error as the matching value corresponding to the current characteristic sub-vector, and obtaining a matching result set { S until all the characteristic sub-vectors are completely matched 1 ,S 2 ,...S m ,...S M }；
Step five: and correcting the actual position of the vehicle by using the root mean square error value obtained by the optimal matching to obtain a position error E.
Firstly, respectively calculating the weight value of each matching window based on the root-mean-square error value of each matching window;
weight value w of mth matching window m The calculation method of (2) is as follows:
then, the error of the actual position of the vehicle corresponding to each matching window is given to each matching window: { e 1 ,e 2 ,...e m ,...e M }；
Finally, the final vehicle position error E is calculated, as follows:
E＝w 1 e 1 +w 2 e 2 +...w m e m +...w M e M
and step six, correcting the current actual position of the vehicle by using the position error E, and projecting the corrected position of the vehicle onto the road on which the vehicle runs again, so that the positioning accuracy of the vehicle is improved.
The correction of the current position of the vehicle using the obtained position error E can reduce only the influence of the translational position error, but cannot correct the deviation of the vehicle position due to the attitude error, and therefore, in order to eliminate this error, the vehicle position after the correction is again projected onto the road on which the vehicle is traveling, thereby further improving the vehicle positioning accuracy.
Claims (2)
1. A vehicle inertial navigation track map matching method based on a curve is characterized by comprising the following specific steps:
firstly, determining a road number where a military vehicle is located from a road network map according to the current position of the military vehicle;
then, extracting characteristic vectors of a curve road section and a road from an actual driving road of the vehicle by using a double-window method;
the specific process is as follows:
firstly, carrying out equidistant interpolation on the actual running track of the vehicle, wherein the set of interpolation points in the long window in the double windows is as follows: p ═ P 1 ,p 2 ,...,p i ,...p I The interpolation point set of the short window is Q ═ Q 1 ,q 2 ,...,q j ,...,q J }；
Then linearly fitting a straight line l by using the interpolation points in the long window, and linearly fitting a straight line k by using the interpolation points in the short window;
then, calculating the sum D of the distances from each point in the long window to the fitted straight line l, judging whether the vehicle is in a straight-going state, and if so, detecting the starting point and the ending point of the vehicle entering the curve road section by using the included angle between the two fitted straight lines; otherwise, when the vehicle is in a turning state, the vehicle is not processed;
the calculation formula of the distance sum D is as follows:
d(p i l) is point p i Distance to the fitted straight line l; t is d Switching a threshold for a vehicle state;
the fitting straight line criterion is that the sum of distances from points in the window to the fitted straight line is minimum, and specifically comprises the following steps:
d(q j k) is a point q j The distance to the point fitting straight line k of the short window;
the calculation formula of the included angle between the two fitting straight lines l and k is as follows:
in the formula k 1 Is the slope of the line l; k is a radical of 2 Is the slope of line k;
when the angle theta is larger than a set threshold value theta', the angle theta is the starting point of the detected curve road section, namely the vehicle enters the curve road section; when the angle theta is smaller than or equal to a threshold value theta', the angle theta is a curve section end point when the curve is finished;
secondly, extracting the characteristic vector of the corresponding numbered road section in the road network map, carrying out optimal matching on the characteristic vector of the road section of the actual curve of the vehicle according to the minimum root mean square error, using the matching result to correct the actual position of the vehicle, and projecting the corrected vehicle position onto the road on which the vehicle runs again, thereby improving the positioning precision of the vehicle;
the specific process of extracting the road feature vector is as follows:
firstly, carrying out equidistant interpolation on the extracted curve road section, wherein the road section vector formed by the nth interpolation point and the (n + 1) th interpolation point is V n The direction of the long window fitting straight line l is taken as a reference direction, and a reference direction vector V b ；
Calculating the included angle between the road section vector between each adjacent interpolation point and the reference direction vector to form the characteristic angle vector F of the curve road section 0 ；
F 0 ＝[α 1 ,α 2 ,...α n ,...,α N ] T
N is a vehicle track characteristic angle vector F 0 Dimension (d); included angle alpha n Comprises the following steps:
then, the characteristic angle vector is converted into a corresponding sine value, and the converted characteristic vector is obtained as follows:
F＝[sin(α 1 ),sin(α 2 ),...,sin(α N )] T
finally, filtering the transformed feature vectors by adopting a median filtering method;
the specific process of the optimal matching is as follows:
firstly, according to the actual start and stop point position of the road section of the vehicle curve, selecting a corresponding candidate road section in a road network map and extracting the characteristics to obtain a section characteristic vector F' of the candidate road:
F'＝[sin(α' 1 ),sin(α' 2 ),...,sin(α' n' ),...,sin(α' N' )] T ；
n 'is the dimension of the section feature vector F' of the candidate road; n' is more than or equal to N;
dividing the road section feature vector F' of the candidate road according to the length L to obtain vectors corresponding to the sliding windows:
F 1 '＝{sin(α' 1 ),sin(α' 2 ),...,sin(α' N/M )}
F 2 '＝{sin(α' 2 ),sin(α' 3 ),...,sin(α' [N/M]+1 )}
...
F m '＝{sin(α' m ),sin(α' m+1 ),...,sin(α' [N/M]+(m-1) )}
...
then, segmenting the feature vector F of the actual curve of the vehicle into M feature sub-vectors with the same length, wherein each feature sub-vector is respectively as follows:
F 1 ＝{sin(α 1 ),sin(α 2 ),...,sin(α N/M )}
F 2 ＝{sin(α [N/M]+1 ),sin(α [N/M]+2 ),...,sin(α 2N/M )}
...
F m ＝{sin(α [(m-1)N/M]+1 ),sin(α [(m-1)N/M]+2 ),...,sin(α mN/M )}
...
F M ＝{sin(α [(M-1)N/M]+1 ),sin(α [(M-1)N/M]+2 ),...,sin(α N )}
finally, sequentially selecting each characteristic sub-vector, performing one-by-one sliding matching on all sliding window vectors of each characteristic sub-vector in the characteristic vector F' of the candidate road section by adopting a segmented sliding window matching method, and calculating the root mean square error corresponding to each sliding window matching;
for feature subvector F m The root mean square error with a single set of sliding window vectors is calculated as follows:
sinα f as feature subvectors F m The f-th element selected in (1); sin alpha' f Is the f element in the single sliding window vector set;
selecting the minimum value from the root mean square error matched and corresponding to each sliding window, namely the minimum value is the specific valueSyndrome vector F m The matching result of (1).
2. A curve-based vehicle inertial navigation track map matching method as claimed in claim 1, wherein the actual position of the vehicle is corrected by the root mean square error value as follows:
firstly, respectively calculating the weight value of each matching window based on the root-mean-square error value matched with each sliding window;
weight value w of mth matching window m The calculation method of (2) is as follows:
S m as feature subvectors F m The minimum root mean square error of the match;
then, the error of the actual position of the vehicle corresponding to each matching window is given to each matching window: { e 1 ,e 2 ,...e m ,...e M }；
Finally, the final vehicle position error E is calculated, as follows:
E＝w 1 e 1 +w 2 e 2 +...w m e m +...w M e M 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110448330.7A CN113267184B (en) | 2021-04-25 | 2021-04-25 | Vehicle inertial navigation track map matching method based on curve |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110448330.7A CN113267184B (en) | 2021-04-25 | 2021-04-25 | Vehicle inertial navigation track map matching method based on curve |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113267184A CN113267184A (en) | 2021-08-17 |
| CN113267184B true CN113267184B (en) | 2022-09-16 |
Family
ID=77229356
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110448330.7A Active CN113267184B (en) | 2021-04-25 | 2021-04-25 | Vehicle inertial navigation track map matching method based on curve |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113267184B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115265557B (en) * | 2021-10-14 | 2024-04-23 | 北京航空航天大学 | Map matching positioning and deviation line judging method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5469360A (en) * | 1991-03-10 | 1995-11-21 | Matsushita Electric Industrial Co., Ltd. | Vehicle position detecting apparatus |
| US6502033B1 (en) * | 2000-10-05 | 2002-12-31 | Navigation Technologies Corp. | Turn detection algorithm for vehicle positioning |
| JP2008134187A (en) * | 2006-11-29 | 2008-06-12 | Alpine Electronics Inc | Onboard navigation apparatus |
| CN105841708A (en) * | 2016-03-16 | 2016-08-10 | 佛山科学技术学院 | Vehicle navigation and positioning track matching method based on path tracing |
| CN107860399A (en) * | 2017-09-22 | 2018-03-30 | 北京机械设备研究所 | Accurate alignment method between a kind of vehicle-mounted inertial navigation based on map match is advanced |
| CN111721289A (en) * | 2020-06-28 | 2020-09-29 | 北京百度网讯科技有限公司 | Vehicle positioning method, device, equipment, storage medium and vehicle |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9234760B2 (en) * | 2010-01-29 | 2016-01-12 | Blackberry Limited | Portable mobile transceiver for GPS navigation and vehicle data input for dead reckoning mode |
-
2021
- 2021-04-25 CN CN202110448330.7A patent/CN113267184B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5469360A (en) * | 1991-03-10 | 1995-11-21 | Matsushita Electric Industrial Co., Ltd. | Vehicle position detecting apparatus |
| US6502033B1 (en) * | 2000-10-05 | 2002-12-31 | Navigation Technologies Corp. | Turn detection algorithm for vehicle positioning |
| JP2008134187A (en) * | 2006-11-29 | 2008-06-12 | Alpine Electronics Inc | Onboard navigation apparatus |
| CN105841708A (en) * | 2016-03-16 | 2016-08-10 | 佛山科学技术学院 | Vehicle navigation and positioning track matching method based on path tracing |
| CN107860399A (en) * | 2017-09-22 | 2018-03-30 | 北京机械设备研究所 | Accurate alignment method between a kind of vehicle-mounted inertial navigation based on map match is advanced |
| CN111721289A (en) * | 2020-06-28 | 2020-09-29 | 北京百度网讯科技有限公司 | Vehicle positioning method, device, equipment, storage medium and vehicle |
Non-Patent Citations (1)
| Title |
|---|
| 基于速度约束与模糊自适应滤波的车载组合导航;胡杰等;《兵工学报》;20200215(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113267184A (en) | 2021-08-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101214143B1 (en) | Method and apparatus for detecting position and orientation | |
| WO2018072350A1 (en) | Vehicle trajectory prediction method and device | |
| US11526173B2 (en) | Traveling trajectory correction method, traveling control method, and traveling trajectory correction device | |
| CN114184200B (en) | Multi-source fusion navigation method combined with dynamic mapping | |
| KR101115012B1 (en) | Apparatus and Method for Compenating Angular Velocity Error for Robot | |
| WO2022052567A1 (en) | Vehicle positioning method and apparatus, vehicle, and storage medium | |
| CN113920198B (en) | Coarse-to-fine multi-sensor fusion positioning method based on semantic edge alignment | |
| CN113267184B (en) | Vehicle inertial navigation track map matching method based on curve | |
| Cho et al. | Map based indoor robot navigation and localization using laser range finder | |
| CN106032985A (en) | Vehicle positioning device | |
| CN111060112A (en) | Vehicle track map matching method and system based on direction angle | |
| CN115265557A (en) | Map matching positioning and deviation line judgment method | |
| CN110395297B (en) | Train positioning method | |
| CN112229422A (en) | Speedometer quick calibration method and system based on FPGA time synchronization | |
| JP2008165275A (en) | Mobile body with self-position identification device | |
| CN116337045A (en) | High-speed map building navigation method based on karto and teb | |
| CN115683124A (en) | Method for determining a driving trajectory | |
| CN108151739B (en) | Gravity matching positioning error suppression method based on vector matching algorithm | |
| TWI448665B (en) | Object positioning method, position calculation system, map system and positioning system | |
| CN108268960A (en) | Driving locus optimization system | |
| Smaili et al. | Multi-sensor fusion method using dynamic Bayesian network for precise vehicle localization and road matching | |
| CN112254728A (en) | Method for enhancing EKF-SLAM global optimization based on key road sign | |
| KR20090070936A (en) | System and method for navigation, storage medium recording that metho program | |
| JP2012137361A (en) | Locus information correcting device, method and program | |
| CN111857121A (en) | Patrol robot walking obstacle avoidance method and system based on inertial navigation and laser radar |
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 |