Water depth inversion and coastal zone topographic mapping method based on laser radar

Abstract

The invention discloses a water depth inversion and coastal zone topographic surveying and mapping method based on a laser radar, and the method is applied to a surveying and mapping instrument mounted on a small unmanned helicopter, and achieves the quick and accurate measurement of the coastal zone topography. The surveying instrument comprises the following instruments: a laser radar, a camera, an inertial measurement unit and a global positioning unit. The method comprises the following steps: completing internal and external parameter calibration of each sensor on a surveying instrument; using the unmanned aerial vehicle to mount the surveying instrument to the to-be-measured area, and moving to obtain topographic data of the to-be-measured area; preliminarily constructing an odometer by using Kalman filtering, and constructing a land topographic map; the method comprises the following steps: recording motion characteristics of sea waves by using a laser radar point cloud elevation, obtaining a laser radar point cloud elevation program sequence, obtaining the water depth of an offshore sea area through a cBathy water depth estimation algorithm, and converting the water depth into a sea area topographic map; and combining the two topographic maps into a complete coastal zone topographic map based on the geographic coordinates.

Description

Technical Field

[0001] The present invention belongs to the technical field of topographic surveying and mapping, and in particular relates to a method for water depth inversion and coastal topographic mapping based on laser radar. Background Art

[0002] An airborne mapping system is an all-in-one solution that integrates mapping sensors (such as lidar and cameras), a high-precision inertial measurement unit (IMU), a high-precision satellite positioning module, a long-distance data transmission module, and a ground-based data post-processing system. In recent years, with the increasing maturity of autonomous drone technology, the use of small autonomous drones equipped with airborne mapping systems for mapping has become increasingly common. Compared to traditional total stations and manual field surveying solutions, airborne mapping systems effectively reduce mapping cycles and costs, and can also effectively map inaccessible areas.

[0003] LiDAR can obtain precise distance and intensity information from the environment, making it suitable for airborne terrain mapping. However, this requires the construction of a high-precision LiDAR odometry system. Currently, there are three main types of LiDAR odometry systems: pure radar models, loosely coupled models, and tightly coupled models. Pure radar models accumulate errors over long periods of operation. Without other sensors, these models are difficult to correct, which can affect subsequent mapping. Loosely coupled models incorporate an IMU to assist with pose correction, but these models use LiDAR and IMU data separately without constructing a new loss function, resulting in lower accuracy. Tightly coupled models, on the other hand, integrate multiple sensors into a single model, establish constraints, and use optimization methods to minimize these constraints. This offers a higher accuracy option. Currently, there are two main approaches to tightly coupled models: filtering-based methods and nonlinear optimization-based methods. Filtering-based methods use Kalman filtering as a basis to obtain optimal estimates, offering high accuracy but prone to drift over long periods of state estimation. Nonlinear optimization-based methods primarily employ graph optimization, using sensor data as factors for factor graph optimization. However, if the sensor observation data is not processed accurately, the factor graph optimization results will also be inaccurate.

[0004] Using airborne cameras for seafloor mapping can quickly and efficiently estimate water depth and construct seafloor topography. However, image-based inversion is susceptible to lighting effects and is prone to errors. Therefore, there is an urgent need for a high-precision mapping method that utilizes drones and multi-sensor data processing technology. Summary of the Invention

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

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] A laser radar-based water depth inversion and coastal topography mapping method is applied to a surveying instrument mounted on a small unmanned helicopter. The surveying instrument includes: a laser radar, a camera, an inertial measurement unit consisting of a speedometer and a gyroscope, and a global positioning module. The laser radar is used to collect three-dimensional terrain data, the camera is used to collect color images, the inertial measurement unit is hereinafter referred to as IMU, the accelerometer is used to collect real-time acceleration, the gyroscope is used to collect real-time angular velocity, and the global positioning module is hereinafter referred to as GNSS, which is used to collect real-time latitude and longitude, 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 IMU, and achieve spatial synchronization between the sensors;

[0009] S2. Control the drone to hover first and then move over the area to scan the area to obtain land area scanning data and ocean area scanning data respectively;

[0010] S3. Obtain land area lidar data from the land area scan data, where the land area lidar data is composed of frames, and recursively infer a surveyor state based on IMU data containing a timestamp of the land area lidar data to obtain a pre-integration state;

[0011] S4. Using a pre-integration state, transform each point of the land area LiDAR data into the surveyor coordinate system B at the time of the last point of the land area LiDAR data. The time of the last point is hereinafter referred to as the final acquisition time, to obtain a dedistorted point cloud.

[0012] S5. Perform error Kalman filtering on the dedistorted point cloud to obtain a 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 scan data, add a priori position factors based on the GNSS data, and perform factor graph optimization to obtain the optimized state;

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

