Global consistency alignment algorithm, system, device and application based on equipment attitude information and laser radar observation fusion

Abstract

The invention discloses a global consistency alignment algorithm, system and device based on equipment attitude information and laser radar observation fusion and application, and belongs to the technical field of intelligent driving, indoor surveying and mapping and robot autonomous navigation, and the global consistency alignment algorithm, system and device based on equipment attitude information and laser radar observation fusion are obtained by fusing data of an inertial measurement unit IMU and a laser radar LiDAR and combining a particle filter and an iterative closest point algorithm ICP. According to the method, high-precision, high-robustness and globally consistent 3D color map construction is achieved, the method is suitable for the fields of intelligent driving, indoor surveying and mapping, robot autonomous navigation and the like, a high-quality 3D map can be constructed in a complex and changeable indoor scene, system accumulative errors are effectively reduced, and the accuracy of pose estimation is improved; the particle filter and the ICP algorithm are combined, so that accurate fusion of multi-sensor data is realized, and the robustness and reliability of the system are enhanced; a low-cost two-dimensional laser radar and rotary table scheme is adopted, so that the equipment cost is reduced, and meanwhile, the high-precision three-dimensional reconstruction capability is kept.

Description

Technical Field

[0001] The present invention belongs to the technical fields of intelligent driving, indoor mapping and robot autonomous navigation, and in particular relates to a global consistency alignment algorithm, system, device and application based on the fusion of device posture information and lidar observation. Background Art

[0002] Global navigation satellite systems generally do not work effectively in indoor environments, leading to a growing demand for high-precision indoor navigation. Obtaining high-precision maps is one of the key challenges in achieving high-precision indoor navigation. RGB-D cameras can provide three-dimensional RGB points, RGB images, and depth information, generating three-dimensional maps through dense reconstruction. However, in low-contrast environments, traditional RGB-D cameras have difficulty obtaining high-quality three-dimensional maps because these cameras rely on structural features in the image for registration. This approach is significantly less effective in environments lacking obvious features. To overcome these limitations, Light Detection and Ranging (LiDAR) has been introduced. It can measure distance more accurately, thus achieving higher accuracy when constructing three-dimensional maps. It is also independent of lighting conditions and can work in various environments. Multi-sensor fusion becomes necessary. A single sensor often cannot meet the needs of complex environments, so it is necessary to fuse data from multiple sensors. By fusing device posture information and LiDAR observation data, the overall accuracy and robustness of the system can be improved. Particle filter (PF) is a commonly used nonlinear filtering method suitable for handling nonlinear and non-Gaussian problems.

[0003] Based on this, the present invention designs a global consistency alignment algorithm, system, device and application based on the fusion of device posture information and lidar observation to solve the above problems. Summary of the Invention

[0004] The purpose of the present invention is to solve the problem that traditional RGB-D cameras have difficulty in obtaining high-quality three-dimensional maps because these cameras rely on structural features in the image for alignment, which is greatly reduced in environments lacking obvious features. A global consistency alignment algorithm, system, device and application based on the fusion of device posture information and lidar observations are proposed.

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

[0006] A global consistency alignment algorithm based on the fusion of device posture information and lidar observations includes the following steps:

[0007] Step 1: Update the device posture. Update the device posture through gyroscope measurements, expressed as a 2D transformation matrix T k , and according to the posture T at the previous moment k-1And the relative motion ΔT calculates the current posture:

[0008] T k =T k-1 ΔT

[0009] Where ·ΔT is calculated from the angular velocity and acceleration measurements of the gyroscope using the following formula:

[0010]

[0011] Where θ is the rotation angle, Δx and Δy are the translation amounts;

[0012] The gyroscope's measurements include angular velocity ω and acceleration a, which are integrated to calculate the rotation angle θ and translation Δx and Δy:

[0013] θ=∫ωdt,Δx=∫a x dt,Δy=∫a y dt

[0014] Where a x and a y are the components of acceleration in the x and y directions;

[0015] Step 2: ICP algorithm performs inter-frame registration;

[0016] Step 3: Particle filter (PF) performs global pose estimation;

