Excavation equipment space pose detection method and device based on multi-heterogeneous information redundancy

Abstract

The application discloses a tunneling equipment space pose detection method and device based on multi-heterogeneous information redundancy and relates to the technical field of intelligent tunneling equipment of coal mines. The tunneling equipment space pose detection method based on multi-heterogeneous information redundancy comprises the following steps: when the state of the output result of a mileage counter is a normal state, a first combined detection structure of the mileage counter combined with inertial navigation is used to determine the space pose of the tunneling equipment; when the output result of the mileage counter is abnormal, the space pose of the tunneling equipment working for a long time is no longer detected by using single inertial navigation, but according to the specific state of the visual condition of the roadway working face, the second combined detection structure of an optical ranging robot combined with inertial navigation or a radar ranging lateral auxiliary positioning mode is selected to determine the space pose of the tunneling equipment when the visual condition is good or poor, so that the detection accuracy of the space pose of the tunneling equipment working for a long time is ensured.

Description

Technical Field

[0001] This invention relates to the field of intelligent tunneling equipment technology in coal mines, and in particular to a method and device for spatial pose detection of tunneling equipment based on redundancy of multiple heterogeneous information. Background Technology

[0002] Tunneling faces are characterized by narrow spaces, harsh environments, high risks, dense personnel, and intense labor, posing significant occupational health threats to workers. Therefore, minimizing manpower at tunneling faces has always been a goal pursued by the industry. Currently, intelligent tunneling status perception and monitoring technologies in coal mines have many shortcomings, such as the pose detection technology for tunneling equipment, which still faces severe challenges. High-precision navigation and positioning technology can improve tunneling efficiency, reduce worker labor intensity, and improve the working environment. It can solve important technical problems in the construction of intelligent tunneling and is of great value in realizing unmanned or minimally manned tunneling faces.

[0003] Tunneling equipment is characterized by limited movement space and long-endurance operation. Navigation parameter acquisition devices must be error-free, unaffected by light interference, and capable of long-endurance operation. Currently, domestic tunneling equipment attitude detection technology mostly uses inertial navigation systems (INS) in conjunction with odometry (OJ) to achieve spatial attitude detection. However, the integral error generated by INS itself reduces the accuracy of fuselage attitude detection, and INS-only spatial attitude calculations quickly accumulate errors, failing to meet the accuracy requirements for spatial attitude detection in tunneling equipment.

[0004] Due to the unique working environment in tunnels, factors such as dust levels, visibility, and space size result in varying visibility conditions at the tunnel face. Furthermore, single odometers are susceptible to slippage and accumulated attitude errors in dead reckoning, leading to abnormal odometer outputs. Therefore, when both odometer outputs are abnormal and visibility conditions at the tunnel face vary, using a single inertial navigation system to detect the spatial attitude of tunneling equipment operating for extended periods simply cannot guarantee the accuracy of spatial attitude detection for such equipment. Summary of the Invention

[0005] The purpose of this invention is to provide a method and device for detecting the spatial pose of tunneling equipment based on the redundancy of multiple heterogeneous information, so as to solve the problem of low detection accuracy when using a single inertial navigation system to detect the spatial pose of tunneling equipment operating for long periods of time, when the odometer output results are abnormal and the visibility conditions of the tunnel face are different.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] A spatial pose detection method for tunneling equipment based on redundancy of heterogeneous information includes:

[0008] The status of the odometer output and the status of the visibility conditions of the roadway working face are obtained; the status of the odometer output includes normal status and abnormal status; the status of the visibility conditions of the roadway working face includes good visibility conditions and poor visibility conditions.

[0009] When the odometer output is in a normal state, the spatial pose of the tunneling equipment is determined by the first combined detection structure of the odometer and inertial navigation.

[0010] When the odometer output is in an abnormal state, different detection methods are selected to determine the spatial pose of the tunneling equipment based on the visibility conditions of the tunnel face. The different detection methods include the second combined detection structure of the optical ranging robot and the inertial navigation system, and the radar ranging lateral assisted positioning method.

[0011] Optionally, the spatial pose of the tunneling equipment is determined using a first combined detection structure of odometer and inertial navigation, specifically including:

[0012] Acquire inertial navigation data and odometer data; the inertial navigation data includes the attitude error angle and drift error of the inertial navigation; the odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the calibration coefficient error of the odometer, and the output delay time error of the odometer.

