Tunneling equipment following control method, device, equipment, medium and product

Abstract

The invention discloses a tunneling equipment following control method and device, equipment, a medium and a product, and relates to the field of coal mining, and the method comprises the steps: determining the absolute coordinates of mobile equipment and the relative distance between the mobile equipment and other mobile equipment according to UWB label data through employing a bidirectional distance measurement algorithm; determining a polar coordinate relative pose with other mobile equipment according to the data of the magnetic attraction sensor; performing short-time pose prediction according to the IMU data to obtain motion data of the mobile equipment; determining a distance mode between the mobile devices according to the absolute coordinates of the mobile devices; determining relative pose estimation values of the two mobile devices according to motion data and distance modes of the mobile devices, relative distances between the mobile devices and the other mobile devices and polar coordinate relative poses between the mobile devices and the other mobile devices, and finally performing local path planning based on a set target to obtain a path planning scheme and constraint conditions. According to the invention, the reliability and flexibility can be improved while the cost is reduced.

Description

Technical Field

[0001] This application relates to the field of coal mining, and in particular to a method, device, equipment, medium, and product for following control of tunneling equipment. Background Technology

[0002] In the following process of complete sets of equipment at coal mine tunneling faces, most methods use a single sensor for guidance and magnetic rails for automatic following. However, these methods have many problems. For example, single-sensor guidance (such as UWB or laser) has poor reliability in dusty environments; the reliance on preset tracks or manual intervention for three-machine coordination leads to low flexibility; and traditional magnetic rail navigation requires pre-laying magnetic strips, resulting in high maintenance costs. Currently, there is no effective way to solve these problems. Summary of the Invention

[0003] The purpose of this application is to provide a method, device, equipment, medium, and product for controlling the following of tunneling equipment, which can improve reliability and flexibility while reducing costs.

[0004] To achieve the above objectives, this application provides the following solution: In a first aspect, this application provides a method for following control of tunneling equipment, including: Acquire raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data. The absolute coordinates of the mobile device itself and its relative distance to other mobile devices are determined using a two-way ranging algorithm based on the UWB tag data. The effective sensing range is determined based on the magnetic sensor data, and the polar coordinate relative pose between the magnetic sensor and other mobile devices is determined based on the signal strength distribution. The motion data of the mobile device itself is obtained by performing short-term pose prediction based on the IMU data. The distance mode between mobile devices is determined based on the absolute coordinates of the mobile devices themselves; the distance mode includes long distance mode, medium-to-short distance mode, and extremely close / dating mode; The relative pose estimate of the two mobile devices is determined based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile devices and other mobile devices. Based on the relative pose estimation values ​​of the two mobile devices, a local path planning scheme and constraints are obtained by performing local path planning based on a set target; the path planning scheme and constraints are used to drive each mobile device to follow autonomously.

[0005] In one embodiment, determining the effective sensing range based on the magnetic sensor data and determining the polar coordinate relative pose with other mobile devices based on the signal strength distribution specifically includes: The resultant field strength is determined based on the sensitivity coefficient of the magnetic attraction sensor according to the data from the magnetic attraction sensor. Determine whether the combined field strength is within a preset effective threshold; If so, the polar coordinate relative pose between the magnetic sensor data and other mobile devices is determined using a relative pose calculation algorithm. If not, the data is deemed invalid and "Get raw data from each mobile device" is returned.

[0006] In one embodiment, determining the resultant field strength based on the sensitivity coefficient of the magnetic attraction sensor according to the magnetic attraction sensor data specifically includes: The magnetic sensor data is low-pass filtered and zero-point self-calibrated to obtain the pre-processed voltage value. The preprocessed voltage value is converted into a magnetic field strength value based on the sensitivity coefficient of the magnetic sensor; The combined field strength is obtained by comprehensively processing the magnetic field strength values.

[0007] In one embodiment, determining the polar coordinate relative pose with other mobile devices based on the magnetic sensor data using a relative pose calculation algorithm specifically includes: Based on the magnetic sensor data, the k-nearest neighbor algorithm is used to perform pattern matching in the lookup table to obtain the feature value that is closest to the magnetic sensor. The coordinates corresponding to the feature value closest to the magnetic sensor are used as the polar coordinate relative pose between the magnetic sensor and other mobile devices.

[0008] In one embodiment, determining the relative pose estimate of two mobile devices based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices specifically includes: If the distance mode is a long-distance mode, then the state prediction is performed based on the system dynamics model according to the motion data of the mobile device itself to obtain the state prediction value at the current moment; The relative distance between the two mobile devices is used as an observation value to correct the state prediction value at the current moment, thereby obtaining the relative pose estimation value between the two mobile devices. If the distance mode is a medium-near distance mode, then data fusion is performed based on the relative distance between the two mobile devices and the polar coordinate relative pose between the two mobile devices to obtain the relative pose estimate of the two mobile devices. If the distance mode is extremely close / docking mode, then the polar coordinate relative pose between the two mobile devices is used as the relative pose estimate of the two mobile devices.

