Laser radar-based water depth inversion and coastal zone topography mapping method

Abstract

The application discloses a kind of water depth inversion and coastal zone topography mapping method based on laser radar, the method is applied to the surveying instrument hung on small unmanned helicopter, realize the fast, accurate measurement to coastal zone topography.Surveying instrument includes following instrument: laser radar, camera, inertial measurement unit, global positioning unit.The method steps are as follows: complete the internal and external parameter calibration of each sensor on surveying instrument;Using unmanned aerial vehicle to carry surveying instrument to go to the region to be measured, moves and obtains the topographic data of the region to be measured;Using Kalman filter to preliminarily construct odometer, constructs land topographic map;Using laser radar point cloud elevation record sea wave movement characteristics, obtain laser radar point cloud elevation sequence, obtain nearshore water depth by cBathy water depth estimation algorithm and convert into sea area topographic map;Two topographic maps are combined into complete coastal zone topographic map based on geographic coordinates.

Description

Technical Field

[0001] This invention belongs to the technical field of topographic mapping, specifically relating to a method for water depth inversion and coastal topographic mapping based on lidar. Background Technology

[0002] Airborne mapping systems are integrated solutions that combine mapping sensors (such as lidar and cameras), high-precision inertial measurement units (IMUs), high-precision satellite positioning modules, long-distance data transmission modules, and ground data post-processing systems. In recent years, with the increasing maturity of autonomous unmanned aerial vehicle (UAV) technology, the use of small autonomous UAVs equipped with airborne mapping systems for surveying has become increasingly common. Compared to traditional total stations and manual field surveying, airborne mapping systems effectively reduce surveying time and costs, and can effectively survey areas inaccessible to personnel.

[0003] LiDAR can acquire precise distance and intensity information from the environment, making it suitable for airborne terrain mapping tasks. However, it requires the construction of high-precision LiDAR odometry. Currently, there are three main types of LiDAR odometry: pure radar models, loosely coupled models, and tightly coupled models. Pure radar models accumulate errors over long periods of operation, and without other sensors, these errors are difficult to correct, affecting subsequent mapping. Loosely coupled models add IMU (Insulated Mutual Detection Unit) assistance for pose correction, but since they essentially use LiDAR and IMU data separately without constructing a new loss function, they are not very accurate. Tightly coupled models input multiple sensors into a single model, constructing constraint relationships and using optimization methods to minimize these constraints, making them a more accurate choice. Currently, there are two main methods for tightly coupled models: filtering-based methods and nonlinear optimization-based methods. Filtering-based methods use Kalman filtering to obtain the optimal estimate, offering high accuracy, but are prone to drift over long periods of state estimation. Nonlinear optimization-based methods primarily use graph optimization, treating sensor data as factors for factor graph optimization. However, if the processing accuracy of the sensor observation data is low, the accuracy of the factor graph optimization results will also be low.

[0004] Using airborne cameras for seabed topographic mapping can quickly and effectively estimate water depth and thus construct seabed topographic maps. However, image-based inversion is easily affected by lighting conditions and prone to errors. Therefore, there is an urgent need to propose a high-precision mapping method that utilizes UAVs and multi-sensor data processing technology. Summary of the Invention

[0005] The main objective of this invention is to overcome the shortcomings and deficiencies of the prior art and provide a method for water depth inversion and coastal topographic mapping based on lidar.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] A method for water depth inversion and coastal topographic mapping based on lidar is applied to a mapping instrument mounted on a small unmanned helicopter. The mapping instrument includes: a lidar, a camera, an inertial measurement unit (IMU) consisting of an accelerometer and a gyroscope, and a global positioning module (GNSS). The lidar is used to acquire three-dimensional topographic data, the camera is used to acquire color images, the IMU is used to acquire real-time acceleration data, the gyroscope is used to acquire real-time angular velocity data, and the GNSS is used to acquire real-time latitude, longitude, altitude, heading angle, and time data. The mapping method includes the following steps:

[0008] S1. Complete the spatial synchronization between the sensors on the surveying instrument, calibrate the external parameters between the lidar and the IMU, and realize the spatial synchronization between the sensors;

[0009] S2. Control the drone to hover first, and then move it above the area to be measured to scan the area to be measured, and obtain land area scan data and ocean area scan data respectively;

[0010] S3. Obtain land area lidar data from land area scanning data. The land area lidar data is composed of frames. Based on IMU data containing the timestamp of land area lidar data, the pre-integration state is obtained by recursively calculating the state of the surveyor.

[0011] S4. Transform each point of the land area lidar data into the mapping instrument coordinate system B at the time of the last point of the land area lidar data using the pre-integration state. The time of the last point is referred to as the end acquisition time, and the distortion-free point cloud is obtained.

[0012] S5. Perform error Kalman filtering on the distortion-free point cloud to obtain the filtered state;

[0013] S6. Calculate the relative pose between the filtered state and the optimized state of the previous frame. Add the relative pose as a laser inertial odometry factor to the factor graph. Obtain GNSS data from the land area scanning data, add a priori position factor based on the GNSS data, and optimize the factor graph to obtain the optimized state.

[0014] S7. Based on the optimized state, convert the land area lidar data to the world coordinate system W and construct a land topographic map;

[0015] S8. Extract the marine area lidar data from the marine area scanning data, preprocess the marine area lidar data based on the optimized state to obtain the point cloud height program column, convert the point cloud height program column into water depth information, represent the water depth information in the form of point cloud, and obtain the seabed topography map.

