Laser vision and inertial navigation fusion positioning and mapping device and method for aerial work platform

Abstract

The present invention provides a laser vision and inertial navigation fusion positioning and mapping device and method for an aerial work platform. The device includes a pan / tilt support, a monocular camera, a laser radar, and an inertial measurement unit (IMU). The laser radar scans the environment, the IMU inertial sensor performs pre-integration to obtain rotation and displacement increments, the camera is triggered by a synchronous frequency division signal to expose and collect RGB images, and point cloud data, image data, and IMU inertial sensor data are fed back to a computing main control unit. Two threads respectively perform adjacent frame matching to obtain a laser odometer, then perform global factor graph optimization, and finally perform loop detection to fuse the calculation results to construct a map. Overall motion information is obtained through back-end nonlinear optimization to determine the real-time position and posture of the work bucket. The data streams of the camera, laser radar, and IMU inertial sensor are input into the computing main control unit through a switching network after time synchronization.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent positioning, laser vision inertial positioning and mapping of aerial work machinery, and specifically relates to a laser vision and inertial navigation fusion positioning and mapping device and method for an aerial work platform. Background Art

[0002] Our daily lives are full of scenes of aerial work, whether it is construction in industry, maintenance of electrical and telecommunication facilities, or tree care and picking in agriculture. The risks of aerial work are high. Improper operation may cause damage to the life and health of engineers, such as the danger of squeezing at high altitudes and the danger of electric shock when working on power grid lines. The main reason for the above pain points is that the existing working mode relies solely on human power to perceive the working environment. The intelligent transformation of aerial work machines can make the perception of the environment more complete during aerial work. For this reason, aerial work machines are equipped with three types of sensors: the first type is lidar, which has a strong three-dimensional environmental perception capability and emits infrared lasers to Light reflects environmental information and publishes data in the form of sparse point clouds with centimeter-level accuracy. The disadvantage of lidar is that it cannot perceive the color information of the high-altitude environment, and the single-frame imaging resolution is low; the second category is cameras, which are low-cost and can perceive the color information of the environment. They publish data in the form of images or video streams. The disadvantage of cameras is that they are easily disturbed by factors such as high-altitude environmental lighting, such as sunlight, and cannot directly obtain three-dimensional information of the working environment; the third category is the inertial navigation module (IMU), which can measure the three-axis acceleration and angular velocity information of the object, and the trajectory can be obtained by integration. The disadvantage of IMU is that the cumulative error accumulates over time, and the vibration of the aerial work platform will affect its accuracy.

[0003] The real-time localization and mapping function of autonomous systems, also known as SLAM, has made significant progress in fields such as robotics and unmanned vehicles. SLAM methods primarily include laser SLAM, visual SLAM, and inertial fusion SLAM. Laser SLAM uses lidar as its primary sensor, sensing the environment through laser reflections and then determining the robot's position. Visual SLAM uses cameras as its primary sensor. It uses exposed images to sense the environment and then determines the robot's position. However, these SLAM technologies have yet to be applied to high-altitude work environments. Due to the unique characteristics of these environments, existing single-laser SLAM methods and visual-inertial SLAM methods are not effectively adapted to these environments. Summary of the Invention

[0004] In light of the advantages and disadvantages of three types of sensors used for positioning and mapping aerial work scenarios, this invention integrates them into a single system using a high-precision time and space synchronization method. The algorithm design leverages the strengths of each sensor, minimizing their weaknesses and complementing each other's strengths. This invention provides a laser vision and inertial navigation fusion positioning and mapping device and method for aerial work platforms. This device enables the aerial work platform to autonomously perceive its environment, locate itself in the work environment, and create three-dimensional maps, thereby enhancing the intelligence and safety of aerial work equipment. Through a device and algorithm that fuses laser, vision, and inertial navigation, this invention addresses the low accuracy and robustness of existing laser SLAM or single-vision SLAM methods when applied to aerial work platforms.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A laser vision and inertial navigation fusion positioning and mapping device for an aerial work platform comprises a pan-tilt support, a monocular camera, a laser radar, and an inertial measurement unit (IMU). The positioning and mapping device is rigidly fixed to the end of a boom truck's lifting platform and moves with the bucket. When the boom truck starts working, a hydraulic arm controls rotation and lifting. The positioning and mapping device is synchronously powered on at the start of lifting. The laser radar scans the environment, and the IMU inertial sensor performs pre-integration to obtain rotation and displacement increments. The camera is triggered by a synchronous frequency division signal to expose and capture RGB images, and point cloud data, image data, and IMU inertial sensor data are fed back to a computing main control unit. Two threads perform adjacent frame matching to obtain a laser odometry, followed by global factor graph optimization. Finally, loopback detection is performed to fuse the calculation results to construct a map. Overall motion information is obtained through back-end nonlinear optimization to determine the real-time position and posture of the bucket. The data streams of the camera, laser radar, and IMU inertial sensor are time-synchronized and input into the computing main control unit through a switching network.

