CN116879917A - Laser radar terrain matching auxiliary navigation method and system - Google Patents

Abstract

The present invention provides a lidar terrain matching auxiliary navigation method and system. The method includes using the pose information provided by the inertial navigation system to realize coordinate conversion of laser three-dimensional terrain point cloud data and flight strip splicing processing, and then perform rough point cloud error. Eliminate and cloth simulate filtering to construct a point cloud digital elevation map; compare the similarity between the point cloud digital elevation map and the prior digital elevation map to obtain the position correction information of the carrier; achieve inertial navigation and laser point cloud terrain matching through optimal filtering The results are fused and the inertial navigation system errors are corrected. The invention effectively improves the terrain elevation detection level and map matching accuracy of the terrain matching auxiliary navigation system through point cloud projection imaging model, point cloud data filtering processing algorithm and point cloud digital elevation similarity matching, and expands the system's ability to operate under complex terrain conditions. availability.

Description

A lidar terrain matching assisted navigation method and system

Technical field

The present invention relates to the technical field of navigation and positioning, and more specifically to a lidar terrain matching assisted navigation method and system.

Background technique

When airborne navigation systems operate in complex working environments, they are often accompanied by various interference and disturbance problems, such as interference or disappearance of GNSS signals in urban or forest areas, environmental magnetic interference, electromagnetic interference, etc., which will cause navigation problems. The system has great hidden dangers in the process of navigation and ensuring flight safety.

Terrain Aided Navigation (TAN), as one of the widely used integrated navigation systems, has the advantages of strong anti-interference ability, high universality and easy operation and implementation. It can effectively solve the problem of suppressing the error divergence of inertial navigation systems in GNSS-denied environments. problem to achieve effective autonomous navigation. The traditional terrain matching assisted navigation method uses barometric altimeters and radio altimeters as measurement sensors to measure terrain elevation profile data along the route and determine the aircraft's geographical location based on the best matching position. However, this type of measurement sensor solution has limited data collection capabilities, and the data collection process is susceptible to interference, leading to false matching results.

As an active measurement system, lidar can measure the terrain structure under the carrier by emitting laser beams. It has a larger measurement range and more accurate measurement results than the traditional terrain matching auxiliary navigation measurement sensor solution, and The operation is more stable. At the same time, combined with efficient and reliable point cloud data processing technology, a more accurate and information-rich terrain elevation map can be established.

Therefore, applying laser point cloud data processing technology to terrain matching-assisted navigation to construct a higher-precision and reliable point cloud terrain elevation map that matches a priori terrain elevation maps can further improve the accuracy and reliability of the terrain matching-assisted navigation and positioning method. , to meet its application needs in more complex terrain conditions.

Contents of the invention

In view of this, the present invention provides a lidar terrain matching auxiliary navigation method and system, which uses point cloud data acquisition, coordinate conversion and flight strip splicing, coarse point elimination and point cloud data filtering processing algorithms, and similarity of point cloud elevation maps. Sexual matching effectively improves the terrain elevation detection level and map matching accuracy of the terrain matching auxiliary navigation system, and also enhances the usability of the terrain matching auxiliary navigation system under complex terrain conditions.

In order to achieve the above objects, the present invention adopts the following technical solutions:

A lidar terrain matching assisted navigation method includes the following steps:

S1: Acquire the three-dimensional terrain point cloud data under the carrier through lidar, and perform coordinate conversion and flight belt splicing on the three-dimensional terrain point cloud data through the carrier pose recursion information provided by inertial navigation, and accumulate point cloud data within a certain flight belt area. ;

S2: Select point cloud data within a certain range of the flight zone area for downsampling; manage the downsampled point cloud data through the KD tree structure, traverse and search for the nearest neighbor points within a certain radius of each point, and calculate the distance of the nearest neighbor point The mean value of the elevation distribution and the standard deviation of the elevation distribution realize the elimination of coarse points in the point cloud;

S3: Filter out the surface object point clouds in the point cloud data after removing the coarse errors, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map; conduct a matching usability evaluation on the point cloud digital elevation map, If the conditions are met, subsequent map similarity matching will be performed, otherwise the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived;

S4: Establish a map search area, retrieve the a priori digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the a priori digital elevation map in the current pane based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between ;

S5: Using the position information of the carrier in the prior digital elevation map as observation information, construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system, estimate the inertial navigation system error, and then correct the inertial navigation system error. .

Preferably, S1 includes:

S11. Obtain the three-dimensional terrain point cloud data below the carrier through lidar;

S12. The terrain matching auxiliary navigation platform uses inertial measurement elements to obtain the carrier's motion acceleration and angular rate, and obtains the carrier's posture recursive information through the inertial navigation recursive calculation and optimal filter prediction processes, specifically including the carrier's position, attitude and navigation. error information;

S13. According to the pose recursion information, perform coordinate conversion and flight belt splicing on the three-dimensional point cloud data ^p l under the lidar local coordinate system l at the corresponding time, and obtain the point cloud data p g in a certain flight belt area under the global coordinate system ^g. , where the coordinates of the i-th point cloud data p _i are converted to:

