Multi-sensor fusion drilling and anchoring robot positioning method

Abstract

The invention relates to the technical field of robot positioning, in particular to a multi-sensor fusion drilling and anchoring robot positioning method, which comprises the following steps of: acquiring point cloud data in an underground roadway in a target time period under a radar coordinate system, acquiring a visual image in the underground roadway in the target time period under a camera coordinate system, and extracting feature points of the visual image; fusing the feature points at the same time point in the target time period with the point cloud data to obtain target point cloud data, and 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 in an underground roadway in a target time period under an inertial navigation coordinate system, establishing a fuselage prediction model based on the inertial navigation data, and fusing the fuselage observation model and the fuselage prediction model to obtain a target model. The target model outputs corresponding target poses of the drilling and anchoring robot at all time points in the target time period; according to the method, the positioning precision of the drilling and anchoring robot in the underground roadway is improved.

Description

Technical Field

[0001] This application relates to the technical field of robot positioning, and particularly to a positioning method for a drilling and anchoring robot with multi-sensor fusion. Background Technique

[0002] With the rapid development of the intelligent construction of coal mines in China, in order to improve the automation level of coal mine drilling and anchoring operations and ensure the safety of workers, coal mine drilling and anchoring robots have emerged as the times require. Due to the complex geological conditions and harsh underground environment of coal mines, and the dynamic changes in the spatial environment during the mining process, how to accurately control 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 positioning methods for drilling and anchoring robots, such as the Chinese patent application with the publication number CN112068543A, propose a precise positioning method for drilling holes of coal mine drilling and anchoring robots based on vision calibration. An electro-optical encoder and an angular displacement sensor are used to form a semi-closed-loop control system for the translation distance and rotation angle of the drill on the drilling and anchoring platform; an inclination sensor is used to collect the ground inclination information where the drilling and anchoring robot is located to realize the pose adjustment of the drill platform to compensate for the error caused by the ground inclination; a vision-based method is used to realize the intelligent alignment of the drill end with the anchor mesh hole to complete the precise drilling target and achieve the precise positioning of the drill. Also, for example, the Chinese patent application with the publication number CN114658486A proposes a method and system for preventing collision between a drilling and anchoring robot and a tunneling machine locomotive. Multiple ultrasonic sensors on the left and right sides of the outer wall of the drilling and anchoring robot are used to detect the distance between each position of the robot body and the coal wall in real time, and multiple ultrasonic sensors on the inner wall of the drilling and anchoring robot are used to detect the distance relationship between the drilling and anchoring robot and the tunneling machine in real time to realize the anti-collision warning during the process of the drilling and anchoring robot and the tunneling machine locomotive. At the same time, according to the different widths of the tunneling machine and other objects, invalid alarm information caused by staff and other factors is eliminated.

[0004] However, in the above-mentioned existing technologies, there are still certain defects in using multiple sensors to position drilling and anchoring robots. Since coal mine operations are often accompanied by complex working environments such as dust and water mist, it greatly affects the acquisition of environmental characteristics by sensors. In addition, errors will accumulate during the long-term use of sensors, and the functions of single sensors have limitations, which further reduces the positioning accuracy of drilling and anchoring robots. Therefore, a positioning method for drilling and anchoring robots with multi-sensor fusion is needed to solve the inherent limitations of single sensors in function and improve the positioning accuracy of drilling and anchoring robots at the same time. Summary of the Invention

[0005] This application provides a positioning method for a drilling and anchoring robot with multi-sensor fusion to solve the problems raised in the above background technique.

[0006] To achieve the above invention objectives, the present invention proposes a positioning method for a drilling and anchoring robot with multi-sensor fusion, including:

[0007] S1: Obtain point cloud data in the underground roadway within the target time period in the radar coordinate system, obtain visual images in the underground roadway within the target time period in the camera coordinate system, extract feature points of the visual images, fuse the feature points and the point cloud data at the same time point within the target time period 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 the feature points, obtain 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 a target model, and the target model outputs the target poses corresponding to each time point of the drilling and anchoring robot within the target time period;

