CN111457902B - Water area measuring method and system based on laser SLAM positioning - Google Patents
Water area measuring method and system based on laser SLAM positioning Download PDFInfo
- Publication number
- CN111457902B CN111457902B CN202010278863.0A CN202010278863A CN111457902B CN 111457902 B CN111457902 B CN 111457902B CN 202010278863 A CN202010278863 A CN 202010278863A CN 111457902 B CN111457902 B CN 111457902B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- point
- water area
- gps
- module
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C13/00—Surveying specially adapted to open water, e.g. sea, lake, river or canal
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Hydrology & Water Resources (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention discloses a water area measuring method and system based on laser SLAM positioning, and belongs to the technical field of surveying and mapping. The method comprises the following steps: measuring a water area by using an unmanned ship carrying a laser radar, an inertial measurement unit IMU and a GPS receiver, and collecting data; carrying out static base initial alignment by utilizing the acquired IMU and GPS data, and providing initial absolute position and attitude information for subsequent inter-travelling laser SLAM resolving; performing point cloud preprocessing operation on data acquired by a laser radar according to a water area measuring environment; estimating attitude transformation between two adjacent frames of point clouds by utilizing the IMU (inertial measurement Unit), and providing an initial value for point cloud registration; registering the point cloud by using a PPICP algorithm; the rear-end graph is optimized and combined with navigation pose information as additional constraint so as to improve the solving precision; and resolving a point cloud three-dimensional coordinate and drawing a three-dimensional point cloud map according to a rear-end optimization result, and marking absolute position information of a water area boundary. The invention can provide accurate reference for the integral protection and comprehensive treatment of the water area.
Description
Technical Field
The invention belongs to the technical field of surveying and mapping, and particularly relates to a water area measuring method and system.
Background
The small water areas such as rural fishponds, small lakes and the like have the characteristics of irregular shapes and complex surrounding environment, and the positioning and surveying work of the boundaries of the small water areas is a hotspot researched by researchers in the industry. Common surveying and mapping methods for small water areas and other real estate units currently include GPS measurement, total station measurement, satellite remote sensing, aerial photography and the like. These current methods have more or less corresponding drawbacks: firstly, when GPS measurement is carried out, each RTK needs at least 4 satellites to be visible to obtain a fixed solution, however, factors such as a large number of trees covering the periphery of small water areas or weather conditions and the like exist, the number of observable satellites is lower than four, and at the moment, a GPS receiver cannot obtain a good fixed solution and cannot meet the requirement of real estate on-site measurement. The total station is a measuring instrument integrating light collection, mechanical integration and electrical integration, and the total station is set before being used, namely lofting is carried out on the basis of a known coordinate system, and then measuring is carried out. In the problem of boundary positioning of a small water area, the total station can effectively perform expansion measurement on specific points on the boundary, but if the boundary outline of the small water area is measured accurately, the number of points to be measured is large, the station setting process is complicated, and the measurement efficiency is greatly reduced by using the total station. The satellite remote sensing technology is a common technical scheme for positioning water area boundaries, and many scholars are dedicated to extraction research of the water area of remote sensing images, but the spatial resolution is low, so that the satellite remote sensing technology is more suitable for water areas with larger areas. The aerial photography technology can arrange a special aerial photographic instrument on an aircraft, photograph a ground or aerial target from the air, and draw a map, but the aerial photography is similar to the GPS-RTK technology, is easily influenced by tree coverage or weather factors, and has poor mapping tendency.
Disclosure of Invention
The purpose of the invention is as follows: in order to solve the problems in the prior art, the invention provides a water area measuring method and system based on laser SLAM (simultaneous Localization and mapping) positioning, and the absolute position information of the boundary of a small water area is accurately obtained in an efficient and simple manner.
The technical scheme is as follows: in a first aspect, a water area measurement method based on laser SLAM positioning is provided, which comprises the following steps:
s1, measuring the water area by using an unmanned ship provided with a laser radar, an inertial measurement unit IMU and a GPS receiver, and collecting data;
s2, carrying out static base initial alignment by utilizing the acquired IMU and GPS data, and providing initial absolute position and attitude information for subsequent inter-travel laser SLAM resolving;
s3, according to the water area measuring environment, point cloud preprocessing operation is carried out on the data collected by the laser radar, and the point cloud preprocessing operation comprises invalid point elimination, outlier elimination, down-sampling filtering and point cloud segmentation simplification;
s4, estimating the attitude transformation between two adjacent frames of point clouds by utilizing the IMU, and providing an initial attitude transformation value for point cloud registration;
s5, registering the preprocessed point cloud by using a PPICP algorithm according to the position transformation initial value directly measured by the GPS and the attitude transformation initial value obtained in the step S4;
s6, in the effective area of GPS, adding the combined navigation position and attitude information as additional constraint on the basis of the conventional back-end graph optimization method, and carrying out graph optimization solution;
and S7, resolving the point cloud three-dimensional coordinates and drawing a three-dimensional point cloud map by using the result of the rear-end optimization, and marking the absolute position information of the water area boundary.
In a second aspect, a water area measurement system based on laser SLAM positioning is provided, which includes:
the data acquisition module is used for measuring a water area by using an unmanned ship carrying a laser radar, an Inertial Measurement Unit (IMU) and a Global Positioning System (GPS) receiver and acquiring data;
the static base initial alignment module is used for carrying out static base initial alignment by utilizing the acquired IMU and GPS data and providing initial absolute position and attitude information for subsequent laser SLAM resolving between marching;
the point cloud preprocessing module is used for carrying out point cloud preprocessing operation on data acquired by the laser radar according to the water area measuring environment;
the attitude transformation initial value module is used for estimating the attitude transformation between two adjacent frames of point clouds by utilizing the IMU and providing an attitude transformation initial value for point cloud registration;
the laser SLAM front-end module is used for registering the preprocessed point cloud by using a PPICP algorithm according to the position transformation initial value directly measured by the GPS and the attitude transformation initial value obtained by the attitude transformation initial value module;
the laser SLAM rear-end module adds combined navigation position and attitude information as additional constraints on the basis of a conventional rear-end graph optimization method in a GPS effective area, and performs graph optimization solution;
and the mapping module is used for resolving the point cloud three-dimensional coordinates and drawing a three-dimensional point cloud map by using the result of the rear-end optimization, and marking the absolute position information of the water area boundary.
Has the advantages that: the method and the system are mainly applied to the measurement of small water areas with the area not more than 5 ten thousand square meters, and aiming at the condition that the boundary GPS signal fails due to the surrounding trees of the small water areas or the standing of houses, a novel multi-sensor fusion mapping method is adopted. The measuring method is rapid, simple and efficient, and can accurately obtain the absolute position information of the boundary of the small water area. By using the measuring scheme provided by the invention, a three-dimensional point cloud map around the small water area can be generated, so that accurate reference is provided for overall protection and comprehensive treatment of the small water area, and the development of real estate unit measuring work is facilitated.
Drawings
FIG. 1 is a flow chart of the overall framework of the laser SLAM scheme applied to water area measurement of the present invention;
FIG. 2 is a schematic view of the unmanned ship measurement of the present invention;
FIG. 3 is a flow chart of the inventive SLAM solution;
FIG. 4 is a flow chart of the point cloud pre-processing of the present invention;
FIG. 5 is a comparison graph of registration results of three point cloud registration algorithms of the present invention;
FIG. 6 is a point cloud registration result diagram of the improved PPICP algorithm of the present invention;
FIG. 7 is a Kalman assisted improved PPICP point cloud registration result diagram of the present invention;
FIG. 8 is a real view of a real field of a real survey water area of the present invention;
FIG. 9 is a schematic view of a three-dimensional point cloud after data processing according to the present invention;
FIG. 10 is a schematic diagram of a three-dimensional grid after data processing according to the present invention.
Detailed Description
The technical scheme of the invention is further explained by combining the attached drawings.
The laser SLAM scheme applied to the small water area measurement can accurately measure the absolute geographical position of the water area boundary when the GPS signal of the small water area boundary fails, and has high measurement efficiency. Referring to fig. 1 and 3, in one embodiment, a water area measurement method based on laser SLAM positioning includes the following steps:
step S1, the unmanned ship equipped with the laser radar, the inertial measurement unit IMU, and the GPS receiver is used to measure the water area and collect data.
The unmanned ship is parked at a GPS effective point for 10min, then the unmanned ship runs for a circle around the outline of a small water area, and two factors need to be considered for a specific running track: firstly, the distance between the unmanned ship and the boundary of the water area is kept to be about 3-7 m in order to avoid low resolution of laser point cloud; secondly, the unmanned ship can run in the effective water area of the GPS as much as possible, and if the trees on the shore are covered seriously, the running track of the unmanned ship can be properly kept away from the boundary of the water area, so that the GPS signal of the unmanned ship is effective. When the two factors cannot be met simultaneously, the adjustment needs to be carried out according to the actual situation. The schematic diagram of unmanned ship measurement is shown in fig. 2, and data of the laser radar/inertial measurement unit/GPS receiver is stored in real time during driving for offline processing after a while.
And step S2, carrying out static base initial alignment by utilizing the acquired IMU and GPS data, and providing initial absolute pose information for subsequent laser SLAM resolving during traveling.
The first 10min of data collected in step S1 is used for initial alignment of the static base in order to provide initial absolute position and attitude information for the entire measurement process. The pose information solving comprises two links of coarse alignment and fine alignment, wherein the coarse alignment uses a double-vector pose determination algorithm to quickly obtain a rough initial pose matrix; and then, performing a fine alignment link on the basis of coarse alignment, estimating a position error and a misalignment angle between a theoretical navigation coordinate system and a calculated navigation coordinate system by using an SINS/GPS combined Kalman filtering technology, writing a system state equation by using an SINS error model, writing an observation equation by using position and speed information provided by a GPS as a measured value, and solving by using a Kalman basic formula to finally obtain more accurate initial pose information. It should be understood that the basic principle and the operation means of the combined navigation Kalman filtering performed between the precise Kalman filtering and the subsequent traveling are the same, and only the motion state of the unmanned ship is different and the purpose of the pose is different.
And step S3, performing point cloud preprocessing operation on the data acquired by the laser radar according to the small water area measurement environment.
Before the point cloud registration operation is performed, the point cloud is preprocessed, and the process schematic diagram is shown in fig. 4, and the process schematic diagram sequentially comprises invalid point elimination, outlier elimination, downsampling filtering and point cloud segmentation simplification. Specifically, invalid point culling: under the actual measurement condition, a plurality of invalid measurement points such as cavities and the like can appear in the laser point cloud, and the points are removed in the first step of pretreatment. Outlier rejection: outlier detection and rejection are performed using a statistical-based approach. Down-sampling filtering: and performing down-sampling filtering by using a voxelization mesh method, dividing a point cloud area into a plurality of three-dimensional voxel grids with the same size, calculating the gravity center of the point cloud contained in each voxel grid, and replacing the actual point cloud in the voxel grid with the gravity center value so as to fulfill the aim of simplifying the point cloud. Point cloud segmentation simplification comprises two links of point cloud segmentation and point cluster filtering, the point cloud segmentation borrows the idea of graph theory, a threshold minDistance is set, the point cloud is segmented into a plurality of discrete connected graphs according to the threshold, and the Euclidean distance of the nearest point of each two connected graphs must be larger than the minDistance; and (3) removing redundant point clusters according to the volume characteristics of the point clusters by point cluster filtering, calculating a minimum peripheral cube of each point cluster, setting a side length threshold value when the cube face is parallel to the plane of a coordinate system, and if the minimum peripheral cube has a side larger than the threshold value, determining the minimum peripheral cube as an effective point cluster, otherwise, determining the minimum peripheral cube as a redundant point cluster and needing to be removed. According to the characteristics of the water area measurement environment, point cloud segmentation and simplification processing are added in the point cloud preprocessing stage, point cloud data which are not noise points but are not small-sized water area shoreside scenes can be effectively eliminated, and the accuracy and efficiency of point cloud registration and subsequent map construction are improved.
And step S4, estimating the attitude transformation between two adjacent frames of point clouds by utilizing the IMU through a strapdown inertial navigation algorithm, and providing an initial attitude transformation value for point cloud registration.
The strapdown inertial navigation calculation mainly relates to angular rate integral operation, and the gyro random constant drift is used as an important error source of the gyro, so that the accuracy of attitude estimation is influenced to a certain extent. In order to avoid the influence of gyro random constant drift on the measurement result, the method of the invention uses SINS/GPS combined navigation Kalman filtering technology to estimate and compensate the random constant drift in the GPS effective area. The state vector X of the Kalman filter estimate is defined here as follows:
wherein, δ vE、δvNEast and north velocity errors, respectively; phi is aE、φNAnd phiUAn east misalignment angle, a north misalignment angle, and an azimuth misalignment angle, respectively; δ L and δ λ are latitude error and longitude error, respectively;
biasing the accelerometer for the x-axis and y-axis, respectively; epsilonx、εyAnd εzGyro drift is x-axis, y-axis and z-axis, respectively. And (4) carrying out combined navigation in the process of the unmanned ship, estimating the random constant drift of the compensation gyro, and simultaneously providing position and attitude information for the step S6.
According to the characteristics that the motion states of carriers are different and the observability of the system is different, the invention carries out random constant drift estimation on different unmanned ship maneuvering schemes, and formulates the unmanned ship maneuvering scheme suitable for small-sized water area measurement by comparing estimation effects: in the running process of the unmanned ship, the change of two horizontal attitude angles does not need to be manually participated in, and only the change of the heading angle is controlled. When the direction of the unmanned ship is controlled, the situation that circular motion with invariable angular rate is directly performed towards one side is avoided as much as possible, S-bending motion can be performed on the basis of the circular motion under the condition of considering measurement efficiency, but the motion angular rate cannot be overlarge and generally does not exceed 0.4rad/S (the overlarge angular rate can cause the cloud registration failure of a front end point), so that the observability of the random constant drift of the gyro is excited, and the estimation precision is improved. The unmanned ship maneuvering scheme improves observability of gyro random constant drift, and obviously improves Kalman filtering estimation effect, so that accuracy of IMU measurement relative attitude transformation is improved, probability of front end point cloud registration failure is reduced, and accuracy of point cloud registration is improved.
And step S5, registering the preprocessed point cloud by using a PPICP algorithm according to the initial values of the position and posture transformation.
Under the condition that an attitude transformation initial value is provided by S4 and a position transformation initial value is provided by GPS (GPS signals are provided if the GPS signals are effective, and the initial values are not provided if the GPS signals are ineffective), Point cloud registration is carried out on the Point cloud after preprocessing. Aiming at the characteristics of a small water area measurement environment, the PPICP algorithm is improved, and the improvement strategy is as follows: normal solution screening work is carried out before point-to-surface registration work is carried out, and when the surface normal of a certain point is solved, k points q closest to the point are searched0...qk-1And calculating and summing the distances from each point to the estimation plane, and if the sum of the distances is greater than a certain threshold value, indicating that the points in the k neighborhood are scattered, and the reliability of the surface normal solved by the points is not high, directly discarding the points, otherwise, keeping the points. After a screening step is carried out, only points with the effective surface normal are allowed to participate in the establishment of the PPICP error metric equation. The PPICP improved strategy can effectively avoid solving the normal of point cloud with low resolution and uneven dispersion, simplifies the number of point-to-surface pairings, avoids the points from participating in the construction of a PPICP error measurement equation, effectively reduces the probability of registration failure and improves the registration accuracy.
And step S6, adding the combined navigation position and attitude information as additional constraints on the basis of a conventional back-end diagram optimization method in the effective area of the GPS, and improving the accuracy of optimization solution.
And (4) the rear-end graph optimization gives an optimization target in a graph theory form by means of a graph theory idea, the positions of the unmanned ship and the point cloud positions to be calculated are taken as nodes, the constraint calculated by the front-end PPICP point cloud registration algorithm in the step S5 and the laser radar measured value are taken as edges, and a local optimal solution close to global optimization is solved by a nonlinear least square method. And when optimization solving is carried out, adding pose information of the integrated navigation solving into the graph optimization constraint condition as prior observation of the pose nodes. Specifically, in a GPS effective area, a GPS is used for providing position and speed measurement values for Kalman filtering, an SINS/GPS combined Kalman filtering technology is used for solving the position and attitude information of the unmanned ship, the position and attitude information calculated by SINS/GPS combined navigation is used as extra constraint and added into a graph for optimization, a direct pose prior observation is provided for the pose vertex of the unmanned ship, the error between the pose vertex estimation value and the combined navigation pose information is used as a new error item, the graph is added for optimizing the overall error, the extra constraint is added, the optimization solving precision is improved, and the precision and the efficiency of finally solving the point cloud position are improved.
And step S7, resolving a point cloud three-dimensional coordinate and drawing a three-dimensional point cloud map by using a rear-end optimization result, and marking absolute position information of a water area boundary.
And (4) drawing according to the result of the optimization calculation of the rear end graph in the step (S6), constructing a three-dimensional point cloud map around the small water area according to the three-dimensional point cloud coordinates, and deducing absolute position information of the boundary of the small water area by using the absolute pose information of the unmanned ship to finish the measurement work of the small water area.
According to another embodiment of the present invention, there is provided a water area measurement system based on laser SLAM positioning, including:
and the data acquisition module is used for measuring the water area by using an unmanned ship carrying a laser radar, an inertial measurement unit IMU and a GPS receiver and acquiring data. When the direction of the unmanned ship is controlled, circular motion with invariable angular rate is avoided from being directly made towards one side, and S-shaped motion with the angular rate not exceeding a preset threshold value can be made on the basis of the whole circular motion under the condition of considering the measurement efficiency. Preferably, the angular rate of movement does not exceed 0.4 rad/s.
And the static base initial alignment module is used for carrying out static base initial alignment by utilizing the acquired IMU and GPS data and providing initial absolute pose information for subsequent laser SLAM resolving during travelling. Specifically, the static base initial alignment module comprises a coarse alignment unit and a fine alignment unit, wherein the coarse alignment unit is configured to obtain an initial attitude matrix by using a dual-vector attitude determination algorithm; the fine alignment unit is configured to estimate a position error, a misalignment angle between a theoretical navigation coordinate system and a calculated navigation coordinate system by using an SINS/GPS combined Kalman filtering technology on the basis of coarse alignment, write a system state equation by using an SINS error model, write an observation equation by using position and speed information provided by a GPS as a measured value column, and then solve by using a Kalman basic formula to finally obtain initial pose information.
And the point cloud preprocessing module is used for carrying out point cloud preprocessing operation on the data acquired by the laser radar according to the small water area measuring environment. Specifically, the point cloud preprocessing module comprises an invalid point eliminating submodule, an outlier eliminating submodule, a down-sampling filtering submodule and a point cloud segmentation and simplification submodule, wherein the invalid point eliminating submodule is used for eliminating void invalid measuring points; an outlier rejection sub-module configured to detect and reject outliers using a statistics-based approach; the down-sampling filtering sub-module is configured to perform down-sampling filtering by using a voxelization mesh method, divide a point cloud area into three-dimensional voxel grids with the same size, calculate the gravity center of the point cloud contained in each voxel grid, and replace the actual point cloud in the voxel grid with the gravity center value; the point cloud segmentation and simplification submodule comprises a point cloud segmentation unit and a point cluster filtering unit, wherein the point cloud segmentation unit is configured to segment a point cloud into a plurality of discrete connected graphs according to a preset threshold minDistance by means of a graph theory idea, and the Euclidean distance of the nearest point of every two connected graphs must be larger than minDistance; the point cluster filtering unit is configured to remove redundant point clusters according to the volume characteristics of the point clusters, calculate a minimum peripheral cube of each point cluster, set a side length threshold value if the face of the cube is parallel to the plane of the coordinate system, and determine that the minimum peripheral cube has a side larger than the threshold value as an effective point cluster, otherwise, determine that the minimum peripheral cube is a redundant point cluster and need to be removed.
And the attitude transformation initial value module estimates the attitude transformation between two adjacent frames of point clouds by utilizing the IMU through a strapdown inertial navigation algorithm and provides an attitude transformation initial value for point cloud registration. In order to avoid the influence of gyro random constant drift on the measurement result, the invention uses SINS/GPS combined navigation Kalman filtering technology to estimate and compensate the random constant drift in the GPS effective area.
And the laser SLAM front-end module is used for registering the preprocessed point cloud by using a PPICP algorithm according to the position transformation initial value directly measured by the GPS and the pose transformation initial value obtained by the pose transformation initial value module. Before the registration work is carried out, normal solving and screening work is firstly carried out, and when the surface normal of a certain point is solved, k points q closest to the point are searched0...qk-1Calculating the distance from each point to the estimation plane and summing, if the sum of the distances is greater than a given threshold value, indicating that the points in the k neighborhood are scattered, and the reliability of the surface normal solved by the points is not high, directly discarding the points, otherwise, keeping the points; after a screening step is carried out, only points with the effective surface normal are allowed to participate in the establishment of the PPICP error metric equation.
The laser SLAM rear end module adds the combined navigation position and attitude information as additional constraints on the basis of a conventional rear end image optimization method in the effective area of a GPS, and improves the precision of optimization solution. Specifically, the laser SLAM rear end module uses the unmanned ship position and point cloud position to be measured as nodes by means of a graph optimization technology, uses the constraint of point cloud registration calculation of a laser SLAM front end PPICP algorithm and a laser radar measured value as edges, and solves a local optimal solution close to global optimization through a nonlinear least square method. When the optimization solution is carried out, the pose information of the integrated navigation solution is added into the graph optimization constraint condition to be used as the prior observation of pose nodes, specifically, in a GPS effective area, a GPS is used for providing position and speed measurement values for Kalman filtering, an SINS/GPS integrated Kalman filtering technology is used for solving the position and attitude information of the unmanned ship, the position and attitude information calculated by the SINS/GPS integrated navigation is used as additional constraints to be added into the graph optimization, a direct pose prior observation is provided for the pose vertex of the unmanned ship, the error between the pose vertex estimation value and the integrated navigation pose information is used as a new error item, the overall error of the graph optimization is added, the additional constraints are added, the precision of the optimization solution is improved, and the precision and the efficiency of finally solving the point cloud position are improved.
And the mapping module is used for resolving the point cloud three-dimensional coordinates and drawing a three-dimensional point cloud map by using the result of the rear-end optimization, and marking the absolute position information of the water area boundary. Specifically, the mapping module carries out mapping according to the result of optimization calculation of the rear-end map, a three-dimensional point cloud map around the small water area can be constructed according to the three-dimensional point cloud coordinates, and in addition, absolute position information of the boundary of the small water area can be obtained through deduction by utilizing the absolute pose information of the unmanned ship, so that the small water area measurement work is completed.
The prior art is not detailed in the description of the invention.
In order to verify the effectiveness of the laser SLAM scheme provided by the invention, a simulation experiment and a physical experiment are respectively carried out. Firstly, a simulation experiment adopts a block-by-block mode, and carries out precision alignment simulation under a static base, point cloud pretreatment simulation, gyro random constant drift estimation and compensation technology simulation, laser SLAM front end point cloud registration technology simulation and the like respectively, a large amount of simulation plays an instructive role in the proposal, and the effectiveness of the proposal is verified at the same time. A physical experiment, namely measuring a specific small water area (the water area boundary GPS signal is effective, and the feasibility of the laser SLAM scheme of the invention can be judged by using position information provided by a GPS as a true value) by using an unmanned ship provided with a laser radar/inertial measurement unit/GPS receiver, carrying out post-process off-line processing on the acquired data according to the laser SLAM scheme of the invention, wherein fig. 5 is a comparison graph of registration results of three point cloud registration algorithms, wherein fig. 5(a) is a schematic diagram of a real track of the unmanned ship, fig. 5(b) is an ICP algorithm point cloud registration result, fig. 5(c) is a PPICP algorithm point cloud registration result, and fig. 5(d) is an NDT algorithm point cloud registration result, and the superiority of the PPICP registration can be verified by comparison; FIG. 6 is a point cloud registration result diagram of the improved PPICP algorithm, which effectively improves the precision of point cloud registration; FIG. 7 is a Kalman assisted improved PPICP point cloud registration result diagram, which effectively reduces the probability of point cloud registration failure; fig. 8 is a real view of an actually measured water area, fig. 9 is a schematic view of a three-dimensional point cloud processed by the method of the present invention, fig. 10 is a schematic view of a three-dimensional grid after processing, a 2D contour map of the water area boundary can be drawn by controlling the grid Z-axis height, the Z-axis height is set according to the actual situation and generally does not exceed [ -1m,1m ], the built map can be seen to be basically consistent with the actual water area, and the feasibility of the measurement scheme of the present invention is verified by comparing with the absolute position information of the water area boundary provided by the GPS.
The above examples are only preferred embodiments of the present invention, it should be noted that: it will be apparent to those skilled in the art that various modifications and equivalents can be made without departing from the spirit of the invention, and it is intended that all such modifications and equivalents fall within the scope of the invention as defined in the claims.
Claims (10)
1. A water area measuring method based on laser SLAM positioning is characterized by comprising the following steps:
step S1, measuring a water area by using an unmanned ship provided with a laser radar, an inertial measurement unit IMU and a GPS receiver, and collecting data;
s2, carrying out static base initial alignment by utilizing the acquired IMU and GPS data, and providing initial absolute position and attitude information for subsequent inter-travel laser SLAM resolving;
step S3, according to the water area measuring environment, point cloud preprocessing operation is carried out on the data collected by the laser radar;
step S4, estimating the attitude transformation between two adjacent frames of point clouds by utilizing the IMU, and providing an initial attitude transformation value for point cloud registration;
step S5, registering the preprocessed point cloud by using a PPICP algorithm according to the position transformation initial value directly measured by the GPS and the attitude transformation initial value obtained in the step S4;
step S6, in the effective area of GPS, adding the combined navigation position and attitude information as additional constraint on the basis of the conventional back-end graph optimization method, and carrying out graph optimization solution;
and step S7, resolving a point cloud three-dimensional coordinate and drawing a three-dimensional point cloud map by using a rear-end optimization result, and marking absolute position information of a water area boundary.
2. The method as claimed in claim 1, wherein in step S1, the unmanned ship stops at the GPS effective point for a specified time, then runs for a circle around the water area contour, and performs measurement, and the unmanned ship performs S-turn motion with an angular rate not exceeding a preset threshold based on the overall circular motion.
3. The water area measurement method based on laser SLAM positioning as claimed in claim 1, wherein the static base initial alignment in step S2 includes two links of coarse alignment and fine alignment, the coarse alignment uses double vector pose algorithm to get initial pose matrix; the precise alignment is based on the rough alignment, a misalignment angle between a position error and a theoretical navigation coordinate system is estimated by using an SINS/GPS combined Kalman filtering technology, a misalignment angle between the theoretical navigation coordinate system and a calculated navigation coordinate system is calculated, a system state equation is written by using an SINS error model, position and speed information provided by a GPS is used as a measurement value column writing observation equation, then a Kalman basic formula is used for solving, and initial pose information is finally obtained.
4. The water area measurement method based on laser SLAM positioning as claimed in claim 1, wherein the point cloud pre-processing operation in step S3 comprises invalid point elimination, outlier elimination, down-sampling filtering, point cloud segmentation reduction, wherein,
the invalid point removing is used for removing void invalid measuring points;
the outlier rejection uses a statistical-based method to detect and reject outliers;
the downsampling filtering uses a voxelization grid method to divide a point cloud area into three-dimensional voxel grids with the same size, the gravity center of the point cloud contained in each voxel grid is calculated, and the actual point cloud in the voxel grid is replaced by a gravity center value;
the point cloud segmentation and simplification comprises two links of point cloud segmentation and point cluster filtering, the point cloud segmentation borrows the idea of graph theory, the point cloud is segmented into a plurality of discrete connected graphs according to a preset threshold minDistance, and the Euclidean distance of the nearest point of each two connected graphs must be larger than the minDistance; and the point cluster filtering eliminates redundant point clusters according to the volume characteristics of the point clusters, calculates a minimum peripheral cube of each point cluster, sets a side length threshold value when the cube face is parallel to the plane of a coordinate system, and if the minimum peripheral cube has a side larger than the threshold value, the minimum peripheral cube is regarded as an effective point cluster, otherwise, the minimum peripheral cube is regarded as a redundant point cluster and needs to be eliminated.
5. The water area measurement method based on laser SLAM positioning as claimed in claim 1, wherein in step S5, before PPICP point cloud registration, normal solution screening is performed, and when finding the surface normal of a certain point, k points q closest to the point q are searched0...qk-1Calculating the distance from each point to the estimation plane and summing, if the sum of the distances is greater than a given threshold value, indicating that the points in the k neighborhood are scattered, and the reliability of the surface normal solved by the points is not high, directly discarding the points, otherwise, keeping the points; after a screening step is carried out, only points with the effective surface normal are allowed to participate in the establishment of the PPICP error metric equation.
6. The method of claim 1, wherein in step S6, the unmanned ship position and the point cloud position to be measured are used as nodes by means of a graph optimization technique, constraints of point cloud registration calculation and lidar measurement values in step S5 are used as edges, and optimization solution is performed by a nonlinear least square method, wherein pose information of combined navigation solution is added to a graph optimization constraint condition as prior observation of pose nodes, and the pose information of combined navigation solution is: in the GPS effective area, the SINS/GPS combined Kalman filtering technology is used for solving the position and attitude information of the unmanned ship, wherein the GPS provides position and speed measurement values for Kalman filtering.
7. A water area measurement system based on laser SLAM positioning is characterized by comprising:
the data acquisition module is used for measuring a water area by using an unmanned ship carrying a laser radar, an Inertial Measurement Unit (IMU) and a Global Positioning System (GPS) receiver and acquiring data;
the static base initial alignment module is used for carrying out static base initial alignment by utilizing the acquired IMU and GPS data and providing initial absolute position and attitude information for subsequent laser SLAM resolving between marching;
the point cloud preprocessing module is used for carrying out point cloud preprocessing operation on data acquired by the laser radar according to the water area measuring environment;
the attitude transformation initial value module is used for estimating the attitude transformation between two adjacent frames of point clouds by utilizing the IMU and providing an attitude transformation initial value for point cloud registration;
the laser SLAM front-end module is used for registering the preprocessed point cloud by using a PPICP algorithm according to the position transformation initial value directly measured by the GPS and the attitude transformation initial value obtained by the attitude transformation initial value module;
the laser SLAM rear-end module adds combined navigation position and attitude information as additional constraints on the basis of a conventional rear-end graph optimization method in a GPS effective area, and performs graph optimization solution;
and the mapping module is used for resolving the point cloud three-dimensional coordinates and drawing a three-dimensional point cloud map by using the result of the rear-end optimization, and marking the absolute position information of the water area boundary.
8. The laser SLAM positioning-based water area measurement system of claim 7, wherein the static base initial alignment module comprises a coarse alignment unit and a fine alignment unit, the coarse alignment unit configured to derive an initial attitude matrix using a dual vector attitude determination algorithm; the fine alignment unit is configured to estimate a position error, a misalignment angle between a theoretical navigation coordinate system and a calculated navigation coordinate system by using an SINS/GPS combined Kalman filtering technology on the basis of coarse alignment, write a system state equation by using an SINS error model, write an observation equation by using position and speed information provided by a GPS as a measurement value, and solve by using a Kalman basic formula to finally obtain initial pose information.
9. The water area measurement system based on laser SLAM positioning of claim 7 wherein the point cloud pre-processing module comprises an invalid point elimination sub-module, an outlier elimination sub-module, a down-sampling filtering sub-module, a point cloud segmentation and simplification sub-module, wherein,
the invalid point removing submodule is used for removing the void invalid measuring points;
the outlier culling sub-module is configured to detect and cull outliers using a statistics-based approach;
the downsampling filtering submodule is configured to perform downsampling filtering by using a voxelization gridding method, divide a point cloud area into three-dimensional voxel grids with the same size, calculate the gravity center of point clouds contained in each voxel grid, and replace actual point clouds in the voxel grids by a gravity center value;
the point cloud segmentation and simplification submodule comprises a point cloud segmentation unit and a point cluster filtering unit, wherein the point cloud segmentation unit is configured to segment a point cloud into a plurality of discrete connected graphs according to a preset threshold minDistance by means of a graph theory idea, and the Euclidean distance of the nearest point of every two connected graphs must be greater than minDistance; the point cluster filtering unit is configured to remove redundant point clusters according to the volume characteristics of the point clusters, calculate a minimum peripheral cube of each point cluster, set a side length threshold value if the face of the cube is parallel to the plane of the coordinate system, and determine that the minimum peripheral cube has a side larger than the threshold value as an effective point cluster, otherwise, determine that the minimum peripheral cube is a redundant point cluster and need to be removed.
10. The water area measurement system based on the laser SLAM positioning as claimed in claim 7, wherein the laser SLAM front end module performs normal solution screening before PPICP point cloud registration, and when finding the surface normal of a certain point, searches for the k points q closest to the point q0...qk-1Calculating the distance from each point to the estimation plane and summing, if the sum of the distances is greater than a given threshold value, indicating that the points in the k neighborhood are scattered, and the reliability of the surface normal solved by the points is not high, directly discarding the points, otherwise, keeping the points; after a screening step is carried out, only points with the effective surface normal are allowed to participate in the establishment of the PPICP error metric equation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010278863.0A CN111457902B (en) | 2020-04-10 | 2020-04-10 | Water area measuring method and system based on laser SLAM positioning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010278863.0A CN111457902B (en) | 2020-04-10 | 2020-04-10 | Water area measuring method and system based on laser SLAM positioning |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111457902A CN111457902A (en) | 2020-07-28 |
| CN111457902B true CN111457902B (en) | 2021-10-29 |
Family
ID=71676271
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010278863.0A Active CN111457902B (en) | 2020-04-10 | 2020-04-10 | Water area measuring method and system based on laser SLAM positioning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111457902B (en) |
Families Citing this family (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112581610B (en) * | 2020-10-16 | 2023-06-13 | 武汉理工大学 | Robust optimization method and system for building map from multi-beam sonar data |
| CN112362051B (en) * | 2020-10-16 | 2021-11-02 | 无锡卡尔曼导航技术有限公司 | Mobile robot navigation positioning system based on information fusion |
| CN112562052B (en) * | 2020-12-03 | 2021-07-27 | 广东工业大学 | Real-time positioning and mapping method for near-shore water area |
| CN113138395B (en) * | 2021-04-25 | 2024-06-14 | 南京鹏畅科技实业有限公司 | Point cloud map construction method based on laser radar data fusion of total station |
| CN113256697B (en) * | 2021-04-27 | 2023-07-18 | 武汉理工大学 | Three-dimensional reconstruction method, system, device and storage medium for underwater scene |
| CN113538677B (en) * | 2021-06-30 | 2023-12-15 | 深圳市优必选科技股份有限公司 | Positioning method, robot and storage medium |
| CN113581439B (en) * | 2021-08-24 | 2022-03-25 | 广东工业大学 | Omnidirectional unmanned ship and control system and method thereof |
| CN113959437A (en) * | 2021-10-14 | 2022-01-21 | 重庆数字城市科技有限公司 | Measuring method and system for mobile measuring equipment |
| CN114046792B (en) * | 2022-01-07 | 2022-04-26 | 陕西欧卡电子智能科技有限公司 | Unmanned ship water surface positioning and mapping method, device and related components |
| CN115127548A (en) * | 2022-06-28 | 2022-09-30 | 苏州精源创智能科技有限公司 | Navigation system integrating inertial navigation and laser dot matrix |
| CN116481545B (en) * | 2023-06-21 | 2023-08-25 | 广州中海电信有限公司 | Ship navigation planning method, storage medium and system based on satellite communication |
| CN116559928B (en) * | 2023-07-11 | 2023-09-22 | 新石器慧通（北京）科技有限公司 | Pose information determining method, device and equipment of laser radar and storage medium |
| CN116908810B (en) * | 2023-09-12 | 2023-12-12 | 天津大学四川创新研究院 | Method and system for measuring earthwork of building by carrying laser radar on unmanned aerial vehicle |
| CN117075171B (en) * | 2023-10-18 | 2024-01-16 | 新石器慧通（北京）科技有限公司 | Pose information determining method, device and equipment of laser radar and storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108303710A (en) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar |
| CN109341706A (en) * | 2018-10-17 | 2019-02-15 | 张亮 | A kind of production method of the multiple features fusion map towards pilotless automobile |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109900265A (en) * | 2019-03-15 | 2019-06-18 | 武汉大学 | A kind of robot localization algorithm of camera/mems auxiliary Beidou |
| CN109945857A (en) * | 2019-03-18 | 2019-06-28 | 东南大学 | A kind of vehicle-mounted inertial positioning method and device thereof towards real estate field survey |
| CN110211228A (en) * | 2019-04-30 | 2019-09-06 | 北京云迹科技有限公司 | For building the data processing method and device of figure |
| CN110361010A (en) * | 2019-08-13 | 2019-10-22 | 中山大学 | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9377310B2 (en) * | 2013-05-02 | 2016-06-28 | The Johns Hopkins University | Mapping and positioning system |
| CN110596683B (en) * | 2019-10-25 | 2021-03-26 | 中山大学 | Multi-group laser radar external parameter calibration system and method thereof |
-
2020
- 2020-04-10 CN CN202010278863.0A patent/CN111457902B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108303710A (en) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | Drawing method is built in the more scene positioning of unmanned plane based on three-dimensional laser radar |
| CN109341706A (en) * | 2018-10-17 | 2019-02-15 | 张亮 | A kind of production method of the multiple features fusion map towards pilotless automobile |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109900265A (en) * | 2019-03-15 | 2019-06-18 | 武汉大学 | A kind of robot localization algorithm of camera/mems auxiliary Beidou |
| CN109945857A (en) * | 2019-03-18 | 2019-06-28 | 东南大学 | A kind of vehicle-mounted inertial positioning method and device thereof towards real estate field survey |
| CN110211228A (en) * | 2019-04-30 | 2019-09-06 | 北京云迹科技有限公司 | For building the data processing method and device of figure |
| CN110361010A (en) * | 2019-08-13 | 2019-10-22 | 中山大学 | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot |
Non-Patent Citations (1)
| Title |
|---|
| 惯性***/激光测距仪组合定位技术研究;闫晶 等;《测绘与空间地理信息》;20161031;第39卷(第10期);第87-90页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111457902A (en) | 2020-07-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111457902B (en) | Water area measuring method and system based on laser SLAM positioning | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| CN112987065B (en) | Multi-sensor-integrated handheld SLAM device and control method thereof | |
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| CN110411462A (en) | A kind of GNSS/ inertia/lane line constraint/odometer multi-source fusion method | |
| JP2019518222A (en) | Laser scanner with real-time on-line egomotion estimation | |
| CN113781582A (en) | Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN110361010A (en) | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot | |
| CN115407357A (en) | Low-beam laser radar-IMU-RTK positioning mapping algorithm based on large scene | |
| CN112967392A (en) | Large-scale park mapping and positioning method based on multi-sensor contact | |
| CN111829514A (en) | Road surface working condition pre-aiming method suitable for vehicle chassis integrated control | |
| CN112455502A (en) | Train positioning method and device based on laser radar | |
| CN116608873A (en) | Multi-sensor fusion positioning mapping method for automatic driving vehicle | |
| CN115183762A (en) | Airport warehouse inside and outside mapping method, system, electronic equipment and medium | |
| CN115127543A (en) | Method and system for eliminating abnormal edges in laser mapping optimization | |
| Srinara et al. | Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme | |
| CN109975848B (en) | Precision optimization method of mobile measurement system based on RTK technology | |
| CN114754741A (en) | Slope section gradient measuring method | |
| CN113959437A (en) | Measuring method and system for mobile measuring equipment | |
| Rebelo et al. | Building 3D city models: Testing and comparing Laser scanning and low-cost UAV data using FOSS technologies | |
| CN115790613B (en) | Visual information-assisted inertial/odometer combined navigation method and device | |
| CN116929363A (en) | Mining vehicle autonomous navigation method based on passable map | |
| CN115930948A (en) | Orchard robot fusion positioning method | |
| CN113403942B (en) | Label-assisted bridge detection unmanned aerial vehicle visual navigation method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |