A Multi-Sensor Fusion Method for Drilling and Anchoring Robot Localization

Abstract

This application relates to the field of robot localization technology, and in particular to a multi-sensor fusion method for locating drilling and anchoring robots. The method includes: acquiring point cloud data of an underground roadway within a target time period in a radar coordinate system; acquiring visual images of the underground roadway within the target time period in a camera coordinate system and extracting feature points from the visual images; fusing the feature points at the same time point within the target time period with the point cloud data to obtain target point cloud data; constructing a roadway model based on the target point cloud data; establishing a fuselage observation model based on the feature points; acquiring inertial navigation data of the underground roadway within the target time period in an inertial navigation coordinate system; establishing a fuselage prediction model based on the inertial navigation data; fusing the fuselage observation model and the fuselage prediction model to obtain a target model; and outputting the target pose of the drilling and anchoring robot at each time point within the target time period. This application improves the localization accuracy of drilling and anchoring robots in underground roadways.

Description

Technical Field

[0001] This application relates to the field of robot positioning technology, and in particular to a multi-sensor fusion method for positioning a drilling and anchoring robot. Background Technology

[0002] With the rapid development of intelligent coal mine construction in my country, coal mine drilling and anchoring robots have emerged to improve the automation level of drilling and anchoring operations and ensure worker safety. Due to the complex geological conditions, harsh underground environment, and dynamic changes in the spatial environment during mining, accurately controlling the positioning of drilling and anchoring robots has become a key factor in improving the safety, reliability, and intelligence of drilling and anchoring operations.

[0003] Existing drilling and anchoring robot positioning methods, such as the Chinese patent application with publication number CN112068543A, propose a vision-based calibration method for precise drilling positioning of coal mine drilling and anchoring robots. This method employs a photoelectric encoder and angular displacement sensor to construct a semi-closed-loop control system for the translational distance and rotation angle of the drilling rig on the drilling and anchoring platform. An inclination sensor is used to collect the ground inclination information of the drilling and anchoring robot, enabling the drilling platform's posture adjustment to compensate for errors caused by the ground inclination. A vision-based approach is used to intelligently align the drilling rig's end with the anchor mesh hole to achieve precise drilling, thus realizing accurate positioning of the drilling rig. Further examples are provided. For example, Chinese patent application CN114658486A proposes a collision avoidance method and system for a drilling and anchoring robot and a tunneling machine. It uses multiple ultrasonic sensors on the left and right sides of the outer wall of the drilling and anchoring robot to detect the distance between various positions of the drilling and anchoring robot and the coal face in real time, and uses multiple ultrasonic sensors on the inner wall of the drilling and anchoring robot to detect the distance relationship between the drilling and anchoring robot and the tunneling machine in real time. This enables collision avoidance warnings between the drilling and anchoring robot and the tunneling machine. At the same time, based on the difference in width between the tunneling machine and other objects, invalid alarm information caused by personnel and other factors is eliminated.

[0004] However, the existing technologies mentioned above, which utilize multiple sensors for drilling and anchoring robot positioning, still have certain drawbacks. Coal mining operations often involve complex working environments such as dust and water mist, which significantly affect the sensors' ability to capture environmental features. Furthermore, sensors accumulate errors over long periods of use, and the functionality of individual sensors is limited, further reducing the positioning accuracy of the drilling and anchoring robot. Therefore, a multi-sensor fusion method for drilling and anchoring robot positioning is needed to overcome the inherent limitations of individual sensor functions while improving the positioning accuracy of the drilling and anchoring robot. Summary of the Invention

[0005] This application provides a multi-sensor fusion method for positioning drilling and anchoring robots to solve the problems mentioned in the background art.

[0006] To achieve the aforementioned objectives, this invention proposes a multi-sensor fusion-based drilling and anchoring robot positioning method, comprising:

[0007] S1: Acquire point cloud data of the underground roadway within the target time period in the radar coordinate system, acquire visual images of the underground roadway within the target time period in the camera coordinate system, extract feature points from the visual images, fuse feature points at the same time point within the target time period with point cloud data to obtain target point cloud data, and construct a roadway model based on the target point cloud data.

[0008] S2: Establish a fuselage observation model based on feature points, acquire inertial navigation data in the underground roadway within the target time period in the inertial navigation coordinate system, establish a fuselage prediction model based on the inertial navigation data, fuse the fuselage observation model and fuselage prediction model to obtain the target model, and output the target pose of the drilling and anchoring robot at each time point within the target time period.

[0009] S3: Establish a spatial transformation model, and based on the spatial transformation model, transform the radar coordinate system, camera coordinate system, and inertial navigation coordinate system to the body coordinate system;

[0010] S4: Transform the target posture and tunnel model into the body coordinate system, and obtain the motion trajectory of the drilling and anchoring robot in the body coordinate system based on the target posture of the drilling and anchoring robot at each time point within the target time period.

[0011] Furthermore, fusing feature points and point cloud data at the same time point within the target time period includes the following steps:

[0012] Based on Formula 1, the point cloud data in the radar coordinate system is transformed to the camera coordinate system. Formula 1 is:

[0013]

[0014] in, For the camera coordinate system, Let be the rotation matrix from the radar coordinate system to the camera coordinate system. For radar coordinate system, This is the translation vector from the radar coordinate system to the camera coordinate system;

[0015] The ICP algorithm is used to register feature points and transformed point cloud data to obtain fused target point cloud data.

[0016] Furthermore, the registration process for the feature points and the transformed point cloud data based on the ICP algorithm includes the following steps:

[0017] Select a subset of feature points as an initial matching point set, determine the initial corresponding point set of the feature points in the transformed point cloud data, calculate the optimal rotation matrix, optimal translation vector, and registration error based on the initial matching point set and the initial corresponding point set, set a first threshold, and determine whether the registration error is less than the first threshold. If yes, output the registered target point cloud data; otherwise, re-match the initial matching point set and the initial corresponding point set based on the optimal rotation matrix and optimal translation vector, iteratively calculate the registration error, until the registration error is less than the first threshold.

[0018] Furthermore, establishing a fuselage observation model based on feature points includes the following steps:

[0019] The displacement vectors of each feature point at adjacent time points within the target time period are obtained. Based on the displacement vectors, the first pose change of the drilling and anchoring robot within the target time period is obtained. The first poses at all time points are used as training data and input into the fuselage observation model. The fuselage observation model outputs the first pose information of the drilling and anchoring robot obtained in the camera coordinate system.

[0020] Furthermore, establishing a fuselage prediction model based on inertial navigation data includes the following steps:

[0021] The inertial navigation data includes the first angular velocity and first acceleration of the drilling and anchoring robot. The acceleration component of the drilling and anchoring robot caused by gravity is obtained. The target acceleration of the drilling and anchoring robot is obtained by subtracting the acceleration component from the first acceleration. The rotation angle of the drilling and anchoring robot is obtained based on the first angular velocity. The standard rotation angle is obtained by integrating the rotation angle based on quaternions. The second pose of the drilling and anchoring robot is generated based on the target acceleration and the standard rotation angle. The second pose at all time points within the target time period is used as training data and input into the fuselage prediction model. The fuselage prediction model outputs the second attitude information of the drilling and anchoring robot in the inertial navigation coordinate system.

[0022] Furthermore, fusing the fuselage observation model and the fuselage prediction model includes the following steps:

[0023] Based on Equation 2, the first attitude information in the camera coordinate system is transformed to the inertial navigation coordinate system. Equation 2 is as follows:

[0024]

[0025] in, For inertial navigation coordinate system, Let be the rotation matrix from the camera coordinate system to the inertial navigation coordinate system. This is the translation vector from the camera coordinate system to the inertial navigation coordinate system;

[0026] The observation error value of the first attitude information obtained based on the fuselage observation model and the prediction error value of the second attitude information obtained based on the fuselage prediction model are obtained. Different weights are assigned to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value. The first attitude information and the second attitude information are fused using the extended Kalman filter algorithm based on the weights to obtain the target pose.

[0027] Furthermore, establishing a spatial transformation model includes the following steps:

[0028] A body coordinate system is established with the center position of the drilling and anchoring robot as the origin of the coordinate system. A spatial transformation model is established based on the body coordinate system. Based on formulas 3, 4 and 5, the data information under the radar coordinate system, camera coordinate system and inertial navigation coordinate system are transformed to the body coordinate system.

[0029] Formula 3 is as follows:

[0030]

[0031] Formula 4 is as follows:

[0032]

[0033] Formula 5 is as follows:

[0034]

[0035] in, Using the body coordinate system, Let be the rotation matrix from the radar coordinate system to the inertial navigation coordinate system. Let be the translation vector from the inertial navigation coordinate system to the radar coordinate system. This is the translation vector from the inertial navigation coordinate system to the body coordinate system;

[0036] By fusing inertial navigation data, visual images, and point cloud data in a unified body coordinate system, comprehensive data is obtained.

[0037] Furthermore, obtaining comprehensive data includes the following steps:

[0038] Obtain the covariance matrix of the fused data, perform eigenvalue decomposition on the covariance matrix, set a second threshold, and determine whether the eigenvalue corresponding to each fused data is less than the second threshold. If it is, the data corresponding to the eigenvalue is determined to be abnormal and defined as abnormal data. The abnormal data is removed and the data after removal is defined as the composite data.

[0039] Furthermore, obtaining the motion trajectory of the drilling and anchoring robot in the body coordinate system includes the following steps:

[0040] Based on the displacement vectors of feature points in the target pose within the target time period, the motion trajectory of each feature point is generated. Based on the motion trajectories of all feature points, the motion trajectory of the drilling and anchoring robot in the underground roadway is generated, and the motion trajectory is smoothed.

[0041] Furthermore, smoothing the motion trajectory includes the following steps:

[0042] The motion trajectory is fitted using a curve fitting algorithm to generate a fitted curve. The fitted curve is then optimized, and the optimized fitted curve is used as the smoothed motion trajectory.

[0043] Compared with the prior art, the beneficial effects of the present invention are at least as follows:

[0044] This invention constructs a tunnel model that intuitively reflects the shape of the underground tunnel, providing an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot. By fusing the observation model and the prediction model, a target model is obtained, improving the accuracy and stability of the robot's pose estimation and reducing the accumulation of errors from a single sensor. By transforming the radar coordinate system, camera coordinate system, and inertial navigation coordinate system into the body coordinate system, unified management and processing of multi-sensor data is achieved, fully utilizing the advantages of different sensors. By obtaining the robot's motion trajectory in the body coordinate system, the movement path of the drilling and anchoring robot in the underground tunnel can be intuitively displayed, which helps to improve the robot's operating efficiency and safety, and effectively improves the positioning accuracy of the drilling and anchoring robot. Attached Figure Description

[0045] Figure 1 This is a flowchart illustrating the steps of a multi-sensor fusion positioning method for a drilling and anchoring robot according to the present invention.

[0046] Figure 2 This is a flowchart illustrating the steps involved in establishing the fuselage observation model according to the present invention.

[0047] Figure 3 This is a flowchart illustrating the steps involved in establishing a fuselage prediction model according to the present invention.

[0048] Figure 4 This is a flowchart illustrating the steps involved in obtaining comprehensive data according to the present invention. Detailed Implementation

[0049] This application provides a multi-sensor fusion-based drilling and anchoring robot positioning method. The terms "first," "second," "third," "fourth," etc. (if present) in the specification, claims, and accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments described herein can be implemented in a sequence other than that illustrated or described herein. Furthermore, the terms "comprising" or "having" and any variations thereof are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or device that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or devices.

[0050] like Figure 1 As shown, a multi-sensor fusion method for drilling and anchoring robot localization includes:

[0051] Step S1: Acquire point cloud data of the underground roadway within the target time period in the radar coordinate system, acquire visual images of the underground roadway within the target time period in the camera coordinate system, extract feature points from the visual images, fuse feature points at the same time point within the target time period with the point cloud data to obtain target point cloud data, and construct a roadway model based on the target point cloud data.

[0052] Specifically, the target time period is first set as the daily working time of the drilling and anchoring robot. Point cloud data of the underground tunnel in the radar coordinate system is acquired using millimeter-wave radar on the robot, while visual images of the underground tunnel are acquired using a visual camera in the camera coordinate system. Then, unique and stable feature points are extracted from the visual images. These feature points at the same time point within the target time period are fused with the point cloud data to obtain target point cloud data. This target point cloud data not only contains the three-dimensional spatial structure information of the underground tunnel but also incorporates visual texture features, thus compensating for the deficiencies of millimeter-wave radar point clouds (sparseness, lack of texture) and visual images (inability to directly obtain depth information). Finally, based on this target point cloud data, an accurate underground tunnel model is constructed. This model can intuitively reflect the shape, size, and obstacle distribution of the underground tunnel, providing an accurate environmental basis for the drilling and anchoring robot's positioning and navigation.

[0053] Step S2: Establish a fuselage observation model based on feature points, acquire inertial navigation data in the underground roadway within the target time period in the inertial navigation coordinate system, establish a fuselage prediction model based on the inertial navigation data, fuse the fuselage observation model and the fuselage prediction model to obtain the target model, and output the target pose of the drilling and anchoring robot at each time point within the target time period.

[0054] Specifically, on the one hand, a fuselage observation model is established based on the extracted visual image feature points; on the other hand, inertial navigation data of the drilling and anchoring robot is obtained using an inertial navigation system. This inertial navigation data includes the robot's first angular velocity and first acceleration. The robot's position and attitude are calculated based on this data, thus establishing a fuselage prediction model. A Kalman filter algorithm is used to fuse the fuselage observation model and the fuselage prediction model. Through continuous iterative updates, a target model is obtained, and the target pose of the drilling and anchoring robot at each time point within the target time period is output. This improves the accuracy and stability of the robot's pose estimation and reduces the accumulation of errors from a single sensor.

[0055] Step S3: Establish a spatial transformation model, and based on the spatial transformation model, transform the radar coordinate system, camera coordinate system, and inertial navigation coordinate system to the body coordinate system;

[0056] Specifically, the installation positions and attitude parameters of the radar coordinate system, camera coordinate system, and inertial navigation coordinate system relative to the body coordinate system are determined, including translation vectors and rotation matrices, in order to establish a spatial transformation model. This model is used to realize coordinate transformation between different coordinate systems, which facilitates subsequent comprehensive processing and analysis of different data.

[0057] Step S4: Transform the target pose and tunnel model into the body coordinate system. Based on the target pose of the drilling and anchoring robot at each time point within the target time period, obtain the motion trajectory of the drilling and anchoring robot in the body coordinate system.

[0058] Specifically, the target posture and the constructed tunnel model are transformed into the body coordinate system. Based on the displacement vector of the target posture of the drilling and anchoring robot at each time point within the target time period, the motion trajectory of the drilling and anchoring robot in the body coordinate system is obtained. This motion trajectory can intuitively show the movement path of the drilling and anchoring robot in the underground tunnel, which helps to improve the operation efficiency and safety of the drilling and anchoring robot and effectively improves the positioning accuracy of the drilling and anchoring robot.

[0059] As a preferred embodiment of the present invention, the fusion of feature points and point cloud data at the same time point within a target time period includes the following steps:

[0060] Based on Formula 1, the point cloud data in the radar coordinate system is transformed to the camera coordinate system. Formula 1 is:

[0061]

[0062] in, For the camera coordinate system, Let be the rotation matrix from the radar coordinate system to the camera coordinate system. For radar coordinate system, This is the translation vector from the radar coordinate system to the camera coordinate system;

[0063] The ICP algorithm is used to register feature points and transformed point cloud data to obtain fused target point cloud data.

[0064] Specifically, the first step is to accurately obtain the rotation matrix and translation vector from the radar coordinate system to the camera coordinate system, as well as the radar coordinate system obtained within the underground tunnel during the target time period. Then, using Formula 1, the point cloud data at each time point is transformed from the radar coordinate system to the camera coordinate system, resulting in point cloud data in the camera coordinate system. After the transformation, the Iterative Closest Point (ICP) algorithm is used to register the transformed point cloud data in the camera coordinate system with the feature points extracted from the visual image. The resulting point cloud data is the fused target point cloud data. This target point cloud data contains both the high-precision distance information measured by millimeter-wave radar and the rich texture and structural information contained in the feature points of the camera visual image.

[0065] The registration process between feature points and transformed point cloud data based on the ICP algorithm includes the following steps:

[0066] Select a subset of feature points as an initial matching point set, determine the initial corresponding point set of the feature points in the transformed point cloud data, calculate the optimal rotation matrix, optimal translation vector, and registration error based on the initial matching point set and the initial corresponding point set, set a first threshold, and determine whether the registration error is less than the first threshold. If yes, output the registered target point cloud data; otherwise, re-match the initial matching point set and the initial corresponding point set based on the optimal rotation matrix and optimal translation vector, iteratively calculate the registration error, until the registration error is less than the first threshold.

[0067] Specifically, from the extracted visual image feature points, a subset of feature points is selected as the initial matching point set. Then, in the transformed point cloud data, the nearest points with similar geometric features are found as the initial corresponding point set. Using the data from the initial matching point set and the initial corresponding point set, the least squares method is used to calculate the optimal rotation matrix and optimal translation vector that achieve the best matching state between the two sets of point sets. Simultaneously, the registration error in the current registration state is calculated. Based on the actual application scenario and the requirements for registration accuracy, a first threshold is set. When the registration error is determined to be less than the first threshold, the point cloud data transformed by the optimal rotation matrix and optimal translation vector is output as the registered target point cloud data. If the registration error is greater than the first threshold, the initial matching point set and the initial corresponding point set are re-transformed using the optimal rotation matrix and optimal translation vector to obtain a new correspondence. Then, based on the new corresponding point set, the optimal rotation matrix, optimal translation vector, and registration error are recalculated. This process is repeated until the registration error is less than the first threshold. Through multiple iterations, the optimal registration result is found, thereby ensuring that the final output target point cloud data meets the high-precision requirements.

[0068] Establishing a fuselage observation model based on feature points includes the following steps:

[0069] The displacement vectors of each feature point at adjacent time points within the target time period are obtained. Based on the displacement vectors, the first pose change of the drilling and anchoring robot within the target time period is obtained. The first poses at all time points are used as training data and input into the fuselage observation model. The fuselage observation model outputs the first pose information of the drilling and anchoring robot in the camera coordinate system.

[0070] Specifically, such as Figure 2 The diagram shows the flowchart for establishing the fuselage observation model. For each feature point extracted from the visual image, images at adjacent time points need to be tracked and matched within the target time period to calculate the displacement vector between adjacent time points. Then, the least squares method is used to statistically analyze the displacement vectors of all feature points. The pose change of the drilling robot can be estimated by calculating the average of these displacement vectors, thus obtaining the first pose change of the drilling robot within the target time period. The first pose changes calculated at all time points are used as training data and input into the fuselage observation model, enabling the model to accurately learn and fit the input pose change data. After training, when new feature point displacement vector data is input, the fuselage observation model can output the first pose information of the drilling robot in the camera coordinate system, including the position coordinates and pose angles of the drilling robot. This method of constructing the fuselage observation model based on multi-feature point comprehensive inference of pose changes can effectively reduce the impact of measurement noise and environmental interference on the positioning results.

[0071] Establishing a fuselage prediction model based on inertial navigation data includes the following steps:

[0072] The inertial navigation data includes the first angular velocity and first acceleration of the drilling and anchoring robot. The acceleration component of the drilling and anchoring robot caused by gravity is obtained. The target acceleration of the drilling and anchoring robot is obtained by subtracting the acceleration component from the first acceleration. The rotation angle of the drilling and anchoring robot is obtained based on the first angular velocity. The standard rotation angle is obtained by integrating the rotation angle based on quaternions. The second pose of the drilling and anchoring robot is generated based on the target acceleration and the standard rotation angle. The second pose at all time points within the target time period is used as training data and input into the fuselage prediction model. The fuselage prediction model outputs the second attitude information of the drilling and anchoring robot in the inertial navigation coordinate system.

[0073] Specifically, such as Figure 3 The diagram shows the steps for establishing a predictive model of the robot's fuselage. An inertial navigation system typically consists of accelerometers and gyroscopes, capable of measuring the first acceleration and first angular velocity of the drilling robot in real time. In the underground tunnel environment, the acceleration measured by the accelerometer includes the first acceleration generated by the drilling robot's own motion and the gravitational acceleration component. To accurately obtain the first acceleration of the drilling robot's own motion, the gravitational acceleration component needs to be separated. Subtracting this component from the first acceleration yields the target acceleration of the drilling robot, avoiding the influence of gravity on the acceleration measurement and thus improving accuracy when calculating pose changes.

[0074] The angular velocity data measured by the gyroscope can be used to calculate the rotation angle of the drilling robot, obtaining the rotation angle of the drilling robot relative to its initial posture over a period of time. The calculated rotation angle is converted into quaternion form and integrated. By iteratively updating the quaternion, the standard rotation angle at the corresponding time point is finally obtained. The second pose of the drilling robot is generated by combining the target acceleration and the standard rotation angle. For example, the displacement is obtained by integrating the acceleration twice, and then the posture is determined by combining it with the rotation matrix, thus obtaining the complete pose information of the drilling robot at that moment, which is the second pose. The second pose at all time points within the target time period is used as training data and input into the fuselage prediction model, enabling the model to accurately learn and predict the input pose data. After training, when new inertial navigation data is input, the same calculation process is used to obtain new second pose information. The fuselage prediction model built based on inertial navigation data can predict and estimate the pose of the drilling robot. In some cases, such as when the visual camera is obstructed or the radar signal is interfered with, the fuselage prediction model can still work normally, providing reliable pose information for the robot.

[0075] The fusion of the airframe observation model and the airframe prediction model includes the following steps:

[0076] Based on Equation 2, the first attitude information in the camera coordinate system is transformed to the inertial navigation coordinate system. Equation 2 is as follows:

[0077]

[0078] in, For inertial navigation coordinate system, Let be the rotation matrix from the camera coordinate system to the inertial navigation coordinate system. This is the translation vector from the camera coordinate system to the inertial navigation coordinate system;

[0079] The observation error value of the first attitude information obtained based on the fuselage observation model and the prediction error value of the second attitude information obtained based on the fuselage prediction model are obtained. Different weights are assigned to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value. The first attitude information and the second attitude information are fused using the extended Kalman filter algorithm based on the weights to obtain the target pose.

[0080] Specifically, firstly, the rotation matrix and translation vector from the camera coordinate system to the inertial navigation coordinate system (INS) need to be accurately obtained, along with the camera coordinate system acquired within the underground roadway during the target time period. Then, using Formula 2, the visual images at each time point are transformed from the camera coordinate system to the INS, resulting in visual images in the INS. Next, the observation error value based on the first attitude information obtained from the fuselage observation model and the prediction error value based on the second attitude information obtained from the fuselage prediction model are obtained through experimental testing. Then, based on the observation and prediction error values, a weighted least squares method is used to assign different weights to the fuselage observation and prediction models; the smaller the error value, the higher the corresponding model weight. Finally, at each time point, the first and second attitude information transformed to the INS are input into the Extended Kalman Filter (EKF) algorithm. Based on the assigned weights, the first and second attitude information are weighted and fused to calculate the target pose of the drilling robot at that time point. This fusion method based on the Extended Kalman Filter algorithm effectively handles the uncertainty and nonlinearity of multi-source data, improving the system's ability to track and predict the pose of the drilling robot.

[0081] Establishing a spatial transformation model includes the following steps:

[0082] A body coordinate system is established with the center position of the drilling and anchoring robot as the origin of the coordinate system. A spatial transformation model is established based on the body coordinate system. Based on formulas 3, 4 and 5, the data information under the radar coordinate system, camera coordinate system and inertial navigation coordinate system are transformed to the body coordinate system.

[0083] Formula 3 is as follows:

[0084]

[0085] Formula 4 is as follows:

[0086]

[0087] Formula 5 is as follows:

[0088]

[0089] in, Using the body coordinate system, Let be the rotation matrix from the radar coordinate system to the inertial navigation coordinate system. Let be the translation vector from the inertial navigation coordinate system to the radar coordinate system. This is the translation vector from the inertial navigation coordinate system to the body coordinate system;

[0090] By fusing inertial navigation data, visual images, and point cloud data in a unified body coordinate system, comprehensive data is obtained.

[0091] Specifically, such as Figure 4 As shown in the flowchart, to obtain the comprehensive data, the geometric center of the drilling and anchoring robot is selected as the center position. Using this point as the origin, three mutually perpendicular coordinate axes are determined to construct a body coordinate system. This established body coordinate system closely relates to the drilling and anchoring robot's own motion and operation, providing a unified benchmark for subsequent spatial transformation and data fusion. Then, according to formulas 3, 4, and 5, each point cloud data, visual image, and inertial navigation data is transformed into the body coordinate system. Next, a fusion algorithm based on feature matching and data association is used, assigning different weights to different sensor data according to their reliability and accuracy. For example, when the tunnel environment is relatively clear and the visual image quality is high, the weight of the visual image data is appropriately increased; when the robot's movement is more intense and the inertial navigation data is relatively more stable, the weight of the inertial navigation data is increased. Through weighted fusion, the three types of data are fused into comprehensive data, which contains more comprehensive and accurate information about the underground tunnel environment and the drilling and anchoring robot's pose. By establishing a unified body coordinate system and converting data from radar, camera, and inertial navigation coordinate systems to the body coordinate system, unified management and processing of multi-sensor data is achieved. This fully leverages the advantages of different sensors: point cloud data provides accurate spatial distance information, visual images supplement rich texture and structural details, and inertial navigation data provides continuous attitude and position change information during robot movement. By fusing these data, they can complement and correct each other, reducing the errors and limitations of single-sensor data.

[0092] Obtaining comprehensive data involves the following steps:

[0093] Obtain the covariance matrix of the fused data, perform eigenvalue decomposition on the covariance matrix, set a second threshold, and determine whether the eigenvalue corresponding to each fused data is less than the second threshold. If it is, the data corresponding to the eigenvalue is determined to be abnormal and defined as abnormal data. The abnormal data is removed and the data after removal is defined as the composite data.

[0094] Specifically, after fusing inertial navigation data, visual images, and point cloud data in the body coordinate system, statistical analysis is performed on the fused data to obtain the covariance matrix. Eigenvalue decomposition is then performed on the covariance matrix. Based on a large amount of experimental data and actual working conditions, a suitable second threshold is set. For each fused data sample, the corresponding eigenvalue is compared with the second threshold. If a certain eigenvalue is less than the second threshold, the entire data sample corresponding to that eigenvalue is determined to be abnormal data. Once abnormal data is identified, it is removed from the dataset. By traversing the data samples, marking and deleting those data that are determined to be abnormal, the remaining data obtained is the comprehensive data. After removing abnormal data, this comprehensive data has higher reliability and consistency, and can more accurately reflect the real state of the drilling and anchoring robot in the underground roadway environment.

[0095] Obtaining the motion trajectory of the drilling and anchoring robot in the body coordinate system includes the following steps:

[0096] Based on the displacement vectors of feature points in the target pose within the target time period, the motion trajectory of each feature point is generated. Based on the motion trajectories of all feature points, the motion trajectory of the drilling and anchoring robot in the underground roadway is generated, and the motion trajectory is smoothed.

[0097] Specifically, the displacement vectors of feature points within the target time period are first extracted from the target pose information. For each feature point, the displacement vector within that time period is obtained by calculating the difference in position coordinates between adjacent sampling times. The position coordinates at each sampling time are then calculated sequentially to obtain the motion trajectory of a single feature point within the target time period. After obtaining the motion trajectory of each feature point, the overall motion trajectory of the drilling and anchoring robot in the underground roadway is generated based on the motion trajectories of all feature points. Since multiple feature points on the drilling and anchoring robot collectively reflect the robot's motion state, the overall motion of the robot is determined through a comprehensive analysis of the motion trajectories of these feature points. For example, the drilling and anchoring robot can be considered as a rigid body. Assuming there are multiple feature points on the robot, the centroid coordinates of all feature points are calculated using the centroid calculation formula. These centroid coordinates are then connected sequentially to form the overall motion trajectory of the drilling and anchoring robot in the underground roadway. The generated motion trajectory is then smoothed. Generating the overall motion trajectory of the drilling and anchoring robot based on the motion trajectories of all feature points improves the accuracy and positioning precision of the motion trajectory, and can meticulously reflect the motion details of the drilling and anchoring robot in the underground roadway.

[0098] Smoothing motion trajectories includes the following steps:

[0099] A curve fitting algorithm is used to fit the motion trajectory, generating a fitted curve. The fitted curve is then optimized, and the optimized fitted curve is used as the smoothed motion trajectory.

[0100] Specifically, after acquiring the motion trajectory of the drilling and anchoring robot, a curve fitting algorithm is used to fit the trajectory. Further, a cubic spline interpolation algorithm is applied to fit the trajectory, generating a smooth fitting curve. Noise points and abrupt changes in this fitted curve are detected and removed, resulting in an optimized fitting curve, which serves as the smoothed motion trajectory. Smoothing through the curve fitting algorithm effectively removes fluctuations and discontinuities in the motion trajectory, further enhancing its accuracy and reliability.

[0101] Compared with the prior art, the beneficial effects of the present invention are at least as follows:

[0102] This invention constructs a tunnel model that intuitively reflects the shape of the underground tunnel, providing an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot. By fusing the observation model and the prediction model, a target model is obtained, improving the accuracy and stability of the robot's pose estimation and reducing the accumulation of errors from a single sensor. By transforming the radar coordinate system, camera coordinate system, and inertial navigation coordinate system into the body coordinate system, unified management and processing of multi-sensor data is achieved, fully utilizing the advantages of different sensors. By obtaining the robot's motion trajectory in the body coordinate system, the movement path of the drilling and anchoring robot in the underground tunnel can be intuitively displayed, which helps to improve the robot's operating efficiency and safety, and effectively improves the positioning accuracy of the drilling and anchoring robot.

[0103] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A multi-sensor fusion method for positioning a drilling and anchoring robot, characterized in that, The method includes the following steps: S1: Acquire point cloud data of the underground roadway within the target time period in the radar coordinate system, acquire visual images of the underground roadway within the target time period in the camera coordinate system, extract feature points from the visual images, fuse feature points at the same time point within the target time period with the point cloud data to obtain target point cloud data, construct a roadway model based on the target point cloud data, and provide an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot. S2: Establish a fuselage observation model based on feature points, acquire inertial navigation data in the underground roadway within the target time period in the inertial navigation coordinate system, establish a fuselage prediction model based on the inertial navigation data, fuse the fuselage observation model and fuselage prediction model to obtain the target model, and output the target pose of the drilling and anchoring robot at each time point within the target time period. S3: Establish a spatial transformation model, and based on the spatial transformation model, transform the radar coordinate system, camera coordinate system, and inertial navigation coordinate system to the body coordinate system; S4: Transform the target posture and tunnel model into the body coordinate system, and obtain the motion trajectory of the drilling and anchoring robot in the body coordinate system based on the target posture of the drilling and anchoring robot at each time point within the target time period. The steps for establishing a fuselage observation model based on feature points are as follows: obtain the displacement vector of each feature point at adjacent time points within the target time period, obtain the first pose change of the drilling and anchoring robot within the target time period based on the displacement vector, input the first pose at all time points as training data into the fuselage observation model, and output the first pose information of the drilling and anchoring robot obtained in the camera coordinate system. The steps for establishing a fuselage prediction model based on inertial navigation data are as follows: The inertial navigation data includes the first angular velocity and the first acceleration of the drilling and anchoring robot. The acceleration component of the drilling and anchoring robot caused by gravity is obtained by using an accelerometer. The target acceleration of the drilling and anchoring robot is obtained by subtracting the acceleration component from the first acceleration. The rotation angle of the drilling and anchoring robot is obtained based on the first angular velocity. The standard rotation angle is obtained by integrating the rotation angle based on quaternions. The second pose of the drilling and anchoring robot is generated based on the target acceleration and the standard rotation angle. The second pose at all time points within the target time period is used as training data and input into the fuselage prediction model. The fuselage prediction model outputs the second attitude information of the drilling and anchoring robot in the inertial navigation coordinate system. The fusion of the fuselage observation model and the fuselage prediction model includes the following steps: obtaining the observation error value of the first attitude information obtained based on the fuselage observation model and the prediction error value of the second attitude information obtained based on the fuselage prediction model; assigning different weights to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value; and fusing the first attitude information and the second attitude information using the extended Kalman filter algorithm based on the weights to obtain the target pose.

2. The method according to claim 1, characterized in that, The fusion of feature points and point cloud data at the same time point within the target time period includes the following steps: Transforming the point cloud data in the radar coordinate system to the camera coordinate system based on Formula 1, where Formula 1 is: ,in, For the camera coordinate system, Let be the rotation matrix from the radar coordinate system to the camera coordinate system. For radar coordinate system, The translation vector is from the radar coordinate system to the camera coordinate system; the feature points and the transformed point cloud data are registered based on the ICP algorithm to obtain the fused target point cloud data.

3. The method according to claim 1, characterized in that, The registration process between feature points and transformed point cloud data based on the ICP algorithm includes the following steps: Select a subset of feature points as the initial matching point set, determine the initial corresponding point set of the feature points in the transformed point cloud data. Based on the initial matching point set and the initial corresponding point set, calculate the optimal rotation matrix, the optimal translation vector, and the registration error. Set a first threshold and determine whether the registration error is less than the first threshold. If yes, output the registered target point cloud data. Otherwise, based on the optimal rotation matrix and the optimal translation vector, rematch the initial matching point set and the initial corresponding point set, iteratively calculate the registration error, until the registration error is less than the first threshold.

4. The method according to claim 1, characterized in that, The fusion of the fuselage observation model and the fuselage prediction model includes the following steps: Based on Equation 2, the first attitude information in the camera coordinate system is transformed to the inertial navigation coordinate system. Equation 2 is as follows: ； in, For inertial navigation coordinate system, Let be the rotation matrix from the camera coordinate system to the inertial navigation coordinate system. This is the translation vector from the camera coordinate system to the inertial navigation coordinate system.

5. The method according to claim 1, characterized in that, Establishing a spatial transformation model includes the following steps: A body coordinate system is established with the center position of the drilling and anchoring robot as the origin of the coordinate system. A spatial transformation model is established based on the body coordinate system. Based on formulas 3, 4 and 5, the data information under the radar coordinate system, camera coordinate system and inertial navigation coordinate system are transformed to the body coordinate system. Formula 3 is as follows: ； Formula 4 is as follows: ； Formula 5 is as follows: ； in, Using the body coordinate system, Let be the rotation matrix from the radar coordinate system to the inertial navigation coordinate system. Let be the translation vector from the inertial navigation coordinate system to the radar coordinate system. This is the translation vector from the inertial navigation coordinate system to the body coordinate system; By fusing inertial navigation data, visual images, and point cloud data in a unified body coordinate system, comprehensive data is obtained.

6. The method according to claim 5, characterized in that, Obtaining comprehensive data involves the following steps: Obtain the covariance matrix of the fused data, perform eigenvalue decomposition on the covariance matrix, set a second threshold, and determine whether the eigenvalue corresponding to each fused data is less than the second threshold. If it is, the data corresponding to the eigenvalue is determined to be abnormal and defined as abnormal data. The abnormal data is removed and the data after removal is defined as the composite data.

7. The method according to claim 1, characterized in that, Obtaining the motion trajectory of the drilling and anchoring robot in the body coordinate system includes the following steps: Based on the displacement vectors of feature points in the target pose within the target time period, the motion trajectory of each feature point is generated. Based on the motion trajectories of all feature points, the motion trajectory of the drilling and anchoring robot in the underground roadway is generated, and the motion trajectory is smoothed.

8. The method according to claim 7, characterized in that, Smoothing motion trajectories includes the following steps: A curve fitting algorithm is used to fit the motion trajectory, generating a fitted curve. The fitted curve is then optimized, and the optimized fitted curve is used as the smoothed motion trajectory.

Images