[0017] Step 4: Global consistency alignment. Through the above steps 1 to 3, each local point cloud is gradually registered into the global framework to build a consistent large-scale 3D point cloud map.

[0018] As a further description of the above technical solution:

[0019] The step 2 comprises the following steps:

[0020] Step 2.1, initial alignment, using the rough device posture obtained from IMU data as the initial transformation matrix T0;

[0021] Step 2.2, nearest neighbor search, for each point p in the point cloud P i , find the corresponding nearest point q in the point cloud Q i ;

[0022] The nearest neighbor search uses a KD-Tree data structure to improve search efficiency;

[0023] Step 2.3, rigid transformation estimation, estimates the rigid transformation [R, t] that minimizes the point set alignment error by minimizing the cost function E(R, t). The expression of the cost function E(R, t) is:

[0024]

[0025] Where R is the rotation matrix and t is the translation vector;

[0026] The rigid transformation [R,t] is solved by the singular value decomposition (SVD) method. The specific steps are as follows:

[0027] Calculate the centroid of point clouds P and Q and

[0028]

[0029] Calculate the covariance matrix H:

[0030]

[0031] Perform singular value decomposition on H to obtain U, ∑, and V:

[0032] H=U∑V T

[0033] Calculate the rotation matrix R and translation vector t:

[0034]

[0035] Step 2.4, iterative refinement, update the transformation matrix T until the registration error converges, and use T to refine the device pose;

[0036] T k =T k ·T.

[0037] As a further description of the above technical solution:

[0038] The step 3 comprises the following steps:

[0039] Step 3.1, Initialization: Generate N particles, representing the possible postures T of the device i ~p(T0);

[0040] Step 3.2, importance sampling, based on the device motion model and measurement likelihood function p(z k |T i ), update the particle weight w i , the measurement likelihood function p(z k |T i ) is:

[0041]

[0042] Where z k is the sensor measurement value, h(T i) is the observation model, σ is the noise parameter;

[0043] The observation model h(T i ) is:

[0044] h(T i )=T i ·p i

[0045] Where p i is a point in the point cloud P;

[0046] Step 3.3, resampling, according to the weight w i Resampling generates new particles and suppresses particle degradation. The resampling strategy is:

[0047] When the effective number of particles When it is lower than the set threshold, resampling is performed;

[0048] After resampling, the weights of the particles are uniformly set to

[0049] The resampling adopts a systematic resampling method, and the specific steps are as follows:

[0050] Calculate cumulative weight

[0051] Generate uniformly distributed random numbers u~U(0,1);

[0052] Choose to meet c i Particles with ≥u are resampled;

[0053] Step 3.4, pose estimation, calculate the final pose estimate T of the current time step by the weighted average of the particles k , the calculation formula is:

[0054]

[0055] Where w i is the weight of the particle, T i is the particle's posture.

[0056] As a further description of the above technical solution:

[0057] In step 4, global consistency alignment is achieved through the following steps:

[0058] The local point cloud P k Register to the global point cloud Q;

[0059] Use the ICP algorithm to optimize the alignment error between the local point cloud and the global point cloud;

[0060] Update the global point cloud Q to Q∪Pk .

[0061] A global consistency alignment system based on the fusion of device posture information and lidar observations, including:

[0062] A multi-sensor fusion system is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion system is a four-directional camera used to obtain RGB images in four directions;

[0063] Adjust the device attitude system, including a six-degree-of-freedom gyroscope, for adjusting the device attitude;

[0064] Particle filter, used to fuse multi-sensor data and estimate the global posture of the device;

[0065] LiDAR, including two-dimensional LiDAR and turntable, used to measure the three-dimensional position of surrounding objects;

[0066] The laser radar measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values refer to azimuth, distance, and turntable azimuth. The specific formula is:

[0067] x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ)

[0068] Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

[0069] As a further description of the above technical solution:

[0070] The global consistency alignment system transmits the acquired three-dimensional map to the user through the communication module and receives remote control instructions from the user;

[0071] The global consistency alignment system includes a data preprocessing module, a map construction module and a visualization module;

[0072] The data preprocessing module is used to filter and remove noise from the sensor data;

[0073] The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is:

[0074]

[0075] Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix;

[0076] The map construction module is used to register the local point cloud into the global map;