[0013] By combining the inertial navigation data and the odometer data, a first system state vector for the inertial navigation system and the odometer is determined;

[0014] Based on the first system state vector, establish the first system state equation for the inertial navigation system and the odometer;

[0015] The state expression of the first system is discretized, and the state optimization estimation of the first system state expression is performed by Kalman filtering method to determine the first attitude transformation matrix of the tunneling equipment.

[0016] The first attitude angle of the tunneling equipment is determined based on the first attitude transformation matrix;

[0017] The spatial orientation of the tunneling equipment is determined based on the first attitude angle of the tunneling equipment.

[0018] Optionally, when the visibility conditions of the tunnel working face are good, the spatial pose of the tunneling equipment is determined by using the optical ranging robot combined with the second combined detection structure of the inertial navigation system.

[0019] Optionally, the spatial pose of the tunneling equipment is determined by using the optical ranging robot combined with the second combined detection structure of the inertial navigation system, specifically including:

[0020] Acquire the inertial navigation data and the optical ranging robot data; the optical ranging robot data includes the position error measured by the optical ranging robot.

[0021] By combining the inertial navigation data and the optical ranging robot data, a second system state vector for the inertial navigation system and the optical ranging robot is determined;

[0022] Based on the second system state vector, establish the second system state equations for the inertial navigation system and the optical ranging robot;

[0023] The second system state expression is discretized, and the Kalman filter method is used to perform optimal estimation of the second system state expression to determine the second attitude transformation matrix of the tunneling equipment.

[0024] The second attitude angle of the tunneling equipment is determined based on the second attitude transformation matrix;

[0025] The spatial orientation of the tunneling equipment is determined based on the second attitude angle of the tunneling equipment.

[0026] Optionally, when the visibility conditions at the tunnel face are poor, radar ranging lateral auxiliary positioning is used to determine the spatial orientation of the tunneling equipment; the radar is located on both sides of the tunneling equipment.

[0027] Optionally, the spatial pose of the tunneling equipment is determined by using radar ranging lateral-assisted positioning, specifically including:

[0028] Based on the installation location of the radar, a first distance between the tunneling equipment and one side of the tunnel and a second distance between the tunneling equipment and the other side of the tunnel are determined;

[0029] The distance difference is obtained based on the first distance and the second distance;

[0030] Based on the distance difference, determine whether the tunneling equipment is within a safe operating range;

[0031] If the tunneling equipment is within a safe operating range, the radar is used to determine the spatial orientation of the tunneling equipment.

[0032] If the tunneling equipment is not within the safe operating range, adjust the position of the tunneling equipment to bring it within the safe operating range, and use radar to determine the spatial orientation of the tunneling equipment.

[0033] Optionally, determining whether the tunneling equipment is within a safe operating range based on the distance difference specifically includes:

[0034] If the distance difference is within a first set threshold range, it is determined that the tunneling equipment is within a safe operating range;

[0035] If the distance difference is not within the first set threshold range, it is determined that the tunneling equipment is not in a safe operating range.

[0036] Optionally, the status of the odometer output can be obtained, specifically including:

[0037] If the odometer output result is within the second set threshold, the state of the odometer output result is determined to be normal.

[0038] If the odometer output result is not within the second set threshold, the state of the odometer output result is determined to be an abnormal state.

[0039] Optionally, the status of the visibility conditions at the working face of the roadway can be obtained, specifically including:

[0040] Obtain the dust concentration value at the working face of the roadway;

[0041] If the dust concentration value is within the third set threshold, the visibility condition of the roadway working face is determined to be a good visibility condition.

[0042] If the dust concentration value is not within the third set threshold, the visibility condition of the roadway working face is determined to be poor.

[0043] To achieve the above objectives, the present invention also provides the following solution:

[0044] A spatial pose detection device for tunneling equipment based on redundancy of heterogeneous information includes:

[0045] Tunneling equipment, inertial navigation, odometers, optical ranging robots and radar;

[0046] The inertial navigation system, the odometer, the optical ranging robot, and the radar are all mounted on the tunneling equipment.

[0047] The inertial navigation system is used to acquire inertial navigation data; the inertial navigation data includes the attitude error angle of the inertial navigation system and the drift error of the inertial navigation system;

[0048] The odometer is used to acquire odometer data; the odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the odometer scale coefficient error, and the output delay time error of the odometer.

[0049] The optical ranging robot is used to acquire optical ranging robot data; the optical ranging robot data includes the position error measured by the optical ranging robot.

[0050] The inertial navigation system combined with the odometer constitutes a first combined detection structure; the first combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in a normal state.

[0051] The inertial navigation system combined with the optical ranging robot constitutes a second combined detection structure; the second combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in an abnormal state and the visibility condition of the roadway working face is in a good state.

[0052] The radar is used to determine the spatial orientation of the tunneling equipment when the odometer output is in an abnormal state and the visibility conditions of the tunnel face are in a poor state.

[0053] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0054] The spatial pose detection method and device for tunneling equipment based on multi-heterogeneous information redundancy provided by this invention determines the spatial pose of the tunneling equipment by using a first combined detection structure of odometer and inertial navigation when the odometer output result is in a normal state. However, when the odometer output result is abnormal, the spatial pose of the tunneling equipment operating for long periods of time is no longer detected by a single inertial navigation system. Instead, the spatial pose is determined by selecting a second combined detection structure of optical ranging robot and inertial navigation or a radar ranging lateral auxiliary positioning method, depending on whether the visibility conditions at the tunnel face are good or poor. This invention uses different detection methods to determine the spatial pose of the tunneling equipment for different situations, such as normal or abnormal odometer output results and good or poor visibility conditions at the tunnel face, thus ensuring the detection accuracy of the spatial pose of the tunneling equipment operating for long periods of time. Attached Figure Description

[0055] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0056] Figure 1 The flowchart shows the spatial pose detection method for tunneling equipment based on redundancy of multiple heterogeneous information provided by the present invention.

[0057] Figure 2 This is a schematic diagram illustrating the detection method combining inertial navigation and odometer in this invention.

[0058] Figure 3 This is a schematic diagram of the detection method of the inertial navigation combined with optical ranging robot in this invention. Detailed Implementation

[0059] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0060] The purpose of this invention is to provide a method and apparatus for detecting the spatial pose of tunneling equipment based on the redundancy of multiple heterogeneous information. By adopting different detection methods to determine the spatial pose of tunneling equipment based on whether the odometer output result is in a normal or abnormal state, and whether the visibility conditions of the tunnel face are good or poor, the detection accuracy of the spatial pose of tunneling equipment operating for long periods of time is guaranteed.

[0061] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0062] Example 1

[0063] like Figure 1 As shown, the spatial pose detection method for tunneling equipment based on redundancy of multiple heterogeneous information provided by the present invention includes:

[0064] Step 101: Obtain the status of the odometer output and the status of the visibility conditions of the roadway working face; the status of the odometer output includes normal status and abnormal status; the status of the visibility conditions of the roadway working face includes good visibility conditions and poor visibility conditions.

[0065] Step 102: Determine whether the odometer output is in a normal state; if yes, proceed to step 103; if no, proceed to step 104.

[0066] Step 103: The spatial pose of the tunneling equipment is determined by the first combined detection structure of odometer and inertial navigation.

[0067] Step 104: Based on the visibility conditions at the tunnel face, select different detection methods to determine the spatial pose of the tunneling equipment; the different detection methods include a second combined detection structure using an optical ranging robot and inertial navigation, and a radar ranging lateral assisted positioning method. The radar can be a millimeter-wave radar.

[0068] Step 105: Determine whether the visibility conditions of the working face of the roadway are good; if yes, proceed to step 106; if no, proceed to step 107.

[0069] Step 106: Select the second combined detection structure of optical ranging robot and inertial navigation to determine the spatial pose of the tunneling equipment.

[0070] Step 107: Use radar ranging lateral auxiliary positioning to determine the spatial orientation of the tunneling equipment; the radar is located on both sides of the tunneling equipment.

[0071] Furthermore, the spatial pose of the tunneling equipment is determined using a first combined detection structure of odometer and inertial navigation, specifically including:

[0072] Acquire inertial navigation (INS) data and odometer data; INS data includes the attitude error angle and drift error of the INS; odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the calibration coefficient error of the odometer, and the output delay time error of the odometer.

[0073] By combining inertial navigation data and odometer data, the first system state vector of the inertial navigation and odometer is determined.

[0074] Based on the first system state vector, the first system state equations for the inertial navigation and odometry are established.

[0075] The state expression of the first system is discretized, and the state optimization estimation of the first system state expression is performed by Kalman filtering method to determine the first attitude transformation matrix of the tunneling equipment.

[0076] The first attitude angle of the tunneling equipment is determined based on the first attitude transformation matrix.