[0007] Furthermore, the laser radar is a mechanical rotating laser radar, a solid-state laser radar, a MEMS laser radar or a digital laser radar.

[0008] Furthermore, the lidar uses a multi-line laser detector to obtain a 360-degree 3D point cloud image. The multi-line laser detector rotates rapidly through a motor to scan the surrounding environment; the multi-line laser detector emits thousands of times per second, providing a rich 3D point cloud; the radar data box provides high-precision, scalable distance detection and intensity data through digital signal processing and waveform analysis, and the lidar publishes point cloud data at an update rate of not less than 5Hz.

[0009] Furthermore, the camera uses a CMOS photosensitive chip for imaging and global exposure, and publishes images at a rate of not less than 10 Hz. The lens is a pinhole lens or a fisheye lens, which is used to capture RGB images of scenes and objects in the front field of view.

[0010] Furthermore, the IMU inertial sensor updates nine-axis data at a rate of not less than 200 Hz, respectively searching for the three-axis attitude angle, three-axis acceleration, and three-axis angular velocity of the positioning and mapping device.

[0011] The present invention is also based on a method for integrating laser vision and inertial navigation into positioning and mapping for an aerial work platform, which is implemented through an equipment installation and calibration module, a time synchronization module, and an aerial positioning and mapping algorithm module; the equipment is installed in the calibration module for designing a pan / tilt bracket; the time synchronization module performs time synchronization of the camera, laser radar, IMU inertial sensor, and main control computing module; the aerial positioning and mapping algorithm module is used for coupling the laser-inertial system and the vision-inertial system, and accepts information from 3D laser point clouds, monocular or multi-camera images, and IMU inertial sensors as input.

[0012] Furthermore, the first level of the time synchronization module is a GNSS receiving module, which obtains UTC true time data with nanosecond accuracy through satellites. The FPGA processes the timing information using low-latency logic parallel circuits, converting the GNSS signal into a PPS signal and a NEMA signal. The lidar receives the two signals for time synchronization, and the PPS signal is simultaneously connected to the IMU inertial sensor and the divider module. The PPS signal ensures that the inertial data timestamp of the IMU inertial sensor is consistent with the UTC time true value. The PPS signal connected to the divider module triggers the camera according to the required frame rate. The camera trigger signal is aligned with the PPS signal at the whole second edge, and the delay between the two signals is within tens of nanoseconds. Therefore, the camera image exposure time is synchronized with the data acquisition time of the IMU inertial sensor. The time of each camera is synchronized during manufacturing, so the trigger signal will simultaneously trigger all cameras on the serial trigger line. The main control computing module receives the NDT network data packet converted and sent by the FPGA and performs time synchronization through the NDT protocol.

[0013] Furthermore, the visual-inertial system receives information from images and IMU inertial sensors, with radar point clouds as optional input; the visual odometry is obtained by minimizing the measurement residuals of the IMU inertial sensor and vision; the laser odometry is obtained by minimizing the distance from the detected line and surface features to the feature map; the feature map is stored in a sliding window for real-time execution; a factor graph is used to optimize the residuals of the pre-integration of the IMU inertial sensor, the visual odometry, the laser odometry and the closed-loop constraints; the visual part of the visual-inertial system obtains the system initial state and sensor parameters from the lidar and IMU inertial sensors, and then associates the radar frame to the image key frame according to the image timestamp, and interpolates to complete the initialization.