[0009] In one embodiment, the set target includes the desired following distance, centerline alignment deviation, and motion constraint parameters.

[0010] Secondly, this application provides a tunneling equipment following control device, comprising: The acquisition module is used to acquire raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data. A two-way ranging module is used to determine the absolute coordinates of the mobile device itself and the relative distance between it and other mobile devices based on the UWB tag data using a two-way ranging algorithm. The effective sensing range determination and relative pose determination module is used to determine the effective sensing range based on the magnetic sensor data and to determine the polar coordinate relative pose between the magnetic sensor and other mobile devices based on the signal strength distribution. The short-time pose prediction module is used to perform short-time pose prediction based on the IMU data to obtain the motion data of the mobile device itself. The distance mode determination module is used to determine the distance mode between mobile devices based on the absolute coordinates of the mobile devices themselves; the distance mode includes a long distance mode, a medium-to-short distance mode, and an extremely close / dating mode; A module for determining the relative pose estimation value of two mobile devices is used to determine the relative pose estimation value of two mobile devices based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices. The local path planning module is used to perform local path planning based on the relative pose estimation values ​​of the two mobile devices and a set target to obtain a path planning scheme and constraints; the path planning scheme and constraints are used to drive each mobile device to follow autonomously.

[0011] Thirdly, this application provides a computer device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the tunneling equipment following control method described above.

[0012] Fourthly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned tunneling equipment following control method.

[0013] Fifthly, this application provides a computer program product, including a computer program that, when executed by a processor, implements the aforementioned tunneling equipment following control method.

[0014] According to the specific embodiments provided in this application, the following technical effects are disclosed: This application provides a method, device, equipment, medium, and product for following control of tunneling equipment. It utilizes multi-source data—IMU data, magnetic sensor data, and UWB tag data—for processing. Then, based on the absolute coordinates of the mobile devices themselves, it determines the distance pattern between mobile devices. Relative pose estimation is performed for different distance patterns to achieve complementary positioning and improve overall accuracy. The magnetic sensor data collection enables reliable operation even in extreme dusty environments. Autonomous following is achieved directly based on multi-source data, avoiding the low flexibility caused by relying on preset tracks or manual intervention in three-machine collaborative systems, and the high maintenance costs of traditional magnetic track navigation requiring pre-laid magnetic strips. This reduces costs and increases flexibility. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of this application 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 this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0016] Figure 1 Here is a flowchart of the polar coordinate relative pose calculation process; Figure 2 This is a diagram of the vehicle control system architecture. Figure 3 A schematic diagram of an automatic following model for tunneling equipment; Figure 4 Flowchart of the tunneling equipment follow-up control method; Figure 5 A schematic diagram of the functional modules of a tunneling equipment following control device provided in an embodiment of this application; Figure 6 This is a schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

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

[0018] Existing solutions lack dynamic obstacle avoidance capabilities and are prone to interrupting operations due to temporary obstacles; collaborative control efficiency is low: equipment relies on manual instructions or fixed path planning, and cannot respond to dynamic changes in the working face in real time; anti-interference capability is weak: traditional wireless communication is easily affected by electromagnetic noise in the well, resulting in delays or loss of control instructions.

[0019] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] In one exemplary embodiment, such as Figure 4 As shown, a method for following and controlling tunneling equipment is provided. This method is executed by a computer device, specifically a terminal or server, or both. In this embodiment, the method is applied to... Figure 1 Taking server 102 as an example, the following steps are included.

[0021] Step 401: Obtain raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data.

[0022] Step 402: Determine the absolute coordinates of the mobile device itself and the relative distance between it and other mobile devices using a two-way ranging algorithm based on the UWB tag data.

[0023] Step 403: Determine the effective sensing range based on the magnetic sensor data and determine the polar coordinate relative pose between the magnetic sensor and other mobile devices based on the signal strength distribution.

[0024] Step 404: Perform short-term pose prediction based on the IMU data to obtain the motion data of the mobile device itself.

[0025] Step 405: Determine the distance mode between mobile devices based on the absolute coordinates of the mobile devices themselves; the distance mode includes long distance mode, medium-to-short distance mode, and very close / dating mode.

[0026] Step 406: Determine the relative pose estimate of the two mobile devices based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices.

[0027] Step 407: Based on the estimated relative pose of the two mobile devices and a set target, perform local path planning to obtain a path planning scheme and constraints; the path planning scheme and constraints are used to drive each mobile device to follow autonomously. In practical applications, the set target includes the desired following distance, centerline alignment deviation, and motion constraint parameters.

[0028] Multi-source data, including IMU data, magnetic sensor data, and UWB tag data, is processed separately. Then, distance patterns between mobile devices are determined based on the absolute coordinates of the mobile devices themselves. Relative pose estimation is performed for different distance patterns to achieve complementary positioning and improve overall accuracy. Data collected by the magnetic sensor enables reliable operation even in extreme dusty environments. Autonomous following is achieved directly based on multi-source data, avoiding the low flexibility caused by relying on preset tracks or manual intervention in three-machine collaborative systems, and the high maintenance costs of traditional magnetic track navigation requiring pre-laid magnetic strips. This reduces costs and increases flexibility.

