A downhole unmanned vehicle positioning method based on multi-sensor active fusion

Abstract

The application discloses a kind of underground unmanned vehicle positioning methods based on multi-sensor active fusion, comprising the following steps: according to the initial parameters of each sensor of unmanned vehicle and position relationship, each sensor is parameter calibrated, the external parameter of each sensor is obtained, and it is unified in same vehicle coordinate system;Wherein, the sensor includes wheel speed odometer, inertial navigation, camera and laser radar;Each sensor and illuminometer are opened, and unmanned vehicle travels a week in accordance with planned path underground, obtains prior information, including inertial navigation prior data, prior map generated by laser radar point cloud and underground illumination condition;Determine positioning main sensor;The positioning information of main sensor and inertial navigation is weighted fusion, and the final positioning of unmanned vehicle is obtained.The method can enhance the environmental perception ability of mine car in underground scene, achieve operation requirements according to accurate positioning, effectively improve the production efficiency of mining area.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving environment perception, in particular to a mine unmanned vehicle positioning method based on multi-sensor active fusion. BACKGROUND

[0002] In recent years, 5G communication and Internet of Vehicles technology have developed rapidly, and unmanned driving has gradually entered the public eye. However, due to reasons such as sensor stability, decision reliability, and control precision, it is difficult for unmanned driving to be popularized in urban areas in a short period of time. However, the mine area has the natural advantages of few people and single obstacles, which are conducive to the application of unmanned driving. In addition, the application of unmanned driving in the mine area to some extent ensures the smooth progress of work under complex conditions and improves work efficiency.

[0003] Mining in the mine area has certain risks. Mining requires a large number of equipment, vehicles, and personnel to be arranged underground. Disasters and accidents caused by various natural or human factors pose a great threat to the personal safety of underground workers. Effective organization and management of underground personnel and vehicles, as well as rescue in the event of accidents and disasters, require accurate position data of underground personnel, vehicles, and equipment. Remote control and intelligent control of underground equipment cannot be achieved without real-time and accurate position data of underground equipment. Precise positioning technology has become an important technical support for mine safety production, and real-time and accurate position data has become an important basis for the daily operation of intelligent mines. At present, the mine area usually adopts inertial navigation positioning or communication positioning: inertial navigation positioning only needs the coordinates of the starting point and calculates the current position according to the gyroscope and accelerometer, but there is a large cumulative error; and communication positioning usually requires the installation of multiple base stations in the mine area, with each base station serving a certain area, but there are often positioning blind areas. Therefore, how to achieve accurate positioning in the mine area is a problem to be solved.

[0004] At present, positioning in the mine area has become a research hotspot, but the application requirements in this scenario are difficult to meet:

[0005] In the paper "Design and Research on Intelligent Positioning System for Coal Mine Underground Personnel", a mine positioning method based on UWB ultra-wideband positioning technology and LoRa wireless communication technology is proposed. Underground workers and vehicles carry corresponding positioning tags, and the tag position information is updated within a certain time without accidents. The specific position is obtained by analyzing the data information on the surface. However, this method requires the specific speed of the signal in the propagation medium when positioning in the mine area, and there is a large error in the complex working condition scenario in the mine area, which cannot accurately obtain the underground positioning information.

[0006] In the paper "Design and Research of New Mine Personnel Positioning System Based on WiFi Wireless Network Technology", a mine positioning method based on advanced WiFi wireless network technology is proposed, underground workers and vehicles carry positioning terminals, and according to the positioning tag upload data information within a certain range, the specific positioning information of the underground is analyzed according to the information. But this method is limited by the WiFi signal strength, the activity scene of the staff, the compatibility of hardware and software, and the positioning blind area in the underground mine area, and lacks complete underground positioning information.

[0007] Based on this, the present application provides a kind of underground unmanned vehicle positioning method based on multi-sensor active fusion, and a variety of sensors such as IMU, wheel speed odometer, camera and laser radar are actively fused in different working conditions, accurate positioning information is output according to actual scene, and accurate positioning of vehicle body in mine area is realized. SUMMARY

[0008] The present application aims to overcome the shortcomings and deficiencies of the prior art, and provides a kind of underground unmanned vehicle positioning method based on multi-sensor active fusion, which aims to enhance the environmental perception ability of mine car in underground scene, and achieve the operation requirement according to accurate positioning, not only guarantee the safety of workers, but also improve the production efficiency of mine area to a certain extent. The present application adopts the following technical scheme:

[0009] A kind of underground unmanned vehicle positioning method based on multi-sensor active fusion, comprising the following steps:

[0010] Step 1, according to the initial parameters of each sensor of unmanned vehicle and the position relationship, the parameters of each sensor are calibrated, the external parameters of each sensor are obtained, and they are unified in the same vehicle coordinate system;Wherein, the sensor includes wheel speed odometer, inertial navigation, camera and laser radar;

[0011] Step 2, start each sensor and illuminometer, and the unmanned vehicle drives in the underground according to the planned path for a week, to obtain prior information, including inertial navigation prior data, prior map generated by laser radar point cloud and underground illumination condition;

[0012] Step 3, in the process of unmanned vehicle executing task, the positioning main sensor is determined according to the prior information: according to the inertial navigation prior data, if the scene slope is less than a given value, wheel speed odometer is used as the main sensor to position the vehicle body;Otherwise, the underground illumination is judged, if the illumination is higher than a given value, camera is used as the main sensor, and the vehicle body is positioned according to the visual odometer information;Otherwise, laser radar is used as the main sensor, and the vehicle body is positioned according to the matching information of current scene point cloud and prior map of laser radar;Wherein, after the main sensor is determined, the positioning is carried out according to the main sensor information, and other sensors remain in the off state.

[0013] Step 4, the positioning information of the main sensor and the inertial navigation is weighted and fused to obtain the final positioning of the unmanned vehicle.

[0014] Further, in the step 1, the camera and the laser radar are calibrated by using a point correspondence method to obtain an accurate rotation and translation matrix and record the update of the matrix; and the inertial navigation is calibrated by measuring the zero bias error in the static state.

[0015] Further, in the step 3, when the wheel speed odometer is used as the main sensor, the displacement of the vehicle body is calculated according to the wheel speed and the motion model to obtain the driving distance for positioning.

[0016] When the camera is used as the main sensor, the dark channel prior image enhancement method is used to reduce the influence of the underground dust on the visual odometer, and then the Vins-mono method is used for positioning.

[0017] When the laser radar is used as the main sensor, the iterative closest point method is used to match the current scene point cloud information with the prior map information in real time to obtain the positioning result.

[0018] Further, the positioning by using the camera as the main sensor comprises the following steps:

[0019] (1) The dark channel prior image enhancement method is used to reduce the influence of the underground dust on the visual odometer, wherein the dark channel of an image is defined as a twice minimum value operation: first, the minimum value in the RGB three channels of an original image is selected, and then the minimum value filtering is performed in a window:

[0020]

[0021] wherein, J dark (x) is the value of the dark channel of the pixel point x, J c (y) is each channel of the color image, Ω(x) is a square region centered at x, and c is the color channel. dark In the three channels, at least one channel value is very small, and the finally calculated dark channel is usually 0, and in the fog or dust scene, it is greater than 0.

[0022] Then, the underground feature points are extracted and tracked in combination with the underground features of the roadside guide.

[0023] (2) The scale information is recovered by using the IMU pre-integration method, and the short-time attitude estimation is given.

[0024] (3) After the front-end processing, the final state estimation result, i.e., the positioning information, is output by the sliding window graph optimization back-end processing.

[0025] Further, the positioning using laser radar as the main sensor comprises the following steps:

[0026] (1) Obtain the current frame point cloud and perform point cloud preprocessing operation, and import the prior map, and perform iterative nearest point matching between the current frame point cloud and the prior map point cloud:

[0027]

[0028] Wherein p i is any point in the current frame point cloud, q i is the point in the map point cloud closest to p i , R is a rotation matrix, and T is a translation matrix;

[0029] (2) Adjust the weight of the corresponding point, and remove the mis-matching point pair, and continuously iterate this process until the minimum loss is solved, that is, the optimal transformation is solved;

[0030] (3) After obtaining the optimal matching, the positioning result at this time is output.

[0031] Compared with the prior art, the present application has the following advantages:

[0032] (1) The present application proposes a dynamic fusion positioning method based on the difference of mine environment. Since the mine environment has differences, the prior environmental information under the demand scene is combined to dynamically determine the main sensor to adapt to different sensing environments, and the vehicle body fusion positioning method is automatically switched. Compared with the traditional underground positioning method, the adaptability is stronger, the real-time performance is better, and the coverage is wider.

[0033] (2) The present application proposes an optimized visual odometry positioning method in the underground scene. Since there are no obvious extractable features in the underground scene and there is dust phenomenon, the present application uses the dark channel priority image enhancement algorithm to remove the dust noise in the scene to obtain a clearer image, and then combines the roadside guide to enrich the underground features and improve the positioning accuracy of the algorithm.

[0034] (3) The present application proposes a high real-time mine positioning method of visual and laser odometry cooperative positioning. The system is applied to the mine unmanned vehicle, has the characteristics of low memory consumption, high real-time performance and high precision, breaks through the high latency problem of the traditional positioning scheme of the mine, and cooperatively solves the positioning problem of the unmanned vehicle without GPS in the underground scene by using the real-time positioning technology of visual and laser radar odometry, and forms a real-time positioning method library for the underground scene. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 It is a mine unmanned vehicle positioning method framework based on multi-sensor active fusion;

[0036] Figure 2An optimized visual odometry positioning method framework for a downhole scene;

[0037] Figure 3 A laser radar odometry positioning method framework. DETAILED DESCRIPTION

[0038] The application will be further described in detail below in combination with the drawings and embodiments.

[0039] A downhole unmanned vehicle positioning method based on multi-sensor active fusion mainly includes the following four parts: multi-sensor calibration, prior information acquisition, main sensor determination and switching, and fusion positioning.

[0040] Step 1: The user calibrates the parameters between the system sensors according to the initial parameters of each sensor of the unmanned vehicle and the positional relationship; wherein the multi-sensor calibration needs to obtain the external parameters of each sensor, including the wheel speed odometry, inertial navigation, camera and laser radar, and the parameters are calibrated after measurement and calculation, and are unified in the same vehicle coordinate system.

[0041] Specifically, the device used in the application is an unmanned vehicle for work in a mine area, which is a working vehicle in a specific scene. The sensor wheel speed odometry is installed on the rear wheel, the camera is installed behind the windshield in the vehicle, and the IMU inertial navigation device and the laser radar are placed above the bumper.

[0042] After the user starts the unmanned vehicle, each sensor starts to work. The user measures the positional and attitude relationship parameters of each sensor using a scale. After the computer obtains the external parameters of each sensor, the camera and the laser radar are calibrated using the point correspondence method to match, obtain the accurate rotation and translation matrix and record, and update the system matrix; the IMU inertial navigation device needs to calculate the zero bias error in the static state, and thus the multi-sensor calibration work is completed.

[0043] Step 2: Turn on the sensor device and the illuminometer, and drive the unmanned vehicle in the downhole scene for one round to obtain prior information, including IMU inertial navigation prior data, prior map generated by laser radar point cloud and downhole illumination condition.

[0044] Specifically, after calibration, the user selects the experimental scene map, that is, frames the map area on the remote control platform, the computer finds the drivable path according to the selected range map, and selects the optimal path as the planning path according to the shortest path algorithm, executes the planning path using the simulated annealing algorithm, finds the drivable section according to the current position and the starting position and drives to the destination, and realizes the automatic driving control of the unmanned vehicle in the mine.

[0045] After arriving at the starting point, the computer starts up various sensors and the illuminometer element, returns after driving according to the planned path, obtains IMU inertial navigation priori data, a priori map generated by laser radar point cloud, and underground illumination conditions.