[0014] Furthermore, the laser-inertial system is used for positioning and dense mapping, including a laser motion distortion correction module, a feature extraction module, an IMU pre-integration module and a map optimization module;

[0015] The laser motion distortion module obtains data from the IMU inertial sensor fixed on the boom truck to roughly initialize the radar's posture, and uses the posture increment relative to the starting time of the laser frame to transform the current laser point to the coordinate system of the laser point at the starting time to achieve correction of the laser frame;

[0016] The feature extraction module calculates the curvature of each point through the point cloud data collected by the high-altitude laser radar, sets the corresponding threshold, and extracts corner points and plane points after comparing the threshold and curvature;

[0017] The IMU pre-integration module is used to obtain each frame of IMU inertial sensor data during high-altitude operation, using the previous frame as the starting point, and more efficiently and quickly obtaining the rotation and translation IMU inertial sensor increments during high-altitude operation;

[0018] The map optimization module first performs inter-frame matching between adjacent data based on the feature points obtained by the feature extraction module: extracts the feature points of the current laser frame, namely corner points, plane points, and feature points of the high-altitude map corresponding to the local key frame, performs iterative optimization of the current frame and the high-altitude map, and updates the current frame pose; performs key frame factor graph optimization, adds factor graphs to the key frames, adds laser odometry factors, GPS factors, and closed-loop factors, performs factor graph optimization, and updates all key frame poses; performs closed-loop detection, finds frames with similar distances and long time intervals in historical key frames and sets them as matching frames, extracts local key frames around the matching frames, and similarly performs single-frame scanning to global map matching to obtain pose transformation, constructs closed-loop factor data, and adds factor graph optimization.

[0019] Furthermore, both the visual-inertial system and the laser-inertial system are equipped with failure detection mechanisms. When the visual part fails, the judgment condition is that the number of tracked feature points is less than a threshold or the IMU inertial sensor deviation value is greater than a certain threshold. An alarm is triggered and the laser-inertial system is notified, and the positioning result depends only on the laser part. When the laser part fails, the laser odometry drifts. By evaluating the nonlinear optimization results of the laser odometry inter-frame matching, if the minimum eigenvalue is less than the threshold of the first optimization iteration, a failure is reported, the laser odometry factor is not included in the factor graph optimization, and the positioning result depends only on the visual part.

[0020] Beneficial effects:

[0021] In this invention, the LiDAR Inertial Vision (LIV) system can still function even if one of the laser inertial navigation and visual inertial navigation subsystems fails, significantly improving the robustness of the aerial work system in scenes lacking texture or features. The LIV system was tested on a dataset and achieved excellent results. The LIV system uses a novel composite SLAM algorithm that integrates lidar, vision, and inertial navigation, making it well-suited for aerial work scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is the engineering design drawing of the camera-lidar-IMU inertial sensor system and gimbal bracket of the present invention;

[0023] Figure 2 This is a schematic diagram of the overall assembly project of the vision, laser and inertial navigation fusion positioning and mapping device and the aerial work machinery of the present invention;

[0024] Figure 3 for Figure 1 Actual engineering implementation diagram;

[0025] Figure 4 This is the positioning trajectory of the visual-inertial system and the terminal pose diagram of the platform when the laser part fails;

[0026] Figure 5 Provides the positioning trajectory of the actual operation of the laser-vision-inertial joint working system and a point cloud map of the aerial work platform working environment;

[0027] Figure 6 This is a time diagram of the sensor front-end clock synchronization trigger signal, synchronized camera, lidar and IMU inertial sensor data flow;

[0028] Figure 7 It is the data flow diagram after sensor clock synchronization. DETAILED DESCRIPTION

[0029] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely for the purpose of explaining the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0030] The aerial work platform laser vision and inertial navigation fusion positioning and mapping method of the present invention is implemented through an equipment installation and calibration module, a time synchronization module, and an aerial positioning and mapping algorithm module.