[0029] In practical applications, raw data from each mobile device is acquired through IMUs, magnetic sensors, and UWB tags. System initialization and data acquisition provide the most raw, multi-dimensional, time-synchronized sensing data for the entire control system.

[0030] like Figure 3 As shown, a UWB base station is fixedly deployed at the top of the tunnel, powered on, and completes self-calibration to form a positioning network. UWB tags installed on mobile devices such as tunneling machines and shuttle cars begin periodically broadcasting signals. Magnetic sensors (receivers) and IMUs on each mobile device are simultaneously powered on and begin collecting raw data. Mobile devices refer to tunneling machines, shuttle cars, and transfer crushers; these devices contain both UWB tags and magnetic sensors. The raw data includes IMU data, magnetic sensor data (voltage signals), and UWB data (the absolute coordinates (X, Y, Z) of the device itself and its relative distance to other device tags).

[0031] As an exemplary embodiment, determining the effective sensing range based on the magnetic sensor data and determining the polar coordinate relative pose with other mobile devices based on the signal strength distribution specifically includes: determining the resultant field strength based on the sensitivity coefficient of the magnetic sensor according to the magnetic sensor data; determining whether the resultant field strength is within a preset effective threshold; if yes, determining the polar coordinate relative pose with other mobile devices using a relative pose calculation algorithm based on the magnetic sensor data; if no, determining the data is invalid and returning "Get the original data on each mobile device".

[0032] Specifically, determining the resultant field strength based on the sensitivity coefficient of the magnetic attraction sensor data includes: performing low-pass filtering and zero-point self-calibration on the magnetic attraction sensor data to obtain a pre-processed voltage value; converting the pre-processed voltage value into a magnetic field strength value based on the sensitivity coefficient of the magnetic attraction sensor; and performing comprehensive processing on the magnetic field strength value to obtain the resultant field strength.

[0033] The polar coordinate relative pose between the magnetic sensor and other mobile devices is determined using a relative pose calculation algorithm based on the magnetic sensor data. Specifically, this includes: using the k-nearest neighbor algorithm to perform pattern matching in a lookup table based on the magnetic sensor data to obtain the feature value that is closest to the magnetic sensor; and using the coordinates corresponding to the feature value that is closest to the magnetic sensor as the polar coordinate relative pose between the magnetic sensor and other mobile devices.

[0034] In practical applications, multi-source data preprocessing and independent computation are performed by the vehicle control unit (VCU) on each mobile device. This VCU transforms raw sensor data with different physical characteristics into unified pose and environmental information with practical physical meaning, preparing for subsequent fusion.

[0035] 1. UWB Data Calculation: The vehicle control unit (VCU) receives signals from the UWB base station and calculates the absolute coordinates (X, Y, Z) of the mobile device itself and the relative distance between it and other mobile device tags through the two-way ranging (TWR) algorithm.

[0036] 2. Magnetic signal processing: such as Figure 1 As shown, the VCU reads the voltage signal from the magnetic sensor. When within the effective sensing range, it calculates the polar coordinate relative pose (distance d and yaw angle θ) between itself and the magnet of the preceding vehicle, and also between itself and other mobile devices, based on the signal strength distribution of the sensor array. Outside the range, this data is invalid.

[0037] ① The VCU reads the voltage value (analog signal) of each Hall element in the magnetic sensor array at a fixed frequency of 100Hz.

[0038] ② Preprocessing: The acquired raw voltage signal is digitally low-pass filtered to suppress interference from high-frequency electronic noise and vibration. Simultaneously, zero-point calibration is performed based on sensor characteristics to eliminate baseline effects from the geomagnetic field and ambient stray magnetic fields.

[0039] ③ The preprocessed voltage value is converted into a magnetic field strength value, in Tesla (T), based on the sensor's sensitivity coefficient S (mV / G). At this point, each Hall element measures a three-dimensional magnetic field vector (Bx, By, Bz) or its component in a specific direction.

[0040] Sensitivity coefficient (S): This is a key parameter of the magnetic sensor, provided by the manufacturer. Its unit is usually mV / G or V / T, representing the change in output voltage corresponding to one unit change in magnetic field. For example, a sensor with a sensitivity of 1.2 mV / G.

[0041] Read the voltage value (V_measured): After low-pass filtering and zero-point calibration, the obtained voltage signal is proportional to the magnetic field strength, and the unit is millivolt (mV) or volt (V).

[0042] Calculate the magnetic field strength (B): Formula: B = V_measured / S.

[0043] Unit conversion: Since 1 G (Gauss) = 10^-4 T (Tesla), if the sensitivity unit is mV / G, the calculated B unit is G, which needs to be multiplied by 10^-4 to convert to the SI unit T.

[0044] Example: Assume the measured voltage is 60 mV and the sensor sensitivity is 1.2 mV / G.

[0045] The magnetic field strength is B = 60 mV / 1.2 mV / G = 50 G.