[0015] S8. Extracting ocean area lidar data from the ocean area scan data, preprocessing the ocean area lidar data based on the optimized state to obtain a point cloud elevation sequence, converting the point cloud elevation sequence into water depth information, and representing the water depth information in the form of a point cloud to obtain a seabed topography map;

[0016] S9. Combine the land topography map and the seabed topography map into a coastal topography map.

[0017] Furthermore, the data collected by each sensor in the surveying instrument, including the lidar, camera, IMU, and GNSS, is timestamped based on the same time base. This time base clarifies the correspondence between the data collected by each sensor and is a prerequisite for state estimation.

[0018] Furthermore, in step S1, the external parameter matrix of the laser radar and IMU is in It is the rotation transformation matrix of the coordinate axis in the IMU coordinate system relative to the Lidar coordinate system. The extrinsic matrix of the LiDAR and IMU is used to transform the coordinates between the LiDAR and IMU coordinate systems, facilitating the fusion of LiDAR and IMU data for land areas.

[0019] Furthermore, the process of step S3 is as follows:

[0020] S31. Assume that the surveyor coordinate system B is equal to the IMU coordinate system, and define the world coordinate system W as the north-east coordinate system. The three axes of the world coordinate system W are 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 downward;

[0021] S32. Define the rotation matrix of the surveyor coordinate system B relative to the world coordinate system W as Location is Define the velocity of the surveyor in the world coordinate system W as v and the angular velocity zero bias as b g , acceleration zero bias is b a , define the state of the surveyor x = Definition b g The derivative of n bg , n bg is the zero bias noise of angular velocity, and b is defined a The derivative of n ba , n ba is the zero-bias noise of acceleration, and the symbol of the pre-integration state is defined as After recursion on the surveying instrument status, v、b g 、b a , define the state symbol after filtering as After error Kalman filtering, v、b g 、b a , define the optimized state symbol as After factor graph optimization, v、b g 、b a ;

[0022] S33, obtain each frame of laser radar data, and define the start time of each frame of laser radar data as t s , the end time is t e , get the value containing t s Time to t e IMU data at time t s The state at this moment is the optimized state

[0023] S34, set at t s Time to t e There are two adjacent frames of IMU data between the timestamps, and the time corresponding to the timestamp is t i and t j , and t i <t j , t i The acceleration measured by the IMU at this moment is The angular velocity measurement is t j The acceleration measured at the moment is The angular velocity measurement is The recursive formula is as follows:

[0024]

[0025] In the above formula, dt is the time interval, a avg and ω avg is the mean acceleration and mean angular velocity, ‖ω avg ‖ is ω avg The module length, yes The antisymmetric matrix of It is t i Moment It is t j Moment a_v wis the acceleration in the world coordinate system W, and the covariance matrix of the error state in the recursive process is The update formula is: in, and It is t i and t j The covariance matrix at time and F w is the coefficient matrix, Q is the noise covariance matrix;

[0026] S35, according to the recursive formula, The update formula and [t s ,t e ] between the IMU data, recursion get Recursion get and t s Moment and and t e Moment and Obtain the propagation state of each IMU data at the corresponding moment in the recursive process, and As a pre-integration state.

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

[0028] Furthermore, in step S4, each point of the land area lidar data is transformed to t according to the propagation state of each IMU data at the corresponding moment. e The dedistorted point cloud is obtained in the surveyor coordinate system B at the moment.

[0029] The present invention uses the timestamp of the final acquisition moment of each frame of land area LiDAR data as the timestamp of a frame of land area LiDAR data during data processing. However, during the acquisition process of a frame of land area LiDAR data, the LiDAR is not completely stationary. Therefore, each point of a frame of land area LiDAR data is in the LiDAR coordinate system at the acquisition moment of the point. If it is assumed that they are all in the LiDAR coordinate system at the final acquisition moment, a distorted point cloud will be obtained. A distorted point cloud will lead to an erroneous state estimation result. Each point of the land area LiDAR data is transformed to t e The dedistorted point cloud is obtained in the surveyor coordinate system B at the moment, which is beneficial to improving the accuracy of state estimation.

[0030] Furthermore, the process of step S5 is as follows:

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

[0032] S52. Assume that there are n points in the land area laser radar data of the current frame, and the coordinates of the kth point in the world coordinate system W are are the coordinate values ​​in the X-axis, Y-axis and Z-axis directions of the world coordinate system W, and the coordinates in the surveyor coordinate system B are Calculate the coordinates of the voxel corresponding to the point in the voxel map are the coordinate values ​​of the voxel corresponding to the kth point in the X-axis, Y-axis, and Z-axis directions of the world coordinate system W, respectively. A voxel is a cube, L x 、L y 、L z It is the side length of the voxel in the X-axis, Y-axis and Z-axis directions under the world coordinate system W. The voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frame land area lidar data. The point cloud is hereinafter referred to as the voxel internal point cloud. If the coordinates The corresponding voxel internal point cloud can fit the plane, substitute the point into the plane equation of the voxel internal point cloud fitting plane, and calculate the distance z from the point to the plane k , calculate the Jacobian matrix corresponding to the kth point in It is t e At the moment is a matrix The transpose of u kis the normal vector of the plane fitted to the point cloud inside the voxel corresponding to the kth point. The symbol “*” represents matrix multiplication. If the point cloud inside the voxel cannot fit the plane or the distance z k If it is greater than 0.1 meters, the kth point is abandoned;