[0031] like Figure 1As shown, the equipment installation and calibration module is used to design a gimbal bracket, rigidly connect the lidar 1, the monocular camera 2 and the IMU inertial sensor 3, to ensure that the relative positions of the IMU inertial sensors 3 do not change during the raising and lowering of the aerial work platform. At the same time, the gimbal bracket is rigidly connected to the working bucket of the aerial work platform to ensure that the IMU inertial sensor faithfully reflects the motion posture of the working bucket of the aerial work platform.

[0032] The LiDAR 1 uses a multi-line laser detector to generate a 360-degree 3D point cloud image. The multi-line laser detector is rapidly rotated by a motor to scan the surrounding environment. It fires thousands of times per second, providing a rich 3D point cloud. The radar data box uses digital signal processing and waveform analysis to provide highly accurate and scalable distance detection and intensity data. The LiDAR 1 publishes point cloud data at an update rate of no less than 5Hz. The LiDAR 1 can be a mechanical rotating LiDAR, solid-state LiDAR, MEMS LiDAR, digital LiDAR, or other similar devices.

[0033] The camera 2 uses a CMOS photosensitive chip for imaging, global exposure, and publishes images at a rate of not less than 10 Hz. The lens is a pinhole lens or a fisheye lens, which can capture RGB images of scenes and objects in the front field of view.

[0034] The IMU inertial sensor 3 updates nine-axis data at a rate of not less than 200 Hz, reflecting the three-axis attitude angle (roll, pitch, yaw), three-axis acceleration (acc_x, acc_y, acc_z) and three-axis angular velocity (w_x, w_y, w_z) of the positioning and mapping device of the present invention.

[0035] The coordinate systems of the laser radar 1, camera 2 and IMU inertial sensor 3 form a translation relationship, with the x-axis facing forward and being a right-handed coordinate system. Before mapping, the laser radar 1, camera 2 and IMU inertial sensor 3 are spatially calibrated to obtain the geometric transformation relationship, transformation matrix and translation vector between their respective coordinate systems.

[0036] The time synchronization module synchronizes the camera 2, the lidar 1, the inertial measurement unit (IMU) 3, and the main control module. The first stage of the time synchronization module is a GNSS receiver module, which acquires nanosecond-accurate UTC true time data from satellites. The FPGA processes the timing information using low-latency logic parallel circuits, converting the GNSS signal into a PPS signal and a NEMA signal. The lidar 1 receives these two signals for time synchronization, while the PPS signal is simultaneously transmitted to the IMU 3 and the frequency divider module. The PPS signal ensures that the inertial data timestamp of the IMU 3 is consistent with the true UTC time value. The PPS signal transmitted to the frequency divider module triggers the camera according to the required frame rate. The camera trigger signal aligns with the PPS signal at the edge of the whole second, and the delay between the two signals is within tens of nanoseconds. Therefore, the camera image exposure time is synchronized with the data acquisition time of the IMU 3. The time of each camera 2 is synchronized during manufacturing, so the trigger signal simultaneously triggers all cameras 2 on the serial trigger line. The main control module receives the NDT network data packet converted and sent by the FPGA and performs time synchronization using the NDT protocol.

[0037] The algorithm of the high-altitude positioning and mapping algorithm module is a coupling of the laser-inertial system and the visual-inertial system, which accepts 3D laser point cloud, monocular or multi-camera images and IMU inertial sensor information as input.

[0038] The visual-inertial system receives information from images and IMU inertial sensors, and radar point clouds are optional inputs. The visual odometry is obtained by minimizing the measurement residuals of the IMU inertial sensor and vision. The laser odometry is obtained by minimizing the distance from the detected line and surface features to the feature map. The feature map is stored in a sliding window for real-time execution. The factor graph is used to optimize the residuals of the pre-integration of the IMU inertial sensor, the visual odometry, the laser odometry and the closed-loop constraints. The main tasks of the visual-inertial system are initialization and positioning. Due to the lack of scale information, the initialization effect of the traditional monocular visual-inertial system depends on the initial motion of the system, and the success rate is insufficient. To address this problem, the present invention uses a robust and scale-obvious (depth information can be directly obtained) radar sensor to assist in the initialization of the visual-inertial system. The visual part obtains the system initial state and sensor parameters from the radar-inertial system, and then associates the radar frame to the image key frame according to the image timestamp, and interpolates to complete the initialization.