[0046] Preferably, priori information collection first needs path planning, the vehicle selects the optimal path according to the drivable path in the region of interest of the user, selects the shortest path according to the Dijkstra algorithm as the optimal path, and plans the vehicle driving state according to the simulated annealing algorithm. The Dijkstra algorithm is a single-source shortest path algorithm, which is more suitable for mine environment. The simulated annealing algorithm is an effective approximate algorithm suitable for large-scale combinatorial optimization problems. It simulates the annealing process of solid matter, controls the continuous decrease of temperature by setting initial temperature, initial state and cooling rate, combines the probability of jumping characteristics, uses the neighborhood structure of the solution space for random search, and finally obtains a relatively smooth planned path. After path planning, the collection equipment is turned on, including the collection of pitch angle information by inertial navigation, the collection of underground illumination intensity by illuminometer, and the collection of point cloud information by laser radar, and the generation of priori map by point cloud matching.

[0047] Step 3, during the execution of the task by the unmanned vehicle, the main positioning sensor is determined according to the priori information: according to the inertial navigation priori data, if the scene slope is small, the wheel speed odometer is used as the main sensor to position the vehicle body; otherwise, the underground illumination is judged, if the illumination is high, the camera is used as the main sensor to position the vehicle body according to the visual odometer information; otherwise, the laser radar is used as the main sensor to position the vehicle body according to the matching information of the current scene point cloud of the laser radar and the priori map. After the main sensor is determined, the vehicle body is positioned according to the main sensor information, and the other sensors remain in the off state to reduce the system complexity.

[0048] Specifically, after receiving the priori information, the computer controls the unmanned vehicle to switch the positioning mode during the execution of the task by the unmanned vehicle according to the priori information state, changes the original IMU positioning to the fusion positioning method of the main sensor and the IMU, and the main sensor determination method and the positioning method thereof are as shown in Figure 1 .

[0049] The main sensor determination is to switch the main positioning sensor according to the experimental environment, to judge according to the priori inertial navigation pitch angle information, if the scene slope is small, the pitch angle change value is less than 60° within a certain time interval, the wheel speed odometer is used as the main sensor to position the vehicle body; if the change value is greater than 60°, it is judged whether the underground environment illumination meets the camera positioning condition, if the illumination is greater than 20 lux, the camera is used as the main sensor; if the illumination is less than 20 lux, the laser radar is used as the main sensor.

[0050] Preferably, if a wheel speed odometer is used as the main sensor for positioning, the positioning method is to calculate the displacement of the vehicle body according to the wheel speed and the motion model, and obtain the driving distance for positioning, and the application uses a robot model to position the vehicle body.

[0051] Preferably, if a camera is used as the main sensor, an optimized visual odometer in the underground scene is used to position the vehicle body. The application uses the Vins-mono method, which is an open-source VIO algorithm of the Hong Kong University of Science and Technology, and is realized by a tight coupling method to recover the scale through monocular + IMU. As shown in Figure 2 , specifically comprising the following steps:

[0052] (1) The application first uses the image enhancement method of dark channel prior (DCP) to reduce the influence of underground dust on the visual odometer. The dark channel of the image is defined as follows: twice minimum value operation: first select the minimum value in the RGB three channels of the original image, and then perform minimum value filtering in the window. The formula is as follows:

[0053]

[0054] J dark (x) is the value of the dark channel of pixel point x, J c is each channel of the color image, and Ω(x) is a square region centered at x. c is the color channel dark In three channels, at least one channel value is very small, and the finally calculated dark channel is usually 0, while in the fog or dust scene, it will be greater than 0.

[0055] Then, combined with the roadside guide, the underground features are enriched, and then the underground feature points are extracted and tracked.

[0056] (2) At the same time, the scale information is recovered by using the IMU pre-integration method, and the short-time attitude estimation is given.

[0057] (3) After the front-end processing is completed, the sliding window graph optimization is performed, and the back-end processing outputs the final state estimation result, i.e. the positioning information output.

[0058] Preferably, if a laser radar is used as the main sensor for positioning, the laser radar uses the iterative closest point method (ICP) to match the current scene point cloud information with the prior map information in real time to obtain the positioning result. As shown in Figure 3 , specifically comprising the following steps:

[0059] (1) First, the current frame point cloud is obtained and the point cloud preprocessing operation is performed, and the prior map is imported, and the current frame point cloud and the prior map point cloud are iteratively matched. The method formula is as follows:

[0060]

[0061] wherein p i is any point in the current frame point cloud, q i is the closest point in the map point cloud to p i , R is a rotation matrix, and T is a translation matrix.