[0033] Each voxel in a voxel map maintains a plane. When calculating the point-to-plane distance for each point in land LiDAR data, the distance between the point and the plane fitted within the voxel's point cloud is calculated, rather than fitting a plane for each point. This improves computational efficiency.

[0034] S53, perform the operation of step S52 on all points, and set H k Matrices are merged to obtain z k Merge to get Calculate Kalman gain Where γ is the noise covariance matrix of the lidar point cloud measurement, and the superscript “-1” indicates the inversion of the matrix and the update state: Among them, the symbol Indicates the addition of state quantities and updates the covariance matrix Among them, I represents the identity matrix, and the obtained state is the state after filtering.

[0035] The distance from point to plane calculated based on the pre-integration state is bound to have errors. The errors mainly come from two aspects: one is the noise of the lidar data in the land area, and the other is the error between the pre-integration state and the actual state. k The probability of being equal to the actual distance is By maximizing The state and covariance update equations of the error Kalman filter can be obtained.

[0036] Furthermore, the process of step S6 is as follows:

[0037] S61, will include t e The GNSS data at the moment is converted to the world coordinate system W, which is recorded as Calculate t using linear fitting e GNSS data at the moment when When the distance from the prior position factor added to the factor graph last time is greater than 5 meters or the posture change is greater than 0.3rad, Added to the factor graph as a priori position factor node;

[0038] S62, the filtered state The relative position of the state after the last optimization is added as the laser inertial odometry factor to the factor graph, and the factor graph is optimized to obtain te The optimized state at the moment

[0039] Error Kalman filtering only imposes relative constraints on states, without global constraints. Therefore, the filtered states still contain errors. When surveying and mapping instruments perform large-scale, long-term surveying operations, errors accumulate, significantly reducing estimation accuracy. Factor graph optimization with GNSS data can add global constraints and reduce accumulated errors.

[0040] Furthermore, the process of step S7 is as follows:

[0041] According to t e The optimized state at all times as well as Transform all points in the current frame land area lidar data to the world coordinate system W, where: and t e Moment and Register the current frame land area lidar data to the voxel map and get the time from the start of mapping to t e Land topographic map at the moment.

[0042] The state after Kalman filtering and factor graph optimization This is the optimal estimate of the surveyor's state. Registering land area LiDAR data with the voxel map using this optimized state can produce a high-precision land topographic map.

[0043] Furthermore, the process of step S8 is as follows:

[0044] S81. Extract ocean area lidar data from ocean area scanning data. Suppose there are m points in the ocean area lidar data. The coordinates of the sth point in the world coordinate system W are The coordinates in the surveyor coordinate system B are According to t e The optimized state at all times as well as Transform all points in the ocean area lidar data to the world coordinate system W to obtain the sea surface point cloud data;

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

[0046] S83. Repeat step S82 for each frame of sea surface point cloud data to obtain a point cloud elevation sequence, and convert the point cloud elevation sequence into time stack data data. Suppose the point cloud elevation sequence has a total of μ frames, and each frame of the point cloud elevation sequence has β elevation data. Then the size of data is μ rows and β columns. Put the Lth frame of the point cloud elevation sequence into the Lth row of data. The 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. Use the cBathy water depth estimation algorithm to estimate the wave frequency and wave number based on the time stack data data, calculate the water depth of each grid according to the water wave formula, and use the water depth plus the tide value as the Z-axis coordinate of each grid to obtain a point cloud under the world coordinate system W. The point cloud is a seabed topographic map, where the tide value is the distance from sea level to the world elevation origin, which is obtained from the lidar data.

[0048] The cBathy water depth estimation algorithm was published in 2013 in the Journal of Geophysical Research: Oceans, Volume 118, Pages 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. The present invention utilizes a multi-sensor surveyor for coastal topography mapping, enabling the mapping of both land and marine areas using a single set of equipment. Existing methods require separate equipment for mapping land and marine areas. Compared to existing methods, the present invention's mapping method is more convenient.

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

[0052] 3. The present invention uses an error Kalman filter to estimate the state of the surveying instrument. Compared with the extended Kalman filter, the error Kalman filter does not require the linearization of the Jacobian matrix, effectively avoiding the errors caused by linearization and having higher computational efficiency and accuracy.