[0039] After initialization, the visual part contains the following four main modules:

[0040] 1. Feature point extraction,adjacent frames are tracked through KLTKLT pyramid optical flow,and feature matching is performed using optical flow method.

[0041] 2. Feature points are projected onto a unit sphere for depth correlation. Due to the sparsity of radar point clouds, several frames are stacked together to obtain a dense depth map. Feature points and radar-generated depth points are first projected onto a unit sphere centered at the camera, correlating image feature points with depth values. Depth points are then downsampled at a certain density and their polar coordinates are stored.

[0042] 3. Key frame selection, in order to ensure triangulation, when selecting key frames, this algorithm must ensure that the average feature disparity between the current frame and the adjacent key frames is greater than a threshold, then it is determined as a key frame, or the number of feature points of the current frame is greater than the threshold, then it is a key frame.

[0043] 4. The IMU inertial sensor calculates the pre-integration between frames k and k+1.

[0044] The task of the laser-inertial system is positioning and dense mapping. The algorithm includes the following parts: laser motion distortion correction module, feature extraction module, IMU pre-integration module, and map optimization module.

[0045] The laser motion distortion module obtains data from the IMU fixed on the boom truck to roughly initialize the radar's posture, and uses the posture increment relative to the starting time of the laser frame to transform the current laser point to the coordinate system of the laser point at the starting time to achieve correction of the laser frame.

[0046] The feature extraction module calculates the curvature of each point through the point cloud data collected by the high-altitude laser radar, sets a corresponding threshold, and extracts corner points and plane points after comparing the threshold and the curvature.

[0047] The IMU pre-integration module is used to obtain each frame of IMU inertial sensor data during high-altitude operations, using the previous frame as the starting point, so as to more efficiently and quickly obtain the rotation and translation IMU inertial sensor increments during high-altitude operations.

[0048] The map optimization module first performs inter-frame matching between adjacent data based on the feature points obtained by the feature extraction module: extracts the feature points of the current laser frame, namely corner points, plane points, and feature points of the high-altitude map corresponding to the local key frame, performs iterative optimization of the current frame and the high-altitude map, and updates the current frame pose; performs key frame factor graph optimization, adds factor graphs to the key frames, adds laser odometry factors, GPS factors, and closed-loop factors, performs factor graph optimization, and updates all key frame poses; performs closed-loop detection, finds frames with similar distances and long time intervals in historical key frames and sets them as matching frames, extracts local key frames around the matching frames, and similarly performs single-frame scanning to global map matching to obtain pose transformation, constructs closed-loop factor data, and adds factor graph optimization.

[0049] Both the visual-inertial system and the laser-inertial system are equipped with failure detection mechanisms. When the visual component fails, dramatic changes in motion, changes in lighting, or a lack of texture in the environment can cause the visual-inertial system to fail. In this case, the number of tracked feature points will decrease or the deviation of the IMU inertial sensor will be large. When the judgment condition is that the number of tracked feature points is less than a threshold or the IMU inertial sensor deviation value is greater than a certain threshold, the system alarms and notifies the laser-inertial system, and the overall positioning result relies solely on the laser component. When the laser component fails, in certain environments with a lack of texture or repeated scenes, the laser odometry will drift. By evaluating the nonlinear optimization results of the laser odometry inter-frame matching, if the minimum eigenvalue is less than the threshold of the first optimization iteration, a failure is reported, and the laser odometry factor is not included in the factor graph optimization. The overall positioning result relies solely on the visual component.