[0077] The map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map;

[0078] The visualization module is used to display the three-dimensional map and the position information of the device in real time;

[0079] The visualization module uses OpenGL or Unity engine to achieve real-time rendering of three-dimensional maps.

[0080] A global consistency alignment device based on the fusion of device posture information and lidar observations, comprising:

[0081] A multi-sensor fusion device is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion device is a four-directional camera used to obtain RGB images in four directions;

[0082] A device for adjusting the device's attitude, including a six-degree-of-freedom gyroscope, for adjusting the device's attitude;

[0083] A particle filter device is used to fuse multi-sensor data and estimate the global posture of the device;

[0084] A laser radar device, including a two-dimensional laser radar and a turntable, for measuring the three-dimensional position of surrounding objects;

[0085] The laser radar device measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values are azimuth, distance, and turntable azimuth. The specific formula is:

[0086] x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ)

[0087] Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

[0088] As a further description of the above technical solution:

[0089] The global consistency alignment device transmits the acquired three-dimensional map to the user through the communication module and receives the user's remote control instructions. The device also includes a data preprocessing module, a map construction module and a visualization module;

[0090] The data preprocessing module is used to filter and remove noise from the sensor data;

[0091] The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is:

[0092]

[0093] Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix;

[0094] The map construction module is used to register the local point cloud into the global map;

[0095] The visualization module is used to display the three-dimensional map and the position information of the device in real time.

[0096] As a further description of the above technical solution:

[0097] The map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map. The visualization module uses OpenGL or Unity engine to achieve real-time rendering of the three-dimensional map.

[0098] A global consistency alignment application based on the fusion of device posture information and lidar observations, which is applied to the following fields:

[0099] Robot navigation, used for real-time positioning and mapping of robots in unknown environments;

[0100] The application in the robot navigation comprises the following steps:

[0101] Acquire environmental information through multi-sensor fusion system;

[0102] Update the robot's posture by adjusting the device posture system;

[0103] Estimate the robot's global posture through particle filters;

[0104] Measure the three-dimensional position of surrounding objects through lidar;

[0105] Build an environment map through a global consistency alignment algorithm and plan the robot's motion path;

[0106] Autonomous driving, for environmental perception and path planning of autonomous vehicles;

[0107] The application in the autonomous driving comprises the following steps:

[0108] Obtain road and obstacle information through a multi-sensor fusion system;

[0109] Update the vehicle's posture by adjusting the device posture system;

[0110] Estimate the global posture of the vehicle through particle filter;

[0111] Measure the three-dimensional position of surrounding objects through lidar;

[0112] Build a road map through a global consistency alignment algorithm and plan the vehicle's driving path;

[0113] Indoor mapping, used for building and updating 3D maps of indoor environments;

[0114] The application of indoor mapping includes the following steps:

[0115] Acquire indoor environment information through multi-sensor fusion system;

[0116] Update the posture of the surveying and mapping equipment by adjusting the equipment posture system;

[0117] Estimate the global pose of the mapping device through particle filter;

[0118] Measure the three-dimensional position of indoor objects through lidar;

[0119] Construct indoor 3D maps through global consistency alignment algorithm;

[0120] Augmented reality, for environmental perception and virtual object localization in augmented reality applications;

[0121] The application of the method in augmented reality includes the following steps:

[0122] Acquire environmental information through multi-sensor fusion system;

[0123] Update the posture of the augmented reality device by adjusting the device posture system;

[0124] Estimate the global pose of the augmented reality device through particle filter;

[0125] Measure the three-dimensional position of surrounding objects through lidar;

[0126] The environment map is constructed and virtual objects are positioned using a global consistency alignment algorithm.

[0127] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0128] 1. Compared with using LiDAR alone, the present invention further improves the accuracy of pose estimation by fusing the information of the inertial measurement unit (IMU) and the LiDAR, effectively compensating for the limitations of a single sensor under specific conditions and improving the robustness and accuracy of the system.

[0129] 2. Compared with the existing SLAM methods, the present invention adopts an accurate and robust global consistency alignment algorithm based on the association of 3D RGB points in different frames, which effectively avoids the problem of cumulative error.