[0062] (2) Adjust the weight of the corresponding points, eliminate the mis-matching point pairs, and iterate this process until the minimum loss is solved, that is, the optimal transformation is solved.

[0063] (3) After obtaining the optimal matching, output the positioning result at this time.

[0064] Step 4, weighting and fusing the positioning information of the main sensor and the inertial navigation to obtain the final positioning of the vehicle body.

[0065] Specifically, the main sensor positioning and the IMU inertial navigation data are fused for positioning, and the specific fusion method is the same as that of the Vins-mono method. The patent adopts a weighted fusion method, and the specific weight coefficient is determined according to the positioning accuracy of the main sensor, wherein the positioning accuracy is determined by the ratio of the difference between the true value of the RTK and the true value of the GPS and the true value of the GPS in the road section with the RTK true value. Finally, the fused positioning information is generated, the computer uploads the positioning information to the cloud, the mine workers collect the positioning information, the system execution ends, and the unmanned working vehicle returns to the workshop.

[0066] The above merely describes specific embodiments of the present application and is not used to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A multi-sensor active fusion based underground unmanned vehicle positioning method, characterized in that, The method comprises the following steps: Step 1, according to the initial parameters of each sensor of the unmanned vehicle and the positional relationship, the parameters of each sensor are calibrated to obtain the external parameters of each sensor, and the external parameters are unified in the same vehicle coordinate system; wherein the sensors include a wheel speed odometer, an inertial navigation, a camera and a laser radar; Step 2, starting each sensor and an illuminometer, the unmanned vehicle drives along the planned path in the underground for one round to obtain prior information, including inertial navigation prior data, a prior map generated by laser radar point cloud and underground illumination; Step 3, during the execution of the task of the unmanned vehicle, the main sensor is determined according to the prior information: according to the inertial navigation prior data, if the scene slope is less than a given value, the wheel speed odometer is used as the main sensor to position the vehicle body; otherwise, the underground illumination is judged, if the illumination is higher than a given value, the camera is used as the main sensor to position the vehicle body according to the visual odometer information; otherwise, the laser radar is used as the main sensor to position the vehicle body according to the matching information of the current scene point cloud and the prior map of the laser radar; wherein after the main sensor is determined, the positioning is carried out according to the information of the main sensor, and the other sensors remain in the shutdown state; Step 4, the positioning information of the main sensor and the inertial navigation is weighted and fused to obtain the final positioning of the unmanned vehicle.

2. The method of claim 1, wherein, In the step 1, the camera and the laser radar are calibrated by using a point correspondence method to obtain an accurate rotation and translation matrix and record the update of the matrix; the inertial navigation is calibrated by measuring the zero offset error in the static state.

3. The method of claim 2, wherein, In the step 3, when the wheel speed odometer is used as the main sensor, the vehicle body displacement is calculated according to the wheel speed and the motion model to obtain the driving distance for positioning; When the camera is used as the main sensor, the dark channel priority image enhancement method is used to reduce the influence of underground dust on the visual odometer, and then the Vins-mono method is used for positioning; When the laser radar is used as the main sensor, the iterative nearest point method is used to match the current scene point cloud information with the prior map information in real time to obtain the positioning result.

4. The method of claim 3, wherein, The positioning using the camera as the main sensor comprises the following steps: (1) the dark channel priority image enhancement method is used to reduce the influence of underground dust on the visual odometer, wherein the dark channel of the image is defined as a twice minimum value operation: first, the minimum value in the RGB three channels of the original image is selected, and then the minimum value filtering is performed in the window; wherein, is the value of the dark channel for the pixel point , is each channel of the color image, is a square region centered at x , is the color channel, under the non-fog or dust scene, at least one channel value is very small in the three channels, and the finally calculated dark channel is usually 0, and in the fog or dust scene, it is greater than 0; Then, the underground feature points are extracted and tracked in combination with the roadside guide underground features; (2) the scale information is recovered by using the IMU pre-integration method, and the short-time attitude estimation is given; (3) after the front-end processing, the final state estimation result, i.e. the positioning information, is output through the sliding window graph optimization back-end processing.

Images