Autonomous positioning method of unmanned equipment with augmented live-action three-dimensional model

Abstract

The invention discloses a live-action three-dimensional model enhanced unmanned equipment autonomous positioning method, and the method comprises the steps: firstly, building a key frame sub-map through searching a nearest neighbor key frame and a nearest neighbor convex hull, and then building a point cloud sub-map set of all key frames, overlapping of the laser point cloud of the current frame and the live-action three-dimensional model is increased, and the matching speed is increased; then, an inertial attitude resolving method based on high-order integration is used to realize a high-frequency inertial odometer and accurate laser point cloud distortion correction; and finally, fusing the matching result of the inertial odometer and the live-action three-dimensional model by using a nonlinear geometric observer to realize high-precision unmanned equipment positioning.

Description

Technical Field

[0001] The invention belongs to the field of new generation information technology, and specifically is an autonomous positioning method for unmanned equipment enhanced by a real-scene three-dimensional model. Background Art

[0002] Industrial plants have complex environments and numerous equipment, so inspection work is crucial for safe production and equipment maintenance in industrial plants. Traditional inspection methods mainly rely on manual inspections, but they have problems such as high personnel risks, low efficiency, and high costs. At the same time, it is difficult to meet the requirements of high efficiency and comprehensiveness of chemical plant inspections. With the development of unmanned equipment and positioning technology, unmanned equipment inspections have become an important means to replace manual inspections. Autonomous positioning and navigation of unmanned equipment is the key technology for unmanned equipment to achieve autonomous and efficient inspections. For outdoor environments, GNSS is already very mature. However, in complex industrial plant environments, due to factors such as multipath effects and building obstructions, GNSS cannot provide accurate location services. In order to ensure the safety of industrial plant workers and the normal operation of equipment and production, it is extremely important to achieve high-precision autonomous positioning of unmanned equipment for plant inspections.

[0003] In recent years, with the rapid development of real-life 3D and the maturity of technology, real-life 3D models have been widely used. Real-life 3D models can truly restore the complex environment of industrial plants through high-precision photogrammetry, laser scanning and other technologies, generate detailed 3D digital models, provide spatial references in the real environment for unmanned equipment, and perform autonomous positioning in 3D space. However, the data volume of real-life 3D models is huge, and the real-time matching and processing speed are slow; the single-sensor LiDAR point cloud matching is not robust in the case of environmental degradation and many obstructions, and cannot achieve high-precision positioning of unmanned equipment. Summary of the invention

[0004] In view of the problems existing in the background technology, the present invention proposes an autonomous positioning method for unmanned equipment enhanced by a real-scene three-dimensional model.

[0005] Technical solution:

[0006] A method for autonomous positioning of unmanned equipment enhanced by a real-scene three-dimensional model, in which three-dimensional laser point cloud data of the unmanned equipment is obtained by laser radar scanning during the movement of the unmanned equipment;

[0007] The positioning method includes the following steps:

[0008] S1. Based on the real-life 3D model, a keyframe submap S is constructed by searching for the nearest neighbor keyframe and the nearest neighbor convex hull. k ; Construct all key frame point cloud submap sets K k , which increases the overlap between the laser point cloud of the current frame and the real-scene 3D model, speeding up the matching speed;

[0009] S2. Use a high-order integral inertial attitude solution method to achieve inertial odometer and laser point cloud distortion correction;

[0010] S3. Use nonlinear geometric observer to fuse the inertial odometer and the real-scene 3D model matching results to achieve unmanned equipment positioning.

[0011] Preferably, in S1, the key frame submap S k The construction steps are:

[0012] S1-1, select the key frame closest to the current pose from the historical key frames of the global map through nearest neighbor search;

[0013] S1-2, stitch the key frame with the corresponding point cloud data to obtain the submap for current frame matching as the key frame submap S k .

[0014] Preferably, each keyframe is linked to its corresponding point cloud scan in the form of a key-value pair.

[0015] Preferably, in S2, the position and posture of the inertial sensor IMU at time i in the real-scene three-dimensional model coordinate system is solved by the following formula:

[0016]