In the formula, b is the carrier coordinate system; is the position information of the laser point cloud data p _i in the global coordinate system g; similarly, is the position information of p _i in the lidar local coordinate system l;/> is the coordinate transformation matrix from the carrier coordinate system to the global coordinate system, which is obtained by integrating the inertial motion data by the inertial navigation system, which involves the process of interpolating the pose data between adjacent moments to obtain the pose transformation information at the intermediate moment; /> It is the coordinate transformation matrix from the local coordinate system of the lidar to the carrier coordinate system. Usually, since the lidar is installed on the carrier in a fixed connection, the transformation matrix does not change with time and is a constant value transformation matrix.

Preferably, S2 includes:

S21. Select the point cloud data within a certain rectangular range of the flight zone area and downsample the point cloud data in the area through the voxel grid method. The voxel grid size is the same as the resolution of the prior digital elevation map;

S22. Manage the downsampled point cloud data based on the KD tree structure, loop through each point in the current point cloud data, and search for neighboring points within a range of radius r with itself as the center and each traversed point. Data, calculate the average value of the elevation distribution μ and the standard deviation of the elevation distribution σ of the neighboring points, and determine whether the height h of the current point satisfies the rough difference judgment condition. The rough difference judgment condition is:

|h-μ|＞3σ

When the above conditions are met, the current point is determined to be a rough point, the current point is removed from the point cloud data and the KD tree is updated. Otherwise, the current point is not a rough point and the current point is retained.

Preferably, S3 includes:

S31. For the point cloud data after the coarse point cloud removal, use the cloth simulation filtering algorithm to filter out the ground object point clouds in the point cloud data after the coarse point cloud removal, and use the cloth simulation method to filter the empty grid of the ground object point cloud. Perform fitting, establish a grid and number the point cloud data according to the resolution of the prior digital elevation map, and obtain a gridded point cloud digital elevation map;

S32. Calculate the terrain distribution characteristic parameters of the point cloud digital elevation map, including elevation mean M _h , elevation standard deviation σ _h and terrain roughness σ _z . The calculation formula is:

In the formula, the size of the point cloud digital elevation map is m×n; h(j,k) is the point cloud elevation value at the jth row and kth column in the point cloud digital elevation map; Q _x and Q _y are the x direction and The roughness of the elevation distribution between adjacent points in the y direction, the calculation formula is:

In the formula, h(j,k+1) represents the point cloud elevation value at the jth row and k+1th column in the point cloud digital elevation map, h(j+1,k) is the jth point cloud digital elevation map Point cloud elevation value at row k of +1;

S33. Evaluate the matching availability of the point cloud digital elevation map. If the terrain distribution characteristic parameters in S32 meet the threshold conditions, then determine that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, abandon the current point cloud digital elevation map data and continue. Recursive inertial navigation data; point cloud digital elevation map matching usability evaluation discriminant is:

In the formula, D _std and D _rough are the elevation standard deviation judgment threshold and terrain roughness judgment threshold respectively; Rule is the map matching usability evaluation result; True means that the built point cloud digital elevation map can be used for subsequent map matching, and False means that the built point cloud digital elevation map can be used for subsequent map matching. Point cloud digital elevation maps cannot be used for subsequent map matching.

Preferably, S4 includes:

S41. According to the position and position error output by the inertial navigation at the current moment, extract the map data within three times the error range from the prior digital elevation map for elevation map matching;

S42. Perform a sliding window traversal search on the extracted a priori digital elevation map data. The window size is consistent with the size of the point cloud digital elevation map. Calculate the point cloud digital elevation map and the current a priori number in the sliding window based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between elevation maps, the calculation model of the normalized cross-correlation coefficient is:

In the formula, N is the number of pixel grids in the point cloud digital image; I _DEM and I _LiDAR are the elevation values of the corresponding points in the prior digital elevation map and the point cloud digital elevation map respectively; (u, v) are the point cloud digital elevation map The position deviation value of the area relative to the a priori digital elevation map area; μ _DEM and μ _LiDAR are respectively the average value of the elevation data in the a priori digital elevation map and the point cloud digital elevation map; σ _DEM and σ _LiDAR are two a priori numbers respectively. The standard deviation of the elevation data in the elevation map and point cloud digital elevation map, NCC(u,v) represents the normalized cross-correlation coefficient;

S43. Determine whether the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold. When the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold, the elevation map matching position result is obtained.

Preferably, S5 includes:

S51. Model the terrain matching-assisted navigation positioning error and the inertial measurement element deviation, and build an optimal filtering state prediction model for the error state;

S52. If the built point cloud digital elevation map has matching availability and the map matching results meet the normalized cross-correlation coefficient determination criteria, then build an error state optimal filtering observation model based on the matching position results, and update each state quantity of the optimal filtering and corrections, otherwise only S51 is executed.