[0046] Converted to Tesla: ×10^-3 T=5 mT.

[0047] In this way, the voltage value output by each Hall element is converted into a magnetic field strength value (Bx, By, Bz) with a clear physical meaning (Tesla), providing a standardized input for subsequent calculation of the combined field strength and mode matching.

[0048] ④ Perform comprehensive processing on the data from the entire sensor array. For example, the resultant field strength (B_total=√(Bx)) can be calculated. 2 +By 2 +Bz 2 Its size is strongly correlated with the distance d.

[0049] ⑤ By analyzing the differences in the measurements of different sensor nodes in the array, the approximate direction of the magnet can be determined. The determined "direction" information is the key input for calculating the angle (yaw angle θ) part of the "polar coordinate relative pose".

[0050] The magnetic field generated by a magnet has a specific vector distribution in space. In a sensor array, sensor nodes at different spatial locations will measure magnetic field vectors of different magnitudes and directions. By comparing the differences in these vectors (e.g., which node has the largest resultant field strength, the sign and proportional relationship of the Bx and By components of each node), the approximate orientation of the magnet relative to the sensor array plane can be inferred.

[0051] The input for lookup table matching is this "general direction" information, which, combined with the magnetic field strength values ​​of each node, forms a feature vector. This feature vector is then used to match the data against a pre-defined lookup table (e.g., using the k-NN algorithm).

[0052] Outputting precise pose: The lookup table stores a one-to-one mapping between (distance d, yaw angle θ) coordinates and sensor feature vectors. Therefore, through feature matching, the final output is precisely the polar coordinate relative pose (d, θ).

[0053] Its role in fusion and planning: This (d,θ) value, in medium-to-close and very-close distance modes, directly serves as or participates in forming the "relative pose estimate of the two moving vehicles". The path planning module uses this pose estimate, which includes distance and angle, to calculate how the following vehicle should adjust its position and heading to maintain the desired formation (such as centerline alignment) with the preceding vehicle.

[0054] ⑥ Determine whether the combined field strength B_total exceeds the preset effective threshold; if B_total < threshold, it is considered that the magnet has not yet entered the effective sensing range, the data of this frame is deemed invalid, no further calculation is performed, and the next frame of data is waited for; if B_total ≥ threshold, it is considered that the magnet has entered the effective range, and the relative pose calculation algorithm is triggered.

[0055] ⑦ In the laboratory, a huge lookup table was built, which stores the one-to-one mapping relationship between position coordinates (d, θ) and sensor feature values.

[0056] ⑧ When the system is running, after the VCU reads the feature value of the current sensor, it uses the k-nearest neighbor (k-NN) algorithm to perform pattern matching in this pre-calibrated lookup table, and finds one or more sets of feature values ​​in the lookup table that are closest to the current sensor reading. The corresponding (d,θ) coordinates are the calculated relative pose.

[0057] 3. IMU Data Processing: The VCU integrates the accelerometer and gyroscope data from the IMU to calculate the instantaneous velocity, acceleration, and heading angle changes of the device, which are used for short-term attitude estimation.

[0058] In an exemplary embodiment, determining the relative pose estimate of two mobile devices based on the motion data of the mobile device itself, the distance mode, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices specifically includes: if the distance mode is a long-distance mode, then performing state prediction based on the motion data of the mobile device itself using a system dynamics model to obtain the state prediction value at the current moment; correcting the state prediction value at the current moment using the relative distance between the mobile device and other mobile devices as an observation value to obtain the relative pose estimate of the two mobile devices; if the distance mode is a medium-near distance mode, then performing data fusion based on the relative distance between the mobile device and other mobile devices and the polar coordinate relative pose between the mobile device and other mobile devices to obtain the relative pose estimate of the two mobile devices; if the distance mode is a very close / dock mode, then using the polar coordinate relative pose between the mobile device and other mobile devices as the relative pose estimate of the two mobile devices.

[0059] Relative pose estimation based on fusion algorithms: The multi-source information fusion algorithm in the vehicle control unit (VCU) is implemented based on a Kalman filter. Data sources include UWB, IMU, and magnetic sensor data. The accelerometer output is the specific force (including gravitational acceleration) of the device in the carrier coordinate system. Gravity interference is first eliminated, and then velocity and displacement are obtained through Simpson integration. The core algorithm revolves around "integral error suppression" to avoid the cumulative drift affecting short-term pose accuracy.

[0060] 1. State Prediction: Based on the optimal estimate of the previous moment and the motion data provided by the IMU, the filter predicts the relative position and speed state of the vehicle and the vehicle in front at the current moment; the motion data includes instantaneous speed, acceleration and heading angle changes.