[0016] S9. Merge the land topographic map and the seabed topographic map into a coastal zone topographic map.

[0017] Furthermore, the data collected by each sensor in the mapping instrument, including lidar, camera, IMU, and GNSS, are all timestamped based on the same time base. These timestamping based on the same time base clearly defines the correspondence between the data collected by each sensor, which is a prerequisite for state estimation.

[0018] Furthermore, in step S1, the extrinsic parameter matrices of the lidar and the IMU are: in It is the rotation transformation matrix of the coordinate axes in the IMU coordinate system relative to the LiDAR coordinate system. This represents the position of the origin of the IMU coordinate system within the LiDAR coordinate system. The extrinsic parameter matrices of the LiDAR and IMU are used to achieve coordinate transformation between the data in the LiDAR coordinate system and the data in the IMU coordinate system, which is beneficial for fusing LiDAR data and IMU data in land areas.

[0019] Furthermore, step S3 is as follows:

[0020] S31. Assume that the coordinate system B of the surveying instrument is equivalent to the IMU coordinate system. Define the world coordinate system W as the northeast-east coordinate system. The three axes of the world coordinate system W point as follows: the X-axis points to the north axis, the Y-axis points to the east axis, and the Z-axis is perpendicular to the ground and downwards.

[0021] S32. Define the rotation matrix of the surveying instrument coordinate system B relative to the world coordinate system W as follows: Location is Define the velocity of the surveying instrument in the world coordinate system W as v, and the zero angular velocity bias as b. g The acceleration has zero bias as b a Define the state of the surveying instrument as x = Define b g The derivative is n bg n bg For zero bias noise of angular velocity, define b a The derivative is n ba n ba For zero bias noise in acceleration, the symbol for the pre-integral state is defined as... These are the recursive results of the surveying instrument status. v、b g b a Define the state symbol after filtering as These are the error Kalman filters, respectively. v、b g b a The optimized state symbol is defined as These are the factor graphs after optimization. v、b g b a ;

[0022] S33. Obtain each frame of LiDAR data and define the start time of each frame of LiDAR data as t. s The end time is t e , to obtain t s Time to t e IMU data at time t s The state at time t is the optimized state.

[0023] S34, Let t s Time to t e There are two adjacent frames of IMU data between time points, with the timestamp corresponding to time t. i and t j , and t i <t j , t i The acceleration measurement value of the IMU at that time Angular velocity measurement value t j The acceleration measurement value at time t is Angular velocity measurement value The recursive formula is as follows:

[0024]

[0025] In the above formula, dt is the time interval, and a avg and ω avg These are the mean values ​​of acceleration and angular velocity, ||ω avg || is ω avg The length of the mold, yes antisymmetric matrix, It is t i Moment It is t j Moment a_v wLet W be the acceleration in the world coordinate system W, and let W be the covariance matrix of the error state during the recursive process. The update formula is: in, and It is t i and t j Covariance matrix at time step and F w Let Q be the coefficient matrix and Q be the noise covariance matrix.

[0026] S35. According to the recursive formula, The update formula and [t] s ,t e IMU data between ], recursively get recursion get and For t s Moment and and For t e Moment and To obtain the propagation state of each IMU data point at the corresponding time point during the recursive process, As a pre-integration state.

[0027] Pre-integral state It is for t e The prediction of the state at each time step provides prior information for the update steps of the error Kalman filter. The recursive covariance matrix takes into account the uncertainty of the system, providing a more accurate estimate for the update steps of the error Kalman filter.

[0028] Furthermore, in step S4, each point of the land area lidar data is transformed to t based on the propagation state of each IMU data at the corresponding time. e At time B, in the coordinate system B of the mapping instrument, the distortion-free point cloud is obtained.

[0029] This invention uses the timestamp of the final acquisition time of each frame of land area lidar data as the timestamp of a single frame of land area lidar data during data processing. However, during the acquisition of a frame of land area lidar data, the lidar is not completely stationary. Therefore, each point in a frame of land area lidar data is located in the lidar coordinate system (Lidar) at the acquisition time of that point. If we assume that they are all located in the lidar coordinate system (Lidar) at the final acquisition time, we will obtain a distorted point cloud. A distorted point cloud will lead to incorrect state estimation results. Transforming each point of the land area lidar data to t... e Obtaining the distortion-free point cloud in the coordinate system B of the mapping instrument at any given time is beneficial for improving the accuracy of state estimation.

[0030] Furthermore, step S5 is as follows:

[0031] S51, according to t e Pre-integral state at time 1 Transform each point of the land area lidar data from the mapping instrument coordinate system B to the world coordinate system W;

[0032] S52. Suppose that the current frame of land area lidar data has a total of n points, and the coordinates of the k-th point in the world coordinate system W are: These are the coordinates along the X, Y, and Z axes in the world coordinate system W, and their coordinates in the surveying instrument coordinate system B are... The coordinates of the voxel corresponding to the calculation point in the voxel map Let X, Y, and Z be the coordinates of the voxel corresponding to the k-th point in the world coordinate system W along the X, Y, and Z axes, respectively. A voxel is a cube, L x L y L z These are the side lengths of a voxel in the X, Y, and Z axes of the world coordinate system W. A voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frames of land area lidar data. This point cloud is referred to as the voxel's internal point cloud. If the coordinates... The corresponding voxel interior point cloud can fit a plane. Substitute the point into the plane equation of the fitted plane of the voxel interior point cloud, and calculate the distance z from the point to the plane. k Calculate the Jacobian matrix corresponding to the k-th point. in It is t e At present For matrix transpose, u kIt is the normal vector of the plane fitted to the voxel interior point cloud corresponding to the k-th point. The symbol "*" represents matrix multiplication. If the voxel interior point cloud cannot fit the plane or the distance z k If the distance is greater than 0.1 meters, then abandon the k-th point;

[0033] In a voxel map, each voxel maintains a plane. When calculating the distance from each point in the land area lidar data to the plane, the distance between the point and the fitted plane of the voxel's internal point cloud is calculated, instead of fitting a plane for each point, which improves computational efficiency.

[0034] S53. Perform step S52 on all points, and set H... k Matrix merging yields z k Merge, and obtain Calculate Kalman gain Where γ is the noise covariance matrix of the lidar point cloud measurement, and the superscript "-1" indicates that the matrix is ​​inverted. Update the state: Among them, symbols This indicates that the state variables are added together to update the covariance matrix. Where I represents the identity matrix, and the obtained state This is the filtered state.

[0035] The point-to-plane distance calculated based on the pre-integrated state inevitably contains errors. These errors mainly originate from two sources: noise in the land area lidar data and the error between the pre-integrated state and the actual state. The point-to-plane distance z calculated based on the pre-integrated state... k The probability that the distance is equal to the actual distance is By maximizing The state and covariance update equations for the error Kalman filter can be obtained.

[0036] Furthermore, step S6 is as follows:

[0037] S61, will include t e The GNSS data at time t is converted to the world coordinate system W, denoted as . t is calculated using linear fitting. e GNSS data at time when If the distance to the prior position factor from the last addition to the factor map is greater than 5 meters or the attitude change is greater than 0.3 rad, then... Added to the factor graph as a priori location factor node;

[0038] S62. Filter the state. The relative pose with respect to the previously optimized state is added to the factor map as a laser inertial odometry factor, and factor map optimization is performed to obtain t.e Optimized state at any moment

[0039] Error Kalman filtering only imposes relative constraints on the state without global constraints, so the filtered state still contains errors. When a surveying instrument performs large-scale, long-term surveying operations, errors accumulate, leading to a significant decrease in estimation accuracy. Factor map optimization by fusing GNSS data can add global constraints and reduce accumulated errors.

[0040] Furthermore, step S7 is as follows:

[0041] According to t e The optimized state at any time as well as Transform all points in the current frame of land area lidar data to the world coordinate system W, where... and For t e Moment and Register the current frame of land area lidar data into the voxel map to obtain the data from the start of mapping to t. e A topographical map of the land at any given time.

[0042] The state after Kalman filtering and factor graph optimization This is the optimal estimate of the mapping instrument's status. Registering the LiDAR data for the land area into a voxel map using the optimized status yields a high-precision land topographic map.

[0043] Furthermore, step S8 is as follows:

[0044] S81. Extract the marine area lidar data from the marine area scanning data. Assume the marine area lidar data has a total of m points, and the coordinates of the s-th point in the world coordinate system W are... The coordinates in coordinate system B of the surveying instrument are: According to t e The optimized state at any time as well as Transform all points in the marine area lidar data to the world coordinate system W to obtain sea surface point cloud data;

[0045] S82. Define a rectangular region in the world coordinate system W. The two sides of the rectangular region are parallel to the direction along the coastline and the direction across the coastline, respectively. Rasterize the sea surface point cloud data in the rectangular region. The grid is a square. The side length of the square is determined according to the inversion resolution. For each grid, extract all radar points in the grid and calculate their average elevation as the elevation of the grid. Here, the elevation refers to the Z-axis coordinate of the radar point in the world coordinate system W. Obtain a frame of point cloud elevation data.

[0046] S83. Repeat step S82 for each frame of sea surface point cloud data to obtain the point cloud elevation sequence. Convert the point cloud elevation sequence into time stack data data. Assume that there are a total of μ frames of the point cloud elevation sequence and β elevation data in each frame of the point cloud elevation sequence. Then the size of data is μ rows and β columns. Place the Lth frame of the point cloud elevation sequence into the Lth row of data. Time stack data is useful for analyzing physical quantities such as the propagation direction, propagation speed and propagation frequency of ocean waves from the sea surface point cloud data.

[0047] S84. Using the cBathy depth estimation algorithm, estimate the wave frequency and wave number based on the time stack data. Calculate the water depth of each grid cell according to the water wave formula. Use the water depth plus the tidal value as the Z-axis coordinate of each grid cell to obtain a point cloud in the world coordinate system W. This point cloud is a seabed topographic map, where the tidal value is the distance from the sea level to the world elevation origin, obtained from lidar data.

[0048] The cBathy water depth estimation algorithm was published in 2013 in the Journal of Geophysical Research: Oceans, Volume 118, pp. 2595-2609, with the title: cBathy: A robust algorithm for estimating nearshore bathymetry.

[0049] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0050] 1. This invention utilizes a multi-sensor mapping instrument for coastal topographic mapping, completing the mapping of both land and marine areas of the coastal zone using only one set of equipment. Existing methods require changing different equipment to map the land and marine areas separately. Compared to existing methods, the mapping method of this invention is more convenient.

[0051] 2. This invention uses voxel maps to store points and fitted planes when constructing maps. Compared to methods that directly use point cloud maps, it can more accurately account for the uncertainties in the map caused by point measurement and lidar point cloud measurement noise, achieving higher mapping accuracy.

