Mine carry-scraper positioning method based on active VIO-AMCL

Abstract

The invention relates to the technical field of mine carry-scraper positioning, in particular to a mine carry-scraper positioning method based on an active VIO-AMCL, and provides a positioning framework VIO-AMCL which is based on an adaptive Monte Carlo positioning AMCL algorithm and fuses depth visual point cloud and a visual inertial odometer VIO. The method comprises the following steps: firstly, based on depth information obtained by a depth vision point cloud model, generating a mine roadway three-dimensional point cloud, mapping the mine roadway three-dimensional point cloud to a global coordinate space, and updating an importance factor to obtain a depth vision observation model; secondly, based on a homogenized ORB feature point extraction strategy and IMU pre-integration, a binocular camera and an inertial measurement unit IMU are combined to realize tight coupling VIO, and high-precision pose information is obtained so as to perform particle swarm motion state prediction; finally, the movement mechanism of the carry-scraper is analyzed, and a carry-scraper movement model with the front vehicle body as the center is obtained. The method has more stable positioning performance, the positioning performance of the mine carry-scraper can be improved, and the positioning requirement of the mine carry-scraper is met.

Description

Technical Field

[0001] The present invention relates to the technical field of mine load-haul-dump (LHD) positioning, and specifically provides a positioning method for mine LHDs based on active Visual-Inertial Odometry (VIO)-AMCL (Adaptive Monte Carlo Localization). Background Art

[0002] With the continuous increase in coal mining depth, the traditional manual operation mode is difficult to meet the safety production requirements in complex mine environments. As the core equipment for underground material transportation, the intelligent transformation of mine LHDs has become a key link in realizing unmanned mining operations. Existing driverless LHDs mostly rely on global navigation systems. However, the deep mine environment has a complete shielding effect on satellite signals, resulting in the ineffective operation of systems such as GPS and Beidou. Although alternative solutions such as radio frequency identification and inertial navigation can be adopted, they have significant defects such as high equipment deployment costs (requiring the establishment of a dense base station network), complex maintenance (the harsh underground environment affects equipment reliability), and insufficient positioning accuracy (commonly with errors of more than ±5 meters). Especially in dangerous working faces such as gas-rich areas and geological structure zones, the lack of reliability of existing positioning technologies directly affects the obstacle avoidance ability and path planning accuracy of unmanned LHDs, seriously restricting the realization of 24-hour continuous operation.

[0003] The positioning method described in the Chinese patent "CN 113960529 A Positioning Device and Positioning Method for Mine Load-haul-dump Machines" is a scheme that combines Ultra-Wideband (UWB) signals and lidar. By deploying a UWB signal transmission module on the roof of the mine LHD, the longitudinal position of the equipment is determined in real time. At the same time, lidars are installed on the front and rear bodies of the vehicle respectively, and high-precision longitudinal distance information is obtained synchronously using laser ranging. Combined with the lateral position data, coordinate calculation is performed to finally achieve the precise positioning of the LHD. However, lidars may have ranging errors when the dust concentration is too high, and the UWB multipath effect may cause positioning drift. The accuracy after fusion is usually centimeter-level. Additional sensors (such as encoders or gyroscopes) are required to provide lateral position data, increasing the system complexity. Moreover, the cost of deploying UWB base stations and lidars is relatively high.

[0004] Moreover, although the existing AMCL algorithm reduces the demand for computing resources, it relies on a single sensor and is difficult to adapt to unstructured roadway environments. Summary of the Invention

[0005] Aiming at the defects in the existing technology, to achieve lightweight, low-cost, and high-precision positioning of mine engineering vehicles, the present invention provides a positioning method for mine LHDs based on active VIO-AMCL to improve the positioning performance of LHDs and enhance the level of unmanned mine construction.

[0006] To achieve the above objectives, the following technical solutions are provided:

[0007] A positioning method for a mine loader based on active VIO-AMCL, characterized by comprising the following steps:

[0008] Step 1: Construct a depth vision observation model; obtain three-dimensional point cloud data of the mine roadway environment through an RGB-D camera, generate a three-dimensional point cloud through a coordinate transformation formula, and map it to the global coordinate system of the prior laser map;