[0017] in, They are the quaternions of the position, velocity, and attitude of the inertial sensor IMU at time i in the real-scene 3D model coordinate system; i = 1, ..., M represents time t k-1 and t k The index of the M IMU measurements between Δt i Represents the time interval between IMU observations i and i-1; is an estimate of the linear acceleration, represents angular acceleration; represents the rotation matrix; Represent the accelerometer measurement and gyroscope measurement values ​​of the IMU respectively; Represents a matrix-tensor product operation.

[0018] Specific:

[0019]

[0020] Preferably, the laser point cloud distortion correction is performed by the following formula:

[0021]

[0022] In the formula, They are the quaternions of position and attitude after distortion correction; use Will Project to the map coordinate system;

[0023] Complete point cloud After projecting all the points in , the distortion-corrected full-frame 3D laser point cloud is obtained.

[0024] Preferably, S3 specifically includes:

[0025] S3-1, 3D laser point cloud-real scene 3D model matching; performed by the following formula:

[0026]

[0027] in, represents the registration pose increment; d j Represents the scanned 3D laser point cloud and point cloud in real 3D model The distance between corresponding point pairs after iterative transformation; and is the covariance matrix corresponding to the corresponding point cluster; ΔT k represents the transformation matrix to be optimized, Indicates ΔT k The transpose of Represents the transformation from 3D laser point cloud to real-scene 3D model;

[0028] S3-2, fusion state estimation based on nonlinear geometric observer is performed by the following formula:

[0029]

[0030] in, Respectively represent the quaternion, velocity, and acceleration of the position, angular velocity, and attitude of the unmanned equipment fusion state estimation; γ l ∈{1,…,5} is a constant gain coefficient, is the interval between two GICP attitudes, represents the real part of the quaternion, represents the imaginary part of the quaternion, Represents relative posture, p e represents the position error; the attitude and position error between the prediction obtained by IMU propagation and the observation obtained by matching the real scene 3D model is and Represents the rotation matrix estimated from the current pose.

[0031] Preferably, in S3, the matching algorithm between the three-dimensional laser point cloud and the real-scene three-dimensional model adopts an NDT method based on local point cloud mean variance matching, a feature-based matching method, or a deep learning-based matching method.

[0032] Preferably, in S3, an extended Kalman filter EKF, a factor graph or a particle filter PF is used to fuse the matching results of the inertial odometer and the real-scene three-dimensional model.

[0033] Beneficial effects of the present invention

[0034] The present invention proposes a sub-map construction method based on keyframes. First, the keyframe closest to the current posture is selected from the historical keyframes of the global map through nearest neighbor search, and then the point cloud data corresponding to these keyframes are spliced ​​together to generate a sub-map for matching the current frame, so that the overlap between the laser point cloud of the current frame and the real-scene three-dimensional model is increased, the calculation overhead is reduced, and the matching speed is accelerated.

[0035] The present invention proposes a laser-inertial fusion positioning method based on real-scene three-dimensional model matching. First, on the basis of the traditional attitude solution model, the third-order time integral of linear acceleration and angular acceleration is added to provide accurate attitude for the motion distortion removal of three-dimensional laser point cloud, and realize high-frequency inertial odometer and precise point cloud distortion correction; then, the attitude estimation of inertial odometer and real-scene three-dimensional model matching is fused by nonlinear geometric observer, and the current posture, angular velocity deviation and acceleration deviation of unmanned equipment are corrected by gain coefficient to reduce the accumulation of system errors and ensure the accuracy of position estimation and velocity estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 A flowchart of the present invention

[0037] Figure 2 Flow chart of the key frame sub-map construction method of the present invention

[0038] Figure 3 This is the principle diagram of point cloud dedistortion of the present invention.

[0039] Figure 4 Schematic diagram of the matching process between 3D laser point cloud and real scene 3D model DETAILED DESCRIPTION

[0040] The present invention will be further described below in conjunction with embodiments, but the protection scope of the present invention is not limited thereto:

[0041] Combination Figure 1 As shown in the flowchart, the positioning method of the present invention comprises the following steps:

[0042] S1. Based on the real-life 3D model, a keyframe submap S is constructed by searching for the nearest neighbor keyframe and the nearest neighbor convex hull. k ; Construct all key frame point cloud submap sets K k , which increases the overlap between the laser point cloud of the current frame and the real-scene 3D model, speeding up the matching speed;

[0043] S2. Use a high-order integral inertial attitude solution method to achieve high-frequency inertial odometer and accurate laser point cloud distortion correction;