[0077] The spatial orientation of the tunneling equipment is determined based on its first attitude angle.

[0078] Furthermore, a second combined detection structure, using an optical ranging robot and an inertial navigation system, is selected to determine the spatial pose of the tunneling equipment, specifically including:

[0079] Acquire inertial navigation data and optical ranging robot data; the optical ranging robot data includes the position error measured by the optical ranging robot.

[0080] By combining inertial navigation data and optical ranging robot data, the second system state vector of the inertial navigation and optical ranging robot is determined.

[0081] Based on the second system state vector, establish the second system state formula for the inertial navigation and optical ranging robot.

[0082] The state expression of the second system is discretized, and the Kalman filter method is used to optimize the state expression of the second system to determine the second attitude transformation matrix of the tunneling equipment.

[0083] The second attitude angle of the tunneling equipment is determined based on the second attitude transformation matrix.

[0084] The spatial orientation of the tunneling equipment is determined based on its second attitude angle.

[0085] Furthermore, radar ranging lateral-assisted positioning is used to determine the spatial pose of the tunneling equipment, specifically including:

[0086] Based on the radar's installation location, the first distance between the tunneling equipment and one side of the tunnel and the second distance between the tunneling equipment and the other side of the tunnel are determined.

[0087] The distance difference is obtained based on the first distance and the second distance.

[0088] Based on the distance difference, determine whether the tunneling equipment is within the safe operating range.

[0089] If the tunneling equipment is within its safe operating range, radar is used to determine its spatial position and orientation. Specifically, the safe operating range is defined as the area between -0.5m and +0.5m from the designed centerline of the tunnel.

[0090] If the tunneling equipment is not within the safe operating range, adjust the position of the tunneling equipment to bring it within the safe operating range, and use radar to determine the spatial attitude of the tunneling equipment.

[0091] Furthermore, based on the distance difference, it is determined whether the tunneling equipment is within the safe operating range, specifically including:

[0092] If the distance difference is within the first preset threshold range, the tunneling equipment is determined to be within the safe operating range. The first preset threshold range is defined as -0.5m to +0.5m from the tunnel design centerline.

[0093] If the distance difference is not within the first set threshold range, it is determined that the tunneling equipment is not within the safe operating range.

[0094] Furthermore, the status of the odometer output is obtained, specifically including:

[0095] If the odometer output is within the second set threshold, the odometer output is considered to be in a normal state. The second set threshold is determined based on the following conditions: abnormal fluctuations in the position, attitude, forward speed, turning speed of the tunneling equipment, and the speed output during the odometer process; sudden increases or decreases in speed instead of stable increases or decreases; or an increase in odometer speed and rotation number without equipment movement; or a significant deviation between the odometer output rotation speed and the actual value.

[0096] If the odometer output result is not within the second set threshold, the odometer output result is determined to be in an abnormal state.

[0097] Furthermore, the status of the visibility conditions at the working face of the roadway is obtained, specifically including:

[0098] Obtain the dust concentration value at the working face of the roadway.

[0099] If the dust concentration is within the third set threshold, the visibility condition of the roadway working face is determined to be good. The third set threshold is 10 mg / m3.

[0100] If the dust concentration value is not within the third set threshold, the visibility condition of the roadway working face is determined to be poor.

[0101] This embodiment addresses the inaccuracies of navigation and positioning using a single sensor and the accumulation of errors in the inertial navigation system (INS) over time, taking into account the technological requirements and operational characteristics of existing tunneling equipment. Since INS drift over time significantly impacts positioning errors, a spatial pose method for tunneling equipment with multi-heterogeneous information redundancy is designed based on the attitude and position information of the INS. This method fully considers different spatial pose design methods depending on whether the odometer is functioning properly and the visibility conditions at the tunnel face are good.

[0102] By using the mileage information output by the odometer and the real-time attitude conversion matrix output by the inertial navigation system, dead reckoning is performed to obtain the position information of the tunneling equipment. In response to the measurement error caused by track slippage, when the visibility at the working face is good, an optical ranging robot is used to observe pre-arranged feature points for position compensation; when the visibility is poor, millimeter-wave radar ranging is used for lateral auxiliary positioning to improve the autonomous positioning and navigation accuracy of the tunneling equipment navigation system.

[0103] Example 2