The invention also discloses a lidar terrain matching auxiliary navigation system, which is suitable for a lidar terrain matching auxiliary navigation method, including: a lidar module, an inertial navigation module, a point cloud coordinate conversion and air strip splicing module, and a point cloud map. Data processing module, point cloud digital elevation map construction and matching availability evaluation module, terrain matching calculation module and navigation error correction module;

Lidar module, used to obtain three-dimensional terrain point cloud data below the carrier;

The inertial navigation module is used to obtain the carrier pose recursion information during the carrier movement provided by the inertial navigation;

The point cloud coordinate conversion and flight belt splicing module is used to perform coordinate conversion and flight belt splicing of three-dimensional terrain point cloud data based on carrier pose recursion information, and accumulate point cloud data within a certain flight belt area;

The point cloud map data processing module is used to downsample point cloud data within a certain range of the flight zone area; manage the downsampled point cloud data through the KD tree structure, and traverse and search each point centered on itself. For neighboring points within a certain radius, the coarse points of the point cloud can be eliminated by calculating the average elevation distribution and standard deviation of the elevation distribution of the neighboring points;

The point cloud digital elevation map construction and matching usability evaluation module is used to filter out the feature point clouds in the point cloud data after removing the coarse points, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map. ;Evaluate the matching availability of the point cloud digital elevation map. If the conditions are met, subsequent map similarity matching will be performed. Otherwise, the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived;

The terrain matching calculation module is used to establish a map search area, retrieve a priori digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the current pane according to the normalized cross-correlation criterion. The normalized cross-correlation coefficient between the prior digital elevation maps. If the normalized cross-correlation coefficient is greater than the set threshold, the matching result is judged to be valid and the position information of the carrier in the prior digital elevation map is obtained. Otherwise, the current match is given up. The results continue to recurse the inertial navigation data;

The navigation error correction module is used to use the position information of the carrier in the prior digital elevation map as observation information to construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system and estimate the inertial navigation system error, and then calculate the inertial navigation system error. Navigation system errors are corrected.

It can be seen from the above technical solution that compared with the traditional terrain matching auxiliary navigation method that uses barometric altimeters and radio altimeters as measurement sensors to obtain terrain elevation profile data of the flight route and perform elevation matching, the present invention uses lidar as the measurement sensor. Point cloud data characteristics are used to construct a point cloud digital elevation map and match it with the airborne prior digital elevation map database. Finally, based on the elevation matching calculation results, the inertial navigation positioning error is corrected. This invention uses lidar to construct a larger point cloud digital elevation map, and expands the elevation "line" data of the traditional measurement scheme into elevation "surface" data, effectively reducing the vulnerability of terrain matching auxiliary navigation to noise interference and causing mismatching. , which improves the level of terrain elevation detection, the accuracy of elevation matching results, and the accuracy of terrain matching-assisted navigation and positioning.

Description of the drawings

In order to explain the embodiments of the present invention or the technical solutions in the prior art more clearly, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description are only These are embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on the provided drawings without exerting creative efforts.

Figure 1 is a flow chart of a lidar terrain matching assisted navigation method provided by the present invention;

Figure 2 is a schematic diagram of the point cloud coarse point elimination process provided by the present invention;

Figure 3 is a schematic structural diagram of a lidar terrain matching auxiliary navigation system provided by the present invention;

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of the present invention.

As shown in Figure 1, an embodiment of the present invention discloses a lidar terrain matching assisted navigation method, which includes the following steps:

S1: Use lidar to obtain the three-dimensional terrain point cloud data below the carrier, and use the carrier pose recursion information provided by inertial navigation to perform coordinate conversion and flight belt splicing on the three-dimensional terrain point cloud data, and accumulate a certain time range, that is, a certain flight belt area. point cloud data;

S2: Select the point cloud data within a certain range in the flight zone area to downsample it; use the KD tree structure to manage the downsampled point cloud data, traverse and search for the nearest neighbor points within a certain radius of each point, and calculate the nearest neighbor The average point elevation distribution and the standard deviation of the elevation distribution realize the elimination of coarse points in the point cloud;

S3: Filter out the surface object point clouds in the point cloud data after removing the coarse errors, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map; conduct a matching usability evaluation on the point cloud digital elevation map, If the conditions are met, subsequent map similarity matching will be performed, otherwise the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived;

S4: Establish a map search area, retrieve the a priori digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the a priori digital elevation map in the current pane based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between navigation data;

S5: Using the position information of the carrier in the prior digital elevation map as observation information, construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system, estimate the inertial navigation system error, and then correct the inertial navigation system error. . Among them, the inertial navigation system is used as the main navigation system, and the inertial navigation is used as the state variable to be estimated for optimal filtering; the lidar terrain matching navigation system is used as the auxiliary navigation system, and the results obtained from terrain matching are used as observation information for correction. Inertial navigation errors.

In this embodiment, S1 specifically includes:

S11. Obtain the three-dimensional terrain point cloud data below the carrier through lidar;