[0130] 3. Compared with the existing radar-camera fusion method, the present invention directly fuses superpixel points with radar points instead of simply correlating the detection results, thereby obtaining denser RGB-D points and saving a lot of computing resources.

[0131] 4. In the present invention, compared with the high-cost three-dimensional laser radar system, a low-cost two-dimensional laser radar assembly solution is adopted, making the equipment more suitable for the consumer market.

[0132] 5. Compared with the limitations of traditional RGB-D cameras in indoor environments, the present invention can still maintain high efficiency and accuracy in scenes with low contrast and lack of obvious features. BRIEF DESCRIPTION OF THE DRAWINGS

[0133] Figure 1 This is a global unified arrangement diagram of a global consistency alignment algorithm, system, device and application based on the fusion of device posture information and lidar observations proposed by the present invention. DETAILED DESCRIPTION

[0134] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0135] Please see the attached Figure 1 The present invention provides a technical solution: a global consistency alignment algorithm based on the fusion of device posture information and lidar observation, comprising the following steps:

[0136] Step 1: Update the device posture. Update the device posture through gyroscope measurements, expressed as a 2D transformation matrix T k , and according to the posture T at the previous moment k-1 And the relative motion ΔT calculates the current posture:

[0137] T k =T k-1 ΔT

[0138] Where ·ΔT is calculated from the angular velocity and acceleration measurements of the gyroscope using the following formula:

[0139]

[0140] Where θ is the rotation angle, Δx and Δy are the translation amounts;

[0141] The gyroscope's measurements include angular velocity ω and acceleration a, which are integrated to calculate the rotation angle θ and translation Δx and Δy:

[0142] θ=∫ωdt,Δx=∫a x dt,Δy=∫a y dt

[0143] Where a x and a y are the components of acceleration in the x and y directions;

[0144] Step 2: ICP algorithm performs inter-frame registration;

[0145] Step 3: Particle filter (PF) performs global pose estimation;

[0146] Step 4: Global consistency alignment. Through the above steps 1 to 3, each local point cloud is gradually registered into the global framework to build a consistent large-scale 3D point cloud map.

[0147] Preferably, the algorithm based on the visualization concept is deduced in detail as follows:

[0148] Step 1: Device posture update:

[0149] The posture of the device at the kth time step Represented as a 2D transformation matrix:

[0150]

[0151] At each time step, the device's pose is calculated by the previous pose and relative motion Update, calculated based on gyroscope measurements:

[0152]

[0153] Step 2: ICP algorithm performs inter-frame registration:

[0154] Step 2.1, Initial Alignment, Coarse Device Positives Obtained from IMU Data Used as the initial transformation matrix

[0155] Step 2.2, nearest neighbor search, for P k-1 Every point P in i , find P k The corresponding closest point P in i ;

[0156] Step 2.3, rigid transformation estimation, estimates the rigid transformation [R, t] that minimizes the alignment error between point sets by minimizing the following cost function:

[0157]

[0158] Step 2.4, iterative refinement, the transformation matrix T is iteratively updated until the registration error converges, and then used Refine the device posture:

[0159]

[0160] Step 3: Particle filter (PF) performs global pose estimation;

[0161] Step 3.1, initialization, generates N particles at the initial time step, representing the possible postures of the device:

[0162]

[0163] Step 3.2, importance sampling, based on the device's motion model and previous estimates, the possible posture of the device at the next time step can be given as:

[0164]

[0165] At the same time, their corresponding weights can be updated as:

[0166]

[0167] in represents the measurement likelihood function, which is calculated using the observation model of the system:

[0168]

[0169] After obtaining the weight of each particle, normalization operation is required:

[0170]

[0171] Step 4, resampling, according to Resampling to generate new particles And set their corresponding weights uniformly to Note that resampling techniques are generally used to suppress particle degradation during the sampling process, where particles with higher weights are more likely to be selected. However, when the number of effective particles N eff Below the set threshold N th When , the resampling strategy is executed, that is:

[0172]

[0173] Step 5, pose estimation, obtains the final pose estimate for the current time step by calculating the weighted average of the particles:

[0174] No resampling:

[0175] With resampling:

[0176] By applying this transformation, each local point cloud is gradually registered into the global framework, creating a consistent large-scale 3D point cloud map.

[0177] Specifically, step 2 includes the following steps:

[0178] Step 2.1, initial alignment, using the rough device posture obtained from IMU data as the initial transformation matrix T0;

[0179] Step 2.2, nearest neighbor search, for each point p in the point cloud P i , find the corresponding nearest point q in the point cloud Q i ;

[0180] The nearest neighbor search uses a KD-Tree data structure to improve search efficiency;

[0181] Step 2.3, rigid transformation estimation, estimates the rigid transformation [R, t] that minimizes the point set alignment error by minimizing the cost function E(R, t). The expression of the cost function E(R, t) is:

[0182]

[0183] Where R is the rotation matrix and t is the translation vector;

[0184] The rigid transformation [R,t] is solved by the singular value decomposition (SVD) method. The specific steps are as follows:

[0185] Calculate the centroid of point clouds P and Q and

[0186]

[0187] Calculate the covariance matrix H:

[0188]

[0189] Perform singular value decomposition on H to obtain U, ∑, and V:

[0190] H=U∑V T

[0191] Calculate the rotation matrix R and translation vector t:

[0192]

[0193] Step 2.4, iterative refinement, update the transformation matrix T until the registration error converges, and use T to refine the device pose;

[0194] T k =T k ·T.

[0195] Specifically, step 3 includes the following steps:

[0196] Step 3.1, Initialization: Generate N particles, representing the possible postures T of the device i ~p(T0);

[0197] Step 3.2, importance sampling, based on the device motion model and measurement likelihood function p(z k |T i ), update the particle weight w i , the measurement likelihood function p(z k |T i ) is:

[0198]

[0199] Where z k is the sensor measurement value, h(T i ) is the observation model, σ is the noise parameter;

[0200] The observation model h(T i ) is:

[0201] h(T i )=T i ·p i

[0202] Where p i is a point in the point cloud P;

[0203] Step 3.3, resampling, according to the weight w i Resampling generates new particles and suppresses particle degradation. The resampling strategy is:

[0204] When the effective number of particles When it is lower than the set threshold, resampling is performed;

[0205] After resampling, the weights of the particles are uniformly set to

[0206] The resampling adopts a systematic resampling method, and the specific steps are as follows:

[0207] Calculate cumulative weight

[0208] Generate uniformly distributed random numbers u~U(0,1);

[0209] Choose to meet c i Particles with ≥u are resampled;

[0210] Step 3.4, pose estimation, calculate the final pose estimate T of the current time step by the weighted average of the particles k , the calculation formula is:

[0211]

[0212] Where w i is the weight of the particle, T i is the particle's posture.

[0213] Specifically, in step 4, global consistency alignment is achieved through the following steps:

[0214] The local point cloud P k Register to the global point cloud Q;

[0215] Use the ICP algorithm to optimize the alignment error between the local point cloud and the global point cloud;

[0216] Update the global point cloud Q to Q∪P k .

[0217] A global consistency alignment system based on the fusion of device posture information and lidar observations, including:

[0218] A multi-sensor fusion system is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion system is a four-directional camera used to obtain RGB images in four directions;

[0219] Adjust the device attitude system, including a six-degree-of-freedom gyroscope, for adjusting the device attitude;

[0220] Particle filter, used to fuse multi-sensor data and estimate the global posture of the device;

[0221] LiDAR, including two-dimensional LiDAR and turntable, used to measure the three-dimensional position of surrounding objects;

[0222] The laser radar measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values refer to azimuth, distance, and turntable azimuth. The specific formula is:

[0223] x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ)

[0224] Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

[0225] Specifically, the global consistency alignment system transmits the acquired three-dimensional map to the user through the communication module and receives remote control instructions from the user;

[0226] The global consistency alignment system includes a data preprocessing module, a map construction module and a visualization module;

[0227] The data preprocessing module is used to filter and remove noise from the sensor data;

[0228] The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is:

[0229]

[0230] Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix;

[0231] The map construction module is used to register the local point cloud into the global map;

[0232] The map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map;

[0233] The visualization module is used to display the three-dimensional map and the position information of the device in real time;