[0104] like Figure 2 As shown, the specific process of determining the spatial pose of tunneling equipment using the first combined detection structure of odometer and inertial navigation in this invention is described in detail. In this embodiment, the inertial navigation system is a strapdown inertial navigation system.

[0105] Select the East-North-Sky geographic coordinate system (OX) n Y n Z n Let be the navigation coordinate system, denoted as the n-system, and select the right-front-up direction (OX). b Y b Z b Let b be the coordinate system of the vehicle, and construct the transformation matrix between the vehicle coordinate system (b) and the navigation coordinate system (n).

[0106]

[0107] By transforming the matrix, the spatial pose information of the tunneling equipment in the carrier coordinate system can be obtained.

[0108] 1. Establish the system state vectors of the inertial navigation and odometer.

[0109] By combining inertial navigation system (INS) data and odometer data, the errors of the INS and odometer are merged into a single vector to obtain the system state vector:

[0110]

[0111] The variables in the formula include: attitude error angle of the inertial navigation system, velocity error, position error, gyroscope drift, accelerometer error, and total station measurement error, totaling 22 dimensions. The gyroscope is a three-axis gyroscope, and the accelerometer is a three-axis accelerometer.

[0112] X is the state vector; The attitude error angle of the inertial navigation system. These represent the attitude error angles of the inertial navigation system in the three directions of northeast and sky; δv represents the velocity error of the carrier. E δv N δv U These represent the velocity errors of the carrier in the three directions of northeast, sky, and geography, respectively, in the geographic coordinate system; δp INS δL, δλ, and δh represent the position error of the carrier calculated by the inertial navigation system, respectively, and are the longitude, latitude, and altitude errors; ε represents the drift error of the gyroscope. E ε N ε U These represent the drift errors of the gyroscope in the three directions of northeast and zenith. For the drift error of the accelerometer, These represent the drift errors of the accelerometer in the three directions of northeast and sky; δp O δL represents the position error measured by the odometer. O ,δλ O δh O These represent the positional errors of the odometer measurements in longitude, latitude, and altitude, respectively; σ O For the odometer in pitch δβO With heading δα O Installation error; δf O dT represents the odometer scale coefficient error; dT represents the odometer output delay time.

[0113] 2. Establish the system state of inertial navigation and odometry. The specific equation of state is as follows:

[0114]

[0115] In the formula, A is the state transition matrix, Γ is the system noise driving matrix, and W is the system noise.

[0116] 3. Establish the system measurement formula Z for inertial navigation and odometer.

[0117] The difference between the position data calculated by the inertial navigation system and the position data measured by the odometer is used as the observation value. The specific observation equation is as follows:

[0118] Z = p INS -p O

[0119] In the formula, p INS For the position data calculated by the inertial navigation system, p O Location data measured by the odometer.

[0120] Combining the two, we obtain the system state equations for the inertial navigation and odometry systems:

[0121]

[0122] In the formula, X is the state vector, A is the state transition matrix, Γ is the system noise driving matrix (system noise coefficient matrix), W is the system noise, Z is the observation value, H is the observation matrix (a user-defined constant matrix), and V is the observation noise matrix (a Gaussian noise matrix with expectation of 0 and variance of R).

[0123] 4. Use the Kalman filter fusion algorithm (KF) to perform state optimization estimation.

[0124] The system state equations of the inertial navigation and odometry are discretized, and then KF is used for optimization estimation to obtain an accurate attitude transformation matrix.

[0125] 4.1 KF state one-step prediction formula:

[0126]

[0127] In the formula, Φ is the one-step prediction value of the (k-1)th discrete state vector; k,k-1 This is a one-step transition matrix; This is the estimated value of the (k-1)th discrete state vector.

[0128] 4.2 One-step prediction mean squared error matrix:

[0129]

[0130] In the formula, Γ k-1 Let Q be the system's discrete noise driving matrix. k-1 Let P be the noise variance. k-1 P represents the root mean square error of the state estimate at time k-1; k,k-1 This is the one-step prediction value of the mean squared error of the (k-1)th state.

[0131] 4.3 Filter Gain Matrix:

[0132]

[0133] In the formula, K k R is the filter gain; k To observe the noise variance, it is generally a fixed value, which is related to the accuracy of the sensor; H k This is a discrete observation matrix.

[0134] 4.4 State estimation formula:

[0135]

[0136] In the formula, Z k These are discrete observations.

[0137] P k =(IK k H k )P k,k-1 .

[0138] In the formula, I is the identity matrix.