[0009] S3: Establish a space conversion model, and convert the radar coordinate system, the camera coordinate system, and the inertial navigation coordinate system to the body coordinate system based on the space conversion model;

[0010] S4: Convert the target pose and the roadway model to the body coordinate system, and obtain the motion trajectory of the drilling and anchoring robot in the body coordinate system based on the target poses of the drilling and anchoring robot at each time point within the target time period.

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

[0012] Convert the point cloud data in the radar coordinate system to the camera coordinate system based on Formula 1. Formula 1 is:

[0013]

[0014] where, is the camera coordinate system, is the rotation matrix from the radar coordinate system to the camera coordinate system, is the radar coordinate system, is the translation vector from the radar coordinate system to the camera coordinate system;

[0015] Perform registration processing on the feature points and the converted point cloud data based on the ICP algorithm to obtain the fused target point cloud data.

[0016] Further, the registration processing of the feature points and the converted point cloud data based on the ICP algorithm includes the following steps:

[0017] Select some feature points as the 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 so, output the registered target point cloud data. If not, rematch the initial matching point set and the initial corresponding point set based on the optimal rotation matrix and optimal translation vector, and iteratively calculate the registration error until the registration error is less than the first threshold.

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

[0019] Obtain the displacement vectors 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 vectors, use the first poses at all time points as training data to input into the fuselage observation model, and the fuselage observation model outputs the first attitude information obtained by the drilling and anchoring robot in the camera coordinate system.

[0020] Further, 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 the first acceleration of the drilling and anchoring robot. Obtain the acceleration component caused by gravity of the drilling and anchoring robot, subtract the acceleration component from the first acceleration to obtain the target acceleration of the drilling and anchoring robot, obtain the rotation angle of the drilling and anchoring robot based on the first angular velocity, perform integral processing on the rotation angle based on quaternions to obtain the standard rotation angle, generate the second pose of the drilling and anchoring robot based on the target acceleration and the standard rotation angle, use the second poses at all time points within the target time period as training data to input into the fuselage prediction model, and the fuselage prediction model outputs the second attitude information of the drilling and anchoring robot in the inertial navigation coordinate system.

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

[0023] Convert the first attitude information in the camera coordinate system to the inertial navigation coordinate system based on Formula 2. Formula 2 is as follows:

[0024]

[0025] Where, is the inertial navigation coordinate system, is the rotation matrix from the camera coordinate system to the inertial navigation coordinate system, is the translation vector from the camera coordinate system to the inertial navigation coordinate system;

[0026] Obtain 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. Allocate different weights to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value. Use the extended Kalman filter algorithm to fuse the first attitude information and the second attitude information based on the weights to obtain the target pose.

[0027] Further, establishing the space transformation model includes the following steps:

[0028] Taking the central position of the drilling and anchoring robot as the coordinate origin, establish a body coordinate system. Based on the body coordinate system, establish a space transformation model, and transform the data information in the radar coordinate system, camera coordinate system, and inertial navigation coordinate system to the body coordinate system based on Formula 3, Formula 4, and Formula 5;

[0029] Formula 3 is as follows:

[0030]

[0031] Formula 4 is as follows:

[0032]

[0033] Formula 5 is as follows:

[0034]

[0035] Among them, is the body coordinate system, is the rotation matrix from the radar coordinate system to the inertial navigation coordinate system, is the translation vector from the inertial navigation coordinate system to the radar coordinate system, is the translation vector from the inertial navigation coordinate system to the body coordinate system;

[0036] Under the condition of unifying the body coordinate system, fuse the inertial navigation data, visual images, and point cloud data to obtain comprehensive data.

[0037] Further, obtaining the 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 so, then determine that the data corresponding to the eigenvalue is abnormal, define it as abnormal data, eliminate the abnormal data, and define the data after elimination as comprehensive data.

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