[0052] 3. This invention uses error Kalman filtering to estimate the state of the surveying instrument. Compared to extended Kalman filtering, error Kalman filtering does not require linearization of the Jacobian matrix, effectively avoiding errors caused by linearization and achieving higher computational efficiency and accuracy.

[0053] 4. This invention uses factor graphs to further optimize the state after error Kalman filtering. By fusing prior GNSS pose data, the mapping method can maintain high-accuracy state estimation in large-scale and long-term mapping operations. Furthermore, factor graphs are easy to incorporate data from various sensors, exhibiting good scalability.

[0054] 5. This invention uses high-order sequence of lidar point cloud for water depth inversion. Compared with water depth inversion methods using camera image sequences, this method can effectively avoid errors caused by environmental factors such as lighting, making the surveying operation more adaptable to the environment and obtaining more accurate surveying results. Attached Figure Description

[0055] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0056] Figure 1 This is a flowchart of the method for water depth inversion and coastal topography mapping based on lidar disclosed in this invention;

[0057] Figure 2 This is a map of the selected surveying area in an implementation example of the lidar-based water depth inversion and coastal topographic mapping method disclosed in this invention.

[0058] Figure 3 This is a schematic diagram of the position trajectory under three conditions in the lidar-based water depth inversion and coastal topography mapping method disclosed in this invention;

[0059] Figure 4 This is a schematic diagram of the water depth results obtained by performing water depth inversion based on point cloud high-order program sequences according to the water depth inversion and coastal topography mapping method based on lidar disclosed in this invention.

[0060] Figure 5 This is a topographical schematic diagram of the water depth inversion and coastal topographic mapping method based on lidar disclosed in this invention;

[0061] Figure 6 This invention discloses a topographic contour map in the water depth inversion and coastal topographic mapping method based on lidar.

[0062] Figure 7 This is a schematic diagram of the water depth results obtained by performing water depth inversion based on optical images using the water depth inversion and coastal topographic mapping method based on lidar disclosed in this invention. Detailed Implementation

[0063] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0064] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application can be combined with other embodiments.

[0065] Example 1

[0066] This embodiment discloses a specific implementation process of a method for water depth inversion and coastal topographic mapping based on lidar. The following is a detailed description... Figure 1 Detailed introduction to the implementation process:

[0067] Step S1: Calibrate the extrinsic parameters between the lidar and the IMU to achieve spatial synchronization between the sensors. The extrinsic parameter matrix between the lidar and the IMU is as follows: It is the rotation transformation matrix of the IMU coordinate system axes relative to the LiDAR coordinate system. It is the position of the origin of the IMU coordinate system in the LiDAR coordinate system.

[0068] Step S2: Operate the drone equipped with a surveying instrument to fly over the area to be measured, and scan the area to be measured to obtain land area scan data and marine area scan data respectively. The data collection location in this embodiment is as follows: Figure 2 As shown.

[0069] Step S3: Obtain land area lidar data from land area scanning data. The land area lidar data is composed of frames. Based on IMU data containing land area lidar data timestamps, the pre-integration state is obtained by recursively calculating the mapper state.

[0070] In this embodiment, the process of recursively obtaining the pre-integral state is as follows:

[0071] S31. Assume that the coordinate system B of the surveying instrument is equivalent to the IMU coordinate system. Define the world coordinate system W as the northeast-east coordinate system. The three axes of the world coordinate system W point as follows: the X-axis points to the north axis, the Y-axis points to the east axis, and the Z-axis is perpendicular to the ground and downwards.

[0072] S32. Define the rotation matrix of the surveying instrument coordinate system B relative to the world coordinate system W as follows: Location is Define the velocity of the surveying instrument in the world coordinate system W as v, and the zero angular velocity bias as b. g The acceleration has zero bias as b a Define the state of the surveying instrument as x = Define b g The derivative is n bg n bg For zero bias noise of angular velocity, define b a The derivative is n ba n ba For zero bias noise in acceleration, the symbol for the pre-integral state is defined as... These are the recursive results of the surveying instrument status. v、b g b a Define the state symbol after filtering as These are the error Kalman filters, respectively. v、b g b a The optimized state symbol is defined as These are the factor graphs after optimization. v、b g b a ;

[0073] S33. Obtain each frame of LiDAR data and define the start time of each frame of LiDAR data as t. s The end time is t e , to obtain t s Time to t e IMU data at time t s The state at time t is the optimized state.

[0074] S34, Let t s Time to t e There are two adjacent frames of IMU data between time points, with the timestamp corresponding to time t. i and t j , and t i <t j , t i The acceleration measurement value of the IMU at that time Angular velocity measurement value t j The acceleration measurement value at time t is Angular velocity measurement value The recursive formula is as follows:

[0075]

[0076] In the above formula, dt is the time interval, and a avg and ω avg These are the mean values ​​of acceleration and angular velocity, ||ω avg || is ω avg The length of the mold, yes antisymmetric matrix, It is t i Moment It is t j Moment a_v w Let W be the acceleration in the world coordinate system W, and let W be the covariance matrix of the error state during the recursive process. The update formula is: in, and It is t i and t j Covariance matrix at time step and F w Let Q be the coefficient matrix and Q be the noise covariance matrix.

[0077] S35. According to the recursive formula, The update formula and [t] s ,t e IMU data between ], recursively get recursion get and For t s Moment and and For t e Moment and To obtain the propagation state of each IMU data point at the corresponding time point during the recursive process, As a pre-integration state.