[0234] The visualization module uses OpenGL or Unity engine to achieve real-time rendering of three-dimensional maps.

[0235] A global consistency alignment device based on the fusion of device posture information and lidar observations, comprising:

[0236] A multi-sensor fusion device is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion device is a four-directional camera used to obtain RGB images in four directions;

[0237] A device for adjusting the device's attitude, including a six-degree-of-freedom gyroscope, for adjusting the device's attitude;

[0238] A particle filter device is used to fuse multi-sensor data and estimate the global posture of the device;

[0239] A laser radar device, including a two-dimensional laser radar and a turntable, for measuring the three-dimensional position of surrounding objects;

[0240] The laser radar device measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values are azimuth, distance, and turntable azimuth. The specific formula is:

[0241] x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ)

[0242] Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

[0243] Specifically, the global consistency alignment device transmits the acquired three-dimensional map to the user through the communication module and receives the user's remote control instructions. The device also includes a data preprocessing module, a map construction module and a visualization module;

[0244] The data preprocessing module is used to filter and remove noise from the sensor data;

[0245] The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is:

[0246]

[0247] Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix;

[0248] The map construction module is used to register the local point cloud into the global map;

[0249] The visualization module is used to display the three-dimensional map and the position information of the device in real time.

[0250] Specifically, the map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map, and the visualization module uses OpenGL or Unity engine to achieve real-time rendering of the three-dimensional map.

[0251] A global consistency alignment application based on the fusion of device posture information and lidar observations, which is applied to the following fields:

[0252] Robot navigation, used for real-time positioning and mapping of robots in unknown environments;

[0253] The application in the robot navigation comprises the following steps:

[0254] Acquire environmental information through multi-sensor fusion system;

[0255] Update the robot's posture by adjusting the device posture system;

[0256] Estimate the robot's global posture through particle filters;

[0257] Measure the three-dimensional position of surrounding objects through lidar;

[0258] Build an environmental map through a global consistency alignment algorithm and plan the robot's motion path; autonomous driving, used for environmental perception and path planning of autonomous vehicles;

[0259] The application in the autonomous driving comprises the following steps:

[0260] Obtain road and obstacle information through a multi-sensor fusion system;

[0261] Update the vehicle's posture by adjusting the device posture system;

[0262] Estimate the global posture of the vehicle through particle filter;

[0263] Measure the three-dimensional position of surrounding objects through lidar;

[0264] Build road maps and plan vehicle paths through a global consistency alignment algorithm; Indoor mapping, used for building and updating 3D maps of indoor environments;

[0265] The application of indoor mapping includes the following steps:

[0266] Acquire indoor environment information through multi-sensor fusion system;

[0267] Update the posture of the surveying and mapping equipment by adjusting the equipment posture system;

[0268] Estimate the global pose of the mapping device through particle filter;

[0269] Measure the three-dimensional position of indoor objects through lidar;

[0270] Construct indoor 3D maps through global consistency alignment algorithm;

[0271] Augmented reality, for environmental perception and virtual object localization in augmented reality applications;

[0272] The application of the method in augmented reality includes the following steps:

[0273] Acquire environmental information through multi-sensor fusion system;

[0274] Update the posture of the augmented reality device by adjusting the device posture system;

[0275] Estimate the global pose of the augmented reality device through particle filter;

[0276] Measure the three-dimensional position of surrounding objects through lidar;

[0277] The environment map is constructed and virtual objects are positioned using a global consistency alignment algorithm.

[0278] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any technician familiar with the technical field, within the technical scope disclosed by the present invention, who makes equivalent replacements or changes based on the technical solution and inventive concept of the present invention, should be covered by the scope of protection of the present invention.

Claims