[0009] Step 2: Construct a binocular vision inertial tightly coupled module; extract uniformized ORB feature points from binocular images, combine IMU pre-integration data, and output pose information through a tightly coupled visual inertial odometer (VIO);

[0010] Step 3: Input the pose data output in Step 2 into an adaptive Monte Carlo localization (AMCL) framework (VIO-AMCL algorithm), combine the articulated motion model of the loader to perform particle swarm pose prediction and weight update, and output the real-time positioning result of the loader.

[0011] Preferably, in Step 1, the specific steps of constructing the depth vision observation model are as follows:

[0012] (1) Based on the RGB-D camera obtaining three-dimensional point cloud data, deduce the conversion from a two-dimensional image to a three-dimensional image, construct a point cloud conversion model based on the RGB image and the depth image, and provide the observation data required for the loader positioning;

[0013] (2) Construct a binocular VIO method mainly based on ORB and combined with IMU data to replace the original wheel odometer, and its output is used as the pose data for Monte Carlo localization;

[0014] (3) Combine the AMCL framework to perform particle probability calculation to deduce the high-precision positioning of the loader, and finally complete the entire VIO-AMCL positioning process.

[0015] Preferably, in Step 1, the coordinate transformation formula for the three-dimensional space coordinate points X, Y, and Z is:

[0016]

[0017] Z = depth(3)

[0018] where f x represents the focal length of the x-axis of the imaging coordinate system, f y represents the focal length of the y-axis of the imaging coordinate system, c x represents the x-coordinate of the base point, which is the center of the image plane along the x-axis, c y represents the y-coordinate of the base point, which is the center of the image plane along the y-axis, u and v are the coordinates in the 2D image, and depth is the depth of the pixel point.

[0019] Preferably, in Step 1, the transformation relationship for mapping the point cloud scanned by the 3D point cloud to the global coordinate system of the prior laser map is shown as follows:

[0020]

[0021] where x and y represent the pose of the device body at this moment, x depth and y depth represent the central coordinates of the depth camera relative to the device body, and θ depth and z t represent the orientation angle and the end point of the depth point cloud relative to the device body respectively.

[0022] Preferably, in Step 2, the specific steps for constructing the binocular vision inertial tightly coupled module are as follows:

[0023] (1) Introduce a quadtree for the extracted ORB feature points to achieve uniform distribution of feature points, and solve the problem of feature redundancy caused by the lack of texture in the mine roadway;

[0024] (2) Optimize the inertial data between key frames through IMU pre-integration, reduce the computational amount and suppress the cumulative error;

[0025] (3) Construct a tightly coupled VIO based on dual cameras and IMU, so that the sensors are fully complementary, while reducing the cumulative error, improving the pose estimation accuracy.

[0026] Preferably, in Step 2, the processing process of the IMU pre-integration includes: integrating the IMU data between camera key frames, constructing an error function by providing the measurement values between two frames, and finally iteratively optimizing the frame pose, using the relative value of the IMU running for a period of time instead of the absolute value of the IMU at a certain moment, that is, the IMU pre-integration factor.

[0027] Preferably, in Step 2, the process of visual inertial initialization in the visual inertial odometer (VIO) includes pure vision maximum a posteriori estimation, pure IMU maximum a posteriori probability estimation, and visual inertial maximum a posteriori probability estimation;

[0028] The pure vision maximum a posteriori estimation runs at a frequency of inserting key frames at 4 Hz, constructs a visual map with scale uncertainty, which contains hundreds of map points of 10 key frames, and uses the method of pure vision optimization to optimize the poses of each frame, and obtains the corresponding coordinate poses from the poses of each frame where R represents the corresponding rotation matrix and p represents the corresponding pose information;

[0029] The pure IMU maximum a posteriori probability estimation estimates the parameters and scale of the IMU through the value of IMU pre-integration between key frames:

[0030]

[0031] where s represents the scale map, and R wg ∈ SO(3) is the rotation matrix of gravity, which represents the rotation relationship from the first reference coordinate system in the map to the real-world coordinate system, and b is the IMU acceleration and gyroscope bias, is the scale-based velocity;