[0139] This embodiment proposes a navigation and positioning method using odometers and inertial navigation systems (INS). In this method, the INS measures the speed, position, and attitude of the tunneling equipment. The mileage information in the odometer data output by the odometer is compared with the real-time attitude transformation matrix of the tunneling equipment output by the INS to calculate dead reckoning. The odometer measures the mileage increment of the tunneling equipment to correct the position information of the INS, compensate for the cumulative error of the INS over time, and achieve accurate positioning and orientation of the tunneling equipment, thus determining the spatial attitude of the tunneling equipment.

[0140] Example 3

[0141] like Figure 3 As shown, the specific process of determining the spatial pose of the tunneling equipment using the second combined detection structure of an optical ranging robot and an inertial navigation system is described in detail. In this embodiment, the inertial navigation system is a strapdown inertial navigation system.

[0142] 1. Establish the system state vector of the inertial navigation and optical ranging robot.

[0143] By combining the errors of the inertial navigation and optical ranging robots into a single vector, the system state vector is obtained:

[0144]

[0145] The variables in the formula include: attitude error angle of the inertial navigation system, velocity error, position error, gyroscope drift, accelerometer error, and measurement error of the optical ranging robot, totaling 18 dimensions. The gyroscope is a three-axis gyroscope, and the accelerometer is a three-axis accelerometer.

[0146] X is the state vector; The attitude error angle of the inertial navigation system. These represent the attitude error angles of the inertial navigation system in the three directions of northeast and sky; δv represents the velocity error of the carrier. E δv N δv U These represent the velocity errors of the carrier in the three directions of northeast and sky in the geographic coordinate system; δp INS δL, δλ, and δh represent the position error of the carrier calculated by the inertial navigation system, respectively, while ε represents the longitude, latitude, and altitude errors; ε is the drift error of the gyroscope. E ε N ε U These represent the drift errors of the gyroscope in the three directions of northeast and south. For the drift error of the accelerometer, These represent the drift errors of the accelerometer in the three directions of northeast and sky; δp T δp represents the position error measured by the optical ranging robot. x δp y δp z These represent the positional errors of the three-axis measurements at the measurement points of the optical ranging robot.

[0147] 2. Establish the system state of the inertial navigation and optical ranging robot. The specific equation of state is as follows:

[0148]

[0149] In the formula, A is the state transition matrix, Γ is the system noise driving matrix, and W is the system noise.

[0150] 3. Establish the system measurement formula Z for inertial navigation and optical ranging robots.

[0151] The difference between the position data calculated by the inertial navigation system and the position data measured by the optical ranging robot is used as the observation value. The specific observation equation is as follows:

[0152] Z = p INS -p T .

[0153] In the formula, p INS For the position data calculated by the inertial navigation system, p T Position data measured by an optical ranging robot.

[0154] Combining the two, we obtain the system state equation for the inertial navigation and optical ranging robot:

[0155]

[0156] In the formula, X is the state vector, A is the state transition matrix, Γ is the system noise driving matrix (system noise coefficient matrix), W is the system noise, Z is the observation value, H is the observation matrix (a user-defined constant matrix), and V is the observation noise matrix (a Gaussian noise matrix with expectation of 0 and variance of R).

[0157] 4. Use the Kalman filter fusion algorithm (KF) to perform state optimization estimation.

[0158] The system state equations of the inertial navigation and optical ranging robot are discretized, and then KF is used for optimization estimation to obtain an accurate attitude transformation matrix.

[0159] 4.1 KF state one-step prediction formula:

[0160]

[0161] In the formula, Φ is the one-step prediction value of the (k-1)th discrete state vector; k,k-1 This is a one-step transition matrix; This is the estimated value of the (k-1)th discrete state vector.

[0162] 4.2 One-step prediction mean squared error matrix:

[0163]

[0164] In the formula, Γ k-1 Q is the system's discrete noise driving matrix. k-1 Let P be the noise variance. k-1 P represents the root mean square error of the state estimate at time k-1; k,k-1 This is the one-step prediction value of the mean squared error of the (k-1)th state.

[0165] 4.3 Filter Gain Matrix:

[0166]

[0167] In the formula, Kk R is the filter gain; k To observe the noise variance, it is generally a fixed value, which is related to the accuracy of the sensor; H k This is a discrete observation matrix.

[0168] 4.4 State estimation formula:

[0169]

[0170] In the formula, Z k These are discrete observations.