[0078] Because the noise matrix is ​​not considered during the recursive process, there will be some error between the pre-integrated state and the true state. To obtain a more accurate state, it is necessary to estimate the error using LiDAR point cloud data, and then add the estimated error to the pre-integrated state to obtain a more accurate state. Since the error is generally small, the higher-order derivative terms in the calculation process can be ignored, which helps to improve the calculation speed and accuracy.

[0079] Step S4: Transform each point in the land area lidar data to the coordinate system B of the mapping instrument at the time when the last point of the lidar point cloud is located using the pre-integration state. The time when the last point is located is referred to as the end acquisition time, thus obtaining the distortion-free point cloud.

[0080] In this embodiment, when processing a frame of land area lidar data, the timestamp of the last acquisition time of this frame of data is used as the timestamp of this frame of land area lidar data. During the scanning process of a frame of land area lidar data, the lidar is not completely stationary. The points scanned by the lidar are in the lidar coordinate system Lidar at the acquisition time of that point. All points of a frame of land area lidar data are transformed from the lidar coordinate system Lidar at the acquisition time of that point to the mapping instrument coordinate system B corresponding to the last acquisition time, so as to realize the motion distortion correction of the point cloud and obtain the distortion-free point cloud.

[0081] Step S5: Perform error Kalman filtering on the distorted point cloud to obtain the filtered state.

[0082] This step involves the following operations:

[0083] S51, according to t e Pre-integral state at time 1 Transform the land area lidar data from the mapping instrument coordinate system B to the world coordinate system W;

[0084] S52. Suppose that the current frame of land area lidar data has a total of n points, and the coordinates of the k-th point in the world coordinate system W are: These are the coordinates along the X, Y, and Z axes in the world coordinate system W, and their coordinates in the surveying instrument coordinate system B are... The coordinates of the voxel corresponding to the calculation point in the voxel map Let X, Y, and Z be the coordinates of the voxel corresponding to the k-th point in the world coordinate system W along the X, Y, and Z axes, respectively. A voxel is a cube, L x L y L zThese are the side lengths of a voxel in the X, Y, and Z axes of the world coordinate system W. A voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frames of land area lidar data, hereinafter referred to as the voxel internal point cloud. If the point cloud inside the corresponding voxel can fit a plane, then substitute the point into the equation of the plane fitted by the point cloud inside the voxel and calculate the distance z from the point to the plane. k Calculate the Jacobian matrix corresponding to the k-th point. in It is t e At present For matrix transpose, u k It is the normal vector of the interior plane of the voxel corresponding to the k-th point. The symbol "*" represents matrix multiplication. If the point cloud inside the voxel cannot fit a plane or the distance z k If the distance is greater than 0.1 meters, then abandon the k-th point;

[0085] S53. Perform the operation of step S52 on all points of the current frame of land area lidar data, and transfer H... k Matrix merging yields z k Merge, and obtain Calculate Kalman gain Where γ is the noise covariance matrix of the lidar point cloud measurement, and the superscript "-1" indicates that the matrix is ​​inverted. Update the state: Among them, symbols This indicates that the state variables are added together to update the covariance matrix. The obtained state This is the filtered state.

[0086] Here, we first use the filtered state as the final state without performing subsequent factor graph optimization operations, and the estimated position is as follows: Figure 3 As shown by the dashed line, Figure 3 The thick solid line represents high-precision location data acquired by GNSS. It is assumed that the location data acquired by GNSS represents the actual location when the surveying instrument is working. As can be seen, the dashed line is only close to the thick solid line at the beginning, but deviates from the thick solid line after a period of time.

[0087] Step S6: Calculate the relative pose of the filtered state and the optimized state of the previous frame, add the relative pose as a laser inertial odometry factor to the factor graph, obtain GNSS data from the land area scanning data, add a priori position factor based on the GNSS data, and perform factor graph optimization to obtain the optimized state.

[0088] Error Kalman filtering only imposes relative constraints on the state without global constraints, so the filtered state still contains errors. When a surveying instrument performs large-scale, long-term surveying operations, errors accumulate, leading to a significant decrease in estimation accuracy. Using factor maps to fuse GNSS data can add global constraints and reduce accumulated errors.

[0089] Factor graph optimization mainly involves the following steps:

[0090] S61, will include t e The GNSS data at time t is converted to the world coordinate system W, denoted as . t is calculated using linear fitting. e GNSS data at time when If the distance to the prior pose factor from the last addition to the factor map is greater than 5 meters or the pose change is greater than 0.3 rad, then... Added to the factor graph as a prior pose factor node;

[0091] S62. Filter the state. The relative pose with respect to the previously optimized state is added to the factor map as a laser inertial odometry factor, and factor map optimization is performed to obtain t. e Optimized state at any moment

[0092] The estimated position corresponding to the optimized state is as follows: Figure 3 As shown by the thin solid line, it can be seen that the thin solid line and the thick solid line are very close most of the time, indicating that the optimized state has high accuracy.

[0093] Step S7: Based on the optimized state, transform the land area lidar data to the world coordinate system W to construct a land topographic map. The optimized state is the best estimated state. Based on this state, transforming the land area lidar data to the world coordinate system W can yield a high-precision land topographic map.

[0094] According to t e The optimized state at any time as well as Transform all radar points in the land area lidar data to the world coordinate system W, where... and For t e Moment and Registering land area lidar data into a voxel map yields data from the start of mapping to t. e A topographical map of the land at any given time.