[0032] The visual-inertial maximum a posteriori probability estimation: After completing the pure-vision maximum a posteriori estimation and the pure-IMU maximum a posteriori probability estimation, optimize the visual error and the inertial error simultaneously, and ensure that there is the same bias b a and b g , and has prior information as in the previous step.

[0033] Preferably, in step three, the construction method of the articulated movement model of the scraper includes the following steps:

[0034] (1) Define the central coordinate and steering angle relationship between the front vehicle body and the rear vehicle body

[0035] x a = x b - L b cosθ b - L a cosθ a , (6)

[0036] y a = y b - L b sinθ b - L a sinθ a ; (7)

[0037] where P A is the articulation point, P a (x a ,y a ) is the center of the front vehicle body, P b (x b ,y b ) is the center of the rear vehicle body (rear-wheel braking), the direction angles of the front vehicle body and the rear vehicle body are θ a and θ b , the distance between P A and P a is L a , the distance between P A and P b is L b , and the angle γ is the angle difference between θ a and θ b : γ = θb -θ a ;

[0038] (2) Based on the articulated steering angular velocity and the vehicle body speed, establish a kinematic equation to predict the position and pose of the scraper:

[0039] The center position P of the front and rear vehicle bodies i is expressed as

[0040]

[0041] where v i is the associated speed, v a is the speed of the front vehicle body, is the angular speed of the γ angle during articulated steering, is the coordinate of point P, are the motion parameters at the motion moment.

[0042] Preferably, in step three, the key steps of the VIO-AMCL algorithm are as follows:

[0043] (1) Load the prior map and initialize the algorithm;

[0044] (2) Particle swarm initialization: Initialize the particle set in the prior map The number of the particle swarm is M, and the same importance factor M is assigned to each particle -1 ;

[0045] (3) Particle swarm pose prediction: Predict the pose of the next state of the particle set according to the VIO odometer information, and obtain the predicted particle set X using the motion model based on Equation (8) t+1 ;

[0046] (4) Update the importance factor of the particle swarm: Update the importance factor w of each particle according to the visual point cloud data, map data information and particle set X in claim 3 t+1 to obtain the particle set after observation t+1

[0047] (5) Particle swarm normalization and resampling: Normalize the importance factor weights

[0048]

[0049] and judge whether to perform KLD sampling according to the comparison between the effective number of particles

[0050]

[0051] and the preset threshold M T If M eff > M TIt is stated that to disperse the particle swarm, the upper limit of the particle swarm quantity needs to be increased; otherwise, the particle swarm quantity is decreased, aiming to improve the particle utilization rate and reduce particle redundancy.

[0052] (6) After the particle swarm calculated according to the importance factor in (4) is clustered according to the pose, the one with the largest weight in the particle cluster is output as the positioning result, and then (3) to (6) are repeated.

[0053] The present invention proposes a positioning framework VIO-AMCL based on the Adaptive Monte Carlo Localization (AMCL) algorithm, which integrates the depth vision point cloud and the Visual Inertial Odometry (VIO). First, based on the depth information obtained from the depth vision point cloud model, a three-dimensional point cloud of the mine roadway is generated and mapped to the global coordinate space, and the depth vision observation model is updated through the importance factor. Second, based on the uniformized ORB feature point extraction strategy and IMU pre-integration, the binocular camera and the Inertial Measurement Unit (IMU) are jointly used to achieve tightly coupled VO to obtain high-precision pose information for predicting the motion state of the particle swarm. Finally, the movement mechanism of the load-haul-dump vehicle is analyzed to obtain the movement model of the load-haul-dump vehicle centered on the front vehicle body.

[0054] The active positioning of the present invention reduces the deployment cost by reducing the establishment of a large number of auxiliary positioning beacons.

[0055] The beneficial effects of the present invention are as follows:

[0056] The present invention proposes a positioning method for mine loaders based on active VIO-AMCL. Through the construction of a depth vision observation model, 3D point cloud data of the mine roadway is obtained using an RGB-D camera. The point cloud is mapped to the global coordinate system using coordinate transformation formulas (combining parameters such as focal length and base point coordinates), and aligned with the prior laser map to generate an observation model containing depth information, providing environmental geometric constraints for positioning. Then, the binocular camera and IMU are heterogeneously fused: the problem of missing mine textures is solved by extracting uniformized ORB feature points, and the inertial data between key frames is optimized by combining IMU pre-integration to construct a tightly coupled VIO module. IMU pre-integration reduces the cumulative error by calculating the relative motion between frames, and at the same time initializes the scale, gravity direction, and sensor bias using the maximum a posteriori estimation of pure vision and pure IMU, and finally fuses the visual and inertial errors to output high-precision pose information. Finally, based on the articulated kinematic model of the loader (defining the relationship between the center coordinates of the front and rear vehicle bodies, steering angle, and speed), the pose output by VIO is input into the AMCL framework, and positioning optimization is achieved through particle swarm prediction, weight update, and resampling. After the particle swarm is initialized, the pose is predicted according to the motion model, the particle weights are updated by combining the point cloud observation data, the number of particles is adjusted by normalization and dynamic KLD sampling, and the particle cluster with the largest weight value is output as the positioning result, improving robustness and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 Schematic diagram of the VIO-AMCL positioning framework in Embodiment 1 of the present invention;

[0058] Figure 2 Schematic diagram of the 3D point cloud coordinate transformation principle in Embodiment 1 of the present invention;

[0059] Figure 3 Schematic diagram of pure vision pose estimation in Embodiment 1 of the present invention;

[0060] Figure 4 Schematic diagram of the articulated kinematic model of the loader in Embodiment 1 of the present invention;

[0061] Figure 5 Schematic diagram of the core framework of VIO-AMCL in Embodiment 1 of the present invention;

[0062] Figure 6 Field view in Embodiment 2 of the present invention;

[0063] Figure 7 Map effect diagram in Embodiment 2 of the present invention;

[0064] Figure 8 Comparison diagram of the positioning error distribution in Embodiment 2 of the present invention;

[0065] Figure 9This is the positioning deviation situation diagram under the actual working conditions in Embodiment 2 of the present invention. Detailed implementation manners

[0066] The following combines the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts fall within the protection scope of the present invention.

[0067] Embodiment 1

[0068] A positioning method for a mine load-haul-dump vehicle based on active VIO-AMCL, as Figure 1 shown, includes the following steps:

[0069] Step 1: Obtain three-dimensional point cloud data of the mine roadway environment through an RGB-D camera, generate a three-dimensional point cloud through a coordinate transformation formula, and map it to the global coordinate system of the prior laser map to construct a depth vision observation model. The specific implementation method is as follows:

[0070] The experimental site of the present invention is selected in a mine roadway of a coal mine in Jining City, Shandong Province. The experimental site is a long and curved roadway with multiple fork roads. The main experimental section is selected on a relatively flat road surface. Due to the uncontrollability of the underground structure, the whole process does not include a closed loop. When constructing the prior map, the environment and obstacle information scanned by the lidar are used to construct a two-dimensional grid map based on the SLAM_TOOL algorithm to obtain the experimental map.

[0071] An additional dimension, namely depth, is added to the traditional two-dimensional color image. The two-dimensional image provides information about the position and color of an object on the horizontal and vertical planes, while the depth dimension adds information about the distance of the object from the observer or measuring device. The RGB-D format is a commonly used 3D image format. The pixels in the image not only contain the color information of red, green, and blue, but also additionally contain the depth value of the pixel point, that is, its depth position information in space.

[0072] The generation of 3D point cloud from a 2D image takes the camera internal parameter matrix as the input, converts the obtained depth point cloud and RGB image (as Figure 2 shown), and solves the pose according to the matrix relationship to generate a three-dimensional visual point cloud observation model of the roadway.

[0073] Step 2: Extract uniformized ORB feature points from the binocular image, combine the IMU pre-integration data, and output pose information through a tightly coupled visual inertial odometer (VIO);

[0074] Select four target points in the experimental environment and set the preset poses. After extracting the ORB feature points, introduce a quadtree to achieve uniform distribution of the feature points. Then, perform feature matching on adjacent frame images through descriptors to implement the pure vision pose estimation process and output the initial pose, as Figure 3 shown.

[0075] Perform pre-integration operations on the IMU data to avoid redundant data increasing the computational load and avoid repeated integration. By integrating the IMU data between camera key frames, use the relative value of the IMU running for a period of time to replace the absolute value of the IMU at a certain moment, that is, the IMU pre-integration factor.

