CN116879917A - Laser radar terrain matching auxiliary navigation method and system - Google Patents
Laser radar terrain matching auxiliary navigation method and system Download PDFInfo
- Publication number
- CN116879917A CN116879917A CN202310938254.7A CN202310938254A CN116879917A CN 116879917 A CN116879917 A CN 116879917A CN 202310938254 A CN202310938254 A CN 202310938254A CN 116879917 A CN116879917 A CN 116879917A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- digital elevation
- map
- matching
- elevation map
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000001914 filtration Methods 0.000 claims abstract description 41
- 238000006243 chemical reaction Methods 0.000 claims abstract description 27
- 238000012545 processing Methods 0.000 claims abstract description 15
- 239000004744 fabric Substances 0.000 claims abstract description 13
- 238000004088 simulation Methods 0.000 claims abstract description 11
- 238000012937 correction Methods 0.000 claims abstract description 9
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 7
- 238000004364 calculation method Methods 0.000 claims description 28
- 238000011156 evaluation Methods 0.000 claims description 24
- 230000008569 process Effects 0.000 claims description 20
- 230000009466 transformation Effects 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000005259 measurement Methods 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 6
- 238000012935 Averaging Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 abstract description 3
- 230000008030 elimination Effects 0.000 abstract description 2
- 238000003379 elimination reaction Methods 0.000 abstract description 2
- 238000003384 imaging method Methods 0.000 abstract 1
- 230000008859 change Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000012876 topography Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000008034 disappearance Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000007306 turnover Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- 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/20—Instruments for performing navigational calculations
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/3811—Point data, e.g. Point of Interest [POI]
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/3826—Terrain data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention provides a laser radar terrain matching auxiliary navigation method and a system, wherein the method comprises the steps of utilizing pose information provided by an inertial navigation system to realize laser three-dimensional terrain point cloud data coordinate conversion and navigation belt splicing processing, and further carrying out rough point elimination and cloth simulation filtering on point cloud to construct a point cloud digital elevation map; performing similarity comparison on the point cloud digital elevation map and the prior digital elevation map to obtain position correction information of the carrier; and integrating inertial navigation and laser point cloud terrain matching results through optimal filtering, and correcting errors of an inertial navigation system. According to the invention, the terrain elevation detection level and the map matching precision of the terrain matching auxiliary navigation system are effectively improved through the point cloud projection imaging model, the point cloud data filtering processing algorithm and the point cloud digital elevation similarity matching, and the usability of the system under complex terrain conditions is expanded.
Description
Technical Field
The invention relates to the technical field of navigation and positioning, in particular to a laser radar terrain matching auxiliary navigation method and system.
Background
When the airborne navigation system operates in a complex working environment, various interference and disturbance problems are often accompanied, such as interference or disappearance of signals of a GNSS in a city or forest area, environmental magnetic interference, electric magnetic interference and the like, which cause great hidden trouble in the navigation system navigation and flight safety guarantee process.
The Terrain aided navigation (Terrain AidedNavigation, TAN) is one of widely used combined navigation systems, has the advantages of strong anti-interference capability, high universality, convenient operation and implementation and the like, can effectively solve the problem of inhibiting error divergence of an inertial navigation system in a GNSS rejection environment, and realizes effective autonomous navigation. The traditional terrain matching auxiliary navigation method adopts a barometric altimeter and a radio altimeter as measurement sensors for measuring the data of the terrain elevation profile along the route, and the geographic position of the aircraft is determined according to the optimal matching position. However, the scheme of the measuring sensor has limited data acquisition capacity, and the data acquisition process is easy to be interfered to cause the occurrence of a mismatching result.
As an active measuring system, the laser radar can measure the topographic structure below the carrier by emitting laser beams, has a larger measuring range and a more accurate measuring result compared with the traditional topographic matching auxiliary navigation measuring sensor scheme, and has more stable running condition. Meanwhile, by combining the efficient and reliable point cloud data processing technology, a terrain elevation map with more accuracy and richer information can be established.
Therefore, the laser point cloud data processing technology is applied to the terrain matching auxiliary navigation, and the point cloud terrain elevation map with higher precision and reliability is constructed to be matched with the priori terrain elevation map, so that the precision and reliability of the terrain matching auxiliary navigation positioning method can be further improved, and the application requirements of the terrain matching auxiliary navigation positioning method under the condition of more complex terrains are met.
Disclosure of Invention
In view of the above, the invention provides a laser radar terrain matching auxiliary navigation method and system, which effectively improves the terrain elevation detection level and the map matching precision of a terrain matching auxiliary navigation system and also enhances the usability of the terrain matching auxiliary navigation system under complex terrain conditions through the point cloud data acquisition, coordinate conversion, navigation belt splicing, rough difference point rejection, a point cloud data filtering processing algorithm and the similarity matching of a point cloud elevation map.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a laser radar terrain matching auxiliary navigation method comprises the following steps:
s1: acquiring three-dimensional terrain point cloud data below a carrier through a laser radar, carrying out coordinate conversion and navigation belt splicing on the three-dimensional terrain point cloud data through carrier pose recurrence information provided by inertial navigation, and accumulating point cloud data in a certain navigation belt area;
S2: selecting point cloud data in a certain range of the navigation belt area to carry out downsampling; managing the down-sampled point cloud data through a KD tree structure, traversing and searching for adjacent points within a certain radius range of each point, and eliminating point cloud rough difference points by calculating an elevation distribution average value and an elevation distribution standard deviation of the adjacent points;
s3: filtering the ground object point cloud in the point cloud data after the rough difference points are removed, and establishing a grid and a number according to the resolution of the prior digital elevation map to obtain a point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data;
s4: establishing a map searching area, calling prior digital elevation map data in the map searching area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, and obtaining position information of a carrier in the prior digital elevation map, otherwise, giving up current matching and continuing recursion inertial navigation data;
S5: and (3) taking the position information of the carrier in the prior digital elevation map as observation information, constructing a combined navigation system formed by an inertial navigation system and a laser radar terrain matching navigation system, estimating an inertial navigation system error, and correcting the inertial navigation system error.
Preferably, S1 comprises:
s11, acquiring three-dimensional terrain point cloud data below a carrier through a laser radar;
s12, the terrain matching auxiliary navigation platform acquires the motion acceleration and the angular velocity of the carrier by using an inertial measurement element, and obtains carrier pose recursive information by inertial navigation recursive calculation and optimal filtering prediction processes, wherein the carrier pose recursive information specifically comprises the position, the pose and the navigation error information of the carrier;
s13, according to pose recurrence information, three-dimensional point cloud data p in a local coordinate system l of the laser radar at the corresponding moment l Performing coordinate conversion and navigation band splicing to obtain point cloud data p in a certain navigation band region under a global coordinate system g g Wherein the ith point cloud data p i The coordinate conversion of (2) is:
wherein b is a carrier coordinate system;is laser point cloud data p i Position information in the global coordinate system g; in the same way, the processing method comprises the steps of,is p i Position information in a local coordinate system of the laser radar; / >The coordinate transformation matrix is a coordinate transformation matrix from a carrier coordinate system to a global coordinate system, and is obtained by integrating inertial motion data by an inertial navigation system, wherein the processing process of obtaining middle moment pose transformation information by interpolation by utilizing adjacent moment pose data is involved; />Seating for a lidar local coordinate system to a carrier coordinate systemThe standard transformation matrix is usually a constant transformation matrix because the lidar is mounted on the carrier in a fixed connection manner, and therefore the transformation matrix does not change with time.
Preferably, S2 comprises:
s21, selecting point cloud data in a certain rectangular range of a navigation belt area, and downsampling the point cloud data in the area by a voxel grid method, wherein the voxel grid is the same as the prior digital elevation map in resolution;
s22, managing the down-sampled point cloud data based on a KD tree structure, circularly traversing each point in the current point cloud data, searching adjacent point data in a range with the radius of r by taking each traversed point as a center, calculating an elevation distribution average value mu and an elevation distribution standard deviation sigma of the adjacent points, and judging whether the elevation h of the current point meets a rough difference point judging condition, wherein the rough difference point judging condition is as follows:
|h-μ|＞3σ
When the conditions are met, the current point is judged to be the rough difference point, the current point is removed from the point cloud data, the KD tree is updated, and if not, the current point is not the rough difference point, and the current point is reserved.
Preferably, S3 comprises:
s31, filtering ground object point clouds in the point cloud data after the rough difference point is removed through a cloth simulation filtering algorithm, fitting the vacant grids after the ground object point clouds are filtered through a cloth simulation method, and building and numbering grids according to the prior digital elevation map resolution ratio of the point cloud data to obtain a gridded point cloud digital elevation map;
s32, calculating the feature parameters of the topographic distribution of the point cloud digital elevation map, including an elevation mean value M h Standard deviation sigma of elevation h And terrain roughness sigma z The calculation formula is as follows:
wherein, the size of the point cloud digital elevation map is m multiplied by n; h (j, k)) The method comprises the steps of taking a point cloud elevation value at a kth column of a jth row in a point cloud digital elevation map; q (Q) x And Q is equal to y Roughness of elevation distribution between adjacent points in x direction and y direction respectively, and a calculation formula is as follows:
wherein h (j, k+1) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map, and h (j+1, k) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map;
S33, evaluating the matching availability of the point cloud digital elevation map, and if the terrain distribution characteristic parameters in S32 meet the threshold condition, judging that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, giving up the current point cloud digital elevation map data, and continuing recursion of the inertial navigation data; the point cloud digital elevation map matching availability evaluation discriminant is:
wherein D is std And D rough Respectively determining a threshold value of the standard deviation of the elevation and a threshold value of the roughness of the terrain; rule is a map matching availability evaluation result; true represents that the established point cloud digital elevation map can be used for subsequent map matching, false represents that the established point cloud digital elevation map cannot be used for subsequent map matching.
Preferably, S4 comprises:
s41, extracting map data in a three-time error range from the prior digital elevation map according to the position and position error output by inertial navigation at the current moment for matching the elevation map;
s42, sliding window traversal search is carried out on the extracted prior digital elevation map data, the window size is consistent with the size of the point cloud digital elevation map, the normalized cross-correlation coefficient between the point cloud digital elevation map and the prior digital elevation map in the current sliding window is calculated according to a normalized cross-correlation judgment criterion, and a normalized cross-correlation coefficient calculation model is as follows:
Wherein N is the number of the pixel grids of the point cloud digital image; i DEM And I LiDAR The elevation values of corresponding points in the prior digital elevation map and the point cloud digital elevation map are respectively; (u, v) is the position deviation value of the point cloud digital elevation map area relative to the prior digital elevation map area; mu (mu) DEM And mu LiDAR Respectively averaging the elevation data in the prior digital elevation map and the point cloud digital elevation map; sigma (sigma) DEM And sigma (sigma) LiDAR The standard deviation of elevation data in the two prior digital elevation maps and the point cloud digital elevation map are respectively, and NCC (u, v) represents normalized cross correlation coefficients;
s43, judging whether the normalized cross-correlation coefficient is larger than a normalized cross-correlation coefficient threshold, and obtaining an elevation map matching position result when the normalized cross-correlation coefficient is larger than the normalized cross-correlation coefficient threshold.
Preferably, S5 comprises:
s51, modeling a terrain matching auxiliary navigation positioning error and an inertial measurement element deviation, and constructing an error state optimal filtering state prediction model;
s52, if the built point cloud digital elevation map has matching availability and the map matching result meets the normalized cross-correlation coefficient judgment criterion, an error state optimal filtering observation model is built based on the matching position result, and all state quantities of optimal filtering are updated and corrected, otherwise, only S51 is executed.
The invention also discloses a laser radar terrain matching auxiliary navigation system which is suitable for a laser radar terrain matching auxiliary navigation method, comprising the following steps: the system comprises a laser radar module, an inertial navigation module, a point cloud coordinate conversion and navigation belt splicing module, a point cloud map data processing module, a point cloud digital elevation map construction and matching availability evaluation module, a terrain matching calculation module and a navigation error correction module;
the laser radar module is used for acquiring three-dimensional terrain point cloud data below the carrier;
the inertial navigation module is used for acquiring carrier pose recurrence information in the carrier motion process provided by inertial navigation;
the point cloud coordinate conversion and navigation belt splicing module is used for carrying out coordinate conversion and navigation belt splicing on three-dimensional terrain point cloud data based on carrier pose recurrence information, and accumulating the point cloud data in a certain navigation belt area;
the point cloud map data processing module is used for downsampling the point cloud data in a certain range of the navigation belt area; managing the down-sampled point cloud data through a KD tree structure, traversing and searching for adjacent points in a certain radius range by taking each point as a center, and eliminating point cloud rough difference points by calculating an elevation distribution average value and an elevation distribution standard deviation of the adjacent points;
The point cloud digital elevation map construction and matching availability evaluation module is used for filtering out the ground object point clouds in the point cloud data after the rough difference points are removed, and building grids and numbers according to the prior digital elevation map resolution to obtain the point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data;
the terrain matching calculation module is used for establishing a map searching area, calling prior digital elevation map data in the map searching area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, and obtaining position information of a carrier in the prior digital elevation map, otherwise, discarding the current matching result and continuing recursion inertial navigation data;
the navigation error correction module is used for constructing a combined navigation system formed by the inertial navigation system and the laser radar terrain matching navigation system by taking the position information in the carrier prior digital elevation map as observation information, estimating the error of the inertial navigation system and correcting the error of the inertial navigation system.
According to the technical scheme, compared with the traditional terrain matching auxiliary navigation method which uses a barometer and a radio altimeter as measurement sensors to acquire the data of the terrain height Cheng Poumian of the flight route and performs elevation matching, the method provided by the invention uses the laser radar as the measurement sensor, builds a point cloud digital elevation map aiming at the point cloud data characteristics, performs matching with an airborne priori digital elevation map database, and finally realizes correction of inertial navigation positioning errors according to the calculation result of elevation matching. According to the invention, a point cloud digital elevation map with a larger range is constructed through the laser radar, elevation 'line' data of a traditional measurement scheme is expanded into elevation 'surface' data, the situation that terrain matching auxiliary navigation is prone to noise interference to cause mismatching is effectively reduced, and the terrain elevation detection level, the accuracy of an elevation matching result and the accuracy of terrain matching auxiliary navigation positioning are improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and that other drawings can be obtained according to the provided drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a laser radar terrain matching assisted navigation method provided by the invention;
FIG. 2 is a schematic diagram of a point cloud coarse difference point rejection process provided by the invention;
FIG. 3 is a schematic diagram of a laser radar terrain matching assisted navigation system according to the present invention;
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
As shown in fig. 1, the embodiment of the invention discloses a laser radar terrain matching auxiliary navigation method, which comprises the following steps:
s1: acquiring three-dimensional terrain point cloud data below a carrier by using a laser radar, performing coordinate conversion and navigation band splicing on the three-dimensional terrain point cloud data by using carrier pose recurrence information provided by inertial navigation, and accumulating point cloud data in a certain time range, namely a certain navigation band region;
S2: selecting point cloud data in a certain range in a navigation belt area to downsample the point cloud data; managing the down-sampled point cloud data by utilizing a KD tree structure, traversing and searching adjacent points within a certain radius range of each point, and calculating the elevation distribution average value and the elevation distribution standard deviation of the adjacent points to realize the elimination of point cloud rough difference points;
s3: filtering the ground object point cloud in the point cloud data after the rough difference points are removed, and establishing a grid and a number according to the resolution of the prior digital elevation map to obtain a point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data;
s4: establishing a map searching area, calling prior digital elevation map data in the map searching area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, obtaining position information of a carrier in the prior digital elevation map, otherwise, giving up the current matching result and continuing recursion inertial navigation data;
S5: and (3) taking the position information of the carrier in the prior digital elevation map as observation information, constructing a combined navigation system formed by an inertial navigation system and a laser radar terrain matching navigation system, estimating an inertial navigation system error, and correcting the inertial navigation system error. The inertial navigation system is taken as a main navigation system, and inertial navigation is taken as a state variable to be estimated by optimal filtering; the laser radar terrain matching navigation system is used as an auxiliary navigation system, and the result obtained by terrain matching is used as observation information for correcting errors of inertial navigation.
In this embodiment, S1 specifically includes:
s11, acquiring three-dimensional terrain point cloud data below a carrier through a laser radar;
s12, the terrain matching auxiliary navigation platform acquires the motion acceleration and the angular velocity of the carrier by using an inertial measurement element, and obtains carrier pose recurrence information by inertial navigation recurrence calculation and Kalman filtering prediction process, wherein the carrier pose recurrence information specifically comprises the position, the pose and the navigation error information of the carrier;
step S13, according to the carrier pose recursion information, point cloud data p in a local coordinate system l of the laser radar at the corresponding moment l Performing coordinate conversion and navigation band splicing to obtain point cloud data p in a certain navigation band region under a global coordinate system g g Wherein the ith point cloud data p i The coordinate conversion of (2) is:
wherein b is a carrier coordinate system;is laser point cloud data p i Position information in the global coordinate system g; in the same way, the processing method comprises the steps of,is p i Position information in a local coordinate system of the laser radar; />A coordinate transformation matrix for transforming the carrier coordinate system into a global coordinate system, which is moved by the inertial navigation system to inertial motionThe data is obtained through integration, wherein the processing process of obtaining middle moment pose transformation information through interpolation by utilizing adjacent moment pose data is involved; />In order to transform the local coordinate system of the laser radar into the coordinate system of the carrier, the transformation matrix is usually a constant transformation matrix because the laser radar is fixedly mounted on the carrier and does not change with time.
In this embodiment, S2 specifically includes:
s21, selecting point cloud data in a certain rectangular range of a navigation belt area, and performing data downsampling on the point cloud data in the selected rectangular area by using a voxel grid method in order to ensure the calculation efficiency of a navigation system, wherein the size of the voxel grid is required to be the same as the resolution of an priori digital elevation map;
s22, as shown in FIG. 2, performing establishment management on point cloud data after voxel grid downsampling based on a KD tree structure, circularly traversing each point in the current point cloud, searching for adjacent point data in a range with the adjacent point data as a center and the radius size r, calculating an average value mu of elevation distribution of the adjacent points and an standard deviation sigma of the elevation distribution, if the elevation of the current point meets a rough difference point judgment condition, removing the current point from the point cloud data, updating the KD tree, and continuing traversing, wherein the rough difference point judgment condition is as follows:
|h-μ|＞3σ
Where h is the elevation value of the current point. When the condition is met, the current point is judged to be the rough difference point, the rough difference point is eliminated, the KD tree is updated, and otherwise, the current point is not the rough difference point and is reserved.
In this embodiment, S3 specifically includes:
s31, filtering the point cloud data with the rough difference points removed by adopting a cloth simulation filtering algorithm, and fitting the vacant grid filtered by the point cloud by adopting a cloth simulation method. The specific process of the cloth simulation filtering algorithm is described in the literature, "Zhang W, qi J, wanp, et al, an easy-to-use airborne LiDAR data filtering method based on cloth simulation [ J ]. Remote sensing,2016,8 (6): 501", the idea is to turn over the point cloud, and if a piece of cloth falls from above under the action of "gravity", the finally fallen cloth is the result of turning over the earth surface part in the representative point cloud, so as to separate the earth surface point cloud from the earth feature point cloud data. Establishing a grid according to the prior digital elevation map resolution ratio and numbering the point cloud data in the S21 to obtain a gridded point cloud digital elevation map;
s32, calculating the feature parameters of the topographic distribution of the digital elevation map of the point cloud: elevation mean value M h Standard deviation sigma of elevation h Terrain roughness sigma z Wherein sigma h Reflecting the discrete degree of the point cloud elevation value in the elevation map and the total fluctuation degree of the topography in the whole area; and sigma (sigma) z The method can be used for representing the average smoothness of the terrain elevation map and describing finer local relief conditions. The calculation formula of the characteristic parameters of each terrain is as follows:
wherein, the size of the point cloud digital elevation map is m multiplied by n; h (j, k) is the point cloud elevation value at the jth row and kth column in the gridding map; q (Q) x And Q is equal to y Roughness of elevation distribution between adjacent points in x direction and y direction respectively, and a calculation formula is as follows:
wherein h (j, k+1) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map, and h (j+1, k) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map;
s33, evaluating the matching availability of the point cloud digital elevation map, and if the feature parameters of the distribution of the terrains in S32 meet the threshold condition, judging that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, giving up the current point cloud digital elevation map data and continuing recursion of the inertial navigation data; the point cloud digital elevation map matching availability evaluation discriminant is:
wherein D is std And D rough Respectively determining threshold values of elevation standard deviation and terrain roughness; rule is a map matching availability evaluation result; true represents that the established point cloud digital elevation map can be used for subsequent map matching, false represents that the established point cloud digital elevation map cannot be used for subsequent map matching.
In this embodiment, S4 specifically includes:
s41, extracting map data in a three-time error range from the prior digital elevation map according to the position and position error output by inertial navigation at the current moment for matching the elevation map;
s42, sliding window traversal search is conducted on the extracted prior digital elevation map data, the window size is consistent with the size of the point cloud digital elevation map, normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current sliding window are calculated according to normalized cross-correlation similarity judgment criteria, elevation distribution similarity assessment is conducted, and a normalized cross-correlation coefficient calculation model is as follows:
wherein N is the number of the pixel grids of the point cloud digital image; i DEM And I LiDAR The elevation values of corresponding points in the prior digital elevation map and the point cloud digital elevation map are respectively; (u, v) is the position deviation value of the point cloud digital elevation map area relative to the prior digital elevation map area; mu (mu) DEM And mu LiDAR Respectively averaging the elevation data in the prior digital elevation map and the point cloud digital elevation map; sigma (sigma) DEM And sigma (sigma) LiDAR The standard deviation of elevation data in the two prior digital elevation maps and the point cloud digital elevation map are respectively, and NCC (u, v) represents normalized cross correlationCoefficients;
s43, in order to ensure accuracy and reliability of the elevation matching result, limiting the elevation matching result, namely introducing a normalized cross-correlation coefficient threshold value, and judging that a sufficiently accurate matching result exists only when the normalized cross-correlation coefficient is larger than the normalized cross-correlation coefficient threshold value, so as to obtain an elevation map matching position result.
In this embodiment, the integrated navigation system adopts an optimal filtering method to realize fusion of different navigation source data, where the optimal filtering includes two steps of prediction and updating, and S5 specifically includes:
s51, modeling a terrain matching auxiliary navigation positioning error and an inertia measurement element deviation, constructing an error state optimal filtering state prediction model, and performing inertial navigation recursive calculation and optimal filtering prediction processes; the optimal filtering state prediction model may be a kalman filtering state prediction model;
s52, if the established point cloud digital elevation map has matching availability and the map matching result meets the normalized cross-correlation coefficient judgment criterion in the step S43, an error state optimal filtering observation model is established based on the matching position result, the error state optimal filtering observation model can be a Kalman filtering observation model, and all state quantities of Kalman filtering are updated and corrected, otherwise, the step S51 is only executed.
As shown in fig. 3, the present invention further provides a laser radar terrain matching auxiliary navigation system, which is suitable for a laser radar terrain matching auxiliary navigation method, including: the system comprises a laser radar module, an inertial navigation module, a point cloud coordinate conversion and navigation belt splicing module, a point cloud map data processing module, a point cloud digital elevation map construction and matching availability evaluation module, a terrain matching calculation module and a navigation error correction module.
The laser radar module is used for measuring the terrain features below the carrier in real time, acquiring three-dimensional terrain point cloud data including terrain elevation, and acting on the subsequent point cloud coordinate conversion and navigation belt splicing module.
The inertial navigation module is used for providing position, gesture and navigation error recurrence information in the carrier movement process, and acts on the point cloud coordinate conversion and navigation belt splicing module and the terrain matching calculation module.
The point cloud coordinate conversion and navigation belt splicing module is used for carrying out coordinate conversion and navigation belt splicing on three-dimensional terrain point cloud data based on carrier pose recurrence information, and accumulating the point cloud data in a certain navigation belt area; the point cloud coordinate conversion and navigation belt splicing module is mainly used for a point cloud map data processing module to provide laser point cloud map original data. The specific treatment process comprises the following steps:
According to the carrier pose recursion information, three-dimensional terrain point cloud data p under a local coordinate system l of the laser radar at the corresponding moment l Performing coordinate conversion and navigation band splicing to obtain point cloud data p in a certain navigation band region under a global coordinate system g g Wherein the ith point cloud data p i The coordinate conversion of (2) is:
wherein b is a carrier coordinate system;for the ith point cloud data p i Position information in the global coordinate system g; />For the ith point cloud data p i Position information in a local coordinate system of the laser radar; />A coordinate transformation matrix for transforming the carrier coordinate system into a global coordinate system; />The coordinate transformation matrix is a coordinate transformation matrix from a local coordinate system of the laser radar to a carrier coordinate system.
The point cloud map data processing module is used for downsampling the point cloud data in the navigation area; managing the down-sampled point cloud data through a KD tree structure, traversing and searching for adjacent points in a certain radius range by taking each point as a center, and eliminating point cloud rough difference points by calculating an elevation distribution average value and an elevation distribution standard deviation of the adjacent points; the point cloud map data processing module is mainly used for the point cloud digital elevation map construction and matching availability evaluation module. The specific treatment process comprises the following steps:
Downsampling the point cloud data in a certain rectangular range of the selected navigation belt region by a voxel grid method, wherein the voxel grid is the same as the prior digital elevation map in resolution;
managing the down-sampled point cloud data based on the KD tree structure, circularly traversing each point in the current point cloud data, searching adjacent point data in a range with the radius of r by taking each traversed point as a center, calculating an elevation distribution average value mu and an elevation distribution standard deviation sigma of the adjacent point, and judging whether the elevation h of the current point meets a rough difference point judging condition, wherein the rough difference point judging condition is as follows:
|h-μ|＞3σ
when the conditions are met, the current point is judged to be the rough difference point, the current point is removed from the point cloud data, the KD tree is updated, and if not, the current point is not the rough difference point, and the current point is reserved.
The point cloud digital elevation map construction and matching availability evaluation module is used for filtering out the ground object point clouds in the point cloud data after the rough difference points are removed, and building grids and numbers according to the prior digital elevation map resolution to obtain the point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data; the point cloud digital elevation map construction and matching availability evaluation module determines whether to perform subsequent terrain matching calculation. The specific treatment process comprises the following steps:
Filtering the point cloud data with the rough difference points removed by a cloth simulation filtering algorithm, fitting the vacant grids filtered by the point cloud data with the rough difference points removed by a cloth simulation method, and establishing and numbering grids according to the resolution of the prior digital elevation map on the point cloud data to obtain a gridded point cloud digital elevation map;
calculating the feature parameters of the topographic distribution of the point cloud digital elevation map, including an elevation mean value M h Standard deviation sigma of elevation h And terrain roughness sigma z The calculation formula is as follows:
wherein, the size of the point cloud digital elevation map is m multiplied by n; h (j, k) is the point cloud elevation value at the jth row and the kth column in the point cloud digital elevation map; q (Q) x And Q is equal to y Roughness of elevation distribution between adjacent points in x direction and y direction respectively, and a calculation formula is as follows:
wherein h (j, k+1) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map, and h (j+1, k) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map;
evaluating the matching availability of the point cloud digital elevation map, and if the feature parameters of the terrain distribution meet the threshold conditions, judging that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, giving up the current point cloud digital elevation map data, and continuing recursion of the inertial navigation data; the point cloud digital elevation map matching availability evaluation discriminant is:
Wherein D is std And D rough Respectively determining a threshold value of the standard deviation of the elevation and a threshold value of the roughness of the terrain; rule is a map matching availability evaluation result; true represents that the established point cloud digital elevation map can be used for subsequent map matching, false represents that the established point cloud digital elevation map cannot be used for subsequent map matching.
The terrain matching calculation module is used for establishing a map search area, calling prior digital elevation map data in the map search area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, and obtaining position information of a carrier in the prior digital elevation map, otherwise, discarding the current matching result and continuing recursion inertial navigation data. The terrain matching calculation module determines whether to perform navigation error correction. The specific treatment process comprises the following steps:
extracting map data within a three-time error range from the prior digital elevation map according to the position and position error output by inertial navigation at the current moment, and using the map data for matching the elevation map;
Sliding window traversal search is carried out on the extracted prior digital elevation map data, the window size is consistent with the size of the point cloud digital elevation map, the normalized cross-correlation coefficient between the point cloud digital elevation map and the prior digital elevation map in the current sliding window is calculated according to the normalized cross-correlation judgment criterion, and the normalized cross-correlation coefficient calculation model is as follows:
wherein N is the number of the pixel grids of the point cloud digital image; i DEM And I LiDAR The elevation values of corresponding points in the prior digital elevation map and the point cloud digital elevation map are respectively; (u, v) is the position deviation value of the point cloud digital elevation map area relative to the prior digital elevation map area; mu (mu) DEM And mu LiDAR Respectively averaging the elevation data in the prior digital elevation map and the point cloud digital elevation map; sigma (sigma) DEM And sigma (sigma) LiDAR The standard deviation of elevation data in the prior digital elevation map and the point cloud digital elevation map are respectively, and NCC (u, v) represents normalized cross correlation coefficients;
judging whether the normalized cross-correlation coefficient is larger than a normalized cross-correlation coefficient threshold value, and obtaining an elevation map matching position result when the normalized cross-correlation coefficient is larger than the normalized cross-correlation coefficient threshold value.
The navigation error correction module is used for constructing a combined navigation system formed by the inertial navigation system and the laser radar terrain matching navigation system by taking the position information of the carrier in the prior digital elevation map as observation information, estimating the error of the inertial navigation system and correcting the error of the inertial navigation system.
According to the terrain matching auxiliary navigation system, a conventional measurement sensor scheme of combining an air pressure altimeter and a radio altimeter is replaced by a laser radar, and three-dimensional point cloud data distributed on the terrain below a carrier are scanned through a laser radar module in the process of carrier flight; the coordinate conversion and navigation belt splicing module completes coordinate conversion of point cloud data according to pose information provided by the inertial navigation module, and completes construction of a laser point cloud digital elevation map and subsequent evaluation of the laser point cloud digital elevation map matching availability through the map construction and map matching availability evaluation module, so that the effectiveness of the laser point cloud digital elevation map data is ensured, the measurement range of a terrain matching auxiliary navigation system is expanded, the situation that a conventional measurement sensor is easily interfered in the scanning process to cause terrain mismatching is effectively avoided, and the stability and reliability of the terrain matching auxiliary navigation system are ensured.
The present embodiment provides a computer device including: the system comprises a memory and a processor, wherein the memory stores a computer program which can be run on the processor, and when the processor executes the computer program, the steps of the laser radar topography matching auxiliary navigation method are realized.
The present embodiment provides a computer-readable storage medium, on which a computer program is stored, which when executed by a processor, implements the steps of a lidar terrain matching assisted navigation method.
Those of ordinary skill in the art will appreciate that: all or part of the steps for implementing the above method embodiments may be implemented by hardware related to program instructions, and the foregoing program may be stored in a computer readable storage medium, where the program, when executed, performs steps including the above method embodiments; and the aforementioned storage medium includes: a mobile storage device, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), a magnetic disk or an optical disk, or the like, which can store program codes.
In the present specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, and identical and similar parts between the embodiments are all enough to refer to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (9)
1. The laser radar terrain matching auxiliary navigation method is characterized by comprising the following steps of:
s1: acquiring three-dimensional terrain point cloud data below a carrier through a laser radar, carrying out coordinate conversion and navigation belt splicing on the three-dimensional terrain point cloud data through carrier pose recurrence information provided by inertial navigation, and accumulating point cloud data in a certain navigation belt area;
s2: selecting point cloud data in a certain range of the navigation belt area to carry out downsampling; managing the down-sampled point cloud data by utilizing a KD tree structure, traversing and searching adjacent points within a certain radius range of each point, and eliminating the point cloud rough difference points by calculating the elevation distribution average value and the elevation distribution standard deviation of the adjacent points;
S3: filtering the ground object point cloud in the point cloud data after the rough difference points are removed, and establishing a grid and a number according to the resolution of the prior digital elevation map to obtain a point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data;
s4: establishing a map searching area, calling prior digital elevation map data in the map searching area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, and obtaining position information of a carrier in the prior digital elevation map, otherwise, giving up the current matching result and continuing recursion inertial navigation data;
s5: and (3) taking the position information of the carrier in the prior digital elevation map as observation information, constructing a combined navigation system formed by an inertial navigation system and a laser radar terrain matching navigation system, estimating an inertial navigation system error, and correcting the inertial navigation system error.
2. The laser radar terrain matching assisted navigation method according to claim 1, wherein S1 comprises:
s11, acquiring three-dimensional terrain point cloud data below a carrier through a laser radar;
s12, acquiring carrier movement acceleration and angular velocity through an inertial measurement element, and acquiring carrier pose recursion information through inertial navigation recursion calculation and optimal filtering prediction processes, wherein the carrier pose recursion information specifically comprises carrier position, carrier pose and navigation error information;
s13, according to carrier pose recurrence information, three-dimensional terrain point cloud data p in a local coordinate system l of the laser radar at the corresponding moment l Performing coordinate conversion and navigation band splicing to obtain point cloud data p in a certain navigation band region under a global coordinate system g g Wherein the ith point cloud data p i The coordinate conversion of (2) is:
wherein b is a carrier coordinate system;for the ith point cloud data p i Position information in the global coordinate system g; />For the ith point cloud data p i Position information in a local coordinate system of the laser radar; />A coordinate transformation matrix for transforming the carrier coordinate system into a global coordinate system; />The coordinate transformation matrix is a coordinate transformation matrix from a local coordinate system of the laser radar to a carrier coordinate system.
3. The laser radar terrain matching assisted navigation method according to claim 1, wherein S2 comprises:
S21, selecting point cloud data in a certain rectangular range of a navigation belt area, and downsampling the point cloud data in the area by a voxel grid method, wherein the voxel grid is the same as the prior digital elevation map in resolution;
s22, managing the down-sampled point cloud data based on a KD tree structure, circularly traversing each point in the current point cloud data, searching adjacent point data in a range with the radius of r by taking each traversed point as a center, calculating an elevation distribution average value mu and an elevation distribution standard deviation sigma of the adjacent points, and judging whether the elevation h of the current point meets a rough difference point judging condition, wherein the rough difference point judging condition is as follows:
|h-μ|＞3σ
when the conditions are met, the current point is judged to be the rough difference point, the current point is removed from the point cloud data, the KD tree is updated, and if not, the current point is not the rough difference point, and the current point is reserved.
4. The laser radar terrain matching assisted navigation method of claim 1, wherein S3 comprises:
s31, filtering ground object point clouds in the point cloud data after the rough difference point is removed through a cloth simulation filtering algorithm, fitting the vacant grids after the ground object point clouds are filtered through a cloth simulation method, and building and numbering grids according to the prior digital elevation map resolution ratio of the point cloud data to obtain a gridded point cloud digital elevation map;
S32, calculating the feature parameters of the topographic distribution of the point cloud digital elevation map, including an elevation mean value M h Standard deviation sigma of elevation h And terrain roughness sigma z The calculation formula is as follows:
wherein, the size of the point cloud digital elevation map is m multiplied by n; h (j, k) is the point cloud elevation value at the jth row and the kth column in the point cloud digital elevation map; q (Q) x And Q is equal to y Roughness of elevation distribution between adjacent points in x direction and y direction respectively, and a calculation formula is as follows:
wherein h (j, k+1) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map, and h (j+1, k) represents the point cloud elevation value at the (k+1) th row in the point cloud digital elevation map;
s33, evaluating the matching availability of the point cloud digital elevation map, and if the terrain distribution characteristic parameters in S32 meet the threshold condition, judging that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, giving up the current point cloud digital elevation map data, and continuing recursion of the inertial navigation data; the point cloud digital elevation map matching availability evaluation discriminant is:
wherein D is std And D rough Respectively determining a threshold value of the standard deviation of the elevation and a threshold value of the roughness of the terrain; rule is a map matching availability evaluation result; true represents that the established point cloud digital elevation map can be used for subsequent map matching, false represents that the established point cloud digital elevation map cannot be used for subsequent map matching.
5. The laser radar terrain matching assisted navigation method of claim 1, wherein S4 comprises:
s41, extracting map data in a three-time error range from the prior digital elevation map according to the position and position error output by inertial navigation at the current moment for matching the elevation map;
s42, sliding window traversal search is carried out on the extracted prior digital elevation map data, the window size is consistent with the size of the point cloud digital elevation map, the normalized cross-correlation coefficient between the point cloud digital elevation map and the prior digital elevation map in the current sliding window is calculated according to a normalized cross-correlation judgment criterion, and a normalized cross-correlation coefficient calculation model is as follows:
wherein N is the number of the pixel grids of the point cloud digital image; i DEM And I LiDAR The elevation values of corresponding points in the prior digital elevation map and the point cloud digital elevation map are respectively; (u, v) is the position deviation value of the point cloud digital elevation map area relative to the prior digital elevation map area; mu (mu) DEM And mu LiDAR Respectively averaging the elevation data in the prior digital elevation map and the point cloud digital elevation map; sigma (sigma) DEM And sigma (sigma) LiDAR The standard deviation of elevation data in the two prior digital elevation maps and the point cloud digital elevation map are respectively, and NCC (u, v) represents normalized cross correlation coefficients;
S43, judging whether the normalized cross-correlation coefficient is larger than a normalized cross-correlation coefficient threshold, and obtaining an elevation map matching position result when the normalized cross-correlation coefficient is larger than the normalized cross-correlation coefficient threshold.
6. The laser radar terrain matching assisted navigation method of claim 1, wherein S5 comprises:
s51, modeling a terrain matching auxiliary navigation positioning error and an inertia measurement element deviation, constructing an error state optimal filtering state prediction model, and performing inertia navigation recursive calculation and optimal filtering prediction processes;
s52, if the built point cloud digital elevation map has matching availability and the map matching result meets the normalized cross-correlation coefficient judgment criterion, an error state optimal filtering observation model is built based on the matching position result, and all state quantities of optimal filtering are updated and corrected, otherwise, only S51 is executed.
7. A lidar terrain matching assisted navigation system adapted for use in a lidar terrain matching assisted navigation method according to any of claims 1 to 6, comprising: the system comprises a laser radar module, an inertial navigation module, a point cloud coordinate conversion and navigation belt splicing module, a point cloud map data processing module, a point cloud digital elevation map construction and matching availability evaluation module, a terrain matching calculation module and a navigation error correction module;
The laser radar module is used for acquiring three-dimensional terrain point cloud data below the carrier;
the inertial navigation module is used for acquiring carrier pose recurrence information in the carrier motion process provided by inertial navigation;
the point cloud coordinate conversion and navigation belt splicing module is used for carrying out coordinate conversion and navigation belt splicing on three-dimensional terrain point cloud data based on carrier pose recurrence information, and accumulating the point cloud data in a certain navigation belt area;
the point cloud map data processing module is used for downsampling the point cloud data in a certain range of the navigation belt area; managing the down-sampled point cloud data by utilizing a KD tree structure, traversing and searching adjacent points in a certain radius range by taking each point as a center, and eliminating point cloud rough difference points by calculating an elevation distribution average value and an elevation distribution standard deviation of the adjacent points;
the point cloud digital elevation map construction and matching availability evaluation module is used for filtering out the ground object point clouds in the point cloud data after the rough difference points are removed, and building grids and numbers according to the prior digital elevation map resolution to obtain the point cloud digital elevation map; carrying out matching availability evaluation on the point cloud digital elevation map, if the condition is met, carrying out subsequent map similarity matching, otherwise, giving up the current point cloud digital elevation map data to continue recursion inertial navigation data;
The terrain matching calculation module is used for establishing a map searching area, calling prior digital elevation map data in the map searching area, carrying out sliding window matching, calculating normalized cross-correlation coefficients between the point cloud digital elevation map and the prior digital elevation map in the current pane according to normalized cross-correlation judgment criteria, judging that a matching result is effective if the normalized cross-correlation coefficients are larger than a set threshold value, and obtaining position information of a carrier in the prior digital elevation map, otherwise, discarding the current matching result and continuing recursion inertial navigation data;
the navigation error correction module is used for constructing a combined navigation system formed by the inertial navigation system and the laser radar terrain matching navigation system by taking the position information in the carrier prior digital elevation map as observation information, estimating the error of the inertial navigation system and correcting the error of the inertial navigation system.
8. A computer device, comprising: a memory and a processor, the memory having stored thereon a computer program executable on the processor, when executing the computer program, performing the steps of the method of any of claims 1 to 6.
9. A computer-readable storage medium, characterized in that the storage medium has stored thereon a computer program which, when executed by a processor, implements the steps of the method according to any of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310938254.7A CN116879917A (en) | 2023-07-28 | 2023-07-28 | Laser radar terrain matching auxiliary navigation method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310938254.7A CN116879917A (en) | 2023-07-28 | 2023-07-28 | Laser radar terrain matching auxiliary navigation method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116879917A true CN116879917A (en) | 2023-10-13 |
Family
ID=88254797
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310938254.7A Pending CN116879917A (en) | 2023-07-28 | 2023-07-28 | Laser radar terrain matching auxiliary navigation method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116879917A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117333688A (en) * | 2023-12-01 | 2024-01-02 | 西安现代控制技术研究所 | High-precision terrain matching method based on multidimensional gradient characteristics |
-
2023
- 2023-07-28 CN CN202310938254.7A patent/CN116879917A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117333688A (en) * | 2023-12-01 | 2024-01-02 | 西安现代控制技术研究所 | High-precision terrain matching method based on multidimensional gradient characteristics |
| CN117333688B (en) * | 2023-12-01 | 2024-03-15 | 西安现代控制技术研究所 | High-precision terrain matching method based on multidimensional gradient characteristics |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111272165B (en) | Intelligent vehicle positioning method based on characteristic point calibration | |
| CN110146909B (en) | Positioning data processing method | |
| CN110412635B (en) | GNSS/SINS/visual tight combination method under environment beacon support | |
| Bergman et al. | Terrain navigation using Bayesian statistics | |
| JP5162849B2 (en) | Fixed point position recorder | |
| JP4984659B2 (en) | Own vehicle position estimation device | |
| CN111982106A (en) | Navigation method, navigation device, storage medium and electronic device | |
| WO2022088723A1 (en) | Data processing method and apparatus | |
| CN116879917A (en) | Laser radar terrain matching auxiliary navigation method and system | |
| CN113515128B (en) | Unmanned vehicle real-time path planning method and storage medium | |
| CN113252038A (en) | Course planning terrain auxiliary navigation method based on particle swarm optimization | |
| KR20230014724A (en) | Vehicle localization system and method | |
| CN110989619B (en) | Method, apparatus, device and storage medium for locating objects | |
| CN115183762A (en) | Airport warehouse inside and outside mapping method, system, electronic equipment and medium | |
| CN113155126B (en) | Visual navigation-based multi-machine cooperative target high-precision positioning system and method | |
| CN117029870A (en) | Laser odometer based on road surface point cloud | |
| CN116106904B (en) | Facility deformation monitoring method and facility deformation monitoring equipment for object MT-InSAR | |
| CN116929363A (en) | Mining vehicle autonomous navigation method based on passable map | |
| CN116952224A (en) | Adaptive inertia/geomagnetic integrated navigation method based on geomagnetic chart suitability evaluation | |
| CN114660641B (en) | Self-adaptive GPS fusion positioning system, method and medium | |
| CN115930948A (en) | Orchard robot fusion positioning method | |
| CN113237482B (en) | Robust vehicle positioning method in urban canyon environment based on factor graph | |
| Topan et al. | Georeferencing accuracy assessment of high-resolution satellite images using figure condition method | |
| Campbell et al. | Terrain‐Referenced Positioning Using Airborne Laser Scanner | |
| Escourrou et al. | Ndt localization with 2d vector maps and filtered lidar scans |
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 |