A visual and inertial navigation fusion positioning device and method of an aerial work machine

Abstract

The application provides a kind of aerial work machine vision and inertial navigation fusion positioning device and method, device includes fixed holder, monocular camera, IMU inertial sensor, power, computing main control unit and time synchronization system.The application is collected monocular camera and IMU inertial sensor data in high-altitude work platform, first, the above-mentioned data collected are extracted, and the IMU inertial sensor is pre-integrated.After feature matching, the rotation matrix and translation of the current data frame obtained by holder are solved.Due to more data in the process of high-altitude work, sliding window is used to limit the number of key frames in the application.Finally, the whole high-altitude work environment is constructed, and the whole optimization of work position positioning is carried out.The application adds IMU pre-integration factor to remove frame matching part, and the motion estimation result is more robust, to realize better pose estimation.

Description

Technical Field

[0001] This invention belongs to the field of intelligent positioning technology and equipment for aerial work machinery, specifically relating to a vision and inertial navigation fusion positioning device and method for aerial work machinery. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a widely used technology in the field of robotics. Visual SLAM uses a camera as the primary sensor and the captured video stream as input to achieve simultaneous localization and mapping. Visual SLAM is widely used in cutting-edge fields such as AR, autonomous driving, intelligent robots, and drones. The visual-inertial fusion SLAM method introduces an inertial navigation sensor (IMU) on top of visual SLAM, fusing the camera's video stream and the IMU's motion information to obtain more accurate and robust localization results.

[0003] Aerial work platforms are mobile work platforms used for various industries to perform high-altitude operations, equipment installation, maintenance, and other tasks. The main types of aerial work platforms include: scissor lifts, trailer-mounted aerial work platforms, articulated boom lifts, telescopic boom lifts, aluminum alloy aerial work platforms, telescopic cylinder lifts, and spider lifts. They are widely used in various industries and daily life.

[0004] Currently, the operation of aerial work platforms is entirely done manually, which creates blind spots and poses risks such as being crushed or electrocuted at height. In addition, relying on human experience carries the risk of unauthorized operation. This invention introduces SLAM technology from the field of autonomous robots into the field of aerial work. By using a low-cost camera and inertial sensor system, it enables the aerial work platform to achieve autonomous positioning and environmental perception. The aim is to solve the pain points in the current field of aerial work by improving the intelligence level of the equipment. Summary of the Invention

[0005] To leverage the high sampling frequency of the IMU (Inertial Measurement Unit) to increase the output frequency of the SLAM (Simultaneous Localization and Mapping) system and address the issues of low accuracy and robustness of existing monocular vision SLAM methods applied to aerial work platforms, this invention provides a vision-inertial navigation fusion positioning device and method for aerial work machinery.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] A vision-inertial navigation fusion positioning device for aerial work machinery includes a fixed gimbal support, a monocular camera, an IMU inertial sensor, a power supply, a computing main control unit, and a time synchronization system. The device is rigidly fixed to the end of the boom lift platform and moves with the work bucket. When the boom lift starts working, the hydraulic arm is manipulated to control rotation and lifting. The device is simultaneously activated at the start of lifting. The monocular camera exposes at a fixed frame rate to perceive the environment, and the IMU inertial sensor performs pre-integration to acquire rotation and displacement increments. The image data and IMU inertial sensor data are fed back to the computing main control unit. The time synchronization system ensures that the monocular camera's image exposure time and the IMU inertial sensor's data acquisition time are synchronized.

[0008] Furthermore, the monocular camera is a global shutter camera or a rolling shutter camera, and depending on the field of view, it is divided into a pinhole camera or a fisheye camera.

[0009] Furthermore, the IMU inertial sensor is synchronized with the satellite clock via a PPS signal, and the signal is used to control the exposure of the monocular camera via a frequency divider. The trigger delay of the monocular camera and IMU inertial sensor signals is controlled in the nanosecond range.

[0010] Furthermore, the fixed frame rate is greater than 20Hz.

[0011] This invention also provides a vision and inertial navigation fusion positioning method for aerial work machinery, which is implemented through an equipment installation and calibration module, a time synchronization module, and an aerial positioning mapping algorithm module.

[0012] The gimbal bracket is designed through the device installation and calibration module to rigidly connect the monocular camera and the IMU inertial sensor, ensuring that the relative position between the monocular camera and the IMU inertial sensor does not change during the lifting and lowering of the aerial work platform. At the same time, the gimbal bracket is rigidly connected to the working bucket of the lifting and lowering aerial work platform to ensure that the IMU inertial sensor accurately reflects the motion posture of the working bucket of the lifting and lowering aerial work platform.

[0013] The time synchronization module keeps the exposure time of the monocular camera image synchronized with the data acquisition time of the IMU inertial sensor.

[0014] The high-altitude positioning and mapping algorithm module is used to fuse and process the working environment images sensed by the camera on the platform and the motion data sensed by the IMU inertial sensor, and output the positioning trajectory and the real-time pose of the work bucket. The high-altitude positioning and mapping algorithm module includes a high-altitude data acquisition preprocessing module, an algorithm initialization module, an algorithm nonlinear optimization module, a repositioning module, a global pose map optimization module, and a pose map management module.

[0015] Furthermore, after installation, the intrinsic and extrinsic parameters of the IMU inertial sensor and the monocular camera are calibrated. An intrinsic parameter model of the monocular camera and an intrinsic parameter model of the IMU inertial sensor are established. Gaussian white noise and random walk error are calibrated. The rotation and translation matrix between the IMU inertial sensor and the monocular camera is constructed using methods such as least squares.

[0016] Furthermore, the first stage of the time synchronization module is a GNSS receiving module, which acquires UTC true time data with nanosecond precision from satellites, converts it into a PPS signal through an FPGA module, and connects the PPS signal to the IMU / INS module to keep the inertial data timestamp of the IMU inertial sensor consistent with the UTC true time. At the same time, the PPS signal is connected to the frequency divider module to trigger the monocular camera according to the required frame rate. The monocular camera trigger signal is aligned with the PPS signal at the whole second edge, and the time delay between the two signals is within tens of nanoseconds.

[0017] Furthermore, the high-altitude data acquisition preprocessing module preprocesses the acquired images and IMU inertial sensor data through the sensing data acquisition device on the gimbal.

[0018] Furthermore, the algorithm initialization module performs the following steps: selecting the sliding window, setting the scale factor, converting the visually obtained pose to the IMU inertial sensor coordinate system, and performing visual-inertial calibration.

[0019] Furthermore, the nonlinear optimization module of the algorithm is used to add constraints to ensure the stable operation of the algorithm. The constraints include: IMU inertial sensor residual constraints, visual reprojection residuals of feature points in the sliding window under the monocular camera, and Marg marginalization residuals.

[0020] Furthermore, the relocation module is used for reusing the same location, specifically including:

[0021] (1) Loop closure detection: similarity is estimated based on the bag-of-words model for detection. At the same time, 500 corner points are added, and only the feature points are retained in each frame while discarding image information.

[0022] (2) Loop closure detection is successful. Remove outliers and use RANSAC and F matrix to test whether it is a loop closure. Use random sampling consensus algorithm and perspective point algorithm to recover the rotation matrix and displacement.

[0023] (3) Tightly coupled relocation: Align the current sliding window with the acquired pose map and perform relocation optimization.

[0024] Furthermore, the global pose graph optimization module is used to add keyframes. When a keyframe is marginalized during local optimization, it is added to the global pose graph. The keyframes have two connection methods:

[0025] (1) Sequential frames, sequentially connected;

[0026] (2) If the keyframe has a loop in the global pose graph, then it is connected to the loop closure frame.

[0027] Furthermore, the pose graph management module is used to avoid an infinitely large pose graph by retaining keyframes with loops and discarding keyframes near the loops.

[0028] Beneficial effects:

[0029] This invention proposes a low-cost, high-reliability solution for autonomous positioning of aerial work platforms. It selects low-cost cameras and IMUs as the environmental perception sensors at the front end of the aerial work platform positioning system. A high-precision time and space synchronization scheme is designed for the pre-fusion of camera image data and IMU motion data. An IMU pre-integration factor is added to the back-end positioning fusion algorithm, complementing the visual odometry frame-to-frame matching part. This makes the motion estimation and positioning results of the aerial work platform more robust, achieving better real-time pose estimation of the platform. Attached Figure Description

[0030] Figure 1 This is a diagram of the clock synchronization signal at the front end of the IMU inertial sensor of this invention;

[0031] Figure 2 A diagram of space synchronization data for the camera-inertial system;

[0032] Figure 3 The actual installation diagram and coordinate system of the camera-inertial sensor system and gimbal;

[0033] Figure 4 This is a schematic diagram of a camera-inertial sensor system and a gimbal.

[0034] Figure 5 A schematic diagram of the overall assembly of a vision and inertial navigation fusion positioning device and aerial work machinery.

[0035] Figure 6 The diagram shows the algorithm structure and data flow of the vision and inertial navigation fusion positioning method for aerial work machinery of the present invention.

[0036] Figure 7 This is the positioning trajectory and platform end pose diagram of the actual operation of the present invention. Detailed Implementation

[0037] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be 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 illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0038] like Figure 1 As shown, the vision and inertial navigation fusion positioning device for aerial work machinery of the present invention includes an equipment installation and calibration module, a time synchronization module, and an aerial positioning mapping algorithm module. Figure 3 , Figure 4 , Figure 5 As shown, the equipment installation and calibration module is used to design the gimbal bracket, rigidly connecting the monocular camera 1 and the IMU inertial sensor 2 to ensure that the relative position between the monocular camera and the IMU inertial sensor does not change during the lifting and lowering of the aerial work platform. Simultaneously, the gimbal bracket is rigidly connected to the working platform's bucket, ensuring that the IMU inertial sensor accurately reflects the motion posture of the working platform's bucket. The overall system is as follows: Figure 5 As shown in the diagram, the gimbal bracket is as follows: Figure 3 , Figure 4 As shown.

[0039] After installation, the intrinsic and extrinsic parameters of IMU inertial sensor 2 and monocular camera 1 are calibrated. An intrinsic parameter model of monocular camera 1 is established, such as a pinhole model or a fisheye model. An intrinsic parameter model of IMU inertial sensor 2 is also established. Gaussian white noise, random walk error, etc. are calibrated. The rotation and translation matrix between IMU inertial sensor 2 and monocular camera 1 is constructed using methods such as least squares. Figure 2 The figure shows the projection error between the monocular camera and the IMU inertial sensor.

[0040] The first stage of the time synchronization module is a GNSS receiving module, which acquires UTC true time data with nanosecond precision from satellites, converts it into a PPS signal through an FPGA module, and connects the PPS signal to the IMU / INS module to keep the inertial data timestamp of the IMU inertial sensor 2 consistent with the UTC true time. At the same time, the PPS signal is connected to the frequency divider module to trigger the monocular camera 1 according to the required frame rate. The trigger signal of the monocular camera 1 is aligned with the PPS signal at the whole second edge, and the time delay between the two signals is within tens of ns. Therefore, the exposure time of the monocular camera 1 and the data acquisition time of the IMU inertial sensor 2 are kept synchronized.

[0041] The high-altitude positioning and mapping algorithm module includes a high-altitude data acquisition preprocessing module, an algorithm initialization module, an algorithm nonlinear optimization module, a relocation module, a global pose graph optimization module, and a pose graph management module.

[0042] The high-altitude data acquisition preprocessing module preprocesses the acquired images and data from the IMU inertial sensor 2 through the sensing data acquisition device on the gimbal.

[0043] The preprocessing includes:

[0044] (1) Feature point extraction: KLT pyramid optical flow is used to track adjacent frames and optical flow method is used for feature matching.

[0045] (2) The feature points are projected onto the unit sphere, and then the F matrix is ​​used to test and remove the points that are mismatched. Then RANSAC (Random Sampling Consensus Algorithm) is used to remove the outliers.

[0046] (3) Key frame selection: In order to ensure triangulation, this invention determines a key frame if the average disparity of features between the current frame and the adjacent key frame is greater than a threshold, or if the number of feature points in the current frame is greater than a threshold.