[0076] Step 3: Input the pose data output in Step 2 into the Adaptive Monte Carlo Localization (AMCL) framework, combine the articulated motion model of the scraper to predict the pose of the particle swarm and update the weights, and output the real-time positioning result of the scraper.

[0077] Figure 4 (a) is a scraper operating in an underground coal mine in Jining City, Shandong Province. After measurement, the wheelbase of the vehicle is 2.5 meters and the maximum steering angle is 45 degrees. It belongs to an articulated vehicle with the body divided into front and rear ends and has a small turning radius in the mine environment. The model schematic diagram is as Figure 4 (b) shown. Where P A is the articulation point, P a (x a , y a ) is the center of the front body, P b (x b , y b ) is the center of the rear body (rear wheel braking). The direction angles of the front body and the rear body are θ a and θ b , respectively. The distance between P A and P a is L a , and the distance between P A and P b is L b , and the angle γ is the angle difference between θ a and θ b : γ = θ b - θ a , and the relationship between P a and P b is as follows:

[0078] x a = x b - L b cosθ b - L a cosθ a

[0079] y a = y b -L b sinθ b -L a sinθ a

[0080] Associated velocity v i with the center position P of the front and rear body of the vehicle i , express P i as

[0081]

[0082] Under ideal conditions, ignoring the influence of tire deformation and vehicle body slip, the kinematic model is obtained:

[0083]

[0084] where v a is the speed of the front body of the vehicle, is the angular velocity of the γ angle during articulated steering.

[0085] The method of the present invention analyzes the movement model of the scraper. Based on the AMCL framework, the VIO output pose is used to update the pose of the particle set, and then the particle weight is updated based on the measurement model of the visual point cloud, as Figure 5 shown. Compared with the original passive positioning, the active positioning of the present invention not only abandons the establishment of a large number of auxiliary positioning beacons, but also reduces the deployment cost. The specific implementation method is as follows:

[0086] (1) Load the prior map and initialize the algorithm;

[0087] (2) Particle swarm initialization: Initialize the particle set in the prior map The number of the particle swarm is M, and the same importance factor M is assigned to each particle -1 ;

[0088] (3) Particle swarm pose prediction: Predict the pose of the next state of the particle set according to the VIO odometer information, and obtain the predicted particle set X using the motion model based on the following formula t+1 ;

[0089]

[0090] (4) Particle swarm importance factor update: Update the importance factor w of each particle according to the visual point cloud data, map data information and particle set X t+1 , and obtain the particle set after observation t+1 ,

[0091] (5) Particle swarm normalization and resampling: Normalize the importance factor weights

[0092]

[0093] And based on the number of effective particles

[0094]

[0095] Compare with the preset threshold M T To determine whether to perform KLD sampling. If M eff > M T It indicates that the particle swarm is dispersed, and the upper limit of the particle swarm quantity needs to be increased. Otherwise, the particle swarm quantity is reduced to achieve the purpose of improving particle utilization and reducing particle redundancy;

[0096] (6) Cluster the particle swarm calculated according to the importance factor according to the pose, and output the one with the largest weight value in the particle cluster as the positioning result. Then repeat steps (3) to (6).

[0097] Embodiment 2

[0098] Evaluation indicators are the key tools to measure the accuracy of pose estimation and the positioning effect of the load-haul-dump machine. They can help understand the advantages and disadvantages of the positioning method of the present invention and the existing methods.

[0099] The experimental site of the present invention is selected in a coal mine roadway in Jining City, Shandong Province. The experimental site is a long and curved roadway with multiple fork roads, as Figure 6 shown. The main experimental section is selected on a relatively flat road surface. Due to the uncontrollability of the underground structure, the whole process does not include closed loops.

[0100] When constructing the prior map, the environmental and obstacle information scanned by the lidar is used, and a two-dimensional grid map is constructed based on the SLAM_TOOL algorithm to obtain the experimental map, as Figure 7 shown. Four target points are selected in the experimental environment, and the preset poses are given. The pose selection values of the four target points are shown in the table. Among them, (x, y, w) is the pose expression of the vehicle, x and y represent the positions in the two-dimensional plane, and w is the yaw angle radian value. In addition, by fixing the camera sensor OAK-D-PRO-W outside the load-haul-dump machine (equipped with a binocular camera and an RGB-D camera at the same time), and riding on the load-haul-dump machine to record the positioning information during the route driving.

[0101] Table 1 Pose selection values of four target points

[0102]

[0103] Since the load-haul-dump (LHD) vehicle selected for the experimental platform has a traditional mechanical structure, for the experimental results to be referenceable, only the pose of the front shovel body of the LHD vehicle when passing through the target point is used as data recording. To evaluate the positioning accuracy of the LHD vehicle positioning method based on active VIO-AMCL, the AMCL, VIO, and VIO-AMCL methods are compared and analyzed. The positioning effect is measured by the error between the real-time manually measured pose and the output pose of the LHD vehicle in the upper computer, as well as the angular error.

[0104] Error evaluation criteria:

[0105] Since the load-haul-dump (LHD) vehicle selected for the experimental platform has a traditional mechanical structure, for the experimental results to be referenceable, only the pose of the front shovel body of the LHD vehicle when passing through the target point is used as data recording. The positioning accuracy of the LHD vehicle can be measured by the error between the real-time manually measured pose and the output pose of the LHD vehicle in the upper computer, including both position error and angular error. The position error σ mainly includes the errors Δx and Δy in the X-axis and Y-axis directions:

[0106] The position error σ mainly includes the errors Δx and Δy in the X-axis and Y-axis directions:

[0107]

[0108] Δθ represents the angular error. The stability δ r represents whether the LHD vehicle can maintain a stable positioning calculation output under a high-load operation mode. In the present invention, the standard deviation is used to define

[0109]

[0110] wherein, represents the average position error.

[0111] Verification:

[0112] After selecting the target points, manually input four target points into the system as the navigation points for the LHD vehicle. And after the LHD vehicle reaches each target point, manually measure the pose and then drive to the next target point. A total of 10 groups of experiments are carried out, and the positioning errors are recorded. The 1st, 4th, and 7th times are intercepted as Figure 8 shown. The red dots represent the distribution of error points of the original AMCL, the blue dots represent the distribution of error points of the AMCL after only fusing VIO, and the green dots represent the distribution of error points of the algorithm of the present invention. It can be seen from the figure that the algorithm of the present invention has a more convergent error distribution compared with the original AMCL algorithm and the AMCL algorithm fusing VIO. This enables the LHD vehicle to obtain more accurate positioning information during roadway driving and at the same time solves the problem of pose estimation of the vehicle body during the movement of the LHD vehicle.

[0113] Stability verification

[0114] To verify the stability of VIO-AMCL under actual working conditions, the trajectory mileage of the scraper during repeated cyclic operations in the mine roadway environment was recorded. The scraper was still traveling back and forth between four target points. After reaching each target point, information about the current position was recorded once, and each recording lasted for 30 minutes. The results are as Figure 9 shown. Compared with the given initial target points, the positioning results of the AMCL algorithm showed multiple deviation phenomena, which also reflected that the method of using a single sensor as an odometer was unreliable. Under unstructured road conditions such as mines, the original AMCL algorithm could not meet the robustness requirements of scraper positioning. The minimum error was 0.352 m, and the maximum error reached 3.302 m. Even when the scraper was running repeatedly, the AMCL that only fuses VIO and the VIO-AMCL of the present invention could still maintain a relatively low error range. The error range of the AMCL that only fuses VIO was between 0.123 m and 1.233 m, and the VIO-AMCL was between 0.097 m and 0.682 m, proving that the algorithm of the present invention can maintain the stability of positioning performance under actual working conditions.

[0115] Table 2 Comparison of average position errors (unit: meter)

[0116]

[0117] Table 3 Comparison of average angle errors (unit: meter)

[0118]

[0119] Based on the comprehensive analysis results and according to the data in Table 2, the AMCL algorithm had the largest error, and in some periods, it even exceeded 0.15 m, with too large an amplitude. This was due to the cumulative error caused by the frequent slipping of the odometer information provided by the pure wheel encoder on the muddy stone sections of the mine. In contrast, the AMCL after fusing VIO and the VIO-AMCL of the present invention did not show large error fluctuations. The error of the algorithm of the present invention was reduced by 36.76% on average compared with AMCL and by 5.7% on average compared with the AMCL that only fuses VIO. In terms of the positioning angle, the angle error of the AMCL algorithm was still the largest, and the angle error of the algorithm of the present invention was the smallest. The comparison of the angle errors in ten specific experiments is shown in Table 3. The angle error of the algorithm of the present invention was reduced by 75.68% on average compared with AMCL and by 3.2% on average compared with the AMCL that only fuses VIO. This was due to the fact that the gyroscope and accelerometer included in the IMU could accurately measure the subtle angle changes, which greatly reduced the rotation error in the pose angle direction.

[0120] It is obvious to those skilled in the art that the present invention is not limited to the details of the above-described exemplary embodiments, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention. Therefore, in any respect, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Accordingly, all changes that fall within the meaning and scope of the equivalent elements of the claims are intended to be embraced within the present invention. Any reference signs in the claims should not be construed as limiting the claims concerned.

Claims

1. A positioning method for a mine loader based on active VIO-AMCL, characterized in that The following steps are involved: Step 1: Build a deep visual observation model; use an RGB-D camera to acquire 3D point cloud data of the mine tunnel environment, generate a 3D point cloud using a coordinate transformation formula, and map it to the global coordinate system of the prior laser map; Step 2: Build a binocular visual-inertial tightly coupled module; extract homogenized ORB feature points from the binocular image, combine it with IMU pre-integration data, and output pose information through a tightly coupled visual-inertial odometry (VIO); Step 3: Input the pose data output from step 2 into the adaptive Monte Carlo localization (AMCL) framework (VIO-AMCL algorithm), combine it with the articulated motion model of the scraper to perform particle swarm pose prediction and weight update, and output the real-time positioning result of the scraper.

2. The method for positioning a mine load-haul-dump vehicle based on active VIO-AMCL according to claim 1, wherein: In step 1, the specific steps of constructing the depth vision observation model include the following: (1) Based on the 3D point cloud data acquired by the RGB-D camera, the conversion from 2D image to 3D image is deduced, and a point cloud conversion model is constructed based on the RGB image and depth image to provide the observation data required for the positioning of the scraper; (2) Construct a binocular VIO method based on ORB and combined with IMU data to replace the original wheel odometry, and use its output as the pose data for Monte Carlo positioning; (3) Combined with the AMCL framework, particle probability calculation is performed to derive the high-precision positioning of the scraper, and finally the entire VIO-AMCL positioning process is completed.

3. The method for positioning a mine load-haul-dump vehicle based on active VIO-AMCL according to claim 2, wherein: In step 1, the coordinate conversion formula of the three-dimensional space coordinate point X, Y, and Z is: Z=depth (3) Among them, f x represents the focal length of the x-axis of the imaging coordinate system, and f y represents the focal length of the y-axis of the imaging coordinate system. c x represents the x-coordinate of the base point, which is the center of the image plane along the x-axis. c y represents the y-coordinate of the base point, which is the center of the image plane along the y-axis. u and v are the coordinates in the 2D image, and depth is the depth of the pixel point.

4. The method for positioning a mine load-haul-dump vehicle based on active VIO-AMCL according to claim 3, wherein: In step 1, the transformation relationship of mapping the scanned point cloud of the 3D point cloud to the global coordinate system of the prior laser map is as follows: where x, y represent the pose of the device body at this moment, x depth , y depth represent the central coordinates of the depth camera relative to the device body, θ depth , z t represent the orientation angle and the end point of the depth point cloud relative to the device body respectively.

5. The positioning method of a mine load-haul-dump vehicle based on active VIO-AMCL according to claim 4, characterized in that: In step 2, the specific steps of constructing the binocular visual-inertial tight coupling module include the following: (1) A quadtree is introduced into the extracted ORB feature points to achieve uniform distribution of feature points and solve the feature redundancy problem caused by the lack of texture in mine tunnels; (2) Optimize the inertial data between key frames through IMU pre-integration to reduce the amount of calculation and suppress the cumulative error; (3) Construct a tightly coupled VIO based on dual cameras and IMU to make the sensors fully complementary, thereby reducing the cumulative error and improving the accuracy of pose estimation.

6. The method for positioning a mine load-haul-dump vehicle based on active VIO-AMCL according to claim 5, characterized in that: In step 2, the IMU pre-integration process includes: integrating the IMU data between the camera key frames, constructing the error function by providing the measurement values between the two frames, and finally iteratively optimizing the frame pose, using the IMU relative value over a period of time to replace the IMU absolute value at a certain moment, that is, the IMU pre-integration factor.

7. The positioning method of the mine load-haul-dump vehicle based on active VIO-AMCL according to claim 6, characterized in that: In step 2, the visual-inertial initialization process in the visual-inertial odometry (VIO) includes pure visual maximum a posteriori estimation, pure IMU maximum a posteriori probability estimation, and visual-inertial maximum a posteriori probability estimation; The pure vision maximum a posteriori estimation runs at a key frame insertion frequency of 4 Hz, constructs a visual map with uncertain scale, which contains hundreds of map points of 10 key frames, and uses the pure vision optimization method to optimize the poses of each frame, and obtains the corresponding coordinate poses from the poses of each frame. Where R represents the corresponding rotation matrix and p represents the corresponding pose information; The pure IMU maximum a posteriori probability estimate estimates the IMU parameters and scale by using the value of IMU pre-integration between key frames: where s represents the scale map, and R wg ∈ SO(3) is the rotation matrix of gravity, which represents the rotation relationship from the first reference coordinate system in the map to the real-world coordinate system, and b is the IMU acceleration and gyroscope bias, is the scale-based velocity; The visual-inertial maximum a posteriori (MAP) estimation optimizes the visual error and the inertial error simultaneously after performing pure-vision MAP estimation and pure-IMU MAP estimation, while ensuring that there is the same bias b between each pair of key frames. a and b g , and has prior information as in the previous step.

8. The method for positioning a mine loader based on active VIO-AMCL according to claim 7, characterized in that: In step three, the method for constructing the articulated motion model of the scraper includes the following steps: (1) Define the relationship between the center coordinates and steering angle of the front and rear bodies x a = x b - L b cosθ b - L a cosθ a ,(6) y a = y b -L b sinθ b -L a sinθ a ; (7) Among them, P A is the hinge point, P a (x a , y a ) is the center of the front vehicle body, P b (x b , y b ) is the center of the rear vehicle body (rear wheel braking). The direction angles of the front vehicle body and the rear vehicle body are θ a and θ b , respectively. The distance between P A and P a is L a , and the distance between P A and P b is L b . And the angle γ is the difference between θ a and θ b : γ = θ b - θ a ; (2) Based on the articulated steering angular velocity and the vehicle body velocity, establish a kinematic equation to predict the position and pose of the scraper: Center position P of the front and rear bodies of the vehicle i Expressed as where, v i is the associated speed, v a is the speed of the front vehicle body, is the angular speed of the γ angle during articulated steering, is the coordinate of point P, are the motion parameters at the motion moment.

9. The positioning method of a mine loader based on active VIO-AMCL according to claim 8, wherein: In step three, the key steps of the VIO-AMCL algorithm are as follows: (1) Load the prior map and initialize the algorithm; (2) Particle swarm initialization: Initialize the particle set in the prior map The number of the particle swarm is M, and the same importance factor M is assigned to each particle -1 ; (3) Particle swarm pose prediction: Predict the pose of the next state of the particle set according to the VIO odometer information, and obtain the predicted particle set using the motion model based on Equation (8). (4) Update of particle swarm importance factor: Update the importance factor w of each particle according to the visual point cloud data, map data information and particle set X in claim 3 t+1 to obtain the observed particle set t+1 ​ (5) Particle swarm normalization and resampling: Normalize the weights of the importance factors and according to the number of effective particles Compare with the preset threshold M T to determine whether to perform KLD sampling. If M eff > M T it indicates that the particle swarm is dispersed, and the upper limit of the particle swarm quantity needs to be increased; otherwise, the particle swarm quantity is reduced to achieve the purpose of improving the particle utilization rate and reducing particle redundancy. (6) After the particle swarm calculated according to the importance factors in (4) is clustered according to the pose, the particle cluster with the largest weight is output as the positioning result, and then repeat (3) to (6).

Images