[0044] S3. Use nonlinear geometric observer to fuse the inertial odometer and the real-scene 3D model matching results to achieve unmanned equipment positioning.

[0045] Specific:

[0046] S1 Submap construction method based on keyframes

[0047] Traditional radius-based search methods may require distance calculations for hundreds of thousands of points in a real-life 3D model. Even if an incremental octree data structure is used, the computational overhead of this process is very high. Therefore, the present invention proposes a sub-map construction method based on key frames, such as Figure 2 As shown in Figure 1. A history of keyframes is kept for search, where each keyframe is linked to its corresponding point cloud scan in the form of a key-value pair. The local submap used for scan-to-submap matching is generated by splicing the point clouds corresponding to the keyframes in the selected subset, rather than directly retrieving local points within a certain radius of the current position of the unmanned device. Even after long traversals, typically only a few hundred points are involved, and more consistent computational performance is provided, reducing the possibility of frame drops.

[0048] In addition, the keyframe-based approach builds a more inclusive submap than the radius-based search method. The size of the submap derived from the keyframe point cloud depends entirely on the range of the lidar sensor, rather than a predetermined distance, and the derived submap can have a larger overlap with the current scan, which helps to better match the point cloud to the real 3D model.

[0049] Select keyframes by nearest neighbor search: To construct the submap S k , stitching the corresponding point clouds from the selected subset of environment keyframes. Let K k is the set of all keyframe point clouds such that Define submap S k Scan Q for the K nearest neighbor keyframes k and L nearest neighbor convex hull scans H k splicing, that is, S k =Q k ∪H k,This combination reduces the overall trajectory drift by maximizing the scan-to-map overlap and provides the system with ,environmental features at multiple scales for alignment.

[0050] Traditional radius-based search methods may need to calculate the distance of hundreds of thousands of points in the real-life 3D model, and the computational overhead of this process is very high. Therefore, the present invention proposes a sub-map construction method based on keyframes. First, the keyframe closest to the current pose is selected from the historical keyframes of the global map through the nearest neighbor search, and then the point cloud data corresponding to these keyframes are spliced ​​together to generate a sub-map for matching the current frame, so that the overlap between the laser point cloud of the current frame and the real-life 3D model is increased, the computational overhead is reduced, and the matching speed is accelerated.

[0051] S2 Inertial attitude solution method based on high-order integration

[0052] 2.1 Inertial odometer

[0053] During the movement of unmanned equipment, due to the working mode of the rotating array scanning of the mechanical laser radar, the points captured by the laser radar will be distorted, thus affecting the point cloud registration and attitude estimation. Due to the high sampling frequency of the inertial measurement unit (IMU), the IMU attitude solution can be used to estimate the posture of the unmanned equipment at the time of three-dimensional laser point cloud acquisition, thereby correcting the motion distortion of the three-dimensional laser point cloud. Different from the uniform motion model used in other works, the present invention adds the third-order time integral of linear acceleration and angular acceleration on the basis of the traditional attitude solution model, thereby providing accurate attitude for the removal of motion distortion of the three-dimensional laser point cloud.

[0054] Assume that the coordinate system of the real-world 3D model is M, and the coordinate system of the unmanned device is B, located at its center of gravity, where the X axis points to the front of the unmanned device, the Y axis points to the left of the unmanned device, and the Z axis points to the top of the unmanned device. The coordinate system of the IMU is I, the coordinate system of the laser radar is L, and the state vector X of the unmanned device at time k is k Defined as:

[0055]

[0056] where p M ∈R 3 is the location of the unmanned device, q M is the quaternion representing the attitude of the unmanned equipment, v M ∈R 3 is the speed of the unmanned equipment, b a ∈R 3 is the offset of the accelerometer, b ω ∈R 3is the offset of the gyroscope. The IMU's accelerometer measurement and the gyroscope and is defined as:

[0057]

[0058] In the raw sensor measurement and In the i and white noise n i , g is the gravity vector after rotation. Next, use high-order integral to estimate the inertial navigation posture. Suppose the 3D laser point cloud scans The collection time is t k i=1,…,M represents the time t k-1 and t k The index of the M IMU measurements between k represents the time interval between IMU observations i and i-1, and equations (4) to (6) are used to solve the position and posture of inertial sensor i in the real-scene 3D model coordinate system at that moment:

[0059]

[0060] in is an estimate of the linear acceleration, is the angular acceleration. and The special Euclidean transformation group The discrete time trajectory rough estimate during the point cloud scanning of this frame is defined. Then, according to the point cloud Every point timestamp, find the rough pose solution that is earlier than and closest to the timestamp in the M IMU measurements to further infer the pose at the time of acquisition of the point and

[0061] 2.2 Point cloud distortion removal

[0062] For a frame of 3D point cloud output by the laser radar It contains N scanning points. The nth scanning point is represented as This point is relative to the scan start time t k The interval is For the point that precedes and is closest to the point in time Inertial odometer pose estimation and IMU measurement at acquisition time i-1 Substituting the time interval t between the scanning point and the corresponding inertial measurement into the continuous time parameterized equations of (7) and (8), we can obtain the point Accurate estimation of the acquisition pose.

[0063]

[0064] use and Will Projected to the map coordinate system. After projecting all the points in , the distortion-corrected full-frame 3D laser point cloud is obtained.

[0065] S3 integrates the real-scene 3D model matching results with the laser-inertial positioning of the inertial odometer

[0066] Although inertial attitude estimation can provide short-term motion estimation of unmanned equipment, when using consumer-grade or low-cost industrial-grade inertial navigation equipment for inertial navigation attitude estimation, the observation offset and noise of wandering changes will seriously affect the accuracy of attitude solution, and ultimately cause the attitude estimation to diverge rapidly over time. Matching the point cloud collected by LiDAR with the real-life 3D model can provide high-precision observations to correct the accumulated drift of the inertial odometer.

[0067] After marking the navigation destination on the real-scene 3D model, the real-scene 3D model is provided to the positioning module of the unmanned equipment, so that the positioning of the unmanned equipment can be unified in the same coordinate system, and the unmanned equipment is also endowed with the ability of point-to-point navigation. Therefore, the present invention introduces a global positioning method based on the real-scene 3D model to perform laser-inertial fusion positioning to achieve positioning enhancement.

[0068] 3.1 3D laser point cloud-real scene 3D model matching

[0069] After completing the current frame Distortion compensation and the initial transformation After solving, the present invention uses the generalized-iterative closet point algorithm (Generalized-Iterative Closest Point, G-ICP) implemented by the Fast Library for Approximate Nearest Neighbors (FLANN) algorithm to perform Realistic 3D model The specific process of 3D laser point cloud-real scene 3D model matching is as follows:

[0070]

[0071] in, Denotes the registration pose increment. d j Represents the scanned 3D laser point cloud and point cloud in real 3D model The distance between corresponding point pairs after iterative transformation. and is the corresponding covariance matrix of the corresponding point cluster.

[0072] Figure 4 The process of using 3D laser point cloud-real scene 3D model matching to perform initial positioning of unmanned equipment and establish the reference coordinate system for positioning is demonstrated.

[0073] 3.2 Fusion state estimation based on nonlinear geometric observer

[0074] Complete the transformation from 3D laser point cloud to real-life 3D model After solving the problem, the present invention uses a nonlinear geometric observer to realize the fusion of real-scene 3D model matching global positioning and IMU odometer to complete the motion state of unmanned equipment The specific process of using nonlinear geometric observer to fuse inertial odometer and real scene 3D model matching is as follows:

[0075]

[0076] Among them, γ l ∈{1,…,5} is a constant gain coefficient, is the interval between two GICP attitudes, and the attitude and position errors between the prediction obtained by IMU propagation and the observation obtained by matching the real scene 3D model are and Formula (11) is combined with the attitude error to correct the current attitude estimate, ensuring that the estimated value is closer to the true value. Then, the attitude error is used to correct the angular velocity deviation through formula (12), the position error is combined with the current position estimate through formula (13), and the position error is used to correct the velocity estimate through formula (14). represents the rotation matrix obtained by the current attitude estimation. Equation (15) corrects the acceleration deviation by combining the position error and attitude estimation to reduce the accumulation of system errors and ensure the accuracy of position estimation and velocity estimation.

