CN116973895A - Real-time pose correction method for laser point cloud matching - Google Patents
Real-time pose correction method for laser point cloud matching Download PDFInfo
- Publication number
- CN116973895A CN116973895A CN202311226492.1A CN202311226492A CN116973895A CN 116973895 A CN116973895 A CN 116973895A CN 202311226492 A CN202311226492 A CN 202311226492A CN 116973895 A CN116973895 A CN 116973895A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- laser
- failure
- observation
- epoch
- 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.)
- Granted
Links
- 238000012937 correction Methods 0.000 title claims abstract description 54
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000005259 measurement Methods 0.000 claims abstract description 42
- 238000004364 calculation method Methods 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000009434 installation Methods 0.000 claims description 7
- 230000001419 dependent effect Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 abstract description 2
- 238000000691 measurement method Methods 0.000 description 1
- 238000013441 quality evaluation Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/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
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- 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/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Abstract
The invention discloses a real-time pose correction method for laser point cloud matching, which belongs to the technical field of electric digital processing and is used for correcting the pose of a navigation system after failure, and comprises the steps of acquiring the spatial position and the pose information of a laser movement measurement system under different observation epochs; secondly, determining a certain number of laser point cloud characteristic points by a point cloud matching method according to adjacent observation epochs before and after the failure of the navigation positioning system; thirdly, constructing a spatial position and posture information correction relation by utilizing the laser point cloud characteristic points under a unified coordinate system, and carrying out nonlinear least square calculation by taking the spatial position and posture as unknown parameters; finally, the corrected space position and posture information is reused for calculating the laser point cloud position of the next observation epoch after failure, and laser movement measurement posture correction is repeated until the navigation positioning system recovers to be positioned accurately; the invention has the feasibility of carrying out short-term real-time pose correction on the laser mobile measurement system carrying various navigation positioning sensors.
Description
Technical Field
The invention discloses a real-time pose correction method for laser point cloud matching, and belongs to the technical field of electric digital processing.
Background
Laser mobile measurement is a high-precision three-dimensional measurement technique that enables three-dimensional spatial measurement and reconstruction of an object by scanning and receiving reflected light. Accurate position and attitude information is the basis for the laser mobile measurement system to acquire high-precision point cloud data, and the laser mobile measurement navigation positioning system at the present stage is mainly based on the combination of an inertial navigation system, a global navigation satellite system or an acoustic positioning system and the like. Although the integrated navigation positioning system can greatly improve the accuracy and the robustness of navigation positioning, the possibility of positioning failure still exists in a complex working environment. When the navigation positioning system fails, the measured position and posture information is inaccurate, so that a larger error exists in the position calculation result of the point cloud obtained by laser movement measurement, and the point cloud is characterized in that the same-name characteristic points in the point cloud are not matched between adjacent observation epochs. Therefore, the real-time pose correction method for laser point cloud matching is provided, real-time continuous correction of the position and the pose of the mobile measurement system is realized by utilizing the point cloud matching property before and after the failure of the navigation positioning system, and the problem of the position misalignment of point cloud data when the navigation positioning system fails is effectively solved.
Disclosure of Invention
The invention aims to provide a real-time pose correction method for laser point cloud matching, which aims to solve the problem that in the prior art, a sensor for providing absolute positioning in integrated navigation is invalid in a short time, and the pose deviation is large, so that the position of the laser point cloud is inaccurate.
A real-time pose correction method for laser point cloud matching comprises the following steps:
step 1: determining an observation epoch before failure of the navigation positioning system and an observation epoch after failure, and acquiring the spatial position and posture information of the laser movement measurement system under different epochs;
step 2: selecting laser point cloud characteristic points through point cloud matching according to adjacent observation epochs before and after the failure of the navigation positioning system;
step 3: constructing a spatial position and posture information correction relation under a unified coordinate system by utilizing laser point cloud characteristic points, and obtaining corrected spatial position and posture information by adopting least square nonlinear solution;
step 4: and reusing the corrected space position and posture information in the laser point cloud position calculation of the next observation epoch after failure, and repeating the laser movement measurement posture correction until the navigation positioning system recovers to be positioned accurately.
Step 1.1: determining a previous observation epoch for a navigation positioning system failureAcquiring epoch->Spatial position of the lower laser movement measurement system +.>And posture information->；
Step 1.2: determining an observation epoch after failure of a navigation positioning systemAcquiring epoch->Spatial position of the lower laser movement measurement system +.>Harmonizing postureStatus information->，/>Longitude, & ->Is a latitude,Is of height, & lt + & gt>Is pitch angle, < >>Is a roll angle>Is the angle of deflection, the->Is the observation epoch of the failure of the navigation positioning system.
Step 2.1: acquiring previous observation epoch of failure of navigation positioning systemPoint cloud data set of laser movement measurement system +.>Observation epoch after failure of navigation positioning system>Point cloud data set of lower laser mobile measurement system +.>；
Step 2.2: determining observation epoch by adopting point cloud matching methodThe lower Point cloud dataset->And observation epochThe lower Point cloud dataset->Is->。
Step 3.1: constructing homonymous feature point sets among adjacent observation epochs according to the conversion relation among a laser radar scanner coordinate system, a carrier coordinate system, a local horizontal coordinate system and a geocentric earth fixed coordinateSpatial positional relationship of:
；
in the formula ,for the geocentric geodetic coordinate system feature point set coordinates, < ->The geodetic coordinates of the station are fixed,for the local horizontal coordinate system feature point set coordinates, < +.>For the coordinates of the feature point set of the carrier coordinate system, +.>For the coordinates of the feature point set of the laser radar scanner coordinate system,/-for>Is a longitude and latitude related rotation matrix, +.>For a gesture-dependent rotation matrix +.>For the installation of the calibration-dependent rotation matrix, +.>For the installation of the calibration translation, +.> and />Are obtained by means of mounting calibration.
Step 3.2: the space position relation between the coordinate system of the laser radar scanner and the earth-centered earth-fixed coordinate system is utilized to observe the epochLongitude of the lower laser movement measuring system +.>Latitude->Height->Pitch angle->Roll angle->Deviation angle->As an unknown parameter->Constructing a position and posture correction equation:
；
in the formula ,for unknown parameter initial value, ++>，/>For correcting the pre-longitude->To correct the latitude before->To correct the front height->To correct the front pitch angle->To correct the front roll angle->To correct the forward yaw angle.
The step 3 comprises the following steps:
step 3.3: and (3) carrying out nonlinear solution on the position and posture correction equation by adopting a least square indirect adjustment method:
the position and posture correction equation is a relation in ideal state, and unknown parameters in actual conditionsAndthere is a residual vector L between:
；/>；
in the formula ,is a linearized coefficient matrix, also called jacobian matrix, ">The observation vector after linearization is also a residual vector, and represents the residual of each data point.
Step 3.1 comprises:
；
。
step 3.2 comprises: the position and attitude correction equations are specifically:
；
；
；
in the formula ,，/>，/>is the lower coordinate of the geocentric earth fixed coordinate system, < + >>，/>，/>For the lower coordinate of the carrier coordinate system,/->，/>，/>The ground center of the station is the ground center of the station.
Step 3.2 comprises:
converting the coordinates of a station point into longitude by using the related parameters of a reference ellipsoidLatitude->Elevation->Representation is made, unify->，/>，/>，/>，/>，/>Solving as an unknown parameter, and reducing the complexity of the matrix B:
；
；
；
；
wherein ,，/>the second eccentricity of the ellipsoid, a is the long radius of the ellipsoid and b is the short radius of the ellipsoid.
Step 4.1: using corrected observation epochsLongitude of->Latitude->Height->Pitch angleRoll angle->Deviation angle->Reusing the laser point cloud position calculation of the next observation epoch after failure to obtain a corrected point cloud data set +.>；
Step 4.2: for observation epoch and />And (3) repeating the step (2) and the step (3) until the navigation positioning system of the laser movement measurement system is recovered.
The invention has the following advantages: when short-term failure or accuracy of a sensor providing absolute positioning accuracy in a navigation positioning system cannot be guaranteed, the provided real-time pose correction method for laser point cloud matching can build a spatial position and pose information correction relation by using sparse and uniformly distributed small quantity of laser point cloud characteristic points through laser point cloud matching, and corrected spatial position and pose information of a mobile measurement system is obtained by least square nonlinear solution, so that accurate calculation of the laser point cloud position is realized, and the problem of short-term failure of the navigation positioning system in the laser mobile measurement process is effectively solved.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the technical solutions in the present invention will be clearly and completely described below, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
A real-time pose correction method for laser point cloud matching comprises the following steps:
step 1: determining an observation epoch before failure of the navigation positioning system and an observation epoch after failure, and acquiring the spatial position and posture information of the laser movement measurement system under different epochs;
step 2: selecting laser point cloud characteristic points through point cloud matching according to adjacent observation epochs before and after the failure of the navigation positioning system;
step 3: constructing a spatial position and posture information correction relation under a unified coordinate system by utilizing laser point cloud characteristic points, and obtaining corrected spatial position and posture information by adopting least square nonlinear solution;
step 4: and reusing the corrected space position and posture information in the laser point cloud position calculation of the next observation epoch after failure, and repeating the laser movement measurement posture correction until the navigation positioning system recovers to be positioned accurately.
Step 1.1: determining a previous observation epoch for a navigation positioning system failureAcquiring epoch->Spatial position of the lower laser movement measurement system +.>And posture information->；
Step 1.2: determining an observation epoch after failure of a navigation positioning systemAcquiring epoch->Spatial position of the lower laser movement measurement system +.>And posture information->，/>Longitude, & ->Is a latitude,Is of height, & lt + & gt>Is pitch angle, < >>Is a roll angle>Is the angle of deflection, the->Is the observation epoch of the failure of the navigation positioning system.
Step 2.1: acquiring previous observation epoch of failure of navigation positioning systemPoint cloud data set of laser movement measurement system +.>Observation epoch after failure of navigation positioning system>Point cloud data set of lower laser mobile measurement system +.>；
Step 2.2: determining observation epoch by adopting point cloud matching methodThe lower Point cloud dataset->And observation epochThe lower Point cloud dataset->Is->。
Step 3.1: constructing homonymous feature point sets among adjacent observation epochs according to the conversion relation among a laser radar scanner coordinate system, a carrier coordinate system, a local horizontal coordinate system and a geocentric earth fixed coordinateSpatial positional relationship of:
；
in the formula ,for the geocentric geodetic coordinate system feature point set coordinates, < ->The geodetic coordinates of the station are fixed,for the local horizontal coordinate system feature point set coordinates, < +.>For the coordinates of the feature point set of the carrier coordinate system, +.>For the coordinates of the feature point set of the laser radar scanner coordinate system,/-for>Is a longitude and latitude related rotation matrix, +.>For a gesture-dependent rotation matrix +.>For the installation of the calibration-dependent rotation matrix, +.>For the installation of the calibration translation, +.> and />Are obtained by means of mounting calibration.
Step 3.2: the space position relation between the coordinate system of the laser radar scanner and the earth-centered earth-fixed coordinate system is utilized to observe the epochLongitude of the lower laser movement measuring system +.>Latitude->Height->Pitch angle->Roll angle->Deviation angle->As an unknown parameter->Constructing a position and posture correction equation:
；
in the formula ,for unknown parameter initial value, ++>，/>For correcting the pre-longitude->To correct the latitude before->To correct the front height->To correct the front pitch angle->To correct the front roll angle->To correct the forward yaw angle.
The step 3 comprises the following steps:
step 3.3: and (3) carrying out nonlinear solution on the position and posture correction equation by adopting a least square indirect adjustment method:
the position and posture correction equation is a relation in ideal state, and unknown parameters in actual conditionsAndthere is a residual vector L between:
；/>；
in the formula ,is a linearized coefficient matrix, also called jacobian matrix, ">The observation vector after linearization is also a residual vector, and represents the residual of each data point.
Step 3.1 comprises:
；
。
step 3.2 comprises: the position and attitude correction equations are specifically:
；
；
；
in the formula ,，/>，/>is the lower coordinate of the geocentric earth fixed coordinate system, < + >>，/>，/>For the lower coordinate of the carrier coordinate system,/->，/>，/>The ground center of the station is the ground center of the station.
Step 3.2 comprises:
converting the coordinates of a station point into longitude by using the related parameters of a reference ellipsoidLatitude->Elevation->Representation is made, unify->，/>，/>，/>，/>，/>Solving as an unknown parameter, and reducing the complexity of the matrix B:
；
；
；
；
wherein ,，/>the second eccentricity of the ellipsoid, a is the long radius of the ellipsoid and b is the short radius of the ellipsoid.
Step 4.1: using corrected observation epochsLongitude of->Latitude->Height->Pitch angleRoll angle->Deviation angle->Reusing the laser point cloud position calculation of the next observation epoch after failure to obtain a corrected point cloud data set +.>；
Step 4.2: for observation epoch and />And (3) repeating the step (2) and the step (3) until the navigation positioning system of the laser movement measurement system is recovered.
In an embodiment, a real-time pose correction method for laser point cloud matching includes:
step 1: determining an observation epoch before failure of the navigation positioning system and an observation epoch after failure, and acquiring the spatial position and posture information of the laser movement measurement system under different epochs;
in the laser movement measurement process, an inertial navigation system provides real-time attitude information, a global navigation satellite system or an acoustic positioning system provides real-time position information, and accurate spatial position and attitude information of the laser movement measurement system can be obtained through a combined navigation algorithm by utilizing installation and calibration parameters among instruments and equipment. When the navigation positioning system formed by the inertial navigation system, the global navigation satellite system or the underwater sound positioning system fails, the observation epoch before the failure of the navigation positioning system and the observation epoch after the failure are determined by a navigation positioning quality evaluation method, and the space position and the gesture information of the laser movement measurement system under different epochs are obtained.
Step 2: selecting a certain number of laser point cloud characteristic points by a point cloud matching method according to adjacent observation epochs before and after the failure of the navigation positioning system;
after the observation calendar elements before and after the failure of the navigation positioning system are determined, acquiring a point cloud data set of the adjacent observation calendar elements, wherein the point cloud position information in the point cloud data set of the observation calendar elements before the failure of the navigation positioning system is accurate at the moment because the space position and the gesture information of the observation calendar elements before the failure of the navigation positioning system are accurate; because the space position and posture information of the observation calendar element after the navigation positioning system is invalid is inaccurate, the point cloud position information in the point cloud data set of the observation calendar element after the navigation positioning system is invalid is inaccurate. And selecting laser point cloud homonymous characteristic points with a certain data volume from the point cloud data sets before and after failure by adopting a point cloud matching method.
Step 3: constructing a spatial position and posture information correction relation under a unified coordinate system by utilizing laser point cloud characteristic points, and obtaining corrected spatial position and posture information by adopting least square nonlinear solution;
and constructing the spatial position relation of the same-name feature point set between adjacent observation epochs by utilizing the conversion relation of the laser point cloud feature points among a laser radar scanner coordinate system, a carrier coordinate system, a local horizontal coordinate system and a geocentric ground solid coordinate. And taking the space position (longitude, latitude and altitude) and the posture (pitch angle, roll angle and yaw angle) of the observation epoch after the navigation positioning is invalid as unknown parameters to construct a position and posture correction equation. Because the equation is a complex nonlinear equation, a nonlinear least square estimation method is adopted for solving the equation, and the corrected spatial position (longitude, latitude and altitude) and posture (pitch angle, roll angle and yaw angle) are obtained.
Step 4: and reusing the corrected space position and posture information in the laser point cloud position calculation of the next observation epoch after failure, and repeating the laser movement measurement posture correction until the navigation positioning system recovers to be positioned accurately.
And reusing the corrected spatial position (longitude, latitude and altitude) and attitude (pitch angle, roll angle and yaw angle) for laser point cloud position calculation of the next observation epoch after failure. And selecting a certain number of laser point cloud characteristic points for correcting the observation epoch and the subsequent observation epoch, constructing a space position and posture information correction relation under a unified coordinate system by utilizing the laser point cloud characteristic points, obtaining space position and posture information by adopting least square nonlinear solution and using the space position and posture information for calculating the position of the laser point cloud, and repeating the steps until the navigation positioning system recovers to be positioned accurately.
In step S3, the corrected position and posture parameters are，/>，/>Correction for position and attitude parameters;
corrected longitude is，/>，/>Correction for longitude;
corrected latitude，/>，/>Is the correction of latitude;
height after correction，/>，/>Is the correction of the height;
corrected pitch angle，/>，/>Is the correction of the pitch angle;
corrected roll angle，/>，/>The correction of the roll angle is adopted;
corrected declination angle，/>，/>Is the correction of the deflection angle;
the specific form of the jacobian matrix is as follows:
a is an ellipsoid with a long radius of about 6378137 m, B is an ellipsoid with a short radius of about 6356752.3142 m, and the position and posture correction equation after the complexity of the matrix B is reduced is as follows:
；
；
；
the experimental analysis is carried out by adopting a vehicle-mounted laser mobile measurement system, the vehicle-mounted laser mobile measurement system consists of a laser scanner, an inertial navigation system and a global navigation satellite system, wherein the laser scanner carries out point cloud scanning, the inertial navigation system carries out attitude measurement, the global navigation satellite system carries out position measurement, and the instrument and equipment of the system carry out strict calibration. The data comprises 9 sets of 12 ten thousand rows each, and table 1 lists the correction before and after 20 rows before the first set of data. Since 0 observation epoch is started, the longitude, latitude, altitude, pitch angle, roll angle and yaw angle obtained by measurement are accumulated and are continuously increased along with time, the pose data is effectively corrected after being processed by the real-time pose correction method for laser point cloud matching, the corrected pose data is used for position calculation of the laser point cloud, the point cloud before navigation positioning failure correction is completely deviated from the real point cloud position, the point cloud after navigation positioning failure correction is basically overlapped with the real point cloud, and therefore the problem of short-term failure of the navigation positioning system in the laser point cloud matching real-time pose correction method provided by the invention is effectively solved.
Table 1 comparison of the first 20 rows of data before and after correction
。
The above embodiments are only for illustrating the technical aspects of the present invention, not for limiting the same, and although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may be modified or some or all of the technical features may be replaced with other technical solutions, which do not depart from the scope of the technical solutions of the embodiments of the present invention.
Claims (10)
1. A real-time pose correction method for laser point cloud matching is characterized by comprising the following steps:
step 1: determining an observation epoch before failure of the navigation positioning system and an observation epoch after failure, and acquiring the spatial position and posture information of the laser movement measurement system under different epochs;
step 2: selecting laser point cloud characteristic points through point cloud matching according to adjacent observation epochs before and after the failure of the navigation positioning system;
step 3: constructing a spatial position and posture information correction relation under a unified coordinate system by utilizing laser point cloud characteristic points, and obtaining corrected spatial position and posture information by adopting least square nonlinear solution;
step 4: and reusing the corrected space position and posture information in the laser point cloud position calculation of the next observation epoch after failure, and repeating the laser movement measurement posture correction until the navigation positioning system recovers to be positioned accurately.
2. The method for correcting the real-time pose of laser point cloud matching according to claim 1, wherein step 1 comprises:
step 1.1: determining a previous observation epoch for a navigation positioning system failureAcquiring epoch->Spatial position of the lower laser movement measurement system +.>And posture information->；
Step 1.2: determining an observation epoch after failure of a navigation positioning systemAcquiring epoch->Spatial position of the lower laser movement measurement system +.>And posture information->，/>Longitude, & ->Is a latitude,Is of height, & lt + & gt>Is pitch angle, < >>Is a roll angle>Is the angle of deflection, the->Is the observation epoch of the failure of the navigation positioning system.
3. The method for correcting the real-time pose of laser point cloud matching according to claim 2, wherein step 2 comprises:
step 2.1: acquiring previous observation epoch of failure of navigation positioning systemPoint cloud data set of laser movement measurement system +.>Observation epoch after failure of navigation positioning system>Point cloud data set of lower laser mobile measurement system +.>；
Step 2.2: determining observation epoch by adopting point cloud matching methodThe lower Point cloud dataset->And observation epoch->The lower Point cloud dataset->Is->。
4. The method for correcting the pose of laser point cloud matching in real time according to claim 3, wherein step 3 comprises:
step 3.1: constructing homonymous feature point sets among adjacent observation epochs according to the conversion relation among a laser radar scanner coordinate system, a carrier coordinate system, a local horizontal coordinate system and a geocentric earth fixed coordinateSpatial positional relationship of:
；
in the formula ,for the geocentric geodetic coordinate system feature point set coordinates, < ->Is the earth-centered coordinate of the standing point, < >>For the local horizontal coordinate system feature point set coordinates, < +.>For the coordinates of the feature point set of the carrier coordinate system, +.>For the coordinates of the feature point set of the laser radar scanner coordinate system,/-for>Is a longitude and latitude related rotation matrix, +.>For a gesture-dependent rotation matrix +.>For the installation of the calibration-dependent rotation matrix, +.>For the installation of the calibration translation, +.> and />Are obtained by means of mounting calibration.
5. The method for correcting the real-time pose of laser point cloud matching according to claim 4, wherein step 3 comprises:
step 3.2: the space position relation between the coordinate system of the laser radar scanner and the earth-centered earth-fixed coordinate system is utilized to observe the epochLongitude of the lower laser movement measuring system +.>Latitude->Height->Pitch angle->Roll angle->Deviation angle->As an unknown parameter->Constructing a position and posture correction equation:
；
in the formula ,for unknown parameter initial value, ++>，/>To correct the longitude before correction,To correct the latitude before->To correct the front height->To correct the front pitch angle->To correct the front roll angle->To correct the forward yaw angle.
6. The method for correcting the real-time pose of laser point cloud matching according to claim 5, wherein step 3 comprises:
step 3.3: and (3) carrying out nonlinear solution on the position and posture correction equation by adopting a least square indirect adjustment method:
the position and posture correction equation is a relation in ideal state, and unknown parameters in actual conditionsAndthere is a residual vector L between:
；/>；
in the formula ,is a linearized coefficient matrix, also called jacobian matrix, ">The observation vector after linearization is also a residual vector, and represents the residual of each data point.
7. The method for correcting the real-time pose of laser point cloud matching according to claim 6, wherein step 3.1 comprises:
；
。
8. the method for correcting the real-time pose of laser point cloud matching according to claim 7, wherein step 3.2 comprises: the position and attitude correction equations are specifically:
；
；
；
in the formula ,，/>，/>is the lower coordinate of the geocentric earth fixed coordinate system, < + >>，/>，/>For the lower coordinate of the carrier coordinate system,/->，/>，/>The ground center of the station is the ground center of the station.
9. The method for correcting the pose of laser point cloud matching in real time according to claim 8, wherein step 3.2 comprises:
converting the coordinates of a station point into longitude by using the related parameters of a reference ellipsoidLatitude->Elevation->Representation is made, unify->，/>，/>，/>，/>，/>Solving as unknown parameters to reduce the complex process of the B matrixDegree:
；
；
；
；
wherein ,，/>the second eccentricity of the ellipsoid, a is the long radius of the ellipsoid and b is the short radius of the ellipsoid.
10. The method for correcting the real-time pose of laser point cloud matching according to claim 9, wherein step 4 comprises:
step 4.1: using corrected observation epochsLongitude of->Latitude->Height->Pitch angle->Roll angle->Deviation angle->Reusing the laser point cloud position calculation of the next observation epoch after failure to obtain a corrected point cloud data set +.>；
Step 4.2: for observation epoch and />And (3) repeating the step (2) and the step (3) until the navigation positioning system of the laser movement measurement system is recovered.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311226492.1A CN116973895B (en) | 2023-09-22 | 2023-09-22 | Real-time pose correction method for laser point cloud matching |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311226492.1A CN116973895B (en) | 2023-09-22 | 2023-09-22 | Real-time pose correction method for laser point cloud matching |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116973895A true CN116973895A (en) | 2023-10-31 |
| CN116973895B CN116973895B (en) | 2023-12-12 |
Family
ID=88479952
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311226492.1A Active CN116973895B (en) | 2023-09-22 | 2023-09-22 | Real-time pose correction method for laser point cloud matching |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116973895B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160063717A1 (en) * | 2014-08-26 | 2016-03-03 | Kabushiki Kaisha Topcon | Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor |
| US20180074176A1 (en) * | 2016-09-14 | 2018-03-15 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Motion compensation method and apparatus applicable to laser point cloud data |
| CN109297510A (en) * | 2018-09-27 | 2019-02-01 | 百度在线网络技术（北京）有限公司 | Relative pose scaling method, device, equipment and medium |
| CN109917356A (en) * | 2019-03-13 | 2019-06-21 | 武汉际上导航科技有限公司 | A kind of airborne lidar systematic error scaling method |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
| CN111505606A (en) * | 2020-04-14 | 2020-08-07 | 武汉大学 | Method and device for calibrating relative pose of multi-camera and laser radar system |
| CN113311411A (en) * | 2021-04-19 | 2021-08-27 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| US20230033404A1 (en) * | 2021-07-30 | 2023-02-02 | The Hong Kong Polytechnic University | 3d lidar aided global navigation satellite system and the method for non-line-of-sight detection and correction |
| CN116203544A (en) * | 2023-02-21 | 2023-06-02 | 西南交通大学 | Method, device and medium for back-and-forth detection and return uncontrolled self-checking of mobile measurement system |
-
2023
- 2023-09-22 CN CN202311226492.1A patent/CN116973895B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160063717A1 (en) * | 2014-08-26 | 2016-03-03 | Kabushiki Kaisha Topcon | Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor |
| US20180074176A1 (en) * | 2016-09-14 | 2018-03-15 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Motion compensation method and apparatus applicable to laser point cloud data |
| CN109297510A (en) * | 2018-09-27 | 2019-02-01 | 百度在线网络技术（北京）有限公司 | Relative pose scaling method, device, equipment and medium |
| CN109917356A (en) * | 2019-03-13 | 2019-06-21 | 武汉际上导航科技有限公司 | A kind of airborne lidar systematic error scaling method |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
| CN111505606A (en) * | 2020-04-14 | 2020-08-07 | 武汉大学 | Method and device for calibrating relative pose of multi-camera and laser radar system |
| CN113311411A (en) * | 2021-04-19 | 2021-08-27 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| US20230033404A1 (en) * | 2021-07-30 | 2023-02-02 | The Hong Kong Polytechnic University | 3d lidar aided global navigation satellite system and the method for non-line-of-sight detection and correction |
| CN116203544A (en) * | 2023-02-21 | 2023-06-02 | 西南交通大学 | Method, device and medium for back-and-forth detection and return uncontrolled self-checking of mobile measurement system |
Non-Patent Citations (2)
| Title |
|---|
| 冯雁飞;张云生;杜守基;: "航空倾斜摄影系统多相机姿态安装误差检校", 测绘工程, no. 08, pages 45 - 52 * |
| 吴献文等: "基于3D SLAM移动式激光扫描技术的地上地下空间一体化测绘", 地矿测绘, vol. 37, no. 3, pages 6 - 11 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116973895B (en) | 2023-12-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200103530A1 (en) | Method for extracting elevation control point with assistance of satellite laser altimetry data | |
| JP4448187B2 (en) | Image geometric correction method and apparatus | |
| KR100728377B1 (en) | Method for real-time updating gis of changed region vis laser scanning and mobile internet | |
| CN107886531B (en) | Virtual control point acquisition method based on laser ranging and object space matching | |
| CN102741706A (en) | Method for geo-referencing an imaged area | |
| CN111398980B (en) | Method and device for processing airborne LiDAR data | |
| KR20190026452A (en) | A method of automatic geometric correction of digital elevation model made from satellite images and provided rpc | |
| CN110006452B (en) | Relative geometric calibration method and system for high-resolution six-size wide-view-field camera | |
| CN113538595B (en) | Method for improving geometric precision of remote sensing stereo image by using laser height measurement data in auxiliary manner | |
| CN113960545B (en) | Satellite-borne SAR field-free geometric calibration method and system based on symmetric geometric configuration constraint | |
| CN110363758B (en) | Optical remote sensing satellite imaging quality determination method and system | |
| CN102346033A (en) | Direct positioning method and system based on satellite observation angle error estimation | |
| CN111798523A (en) | Satellite camera on-orbit calibration attitude determination and remote sensing image geometric positioning method and system | |
| CN113267794A (en) | Antenna phase center correction method and device with base line length constraint | |
| CN110068313B (en) | Digital zenith instrument orientation method based on projection transformation | |
| CN110986888A (en) | Aerial photography integrated method | |
| Li et al. | Improve the ZY-3 height accuracy using ICESat/GLAS laser altimeter data | |
| CN105571598A (en) | Satellite laser altimeter footprint camera pose measuring method | |
| CN116973895B (en) | Real-time pose correction method for laser point cloud matching | |
| CN115683170B (en) | Calibration method based on radar point cloud data fusion error | |
| CN117031513A (en) | Real-time monitoring and positioning method, system and device for roads and accessories | |
| KR20190086951A (en) | System and Method for Calibration of Mobile Mapping System Using Terrestrial LiDAR | |
| CN111044076B (en) | Geometric calibration method for high-resolution first-number B satellite based on reference base map | |
| CN114004949A (en) | Airborne point cloud assisted mobile measurement system arrangement parameter calibration method and system | |
| CN113126058A (en) | Memory, control method and device for airborne laser radar system |
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 |