A drilling rig navigation system and method

Abstract

The drilling rig navigation system of this invention can efficiently collect and process preliminary data through sensor modules, data processing modules, fusion modules, path planning modules, and decision control modules. At the algorithm level, the A algorithm has been improved to generate a global path based on global positioning coordinates and preset drilling target points, and to plan local trajectories based on environmental maps and the global path. It can deeply integrate multimodal data, improve adaptability to complex environments, optimize the navigation framework in layers, balance global efficiency and local obstacle avoidance, and has an intelligent fault tolerance mechanism to ensure continuous and stable operation of the system, making it suitable for large-scale industrial use and promotion.

Description

Technical Field

[0001] This invention belongs to the field of industrial information technology, and specifically relates to a drilling rig navigation system and method. Background Technology

[0002] With the increasing demand for intelligent construction in complex scenarios such as mines and tunnels, autonomous navigation technology for drilling rigs is gradually becoming a key to improving operational efficiency and safety. Currently, multimodal sensor fusion technology has been widely applied in the field of engineering machinery navigation, providing equipment with environmental perception and positioning capabilities through the collaboration of multi-source data such as lidar, visual sensors, and inertial navigation systems.

[0003] At the algorithm level, the optimization of path planning and motion control algorithms continues to drive the development of autonomous navigation technology, significantly improving the smoothness and safety of drilling rig movement in complex terrain. However, the deep fusion of multimodal sensor data in existing technologies is not efficient enough, resulting in insufficient efficiency when combining algorithms and sensors for drilling rigs, thus leading to poor path planning performance during drilling. Summary of the Invention

[0004] The purpose of this invention is to provide a drilling rig navigation system and method to solve the problem of poor path planning effect of existing technologies during drilling.