S12. The terrain matching auxiliary navigation platform uses inertial measurement elements to obtain the carrier's motion acceleration and angular rate, and obtains the carrier's posture recursive information through the inertial navigation recursive calculation and Kalman filter prediction processes, specifically including the carrier's position, attitude and navigation. error information;

Step S13: According to the carrier pose recursion information, coordinate conversion and flight belt splicing are performed on the point cloud data p ^l in the lidar local coordinate system l at the corresponding moment, to obtain point cloud data p in a certain flight zone area in the global coordinate system g. ^g , where the coordinates of the i-th point cloud data p _i are converted to:

In the formula, b is the carrier coordinate system; is the position information of the laser point cloud data p _i in the global coordinate system g; similarly, is the position information of p _i in the lidar local coordinate system l;/> is the coordinate transformation matrix from the carrier coordinate system to the global coordinate system, which is obtained by integrating the inertial motion data by the inertial navigation system, which involves the process of interpolating the pose data between adjacent moments to obtain the pose transformation information at the intermediate moment; /> It is the coordinate transformation matrix from the local coordinate system of the lidar to the carrier coordinate system. Usually, since the lidar is installed on the carrier in a fixed connection, the transformation matrix does not change with time and is a constant value transformation matrix.

In this embodiment, S2 specifically includes:

S21. Select point cloud data within a certain rectangular range of the flight zone area. In order to ensure the calculation efficiency of the navigation system, use the voxel grid method to downsample the point cloud data in the selected rectangular area. The voxel grid size needs to be the same as The a priori digital elevation maps have the same resolution;

S22. As shown in Figure 2, establish management of point cloud data after voxel grid downsampling based on the KD tree structure, loop through each point in the current point cloud, and search for each traversed point with its own center and radius For the neighboring point data within a range of size r, calculate the average value μ of the neighboring point elevation distribution and the standard deviation σ of the elevation distribution. If the height of the current point meets the rough point determination condition, the current point is removed from the point cloud data and the KD tree is updated. , continue traversing, and the rough difference determination condition is:

|h-μ|＞3σ

In the formula, h is the altitude value of the current point. When the above conditions are met, the current point is determined to be a rough point and removed and the KD tree is updated. Otherwise, it is not a rough point and retained.

In this embodiment, S3 specifically includes:

S31. For the point cloud data after removing the coarse points, further use the cloth simulation filtering algorithm to filter out the ground object point clouds, and use the cloth simulation method to fit the vacant grid after filtering the ground object point clouds. The specific process of cloth simulation filtering algorithm is recorded in the document "Zhang W, Qi J, Wan P, et al. An easy-to-use airborne LiDARdata filtering method based on cloth simulation[J].Remote sensing, 2016, 8(6) :501.", the idea is to flip the point cloud. Assume that a piece of cloth falls from above due to the effect of "gravity", and the final falling cloth represents the result of flipping the surface part of the point cloud. In this way, the ground Separate point cloud and ground object point cloud data. Establish a grid and number the point cloud data according to the resolution of the prior digital elevation map in S21 to obtain a gridded point cloud digital elevation map;

S32. Calculate the terrain distribution characteristic parameters of the point cloud digital elevation map: elevation mean M _h , elevation standard deviation σ _h and terrain roughness σ _z , where σ _h reflects the degree of discreteness of the point cloud elevation values in the elevation map and the total terrain in the entire region. The degree of fluctuation; and σ _z can be used to characterize the average smoothness of the terrain elevation map and describe the more subtle local fluctuations. The calculation formula of each terrain characteristic parameter is:

In the formula, the size of the point cloud digital elevation map is m×n; h(j,k) is the point cloud elevation value at the jth row and kth column in the gridded map; Q _x and Q _y are the x direction and y respectively. The roughness of the elevation distribution between adjacent points in the direction, the calculation formula is:

In the formula, h(j,k+1) represents the point cloud elevation value at the jth row and k+1th column in the point cloud digital elevation map, h(j+1,k) is the jth point cloud digital elevation map Point cloud elevation value at row k of +1;

S33. Evaluate the matching availability of the point cloud digital elevation map. If the terrain distribution characteristic parameters in S32 meet the threshold conditions, then determine that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, abandon the current point cloud digital elevation map data and continue. Recursive inertial navigation data; point cloud digital elevation map matching usability evaluation discriminant is:

In the formula, D _std and D _rough are the elevation standard deviation and terrain roughness determination threshold respectively; Rule is the map matching usability evaluation result; True means that the built point cloud digital elevation map can be used for subsequent map matching, and False means that the built point cloud Digital elevation maps cannot be used for subsequent map matching.

In this embodiment, S4 specifically includes:

S41. According to the position and position error output by the inertial navigation at the current moment, extract the map data within three times the error range from the prior digital elevation map for elevation map matching;

S42. Perform a sliding window traversal search on the extracted a priori digital elevation map data. The window size is consistent with the size of the point cloud digital elevation map. Calculate the difference between the point cloud digital elevation map and the current sliding window based on the normalized cross-correlation similarity determination criterion. The normalized cross-correlation coefficient between prior digital elevation maps is used to evaluate the similarity of elevation distribution. The calculation model of the normalized cross-correlation coefficient is:

In the formula, N is the number of pixel grids in the point cloud digital image; I _DEM and I _LiDAR are the elevation values of the corresponding points in the prior digital elevation map and the point cloud digital elevation map respectively; (u, v) are the point cloud digital elevation map The position deviation value of the area relative to the a priori digital elevation map area; μ _DEM and μ _LiDAR are respectively the average value of the elevation data in the a priori digital elevation map and the point cloud digital elevation map; σ _DEM and σ _LiDAR are two a priori numbers respectively. The standard deviation of the elevation data in the elevation map and point cloud digital elevation map, NCC(u,v) represents the normalized cross-correlation coefficient;

S43. In order to ensure the accuracy and reliability of the elevation matching results, the elevation matching results are restricted, that is, a normalized cross-correlation coefficient threshold is introduced. Only when the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold, it is determined to be There are sufficiently accurate matching results to obtain elevation map matching location results.

In this embodiment, the integrated navigation system uses an optimal filtering method to achieve the fusion of different navigation source data, where the optimal filtering includes two steps: prediction and update. S5 specifically includes:

S51. Model the terrain matching-assisted navigation positioning error and the deviation of the inertial measurement element, construct an optimal filtering state prediction model of the error state, and perform the inertial navigation recursive calculation and optimal filtering prediction process; the optimal filtering state prediction model can be Kalman filter 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 determination criterion in step S43, then an error state optimal filter observation model is constructed based on the matching position result, and the error state optimal filter observation is The model can be a Kalman filter observation model, and each state quantity of the Kalman filter is updated and corrected. Otherwise, only step S51 is performed.

As shown in Figure 3, the present invention also provides a lidar terrain matching auxiliary navigation system, which is suitable for a lidar terrain matching auxiliary navigation method, including: lidar module, inertial navigation module, point cloud coordinate conversion and air strip splicing module, point cloud map data processing module, point cloud digital elevation map construction and matching usability evaluation module, terrain matching calculation module and navigation error correction module.

The lidar module is used to measure the terrain features under the carrier in real time, obtain three-dimensional terrain point cloud data including terrain elevation, and act on the subsequent point cloud coordinate conversion and flight strip splicing modules.

The inertial navigation module is used to provide the position, attitude and navigation error recursive information during the movement of the carrier, and acts on the point cloud coordinate conversion and air strip splicing module, and the terrain matching calculation module.

The point cloud coordinate conversion and flight belt splicing module is used to perform coordinate conversion and flight belt splicing of three-dimensional terrain point cloud data based on carrier pose recursion information, and accumulate point cloud data within a certain flight belt area; point cloud coordinate conversion and flight belt splicing. The splicing module is mainly used in the point cloud map data processing module to provide the original data of the laser point cloud map. The specific processing process is:

According to the carrier pose recursion information, the three-dimensional terrain point cloud data p ^l under the lidar local coordinate system l at the corresponding moment is coordinate transformed and air strip spliced, and the point cloud data p g in a certain air zone area under the global coordinate system ^g is obtained. , where the coordinates of the i-th point cloud data p _i are converted to:

In the formula, b is the carrier coordinate system; is the position information of the i-th point cloud data p _i in the global coordinate system g;/> is the position information of the i-th point cloud data p _i in the lidar local coordinate system l;/> is the coordinate transformation matrix from the carrier coordinate system to the global coordinate system;/> is the coordinate transformation matrix from the lidar local coordinate system to the carrier coordinate system.

The point cloud map data processing module is used to downsample the point cloud data in the air zone area; manage the downsampled point cloud data through the KD tree structure, and traverse and search each point with itself as the center and a certain radius. For nearby points within the range, the coarse points of the point cloud are eliminated by calculating the average elevation distribution and the standard deviation of the elevation distribution of the neighboring points; the point cloud map data processing module is mainly used in the construction of point cloud digital elevation maps and the matching usability evaluation module. The specific processing process is:

The point cloud data within a certain rectangular range of the selected flight zone area is downsampled through the voxel grid method. The voxel grid size is the same as the resolution of the prior digital elevation map;

The downsampled point cloud data is managed based on the KD tree structure, and each point in the current point cloud data is cyclically traversed, and each traversed point is searched for neighboring point data within a range of radius r with itself as the center. Calculate the average value μ of the elevation distribution and the standard deviation σ of the elevation distribution of the neighboring points, and determine whether the height h of the current point satisfies the rough difference judgment condition. The rough difference judgment condition is:

|h-μ|＞3σ

When the above conditions are met, the current point is determined to be a rough point, the current point is removed from the point cloud data and the KD tree is updated. Otherwise, the current point is not a rough point and the current point is retained.

The point cloud digital elevation map construction and matching usability evaluation module is used to filter out the feature point clouds in the point cloud data after removing the coarse points, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map. ; Perform matching usability evaluation on the point cloud digital elevation map. If the conditions are met, subsequent map similarity matching will be performed. Otherwise, the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived. Point cloud digital elevation map construction and matching usability evaluation module Determines whether to perform subsequent terrain matching calculations. The specific processing process is:

For the point cloud data after the coarse difference removal, the cloth simulation filtering algorithm is used to filter out the ground object point clouds in the point cloud data after the coarse difference removal, and the cloth simulation method is used to simulate the empty grid after the ground object point cloud filtering. Combined, the point cloud data is gridded and numbered according to the resolution of the prior digital elevation map, and a gridded point cloud digital elevation map is obtained;

Calculate the terrain distribution characteristic parameters of the point cloud digital elevation map, including elevation mean M _h , elevation standard deviation σ _h and terrain roughness σ _z . The calculation formula is:

In the formula, the size of the point cloud digital elevation map is m×n; h(j,k) is the point cloud elevation value at the jth row and kth column in the point cloud digital elevation map; Q _x and Q _y are the x direction and The roughness of the elevation distribution between adjacent points in the y direction, the calculation formula is:

In the formula, h(j,k+1) represents the point cloud elevation value at the jth row and k+1th column in the point cloud digital elevation map, h(j+1,k) is the jth point cloud digital elevation map Point cloud elevation value at row k of +1;

Evaluate the matching availability of the point cloud digital elevation map. If the terrain distribution characteristic parameters meet the threshold conditions, it is determined that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, the current point cloud digital elevation map data is abandoned and the recursive inertial navigation continues. Data; point cloud digital elevation map matching usability evaluation discriminant is:

In the formula, D _std and D _rough are the elevation standard deviation judgment threshold and terrain roughness judgment threshold respectively; Rule is the map matching usability evaluation result; True means that the built point cloud digital elevation map can be used for subsequent map matching, and False means that the built point cloud digital elevation map can be used for subsequent map matching. Point cloud digital elevation maps cannot be used for subsequent map matching.

The terrain matching calculation module is used to establish a map search area, retrieve the prior digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the prior digital elevation map in the current pane based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between the prior digital elevation maps. If the normalized cross-correlation coefficient is greater than the set threshold, the matching result is judged to be valid and the position information of the carrier in the prior digital elevation map is obtained. Otherwise, the current matching result is discarded. Continue to recurse inertial navigation data. The judgment result of the terrain matching calculation module determines whether to perform navigation error correction. The specific processing process is:

According to the position and position error output by inertial navigation at the current moment, map data within three times the error range is extracted from the prior digital elevation map for elevation map matching;

Perform a sliding window traversal search on the extracted a priori digital elevation map data. The window size is consistent with the size of the point cloud digital elevation map. The point cloud digital elevation map and the a priori digital elevation map within the current sliding window are calculated based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between , the normalized cross-correlation coefficient calculation model is:

In the formula, N is the number of pixel grids in the point cloud digital image; I _DEM and I _LiDAR are the elevation values of the corresponding points in the prior digital elevation map and the point cloud digital elevation map respectively; (u, v) are the point cloud digital elevation map The position deviation value of the area relative to the a priori digital elevation map area; μ _DEM and μ _LiDAR are the average values of the elevation data in the a priori digital elevation map and point cloud digital elevation map respectively; σ _DEM and σ _LiDAR are the a priori digital elevation respectively. The standard deviation of elevation data in maps and point cloud digital elevation maps, NCC(u,v) represents the normalized cross-correlation coefficient;

Determine whether the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold. When the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold, the elevation map matching position result is obtained.

The navigation error correction module is used to use the position information of the carrier in the a priori digital elevation map as observation information to construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system and estimate the inertial navigation system error, and then correct the inertial navigation System errors are corrected.

For the terrain matching auxiliary navigation system, the present invention replaces the conventional measurement sensor solution that combines a barometric altimeter and a radio altimeter with a lidar. During the flight of the carrier, the lidar module scans the three-dimensional point cloud data of the terrain distribution below the carrier; the coordinates The conversion and flight strip splicing module completes the coordinate conversion of the point cloud data based on the pose information provided by the inertial navigation module, and completes the construction of the laser point cloud digital elevation map and subsequent laser point cloud through the map construction and map matching usability assessment module. The evaluation of the usability of digital elevation map matching ensures the validity of laser point cloud digital elevation map data, expands the measurement range of the terrain matching auxiliary navigation system, and effectively avoids terrain mismatching caused by conventional measurement sensors being susceptible to interference during the scanning process. occurs, ensuring the stability and reliability of the terrain matching auxiliary navigation system.

This embodiment provides a computer device, including: a memory and a processor. The memory stores a computer program that can be run on the processor. When the processor executes the computer program, it implements the steps of the lidar terrain matching assisted navigation method.

This embodiment provides a computer-readable storage medium. A computer program is stored on the storage medium. When the computer program is executed by a processor, the steps of the laser radar terrain matching assisted navigation method are implemented.

Those of ordinary skill in the art can understand that all or part of the steps to implement the above method embodiments can be completed through hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, the execution includes: The steps of the above method embodiment; and the aforementioned storage media include: mobile storage devices, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), magnetic disks or optical disks, etc. The medium on which program code is stored.