[0040] Generate the motion trajectory of each feature point based on the displacement vector of the feature points in the target pose within the target time period, generate the motion trajectory of the drilling and anchoring robot in the underground roadway based on the motion trajectories at all feature points, and smooth the motion trajectory.

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

[0042] Use the curve fitting algorithm to fit the motion trajectory, generate a fitting curve, optimize the fitting curve, and use the optimized fitting curve as the smoothed motion trajectory.

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

[0044] By constructing a roadway model, the present invention can intuitively reflect the shape of the underground roadway, providing an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot; by fusing the fuselage observation model and the fuselage prediction model to obtain a target model, the accuracy and stability of the pose estimation of the drilling and anchoring robot are improved, and the error accumulation caused by a single sensor is reduced; by converting the radar coordinate system, the camera coordinate system, and the inertial navigation coordinate system into the body coordinate system, the unified management and processing of multi-sensor data are realized, and the advantages of different sensors are fully utilized; by obtaining the motion trajectory of the drilling and anchoring robot in the body coordinate system, the moving path of the drilling and anchoring robot in the underground roadway can be intuitively displayed, which helps to improve the operation efficiency and safety of the drilling and anchoring robot and effectively improve the positioning accuracy of the drilling and anchoring robot. Description of the Drawings

[0045] Figure 1 It is the step flow chart of a positioning method for a multi-sensor fusion drilling and anchoring robot of the present invention;

[0046] Figure 2 It is the step flow chart of establishing the fuselage observation model of the present invention;

[0047] Figure 3 It is the step flow chart of establishing the fuselage prediction model of the present invention;

[0048] Figure 4 It is the step flow chart of obtaining comprehensive data of the present invention. Detailed Embodiments

[0049] The embodiment of the present application provides a positioning method for a drilling and anchoring robot with multi-sensor fusion. Terms such as "first", "second", "third", "fourth", etc. (if any) in the specification, claims and the above-mentioned drawings of the present application are used to distinguish similar objects, and do not necessarily need to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances, so that the embodiments described here can be implemented in an order other than that illustrated or described here. In addition, the term "including" or "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device including a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

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

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

[0052] Specifically, first set the target time period as the daily working time period of the drilling and anchoring robot, obtain the point cloud data in the underground roadway in the radar coordinate system through the millimeter-wave radar on the drilling and anchoring robot, and collect the visual image in the underground roadway by the visual camera in the camera coordinate system. Then extract the feature points with uniqueness and stability on the visual image, fuse the feature points at the same time point within the target time period with the point cloud data to obtain the target point cloud data. The target point cloud data not only contains the three-dimensional space structure information of the underground roadway, but also incorporates the visual texture features, thus making up for the defects of sparse point cloud and lack of texture of the millimeter-wave radar, as well as the inability of the visual image to directly obtain depth information. Finally, based on these target point cloud data, construct an accurate underground roadway model, which can intuitively reflect information such as the shape, size and obstacle distribution of the underground roadway, providing an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot.

[0053] Step S2: Establish a fuselage observation model based on the feature points, obtain the 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 the target model outputs the corresponding target pose of the drilling and anchoring robot at each time point within the target time period;

[0054] Specifically, on the one hand, according to the extracted visual image feature points, a fuselage observation model is established; on the other hand, inertial navigation data of the drilling and anchoring robot is obtained by using an inertial navigation system. The inertial navigation data includes the first angular velocity and the first acceleration of the drilling and anchoring robot, and the position and attitude of the drilling and anchoring robot are calculated based on the inertial navigation data, so as to establish a fuselage prediction model. The Kalman filtering algorithm is used to fuse the fuselage observation model and the fuselage prediction model, and the target model is obtained through continuous iterative update, and the target pose corresponding to each time point within the target time period of the drilling and anchoring robot is output, which improves the accuracy and stability of the pose estimation of the drilling and anchoring robot and reduces the error accumulation caused by a single sensor.

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

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