[0047] (4) IMU inertial sensor 2 calculates the pre-integration between frames k and k+1.

[0048] The functions of the algorithm initialization module include:

[0049] (1) Select a sliding window. Find a sliding window in the last frame and the frame before the sliding window that satisfies the requirement that the number of feature points to be tracked is greater than 30 and the disparity is greater than 20. Then perform the recovery rotation matrix and translation.

[0050] (2) Set the scale factor, triangulate the landmark points observed in the two frames, i.e. the points for mapping, and use PnP (Perspective-n-Point algorithm) to calculate the pose of the remaining frames in the sliding window.

[0051] (3) Transform the pose obtained from vision into the IMU inertial sensor coordinate system (external parameters have been calibrated).

[0052] (4) Visual inertial calibration is achieved by calibrating the external parameters between the monocular camera 1 and the IMU inertial sensor 2 of the high-altitude gimbal.

[0053] The nonlinear optimization module of the algorithm serves to address the numerous uncontrollable factors that may affect high-altitude operations, thus requiring the addition of constraints to ensure stable algorithm operation. These constraints include:

[0054] (1) IMU inertial sensor residual constraint is generated by the IMU inertial sensor between adjacent frames in the sliding window.

[0055] (2) Visual reprojection residuals of feature points in the sliding window under a monocular camera.

[0056] (3) Marg marginalizes the residual part and optimizes the calculation of the H matrix after the new sliding window frame is added.

[0057] The repositioning module serves to ensure the full utilization of overlapping positional information at high altitudes and the reuse of identical locations. Specifically, it includes:

[0058] (1) Loop closure detection: similarity is estimated based on the bag-of-words model for detection. At the same time, 500 corner points are added, and only the feature points are retained in each frame while discarding image information.

[0059] (2) Loop closure detection was successful. Outliers were removed. RANSAC and the F matrix were used to test for loop closure. RANSAC+PNP (RANSAC - Random Sample Consensus Algorithm + PnP (Perspective-n-Point Algorithm)) was used to recover the rotation matrix and translation. PnP (Perspective-n-Point Algorithm) is a method for solving the motion of 3D to 2D point pairs. When n 3D spatial points and their projected positions are known, this algorithm is used to estimate the camera pose.

[0060] (3) Tightly coupled relocation: Align the current sliding window with the acquired pose map and perform relocation optimization. However, unlike the front-end optimization, the loopback frame is used as a constant. All the observations of the IMU inertial sensors, local camera information, and the corresponding feature points in the loopback frame are used as constraints to jointly optimize the current frame.

[0061] The global pose graph optimization module adds keyframes. When a keyframe is marginalized during local optimization, it is added to the global pose graph. At this point, the keyframe has two connection methods:

[0062] (1) Sequential frames, sequentially connected;

[0063] (2) If the keyframe has a loop in the global pose graph, then it is connected to the closed loop frame. Adding a frame requires 4-DOF optimization.

[0064] The pose graph management module is designed to handle the large pose graph generated by the continuous input of environmental perception sensor groups, including a monocular camera and an IMU inertial sensor, from the gimbal as high-altitude operations proceed. In order to avoid an infinitely large pose graph, keyframes with loops are retained, while keyframes near the loops are discarded.

[0065] Specifically, such as Figure 1As shown, ① represents the 1Hz PPS clock synchronization signal displayed on the oscilloscope, and ② represents the 20Hz camera synchronization exposure trigger signal generated by the PPS signal after frequency division. The IMU inertial sensor 2 synchronizes with the satellite clock via the PPS signal, and the signal controls the exposure of the monocular camera 1 through the frequency divider. The trigger delay of the signals from the monocular camera 1 and the IMU inertial sensor 2 is controlled in the nanosecond range. Figure 2 As shown, the monocular camera 1 and the IMU inertial sensor 2 are used for spatial calibration, with a projection accuracy error within 1 pixel. Figure 3 , Figure 4 As shown, the device of the present invention consists of a fixed gimbal support, a monocular camera 1, an IMU inertial sensor 2, a power supply, a computing main control unit, a time synchronization system, and other components. The monocular camera 1 can be a global shutter camera or a rolling shutter camera, and according to different field of view angles, it can be a pinhole camera or a fisheye camera.