Each embodiment in this specification is described in a progressive manner. Each embodiment focuses on its differences from other embodiments. The same and similar parts between the various embodiments can be referred to each other. As for the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple. For relevant details, please refer to the description in the method section.

The above description of the disclosed embodiments enables those 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 practiced in 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. A lidar terrain matching assisted navigation method, characterized by comprising the following steps: S1: Acquire the three-dimensional terrain point cloud data under the carrier through lidar, and perform coordinate conversion and flight belt splicing on the three-dimensional terrain point cloud data through the carrier pose recursion information provided by inertial navigation, and accumulate point cloud data within a certain flight belt area. ; S2: Select point cloud data within a certain range of the flight zone area for downsampling; use the KD tree structure to manage the downsampled point cloud data, traverse and search for the nearest neighbor points within a certain radius of each point, and calculate the distance of the nearest neighbor point The mean value of the elevation distribution and the standard deviation of the elevation distribution realize the elimination of coarse points in the point cloud; S3: Filter out the surface object point clouds in the point cloud data after removing the coarse errors, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map; conduct a matching usability evaluation on the point cloud digital elevation map, If the conditions are met, subsequent map similarity matching will be performed, otherwise the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived; S4: Establish a map search area, retrieve the a priori digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the a priori digital elevation map in the current pane based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between data; S5: Using the position information of the carrier in the prior digital elevation map as observation information, construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system, estimate the inertial navigation system error, and then correct the inertial navigation system error. . 2. A lidar terrain matching assisted navigation method according to claim 1, characterized in that S1 includes: S11. Obtain the three-dimensional terrain point cloud data below the carrier through lidar; S12. Obtain the carrier's motion acceleration and angular rate through the inertial measurement element, and obtain the carrier's posture recursive information through the inertial navigation recursive solution and optimal filter prediction process, specifically including the carrier's position, attitude, and navigation error information; S13. According to the carrier pose recursion information, perform coordinate conversion and flight belt splicing on the three-dimensional terrain point cloud data p ^l under the lidar local coordinate system l at the corresponding moment, to obtain point cloud data in a certain flight belt area under the global coordinate system g. p ^g , where the coordinates of the i-th point cloud data p _i are converted to: In the formula, b is the carrier coordinate system; is the position information of the i-th point cloud data p _i in the global coordinate system g;/> is the position information of the i-th point cloud data p _i in the lidar local coordinate system l;/> is the coordinate transformation matrix from the carrier coordinate system to the global coordinate system;/> is the coordinate transformation matrix from the lidar local coordinate system to the carrier coordinate system. 3. A lidar terrain matching assisted navigation method according to claim 1, characterized in that S2 includes: S21. Select the point cloud data within a certain rectangular range of the flight zone area and downsample the point cloud data in the area through the voxel grid method. The voxel grid size is the same as the resolution of the prior digital elevation map; S22. Manage the downsampled point cloud data based on the KD tree structure, loop through each point in the current point cloud data, and search for neighboring points within a range of radius r with itself as the center and each traversed point. Data, calculate the average value of the elevation distribution μ and the standard deviation of the elevation distribution σ of the neighboring points, and determine whether the height h of the current point satisfies the rough difference judgment condition. The rough difference judgment condition is: |h-μ|＞3σ When the above conditions are met, the current point is determined to be a rough point, the current point is removed from the point cloud data and the KD tree is updated. Otherwise, the current point is not a rough point and the current point is retained. 4. A lidar terrain matching assisted navigation method according to claim 1, characterized in that S3 includes: S31. For the point cloud data after the coarse point cloud removal, use the cloth simulation filtering algorithm to filter out the ground object point clouds in the point cloud data after the coarse point cloud removal, and use the cloth simulation method to filter the empty grid of the ground object point cloud. Perform fitting, establish a grid and number the point cloud data according to the resolution of the prior digital elevation map, and obtain a gridded point cloud digital elevation map; S32. Calculate the terrain distribution characteristic parameters of the point cloud digital elevation map, including elevation mean M _h , elevation standard deviation σ _h and terrain roughness σ _z . The calculation formula is: In the formula, the size of the point cloud digital elevation map is m×n; h(j,k) is the point cloud elevation value at the jth row and kth column in the point cloud digital elevation map; Q _x and Q _y are the x direction and The roughness of the elevation distribution between adjacent points in the y direction, the calculation formula is: In the formula, h(j,k+1) represents the point cloud elevation value at the jth row and k+1th column in the point cloud digital elevation map, h(j+1,k) is the jth point cloud digital elevation map Point cloud elevation value at row k of +1; S33. Evaluate the matching availability of the point cloud digital elevation map. If the terrain distribution characteristic parameters in S32 meet the threshold conditions, then determine that the current point cloud digital elevation map can be used for subsequent map matching; otherwise, abandon the current point cloud digital elevation map data and continue. Recursive inertial navigation data; point cloud digital elevation map matching usability evaluation discriminant is: In the formula, D _std and D _rough are the elevation standard deviation judgment threshold and terrain roughness judgment threshold respectively; Rule is the map matching usability evaluation result; True means that the built point cloud digital elevation map can be used for subsequent map matching, and False means that the built point cloud digital elevation map can be used for subsequent map matching. Point cloud digital elevation maps cannot be used for subsequent map matching. 5. A lidar terrain matching assisted navigation method according to claim 1, characterized in that S4 includes: S41. According to the position and position error output by the inertial navigation at the current moment, extract the map data within three times the error range from the prior digital elevation map for elevation map matching; S42. Perform a sliding window traversal search on the extracted a priori digital elevation map data. The window size is consistent with the size of the point cloud digital elevation map. Calculate the point cloud digital elevation map and the current a priori number in the sliding window based on the normalized cross-correlation criterion. The normalized cross-correlation coefficient between elevation maps, the calculation model of the normalized cross-correlation coefficient is: In the formula, N is the number of pixel grids in the point cloud digital image; I _DEM and I _LiDAR are the elevation values of the corresponding points in the prior digital elevation map and the point cloud digital elevation map respectively; (u, v) are the point cloud digital elevation map The position deviation value of the area relative to the a priori digital elevation map area; μ _DEM and μ _LiDAR are respectively the average value of the elevation data in the a priori digital elevation map and the point cloud digital elevation map; σ _DEM and σ _LiDAR are two a priori numbers respectively. The standard deviation of the elevation data in the elevation map and point cloud digital elevation map, NCC(u,v) represents the normalized cross-correlation coefficient; S43. Determine whether the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold. When the normalized cross-correlation coefficient is greater than the normalized cross-correlation coefficient threshold, the elevation map matching position result is obtained. 6. A lidar terrain matching assisted navigation method according to claim 1, characterized in that S5 includes: S51. Model the terrain matching-assisted navigation positioning error and inertial measurement element deviation, construct an error state optimal filter state prediction model, and perform the inertial navigation recursive solution and optimal filter prediction process; S52. If the built point cloud digital elevation map has matching availability and the map matching results meet the normalized cross-correlation coefficient determination criteria, then build an error state optimal filtering observation model based on the matching position results, and update each state quantity of the optimal filtering and corrections, otherwise only S51 is executed. 7. A lidar terrain matching auxiliary navigation system, which is suitable for a lidar terrain matching auxiliary navigation method according to any one of claims 1 to 6, characterized in that it includes: a lidar module, an inertial navigation module , point cloud coordinate conversion and flight strip splicing module, point cloud map data processing module, point cloud digital elevation map construction and matching usability evaluation module, terrain matching calculation module and navigation error correction module; Lidar module, used to obtain three-dimensional terrain point cloud data below the carrier; The inertial navigation module is used to obtain the carrier pose recursion information during the carrier movement provided by the inertial navigation; The point cloud coordinate conversion and flight belt splicing module is used to perform coordinate conversion and flight belt splicing of three-dimensional terrain point cloud data based on carrier pose recursion information, and accumulate point cloud data within a certain flight belt area; The point cloud map data processing module is used to downsample the point cloud data within a certain range of the air zone area; use the KD tree structure to manage the downsampled point cloud data, and traverse and search each point centered on itself. For neighboring points within a certain radius, the coarse points of the point cloud can be eliminated by calculating the average elevation distribution and standard deviation of the elevation distribution of the neighboring points; The point cloud digital elevation map construction and matching usability evaluation module is used to filter out the feature point clouds in the point cloud data after removing the coarse points, establish grids and numbers according to the resolution of the prior digital elevation map, and obtain the point cloud digital elevation map. ;Evaluate the matching availability of the point cloud digital elevation map. If the conditions are met, subsequent map similarity matching will be performed. Otherwise, the current point cloud digital elevation map data will be abandoned and the inertial navigation data will continue to be recursively derived; The terrain matching calculation module is used to establish a map search area, retrieve a priori digital elevation map data in the map search area, perform sliding window matching, and calculate the point cloud digital elevation map and the current pane according to the normalized cross-correlation criterion. The normalized cross-correlation coefficient between the prior digital elevation maps. If the normalized cross-correlation coefficient is greater than the set threshold, the matching result is judged to be valid and the position information of the carrier in the prior digital elevation map is obtained. Otherwise, the current match is given up. The results continue to recurse the inertial navigation data; The navigation error correction module is used to use the position information of the carrier in the prior digital elevation map as observation information to construct a combined navigation system formed by an inertial navigation system and a lidar terrain matching navigation system and estimate the inertial navigation system error, and then calculate the inertial navigation system error. Navigation system errors are corrected. 8. A computer device, characterized in that it includes: a memory and a processor, the memory stores a computer program that can run on the processor, and when the processor executes the computer program, the claim is realized Steps of the method described in any one of 1 to 6. 9. A computer-readable storage medium, characterized in that a computer program is stored on the storage medium, and when the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.