[0053] 4. This invention uses factor graphs to further optimize the state after the error Kalman filter. By integrating prior GNSS pose data, the surveying and mapping method can maintain high-precision state estimation over large-scale and long-term surveying and mapping operations. Furthermore, factor graphs facilitate the addition of various sensor data, demonstrating excellent scalability.

[0054] 5. The present invention uses a lidar point cloud high-level sequence for water depth inversion. Compared with the water depth inversion method using a camera image sequence, this method can effectively avoid errors caused by environmental factors such as lighting, making the surveying and mapping operation more adaptable to the environment and obtaining more accurate surveying and mapping results. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0056] Figure 1 It is a flow chart of the method for water depth inversion and coastal topography mapping based on laser radar disclosed in the present invention;

[0057] Figure 2 It is a map of the surveying area selected in the implementation example of the method for water depth inversion and coastal topography surveying based on laser radar disclosed in the present invention;

[0058] Figure 3 Schematic diagrams of position trajectories in three cases in the laser radar-based water depth inversion and coastal topography mapping method disclosed in the present invention;

[0059] Figure 4 It is a schematic diagram of the depth results of the depth inversion and coastal topography mapping method based on the laser radar disclosed in the present invention based on the point cloud elevation sequence;

[0060] Figure 5 It is a topographic diagram of the method for depth inversion and coastal topography mapping based on laser radar disclosed in the present invention;

[0061] Figure 6 It is a topographic contour map in the method for water depth inversion and coastal topography mapping based on laser radar disclosed in the present invention;

[0062] Figure 7 It is a schematic diagram of the depth results of the depth inversion and coastal topography mapping method based on laser radar disclosed in the present invention based on optical images. DETAILED DESCRIPTION

[0063] In order to enable those skilled in the art to better understand the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0064] References to "embodiments" in this application mean that a particular feature, structure, or characteristic described in connection with the embodiment may be included in at least one embodiment of the application. The appearance of this phrase in various places in the specification does not necessarily refer to the same embodiment, nor does it constitute an independent or alternative embodiment that is mutually exclusive of other embodiments. It is understood, both explicitly and implicitly, by those skilled in the art that the embodiments described in this application may 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 topography mapping based on laser radar. Figure 1 Detailed introduction to the implementation process:

[0067] Step S1: calibrate the external parameters between the laser radar and the IMU to complete the spatial synchronization between the sensors. The external parameter matrix between the laser radar and the IMU is It is the rotation transformation matrix of the IMU coordinate system axis 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: Control the UAV to fly over the area to be measured, scan the area to be measured, and obtain the land area scanning data and the ocean area scanning data respectively. The data collection location of this embodiment is as follows: Figure 2 shown.

[0069] Step S3: Obtain land area lidar data from the land area scanning data, where the land area lidar data is composed of frames. Recursively infer the surveyor state based on the IMU data containing the timestamp of the land area lidar data to obtain a pre-integration state.

[0070] The operation process of recursively obtaining the pre-integration state in this embodiment is as follows:

[0071] S31. Assume that the surveyor coordinate system B is equal to the IMU coordinate system, and define the world coordinate system W as the north-east coordinate system. The three axes of the world coordinate system W are 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 downward;

[0072] S32. Define the rotation matrix of the surveyor coordinate system B relative to the world coordinate system W as Location is Define the velocity of the surveyor in the world coordinate system W as v and the angular velocity zero bias as b g , acceleration zero bias is b a , define the state of the surveyor x = Definition b g The derivative of n bg , n bg is the zero bias noise of angular velocity, and b is defined a The derivative of n ba , n ba is the zero-bias noise of acceleration, and the symbol of the pre-integration state is defined as After recursion on the surveying instrument status, v、b g 、b a , define the state symbol after filtering as After error Kalman filtering, v、b g 、b a , define the optimized state symbol as After factor graph optimization, v、b g 、b a ;

[0073] S33, obtain each frame of laser radar data, and define the start time of each frame of laser radar data as t s , the end time is t e , get the value containing t s Time to t e IMU data at time t s The state at this moment is the optimized state

[0074] S34, set at t s Time to t e There are two adjacent frames of IMU data between the timestamps, and the time corresponding to the timestamp is t i and t j , and t i <t j , t i The acceleration measured by the IMU at this moment is The angular velocity measurement is t j The acceleration measured at the moment is The angular velocity measurement is The recursive formula is as follows:

[0075]

[0076] In the above formula, dt is the time interval, a avg and ω avg is the mean acceleration and mean angular velocity, ‖ω avg ‖ is ω avg The module length, yes The antisymmetric matrix of It is t i Moment It is t j Moment a_v w is the acceleration in the world coordinate system W, and the covariance matrix of the error state in the recursive process is The update formula is: in, and It is t i and t j The covariance matrix at time and F w is the coefficient matrix, Q is the noise covariance matrix;