[0050] like Figure 1 The aerial work platform laser vision and inertial navigation fusion positioning and mapping device of the present invention includes a front-end data acquisition system, which includes a pan-tilt support, a monocular camera 2, a laser radar 1, and an IMU inertial sensor. The laser radar 1 can be a mechanical rotating laser radar or a solid-state array laser radar. Figure 2 As shown in Figure 3, the positioning and mapping device 4 of the present invention is installed on the aerial work machinery platform 5. The positioning and mapping device 4 is rigidly fixed to the end of the boom truck lifting platform and moves with the work bucket. When the boom truck starts working, the operator controls the hydraulic arm to control rotation and lifting. The positioning and mapping device 4 is synchronously turned on at the start of lifting. The laser radar 1 scans the environment, the IMU inertial sensor 3 performs pre-integration to obtain rotation and displacement increments, and the camera 2 is triggered by the synchronous frequency division signal to expose and capture RGB images. The point cloud data, image data, and IMU inertial sensor data are fed back to the computer. At this time, two threads respectively perform adjacent frame matching to obtain the laser odometry, followed by global factor graph optimization. Finally, loop detection is performed to fuse the calculation results to construct a map. The overall motion information is obtained through back-end nonlinear optimization to determine the real-time position and posture of the work bucket. Figure 4 The figure shows the positioning trajectory of the visual-inertial system and the terminal pose of the platform when the laser part fails. Figure 5 The laser visual-inertial coupling mapping and positioning effect shows that the laser-inertial system works normally. Figure 6 This is a timing diagram of the sensor front-end clock synchronization trigger signal, synchronized camera, lidar, and IMU inertial sensor data flow. Figure 7 This is the data flow diagram after sensor clock synchronization. The data streams of camera 2, lidar 1 and IMU inertial sensor 3 are time-synchronized and then input into the computing main control unit of the industrial computer through the switching network.

[0051] It will be easily understood by those skilled in the art that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for positioning and mapping using laser vision and inertial navigation fusion for aerial work platforms, characterized by: The laser vision and inertial navigation fusion positioning and mapping device of the aerial work platform includes a pan-tilt support, a monocular camera, a laser radar and an IMU inertial sensor; the positioning and mapping device is rigidly fixed to the end of the boom truck lifting platform and moves with the working bucket. When the boom truck starts working, the hydraulic arm controls the rotation and lifting. The positioning and mapping device is started synchronously at the start of the lifting. The laser radar scans the environment, and the IMU inertial sensor performs pre-integration to obtain rotation and displacement increments. The camera is triggered by a synchronous frequency division signal to expose and collect RGB images, and the point cloud data, image data and IMU inertial sensor data are fed back to the main computing control unit. Two lines The process performs adjacent frame matching to obtain the laser odometer, then performs global factor graph optimization, and finally performs loop detection to fuse the calculation results to construct a map. The overall motion information is obtained through back-end nonlinear optimization to determine the real-time position and posture of the work bucket; the data streams of the camera, lidar and IMU inertial sensor are time-synchronized and input into the computing main control unit through the switching network; it is realized through the equipment installation and calibration module, the time synchronization module, and the high-altitude positioning and mapping algorithm module; the equipment is installed in the calibration module for designing the gimbal bracket; the time synchronization module performs time synchronization of the camera, lidar, IMU inertial sensor and the main control computing module; the high-altitude positioning and mapping algorithm module is used for the coupling of the laser-inertial system and the visual-inertial system, and it accepts 3D laser point cloud, monocular or multi-eye image and IMU inertial sensor information as input.

2. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: The laser radar is a mechanical rotating laser radar, a solid-state laser radar, a MEMS laser radar or a digital laser radar.

3. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: The lidar uses a multi-line laser detector to obtain a 360-degree 3D point cloud image. The multi-line laser detector rotates rapidly via a motor to scan the surrounding environment. The multi-line laser detector emits thousands of times per second, providing a rich 3D point cloud. The radar data box provides high-precision, scalable distance detection and intensity data through digital signal processing and waveform analysis. The lidar publishes point cloud data at an update rate of no less than 5Hz.

4. The method for positioning and mapping aerial work platforms using laser vision and inertial navigation fusion according to claim 1, characterized in that: The camera uses a CMOS photosensitive chip for imaging, global exposure, and publishes images at a rate of no less than 10 Hz. The lens is a pinhole lens or a fisheye lens, which is used to capture RGB images of scenes and objects in the front field of view.

5. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1 is characterized by: The IMU inertial sensor updates nine-axis data at a rate of not less than 200 Hz, respectively searching for the three-axis attitude angle, three-axis acceleration, and three-axis angular velocity of the positioning and mapping device.

6. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: The first level of the time synchronization module is a GNSS receiving module, which obtains UTC true time data with nanosecond accuracy through satellites. The FPGA processes the timing information using low-latency logic parallel circuits, converting the GNSS signal into PPS signal and NEMA signal. The lidar receives these two signals for time synchronization, and the PPS signal is simultaneously connected to the IMU inertial sensor and the divider module. The PPS signal ensures that the inertial data timestamp of the IMU inertial sensor is consistent with the UTC time true value. The PPS signal connected to the divider module triggers the camera according to the required frame rate. The camera trigger signal is aligned with the PPS signal at the whole second edge, and the delay between the two signals is within tens of nanoseconds. Therefore, the camera image exposure time is synchronized with the data acquisition time of the IMU inertial sensor. The time of each camera is synchronized during manufacturing, so the trigger signal will simultaneously trigger all cameras on the serial trigger line. The main control calculation module receives the NDT network data packet converted and sent by the FPGA and performs time synchronization through the NDT protocol.

7. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: The visual-inertial system receives information from images and IMU inertial sensors, with radar point clouds as optional input. The visual odometry is obtained by minimizing the measurement residuals of the IMU inertial sensor and vision. The laser odometry is obtained by minimizing the distance from the detected line and surface features to the feature map. The feature map is stored in a sliding window for real-time execution. A factor graph is used to optimize the residuals of the pre-integration of the IMU inertial sensor, the visual odometry, the laser odometry, and the closed-loop constraints. The visual part of the visual-inertial system obtains the system initial state and sensor parameters from the lidar and IMU inertial sensors, and then associates the radar frame with the image key frame according to the image timestamp and performs interpolation to complete initialization.

8. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: The laser-inertial system is used for positioning and dense mapping, including a laser motion distortion correction module, a feature extraction module, an IMU pre-integration module and a map optimization module; The laser motion distortion module obtains data from the IMU inertial sensor fixed on the boom truck to roughly initialize the radar's posture, and uses the posture increment relative to the starting time of the laser frame to transform the current laser point to the coordinate system of the laser point at the starting time to achieve correction of the laser frame; The feature extraction module calculates the curvature of each point through the point cloud data collected by the high-altitude laser radar, sets the corresponding threshold, and extracts corner points and plane points after comparing the threshold and curvature; The IMU pre-integration module is used to obtain each frame of IMU inertial sensor data during high-altitude operation, using the previous frame as the starting point, and more efficiently and quickly obtaining the rotation and translation IMU inertial sensor increments during high-altitude operation; The map optimization module first performs inter-frame matching between adjacent data based on the feature points obtained by the feature extraction module: extracts the feature points of the current laser frame, i.e., corner points, plane points, and feature points of the aerial map corresponding to the local keyframe, performs iterative optimization of the current frame and the aerial map, and updates the current frame pose; Perform keyframe factor graph optimization, add factor graphs to keyframes, add laser odometry factors, GPS factors, and closed-loop factors, perform factor graph optimization, and update all keyframe poses; perform closed-loop detection, find frames with similar distances and long time intervals in historical keyframes and set them as matching frames, extract local keyframes around the matching frames, and similarly perform single-frame scanning to global map matching to obtain pose transformation, construct closed-loop factor data, and add factor graph optimization.

9. The method for positioning and mapping by integrating laser vision and inertial navigation for aerial work platforms according to claim 1, characterized in that: Both the visual-inertial system and the laser-inertial system are equipped with failure detection mechanisms. When the visual part fails, the judgment condition is that the number of tracked feature points is less than a threshold or the IMU inertial sensor deviation value is greater than a certain threshold. An alarm is triggered and the laser-inertial system is notified, and the positioning result relies solely on the laser part. When the laser part fails, the laser odometry drifts. By evaluating the nonlinear optimization results of the laser odometry inter-frame matching, if the minimum eigenvalue is less than the threshold of the first optimization iteration, a failure is reported. The laser odometry factor is not included in the factor graph optimization, and the positioning result relies solely on the visual part.

Images