[0057] Step S4: Transform the target attitude and the roadway model into the body coordinate system, and based on the target attitude 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, transform the target attitude and the constructed roadway model into the body coordinate system, and based on the displacement vector of the target attitude 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. This motion trajectory can intuitively display the movement path of the drilling and anchoring robot in the underground roadway, 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 technical solution of the present invention, the fusion of the feature points and the point cloud data at the same time point within the target time period includes the following steps:

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

[0061]

[0062] Where is the camera coordinate system, is the rotation matrix from the radar coordinate system to the camera coordinate system, is the radar coordinate system, is the translation vector from the radar coordinate system to the camera coordinate system;

[0063] Perform registration processing on the feature points and the transformed point cloud data based on the ICP algorithm to obtain the fused target point cloud data.

[0064] Specifically, first, 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 in the underground roadway during the target time period. Transform the point cloud data at each time point from the radar coordinate system to the camera coordinate system through Formula 1 to obtain the point cloud data in the camera coordinate system. After completing the transformation of the point cloud data to the camera coordinate system, use the Iterative Closest Point (ICP) algorithm to register the transformed point cloud data in the camera coordinate system and the feature points extracted from the visual image. The obtained point cloud data is the fused target point cloud data. This target point cloud data contains both the high-precision distance information measured by the millimeter-wave radar and the rich texture and structural information contained in the feature points of the camera visual image.

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

[0066] Select some 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, optimal translation vector, and registration error. Set a first threshold and judge whether the registration error is less than the first threshold. If so, output the registered target point cloud data. If not, based on the optimal rotation matrix and optimal translation vector, rematch the initial matching point set and the initial corresponding point set, and 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 part of the feature points are selected as the initial matching point set, and then the points with the closest distance and similar geometric features are searched in the transformed point cloud data as the initial corresponding point set. Using the data of the initial matching point set and the initial corresponding point set, the least squares method is used to calculate the optimal rotation matrix and the optimal translation vector that make the two point sets reach the best matching state, and the registration error in the current registration state is calculated at the same time. According to the actual application scenario and the requirement for registration accuracy, a first threshold is set. When it is judged that the registration error is less than the first threshold, the point cloud data transformed by the optimal rotation matrix and the 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 the optimal translation vector to obtain a new correspondence. Then, based on the new corresponding point set, the optimal rotation matrix, the optimal translation vector, and the registration error are recalculated, and this process is repeated continuously until the registration error is less than the first threshold. The optimal registration result is found through multiple iterations, so as to ensure that the finally output target point cloud data meets the requirements of high precision.

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

[0069] 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, and input the first poses at all time points as training data into the fuselage observation model. The fuselage observation model outputs the first attitude information obtained by the drilling and anchoring robot in the camera coordinate system.

[0070] Specifically, as Figure 2 shown, it is the flow chart of the steps for establishing the fuselage observation model. For each feature point extracted from the visual image, it is necessary to track and match the images at adjacent time points within the target time period, so as to calculate the displacement vector of the feature point 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 and anchoring robot can be estimated by calculating the average value of these displacement vectors, so as to obtain the first pose change of the drilling and anchoring robot within the target time period. Input the first pose changes calculated at all time points as training data into the fuselage observation model, so that the model can 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 attitude information obtained by the drilling and anchoring robot in the camera coordinate system, and this information includes the position coordinates and attitude angles of the drilling and anchoring robot, etc. This method of constructing the fuselage observation model based on the comprehensive inference of pose changes by multiple feature points can effectively reduce the influence of measurement noise and environmental interference on the positioning result.