[0077] S35, according to the recursive formula, The update formula and [t s ,t e ] between the IMU data, recursion get Recursion get and t s Moment and and t e Moment and Obtain the propagation state of each IMU data at the corresponding moment in the recursive process, and As a pre-integration state.

[0078] Because the noise matrix is ​​not considered during the recursive process, there will be a certain error between the pre-integrated state and the actual state. To obtain a more accurate state, it is necessary to estimate the error using the lidar point cloud data. This estimated error is then added to the pre-integrated state to obtain a more accurate state. Since the error is generally small, higher-order derivatives can be ignored during the calculation, which improves both computation speed and accuracy.

[0079] Step S4: Use the pre-integration state to transform each point in the land area lidar data into the surveyor coordinate system B at the time when the last point of the lidar point cloud is located. The time when the last point is located is hereinafter referred to as the end acquisition time, to obtain a dedistorted point cloud.

[0080] When processing a frame of land area LiDAR data, this embodiment uses the timestamp of the final acquisition moment of the frame as the timestamp of the land area LiDAR data. However, 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 at the acquisition moment of the point. All points in the frame of land area LiDAR data are transformed from the LiDAR coordinate system at the acquisition moment of the point to the surveying instrument coordinate system B corresponding to the final acquisition moment to achieve point cloud motion distortion correction and obtain a dedistorted point cloud.

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

[0082] This step has the following operations:

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

[0084] S52. Assume that there are n points in the land area laser radar data of the current frame, and the coordinates of the kth point in the world coordinate system W are are the coordinate values ​​in the X-axis, Y-axis and Z-axis directions of the world coordinate system W, and the coordinates in the surveyor coordinate system B are Calculate the coordinates of the voxel corresponding to the point in the voxel map are the coordinate values ​​of the voxel corresponding to the kth point in the X-axis, Y-axis, and Z-axis directions of the world coordinate system W, respectively. A voxel is a cube, L x 、L y 、L zIt is the side length of the voxel in the X-axis, Y-axis and Z-axis directions under the world coordinate system W. The voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frame land area lidar data, hereinafter referred to as the voxel internal point cloud. If the coordinates If the corresponding voxel internal point cloud can fit the plane, then substitute the point into the plane equation of the voxel internal point cloud fitting and calculate the distance z from the point to the plane k , calculate the Jacobian matrix corresponding to the kth point in It is t e At the moment is a matrix The transpose of u k is the normal vector of the plane inside the voxel corresponding to the kth point. The symbol “*” represents matrix multiplication. If the point cloud inside the voxel cannot fit the plane or the distance z k If it is greater than 0.1 meters, the kth point is abandoned;

[0085] S53, perform the operation of step S52 on all points of the land area laser radar data of the current frame, and k Matrices are merged to obtain z k Merge to get Calculate Kalman gain Where γ is the noise covariance matrix of the lidar point cloud measurement, and the superscript “-1” indicates the inversion of the matrix and the update state: Among them, the symbol Indicates the addition of state quantities and updates the covariance matrix The status obtained is the state after filtering.

[0086] Here, we 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 dotted line, Figure 3 The medium-thick solid line is the high-precision position data collected by GNSS. The position data collected by GNSS is considered to be the actual position of the surveying instrument when it is working. It can be seen that the dotted line is close to the thick solid line only at the beginning, but deviates from the thick solid line after a period of time.

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

[0088] The error Kalman filter only imposes relative constraints on the state, without global constraints. Therefore, the filtered state still contains errors. When a surveyor performs large-scale, long-term surveying operations, errors accumulate, significantly reducing estimation accuracy. Using factor graphs to fuse GNSS data can add global constraints and reduce accumulated errors.

[0089] Factor graph optimization mainly consists of the following steps:

[0090] S61, will include t e The GNSS data at the moment is converted to the world coordinate system W, which is recorded as Calculate t using linear fitting e GNSS data at the moment when When the distance from the prior pose factor added to the factor graph last time is greater than 5 meters or the pose change is greater than 0.3rad, Added to the factor graph as a priori pose factor node;

[0091] S62, the filtered state The relative position of the state after the last optimization is added as the laser inertial odometry factor to the factor graph, and the factor graph is optimized to obtain t e The optimized state at the moment

[0092] The estimated position corresponding to the optimized state is as follows Figure 3 As shown by the medium thin solid line, we can see that most of the time the thin solid line is very close to the thick solid line, indicating that the optimized state accuracy is high.

[0093] Step S7: Based on the optimized state, the land area LiDAR data is converted to the world coordinate system W to construct a land topographic map. The optimized state is the best estimated state, and based on this state, the land area LiDAR data is converted to the world coordinate system W to obtain a high-precision land topographic map.

[0094] According to t e The optimized state at all times as well as Transform all radar points in the land area lidar data to the world coordinate system W, where: and t e Moment and Register the land area lidar data to the voxel map and get the time from surveying to t e Land topographic map at the moment.