[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution:

[0006] A drilling rig navigation method includes the following steps: Step 0: Preset the drilling path and set the sampling interval; Step 1: The drilling rig is instructed to carry out drilling operations in the work area according to the preset drilling rig path. When the sampling interval is reached, the three-dimensional dot cloud image in front of the drilling rig, the distance information between the drilling rig and the obstacle, the two-dimensional image of the obstacle, and the pose data of the drilling rig are collected. Step 2: Compress the 3D raster cloud image into a 2D planar image, and use the pixel values ​​of the 2D planar image to reflect the environmental structural features; Construct the covariance matrix of the distance between the drilling rig and the obstacle and the covariance matrix of the pose data, and then calculate the weight matrix; calculate the drilling rig pose value based on the weight matrix and the drilling rig pose data; The U-Net model is used to perform semantic segmentation on the 2D obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the 2D obstacle image; Step 3: Extract features from the drilling rig path, environmental structure features, drilling rig pose value, obstacle category, and the coordinates of the two corner points of the obstacle bounding box, and then obtain the path features, environmental features, pose features, and obstacle features accordingly. Assign weights to the features mentioned above and generate an environment map; Based on the environmental map and 3D dot cloud image, the ICP algorithm is used to obtain the global positioning coordinates of the area to be operated. Step 4: Based on the environmental map and the global positioning coordinates of the area to be worked on, use A The algorithm obtains the optimal global path for the drilling rig; Step 5: Based on the optimal global path and the environment map, use the TEB algorithm to plan the local trajectory; Step 6: Based on obstacle distance information and local trajectory, generate control commands using the MPC algorithm; Step 7: Drive the drilling rig according to the control command, so that the drilling rig moves along the local trajectory; Step 8: Repeat steps 1-7 until the drilling operation is completed.

[0007] The present invention also has the following features: Furthermore, step 2 includes the following sub-steps: Step 21: Perform gridding processing on the three-dimensional point cloud image, uniformly divide the three-dimensional point cloud image into multiple pixel units, and calculate the point cloud density in each pixel unit. Next, the 3D point cloud image is compressed into a 2D planar image. Each pixel value in the 2D planar image is used to represent the point cloud density at that location, and then the point cloud density is used to reflect the environmental structural features. Step 22: Construct the covariance matrix of the distance between the drilling rig and the obstacle, and the covariance matrix of the pose data. Calculate the weight matrix based on these two matrices, as shown below:

[0008] in, Represents the weight matrix; The covariance matrix representing the distance between the drilling rig and the obstacle; Represents the covariance matrix of the pose data; Step 23, based on the weight matrix Calculate the fused drilling rig pose value and use it as the final drilling rig pose value, as shown in the following formula: ; in, This represents the fused drilling rig pose value; This represents the XOR operator. This represents the drilling rig's position and orientation data; Indicates the distance between the drilling rig and the obstacle; Step 24: Perform semantic segmentation on the two-dimensional obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the image.

[0009] Furthermore, step 3 includes the following sub-steps: Step 31: Extract features from the drilling rig path preset in Step 0, the environmental structure features obtained in Step 2, the drilling rig pose value, the obstacle category, and the coordinates of the two corner points of the obstacle bounding box, and obtain the corresponding path features, environmental structure features, pose features, and obstacle features. Step 32: Use an attention mechanism to assign corresponding weights to path features, environmental structure features, pose features, and obstacle features to generate an environment map; Step 33: Based on the environmental map and the 3D dot cloud image, the global positioning coordinates of the drilling rig are obtained using the ICP algorithm.

[0010] Furthermore, in step 32, if any modal data of environmental structure features, pose features, or obstacle features fails, the weight of path features is increased. ,make:

[0011] If any two or more modal data fail, the drilling rig will stop drilling.

[0012] Furthermore, in step 32, the environmental structural features, pose features, and obstacle features are verified respectively, and the modal data is determined to be invalid based on the verification results. Specifically, when verifying environmental structural features, a density threshold is first preset, which is the maximum detection density of the sensor. The density of the dot cloud in each pixel unit is compared with the density threshold. If the density threshold is exceeded, the environmental structural feature is considered to be invalid. When verifying the pose features, the specific steps are to verify whether the coordinates and attitude angles of the pose feature values ​​are within the working area of ​​the drilling rig; if any one of them exceeds the working area of ​​the drilling rig, the pose feature is considered invalid. When validating obstacle features, a set of obstacle types consisting of multiple obstacle types is first preset, and it is determined whether an obstacle type falls into the obstacle type set; if not, the obstacle feature is considered invalid.

[0013] Furthermore, step 4 includes the following sub-steps: Step 41: Read the preset drilling target point coordinates from the construction design file of the area to be worked, and mark the key locations as points of interest in the environmental map obtained in step 3; The points of interest include must-pass points, preferred points, and avoidance points; The aforementioned "must-pass points" are specifically points that the drilling rig must pass through and cannot bypass in its path; The preferred points are specifically points in the drilling rig path that can be passed through or bypassed; The avoidance points mentioned are specifically the points that must be avoided in the drilling rig path; Step 42: Assign a corresponding weight to each point of interest, such that the weight of a point that must be visited is greater than that of a point that is preferred, which is greater than that of a point that should be avoided. Specifically, the weight of necessary points is 0.8-1; the weight of preferred points is 0.3-0.7; and the weight of avoidance points is 0-0.2. Step 43: Set the current location of the drilling rig as the path start point and the target point of the work area as the path end point, and establish a list of nodes to be evaluated that includes the path start point, all points of interest and the path end point. Step 44: Take the starting point of the path as a candidate node. Starting from the candidate node, calculate the cost of moving from the candidate node to each adjacent node. Select the adjacent node with the smallest cost as the new candidate node. Step 45: Designate the new candidate node as a candidate node, and repeat step 44 until the end of the path is reached. Step 46: Perform curve fitting on the discrete curve composed of the path start point, all candidate nodes and the path end point to generate a continuous and smooth curve, which serves as the optimal global path for the drilling rig.

[0014] Further, in step 44, the cost function is used to calculate the cost of moving from the candidate node to each adjacent node. Among them, A The cost function of the algorithm is as follows: , in, This indicates that A is used in step 44. The cost function of the algorithm; Indicates the corrected A The cost function of the algorithm; This represents the weight of the influence of the node curvature term on the cost function, [0.05, 0.3]; Indicates the length of the time domain; Indicates time; Represents the curvature of a node; Represents the acceleration of a node; Represented as a constant; This is the integration operator; The weight representing the influence of node acceleration on the cost function is [0.05, 0.3]. Specifically,

[0015] in, Indicates candidate nodes; A represents The algorithm's original cost function; Indicates the corrected A Algorithm cost function This represents any adjacent node in the list of nodes to be evaluated that is adjacent to the candidate node. This represents the list of nodes to be evaluated. This represents the weight value corresponding to that node; Indicates candidate nodes To adjacent nodes Euclidean distance.

[0016] A drilling rig navigation method corresponding to the above-mentioned drilling rig navigation method includes a sensor module, a data processing module, a fusion module, a path planning module, and a decision control module; The sensor module is used for: Collect 3D dot cloud images in front of the drilling rig, the distance between the drilling rig and obstacles in front, 2D images of obstacles, the pose data of the drilling rig, and the preset drilling rig path; The data processing module is used for: Image processing is performed on a 3D raster cloud image to generate a 2D top-view projection image; Based on obstacle distance information and drilling rig pose data, generate pose estimation values; Based on the two-dimensional image of the obstacle, identify the obstacle category, delineate the bounding box of the obstacle, and obtain the coordinates of the bounding box; The fusion module is used for: Feature extraction is performed on the two-dimensional top-view projection image to extract two-dimensional projection features; Feature extraction is performed on the pose estimate to extract fused pose features; Feature extraction is performed for obstacle and bounding box categories, specifically extracting obstacle features; Feature extraction is performed on the drilling rig path to extract path features; Weights are assigned to the two-dimensional projection features, fused pose features, obstacle features and path features respectively to generate fused features, and a two-dimensional environment map and global positioning coordinates are generated based on the fused features. The path planning module is used for: Generate a global path based on global positioning coordinates; Generate local paths based on the 2D environment map and global paths; The decision control module is used for: Control commands are generated based on local paths and sent to the drilling rig's controller, instructing the drilling rig to drill according to the control commands.

[0017] Furthermore, the sensor module includes a lidar, a millimeter-wave radar, a vision sensor, an inertial measurement unit, and a magnetic navigation sensor; The aforementioned lidar is used to acquire three-dimensional dot cloud images in front of the drilling rig; The millimeter-wave radar is used to collect distance information between the drilling rig and obstacles. The aforementioned vision sensor is used to acquire two-dimensional images of obstacles in front of the drilling rig; The inertial measurement unit is used to collect the pose data of the drilling rig; The magnetic navigation sensor is used to preset the drilling rig path.

[0018] Compared with the prior art, the present invention has the following technical effects: The drilling rig navigation system of this invention, through a sensor module, data processing module, fusion module, path planning module, and decision control module, can efficiently acquire and process preliminary data. At the algorithm level, for A... The algorithm has been improved, generating a global path based on global positioning coordinates and preset drilling target points, and planning local trajectories based on the environmental map and the global path. It can deeply integrate multimodal data, improving adaptability to complex environments, and hierarchically optimize the navigation framework, balancing global efficiency and local obstacle avoidance. It also features an intelligent fault-tolerance mechanism to ensure continuous and stable system operation, making it suitable for large-scale industrial use and promotion. Detailed implementation methods are described below. It should be noted that, unless otherwise specified, all components in this invention are known in the prior art. For example, the inertial measurement unit uses a commonly used inertial measurement unit.

[0019] The following are specific embodiments of the present invention. It should be noted that the present invention is not limited to the following specific embodiments. All equivalent modifications made based on the technical solutions of this application fall within the protection scope of the present invention.

[0020] A drilling rig navigation system includes a sensor module, a data processing module, a fusion module, a path planning module, and a decision control module; The sensor module includes multiple sensors, which are used to acquire three-dimensional dot cloud images in front of the drilling rig, the distance between the drilling rig and obstacles in front, two-dimensional images of obstacles, the pose data of the drilling rig, and the preset drilling rig path. The data processing module is used for: Image processing is performed on a 3D raster cloud image to generate a 2D top-view projection image; Generate pose estimates based on obstacle distance information and pose data; Based on the two-dimensional image of the obstacle, identify the obstacle category, delineate the bounding box of the obstacle, and obtain the coordinates of the bounding box; The fusion module is used for: Feature extraction is performed on the two-dimensional top-view projection image to extract two-dimensional projection features; Feature extraction is performed on the pose estimate to extract fused pose features; Feature extraction is performed for obstacle and bounding box categories, specifically extracting obstacle features; Feature extraction is performed on the drilling rig path to extract path features; Weights are assigned to the two-dimensional projection features, fused pose features, obstacle features and path features respectively to generate fused features, and a two-dimensional environment map and global positioning coordinates are generated based on the fused features. The route planning module is used for: Generate a global path based on global positioning coordinates; Generate local paths based on the 2D environment map and global paths; The decision control module is used for: Control commands are generated based on local paths and sent to the drilling rig's controller, instructing the drilling rig to drill according to the control commands.

[0021] As a more specific implementation, the sensor module includes a lidar, a millimeter-wave radar, a vision sensor, an inertial measurement unit, and a magnetic navigation sensor; LiDAR is used to acquire three-dimensional dot cloud images in front of the drilling rig; Millimeter-wave radar is used to collect distance information between drilling rigs and obstacles. The vision sensor is used to acquire two-dimensional images of obstacles in front of the drilling rig; The inertial measurement unit is used to acquire the drilling rig's pose data; Magnetic navigation sensors are used to preset the drilling rig path.

[0022] As an optional implementation, the drilling path of the magnetic navigation sensor is encoded through an RFID tag array, with each RFID tag storing absolute coordinates and path curvature parameters. LiDAR with Frequency acquisition of 3D point cloud data ; in, The ranging accuracy is , These are the three-dimensional coordinates of the point cloud; LiDAR emits a laser beam and receives the reflected light. It calculates the distance to the target object based on the time of flight of the light, thereby obtaining three-dimensional point cloud information of the surrounding environment. This point cloud data can accurately describe the spatial position and shape of objects in the environment. Millimeter-wave radar acquires obstacle distance information through FMCW technology, with a maximum detection range. Precision Specifically, FMCW technology calculates the distance to obstacles by transmitting a continuous wave whose frequency varies over time and comparing the frequency difference between the transmitted and received waves. It has high accuracy and real-time performance and can quickly detect obstacles around the drilling rig. Inertial Measurement Unit (IMU) outputs pose data , including rotation matrix Translation vector The update rate is ; The inertial measurement unit (IMU) calculates the attitude and position changes of the drilling rig by measuring acceleration and angular velocity, and can provide high-precision attitude information in a short time, providing basic data for the navigation of the drilling rig. Magnetic navigation sensors are implemented using RFID tag arrays, with each tag storing absolute coordinates. and path curvature parameters This constitutes the preset path code. Label spacing ; When the drilling rig passes by the RFID tag, it can read the information in the tag, obtain the preset path planning information, and provide a reference path for navigation; Visual sensors Frame rate acquisition of dynamic obstacle images The resolution is The visual sensor uses the parallax between two cameras to calculate the depth information of an object, thereby identifying the position and shape of dynamic obstacles and providing an important basis for obstacle avoidance. The following describes a drilling rig navigation method based on the above system, including the following steps: Step 0: Set the sampling interval and preset the drilling path; In this embodiment, the drilling path is represented by a drilling path code. The drilling path is preset and designed during the borehole design before construction begins. It mainly refers to the path of the drilling rig from the tunnel entrance to the borehole location.

[0023] Step 1: The drilling rig is instructed to carry out drilling operations in the work area according to the preset drilling rig path. When the sampling interval is reached, the three-dimensional dot cloud image in front of the drilling rig, the distance information between the drilling rig and the obstacle, the two-dimensional image of the obstacle, and the pose data of the drilling rig are collected. Step 2: Compress the 3D raster cloud image into a 2D planar image, and use the pixel values ​​of the 2D planar image to reflect the environmental structural features; Construct the covariance matrix of the distance between the drilling rig and the obstacle and the covariance matrix of the pose data, and then calculate the weight matrix; calculate the drilling rig pose value based on the weight matrix and the drilling rig pose data; The U-Net model is used to perform semantic segmentation on the 2D obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the 2D obstacle image; Furthermore, step 2 includes the following sub-steps: Step 21: Perform gridding processing on the three-dimensional point cloud image, uniformly divide the three-dimensional point cloud image into multiple pixel units, and calculate the point cloud density in each pixel unit. In this embodiment, the 3D raster cloud image is divided into... The cells are then used to calculate the point cloud density within each cell to generate a two-dimensional top-view projection.

[0024] Rasterization can convert 3D point cloud data into a more easily processed 2D image format, highlighting the structural features of the environment and facilitating subsequent analysis and processing. Next, the 3D point cloud image is compressed into a 2D planar image. Each pixel value in the 2D planar image represents the point cloud density at that location, and the point cloud density reflects the environmental structural features. Feature extraction is performed on the pose data and the distance information between the drilling rig and obstacles to obtain the drilling rig pose value.

[0025] Step 22: Construct the covariance matrix of the distance between the drilling rig and the obstacle, and the covariance matrix of the pose data. Calculate the weight matrix based on these two matrices, as shown below:

[0026] in, Represents the weight matrix; The covariance matrix representing the distance between the drilling rig and the obstacle; Represents the covariance matrix of the pose data; Step 23, based on the weight matrix Calculate the fused drilling rig pose value and use it as the final drilling rig pose value, as shown in the following formula: ; in, This represents the fused drilling rig pose value; This represents the XOR operator. This represents the drilling rig's position and orientation data; Indicates the distance between the drilling rig and the obstacle; Step 24: Perform semantic segmentation on the two-dimensional obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the image.

[0027] In this step, the two types of data are fused by calculating a weight matrix to improve the accuracy and stability of pose estimation; When performing semantic segmentation on 2D obstacle images, the U-Net model is specifically used. The U-Net model is a classic architecture in image segmentation tasks. This model requires training, and the training set consists of binocular vision images labeled with obstacle information. This model was proposed by foreign scholars in 2015 for processing medical images.

[0028] In the steps of this embodiment, the two-dimensional image itself does not contain semantic information. The U-Net model is trained using conventional methods, such as using images with semantic meaning as the training set, so that the U-Net model can extract the semantic information of the image by learning from the labeled images.

[0029] After the U-Net model is trained, the two-dimensional obstacle image that needs semantic segmentation is input into the model. The model can assign meaning to each region of the image, such as background and obstacle. The U-Net model accurately identifies and locates dynamic obstacles in the image, and outputs the obstacle category and bounding box coordinates, providing key information for subsequent obstacle avoidance decisions. In addition, this embodiment provides a more specific implementation method. For obstacle distance information, Kalman filtering is used to eliminate its noise interference, and based on timestamp synchronization and coordinate transformation matrix, the obstacle distance information and dynamic obstacle image are spatiotemporally aligned and unified to the coordinate system of the lidar to ensure the consistency of multi-source data in time and space.

[0030] The U-Net model takes an image as input. The output is the obstacle category. and bounding box coordinates ,in and These are the coordinates of the two corner points of the bounding box.

[0031] Step 3: Extract features from the drilling rig path preset in Step 0, the environmental structural features obtained in Step 2, the drilling rig pose value, the obstacle category, and the coordinates of the two corner points of the obstacle bounding box. Weights are assigned to the extracted features to generate an environment map; Based on the environmental map and 3D dot cloud image, the ICP algorithm is used to obtain the global positioning coordinates of the area to be operated. Furthermore, step 3 includes the following sub-steps: Step 31: Extract features from the drilling rig path preset in Step 0, the environmental structure features obtained in Step 2, the drilling rig pose value, the obstacle category, and the coordinates of the two corner points of the obstacle bounding box, and obtain the corresponding path features, environmental structure features, pose features, and obstacle features. In this step, the environmental structure features are obtained by extracting features from the two-dimensional top-view projection map of step 2 using a CNN convolutional neural network. Specifically, the feature extraction process of a CNN convolutional neural network from a two-dimensional top-view projection image is well-known in the art. This embodiment provides a more specific extraction process: Environmental structural features are extracted using CNN to extract two-dimensional projection map features. CNN convolutional neural networks include Convolutional layers, in which This is a two-dimensional top-view projection from the previous steps; More specifically, a two-dimensional top-down projection map is a grayscale image. Taking a 0.1m x 0.1m area in the actual environment as an example, where each pixel corresponds to a pixel value, the pixel value represents the point cloud density at that location (i.e., the number density of points scanned by the LiDAR). This image visually reflects the distribution of ground obstacles, walls, slopes, and other structures.

[0032] A CNN (Convolutional Neural Network) contains 12 convolutional layers, each consisting of multiple convolutional kernels (filters). It can automatically detect local patterns in an image. A two-dimensional top-view projection image is fed into the CNN and passes through each layer in sequence. In the early, shallow convolutional layers (such as the first 2-3 layers), the kernel size is small (e.g., 3×3 or 5×5), primarily responding to low-level geometric features in the image, such as edges (obstacle boundaries), corners, and lines (ground textures). The output consists of multiple feature maps, each highlighting a specific local structure.

[0033] Mid-level convolutional layers (such as layers 4-8): These combine shallow features to detect shape features, such as rectangular obstacles, circular devices, the outline of ramps, and the boundaries of narrow passages. At this stage, the size of the feature map gradually decreases, but the number of channels (feature types) increases.

[0034] Deep convolutional layers (such as layers 9-12) further develop high-level semantic features, such as region flatness assessment, obstacle density estimation, distinction between feasible and infeasible regions, and channel width perception. These features go far beyond the original pixels and represent a high-level understanding of the environmental structure.

[0035] After 12 convolutional layers, the network outputs a high-dimensional feature map (e.g., with a size of 16×16×512, i.e., 256 spatial locations, each with 512 feature channels).

[0036] To obtain a feature vector of fixed length, the system performs the following operations: Global average pooling: For each feature channel, calculate the average value in its spatial dimension, compressing the feature map into a 512-dimensional vector (one average value for each channel).

[0037] Fully connected dimensionality reduction: The 512-dimensional vector is input into a fully connected layer, which outputs a 256-dimensional environmental structure feature vector. This vector condenses the key geometric and structural information in the original projection image.

[0038] The resulting environmental structure feature vector will serve as an input to the attention mechanism, participating in subsequent weighted fusion along with other features (pose features, obstacle features, path features).

[0039] The pose features are obtained by extracting features from the fused drilling rig pose values ​​in step 2 using an MLP (Multilayer Perceptron). The process of extracting features from the fused drilling rig pose values ​​in step 2 using an MLP (Multilayer Perceptron) is also well-known in the field. This embodiment provides a more specific extraction method: Pose features are extracted and fused using MLP. ,Include Convolutional layers, in which This refers to the pose information fused from the previous steps; More specifically, the drilling rig pose value is a homogeneous transformation matrix, which in this embodiment is a 4×4 homogeneous transformation matrix, including a rotation matrix. (3×3): Describe the orientation of the drilling rig in three-dimensional space (heading angle, pitch angle, roll angle).

[0040] Translation vector (3×1): Describes the position coordinates (x, y, z) of the drilling rig, with the last row fixed at [0, 0, 0, 1]. In addition, the fused pose values ​​may also include information such as velocity and angular velocity, which are determined based on the actual working conditions.

[0041] When extracting features from the drilling rig pose values, since the input to the MLP requires a fixed-length one-dimensional vector, the 4×4 homogeneous transformation matrix is ​​first flattened into a 16-dimensional original vector.

[0042] MLPs consist of alternating fully connected layers and nonlinear activation functions, and a typical structure is as follows: First layer (input layer → hidden layer 1): Input dimension = 16 (or more, or 19 if velocity is included) Output dimension = 128 Activation function: ReLU (Modified Linear Unit) Second layer (Hidden layer 1 → Hidden layer 2): Input dimension = 128 Output dimension = 256 Activation function: ReLU Third layer (Hidden layer 2 → Output layer): Input dimension = 256 Output dimension = 256 The activation function is adaptively selected based on the actual operating conditions.

[0043] The final output is a 256-dimensional pose feature vector, denoted as... .

[0044] The flattened pose vector is fed into the MLP, and the following calculations are performed sequentially: First layer: ,in It is a weight matrix (128×16). It is a bias vector (128 dimensions).

[0045] Second layer: , It is 256×128. It has 256 dimensions.

[0046] Third layer: , It is 256×256. It has 256 dimensions.

[0047] Through these linear transformations and nonlinear mappings, the original pose values ​​are encoded into higher-level feature representations.

[0048] The final pose feature is a feature vector that includes key information such as the spatial position, attitude, and velocity of the drilling rig.

[0049] Feature extraction is performed by extracting bounding boxes and obstacle category features using an MLP (Multilayer Perceptron) to obtain obstacle features; Extracting obstacle features using an MLP is a well-known technique in the field. This embodiment provides a more detailed process: First, the obstacle category labels are converted into vectors through hot coding or embedding layers, and the coordinates of the two corner points of the bounding box are normalized to the [0,1] interval. Then, the category codes and normalized coordinates are concatenated into a low-dimensional original vector. Next, this vector is input into a multilayer perceptron (MLP), and after two or three fully connected layers (e.g., 14→128→256) of nonlinear mapping, a 256-dimensional obstacle feature vector is finally output for subsequent multimodal fusion.

[0050] Obstacle features are extracted into bounding boxes and category features using MLP. ,in and These represent the obstacle category and bounding box coordinates output by the model in the previous steps, respectively. The path features are obtained by extracting features from the preset path encoding through the embedding layer; The following is a specific embodiment illustrating the specific process of this step: In this step, the input is a preset path encoding sequence. ,Include There are 3 points, each with 3 values ​​(x, y, κ).

[0051] For each point, a 128-dimensional vector is obtained through the embedding layer.

[0052] Will The 128-dimensional vectors of each point are averaged element-wise to obtain a 128-dimensional path feature. This path feature is then increased to 256 dimensions using a fully connected layer (128→256), yielding the final path feature. It contains global geometric information (position direction, curvature change) of the preset path.

[0053] Preset path features are generated through the embedding layer of a CNN convolutional neural network: The embedding dimension is 128, where This refers to the three-dimensional dot cloud image acquired by the lidar in the previous steps; Step 32: Use an attention mechanism to assign corresponding weights to the environmental structure features, pose features, obstacle features and path features obtained in step 31, and generate an environmental map. In this step, it is well known that attention mechanisms are used to generate environment maps from features. This embodiment, based on existing technology, assigns corresponding weights to the environmental structure features, pose features, obstacle features, and path features obtained in step 31 to generate an environment map, as detailed below: The generation of the environment map begins with four types of data: a two-dimensional top-down projection map (reflecting point cloud density), fused drill rig pose values, obstacle categories and bounding box coordinates, and a pre-defined path encoding. Next, these are extracted into a unified-dimensional feature vector (e.g., 256-dimensional) using a CNN (environment), MLP (pose), MLP (obstacle), and embedding layer (path), respectively. Then, an attention mechanism is used with pose features as the query, environment features as the key, and obstacle features as the value to calculate dynamic weights. These weighted sums are then superimposed with the path features to form a fused feature vector. Finally, this vector is upsampled and reconstructed using a decoder (transposed convolutional network) to generate a two-dimensional raster map.

[0054] An environmental map is a two-dimensional raster map, offered as an alternative: Each 0.1m×0.1m grid cell represents the probability of an obstacle existing at that location (a continuous value from 0 to 1), derived from lidar point cloud density and radar distance information.

[0055] The semantic labels (such as rocks, equipment, and people) and their credibility for each grid are derived from the semantic segmentation results.

[0056] The map incorporates the direction and curvature information of the preset path (through path feature overlay), enabling subsequent path planning to prioritize searching along the preset path direction.

[0057] The map coordinate system is aligned with the global positioning coordinates (real-time point cloud and map are registered via ICP), and the pose values ​​ensure the spatiotemporal consistency of multi-frame data projection.

[0058] In this embodiment, the environment map is a two-dimensional raster map that integrates geometric occupancy, semantic category, and path prior, providing a unified environment representation for global path planning in subsequent steps.

[0059] If any modal data of environmental structure features, pose features, or obstacle features fails, the weight of path features is increased. ,make:

[0060] If any two or more modal data fail, the drilling rig will stop drilling.

[0061] The preset path encoding weight can be forcibly increased to 0.5, which means that the preset path encoding will dominate.

[0062] In practice, the magnitude of the increase in path feature weights is positively correlated with the importance of the failure mode. For example, failure of environmental structural features leads to the largest increase in weights; failure of pose estimation leads to a moderate increase in weights; and failure of obstacle perception leads to the smallest increase in weights. The specific increase value should be adaptively adjusted by those skilled in the art based on the actual situation.

[0063] It should be further explained that in this step, data validation is required for environmental structural features, short-term pose estimation, obstacle category, and bounding box coordinates. If any of them fails the data validation, the corresponding data is removed, and the weight of the preset path code is forcibly increased to be greater than or equal to a preset threshold, such as 0.5.

[0064] If any two of them fail the data verification, a stop working command is output to the drilling rig drive controller to control the drilling rig to stop working. In this step, data validation is performed on environmental structural features, short-term pose estimates, and obstacle categories. This can be done through the following processing: When verifying environmental structural features, a density threshold is first preset, which is the maximum detection density of the sensor. The density of the dot cloud in each pixel unit is compared with the density threshold. If the density threshold is exceeded, the environmental structural feature is considered to be invalid. When verifying the pose features, the specific steps are to verify whether the coordinates and attitude angles of the pose feature values ​​are within the working area of ​​the drilling rig. If any one of them exceeds the working area of ​​the drilling rig, the pose feature is considered to be invalid. When validating obstacle features, a set of obstacle types consisting of multiple obstacle types is first preset. The system then determines whether an obstacle type falls into the obstacle type set. If not, the obstacle feature is considered invalid.

[0065] The preset density range, preset construction range, preset angle range, and preset category set can be set by relevant personnel on site. This embodiment only provides one specific setting method, which will not be elaborated here.

[0066] In addition, this embodiment provides a further solution. To ensure the reliability of the data, a consistency check can be performed on it. The environmental structural features, short-term pose estimation values, obstacle categories and bounding box coordinates are checked. If any one of them fails the data check, the modal data is considered invalid and the corresponding data is removed. If any two of them fail the data check, a stop working command is output to the drilling rig drive controller to control the drilling rig to stop working. The consistency check specifically compares the timestamps of multi-source data to ensure that the time difference between the short-term pose estimation value and the acquisition time of the environmental structural features is within the threshold, and checks the spatial consistency between the bounding box coordinates and the obstacle distance information. It should also be noted that the weight of the preset path code can be set as follows: if any one of the checks fails, the data is removed and the weight of the preset path code is increased; if any two checks fail, an emergency stop command is triggered.

[0067] Step 33: Based on the environmental map and the 3D point cloud image, the ICP algorithm is used to obtain the global positioning coordinates of the drilling rig. The global positioning coordinates are calculated by the degree of registration between the point cloud data collected by the lidar and the environmental map.

[0068] Specifically, the optimal transformation matrix (including translation and rotation, corresponding to the position and orientation of global coordinates) between the real-time laser point cloud and the map point cloud is found iteratively to minimize the distance error between the two point sets until the error is less than the threshold or the maximum number of iterations is reached. Finally, the coordinates corresponding to the transformation matrix are the global coordinates.

[0069] Step 4: Based on the environmental map and the global positioning coordinates of the area to be worked on, use A Algorithm to obtain the optimal global path for the drilling rig; Specifically, step 4 includes the following sub-steps: Step 41: Read the preset drilling target point coordinates from the construction design file of the area to be worked, and mark the key locations as points of interest in the environmental map obtained in step 3; Among them, points of interest include must-pass points, preferred points, and points to avoid; A necessary point is a point that the drilling rig must pass through and cannot bypass in its path; The preferred points are the points that can be selected to pass through in the drilling rig path; The avoidance point is specifically the point that must be avoided in the drilling rig path; In this field, those skilled in the art can adaptively define the necessary points, preferred points, and avoidance points according to the actual situation. This embodiment does not specifically limit them, but provides an optional implementation method: Typical scenarios of essential points of passage: 1. Narrow passage entrance: In mines or tunnels, there are certain areas that can only be accessed through a single passage, and the drilling rig must pass through this passage to reach the target area.

[0070] 2. Key turning points: Some curves are the only viable turning points; going around them will prevent you from continuing forward.

[0071] 3. Equipment maintenance points: If the drilling rig needs to pass through a certain equipment point for inspection or resupply during its journey, that location becomes a necessary point.

[0072] 4. Connecting nodes of two paths: When a global path is composed of multiple sub-paths, the connecting node is a necessary point.

[0073] Typical scenarios for preference points: 1. Main road nodes: In the mine road network, the main roads are in better condition and flatter. Drilling rigs should prioritize traveling along the main roads rather than taking rugged paths.

[0074] 2. Flat road areas: Some areas have flat terrain and gentle slopes, which are more fuel-efficient and safer to drive in, and should be given priority.

[0075] 3. Known safe areas: Areas that have been confirmed to be free of falling rocks and collapse risks should be passed through first.

[0076] 4. Signal coverage area: If communication needs to be maintained, the path with signal coverage should be selected first.

[0077] Typical scenarios for avoiding points: 1. Dangerous areas: There may be dangerous areas such as rockfall areas, collapse areas, and water accumulation areas in the mine. Drilling rigs should stay away from these areas.

[0078] 2. Fixed obstacles: Large equipment, support columns, material storage areas and other fixed obstacles that cannot be crossed should be kept away to avoid collisions.

[0079] 3. Sensitive equipment areas: Drilling rigs should maintain a safe distance around certain precision equipment or important facilities.

[0080] 4. Noise / Vibration Sensitive Areas: If the construction area has strict requirements for vibration, it should be kept away from this area.

[0081] Step 42: Assign a corresponding weight to each point of interest, such that the weight of a point that must be visited is greater than that of a point that is preferred, which is greater than that of a point that should be avoided. Specifically, the weight of necessary points is 0.8-1; the weight of preferred points is 0.3-0.7; and the weight of avoidance points is 0-0.2. Step 43: Set the current location of the drilling rig as the path start point and the target point of the work area as the path end point, and establish a list of nodes to be evaluated that includes the path start point, all points of interest and the path end point. Step 44: Take the starting point of the path as a candidate node. Starting from the candidate node, calculate the cost of moving from the candidate node to each adjacent node. Select the adjacent node with the smallest cost as the new candidate node. In step 44, specifically for A... The cost function of the algorithm is modified, and the modified A is used. The algorithm's cost function calculates the cost from a candidate node to each of its neighboring nodes; A The algorithm itself is an existing algorithm primarily used to solve path planning problems, such as robot navigation or pathfinding for game NPCs. It combines the accuracy of Dijkstra's algorithm with the efficiency of best-first search, using a heuristic function to guide the search direction and improve search efficiency.

[0082] Correction A The cost function of the algorithm specifically includes the following steps: First, A is corrected using candidate nodes and neighboring nodes. The cost function of the algorithm is as follows:

[0083] in, Indicates candidate nodes; A represents The cost function of the algorithm; This represents the cost function after the initial correction. This represents any adjacent node in the list of nodes to be evaluated that is adjacent to the candidate node. This represents the list of nodes to be evaluated. This represents the weight value corresponding to that node; Indicates candidate nodes To adjacent nodes Euclidean distance; The cost function after the initial correction is kinematically corrected again to obtain the corrected cost function, as shown in the following equation: , in, This represents the cost function after the correction is completed; This represents the weight of the influence of the node curvature term on the cost function, [0.05, 0.3]; Represents the curvature of a node; Represents the acceleration of a node; Indicates the length of the time domain; Indicates time; Represented as a constant; This is the integration operator; The weights representing the influence of node acceleration on the cost function are [0.05, 0.3].

[0084] Compared to traditional algorithms, the improved A in this embodiment... The algorithm significantly improves detour rate, path execution success rate, average curvature, path planning time, and redundant node computation, as shown in Table 1. Table 1 Traditional Traditional A Algorithm and Improved A Algorithm Comparison Table

[0085] Step 45: Designate the new candidate node as a candidate node, and repeat step 44 until the end of the path is reached. Step 46: Perform curve fitting on the discrete curve composed of the path start point, all candidate nodes and the path end point to generate a continuous and smooth curve, which serves as the optimal global path for the drilling rig.

[0086] Step 5: Based on the optimal global path and the environment map, use the TEB algorithm to plan the local trajectory; Using the TEB algorithm to plan local trajectories is a common method in this field. As an optional solution, obstacle distance information and dynamic obstacle images are spatiotemporally aligned and unified to the coordinate system of the lidar based on timestamp synchronization and coordinate transformation matrix. Spatiotemporal alignment can unify data acquired by different sensors in time and space, ensuring data consistency and accuracy, and providing reliable data support for local trajectory planning. Specifically, the objective function is to minimize the trajectory deviation and the change in control input; ,in, For a moment t The actual pose. The reference pose is obtained from global path planning. For changes in velocity and angular velocity; Constraints ; The TEB algorithm can plan local trajectories that meet constraints in real time based on the environment map and global path, enabling the drilling rig to operate safely and efficiently in complex environments. Step 6: Based on obstacle distance information and local trajectory, generate control commands using the MPC algorithm; Generating control commands using the MPC algorithm is a well-known technique in this field. As an optional approach, the feasibility of the control commands is verified using the drilling rig kinematic model, and feasible control commands are output. Specifically, the kinematic model of the drilling rig. ,in, These are the coordinates of the drilling rig's position. The linear velocity of the drilling rig. The drilling rig's heading angle. For steering angle, This refers to the wheelbase; In the prediction time domain Internal optimization control input The objective function is, The optimal control sequence is solved by rolling optimization; The MPC algorithm can optimize the control input in real time based on the current state and the predicted future state, enabling the drilling rig to better track the local trajectory and improve the smoothness and stability of the motion. The feasibility of the steering angle was verified by inverse kinematics. ,make sure and ; By verifying the feasibility of control commands, we can prevent the drilling rig from executing unreasonable control commands and ensure the safe operation of the drilling rig. Step 7: Drive the drilling rig according to the control command, so that the drilling rig moves along the local trajectory.

[0087] As an optional solution, control commands are sent to the drilling rig drive controller via the CAN bus. The data frame of the control command includes: speed command field, steering angle command field, and emergency stop flag. Specifically, control commands are sent via the CAN bus, using the J1939 protocol, and the data frame format is as follows. ,in, The range is (1 byte) The range is (2 bytes) Emergency stop flag Occupying 1 position; Communication baud rate set to Priority is 2 (maximum is 0); The CAN bus has advantages such as high reliability and strong real-time performance, which can ensure that control commands are transmitted to the drilling rig drive controller accurately and quickly. After receiving control commands, the drilling rig drive controller controls the drilling rig's motor and steering system according to the speed and steering angle commands in the commands, causing the drilling rig to travel along the planned local trajectory. If the emergency stop flag is triggered, the drilling rig drive controller will immediately stop the drilling rig to ensure the safety of equipment and personnel. Step 8: Repeat steps 1-7 until the drilling operation is completed.

[0088] In summary, the drilling rig navigation method and system disclosed in this embodiment can deeply integrate multimodal data, improve adaptability to complex environments, optimize the navigation framework in layers, balance global efficiency and local obstacle avoidance, and have an intelligent fault-tolerant mechanism to ensure the continuous and stable operation of the system.

Claims

1. A drilling rig navigation method, characterized in that, Includes the following steps: Step 0: Preset the drilling path and set the sampling interval; Step 1: The drilling rig is instructed to carry out drilling operations in the work area according to the preset drilling rig path. When the sampling interval is reached, the three-dimensional dot cloud image in front of the drilling rig, the distance information between the drilling rig and the obstacle, the two-dimensional image of the obstacle, and the pose data of the drilling rig are collected. Step 2: Compress the 3D raster cloud image into a 2D planar image, and use the pixel values ​​of the 2D planar image to reflect the environmental structural features; Construct the covariance matrix of the distance between the drilling rig and the obstacle and the covariance matrix of the pose data, and then calculate the weight matrix; calculate the drilling rig pose value based on the weight matrix and the drilling rig pose data; The U-Net model is used to perform semantic segmentation on the 2D obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the 2D obstacle image; Step 3: Extract features from the drilling rig path, environmental structure features, drilling rig pose value, obstacle category, and the coordinates of the two corner points of the obstacle bounding box, and then obtain the path features, environmental features, pose features, and obstacle features accordingly. Assign weights to the features mentioned above and generate an environment map; Based on the environmental map and 3D dot cloud image, the ICP algorithm is used to obtain the global positioning coordinates of the area to be operated. Step 4: Based on the environmental map and the global positioning coordinates of the area to be worked on, use A The algorithm obtains the optimal global path for the drilling rig; Step 5: Based on the optimal global path and the environment map, use the TEB algorithm to plan the local trajectory; Step 6: Based on obstacle distance information and local trajectory, generate control commands using the MPC algorithm; Step 7: Drive the drilling rig according to the control command, so that the drilling rig moves along the local trajectory; Step 8: Repeat steps 1-7 until the drilling operation is completed.

2. The drilling rig navigation method as described in claim 1, characterized in that, Step 2 includes the following sub-steps: Step 21: Perform gridding processing on the three-dimensional point cloud image, uniformly divide the three-dimensional point cloud image into multiple pixel units, and calculate the point cloud density in each pixel unit. Next, the 3D point cloud image is compressed into a 2D planar image. Each pixel value in the 2D planar image is used to represent the point cloud density at that location, and then the point cloud density is used to reflect the environmental structural features. Step 22: Construct the covariance matrix of the distance between the drilling rig and the obstacle, and the covariance matrix of the pose data. Calculate the weight matrix based on these two matrices, as shown below: in, Represents the weight matrix; The covariance matrix representing the distance between the drilling rig and the obstacle; Represents the covariance matrix of the pose data; Step 23, based on the weight matrix Calculate the fused drilling rig pose value and use it as the final drilling rig pose value, as shown in the following formula: ； in, This represents the fused drilling rig pose value; This represents the XOR operator. This represents the drilling rig's position and orientation data; Indicates the distance between the drilling rig and the obstacle; Step 24: Perform semantic segmentation on the two-dimensional obstacle image to obtain the obstacle category and the coordinates of the two corner points of the obstacle bounding box in the image.

3. The drilling rig navigation method as described in claim 2, characterized in that, Step 3 includes the following sub-steps: Step 31: Extract features from the drilling rig path preset in Step 0, the environmental structure features obtained in Step 2, the drilling rig pose value, the obstacle category, and the coordinates of the two corner points of the obstacle bounding box, and obtain the corresponding path features, environmental structure features, pose features, and obstacle features. Step 32: Use an attention mechanism to assign corresponding weights to path features, environmental structure features, pose features, and obstacle features to generate an environment map; Step 33: Based on the environmental map and the 3D dot cloud image, the global positioning coordinates of the drilling rig are obtained using the ICP algorithm.

4. The drilling rig navigation method as described in claim 3, characterized in that, In step 32, if any modal data of environmental structure features, pose features, or obstacle features fails, the weight of path features is increased. ,make: If any two or more modal data fail, the drilling rig will stop drilling.

5. The drilling rig navigation method as described in claim 4, characterized in that, In step 32, the environmental structural features, pose features, and obstacle features are verified respectively, and the modal data is determined to be invalid based on the verification results. Specifically, when verifying environmental structural features, a density threshold is first preset, which is the maximum detection density of the sensor. The density of the dot cloud in each pixel unit is compared with the density threshold. If the density threshold is exceeded, the environmental structural feature is considered to be invalid. When verifying the pose features, the specific steps are to verify whether the coordinates and attitude angles of the pose feature values ​​are within the working area of ​​the drilling rig; if any one of them exceeds the working area of ​​the drilling rig, the pose feature is considered invalid. When validating obstacle features, a set of obstacle types consisting of multiple obstacle types is first preset, and it is determined whether an obstacle type falls into the obstacle type set; if not, the obstacle feature is considered invalid.

6. The drilling rig navigation method as described in claim 5, characterized in that, Step 4 includes the following sub-steps: Step 41: Read the preset drilling target point coordinates from the construction design file of the area to be worked, and mark the key locations as points of interest in the environmental map obtained in step 3; The points of interest include must-pass points, preferred points, and avoidance points; The aforementioned "must-pass points" are specifically points that the drilling rig must pass through and cannot bypass in its path; The preferred points are specifically points in the drilling rig path that can be passed through or bypassed; The avoidance points mentioned are specifically the points that must be avoided in the drilling rig path; Step 42: Assign a corresponding weight to each point of interest, such that the weight of a point that must be visited is greater than that of a point that is preferred, which is greater than that of a point that should be avoided. Specifically, the weight of necessary points is 0.8-1; the weight of preferred points is 0.3-0.7; and the weight of avoidance points is 0-0.

2. Step 43: Set the current location of the drilling rig as the path start point and the target point of the work area as the path end point, and establish a list of nodes to be evaluated that includes the path start point, all points of interest and the path end point. Step 44: Take the starting point of the path as a candidate node. Starting from the candidate node, calculate the cost of moving from the candidate node to each adjacent node. Select the adjacent node with the smallest cost as the new candidate node. Step 45: Designate the new candidate node as a candidate node, and repeat step 44 until the end of the path is reached. Step 46: Perform curve fitting on the discrete curve composed of the path start point, all candidate nodes and the path end point to generate a continuous and smooth curve, which serves as the optimal global path for the drilling rig.

7. The drilling rig navigation method as described in claim 6, characterized in that, In step 44, the cost function is used to calculate the cost of moving from the candidate node to each of the adjacent nodes. Among them, A The cost function of the algorithm is as follows: ， in, This indicates that A is used in step 44. The cost function of the algorithm; Indicates the corrected A The cost function of the algorithm; This represents the weight of the influence of the node curvature term on the cost function, [0.05, 0.3]; Indicates the length of the time domain; Indicates time; Represents the curvature of a node; Represents the acceleration of a node; Represented as a constant; This is the integration operator; The weight representing the influence of node acceleration on the cost function is [0.05, 0.3]. Specifically, in, Indicates candidate nodes; A represents The algorithm's original cost function; Indicates the corrected A Algorithm cost function This represents any adjacent node in the list of nodes to be evaluated that is adjacent to the candidate node. This represents the list of nodes to be evaluated. This represents the weight value corresponding to that node; Indicates candidate nodes To adjacent nodes Euclidean distance.

8. A drilling rig navigation method corresponding to the drilling rig navigation method of claim 7, characterized in that, It includes a sensor module, a data processing module, a fusion module, a path planning module, and a decision control module; The sensor module is used for: Collect 3D dot cloud images in front of the drilling rig, the distance between the drilling rig and obstacles in front, 2D images of obstacles, the pose data of the drilling rig, and the preset drilling rig path; The data processing module is used for: Image processing is performed on a 3D raster cloud image to generate a 2D top-view projection image; Based on obstacle distance information and drilling rig pose data, generate pose estimation values; Based on the two-dimensional image of the obstacle, identify the obstacle category, delineate the bounding box of the obstacle, and obtain the coordinates of the bounding box; The fusion module is used for: Feature extraction is performed on the two-dimensional top-view projection image to extract two-dimensional projection features; Feature extraction is performed on the pose estimate to extract fused pose features; Feature extraction is performed for obstacle and bounding box categories, specifically extracting obstacle features; Feature extraction is performed on the drilling rig path to extract path features; Weights are assigned to the two-dimensional projection features, fused pose features, obstacle features and path features respectively to generate fused features, and a two-dimensional environment map and global positioning coordinates are generated based on the fused features. The path planning module is used for: Generate a global path based on global positioning coordinates; Generate local paths based on the 2D environment map and global paths; The decision control module is used for: Control commands are generated based on local paths and sent to the drilling rig's controller, instructing the drilling rig to drill according to the control commands.

9. The drilling rig navigation system as described in claim 8, characterized in that, The sensor module includes a lidar, a millimeter-wave radar, a vision sensor, an inertial measurement unit, and a magnetic navigation sensor; The aforementioned lidar is used to acquire three-dimensional dot cloud images in front of the drilling rig; The millimeter-wave radar is used to collect distance information between the drilling rig and obstacles. The aforementioned vision sensor is used to acquire two-dimensional images of obstacles in front of the drilling rig; The inertial measurement unit is used to collect the pose data of the drilling rig; The magnetic navigation sensor is used to preset the drilling rig path.

Images