[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 the first acceleration of the drilling and anchoring robot. Obtain the acceleration component caused by gravity of the drilling and anchoring robot, subtract the acceleration component from the first acceleration to obtain the target acceleration of the drilling and anchoring robot, obtain the rotation angle of the drilling and anchoring robot based on the first angular velocity, perform integral processing on the rotation angle based on quaternions to obtain the standard rotation angle, generate the second pose of the drilling and anchoring robot based on the target acceleration and the standard rotation angle, and use the second poses at all time points within the target time period as training data to 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, as Figure 3 shown, it is a flow chart of the steps to establish the fuselage prediction model. The inertial navigation system usually consists of an accelerometer and a gyroscope, which can measure the first acceleration and the first angular velocity of the drilling and anchoring robot in real time. In the underground roadway environment, the acceleration measured by the accelerometer includes the first acceleration generated by the movement of the drilling and anchoring robot itself and the gravity acceleration component. In order to accurately obtain the first acceleration of the movement of the drilling and anchoring robot itself, it is necessary to separate the gravity acceleration component. Subtracting the acceleration component from the first acceleration can obtain the target acceleration of the drilling and anchoring robot, avoiding the influence of gravity interference on acceleration measurement, and thus being more accurate when calculating pose changes.

[0074] The first angular velocity data measured by the gyroscope can be used to calculate the rotation angle of the drilling and anchoring robot, obtaining the rotation angle of the drilling and anchoring robot relative to the initial pose within a period of time. Convert the calculated rotation angle into quaternion form for integral processing. By continuously iterating and updating the quaternion, the standard rotation angle corresponding to the time point is finally obtained. Combine the target acceleration and the standard rotation angle to generate the second pose of the drilling and anchoring robot. For example, obtain the displacement by integrating the acceleration twice, and then combine the rotation matrix to determine the pose, so as to obtain the complete pose information of the drilling and anchoring robot at this moment, which is the second pose. Use the second poses at all time points within the target time period as training data to 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, new second attitude information is obtained through the same calculation process. The fuselage prediction model established based on inertial navigation data can predict and estimate the pose of the drilling and anchoring robot. In some cases, such as when the vision camera is blocked or the radar signal is interfered, the fuselage prediction model can still work normally and provide reliable pose information for the robot.

[0075] Fusing the fuselage observation model and the fuselage prediction model includes the following steps:

[0076] Convert the first attitude information in the camera coordinate system to the inertial navigation coordinate system based on Formula 2, and Formula 2 is as follows:

[0077]

[0078] where, is the inertial navigation coordinate system, is the rotation matrix from the camera coordinate system to the inertial navigation coordinate system, is the translation vector from the camera coordinate system to the inertial navigation coordinate system;

[0079] Obtain 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. Assign different weights to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value, and fuse the first attitude information and the second attitude information using the extended Kalman filter algorithm based on the weights to obtain the target pose.

[0080] Specifically, first, it is necessary to accurately obtain the rotation matrix and translation vector from the camera coordinate system to the inertial navigation coordinate system, as well as the camera coordinate system obtained in the underground roadway during the target time period. Convert the visual images at each time point from the camera coordinate system to the inertial navigation coordinate system through Formula 2 to obtain the visual images in the inertial navigation coordinate system. Then, obtain 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 through experimental tests. Next, according to the observation error value and the prediction error value, use the weighted least squares method to assign different weights to the fuselage observation model and the fuselage prediction model. The smaller the error value, the higher the corresponding model weight. Finally, at each time point, input the first attitude information and the second attitude information converted to the inertial navigation coordinate system into the extended Kalman filter (EKF) algorithm, and perform weighted fusion on the first attitude information and the second attitude information according to the assigned weights to calculate the target pose of the drill-anchoring robot at this time point. Based on the fusion method of the extended Kalman filter algorithm, it can effectively handle the uncertainty and nonlinear problems of multi-source data and improve the system's ability to track and predict the pose of the drill-anchoring robot.

[0081] Establishing a space conversion model includes the following steps:

[0082] Taking the central position of the drill-anchoring robot as the coordinate origin, establish a body coordinate system, establish a space conversion model based on the body coordinate system, and convert the data information in the radar coordinate system, camera coordinate system, and inertial navigation coordinate system to the body coordinate system based on Formulas 3, 4, and 5;

[0083] Formula 3 is as follows:

[0084]

[0085] Formula 4 is as follows:

[0086]

[0087] Formula 5 is as follows:

[0088]

[0089] Wherein, is the body coordinate system, is the rotation matrix from the radar coordinate system to the inertial navigation coordinate system, is the translation vector from the inertial navigation coordinate system to the radar coordinate system, is the translation vector from the inertial navigation coordinate system to the body coordinate system;

[0090] Under the condition of unifying the body coordinate system, the inertial navigation data, visual images and point cloud data are fused to obtain comprehensive data.

[0091] Specifically, as Figure 4 shown, it is the step flow chart for obtaining comprehensive data. The geometric center of the drilling and anchoring robot is selected as the central position, and with this point as the coordinate origin, three mutually perpendicular coordinate axis directions are determined to construct the body coordinate system. The body coordinate system established in this way can closely relate to the movement and operation of the drilling and anchoring robot itself, providing a unified benchmark for subsequent space conversion and data fusion. Then, according to Formula 3, Formula 4 and Formula 5, each point cloud data, visual image and inertial navigation data are converted to the body coordinate system. Next, a fusion algorithm based on feature matching and data association is adopted. According to the reliability and accuracy of different sensor data, different weights are assigned to them. For example, when the roadway environment is relatively clear and the quality of the visual image is high, the weight of the visual image data is appropriately increased; when the robot moves violently and the inertial navigation data is relatively more stable, the weight of the inertial navigation data is increased. Through the weighted fusion method, the three kinds of data are fused into comprehensive data, which contains more comprehensive and accurate information about the underground roadway environment and the pose of the drilling and anchoring robot. By establishing a unified body coordinate system and converting the data in the radar coordinate system, camera coordinate system and inertial navigation coordinate system to the body coordinate system, the unified management and processing of multi-sensor data are realized, making full use of the advantages of different sensors. The point cloud data can provide accurate spatial distance information, the visual image can supplement rich texture and structural details, and the inertial navigation data provides continuous attitude and position change information during the movement of the robot. By fusing these data, they can complement and correct each other, reducing the errors and limitations of single-sensor data.

[0092] Obtaining the comprehensive data includes 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 so, then determine that the data corresponding to the eigenvalue is abnormal, define it as abnormal data, eliminate the abnormal data, and define the data after elimination as comprehensive data.

[0094] Specifically, after completing the fusion of inertial navigation data, visual images, and point cloud data in the body coordinate system, perform statistical analysis on the fused data to obtain the covariance matrix, and perform eigenvalue decomposition on the covariance matrix. According to a large amount of experimental data and actual working conditions, set a suitable second threshold. For the eigenvalue corresponding to each fused data sample, compare it with the second threshold. If a certain eigenvalue is less than the second threshold, then determine that the entire data sample corresponding to the eigenvalue is abnormal data. Once the abnormal data is determined, eliminate it from the dataset. By traversing the data samples, mark and delete those data determined to be abnormal. The final remaining data is the comprehensive data. After eliminating the abnormal data, these comprehensive data have higher reliability and consistency, and can more accurately reflect the true state of the drill-anchor robot in the underground roadway environment.

[0095] Obtaining the motion trajectory of the drill-anchor robot in the body coordinate system includes the following steps:

[0096] Generate the motion trajectory of each feature point based on the displacement vector of the feature point in the target pose within the target time period, generate the motion trajectory of the drill-anchor robot in the underground roadway based on the motion trajectories at all feature points, and smooth the motion trajectory.

[0097] Specifically, first extract the displacement vector of the feature point in the target time period from the target pose information. For each feature point, obtain the displacement vector within this time period by calculating the difference in position coordinates at adjacent sampling times, and calculate the position coordinates at each sampling time in turn, so as to obtain the motion trajectory of a single feature point in the target time period. After obtaining the motion trajectory of each feature point, generate the overall motion trajectory of the drill-anchor robot in the underground roadway based on the motion trajectories of all feature points. Since multiple feature points on the drill-anchor robot jointly reflect the motion state of the robot, determine the overall motion situation of the robot through comprehensive analysis of the motion trajectories of these feature points. For example, regard the drill-anchor robot as a rigid body, assume there are multiple feature points on the robot, calculate the centroid coordinates of all feature points through the centroid calculation formula, and then connect these centroid coordinates in sequence to form the overall motion trajectory of the drill-anchor robot in the underground roadway, and then smooth the generated motion trajectory. Generating the overall motion trajectory of the drill-anchor robot based on the motion trajectories of all feature points improves the accuracy and positioning accuracy of the motion trajectory, and can reflect the motion details of the drill-anchor robot in the underground roadway in detail.

[0098] Smoothing the motion trajectory includes the following steps:

[0099] Adopt a curve fitting algorithm to fit the motion trajectory, generate a fitting curve, optimize the fitting curve, and use the optimized fitting curve as the smoothed motion trajectory.

[0100] Specifically, after obtaining the motion trajectory of the drilling and anchoring robot, a curve fitting algorithm is used to fit the motion trajectory. Further, a cubic spline interpolation algorithm is used to fit the motion trajectory to generate a smooth fitting curve. The noise points and mutation points of the fitting curve are detected and removed, and finally the optimized fitting curve is obtained as the smoothed motion trajectory. Through the curve fitting algorithm for smoothing, the fluctuations and discontinuities of the motion trajectory are effectively removed, and the accuracy and reliability of the motion trajectory are further enhanced.

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

[0102] By constructing a roadway model, the present invention can intuitively reflect the shape of the underground roadway, providing an accurate environmental basis for the positioning and navigation of the drilling and anchoring robot; by fusing the fuselage observation model and the fuselage prediction model to obtain a target model, the accuracy and stability of the pose estimation of the drilling and anchoring robot are improved, and the error accumulation caused by a single sensor is reduced; by converting the radar coordinate system, the camera coordinate system and the inertial navigation coordinate system into the body coordinate system, the unified management and processing of multi-sensor data are realized, and the advantages of different sensors are fully utilized; by obtaining the motion trajectory of the drilling and anchoring robot in the body coordinate system, the moving path of the drilling and anchoring robot in the underground roadway can be intuitively displayed, which helps to improve the operation efficiency and safety of the drilling and anchoring robot and effectively improve the positioning accuracy of the drilling and anchoring robot.

[0103] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, and are not intended to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A positioning method for a drilling and anchoring robot with multi-sensor fusion, characterized in that The method includes the following steps: S1: Obtain the point cloud data in the underground roadway during the target time period in the radar coordinate system, obtain the visual image in the underground roadway during the target time period in the camera coordinate system, extract the feature points of the visual image, fuse the feature points and the point cloud data at the same time point during the target time period to obtain the target point cloud data, and construct a roadway model based on the target point cloud data; S2: Establish a fuselage observation model based on the feature points, obtain the inertial navigation data in the underground roadway during 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 a target model, and the target model outputs the target pose corresponding to each time point of the drilling and anchoring robot during the target time period; S3: Establish a space transformation model, and transform the radar coordinate system, the camera coordinate system, and the inertial navigation coordinate system into the body coordinate system based on the space transformation model; S4: Transform the target pose and the roadway 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 pose of the drilling and anchoring robot at each time point during the target time period.

2. The method according to claim 1, wherein The fusion of the feature points and the point cloud data at the same time point during the target time period includes the following steps: Convert the point cloud data in the radar coordinate system to the camera coordinate system based on Formula 1, and Formula 1 is: Among them, is the camera coordinate system, is the rotation matrix from the radar coordinate system to the camera coordinate system, is the radar coordinate system, is the translation vector from the radar coordinate system to the camera coordinate system; Perform registration processing on the feature points and the converted point cloud data based on the ICP algorithm to obtain the fused target point cloud data.

3. The method according to claim 2, wherein The registration processing of the feature points and the converted point cloud data based on the ICP algorithm includes the following steps: Select some feature points as the initial matching point set, determine the initial corresponding point set of the feature points in the converted point cloud data, calculate the optimal rotation matrix, the optimal translation vector, and the 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 so, output the registered target point cloud data. If not, re-match the initial matching point set and the initial corresponding point set based on the optimal rotation matrix and the optimal translation vector, and iteratively calculate the registration error until the registration error is less than the first threshold.

4. The method according to claim 1, wherein The establishment of the fuselage observation model based on the feature points includes the following steps: Obtain the displacement vector of each feature point at adjacent time points during the target time period, obtain the first pose change of the drilling and anchoring robot during the target time period based on the displacement vector, input the first poses at all time points as training data into the fuselage observation model, and the fuselage observation model outputs the first attitude information obtained by the drilling and anchoring robot in the camera coordinate system.

5. The method according to claim 4, characterized in that, The establishment of the fuselage prediction model based on the inertial navigation data includes the following steps: The inertial navigation data includes a first angular velocity and a first acceleration of the drilling and anchoring robot, an acceleration component of the drilling and anchoring robot caused by gravity is obtained, the acceleration component is subtracted from the first acceleration to obtain a target acceleration of the drilling and anchoring robot, the rotation angle of the drilling and anchoring robot is obtained based on the first angular velocity, the rotation angle is integrated based on a quaternion to obtain a standard rotation angle, a second posture of the drilling and anchoring robot is generated based on the target acceleration and the standard rotation angle, the second posture at all time points within a target time period is input as training data into a fuselage prediction model, and the fuselage prediction model outputs second posture information of the drilling and anchoring robot in the inertial navigation coordinate system.

6. The method according to claim 5, characterized in that The fusion of the fuselage observation model and the fuselage prediction model comprises the following steps: The first posture information in the camera coordinate system is converted to the inertial navigation coordinate system based on Formula 2, and Formula 2 is as follows: Among them, is the inertial navigation coordinate system, is the rotation matrix from the camera coordinate system to the inertial navigation coordinate system, is the translation vector from the camera coordinate system to the inertial navigation coordinate system; Obtain an observation error value of the first posture information obtained based on the fuselage observation model and a prediction error value of the second posture information obtained based on the fuselage prediction model, assign different weights to the fuselage observation model and the fuselage prediction model based on the observation error value and the prediction error value, and fuse the first posture information and the second posture information using an extended Kalman filter algorithm based on the weights to obtain a target posture.

7. The method according to claim 1, wherein Building a spatial transformation model includes the following steps: Taking the center position of the anchor drilling robot as the coordinate origin, a body coordinate system is established, a space conversion model is established based on the body coordinate system, and data information in the radar coordinate system, the camera coordinate system, and the inertial navigation coordinate system are converted to the body coordinate system based on Formula 3, Formula 4, and Formula 5; The formula 3 is as follows: The formula 4 is as follows: The formula 5 is as follows: Among them, is the body coordinate system, is the rotation matrix from the radar coordinate system to the inertial navigation coordinate system, is the translation vector from the inertial navigation coordinate system to the radar coordinate system, is the translation vector from the inertial navigation coordinate system to the body coordinate system; In the case of a unified body coordinate system, the inertial navigation data, the visual image and the point cloud data are fused to obtain comprehensive data.

8. The method according to claim 7, wherein Obtaining comprehensive data includes 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 so, determine that the data corresponding to the eigenvalue is abnormal and define it as abnormal data. Eliminate the abnormal data and define the eliminated data as comprehensive data.

9. The method according to claim 1, wherein Obtaining the motion trajectory of the anchor drilling robot in the body coordinate system comprises the following steps: Based on the displacement vector of the feature point in the target posture within the target time period, the motion trajectory of each feature point is generated, based on the motion trajectories at all feature points, the motion trajectory of the drilling and anchoring robot in the underground tunnel is generated, and the motion trajectory is smoothed.

10. The method according to claim 9, wherein The smoothing process of the motion trajectory comprises the following steps: The motion trajectory is fitted by using a curve fitting algorithm to generate a fitting curve, the fitting curve is optimized, and the optimized fitting curve is used as the motion trajectory after smoothing.

Images