[0095] Step S8: Extract the ocean area lidar data from the ocean area scanning data, pre-process the ocean area lidar data based on the optimized state to obtain a point cloud elevation sequence, convert the point cloud elevation sequence into water depth information, and represent the water depth information in the form of a point cloud to obtain a seabed topographic map.

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

[0097] S81. Extract ocean area lidar data from ocean area scanning data. Suppose there are m points in the ocean area lidar data. The coordinates of the sth point in the world coordinate system W are The coordinates in the surveyor coordinate system B are According to t e The optimized state at all times as well as Transform all points in the ocean area lidar data to the world coordinate system W to obtain the sea surface point cloud data;

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

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

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

[0101] The water depth obtained by using the cBathy water depth estimation algorithm based on the point cloud elevation sequence is as follows Figure 4 The root mean square error between it and the true water depth is 0.8566 meters.

[0102] The cBathy bathymetry method was published in 2013 in the Journal of Geophysical Research: Oceans, Volume 118, Pages 2595–2609. The article is titled “cBathy: A robust algorithm for estimating nearshore bathymetry.”

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

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

[0105] The final topographic map is as follows Figure 5 As shown, Figure 5 The lighter areas are land areas, and the darker areas are ocean areas. The darker the color, the lower the elevation. Figure 6 for the reason Figure 5 The obtained topographic contour map shows that the topographic map has a trend of gradually decreasing height from the land area to the ocean area, which is consistent with the changing trend of the actual coastal topography.

[0106] Example 2

[0107] This embodiment further discloses a specific implementation process of a method for depth inversion and coastal topography mapping based on laser radar. The data collection location of this embodiment is the same as that of Example 1, and only some steps are different. This embodiment includes the following steps:

[0108] Step S1: calibrate the external parameters between the laser radar and the IMU, and the external parameters between the camera and the IMU to complete the spatial synchronization between the sensors. The external parameter matrix between the laser radar and the IMU is in It is the rotation transformation matrix of the coordinate axis in the IMU coordinate system relative to the Lidar coordinate system. is the position of the origin of the coordinate system in the IMU coordinate system under the Lidar coordinate system. The external parameter matrix between the camera and IMU is in is the rotation transformation matrix of the coordinate axis 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 an image of the ocean area to be measured from the ocean area scan data. Based on the optimized state, perform ortho transformation on the optical image data to obtain an orthophoto. The orthophoto is converted into time-stacked data dataC, which is then converted into water depth information. The water depth information is represented as a point cloud to obtain a seabed topographic map.

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

[0112] S81. The coordinates of the pixel points in the image of the sea area to be measured in the world coordinate system W can be calculated according to the camera imaging model. e The coordinates of a point in the world coordinate system W at the moment are The coordinates in the camera coordinate system C are The pixel coordinates projected onto the image are The camera imaging model is: in and It is t e Moment and

[0113] S82. Define a rectangular area in the world coordinate system W. The coordinates of the rectangular area are consistent with those of the rectangular area defined in step S82 in Example 1 in the world coordinate system W. Uniformly collect three-dimensional spatial points. According to the camera imaging model, the pixel coordinates of the sea area image corresponding to each point in the rectangular area can be obtained. Bilinear interpolation is performed on the pixel coordinates, and they are rearranged to form a new image. This image is the orthophoto. The sampling interval between sampling points should not exceed 1 meter. Otherwise, the orthophoto will lose information and affect the accuracy of water depth estimation.

[0114] S82. Convert the orthophoto image into time stack data dataC. Assume that there are a total of dataCN orthophoto images, and the length and width of the orthophoto image are cols and rows respectively. Store the values ​​of each row of pixels in the orthophoto image into the array dataC row by row. The size of dataC is cols rows * rows columns. Converting the orthophoto image into time stack data dataC facilitates frequency analysis.

[0115] S83. Use the cBathy water depth estimation algorithm to estimate the wave frequency and wave number based on the time stack data dataC, calculate the water depth of each point in the rectangular area according to the water wave formula, and use the water depth plus the tide value as the Z-axis coordinate of each point in the rectangular area to obtain a point cloud under the world coordinate system W. The point cloud is a seabed topographic map, where the tide value is the distance from sea level to the world elevation origin, which is obtained from the lidar data.

[0116] The water depth obtained by using the cBathy water depth estimation algorithm based on the orthophoto is as follows Figure 7 The root mean square error between the depth and the true water depth is 1.5674 meters. Compared with the water depth obtained by inversion based on the point cloud elevation sequence, the water depth obtained based on the orthophoto has a larger error.

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