[0095] Step S8: Extract the marine area lidar data from the marine area scanning data, preprocess the marine area lidar data based on the optimized state to obtain the point cloud height sequence, convert the point cloud height sequence into water depth information, and represent the water depth information in the form of point cloud to obtain the seabed topography map.

[0096] The main process of this step is as follows:

[0097] S81. Extract the marine area lidar data from the marine area scanning data. Assume the marine area lidar data has a total of m points, and the coordinates of the s-th point in the world coordinate system W are... The coordinates in coordinate system B of the surveying instrument are: According to t e The optimized state at any time as well as Transform all points in the marine area lidar data to the world coordinate system W to obtain sea surface point cloud data;

[0098] S82. Define a rectangular region in the world coordinate system W. The two sides of the rectangular region are parallel to the direction along the coastline and the direction across the coastline, respectively. Rasterize the sea surface point cloud data in the rectangular region. The grid is a square. The side length of the square is determined according to the inversion resolution. For each grid, extract all radar points in the grid and calculate their average elevation. Here, the elevation refers to the Z-axis coordinate of the radar point in the world coordinate system W. Obtain a frame of point cloud elevation data.

[0099] S83. Repeat step S82 for each frame of sea surface point cloud data to obtain point cloud elevation sequence. Convert the point cloud elevation sequence into time stack data data. Assume that there are a total of μ frames of point cloud elevation sequence and β elevation data in each frame of point cloud elevation sequence. Then the size of data is μ rows and β columns. Place the Lth point cloud elevation sequence into the Lth row of data.

[0100] S84. Using the cBathy depth estimation algorithm, estimate the wave frequency and wave number based on the time stack data. Calculate the water depth of each grid cell according to the water wave formula. Use the water depth plus the tidal value as the Z-axis coordinate of each grid cell to obtain a point cloud in the world coordinate system W. This point cloud is a seabed topographic map, where the tidal value is the distance from the sea level to the world elevation origin, obtained from lidar data.

[0101] The water depth obtained using the cBathy water depth estimation algorithm based on point cloud height sequence is as follows: Figure 4 As shown, the root mean square error between it and the actual water depth is 0.8566 meters.

[0102] The cBathy bathymetry method was published in 2013 in the Journal of Geophysical Research: Oceans, Volume 118, pp. 2595-2609, with the title: cBathy: A robust algorithm for estimating nearshore bathymetry.

[0103] Step S9: Merge the land topographic map and the seabed topographic map into a coastal zone topographic map.

[0104] Since the two topographic maps use the same world coordinate system W, they can be merged based on their coordinates in the world coordinate system W to obtain the final coastal topographic map.

[0105] The final topographic map is as follows Figure 5 As shown, Figure 5 The lighter-colored areas represent land areas, and the darker-colored areas represent ocean areas. The darker the point, the lower its elevation. Figure 6 For the reason Figure 5 The resulting topographic contour map shows a trend of gradually decreasing elevation from land to sea, which is consistent with the actual trend of coastal topography.

[0106] Example 2

[0107] This embodiment continues to disclose a specific implementation process of a method for water depth inversion and coastal topographic mapping based on lidar. The data acquisition location in this embodiment is the same as in Embodiment 1, with only some steps differing. This embodiment includes the following steps:

[0108] Step S1: Calibrate the extrinsic parameters between the LiDAR and IMU, and between the camera and IMU, to achieve spatial synchronization between the sensors. The extrinsic parameter matrix between the LiDAR and IMU is as follows: in It is the rotation transformation matrix of the coordinate axes in the IMU coordinate system relative to the LiDAR coordinate system. This represents the position of the origin of the IMU coordinate system in the LiDAR coordinate system. The extrinsic parameter matrix between the camera and the IMU is... in It is the rotation transformation matrix of the coordinate axes in the IMU coordinate system relative to the camera coordinate system C. It is the position of the origin of the IMU coordinate system in the camera coordinate system C.

[0109] Steps S2, S3, S4, S5, S6, and S7 are the same as steps S2, S3, S4, S5, S6, and S7 in Example 1.

[0110] Step S8: Extract the image of the target sea area from the ocean area scan data. Perform orthorectification on the optical image data based on the optimized state to obtain an orthorectified image. Convert the orthorectified image into time stack data (dataC), convert the time stack data into water depth information, and represent the water depth information as a point cloud to obtain a seabed topographic map.

[0111] In this embodiment, a seabed topographic map is constructed using images of the sea area to be measured captured by a camera. The main steps are as follows:

[0112] S81. Based on the camera imaging model, the coordinates of pixels in the image of the sea area to be measured in the world coordinate system W can be calculated. Let t e The coordinates of a point in the world coordinate system W at time 1 are: The coordinates in camera coordinate system C are: The pixel coordinates projected onto the image are The camera imaging model is as follows: in and It is t e Moment and

[0113] S82. Define a rectangular area in the world coordinate system W. The rectangular area has the same coordinates as the rectangular area defined in step S82 of Example 1 in the world coordinate system W. Collect three-dimensional spatial points evenly. According to the camera imaging model, the sea area image pixel coordinates corresponding to each point in the rectangular area can be obtained. Perform bilinear interpolation on the pixel coordinates and rearrange them to form a new image. This image is the orthophoto. The sampling point interval should not exceed 1 meter during sampling. Otherwise, the orthophoto will lose information and affect the accuracy of water depth estimation.

[0114] S82. Convert the orthophoto into time stack data dataC. Assume there are a total of dataCN orthophotos, with the length and width of the orthophotos being cols and rows respectively. Store the value of each row of pixels in the orthophoto into the array dataC, with the size of dataC being dataCN rows * cols * rows. Converting the orthophoto into time stack data dataC is beneficial for frequency analysis.

