Vehicle body positioning method and system of rock drilling robot based on visual positioning

Abstract

The invention discloses a vehicle body positioning method and system of a rock drilling robot based on visual positioning. According to the method, a multi-view visual sensor is used for collecting an iron mine roadway three-dimensional environment image, rock wall feature point cloud data are extracted and matched with a reference map, inertial navigation data are combined, and position parameters are fused and optimized through a Kalman filtering algorithm; establishing an environment interference model for dust and illumination interference, and correcting position parameters by switching an enhanced positioning mode; and generating a target pose matrix according to the correction parameters, and controlling the vehicle body to move to form closed-loop control. The system comprises a visual perception module, a reference map construction module, a position calculation module, an error compensation module and a motion control module. Multi-dimensional information of the roadway rock wall is obtained through multiple cameras, visual positioning accidental errors and inertial navigation accumulative errors are effectively eliminated, the system can adapt to the complex environment, the positioning reliability is improved, and the system is suitable for precise positioning operation of the rock drilling robot in the iron ore roadway.

Description

Technical Field

[0001] The present invention relates to the technical field of body positioning of a rock drilling robot, and in particular to a body positioning method and system of a rock drilling robot based on vision positioning. Background Art

[0002] In iron ore mining scenarios, precise positioning of rock drilling robots is one of the core technologies for achieving automated operations. Vehicle positioning methods primarily rely on inertial navigation systems, but these systems suffer from error accumulation, significantly reducing positioning accuracy over long periods of operation. While purely vision-based positioning technologies can obtain position information by matching environmental features, interference factors such as high dust concentrations, unstable lighting conditions, and complex rock wall textures in iron ore tunnels can degrade the image quality captured by visual sensors, reducing the accuracy of feature point matching and, in turn, impacting positioning reliability. Summary of the Invention

[0003] The purpose of the present invention is to provide a body positioning method and system for a rock drilling robot based on vision positioning, so as to solve the problems raised in the above background technology.

[0004] To achieve the above object, the present invention provides the following technical solution: a method for positioning a rock drilling robot body based on visual positioning, comprising the following steps:

[0005] S1. Using a multi-camera vision sensor mounted on top of the rock drilling robot, real-time three-dimensional environmental images of the iron ore tunnel are collected. The multi-camera vision sensor includes at least two high-definition cameras for acquiring environmental data from different perspectives.

[0006] S2. Preprocess the collected 3D environmental image and use a feature detection algorithm to extract feature point cloud data of the tunnel rock wall, including contour edges, texture features, and landmark structures; match the real-time feature point cloud data with a pre-built 3D reference map of the iron ore tunnel to determine the spatial coordinate correspondence of the feature point cloud;

[0007] S3. Based on the matched feature point cloud coordinates, the real-time position parameters of the vehicle relative to the reference map are calculated using the principle of triangulation, including the vehicle's three-dimensional coordinates (x, y, z), heading angle, pitch angle, and roll angle. Combined with the real-time motion data of the inertial navigation system, the position parameters are optimized through the Kalman filter algorithm to eliminate accidental errors in visual positioning.

[0008] S4. Establish an environmental interference model for iron ore tunnels. To address the impact of factors such as dust and lighting changes on visual positioning, a preset compensation algorithm is used to correct the calculated position parameters. This is done by marking typical interference areas on the baseline map and automatically switching to enhanced positioning mode when entering that area. This enhanced positioning mode increases the number of feature matching iterations and incorporates point cloud data from tunnel lidar for assisted positioning.

[0009] S5. Generate the target posture matrix of the vehicle body based on the corrected position parameters; calculate the rotation speed and steering angle of each drive wheel through the inverse kinematic solution, and control the vehicle body drive system to adjust the travel direction and speed so that the vehicle body gradually approaches the target posture; during the adjustment process, the deviation between the current posture and the target posture is fed back in real time to form a closed-loop control until the positioning error is less than the preset threshold.

[0010] Furthermore, the multi-eye vision sensor includes at least three high-definition cameras, which are installed on the top of the vehicle body in a 120° circular array, two of which are used to collect color RGB images, and one camera is used to collect depth images to obtain the texture features, contour edges and three-dimensional depth information of the tunnel rock wall.

[0011] Furthermore, the feature detection algorithm extracts feature points from the preprocessed image, specifically including: constructing a multi-scale image space through a Gaussian pyramid; detecting extreme points and calculating the main direction using the Hessian matrix; and generating a 128-dimensional feature descriptor for feature point matching.

[0012] Furthermore, the steps for constructing the three-dimensional reference map include: offline scanning of the iron ore tunnels using a drone equipped with a laser radar to obtain point cloud data of the tunnel rock walls; aligning multiple frames of point cloud data to generate a three-dimensional geometric model of the tunnel; and manually marking feature points on the rock walls, including the coordinate positions of landmark structures such as anchor rods and water pipe interfaces.

[0013] Furthermore, in the process of integrating the inertial navigation system with visual positioning, the state vector of the Kalman filter algorithm includes the position, velocity, attitude angle of the vehicle body and the zero bias error of the inertial device, the measurement vector is the position parameter solved by the visual positioning, and the nonlinear motion model is processed by the extended Kalman filter.

[0014] Furthermore, in the enhanced positioning mode, the fusion method of the lidar point cloud data includes: converting the coordinate system of the lidar point cloud data and the visual feature point cloud to unify them into the vehicle coordinate system; using a point cloud registration algorithm to match the real-time point cloud and the reference map, and outputting the weighted average position parameters.

[0015] A rock drilling robot body positioning system based on visual positioning includes a visual perception module, a reference map construction module, a position solution module, an error compensation module, and a motion control module:

[0016] Visual perception module: includes a multi-camera visual sensor and an environmental preprocessing unit. The multi-camera visual sensor is used to collect three-dimensional environmental images of the iron ore tunnel. The environmental preprocessing unit performs noise reduction and enhancement processing on the image and extracts rock wall feature point cloud data.

[0017] Baseline map construction module: used to construct a 3D baseline map of the iron ore tunnels, storing the tunnel geometry, feature point coordinates, and typical interference area information; supports offline modeling and online updates, and improves map accuracy by integrating historical visual data and inertial navigation data;

[0018] Position calculation module: This module includes a feature matching unit and a data fusion unit. The feature matching unit matches the real-time feature point cloud with the reference map to determine the spatial coordinate correspondence. The data fusion unit uses triangulation and Kalman filtering algorithms to fuse visual positioning data with inertial navigation data to calculate and optimize the real-time position parameters of the vehicle body.

[0019] Error compensation module: stores the iron ore tunnel environmental interference model and compensation algorithm. It automatically identifies interference areas and switches to enhanced positioning mode to correct position parameters based on interference factors such as dust and light.

[0020] Motion control module: includes a posture planning unit and a drive control unit. The posture planning unit generates the target posture matrix and drive wheel control parameters based on the corrected position parameters; the drive control unit adjusts the driving state by controlling the vehicle drive system and provides real-time feedback on posture deviations to form a closed-loop control.

[0021] Furthermore, a detachable dust cover is provided on the outside of the high-definition camera of the visual perception module, and an ultrasonic dust removal device is integrated inside. When it is detected that the dust concentration exceeds a threshold, the dust removal program is automatically started.

[0022] Compared with the prior art, the present invention has the following beneficial effects:

[0023] A circular array of at least three high-definition cameras can capture the texture features, contour edges, and three-dimensional depth information of the tunnel rock walls. A Kalman filter algorithm is used to fuse visual positioning data with inertial navigation data. The state vector includes position, velocity, attitude angle, and zero-bias error of the inertial device. By extending the Kalman filter to process nonlinear motion models, the system effectively eliminates the accidental errors of visual positioning and the cumulative errors of inertial navigation, improving the stability and continuity of positioning results. An environmental interference model for iron ore tunnels is established. To address factors such as dust and lighting changes, typical interference areas are marked on the baseline map and automatically switched to enhanced positioning mode, achieving adaptive adaptation to complex environments. A coordinate system and registration algorithm for the LiDAR point cloud data and the visual feature point cloud further improve the reliability of positioning in interference areas. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

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

[0026] See also Figure 1 A method for positioning a rock drilling robot body based on visual positioning comprises the following steps:

[0027] S1. A multi-camera vision sensor mounted on top of the rock drilling robot collects three-dimensional environmental images of the iron ore tunnel in real time. The multi-camera vision sensor includes at least two high-definition cameras for acquiring environmental data from different perspectives.

[0028] The multi-viewing sensor consists of at least three high-definition cameras installed on the top of the vehicle in a 120° circular array. Two of the cameras are used to collect color RGB images, and one camera is used to collect depth images to obtain the texture features, contour edges and three-dimensional depth information of the tunnel rock wall.

[0029] The outer side is equipped with a detachable nano-coated dust cover and a built-in ultrasonic dust removal device; dust concentration> 100mg / m 3 It starts automatically at 10 seconds, and the single dust removal time is 10 seconds. It can achieve double cleaning with the air pump blowing.

[0030] S2. Preprocess the collected 3D environmental image and use a feature detection algorithm to extract feature point cloud data of the tunnel rock wall, including contour edges, texture features, and landmark structures; match the real-time feature point cloud data with a pre-built 3D reference map of the iron ore tunnel to determine the spatial coordinate correspondence of the feature point cloud;

[0031] The feature detection algorithm extracts feature points from the preprocessed image, specifically including: constructing a multi-scale image space through a Gaussian pyramid; using the Hessian matrix to detect extreme points and calculate the main direction; and generating a 128-dimensional feature descriptor for feature point matching.

[0032] The steps for constructing a three-dimensional benchmark map include: offline scanning of iron ore tunnels using a drone equipped with a lidar to obtain point cloud data of the tunnel rock walls; aligning multiple frames of point cloud data to generate a three-dimensional geometric model of the tunnel; and manually marking feature points on the rock walls, including the coordinate positions of landmark structures such as anchor rods and water pipe interfaces.

[0033] S3. Based on the matched feature point cloud coordinates, the real-time position parameters of the vehicle relative to the reference map are calculated using the principle of triangulation, including the vehicle's three-dimensional coordinates (x, y, z), heading angle, pitch angle, and roll angle. Combined with the real-time motion data of the inertial navigation system, the position parameters are optimized through the Kalman filter algorithm to eliminate accidental errors in visual positioning.

[0034] In the process of integrating the inertial navigation system with visual positioning, the state vector of the Kalman filter algorithm includes the position, velocity, attitude angle of the vehicle body and the zero bias error of the inertial device. The measurement vector is the position parameter solved by the visual positioning, and the nonlinear motion model is processed by the extended Kalman filter.

[0035] S4. Establish an environmental interference model for iron ore tunnels. To address the impact of factors such as dust and lighting changes on visual positioning, a preset compensation algorithm is used to correct the calculated position parameters. This is done by marking typical interference areas on the baseline map and automatically switching to enhanced positioning mode when entering that area. This enhanced positioning mode increases the number of feature matching iterations and incorporates point cloud data from tunnel lidar for assisted positioning.

[0036] In the enhanced positioning mode, the fusion method of the lidar point cloud data includes: converting the coordinate system of the lidar point cloud data and the visual feature point cloud to unify them into the vehicle coordinate system; using the point cloud registration algorithm to match the real-time point cloud with the reference map, and outputting the weighted average position parameters.

[0037] S5. Generate the target posture matrix of the vehicle body based on the corrected position parameters; calculate the rotation speed and steering angle of each drive wheel through the inverse kinematic solution, and control the vehicle body drive system to adjust the travel direction and speed so that the vehicle body gradually approaches the target posture; during the adjustment process, the deviation between the current posture and the target posture is fed back in real time to form a closed-loop control until the positioning error is less than the preset threshold.

[0038] A rock drilling robot body positioning system based on visual positioning includes a visual perception module, a reference map construction module, a position solution module, an error compensation module, and a motion control module:

[0039] Visual perception module: This module includes a multi-camera vision sensor and an environmental preprocessing unit. The multi-camera vision sensor is used to collect 3D environmental images of the iron ore tunnel. The environmental preprocessing unit performs noise reduction and enhancement on the images and extracts feature point cloud data of the rock wall.

[0040] The high-definition camera of the visual perception module is equipped with a detachable dust cover on the outside and an ultrasonic dust removal device inside. When the dust concentration is detected to exceed the threshold, the dust removal program is automatically started.