[0118] In summary, the present invention enables rapid mapping of both coastal land and marine areas using only one set of equipment. Based on error Kalman filtering and factor graph optimization, the present invention estimates instrument state, achieving high state estimation accuracy even in large-scale, long-term mapping operations. The present invention uses point cloud high-level sequences for depth inversion, resulting in more accurate depth estimates and seafloor topography compared to depth inversion based on orthophotos.

[0119] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, 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 implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be considered as equivalent replacement methods and are included in the scope of protection of the present invention.

Claims

1. A method for water depth inversion and coastal topography mapping based on laser radar, applied to a mapping instrument mounted on a small unmanned helicopter, the mapping instrument comprising: A laser radar, a camera, an inertial measurement unit consisting of a speedometer and a gyroscope, and a global positioning module, wherein the laser radar is used to collect three-dimensional spatial data of the terrain, the camera is used to collect color images, the inertial measurement unit is hereinafter referred to as IMU, the accelerometer is used to collect real-time acceleration, the gyroscope is used to collect real-time angular velocity, and the global positioning module is hereinafter referred to as GNSS, which is used to collect real-time latitude and longitude, heading angle, and time data. The surveying and mapping method is characterized in that the method comprises the following steps: S1. Complete the spatial synchronization between the sensors on the surveying instrument, calibrate the external parameters between the lidar and IMU, and achieve spatial synchronization between the sensors; S2. Control the drone to hover first and then move over the area to scan the area to obtain land area scanning data and ocean area scanning data respectively; S3. Obtain land area lidar data from the land area scan data, where the land area lidar data is composed of frames, and recursively infer a surveyor state based on IMU data containing a timestamp of the land area lidar data to obtain a pre-integration state; S4. Using a pre-integration state, transform each point of the land area LiDAR data into the surveyor coordinate system B at the time of the last point of the land area LiDAR data. The time of the last point is hereinafter referred to as the final acquisition time, to obtain a dedistorted point cloud. S5. Perform error Kalman filtering on the dedistorted point cloud to obtain a 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 scan data, add a priori position factors based on the GNSS data, and perform factor graph optimization to obtain the optimized state; S7. Based on the optimized state, convert the land area lidar data into the world coordinate system W to construct a land topographic map; S8. Extracting ocean area lidar data from the ocean area scan data, preprocessing the ocean area lidar data based on the optimized state to obtain a point cloud elevation sequence, converting the point cloud elevation sequence into water depth information, and representing the water depth information in the form of a point cloud to obtain a seabed topography map; S9. Combine the land topography map and the seabed topography map into a coastal topography map.

2. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 1, characterized in that: The data collected by sensors in the surveyor, including lidar, camera, IMU and GNSS, are all timestamps based on the same time base.

3. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 2, characterized in that: In step S1, the external parameter matrix of the laser radar and IMU is in It is the rotation transformation matrix of the coordinate axis in the IMU coordinate system relative to the Lidar coordinate system. It is the position of the origin of the coordinate system in the IMU coordinate system in the Lidar coordinate system.

4. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 3, characterized in that: The process of step S3 is as follows: S31. Assume that the surveyor coordinate system B is equal to the IMU coordinate system, and define the world coordinate system W as the north-east coordinate system. The three axes of the world coordinate system W are 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 downward; S32. Define the rotation matrix of the surveyor coordinate system B relative to the world coordinate system W as Location is Define the velocity of the surveyor in the world coordinate system W as v and the angular velocity zero bias as b g , acceleration zero bias is b a , defines the state of the surveyor Definition b g The derivative of n bg , n bg is the zero bias noise of angular velocity, and b is defined a The derivative of n ba , n ba is the zero-bias noise of acceleration, and the symbol of the pre-integration state is defined as After recursion on the surveying instrument status, v、b g 、b a , define the state symbol after filtering as After error Kalman filtering, v、b g 、b a , define the optimized state symbol as After factor graph optimization, v、b g 、b a ; S33, obtain each frame of laser radar data, and define the start time of each frame of laser radar data as t s , the end time is t e , get the value containing t s Time to t e IMU data at time t s The state at this moment is the optimized state S34, set at t s Time to t e There are two adjacent frames of IMU data between the timestamps, and the time corresponding to the timestamp is t i and t j , and t i <t j , t i The acceleration measured by the IMU at this moment is The angular velocity measurement is t j The acceleration measured at the moment is The angular velocity measurement is The recursive formula is as follows: In the above formula, dt is the time interval, a avg and ω avg is the mean acceleration and mean angular velocity, ‖ω avg ‖ is ω avg The module length, yes The antisymmetric matrix of It is t i Moment It is t j Moment a_v w is the acceleration in the world coordinate system W, and the covariance matrix of the error state in the recursive process is The update formula is: in, and It is t i and t j The covariance matrix at time and F w is the coefficient matrix, Q is the noise covariance matrix; S35, according to the recursive formula, The update formula and [t s ,t e ] between the IMU data, recursion get Recursion get and t s Moment and and t e Moment and Obtain the propagation state of each IMU data at the corresponding moment in the recursive process, and As a pre-integration state.

5. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 4, characterized in that: In step S4, each point of the land area lidar data is transformed to t according to the propagation state of each IMU data at the corresponding moment. e The dedistorted point cloud is obtained in the surveyor coordinate system B at the moment.

6. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 5, characterized in that: The process of step S5 is as follows: S51, according to t e Pre-integration state at time Transform each point of the land area lidar data from the surveyor coordinate system B to the world coordinate system W; S52. Assume that there are n points in the land area laser radar data of the current frame, and the coordinates of the kth point in the world coordinate system W are are the coordinate values ​​in the X-axis, Y-axis and Z-axis directions of the world coordinate system W, and the coordinates in the surveyor coordinate system B are Calculate the coordinates of the voxel corresponding to the point in the voxel map are the coordinate values ​​of the voxel corresponding to the kth point in the X-axis, Y-axis, and Z-axis directions of the world coordinate system W, respectively. A voxel is a cube, L x , L y , L z It is the side length of the voxel in the X-axis, Y-axis and Z-axis directions under the world coordinate system W. The voxel map is a map composed of voxels. Each voxel in the voxel map stores a point cloud composed of historical frame land area lidar data. The point cloud is hereinafter referred to as the voxel internal point cloud. If the coordinates The corresponding voxel internal point cloud can fit the plane, substitute the point into the plane equation of the voxel internal point cloud fitting plane, and calculate the distance z from the point to the plane k , calculate the Jacobian matrix corresponding to the kth point in It is t e At the moment is a matrix The transpose of u k is the normal vector of the plane fitted to the point cloud inside the voxel corresponding to the kth point. The symbol "*" represents matrix multiplication. If the point cloud inside the voxel cannot fit the plane or the distance z k If it is greater than 0.1 meters, the kth point is abandoned; S53, perform the operation of step S52 on all points, and set H k Merge the matrices to get z k Merge to get Calculate Kalman gain Where γ is the noise covariance matrix of the lidar point cloud measurement, and the superscript "-1" indicates the inversion of the matrix and the update state: Among them, the symbol Indicates the addition of state quantities and updates the covariance matrix Among them, I represents the identity matrix, and the obtained state is the state after filtering.

7. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 6, characterized in that: The process of step S6 is as follows: S61, will include t e The GNSS data at the moment is converted to the world coordinate system W, which is recorded as Calculate t using linear fitting e GNSS data at the moment when When the distance from the prior position factor added to the factor graph last time is greater than 5 meters or the posture change is greater than 0.3rad, Added to the factor graph as a priori position factor node; S62, the filtered state The relative position of the state after the last optimization is added as the laser inertial odometry factor to the factor graph, and the factor graph is optimized to obtain t e The optimized state at the moment 8. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 7, characterized in that: The process of step S7 is as follows: According to t e The optimized state at all times as well as Transform all points in the current frame land area lidar data to the world coordinate system W, where: and t e Moment and Register the current frame land area lidar data to the voxel map and get the time from the start of mapping to t e Land topographic map at the moment.

9. The method for water depth inversion and coastal topography mapping based on laser radar according to claim 8, characterized in that: The process of step S8 is as follows: S81. Extract ocean area lidar data from ocean area scanning data. Suppose there are m points in the ocean area lidar data. The coordinates of the sth point in the world coordinate system W are The coordinates in the surveyor coordinate system B are According to t e The optimized state at all times as well as Transform all points in the ocean area lidar data to the world coordinate system W to obtain the sea surface point cloud data; S82. Define a rectangular area in the world coordinate system W, where two sides of the rectangular area are parallel to the along-coastline direction and the across-coastline direction, respectively. Rasterize the sea surface point cloud data within the rectangular area, where the grid is a square with a side length determined according to the inversion resolution. For each grid, extract all radar points within the grid and calculate their mean elevation as the grid elevation. The elevation here refers to the Z-axis coordinate of the radar point in the world coordinate system W, thereby obtaining one frame of point cloud elevation data. S83. Repeat step S82 for each frame of sea surface point cloud data to obtain a point cloud elevation sequence. Convert the point cloud elevation sequence into time stack data data. Assume that the point cloud elevation sequence has a total of μ frames, and each frame of the point cloud elevation sequence has β elevation data. The size of data is μ rows and β columns. Place the Lth frame of the point cloud elevation sequence in the Lth row of data. S84. Use the cBathy water depth estimation algorithm to estimate the wave frequency and wave number based on the time stack data data, calculate the water depth of each grid according to the water wave formula, and use the water depth plus the tide value as the Z-axis coordinate of each grid to obtain a point cloud under the world coordinate system W. The point cloud is a seabed topographic map, where the tide value is the distance from sea level to the world elevation origin, which is obtained from the lidar data.

Images