[0171] P k =(IK k H k )P k,k-1 .

[0172] In the formula, I is the identity matrix.

[0173] In this embodiment, the attitude angle of the tunneling equipment body is provided by the inertial navigation system, and the optical ranging robot measures the accurate three-dimensional coordinates of the tunneling equipment body. Since the inertial navigation system accumulates errors over time, the KF algorithm is used to combine the attitude data given by the inertial navigation system and the position coordinates of the automatic total station to establish the system state vector, system state expression, and system measurement expression of the inertial navigation system and the optical ranging robot. System state prediction and optimization estimation are performed to obtain the position and attitude of the tunneling equipment of the inertial navigation system and the optical ranging robot.

[0174] Example 4

[0175] The spatial pose detection device for tunneling equipment based on multi-heterogeneous information redundancy provided by the present invention includes: tunneling equipment, inertial navigation, odometer, optical ranging robot and radar.

[0176] Inertial navigation, odometer, optical ranging robot and radar are all installed on the tunneling equipment.

[0177] Inertial navigation systems (INS) are used to acquire INS data; INS data includes the attitude error angle and the drift error of the INS.

[0178] The odometer is used to acquire odometer data; the odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the odometer scale coefficient error, and the odometer output delay time error.

[0179] Optical ranging robots are used to acquire optical ranging robot data; optical ranging robot data includes the positional error measured by the optical ranging robot.

[0180] The inertial navigation system combined with the odometer constitutes the first combined detection structure; the first combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in a normal state.

[0181] The inertial navigation system combined with the optical ranging robot constitutes the second combined detection structure; the second combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in an abnormal state and the visibility condition of the roadway working face is in a good state.

[0182] The radar is used to determine the spatial orientation of the tunneling equipment when the odometer output is in an abnormal state and the visibility conditions at the tunnel face are poor.

[0183] This invention proposes a method using odometers, optical ranging robots, inertial navigation systems (INS), and millimeter-wave radar to achieve global autonomous spatial position and heading attitude detection for tunneling equipment. By using the odometer's output mileage information and the INS' real-time attitude transformation matrix, dead reckoning is performed to obtain the tunneling equipment's position information. To address mileage errors caused by track slippage, under conditions of good visibility at the working face, optical ranging robots are used to observe pre-placed feature points for position compensation. Under poor visibility conditions, millimeter-wave radar ranging is used for lateral auxiliary positioning, improving the autonomous positioning and navigation accuracy of the tunneling equipment's navigation system.

[0184] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0185] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method, structure, and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for spatial pose detection of tunneling equipment based on redundancy of heterogeneous information, characterized in that, include: Acquire the status of the odometer output results and the status of the visibility conditions of the roadway working face; The status of the odometer output includes normal status and abnormal status; The visibility conditions of the roadway working face include good visibility and poor visibility. When the odometer output is in a normal state, the spatial pose of the tunneling equipment is determined by the first combined detection structure of the odometer and inertial navigation. When the odometer output is in an abnormal state, different detection methods are selected to determine the spatial pose of the tunneling equipment based on the visibility conditions of the tunnel face. The different detection methods include the second combined detection structure of the optical ranging robot and the inertial navigation system, and the radar ranging lateral assisted positioning method. When the odometer output is in an abnormal state, different detection methods are selected to determine the spatial pose of the tunneling equipment based on the visibility conditions of the roadway face. Specifically, this includes: When the visibility conditions at the tunnel face are good, the spatial pose of the tunneling equipment is determined using the second combined detection structure of the optical ranging robot and the inertial navigation system. Specifically, determining the spatial pose of the tunneling equipment using this second combined structure includes: acquiring inertial navigation data and optical ranging robot data; the optical ranging robot data includes the position error measured by the optical ranging robot; combining the inertial navigation data and the optical ranging robot data to determine the second system state vector of the inertial navigation system and the optical ranging robot; establishing the second system state expression of the inertial navigation system and the optical ranging robot based on the second system state vector; discretizing the second system state expression and using Kalman filtering to perform optimal estimation of the second system state expression to determine the second attitude transformation matrix of the tunneling equipment; determining the second attitude angle of the tunneling equipment based on the second attitude transformation matrix; and determining the spatial pose of the tunneling equipment based on the second attitude angle. When the visibility conditions at the working face of the tunnel are poor, the spatial pose of the tunneling equipment is determined using radar ranging lateral auxiliary positioning. The radar is located on both sides of the tunneling equipment. Specifically, determining the spatial pose of the tunneling equipment using radar ranging lateral auxiliary positioning includes: determining a first distance between the tunneling equipment and one side of the tunnel and a second distance between the tunneling equipment and the other side of the tunnel based on the installation position of the radar; obtaining the distance difference based on the first and second distances; determining whether the tunneling equipment is within a safe operating range based on the distance difference; if the tunneling equipment is within a safe operating range, determining the spatial pose of the tunneling equipment using the radar; if the tunneling equipment is not within a safe operating range, adjusting the position of the tunneling equipment to bring it within a safe operating range, and then determining the spatial pose of the tunneling equipment using the radar.