1. A global consistency alignment algorithm based on the fusion of device posture information and lidar observations, characterized by: The following steps are involved: Step 1: Update the device posture. Update the device posture through gyroscope measurements, expressed as a 2D transformation matrix T k , and according to the posture T at the previous moment k-1 And the relative motion ΔT calculates the current posture: T k =T k-1 ·ΔT Among them, ΔT is calculated by the angular velocity and acceleration measurements of the gyroscope. The specific formula is: Where θ is the rotation angle, Δx and Δy are the translation amounts; The gyroscope's measurements include angular velocity ω and acceleration a, which are integrated to calculate the rotation angle θ and translation Δx and Δy: θ=∫ωdt,Δx=∫a x dt,Δy=∫a y dt Where a x and a y are the components of acceleration in the x and y directions; Step 2: ICP algorithm performs inter-frame registration; Step 3: Particle filter (PF) performs global pose estimation; Step 4: Global consistency alignment. Through the above steps 1 to 3, each local point cloud is gradually registered into the global framework to build a consistent large-scale 3D point cloud map.

2. The global consistency alignment algorithm based on the fusion of device posture information and lidar observation according to claim 1 is characterized in that: The step 2 comprises the following steps: Step 2.1, initial alignment, using the rough device posture obtained from IMU data as the initial transformation matrix T0; Step 2.2, nearest neighbor search, for each point p in the point cloud p i , find the corresponding nearest point q in the point cloud Q i ; The nearest neighbor search uses a KD-Tree data structure to improve search efficiency; Step 2.3, rigid transformation estimation, estimates the rigid transformation [R, t] that minimizes the point set alignment error by minimizing the cost function E(R, t). The expression of the cost function E(R, t) is: Where R is the rotation matrix and t is the translation vector; The rigid transformation [R,t] is solved by the singular value decomposition (SVD) method. The specific steps are as follows: Calculate the centroid of point clouds P and Q and Calculate the covariance matrix H: Perform singular value decomposition on H to obtain U, ∑, and V: H=UΣV T Calculate the rotation matrix R and translation vector t: Step 2.4, iterative refinement, update the transformation matrix T until the registration error converges, and use T to refine the device pose; T k =T k ·T。 3. The global consistency alignment algorithm based on the fusion of device posture information and lidar observation according to claim 2 is characterized in that: The step 3 comprises the following steps: Step 3.1, Initialization: Generate N particles, representing the possible postures T of the device i ~p(T0); Step 3.2, importance sampling, based on the device motion model and measurement likelihood function p(z k |T i ), update the particle weight w i , the measurement likelihood function p(z k |T i ) is: Where z k is the sensor measurement value, h(T i ) is the observation model, σ is the noise parameter; The observation model h(T i ) is: h(T i )=T i ·p i Where p i is a point in the point cloud P; Step 3.3, resampling, according to the weight w i Resampling generates new particles and suppresses particle degradation. The resampling strategy is: When the effective number of particles When it is lower than the set threshold, resampling is performed; After resampling, the weights of the particles are uniformly set to The resampling adopts a systematic resampling method, and the specific steps are as follows: Calculate cumulative weight Generate uniformly distributed random numbers u~U(0,1); Choose to meet c i Particles with ≥u are resampled; Step 3.4, pose estimation, calculate the final pose estimate T of the current time step by the weighted average of the particles k , the calculation formula is: Where w i is the weight of the particle, T i is the particle's posture.

4. The global consistency alignment algorithm based on the fusion of device posture information and lidar observation according to claim 3 is characterized in that: In step 4, global consistency alignment is achieved through the following steps: The local point cloud P k Register to the global point cloud Q; Use the ICP algorithm to optimize the alignment error between the local point cloud and the global point cloud; Update the global point cloud Q to Q∪P k .

5. A global consistency alignment system based on the fusion of device posture information and lidar observations, according to the global consistency alignment algorithm based on the fusion of device posture information and lidar observations according to claim 4, characterized in that: include: A multi-sensor fusion system is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion system is a four-directional camera used to obtain RGB images in four directions; Adjust the device attitude system, including a six-degree-of-freedom gyroscope, for adjusting the device attitude; Particle filter, used to fuse multi-sensor data and estimate the global posture of the device; LiDAR, including two-dimensional LiDAR and turntable, used to measure the three-dimensional position of surrounding objects; The laser radar measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values refer to azimuth, distance, and turntable azimuth. The specific formula is: x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ) Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