[0066] The device of this invention is rigidly fixed to the end of the bucket truck lifting platform and moves with the bucket. When the bucket truck starts working, the operator manipulates the hydraulic arm to control rotation and lifting. The system is turned on synchronously at the start of lifting. The monocular camera 1 exposes at a fixed frame rate to perceive the environment (usually greater than 20Hz), and the IMU inertial sensor 2 performs pre-integration to obtain rotation and displacement increments. The image data and IMU inertial sensor data are fed back to the main control unit.

[0067] like Figure 6 As shown, this invention first extracts features from the data collected by a monocular camera and an IMU (Inertial Measurement Unit) in a high-altitude work platform, and then pre-integrates the data from the IMU. After feature matching, the rotation matrix and translation of the current data frame acquired by the gimbal are solved. Due to the large amount of data during high-altitude operations, a sliding window is used to limit the number of keyframes in this invention. Finally, the overall environment construction and positioning of the entire high-altitude operation are optimized. The IMU and monocular camera collect images of the high-altitude environment and extract features, while simultaneously collecting IMU data (three-axis acceleration, angular velocity, and magnetometer data) of the platform's movement and pre-integrating the IMU data. After execution, the system checks whether initialization is complete. If not, it sets the initial state based on visual SFM information and initial calibration information and then proceeds to the positioning section. If it is complete, it directly proceeds to the positioning section. The positioning module predicts the monocular camera's attitude using the IMU inertial sensor, performs odometry and stereo information calculation through image feature point triangulation, optimizes the calculation using a sliding window, deletes old state variables and adds new state variables to ensure sufficient computing power, and finally outputs the positioning trajectory and pose information. Figure 7 The image shows the real-time estimation of the position and attitude of an aerial work platform and the optimized trajectory based on monocular inertial data.

[0068] Those skilled in the art will readily understand 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 within the scope of protection of the present invention.

Claims

1. A vision and inertial navigation fusion positioning device for aerial work machinery, characterized in that: The device includes a fixed gimbal bracket, a monocular camera, an IMU inertial sensor, a power supply, a computing main control unit, and a time synchronization system. The device is rigidly fixed to the end of the boom lift platform and moves with the bucket. When the boom lift starts working, the hydraulic arm is manipulated to control rotation and lifting. The device is simultaneously powered on at the start of lifting. The monocular camera exposes at a fixed frame rate to perceive the environment, and the IMU inertial sensor performs pre-integration to acquire rotation and displacement increments. The image data and IMU inertial sensor data are fed back to the computing main control unit. The time synchronization system ensures that the monocular camera's image exposure time is synchronized with the IMU inertial sensor's data acquisition time. It also includes a relocation module for reusing the same location, specifically including: (1) Loop closure detection: similarity is estimated based on bag-of-words model for detection, and 500 corner points are added. Only the feature points are retained in each frame, and the image information is discarded. (2) If the loop closure detection is successful, remove the outliers, use RANSAC and F matrix to test whether it is a loop closure, and use the random sampling consensus algorithm and perspective point algorithm to recover the rotation matrix and displacement; (3) Tightly coupled relocation: Align the current sliding window with the acquired pose map and perform relocation optimization.

2. The vision and inertial navigation fusion positioning device for aerial work machinery according to claim 1, characterized in that: The monocular camera is either a global shutter camera or a rolling shutter camera, and can be classified as a pinhole camera or a fisheye camera depending on the field of view.

3. The vision and inertial navigation fusion positioning device for aerial work machinery according to claim 1, characterized in that: The IMU inertial sensor is synchronized with the satellite clock via a PPS signal, and the signal is used to control the exposure of the monocular camera via a frequency divider. The trigger delay of the monocular camera and IMU inertial sensor signals is controlled in the nanosecond range.

4. The vision and inertial navigation fusion positioning device for aerial work machinery according to claim 1, characterized in that: The fixed frame rate is greater than 20Hz.