[0115] S83. Using the cBathy water depth estimation algorithm, the wave frequency and wave number are estimated based on the time stack data dataC. The water depth of each point within the rectangular area is calculated according to the water wave formula. The water depth plus the tidal value is used as the Z-axis coordinate of each point within the rectangular area to obtain the point cloud in the world coordinate system W. This point cloud is a seabed topographic map. The tidal value is the distance from the sea level to the world elevation origin, which is obtained from lidar data.

[0116] Water depth obtained from orthophotos using the cBathy depth estimation algorithm is as follows: Figure 7 As shown, the root mean square error between the depth obtained and the actual water depth is 1.5674 meters. Compared to the water depth obtained from point cloud high-order sequence inversion, the water depth obtained from orthophotos has a larger error.

[0117] Step S9 is the same as step S9 in Example 1.

[0118] In summary, this invention enables rapid mapping of both land and marine coastal areas using only one set of equipment. Based on error Kalman filtering and factor map optimization, this invention estimates the state of the mapping instrument, achieving high accuracy even in large-scale, long-term mapping operations. Furthermore, this invention uses a high-order point cloud sequence for water depth inversion, yielding more accurate water depth estimates and seabed topographic maps compared to water depth inversion based on orthophotos.

[0119] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0120] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A method for water depth inversion and coastal topographic mapping based on lidar, applied to a mapping instrument mounted on a small unmanned helicopter, the mapping instrument comprising: The mapping instrument comprises a lidar, a camera, an inertial measurement unit (IMU) consisting of a velocimeter and a gyroscope, and a global positioning module. Data collected by the sensors, including the lidar, camera, IMU, and GNSS, are all timestamped based on the same time reference. The lidar is used to collect three-dimensional terrain data, the camera is used to collect color images, the IMU is used to collect real-time acceleration, the gyroscope is used to collect real-time angular velocity, and the GNSS is used to collect real-time latitude, longitude, altitude, heading angle, and time data. The mapping method includes the following steps: S1. Achieve spatial synchronization between the sensors on the surveying instrument, calibrate the extrinsic parameters between the lidar and the IMU, and realize spatial synchronization between the sensors; wherein, the extrinsic parameter matrix of the lidar and the IMU is as follows: ,in The coordinate axes in the IMU coordinate system are relative to the lidar coordinate system. The rotation transformation matrix of r, The origin of the IMU coordinate system is in the lidar coordinate system. The lower position; S2. Control the drone to hover first, and then move it above the area to be measured to scan the area to be measured, and obtain land area scan data and ocean area scan data respectively; S3. Obtain land area lidar data from land area scanning data. The land area lidar data is composed of frames. Based on IMU data containing timestamps of the land area lidar data, recursively calculate the pre-integration state of the mapping instrument. The process is as follows: S31. Assume that the coordinate system B of the surveying instrument is equivalent to the IMU coordinate system. Define the world coordinate system W as the northeast-east coordinate system. The three axes of the world coordinate system W point as follows: the X-axis points to the north axis, the Y-axis points to the east axis, and the Z-axis is perpendicular to the ground and downwards. S32. Define the rotation matrix of the surveying instrument coordinate system B relative to the world coordinate system W as follows: Location is Define the velocity of the surveying instrument in the world coordinate system W as Angular velocity with zero bias Acceleration with zero bias Define the state of the surveying instrument ,definition The derivative is , For zero bias noise of angular velocity, define The derivative is , For zero bias noise in acceleration, the symbol for the pre-integral state is defined as... , , , , , These are the recursive results of the surveying instrument status. , , , , Define the state symbol after filtering as , , , , , These are the error Kalman filters, respectively. , , , , Define the optimized state symbol as , , , , , These are the factor graphs after optimization. , , , , ; S33. Obtain each frame of LiDAR data and define the start time of each frame of LiDAR data as... The end time is , to obtain Time's up IMU data at any given time The state at time t is the optimized state. ; S34, Let's assume... Time's up There are two adjacent frames of IMU data between the time points, and the timestamps correspond to the time points. and ,and , The acceleration measurement value of the IMU at time t is The measured angular velocity value is , The acceleration measurement value at time t is The measured angular velocity value is The recursive formula is as follows: ， In the above formula It is a time interval. and These are the mean values ​​of acceleration and angular velocity. yes The length of the mold, yes antisymmetric matrix, , , , , yes Moment , , , , , , , , , yes Moment , , , , , Let W be the acceleration in the world coordinate system W, and let W be the covariance matrix of the error state during the recursive process. The update formula is: ,in, and yes and Covariance matrix at time step , and The coefficient matrix, Here is the noise covariance matrix; S35. According to the recursive formula, The update formula and [ IMU data, recursive get recursion get , and for Moment and , and for Moment and This yields the propagation state of each IMU data point at the corresponding time point during the recursive process. As a pre-integration state; S4. Transform each point of the land area lidar data into the mapping instrument coordinate system B at the time of the last point of the land area lidar data using the pre-integration state. The time of the last point is referred to as the end acquisition time, and the distortion-free point cloud is obtained. S5. Perform error Kalman filtering on the distortion-free point cloud to obtain the filtered state; S6. Calculate the relative pose between the filtered state and the optimized state of the previous frame. Add the relative pose as a laser inertial odometry factor to the factor graph. Obtain GNSS data from the land area scanning data, add a priori position factor based on the GNSS data, and optimize the factor graph to obtain the optimized state. S7. Based on the optimized state, convert the land area lidar data to the world coordinate system W and construct a land topographic map; S8. Extract the marine area lidar data from the marine area scanning data, preprocess the marine area lidar data based on the optimized state to obtain the point cloud height program column, convert the point cloud height program column into water depth information, represent the water depth information in the form of point cloud, and obtain the seabed topography map. S9. Merge the land topographic map and the seabed topographic map into a coastal zone topographic map.