2. The method for spatial pose detection of tunneling equipment based on redundancy of multiple heterogeneous information as described in claim 1, characterized in that, The spatial pose of the tunneling equipment is determined using a first-combination detection structure that combines odometer and inertial navigation, specifically including: The inertial navigation data includes the attitude error angle and drift error of the inertial navigation; odometer data is acquired; the odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the scale coefficient error of the odometer, and the output delay time error of the odometer; By combining the inertial navigation data and the odometer data, a first system state vector for the inertial navigation system and the odometer is determined; Based on the first system state vector, establish the first system state equation for the inertial navigation system and the odometer; The state expression of the first system is discretized, and the state optimization estimation of the first system state expression is performed by Kalman filtering method to determine the first attitude transformation matrix of the tunneling equipment. The first attitude angle of the tunneling equipment is determined based on the first attitude transformation matrix; The spatial orientation of the tunneling equipment is determined based on the first attitude angle of the tunneling equipment.

3. The spatial pose detection method for tunneling equipment based on multi-heterogeneous information redundancy according to claim 1, characterized in that, Determining whether the tunneling equipment is within a safe operating range based on the distance difference specifically includes: If the distance difference is within a first set threshold range, it is determined that the tunneling equipment is within a safe operating range; If the distance difference is not within the first set threshold range, it is determined that the tunneling equipment is not in a safe operating range.

4. The spatial pose detection method for tunneling equipment based on redundancy of multiple heterogeneous information as described in claim 1, characterized in that, The status of the odometer output is obtained, specifically including: If the odometer output result is within the second set threshold, the state of the odometer output result is determined to be normal. If the odometer output result is not within the second set threshold, the state of the odometer output result is determined to be an abnormal state.

5. The method for spatial pose detection of tunneling equipment based on redundancy of multiple heterogeneous information as described in claim 1, characterized in that, Obtain the status of the visibility conditions at the working face of the roadway, specifically including: Obtain the dust concentration value at the working face of the roadway; If the dust concentration value is within the third set threshold, the visibility condition of the roadway working face is determined to be a good visibility condition. If the dust concentration value is not within the third set threshold, the visibility condition of the roadway working face is determined to be poor.

6. A spatial pose detection device for tunneling equipment based on redundancy of heterogeneous information, characterized in that, The tunneling equipment spatial pose detection device based on multiple heterogeneous information redundancy adopts the tunneling equipment spatial pose detection method based on multiple heterogeneous information redundancy as described in any one of claims 1-5. The tunneling equipment spatial pose detection device based on multiple heterogeneous information redundancy includes: tunneling equipment, inertial navigation, odometer, optical ranging robot and radar. The inertial navigation system, the odometer, the optical ranging robot, and the radar are all mounted on the tunneling equipment. The inertial navigation system is used to acquire inertial navigation data; the inertial navigation data includes the attitude error angle of the inertial navigation system and the drift error of the inertial navigation system; The odometer is used to acquire odometer data; the odometer data includes the position error of the tunneling equipment measured by the odometer, the installation error of the odometer, the scale coefficient error of the odometer, and the output delay time error of the odometer. The optical ranging robot is used to acquire optical ranging robot data; the optical ranging robot data includes the position error measured by the optical ranging robot. The inertial navigation system combined with the odometer constitutes a first combined detection structure; the first combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in a normal state. The inertial navigation system combined with the optical ranging robot constitutes a second combined detection structure; the second combined detection structure is used to determine the spatial pose of the tunneling equipment when the odometer output result is in an abnormal state and the visibility condition of the roadway working face is in a good state. The radar is used to determine the spatial orientation of the tunneling equipment when the odometer output is in an abnormal state and the visibility conditions of the tunnel face are in a poor state.

Images