[0041] The dust concentration detection frequency is 10Hz. When the detection value is greater than the threshold for three consecutive times, the air pump is started first to purge, and then the ultrasonic vibration is turned on. After dust removal, the camera internal reference self-calibration is automatically performed.

[0042] Baseline map construction module: used to construct a 3D baseline map of the iron ore tunnels, storing the tunnel geometry, feature point coordinates, and typical interference area information; supports offline modeling and online updates, and improves map accuracy by integrating historical visual data and inertial navigation data;

[0043] When the cumulative positioning frames are greater than 100 frames and the map matching error is greater than 5cm, a local map update is triggered. Factor graph optimization is used to fuse the new visual data and inertial navigation data. The update area is within ±5m of the current position. After the update, the map accuracy is improved to ±2cm.

[0044] Position calculation module: This module includes a feature matching unit and a data fusion unit. The feature matching unit matches the real-time feature point cloud with the reference map to determine the spatial coordinate correspondence. The data fusion unit uses triangulation and Kalman filtering algorithms to fuse visual positioning data with inertial navigation data to calculate and optimize the real-time position parameters of the vehicle.

[0045] Error compensation module: stores the iron ore tunnel environmental interference model and compensation algorithm. It automatically identifies interference areas and switches to enhanced positioning mode to correct position parameters based on interference factors such as dust and light.

[0046] In addition to the lidar, a UWB positioning interface is reserved (supporting TOF ranging, communication distance of 50m). UWB base stations can be deployed in tunnel-type lanes to form a positioning network, and multi-source fusion can be achieved through federal Kalman filtering to improve reliability in complex environments.

[0047] Motion control module: It includes a posture planning unit and a drive control unit. The posture planning unit generates the target posture matrix and drive wheel control parameters based on the corrected position parameters. The drive control unit adjusts the driving state by controlling the vehicle drive system and provides real-time feedback on posture deviations to form a closed-loop control.

[0048] The motion control module's drive system is a four-wheel independent drive structure. The inverse kinematics algorithm calculates the speed and steering angle of each drive wheel based on the vehicle's target posture matrix using the following formula:

[0049] v i =v+ω·r·cosθ i

[0050]

[0051] Among them, v is the linear velocity of the vehicle body, ω is the angular velocity, r is the wheelbase of the driving wheel, θ i is the steering angle of each driving wheel.

Claims

1. A method for positioning a rock drilling robot body based on visual positioning, characterized in that: The following steps are involved: S1. Using a multi-camera vision sensor mounted on top of the rock drilling robot, real-time three-dimensional environmental images of the iron ore tunnel are collected. The multi-camera vision sensor includes at least two high-definition cameras for acquiring environmental data from different perspectives. S2. Preprocess the collected 3D environmental image and use a feature detection algorithm to extract feature point cloud data of the tunnel rock wall, including contour edges, texture features, and landmark structures; match the real-time feature point cloud data with a pre-built 3D reference map of the iron ore tunnel to determine the spatial coordinate correspondence of the feature point cloud; S3. Based on the matched feature point cloud coordinates, the real-time position parameters of the vehicle relative to the reference map are calculated using the principle of triangulation, including the vehicle's three-dimensional coordinates (x, y, z), heading angle, pitch angle, and roll angle. Combined with the real-time motion data of the inertial navigation system (INS), the position parameters are optimized through the Kalman filter algorithm to eliminate accidental errors in visual positioning. S4. Establish an environmental interference model for iron ore tunnels. To address the impact of factors such as dust and lighting changes on visual positioning, a preset compensation algorithm is used to correct the calculated position parameters. This is done by marking typical interference areas on the baseline map and automatically switching to enhanced positioning mode when entering that area. This enhanced positioning mode increases the number of feature matching iterations and incorporates point cloud data from tunnel lidar for assisted positioning. S5. Generate the target posture matrix of the vehicle body based on the corrected position parameters; calculate the rotation speed and steering angle of each drive wheel through the inverse kinematic solution, and control the vehicle body drive system to adjust the travel direction and speed so that the vehicle body gradually approaches the target posture; during the adjustment process, the deviation between the current posture and the target posture is fed back in real time to form a closed-loop control until the positioning error is less than the preset threshold.

2. A rock drilling robot body positioning system based on vision positioning, applied to the positioning method according to claim 1, characterized in that: It includes visual perception module, reference map construction module, position solution module, error compensation module and motion control module: Visual perception module: includes a multi-camera visual sensor and an environmental preprocessing unit. The multi-camera visual sensor is used to collect three-dimensional environmental images of the iron ore tunnel. The environmental preprocessing unit performs noise reduction and enhancement processing on the image and extracts rock wall feature point cloud data. Baseline map construction module: used to construct a 3D baseline map of the iron ore tunnels, storing the tunnel geometry, feature point coordinates, and typical interference area information; supports offline modeling and online updates, and improves map accuracy by integrating historical visual data and inertial navigation data; Position calculation module: This module includes a feature matching unit and a data fusion unit. The feature matching unit matches the real-time feature point cloud with the reference map to determine the spatial coordinate correspondence. The data fusion unit uses triangulation and Kalman filtering algorithms to fuse visual positioning data with inertial navigation data to calculate and optimize the real-time position parameters of the vehicle body. Error compensation module: stores the iron ore tunnel environmental interference model and compensation algorithm. It automatically identifies interference areas and switches to enhanced positioning mode to correct position parameters based on interference factors such as dust and light. Motion control module: includes a posture planning unit and a drive control unit. The posture planning unit generates the target posture matrix and drive wheel control parameters based on the corrected position parameters; the drive control unit adjusts the driving state by controlling the vehicle drive system and provides real-time feedback on posture deviations to form a closed-loop control.

3. The method for positioning a rock drilling robot body based on vision positioning according to claim 1, characterized in that: The multi-eye vision sensor includes at least three high-definition cameras installed on the top of the vehicle body in a 120° circular array, two of which are used to collect color RGB images, and one camera is used to collect depth images to obtain the texture features, contour edges and three-dimensional depth information of the tunnel rock wall.

4. The method for positioning a rock drilling robot body based on vision positioning according to claim 1, characterized in that: The feature detection algorithm extracts feature points from the preprocessed image, specifically including: constructing a multi-scale image space through a Gaussian pyramid; detecting extreme points and calculating the main direction using the Hessian matrix; and generating a 128-dimensional feature descriptor for feature point matching.

5. The method for positioning a rock drilling robot body based on vision positioning according to claim 1, characterized in that: The steps for constructing the three-dimensional reference map include: offline scanning of the iron ore tunnels using a drone equipped with a laser radar to obtain point cloud data of the tunnel rock walls; aligning multiple frames of point cloud data to generate a three-dimensional geometric model of the tunnel; and manually marking feature points on the rock walls, including the coordinate positions of landmark structures such as anchor rods and wind and water pipe interfaces.

6. The method for positioning a rock drilling robot body based on vision positioning according to claim 1, characterized in that: In the fusion process of the inertial navigation system and visual positioning, the state vector of the Kalman filter algorithm includes the position, velocity, attitude angle of the vehicle body and the zero bias error of the inertial device. The measurement vector is the position parameter solved by the visual positioning, and the nonlinear motion model is processed by the extended Kalman filter.

7. The method for positioning a rock drilling robot body based on vision positioning according to claim 1, characterized in that: In the enhanced positioning mode, the method for fusing lidar point cloud data includes: converting the coordinate system of the lidar point cloud data and the visual feature point cloud to unify them into the vehicle coordinate system; using a point cloud registration algorithm to match the real-time point cloud with the reference map, and outputting the weighted average position parameters.

8. The rock drilling robot body positioning system based on vision positioning according to claim 2, characterized in that: A detachable dust cover is provided on the outside of the high-definition camera of the visual perception module, and an ultrasonic dust removal device is integrated inside. When it is detected that the dust concentration exceeds a threshold, the dust removal program is automatically started.

9. The rock drilling robot body positioning system based on vision positioning according to claim 2, characterized in that: The drive system of the motion control module is a four-wheel independent drive structure. The inverse kinematics algorithm calculates the speed and steering angle of each drive wheel according to the target posture matrix of the vehicle body using the following formula: v i =v+ω·r·cosθ i Among them, v is the linear velocity of the vehicle body, ω is the angular velocity, r is the wheelbase of the driving wheel, θ i is the steering angle of each driving wheel.

Images