5. A positioning method for a vision and inertial navigation fusion positioning device for aerial work machinery according to any one of claims 1-4, characterized in that: The positioning method is implemented through an equipment installation and calibration module, a time synchronization module, and a high-altitude positioning and mapping algorithm module. The gimbal bracket is designed through the device installation and calibration module to rigidly connect the monocular camera and the IMU inertial sensor, ensuring that the relative position between the monocular camera and the IMU inertial sensor does not change during the lifting and lowering of the aerial work platform. At the same time, the gimbal bracket is rigidly connected to the working bucket of the lifting and lowering aerial work platform to ensure that the IMU inertial sensor accurately reflects the motion posture of the working bucket of the lifting and lowering aerial work platform. The time synchronization module keeps the exposure time of the monocular camera image synchronized with the data acquisition time of the IMU inertial sensor. The high-altitude positioning and mapping algorithm module is used to fuse and process the images of the working environment sensed by the camera on the platform and the motion data sensed by the IMU inertial sensor, and output the positioning trajectory and the real-time pose of the work bucket. The high-altitude positioning and mapping algorithm module includes a high-altitude data acquisition preprocessing module, an algorithm initialization module, an algorithm nonlinear optimization module, a repositioning module, a global pose graph optimization module, and a pose graph management module. The relocation module is used to reuse the same location, specifically including: (1) Loop closure detection: similarity is estimated based on bag-of-words model for detection, and 500 corner points are added. Only the feature points are retained in each frame, and the image information is discarded. (2) If the loop closure detection is successful, remove the outliers, use RANSAC and F matrix to test whether it is a loop closure, and use the random sampling consensus algorithm and perspective point algorithm to recover the rotation matrix and displacement; (3) Tightly coupled relocation: Align the current sliding window with the acquired pose map and perform relocation optimization.

6. The positioning method according to claim 5, characterized in that: After installation, the intrinsic and extrinsic parameters of the IMU inertial sensor and the monocular camera are calibrated. An intrinsic parameter model of the monocular camera and an intrinsic parameter model of the IMU inertial sensor are established. Gaussian white noise and random walk error are calibrated. The rotation and translation matrix between the IMU inertial sensor and the monocular camera is constructed using the least squares method.

7. The positioning method according to claim 5, characterized in that: The first stage of the time synchronization module is a GNSS receiving module, which acquires UTC true time data with nanosecond precision from satellites, converts it into a PPS signal through an FPGA module, and connects the PPS signal to the IMU / INS module to keep the inertial data timestamp of the IMU inertial sensor consistent with the UTC true time. At the same time, the PPS signal is connected to the frequency divider module to trigger the monocular camera according to the required frame rate. The monocular camera trigger signal is aligned with the PPS signal at the whole second edge, and the time delay between the two signals is within tens of ns.

8. The positioning method according to claim 5, characterized in that: The high-altitude data acquisition preprocessing module preprocesses the acquired images and IMU inertial sensor data through the sensing data acquisition device on the gimbal.

9. The positioning method according to claim 5, characterized in that: The algorithm initialization module performs tasks such as selecting the sliding window, setting the scale factor, converting the visually obtained pose to the IMU inertial sensor coordinate system, and performing visual-inertial calibration.

10. The positioning method according to claim 5, characterized in that: The nonlinear optimization module of the algorithm is used to add constraints to ensure the stable operation of the algorithm. The constraints include: IMU inertial sensor residual constraints, visual reprojection residuals of feature points in the sliding window under the monocular camera, and Marg marginalization residuals.

11. The positioning method according to claim 5, characterized in that: The global pose graph optimization module is used to add keyframes. When a keyframe is marginalized during local optimization, it is added to the global pose graph. The keyframes have two connection methods: (1) Sequential frames, sequentially connected; (2) If the keyframe has a loop in the global pose graph, then it is connected to the loop closure frame.

12. The positioning method according to claim 5, characterized in that: The pose graph management module is used to avoid infinitely large pose graphs by retaining keyframes with loops and discarding keyframes near the loops.

Images