[0077] The present invention proposes a laser-inertial fusion positioning method based on real-scene three-dimensional model matching. First, a high-order integral inertial attitude solution is used. On the basis of the traditional attitude solution model, the present invention adds the third-order time integral of linear acceleration and angular acceleration, thereby providing accurate attitude for the motion distortion removal of three-dimensional laser point cloud, realizing high-frequency inertial odometer and precise point cloud distortion correction; then, a nonlinear geometric observer is used to fuse the attitude estimation of the inertial odometer and the real-scene three-dimensional model matching, and the gain coefficient is used to correct the current posture, angular velocity deviation and acceleration deviation of the unmanned equipment to reduce the accumulation of system errors and ensure the accuracy of position estimation and velocity estimation.

[0078] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in similar ways, but they will not deviate from the spirit of the present invention or exceed the scope defined by the appended claims.

Claims

1. An autonomous positioning method for unmanned equipment enhanced by a real-scene 3D model, in which 3D laser point cloud data of the unmanned equipment is obtained by laser radar scanning during the movement of the unmanned equipment; Features The positioning method includes the following steps: S1. Based on the real-life 3D model, a keyframe submap S is constructed by searching for the nearest neighbor keyframe and the nearest neighbor convex hull. k ; Construct all key frame point cloud submap sets K k , which increases the overlap between the laser point cloud of the current frame and the real-scene 3D model, speeding up the matching speed; S2. Use a high-order integral inertial attitude solution method to achieve inertial odometer and laser point cloud distortion correction; S3. Use nonlinear geometric observer to fuse the inertial odometer and the real-scene 3D model matching results to achieve unmanned equipment positioning.

2. The method according to claim 1, characterized in that In S1, the keyframe submap S k The construction steps are: S1-1, select the key frame closest to the current pose from the historical key frames of the global map through nearest neighbor search; S1-2, stitch the key frame with the corresponding point cloud data to obtain the submap for current frame matching as the key frame submap S k .

3. The method according to claim 2, characterized in that Each keyframe is linked to its corresponding point cloud scan in the form of a key-value pair.

4. The method according to claim 1, characterized in that In S2, the position and posture of the inertial sensor IMU at time i in the real-scene 3D model coordinate system is solved by the following formula: in, They are the quaternions of the position, velocity, and attitude of the inertial sensor IMU at time i in the real-scene 3D model coordinate system; i = 1, ..., M represents time t k-1 and t k The index of the M IMU measurements between Δt i Represents the time interval between IMU observations i and i-1; is an estimate of the linear acceleration, represents angular acceleration; represents the rotation matrix; Represent the accelerometer measurement and gyroscope measurement values ​​of the IMU respectively; Represents a matrix-tensor product operation.

5. The method according to claim 4, characterized in that:

6. The method according to claim 4, characterized in that The laser point cloud distortion correction is performed by the following formula: In the formula, They are the quaternions of position and attitude after distortion correction; use Will Project to the map coordinate system; Complete point cloud After projecting all the points in , the distortion-corrected full-frame 3D laser point cloud is obtained.

7. The method according to claim 1, characterized in that S3 specifically includes: S3-1, 3D laser point cloud-real scene 3D model matching; performed by the following formula: in, represents the registration pose increment; d j Represents the scanned 3D laser point cloud and point cloud in real 3D model The distance between corresponding point pairs after iterative transformation; and is the covariance matrix corresponding to the corresponding point cluster; ΔT k represents the transformation matrix to be optimized, Indicates ΔT k The transpose of Represents the transformation from 3D laser point cloud to real-scene 3D model; S3-2, fusion state estimation based on nonlinear geometric observer is performed by the following formula: in, Respectively represent the quaternion, velocity, and acceleration of the position, angular velocity, and attitude of the unmanned equipment fusion state estimation; γ l ∈{1,…,5} is a constant gain coefficient, is the interval between two GICP attitudes, represents the real part of the quaternion, represents the imaginary part of the quaternion, Represents relative posture, p e represents the position error; the attitude and position error between the prediction obtained by IMU propagation and the observation obtained by matching the real scene 3D model is and Represents the rotation matrix estimated from the current pose.

8. The method according to claim 1, characterized in that In S3, the matching algorithm between the 3D laser point cloud and the real-scene 3D model adopts an NDT method based on local point cloud mean variance matching, a feature-based matching method, or a deep learning-based matching method.

9. The method according to claim 1, characterized in that In S3, the extended Kalman filter EKF, factor graph or particle filter PF is used to fuse the matching results of the inertial odometer and the real-scene 3D model.

Images