[0061] ① Based on the system dynamics model: Kalman filter state prediction is based on the system's dynamics model, and its discrete-time model state equation is: Among them, x k It is the state vector at time k, containing information such as the relative position and speed of the vehicle and the vehicle in front; x k-1 It is the state vector of the previous time step k-1, i.e., the optimal estimate of the previous time step; F k It is the system state transition matrix, which describes how the state transitions from one point in time to the next. For the motion of the vehicle, F k It can be determined based on kinematic principles, for example, in a uniform motion model, F k It can be represented as Where Δt is the time interval; B k It is the control matrix, u k It is a control input; when using IMU data for prediction, the acceleration data provided by the IMU can be used as the control input u.k B k This reflects the impact of these control inputs on the state; w k It is process noise, which represents the uncertainty factors present in the system. It is usually assumed to have a mean of 0 and a covariance matrix of Q. k The Gaussian distribution.

[0062] ② State prediction calculation: Based on the above state equations, the Kalman filter calculates the state prediction using the formula... To calculate the predicted state at the current time. .in It is the updated optimal state estimate from the previous moment, obtained by comparing the optimal estimate from the previous moment with the motion data provided by the IMU (i.e., the control input u). k By combining these factors and multiplying them by the corresponding matrix, we can obtain the predicted state value at the current moment.

[0063] 2. Observation Update: (1) Long-distance mode: mainly uses the relative distance provided by UWB as the observation value to correct the predicted state.

[0064] ① First, establish the observation equations to relate the relative distance measured by UWB to the system state vector. The general form of the observation equations is: , where z k h(x) is the observation value at time k, i.e., the relative distance between the vehicle and the vehicle in front as measured by UWB; k ) is the observation function, which is based on the state vector x. k Calculate the theoretical observed value; v k This is observation noise, typically assumed to have a mean of 0 and a covariance matrix of R. k The Gaussian distribution.

[0065] ②Kalman gain K k The formula used to determine the degree of correction of the observed values ​​to the predicted state is as follows: ,in It is the prediction error covariance matrix. It is the observation function h(x) k Regarding the state vector x k The Jacobian matrix describes the degree to which state changes affect the observations. For observing noise v k The covariance matrix.

[0066] ③Measure the residual y k It is the difference between the actual observed value and the predicted observed value, calculated using the following formula: ,in It is the predicted state vector.

[0067] ④ Update the predicted state based on the measurement residuals and Kalman gain to obtain a more accurate state estimate. Gain and residual, used to estimate the latest state value, were described in the previous state prediction section, where x... k It is the state vector at time k, containing information such as the relative position and speed of the vehicle and the vehicle in front. The update covariance matrix is ​​the prediction error covariance matrix.

[0068] ⑤ Update the error covariance matrix to reflect the uncertainty of the updated state estimate, using the following formula: , where I is the identity matrix.

[0069] (2) Medium-near distance mode: The relative distance of UWB and the extremely high-precision relative distance and angle provided by the magnetic sensor are used as observation values ​​to perform data fusion and obtain the optimal estimate.

[0070] (3) Extremely close / docking mode: The error of UWB signal is large within 1m. At this time, the observation value of magnetic sensor is completely trusted to achieve accurate positioning for docking.

[0071] 3. The filter continuously cycles through the "prediction-update" steps to output a stable, smooth, high-precision, and real-time estimate of the relative pose of the two vehicles (including distance, azimuth, relative speed, etc.).

[0072] The vehicle control unit (VCU) is responsible for path planning and motion control.

[0073] 1. Set target: The system presets a safe desired following distance and a formation where the center lines of the two devices are aligned.

[0074] (1) Expected following distance D0: The safe distance between the front end of the rear car (or key components, such as the shuttle car bucket) and the rear end of the front car (or the feed inlet of the transfer crusher) is the core benchmark for collision prevention. The shuttle car is 10m long, so the minimum safe distance is not less than 2m; Motion inertia: When the shuttle car is heavily loaded (8 tons of coal), the braking distance is 1m, and a buffer needs to be reserved; Typical value: Normal following distance D13m, docking stage (such as the shuttle car unloading coal to the transfer machine) D2=0.3-0.5m.

[0075] (2) Centerline alignment deviation ΔL: The maximum allowable horizontal offset between the centerline of the rear vehicle and the centerline of the front vehicle, ensuring that the equipment travels along the centerline of the roadway and avoids scraping. Roadway wall and roadway width: The roadway width at the tunneling face is 4-6m, and the equipment width is 3m, so ΔL≤0.5m; - Docking accuracy: Strict alignment is required during coal unloading / transfer, ΔL≤0.1m (10cm).

[0076] (3) Motion constraint parameters: These are the physical boundaries that limit the movement of the equipment, preventing mechanical damage or instability. Maximum speed v axTunneling machine: 0.1-0.3 m / s, shuttle car: 0.5-1 m / s (30% reduction under heavy load); maximum acceleration a ax ±0.1m / s² (to avoid hydraulic system shock); Maximum steering angle θ ax ±30° (to prevent wheel slippage or track deviation).

[0077] These three parameters are hard constraints in the local path planning and control quantity calculation stages, ensuring that the planned path and generated instructions are physically executable and safe.

[0078] In path planning (Model Predictive Control, MPC): When optimization algorithms solve for the optimal control sequence (vehicle speed, steering angle) over a future time period, they must satisfy these constraints. For example, the planned speed curve cannot exceed v. ax The change in acceleration cannot exceed a ax The planned turning radius corresponds to a steering angle that cannot exceed θ. ax .

[0079] These constraints are incorporated into the mathematical model of the MPC optimization problem as inequality constraints, ensuring that the solved path is feasible in terms of equipment dynamics.

[0080] In control quantity calculation (VCU): Even if the planning module provides the theoretical target speed and steering angle, the final command output still needs to be limited.

[0081] For example, the target speed command calculated by the VCU will be compared with the target speed command before being sent to the underlying actuators (hydraulic valves, motors). ax and a ax Comparison and limiting are performed to ensure that commands do not cause the equipment to overspeed or accelerate / decelerate suddenly.

[0082] Similarly, the calculated steering angle command will also be limited to ±θ. ax Within.

[0083] In summary, these three parameters are the bridge between "ideal planning" and "safe execution" of the system, preventing equipment damage, instability, or slippage caused by excessive control commands.

[0084] 2. Path generation: The controller uses the real-time relative pose output by the fusion algorithm as input, and combines it with the movement trend of the preceding vehicle to plan a local path that allows the following vehicle to approach smoothly and safely while maintaining the target distance.

[0085] MPC is an existing and advanced closed-loop optimization control strategy, but this patent applies it to this specific scenario and combines it with multi-source sensing data to form an innovative specific implementation scheme.

[0086] The specific process of local path planning based on MPC: Suppose that the following vehicle needs to plan its own movement based on its relative position (d, θ) with the vehicle in front and the movement trend of the vehicle in front.

[0087] Establish a predictive model: The controller contains a kinematic or dynamic model of the vehicle behind it. This is a simplified mathematical equation that describes how the vehicle's state (position, speed, heading angle) changes with control inputs (acceleration, front wheel steering angle).

[0088] Define the optimization objective: Objective functions typically include: Tracking error minimization: Minimize the deviation between the endpoint of the predicted trajectory and the target pose of "expected following distance" and "centerline alignment".

[0089] Smoothness of control parameters: To make changes in acceleration and steering angle as gradual as possible, thereby improving ride comfort and equipment lifespan.

[0090] Matching the relative speed with the vehicle in front: making the speed of the following vehicle approach the speed of the vehicle in front, and maintaining a stable following.

[0091] Apply constraints: The v mentioned above ax a ax θ ax It is added to the optimization problem as a hard constraint.

[0092] Scrolling optimization and feedback: In each control cycle (e.g., 50ms): Measurement: Obtain the latest "relative pose estimate of the two mobile devices" (from the fusion algorithm) as the current state.

[0093] Prediction: Based on the current state and the prediction model, predict the state evolution of the system under different control input sequences within a finite time window (e.g., 2 seconds) in the future.

[0094] Optimization: Solve a constrained optimization problem by finding a set of future control input sequences such that the predicted state best matches the optimization objective under the action of these sequences.

[0095] Execution: Only the first control instruction (i.e. the optimal control quantity at the current moment, such as target speed and steering angle) in the optimized control sequence is taken and sent to the underlying actuator.

[0096] In the next cycle, the above process is repeated, and optimization is performed again based on the new measurements. This "rolling optimization" feature allows MPC to continuously correct the control based on real-time feedback, giving it a strong ability to adapt to the dynamic changes of the preceding vehicle and system uncertainties.

[0097] (1) The core is “local path planning” (planning cycle 50-100ms, covering the movement trajectory in the next 1-2 seconds), rather than global path (underground roadways are fixed and do not require global planning).

[0098] (2) The mainstream solution in coal mine scenarios is the model predictive control (MPC) path planning algorithm.

[0099] 3. Control Calculation: The VCU calculates a series of optimal control commands based on the planned path and constraints (maximum speed, acceleration), mainly including the target speed calculated by the position PID controller and the target steering angle controlled by the PID controller based on the "left and right offset and heading angle error".

[0100] Command execution and vehicle drive.

[0101] like Figure 2 As shown, the VCU converts the generated target speed and control quantity into specific analog quantities or bus commands (CAN bus messages); the VCU communicates with the underlying controller (hydraulic valve controller, motor frequency converter) through the CAN bus (strong anti-interference in coal mines, transmission rate 250kbps-1Mbps), and needs to convert the "target speed / steering angle" into "control messages" that conform to the bus protocol. At the same time, some analog quantity drives need to undergo additional "digital-to-analog conversion".

[0102] These commands are sent to the vehicle's underlying controllers (hydraulic valves, servo motors, frequency converters), driving the hydraulic system to change the oil volume or the motor to change the speed / torque, thereby controlling the vehicle to accelerate, decelerate, or steer as required by the commands. The underlying controllers (hydraulic valve controllers, motor frequency converters) act as a bridge between "commands" and "actions," requiring precise execution based on the equipment's drive method (hydraulic / motor). The core principle is "closed-loop control" (ensuring that the actual action matches the target command). The vehicle control unit (VCU) controls the hydraulic valves, servo motors, and frequency converters via analog signals, digital signals, and the CAN bus.

[0103] Table 1. Parameters for Communication Methods Between Devices

[0104] This application provides a multi-source sensing fusion technology based on UWB positioning base stations, UWB tags, magnetic sensors, and IMUs (Inertial Measurement Units) to achieve autonomous following control of complete sets of equipment (such as tunneling machines, shuttle cars, and transfer crushers) in coal mine tunneling faces. This application has the following advantages: 1. Complementary near and long range, high precision throughout: Long range: UWB provides stable global positioning. Short range: Magnetic sensors are unaffected by electromagnetic fields, dust, or obstructions, providing centimeter-level or even millimeter-level relative pose information, perfectly solving the accuracy problem of the "last meter".

[0105] 2. The magnetic sensor can still work reliably even in extreme conditions where dust is everywhere and visibility is completely obscured.

[0106] 3. UWB and magnetic sensors provide continuous external absolute and relative observations, which can correct the IMU's calculation errors in real time and form the optimal estimate.

[0107] 4. The magnetic sensor can still work reliably in the closest range where UWB fails, avoiding the risk of collision due to sensor failure.

[0108] 5. Multiple sensors provide mutual backup. Even if UWB temporarily fails, the combination of IMU, magnetic sensor, and radar can still support the safe parking of the vehicle, ensuring that all sensors will not fail simultaneously, greatly improving the system's fault tolerance and safety level.

[0109] 6. Once the UWB base station is fixed, the travel path of the preceding vehicle (tunneling machine) is the same as the path of the following vehicle (shuttle car), without any change to the physical track.

[0110] 7. "One-click start" allows the following vehicle to automatically follow the vehicle in front, reducing or even eliminating the need for manual intervention.

[0111] 8. The system has a fast response speed, precise actions, maximizes the efficiency of inter-device collaboration, seamlessly connects, significantly shortens cycle operation time, and improves operation efficiency.

[0112] Based on the same inventive concept, this application also provides a tunneling equipment following control device for implementing the tunneling equipment following control method described above. The solution provided by this device is similar to the solution described in the above method; therefore, the specific limitations of one or more tunneling equipment following control device embodiments provided below can be found in the limitations of the tunneling equipment following control method described above, and will not be repeated here.

[0113] In one exemplary embodiment, such as Figure 5 As shown, a tunneling equipment following control device is provided, comprising: The acquisition module is used to acquire raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data.

[0114] The bidirectional ranging module is used to determine the absolute coordinates of the mobile device itself and the relative distance between it and other mobile devices based on the UWB tag data using a bidirectional ranging algorithm.

[0115] The effective sensing range determination and relative pose determination module is used to determine the effective sensing range based on the magnetic sensor data and to determine the polar coordinate relative pose between the magnetic sensor and other mobile devices based on the signal strength distribution.

[0116] The short-time pose prediction module is used to perform short-time pose prediction based on the IMU data to obtain the motion data of the mobile device itself.

[0117] The distance mode determination module is used to determine the distance mode between mobile devices based on the absolute coordinates of the mobile devices themselves; the distance mode includes long distance mode, medium-to-short distance mode and very close / dating mode.

[0118] The relative pose estimation module for two mobile devices is used to determine the relative pose estimation value of two mobile devices based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices.

[0119] The local path planning module is used to perform local path planning based on the relative pose estimation values ​​of the two mobile devices and a set target to obtain a path planning scheme and constraints; the path planning scheme and constraints are used to drive each mobile device to follow autonomously.

[0120] In one exemplary embodiment, a computer device is provided, which may be a server or a terminal, and its internal structure diagram may be as follows. Figure 6As shown, the computer device includes a processor, memory, input / output (I / O) interfaces, and a communication interface. The processor, memory, and I / O interfaces are connected via a system bus, and the communication interface is also connected to the system bus via the I / O interfaces. The processor provides computational and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system, computer programs, and a database. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The database stores tunneling equipment following control data. The I / O interfaces are used for exchanging information between the processor and external devices. The communication interface is used for communicating with external terminals via a network connection. When the computer program is executed by the processor, it implements a tunneling equipment following control method.

[0121] Those skilled in the art will understand that Figure 6 The structures shown are merely block diagrams of some structures related to the present application and do not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than shown in the figures, or combine certain components, or have different component arrangements. In an exemplary embodiment, a computer device is provided, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the above-described method embodiments.

[0122] In one exemplary embodiment, a computer-readable storage medium is provided storing a computer program that, when executed by a processor, implements the above-described method embodiments.

[0123] In one exemplary embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the above-described method embodiments.

[0124] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0125] In this application, all actions to acquire signals, information, or data are carried out in compliance with the relevant data protection laws and policies of the country where the location is situated, and with the authorization granted by the owner of the relevant device.

[0126] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments described above. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0127] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0128] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0129] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, 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 this application.

Claims

1. A method for following control of tunneling equipment, characterized in that, The tunneling equipment following control method includes: Acquire raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data. The absolute coordinates of the mobile device itself and its relative distance to other mobile devices are determined using a two-way ranging algorithm based on the UWB tag data. The effective sensing range is determined based on the magnetic sensor data, and the polar coordinate relative pose between the magnetic sensor and other mobile devices is determined based on the signal strength distribution. The motion data of the mobile device itself is obtained by performing short-term pose prediction based on the IMU data. The distance mode between mobile devices is determined based on the absolute coordinates of the mobile devices themselves; the distance mode includes long distance mode, medium-to-short distance mode, and extremely close / dating mode; The relative pose estimate of the two mobile devices is determined based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile devices and other mobile devices. Based on the relative pose estimation values ​​of the two mobile devices, a local path planning scheme and constraints are obtained by performing local path planning based on a set target; the path planning scheme and constraints are used to drive each mobile device to follow autonomously.

2. The tunneling equipment following control method according to claim 1, characterized in that, The effective sensing range is determined based on the magnetic sensor data, and the polar coordinate relative pose between the sensor and other mobile devices is determined based on the signal strength distribution. Specifically, this includes: The resultant field strength is determined based on the sensitivity coefficient of the magnetic attraction sensor according to the data from the magnetic attraction sensor. Determine whether the combined field strength is within a preset effective threshold; If so, the polar coordinate relative pose between the magnetic sensor data and other mobile devices is determined using a relative pose calculation algorithm. If not, the data is deemed invalid and "Get raw data from each mobile device" is returned.

3. The tunneling equipment following control method according to claim 2, characterized in that, The resultant field strength is determined based on the sensitivity coefficient of the magnetic attraction sensor according to the magnetic attraction sensor data, specifically including: The magnetic sensor data is low-pass filtered and zero-point self-calibrated to obtain the pre-processed voltage value. The preprocessed voltage value is converted into a magnetic field strength value based on the sensitivity coefficient of the magnetic sensor; The combined field strength is obtained by comprehensively processing the magnetic field strength values.

4. The tunneling equipment following control method according to claim 2, characterized in that, Based on the magnetic sensor data, a relative pose calculation algorithm is used to determine the polar coordinate relative pose with other mobile devices, specifically including: Based on the magnetic sensor data, the k-nearest neighbor algorithm is used to perform pattern matching in the lookup table to obtain the feature value that is closest to the magnetic sensor. The coordinates corresponding to the feature value closest to the magnetic sensor are used as the polar coordinate relative pose between the magnetic sensor and other mobile devices.

5. The tunneling equipment following control method according to claim 1, characterized in that, The relative pose estimate of the two mobile devices is determined based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile devices and other mobile devices. Specifically, this includes: If the distance mode is a long-distance mode, then the state prediction is performed based on the system dynamics model according to the motion data of the mobile device itself to obtain the state prediction value at the current moment; The relative distance between the two mobile devices is used as an observation value to correct the state prediction value at the current moment, thereby obtaining the relative pose estimation value between the two mobile devices. If the distance mode is a medium-near distance mode, then data fusion is performed based on the relative distance between the two mobile devices and the polar coordinate relative pose between the two mobile devices to obtain the relative pose estimate of the two mobile devices. If the distance mode is extremely close / docking mode, then the polar coordinate relative pose between the two mobile devices is used as the relative pose estimate of the two mobile devices.

6. The tunneling equipment following control method according to claim 1, characterized in that, The set targets include the desired following distance, centerline alignment deviation, and motion constraint parameters.

7. A tunneling equipment following control device, characterized in that, The tunneling equipment following control device includes: The acquisition module is used to acquire raw data from each mobile device; the raw data includes IMU data, magnetic sensor data, and UWB tag data. A two-way ranging module is used to determine the absolute coordinates of the mobile device itself and the relative distance between it and other mobile devices based on the UWB tag data using a two-way ranging algorithm. The effective sensing range determination and relative pose determination module is used to determine the effective sensing range based on the magnetic sensor data and to determine the polar coordinate relative pose between the magnetic sensor and other mobile devices based on the signal strength distribution. The short-time pose prediction module is used to perform short-time pose prediction based on the IMU data to obtain the motion data of the mobile device itself. The distance mode determination module is used to determine the distance mode between mobile devices based on the absolute coordinates of the mobile devices themselves; the distance mode includes a long distance mode, a medium-to-short distance mode, and an extremely close / dating mode; A module for determining the relative pose estimation value of two mobile devices is used to determine the relative pose estimation value of two mobile devices based on the motion data of the mobile device itself, the distance pattern, the relative distance between the mobile device and other mobile devices, and the polar coordinate relative pose between the mobile device and other mobile devices. The local path planning module is used to perform local path planning based on the relative pose estimation values ​​of the two mobile devices and a set target to obtain a path planning scheme and constraints; the path planning scheme and constraints are used to drive each mobile device to follow autonomously.

8. A computer device, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that the processor executes the computer program to implement the tunneling equipment following control method according to any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the tunneling equipment following control method as described in any one of claims 1-6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the tunneling equipment following control method as described in any one of claims 1-6.

Images