2. The method for water depth inversion and coastal topographic mapping based on lidar according to claim 1, characterized in that, In step S4, each point of the land area lidar data is transformed to the desired value based on the propagation state of each IMU data at the corresponding time. At time B, in the coordinate system B of the mapping instrument, the distortion-free point cloud is obtained.

3. The method for water depth inversion and coastal topographic mapping based on lidar according to claim 1, characterized in that, The process of step S5 is as follows: S51, according to Pre-integral state at time 1 Transform each point of the land area lidar data from the mapping instrument coordinate system B to the world coordinate system W; S52. Suppose that the current frame of land area lidar data has a total of n points, and the coordinates of the k-th point in the world coordinate system W are: , These are the coordinates along the X, Y, and Z axes in the world coordinate system W, and their coordinates in the surveying instrument coordinate system B are... Calculate the coordinates of the voxel corresponding to the calculated point in the voxel map. , Let X, Y, and Z be the coordinates of the voxel corresponding to the k-th point in the world coordinate system W along the X, Y, and Z axes, respectively. = , = , = A voxel is a cube. , , These are the side lengths of a voxel in the X, Y, and Z axes of the world coordinate system W. A voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frames of land area lidar data. This point cloud is referred to as the voxel's internal point cloud. If the coordinates... The corresponding voxel interior point cloud can fit a plane. Substitute the point into the plane equation of the fitted plane of the voxel interior point cloud, and calculate the distance from the point to the plane. Calculate the Jacobian matrix corresponding to the k-th point. ,in yes At present , For matrix transpose, It is the normal vector of the fitting plane of the voxel interior point cloud corresponding to the k-th point, denoted by "". "" indicates matrix multiplication; if the point cloud inside a voxel cannot fit a plane or distance If the distance is greater than 0.1 meters, then abandon the k-th point; S53. Perform step S52 on all points, Matrix merging yields ,Will Merge, and obtain Calculate Kalman gain ,in The noise covariance matrix for lidar point cloud measurements is given. The superscript "-1" indicates that the matrix is ​​inverted. The state is then updated. , where the symbol " "" indicates that the state variables are added together to update the covariance matrix. ,in, Representing the identity matrix, the resulting state This is the filtered state.

4. The method for water depth inversion and coastal topographic mapping based on lidar according to claim 3, characterized in that, The process of step S6 is as follows: S61, will include The GNSS data at time t is converted to the world coordinate system W, denoted as . Calculate using linear fitting. GNSS data at time ,when If the distance to the prior position factor from the last addition to the factor map is greater than 5 meters or the attitude change is greater than 0.3 rad, then... Added to the factor graph as a priori location factor node; S62. Filter the state. The relative pose to the previously optimized state is added to the factor map as a laser inertial odometry factor, and factor map optimization is performed to obtain... Optimized state at any moment .

5. The method for water depth inversion and coastal topographic mapping based on lidar according to claim 4, characterized in that, The process of step S7 is as follows: according to The state after optimization at any time as well as Transform all points in the current frame of land area lidar data to the world coordinate system W, where, and for Moment and Register the current frame of land area lidar data into the voxel map to obtain the data from the start of the survey to... A topographical map of the land at any given time.

6. The method for water depth inversion and coastal topographic mapping based on lidar according to claim 5, characterized in that, The process of step S8 is as follows: S81. Extract the marine area lidar data from the marine area scanning data. Assume the marine area lidar data has a total of m points, and the coordinates of the s-th point in the world coordinate system W are... The coordinates in the coordinate system B of the surveying instrument are ,according to The state after optimization at any time as well as Transform all points in the marine area lidar data to the world coordinate system W to obtain sea surface point cloud data. S82. Define a rectangular region in the world coordinate system W. The two sides of the rectangular region are parallel to the direction along the coastline and the direction across the coastline, respectively. Rasterize the sea surface point cloud data in the rectangular region. The grid is a square. The side length of the square is determined according to the inversion resolution. For each grid, extract all radar points in the grid and calculate their average elevation as the elevation of the grid. Here, the elevation refers to the Z-axis coordinate of the radar point in the world coordinate system W. Obtain a frame of point cloud elevation data. S83. Repeat step S82 for each frame of sea surface point cloud data to obtain point cloud elevation program sequence. Convert the point cloud elevation program sequence into time stack data data. Assume that there are a total of μ frames of point cloud elevation program sequence and β elevation data in each frame of point cloud elevation program sequence. Then the size of data is μ rows and β columns. Place the Lth frame of point cloud elevation program sequence into the Lth row of data. S84. Using the cBathy depth estimation algorithm, estimate the wave frequency and wave number based on the time stack data. Calculate the water depth of each grid cell according to the water wave formula. Use the water depth plus the tidal value as the Z-axis coordinate of each grid cell to obtain a point cloud in the world coordinate system W. This point cloud is a seabed topographic map, where the tidal value is the distance from the sea level to the world elevation origin, obtained from lidar data.

Images