6. The global consistency alignment system based on the fusion of device posture information and lidar observation according to claim 5 is characterized in that: The global consistency alignment system transmits the acquired three-dimensional map to the user through the communication module and receives remote control instructions from the user; The global consistency alignment system includes a data preprocessing module, a map construction module and a visualization module; The data preprocessing module is used to filter and remove noise from the sensor data; The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is: Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix; The map construction module is used to register the local point cloud into the global map; The map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map; The visualization module is used to display the three-dimensional map and the position information of the device in real time; The visualization module uses OpenGL or Unity engine to achieve real-time rendering of three-dimensional maps.

7. A global consistency alignment device based on the fusion of device posture information and laser radar observation, according to the global consistency alignment system based on the fusion of device posture information and laser radar observation according to claim 6, characterized in that: include: A multi-sensor fusion device is used to obtain color information and depth information of surrounding objects. The multi-sensor fusion device is a four-directional camera used to obtain RGB images in four directions; A device for adjusting the device's attitude, including a six-degree-of-freedom gyroscope, for adjusting the device's attitude; A particle filter device is used to fuse multi-sensor data and estimate the global posture of the device; A laser radar device, including a two-dimensional laser radar and a turntable, for measuring the three-dimensional position of surrounding objects; The laser radar device measures the three-dimensional position of surrounding objects by converting three observation values, where the three observation values are azimuth, distance, and turntable azimuth. The specific formula is: x=r·cos(θ)·cos(φ), y=r·sin(θ)·cos(φ), z=r·sin(φ) Where r is the distance, θ is the azimuth, and φ is the turntable azimuth.

8. The global consistency alignment device based on the fusion of device posture information and lidar observation according to claim 7 is characterized in that: The global consistency alignment device transmits the acquired three-dimensional map to the user through the communication module and receives the user's remote control instructions. The device also includes a data preprocessing module, a map construction module and a visualization module; The data preprocessing module is used to filter and remove noise from the sensor data; The data preprocessing module uses a Kalman filter to filter the sensor data. The specific formula is: Where, is the state estimate, K k is the Kalman gain, z k is the measurement value, H is the observation matrix; The map construction module is used to register the local point cloud into the global map; The visualization module is used to display the three-dimensional map and the position information of the device in real time.

9. The global consistency alignment device based on the fusion of device posture information and lidar observation according to claim 8, characterized in that: The map construction module uses an octree data structure to store the global point cloud to improve the storage and query efficiency of the map. The visualization module uses OpenGL or Unity engine to achieve real-time rendering of the three-dimensional map.

10. A global consistency alignment application based on the fusion of device posture information and laser radar observation, according to the global consistency alignment device based on the fusion of device posture information and laser radar observation according to claim 9, characterized in that: Applied in the following fields: Robot navigation, used for real-time positioning and mapping of robots in unknown environments; The application in the robot navigation comprises the following steps: Acquire environmental information through multi-sensor fusion system; Update the robot's posture by adjusting the device posture system; Estimate the robot's global posture through particle filters; Measure the three-dimensional position of surrounding objects through lidar; Build an environment map through a global consistency alignment algorithm and plan the robot's motion path; Autonomous driving, for environmental perception and path planning of autonomous vehicles; The application in the autonomous driving comprises the following steps: Obtain road and obstacle information through a multi-sensor fusion system; Update the vehicle's posture by adjusting the device posture system; Estimate the global posture of the vehicle through particle filter; Measure the three-dimensional position of surrounding objects through lidar; Build road maps and plan vehicle paths through a global consistency alignment algorithm; Indoor mapping, used for building and updating 3D maps of indoor environments; The application of indoor mapping includes the following steps: Acquire indoor environment information through multi-sensor fusion system; Update the posture of the surveying and mapping equipment by adjusting the equipment posture system; Estimate the global pose of the mapping device through particle filter; Measure the three-dimensional position of indoor objects through lidar; Construct indoor 3D maps through global consistency alignment algorithm; Augmented reality, for environmental perception and virtual object positioning in augmented reality applications; the method for augmented reality applications includes the following steps: Acquire environmental information through multi-sensor fusion system; Update the posture of the augmented reality device by adjusting the device posture system; Estimate the global pose of the augmented reality device through particle filter; Measure the three-dimensional position of surrounding objects through lidar; The environment map is constructed and virtual objects are positioned using a global consistency alignment algorithm.

Images