Subway foundation pit robot group coordination and obstacle avoidance method based on hydraulic body perception

Abstract

The present application relates to the field of underground engineering machinery control, in particular to a subway foundation pit robot group coordination and obstacle avoidance method based on hydraulic body perception, comprising real-time collection of hydraulic load, body vibration and working arm spread of the excavator; based on this dynamic calculation of potential energy field safety radius, adaptive safety boundary is constructed; through UWB positioning to obtain the distance between devices, the potential energy interference depth is calculated, and arbitration is carried out based on the potential energy density representing the working intensity to determine the master working machine and the slave avoiding machine; for the slave avoiding machine, the damping coefficient is calculated according to the interference depth, and the hydraulic main pump swash plate inclination is directly adjusted to control the flow, realizing flexible avoidance, getting rid of the dependence on external sensors such as vision, and adapting to the harsh underground environment; through dynamic potential energy field and physical intelligent arbitration, the problems of static safety distance limitation and unreasonable road right allocation are solved; through the bottom layer hydraulic flow admittance control, non-impact flexible obstacle avoidance is realized, and the working safety, continuity and equipment life are improved.

Description

Technical Field

[0001] This invention relates to the field of underground engineering machinery control technology, specifically, a method for collaborative obstacle avoidance and obstacle swarming of subway foundation pit robots based on hydraulic body perception. Background Technology

[0002] In confined underground spaces such as subway foundation pit excavation and mine tunneling, multiple hydraulic excavating robots need to perform high-density collaborative operations in narrow, unstructured environments. These working environments are often accompanied by extreme conditions such as high concentrations of dust, diffuse water mist, and severely insufficient lighting, posing a serious challenge to the robots' environmental perception and autonomous collaborative capabilities.

[0003] Currently, robot obstacle avoidance and swarm scheduling technologies applied to such scenarios are mainly based on the following types of solutions, but all of them have significant drawbacks:

[0004] 1. Obstacle avoidance methods based on external environment perception: Mainstream methods rely on external sensors such as visual cameras, LiDAR, and millimeter-wave radar to construct environmental maps and detect obstacles. However, in the harsh working conditions of underground environments with high dust and humidity, optical sensors are prone to failure due to mirror contamination, signal attenuation, or complete blockage; millimeter-wave radar, limited by resolution, struggles to accurately identify the complex robotic arm shapes of nearby equipment. Furthermore, these methods require significant computing resources for real-time point cloud or image processing, resulting in large response delays and high false alarm rates in the dynamically changing underground environment, failing to meet the real-time requirements of millisecond-level safety collaboration.

[0005] 2. Collision avoidance strategy based on fixed safety zones: Many existing industrial control systems set a static, geometrically fixed safety envelope (such as a cylinder or sphere) as the collision avoidance boundary for each piece of equipment. While this method is simple and reliable, it lacks adaptability: when the equipment is performing high-load, tackling operations (such as breaking hard rock), the hydraulic system pressure increases sharply, causing severe vibration and attitude drift in the machine body. The fixed safety boundary cannot accommodate this dynamic uncertainty, easily leading to actual collisions. Conversely, under low-load or no-load conditions, an excessively large safety boundary will seriously waste the already limited working space, reducing the overall operational efficiency of the cluster. This "one-size-fits-all" strategy is essentially a static compromise between space utilization and safety redundancy, and it cannot adapt to the complex and variable geological loads underground.

[0006] 3. Collaborative methods based on preset rules or centralized scheduling: Some systems employ pre-planned paths, time-division multiplexing, or central control tower scheduling to avoid conflicts. These methods lack awareness and response to the real-time operational status of equipment. For example, common "first-come, first-served" or distance-based avoidance rules may force equipment performing critical, high-load, and uninterrupted operations (such as rock penetration or precision excavation) to give way to low-priority tasks (such as idle rotation or movement), causing work interruptions, energy waste, and even hydraulic shocks or equipment jamming due to sudden unloading. Centralized scheduling, on the other hand, carries the risk of single-point failure, and the stability of wireless communication underground is difficult to guarantee.

[0007] Therefore, existing technologies have not been able to effectively solve the three core challenges of collaborative operation of downhole robot swarms: "lack of reliable external sensing," "mismatch between dynamic and static safety boundaries," and "intelligent priority arbitration based on physical state." This invention addresses these bottlenecks by proposing a novel collaborative obstacle avoidance method that does not rely on external environmental sensing, but rather on the equipment's own hydraulic and vibration signals to achieve dynamic boundary adjustment and physical intelligent arbitration. Summary of the Invention

[0008] The purpose of this invention is to provide a method for collaborative obstacle avoidance and obstacle swarming of subway pit robots based on hydraulic body perception, so as to solve the problems mentioned in the background art.

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

[0010] A method for collaborative obstacle avoidance of a swarm of robots in a subway foundation pit based on hydraulic body perception includes the following steps:

[0011] S1. Real-time collection of hydraulic load pressure, body vibration intensity and boom extension of a single excavator;

[0012] S2. Based on the hydraulic load pressure, machine vibration intensity and boom extension, dynamically calculate the safety radius of the potential energy field of the excavator and construct a dynamic safety boundary that changes with the working conditions.

[0013] S3. Share location information and potential energy field safety radius among multiple excavators, calculate the potential energy interference depth between devices, and arbitrate based on their respective calculated potential energy densities to determine the main excavator and the secondary avoidance excavator.

[0014] S4. For excavators identified as needing to avoid an obstacle, calculate the obstacle avoidance damping coefficient based on the potential energy interference depth, and adjust the swashplate angle of the hydraulic main pump accordingly to control the output flow rate and achieve flexible obstacle avoidance action.

[0015] As a further aspect of the present invention: step S1 specifically includes:

[0016] S11. Hydraulic pressure signals are collected by pressure sensors installed in the boom cylinder and stick cylinder. and After filtering, the comprehensive excavation load is calculated according to preset weighting coefficients. ;

[0017] S12. The vertical axial acceleration is collected by an inertial measurement unit installed on the fuselage, and the effective value of vibration is calculated after filtering. As the intensity of fuselage vibration;

[0018] S13. Obtain the rotation angle by means of an angle sensor installed at the joint between the boom and the stick. and Combined with the known boom length Length of the bucket pole Calculate the horizontal working boom extension of the bucket teeth relative to the center of rotation. .

[0019] As a further aspect of the present invention: in step S2, the safe radius of the potential energy field... The calculation formula is:

[0020] ;

[0021] in, Based on the radius of gyration, For the system's ultimate pressure, and These are the load gain coefficient and vibration gain coefficient obtained through on-site calibration, respectively.

[0022] As a further aspect of the present invention: step S3 specifically includes:

[0023] S31. Based on UWB positioning technology, obtain the coordinates of the rotation center of each excavator and calculate the center distance between any two machines. And combined with their respective potential energy field safety radii and Through formula Calculate the depth of potential energy interference ,when A conflict is determined when the value is greater than 0;

[0024] S32. Calculate the potential energy density of each device. , ,in, For load percentage, As a vibration correction factor, compare the potential energy densities of the two devices involved in the collision; if > Then equipment A is the master machine and equipment B is the slave machine; if < Then equipment B is the master machine and equipment A is the slave machine; if = At this point, based on "operational continuity," the machine with the longer continuous operation time is designated as the master machine, and the other machine is designated as the pass-through machine. If the continuous operation time is the same, the machines are sorted by their numbers from smallest to largest, with the machine with the smaller number being the master machine.

[0025] As a further aspect of the present invention: step S4 specifically includes:

[0026] S41. Based on the depth of potential energy interference and preset buffer distance Calculate the avoidance damping coefficient The formula is = Its value ranges from 0 to 1;

[0027] S42. Correct the desired flow rate command based on the avoidance damping coefficient. Receive actual traffic instructions = and will This is converted into a control signal to adjust the proportional solenoid valve of the hydraulic master pump, changing the swashplate angle and thus controlling the output flow.

[0028] As a further aspect of the present invention: when ≥ hour, =1, the equipment stops operating; with Decrease Decrease As the volume increases, the equipment's operating speed gradually recovers.

[0029] A collaborative control system for underground robot swarms to implement the hydraulic body perception-based collaborative obstacle avoidance method for subway pit robot swarms as described above, comprising:

[0030] The sensing module, including a pressure sensor, an inertial measurement unit, an angle sensor, and a UWB positioning tag, is used to collect information on hydraulic pressure, machine vibration, joint angles, and equipment position.

[0031] The vehicle controller is connected to the sensing module and is used to execute steps S1 to S4 in the above method to realize data calculation, potential energy field construction, conflict arbitration and generate control commands.

[0032] The execution module includes a proportional solenoid valve and a hydraulic master pump connected to the vehicle controller, used to receive control commands and adjust the swashplate tilt angle to control the hydraulic flow output.

[0033] As a further aspect of the present invention: the control cycle of the vehicle controller is 20ms, and the data transmission delay of the sensing module does not exceed 5ms.

[0034] An underground engineering machine is equipped with the aforementioned underground robot swarm collaborative control system.

[0035] Compared with the prior art, the beneficial effects of the present invention are:

[0036] 1. Strong environmental robustness: It relies entirely on the robot's own perception (hydraulic pressure, vibration), eliminating the dependence on external optical sensors that are susceptible to the harsh underground environment, and achieving reliable obstacle avoidance in "blind environments".

[0037] 2. Adaptive safety boundary: The safety radius is dynamically adjusted according to load, vibration and boom extension, which solves the "space-dynamic mismatch" problem of static safety distance under alternating soft and hard geological conditions, and takes into account both space utilization and safety.

[0038] 3. Intelligent physical arbitration: Right-of-way arbitration is based on potential energy density (coupling of load and vibration), which ensures the continuity of high-load tackling operations, avoids irrelevant interruptions to key processes, and improves overall energy efficiency.

[0039] 4. Flexible obstacle avoidance without impact: By directly adjusting the tilt angle of the hydraulic main pump swashplate to control the speed of the equipment, the obstacle avoidance process exhibits a linear speed decay, eliminating the hydraulic shock and mechanical stress caused by sudden stops, protecting the equipment, and improving the smoothness of operation. Attached Figure Description

[0040] Figure 1 This is the overall control flowchart of the method of the present invention.

[0041] Figure 2 A schematic diagram illustrating the principle of constructing a potential energy field model for a single-machine hydraulic load.

[0042] Figure 3 This is a schematic diagram illustrating the scenario of multi-machine potential field interference detection and physical game arbitration principle.

[0043] Figure 4 This is a curve showing the flow control characteristics of the hydraulic main pump swashplate based on the depth of interference.

[0044] Figure 5 The system hardware composition and signal flow diagram for implementing the method of the present invention. Detailed Implementation

[0045] The technical solution of this application will be further described in detail below with reference to specific embodiments.

[0046] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain this application, and should not be construed as limiting this application.

[0047] This invention is executed entirely by the on-board controller (ECU) installed on each excavator, with the control cycle strictly set to 20ms to ensure the real-time performance of data acquisition, calculation and control command output, adapting to the dynamic response requirements of multi-device collaborative operation in underground confined spaces.

[0048] Please see Figure 1 In one embodiment of the present invention, a method for collaborative obstacle avoidance of a subway pit robot swarm based on hydraulic body perception includes the following steps:

[0049] S1: Multidimensional physical quantity acquisition and feature calculation

[0050] The core of this step is to acquire key physical parameters during the excavator's operation using various high-precision sensors, clarify the data acquisition source, processing standards, and feature extraction logic, and provide objective and reliable input data for the subsequent potential energy field construction. The acquisition process is synchronized with the control cycle, and the data transmission delay does not exceed 5ms.

[0051] S11: Hydraulic load acquisition

[0052] First, install one high-precision pressure sensor (requirements: range 0-40MPa, measurement accuracy) in the rodless chamber of the boom cylinder and the rodless chamber of the stick cylinder of the excavator. The FS sensor (with a response time ≤1ms and an IP67 protection rating, suitable for damp and dusty underground working environments) is sealed to the cylinder cavity via a hydraulic connector to ensure no pressure signal leakage. The boom cylinder pressure sensor is used to collect the hydraulic pressure signal corresponding to the load borne by the boom during excavation operations in real time. The boom cylinder pressure sensor collects the hydraulic pressure signal corresponding to the load borne by the boom. The data acquisition frequency is consistent with the controller cycle, i.e., data is acquired once every 20ms. The acquired raw pressure signal needs to be preprocessed by the ECU's built-in low-pass filter module (cutoff frequency 5Hz) to filter out high-frequency interference signals caused by instantaneous impacts in the hydraulic system, ensuring data stability. Subsequently, based on the mechanical characteristics of the excavation operation, a weighted summation method is used to calculate the comprehensive excavation load. The calculation formula is:

[0053] ;

[0054] The weighting coefficients of 0.6 and 0.4 are not fixed values, but were determined through field tests of more than 100 different working conditions (including soft soil excavation, hard rock tackling, and no-load slewing). The core basis is that the boom bears the main lifting and support load in excavation operations, accounting for about 60% of the force, while the stick assists in excavation and material pushing, accounting for about 40% of the force. This weighting method can accurately reflect the actual operating load status of the equipment.

[0055] S12: Vibration Intensity Data Acquisition

[0056] A six-axis inertial measurement unit (IMU) is installed in the front part of the excavator frame, near the center of rotation. (Requirements: vertical axial acceleration measurement range ±5g, measurement accuracy...) With a weight of 0.01g and a sampling rate of 100Hz, this IMU is designed to resist vibration and electromagnetic interference. During installation, it is secured with a shock-absorbing bracket, which is bolted to the frame to ensure the IMU accurately captures the overall vibration of the machine and avoids interference from localized boom movements. This IMU is specifically designed for acceleration along the vertical axis (perpendicular to the ground). Real-time data acquisition is performed. The raw acceleration data is first filtered through a high-pass filter (cutoff frequency 0.5Hz) to remove the static influence of gravitational acceleration, and then filtered through a low-pass filter (cutoff frequency 10Hz) to remove high-frequency mechanical noise. Subsequently, the effective vibration value is calculated based on the effective data acquired within the most recent second. Since the control period is 20ms, there are 50 sampling points within 1 second (i.e., N=50). The calculation formula is as follows:

[0057] ;

[0058] The summation operation covers the squared acceleration values ​​of all 50 sampling points within 1 second. (Vibration RMS value) It directly reflects the severity of fuselage vibration. The vibration is slight at 0.2g-0.5g, and moderate at 0.2g-0.5g. When the vibration level is greater than 0.5g, it is considered a violent vibration. Different vibration levels correspond to different expansion amplitudes of the subsequent potential energy field buffer space.

[0059] S13: Working Arm Span Calculation

[0060] One non-contact Hall angle sensor (selection requirements: measurement range 0-180°, measurement accuracy ±0.1°, response time ≤5ms, protection rating IP65) is installed at the hinge points of the boom and chassis, and the stick and boom. The sensors are fixed to the side of the hinge points by brackets to ensure accurate capture of the real-time rotation angles of the boom relative to the chassis and the stick relative to the boom, without affecting the normal operation of the components. Simultaneously, the mechanical structural parameters of the current excavator, including the boom length, are obtained in advance. (Distance from the frame hinge point to the boom hinge point), boom length (Distance from the boom hinge point to the bucket tooth tip), these parameters are taken from the equipment's manufacturer's specifications. If the equipment has been modified, it needs to be re-measured and calibrated. The ECU uses the boom angle collected by the angle sensor. , pole angle Based on the known mechanical structural parameters, the horizontal distance between the bucket tooth tip and the excavator's swing center is calculated using the kinematic formulas of the planar linkage mechanism. (i.e., boom span). The specific calculation process is as follows: Establish a horizontal rectangular coordinate system with the center of rotation as the origin. The X-axis is parallel to the ground and points in the working direction, and the Y-axis is perpendicular to the X-axis. First, calculate the boom angle... Calculate the X coordinate of the boom hinge point: Combined with the angle of the boom (Based on the boom extension direction) Calculate the X coordinate of the bucket tooth tip: , That is The absolute value of the arm span is calculated in real time in the ECU, updating the arm span data every 20ms to ensure synchronization with the dynamic changes in the equipment's working posture.

[0061] Step S2: Construct a single-machine hydraulic load potential energy field model

[0062] like Figure 2 This step establishes a mapping relationship between "physical load and spatial safety boundary," transforming the excavator's real-time operating state (load, vibration, boom extension) into a dynamically changing safety radius. This allows the safety boundary to adaptively adjust with working conditions, avoiding the limitations of static safety distances. The controller calculates the machine's potential energy field safety radius in real time according to a control cycle (20ms). The specific implementation is as follows:

[0063] ;

[0064] Among them: 1. (Basic radius): refers to the maximum slewing radius constant of the excavator, that is, the maximum trajectory radius of the bucket teeth around the center of rotation when the excavator's boom is fully extended. This parameter is directly taken from the equipment's factory specifications. If the boom is replaced or the structure is modified, it needs to be re-measured to ensure the accuracy of the basic radius.

[0065] 2. (System Limit Pressure): This refers to the set pressure of the main relief valve of the excavator's hydraulic system. It is the maximum safe pressure that the hydraulic system can withstand. It is taken from the hydraulic system technical manual. This parameter is a fixed value and does not need to be changed unless the relief valve setting is adjusted.

[0066] 3. (Load Gain Coefficient): Used to quantify the expansion requirements of the safety space under high load conditions. Its core function is to compensate for the safety risks caused by the elastic deformation and posture instability of the boom under high load. This coefficient is obtained through on-site calibration: selecting equipment operating at full load ( = And the wingspan reaches its maximum ( Under extreme working conditions (maximum value), the elastic deformation of the working arm is measured using a laser rangefinder, and combined with the target safety redundancy, the following can be deduced: After calibration, it needs to be verified under three different full-load conditions to ensure that the space expansion corresponding to the load meets the actual safety requirements.

[0067] 4. (Vibration Gain Coefficient): Used to quantify the buffer space required when the machine body vibrates. The more intense the vibration, the larger the buffer space, to avoid collisions caused by equipment attitude fluctuations due to vibration. This coefficient is also calibrated on-site: different vibration intensities are simulated in the test field (achieved by changing the hardness of the working ground and the digging speed), and for every 1g increase in vibration acceleration (i.e., Add 1g), and determine the required additional safety distance through actual measurement (the empirical value is usually 0.3-0.5m), and use this as the basis for... The value of is, for example, if a safety distance of 0.3m is required for every 1g increase in vibration, then =0.3. After calibration, multiple verifications need to be conducted within the vibration intensity range of 0.2g-1.0g to ensure that the buffer space can effectively offset the safety risks caused by vibration.

[0068] Calculation process explanation:

[0069] Safe radius of potential energy field It consists of three superimposed parts: the base radius Ensure the basic safety boundaries of equipment under static operating conditions; load-related items · · The value of this component increases dynamically with the excavation load and boom span; the greater the load and the longer the boom span, the larger the value of this component, and the further outward the safety boundary extends; vibration-related items The vibration intensity changes dynamically; the more intense the vibration, the larger this value, and the more ample the buffer space. These three factors work together to achieve a dynamic adaptation effect where "space breathes with the load and expands with vibration," recalculating every 20ms to ensure the safety radius accurately matches the real-time status of the equipment.

[0070] Figure 2This diagram visually illustrates how the safety boundaries of an excavator are dynamically constructed, embodying the core concept of this invention: "space breathes with load." The excavator entity is at the center of the diagram, and the surrounding concentric circles represent different levels of safety boundaries. The inner solid line (mechanical boundary) represents the physical range of rotation of the equipment's rigidity. The middle dashed line (load expansion zone): indicates that the size of this zone expands outward as the real-time digging resistance P increases. The outer dashed line (vibration expansion zone): indicates that the size of this zone varies with the intensity of the machine's vibration. As the potential energy increases, it expands outwards further, forming the final total potential energy field.

[0071] Step S3: Full-field potential energy interference detection and physical game arbitration

[0072] like Figure 3 The core of this step is to solve the problem of conflict determination and avoidance priority in multi-device collaborative operation. It uses UWB positioning technology to realize the position perception between devices, combines potential energy field parameters for interference detection, and then uses potential energy density arbitration based on pure physical logic to ensure the fairness and rationality of avoidance decision. The whole process takes no more than 10ms, which meets the requirements of real-time control.

[0073] S31: Interference Calculation

[0074] 1. Location Acquisition:

[0075] Two UWB positioning tags are installed above the slewing center of each excavator (selection requirements: positioning accuracy ±0.1m, communication distance ≥50m, communication frequency 50Hz, strong anti-obstruction capability). The tags are fixed with brackets to ensure consistent installation height (1.5m above the ground). Simultaneously, three UWB anchor points are installed at fixed locations in the work area (edge ​​of the pit) to form a positioning network. The UWB tags communicate with the anchor points in real time, calculating the three-dimensional coordinates (X, Y, Z) of the slewing center of each excavator using a TOF (Time-of-Flight) positioning algorithm. The Z coordinate is used to filter interference from ground height differences, retaining only the X and Y coordinates on the horizontal plane for distance calculation. The coordinate data is transmitted to the excavator's ECU every 20ms and simultaneously synchronized to the ECUs of adjacent devices via a wireless communication module, ensuring real-time sharing of position information between devices.

[0076] 2. Calculation of center distance:

[0077] The ECU uses the machine's rotation center coordinates ( , ) and the coordinates of the slewing center of the adjacent machine ( , The horizontal distance between the two centers is calculated using the distance formula. The formula is The calculation results are rounded to two decimal places to ensure distance accuracy.

[0078] 3. Interference depth calculation and conflict determination:

[0079] The ECU calls upon the potential energy field safety radius calculated in real time by the device. The safe radius of the potential energy field synchronized with the neighboring machine Through formula Calculate the depth of potential energy interference .when When the potential energy fields of the two devices are greater than 0, it is determined that their potential energy fields overlap, indicating a spatial conflict, and arbitration should be initiated to avoid the collision. When the value is ≤0, no conflict is determined, and the equipment remains in normal operating condition. Among these parameters, the interference depth... The size reflects the severity of the conflict: 0 < ≤0.3m is considered a minor collision; 0.3m < ≤0.8m is considered moderate conflict. A collision exceeding 0.8m is considered a severe conflict, and the response speed of subsequent avoidance actions increases with the severity of the conflict.

[0080] S32: Energy Density Arbitration

[0081] 1. Calculation of potential energy density:

[0082] Potential energy density It is a core indicator representing the operational priority of equipment, comprehensively reflecting the current load intensity and vibration severity of the equipment. The higher the load, the more severe the vibration. The higher the value, the greater the operational difficulty and the higher the requirement for attitude stability of the equipment, thus it should enjoy a higher priority in terms of road rights. The calculation formula is as follows: ,in This represents the load percentage (between 0 and 1). Vibration correction factor ( The unit is g), and multiplying the two achieves the coupling quantification of load and vibration. For example, equipment A is in the condition of excavating hard rock. =35MPa =35MPa), =0.6g, then Equipment B is in an unloaded rotation condition. =5MPa, =0.1g, then Clearly, device A has a higher priority.

[0083] 2. Arbitration logic enforcement:

[0084] Let the local unit be A and the neighboring unit be B. The ECU simultaneously acquires the potential energy density of both the local and neighboring units. and (Synchronized via wireless communication, data transmission delay ≤5ms).

[0085] like > The machine is designated as the master machine and maintains the current operating status unchanged. This includes maintaining the original commands for digging actions, operating speed, and hydraulic output to ensure that high-priority operations are not affected.

[0086] like < The machine determines that it is a slave and immediately triggers the active avoidance control in step S4, and the response speed and interference depth of the avoidance action are... Positive correlation: the more severe the conflict, the faster the avoidance action.

[0087] like = In this case, arbitration is based on the principle of "operational continuity," meaning the machine with the longer continuous operation time is the master machine, and the other is the slave machine. If the continuous operation time is the same, the machines are sorted by number from smallest to largest, with the one with the smallest number being the master machine, to ensure that the arbitration is unambiguous.

[0088] Figure 3 The physical basis and results of the arbitration mechanism are illustrated through comparison. Equipment A on the left is in a "hard rock" state (excavating hard rock), with a large potential energy field radius and high potential energy density. The high-speed device is determined to be the Master with the right-of-way. Device B on the right is in a "free state" (idle rotation), with a small potential energy field radius and low potential energy density. Low. The overlapping area (shaded area) represents the interference of the potential energy fields of the two devices, and the depth of interference is marked. The arrows and dashboard icons in the image indicate that... < Device B was identified as a Slave and triggered a flow throttling action to reduce the swashplate angle for avoidance.

[0089] Step S4: Hydraulic throttling control based on swashplate adjustment

[0090] This step directly intervenes in the core component of the hydraulic power system: the main pump swashplate, to cut off the flow and control the equipment's movement speed, achieving a flexible obstacle avoidance effect. This avoids the lag of traditional path planning-based obstacle avoidance, ensuring a smooth, safe, and shock-free obstacle avoidance process. Once the machine is identified as the obstacle avoidance slave, the ECU immediately initiates the following closed-loop control process:

[0091] S41: Calculate the obstacle avoidance damping coefficient

[0092] Avoidance damping coefficient Used to quantify the degree of flow interception, its value is related to the potential energy interference depth. Proportional, the formula is = ,in: (Preset buffer distance) refers to the maximum allowable interference depth. When the interference depth reaches this value, the equipment must completely stop to avoid collision. This parameter is set based on the size of the working space and the equipment's response time; it is set appropriately in confined spaces such as subway pits. =0.8m, and 1.2m for relatively open spaces such as tunnels. After setting, it needs to be verified by on-site test to ensure that the equipment can stop smoothly within this buffer distance.

[0093] The value range is 0-1, when When ≤0, =0 (no need to avoid obstacles); when 0 < < hour, Follow Linear growth (the degree of avoidance gradually increases); when ≥ hour, =1 (maximum avoidance, equipment stops operating). Synchronized with the control cycle, every 20ms based on the latest interference depth Recalculate to ensure that the degree of traffic interception matches the changes in conflict in real time.

[0094] S42: Adjust the swashplate angle of the main pump

[0095] The ECU first reads the desired flow command output by the upper-level motion controller. This instruction is based on the hydraulic flow requirements planned according to the current work task (such as excavation, rotation, unloading). Then, it is based on the avoidance damping coefficient. The desired flow command is modified using the following formula: = ,in This is the flow retention factor. When... When =1, =0L / min (flow completely truncate). Corrected flow command. Minimum flow rate must be met (usually 10% of rated flow rate) to prevent hydraulic system jamming due to excessively low flow rate. ≥ Traffic is allowed to drop to 0 at times.

[0096] The ECU then sends the corrected flow command. This is converted into a corresponding current signal (the control signal of the proportional solenoid valve). The conversion relationship is a linear mapping. For example, if the rated control current of the proportional solenoid valve is 0-20mA, corresponding to the maximum output flow rate of the main pump being 0-100L / min, then... =40L / min, then the converted current signal is 8mA. This current signal is transmitted through a shielded cable to the proportional solenoid valve of the hydraulic master pump (response time ≤10ms). The proportional solenoid valve adjusts the valve core opening according to the magnitude of the current signal, thereby controlling the tilt angle of the master pump swashplate: the smaller the current signal, the smaller the valve core opening, the smaller the swashplate tilt angle, and the lower the output flow of the master pump; conversely, the larger the current signal, the higher the flow rate. The hydraulic master pump is equipped with a swashplate tilt angle sensor (measurement accuracy ±0.1°), which collects the actual swashplate tilt angle in real time and feeds it back to the ECU. The ECU compares the flow rate corresponding to the actual tilt angle with the corrected flow rate command. If the deviation exceeds 5%, the current signal is adjusted for compensation to ensure accurate matching of the main pump's output flow. This enables closed-loop control.

[0097] As the neighboring aircraft approaches, the depth of interference... Gradually increase Increase synchronously. Linear decay, with the equipment's operating speed gradually decreasing: from normal operating speed to... ≥ The machine comes to a complete stop without any sudden impact, preventing boom shaking, material spillage, or loss of equipment control, thus achieving "flexible avoidance." When adjacent machines are far away, the interference depth... Decrease Synchronous reduction, Gradually recover, the equipment slowly returns to normal operating status to ensure continuous operation.

[0098] Please see Figure 4 This figure quantitatively describes the specific execution rules of avoidance control in step S4. The horizontal axis represents the depth of interference. (Unit: meters), the vertical axis represents the percentage of the main pump's output flow. The solid line in the figure indicates that as the interference depth increases... As the flow rate increases from 0, the main pump flow rate decreases linearly. The dashed line in the graph indicates the preset buffer limit. When the interference depth reaches this limit, the flow output drops to 0%, and the equipment physically shuts down, thus ensuring the flexibility and determinism of the avoidance process.

[0099] In one embodiment of the present invention, please refer to Figure 5A collaborative control system for underground robot swarms, used to implement the aforementioned collaborative and obstacle avoidance method for subway pit robot swarms based on hydraulic body perception, comprises: a perception module, including a pressure sensor, an IMU sensor, and a UWB tag, for collecting hydraulic pressure, body vibration, joint angle, and equipment position information; an onboard controller, connected to the perception module, for executing steps S1 to S4 of the aforementioned method, realizing data calculation, potential energy field construction, conflict arbitration, and generating control commands; and an execution module, including a proportional solenoid valve and a hydraulic main pump connected to the onboard controller, for receiving control commands and adjusting the swashplate tilt angle, and controlling the hydraulic flow output, wherein the control cycle of the onboard controller is 20ms, and the data transmission delay of the perception module does not exceed 5ms.

[0100] This method for collaborative obstacle avoidance of subway pit robots based on hydraulic proprioception addresses the pain points of optical sensor failure caused by high dust, water mist, and low light conditions underground. The invention constructs a virtual potential energy field through robot proprioception. Even in extreme conditions with zero visibility, the robots can accurately perceive their own "danger zone" through load feedback from the hydraulic system and interact with neighboring robots. This gives the robot swarm strong environmental robustness, ensuring inherent safety in underground operations. When robots are performing fine work in soft soil layers, the potential energy field automatically contracts, allowing robots to be closely arranged and operate in high-density parallelism in extremely narrow spaces, significantly improving the spatial and temporal utilization of the limited cross-section underground. When robots encounter localized hard rock or isolated boulders (common geological abrupt changes underground), the potential energy field, driven by a surge in load, automatically expands within milliseconds, instantly creating sufficient physical buffer space. This mechanism, which adapts to changing conditions, perfectly resolves the contradiction between space utilization and defense against sudden geological risks. This invention also changes the blindness of traditional scheduling methods that rely on first-come, first-served or random avoidance. The system can autonomously identify key robots performing high-load tasks (such as rock breaking and tunneling) and assign them the highest right-of-way; robots in a free, rotating, or low-load state automatically assume the responsibility of avoiding obstacles. This mechanism effectively avoids the risks of stuck drill bits, seizure, or hard rock backlash caused by forced shutdown, ensuring the continuity and high energy efficiency of the most difficult downhole operations. This invention does not use upper-level planned emergency stop / braking commands, but instead delves into the underlying power system, establishing an admittance mapping relationship between interference depth and hydraulic flow. When avoidance occurs, the robot exhibits a physical characteristic of being "softly pushed away" by a high potential energy field (linear velocity decay rather than a step jump). This control method eliminates the hydraulic water hammer effect and mechanical structural impact caused by emergency stops, significantly reducing the failure rate of expensive downhole robotic equipment and extending its lifespan.

[0101] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make several modifications and improvements without departing from the concept of the present invention, and these should also be considered within the scope of protection of the present invention. These will not affect the effectiveness of the implementation of the present invention or the practicality of the patent.

Claims

1. A method for collaborative obstacle avoidance and obstacle swarming of subway foundation pit robots based on hydraulic body perception, characterized in that, Includes the following steps: S1. Real-time collection of hydraulic load pressure, body vibration intensity and boom extension of a single excavator; S2. Based on the hydraulic load pressure, machine vibration intensity and boom extension, dynamically calculate the safety radius of the potential energy field of the excavator and construct a dynamic safety boundary that changes with the working conditions. S3. Share location information and potential energy field safety radius among multiple excavators, calculate the potential energy interference depth between devices, and arbitrate based on their respective calculated potential energy densities to determine the main excavator and the secondary avoidance excavator. S4. For excavators identified as needing to avoid an obstacle, calculate the obstacle avoidance damping coefficient based on the potential energy interference depth, and adjust the swashplate angle of the hydraulic main pump accordingly to control the output flow rate and achieve flexible obstacle avoidance action.

2. The method for collaborative obstacle avoidance of subway pit robot swarms based on hydraulic body perception according to claim 1, characterized in that, Step S1 specifically includes: S11. Hydraulic pressure signals are collected by pressure sensors installed in the boom cylinder and stick cylinder. and After filtering, the comprehensive excavation load is calculated according to preset weighting coefficients. ; S12. The vertical axial acceleration is collected by an inertial measurement unit installed on the fuselage, and the effective value of vibration is calculated after filtering. As the intensity of fuselage vibration; S13. Obtain the rotation angle by means of an angle sensor installed at the joint between the boom and the stick. and Combined with the known boom length Length of the bucket pole Calculate the horizontal working boom extension of the bucket teeth relative to the center of rotation. .

3. The method for collaborative obstacle avoidance of subway pit robot swarms based on hydraulic body perception according to claim 2, characterized in that, In step S2, the safe radius of the potential energy field The calculation formula is: ； in, Based on the radius of gyration, For the system's ultimate pressure, and These are the load gain coefficient and vibration gain coefficient obtained through on-site calibration, respectively.

4. The method for collaborative obstacle avoidance of subway pit robot swarms based on hydraulic body perception according to claim 1, characterized in that, Step S3 specifically includes: S31. Based on UWB positioning technology, obtain the coordinates of the rotation center of each excavator and calculate the center distance between any two machines. And combined with their respective potential energy field safety radii and Through formula Calculate the depth of potential energy interference ,when A conflict is determined when the value is greater than 0; S32. Calculate the potential energy density of each device. , ,in, For load percentage, As a vibration correction factor, compare the potential energy densities of the two devices involved in the collision; if > Then equipment A is the master machine and equipment B is the slave machine; if < Then equipment B is the master machine and equipment A is the slave machine; if = At this point, based on "operational continuity," the machine with the longer continuous operation time is designated as the master machine, and the other machine is designated as the pass-through machine. If the continuous operation time is the same, the machines are sorted by their numbers from smallest to largest, with the machine with the smaller number being the master machine.

5. The method for collaborative obstacle avoidance of subway pit robot swarms based on hydraulic body perception according to claim 4, characterized in that, Step S4 specifically includes: S41. Based on the potential energy interference depth δ and the preset buffer distance Calculate the avoidance damping coefficient The formula is = Its value ranges from 0 to 1; S42. Correct the desired flow rate command based on the avoidance damping coefficient. Receive actual traffic instructions = and will This is converted into a control signal to adjust the proportional solenoid valve of the hydraulic master pump, changing the swashplate angle and thus controlling the output flow.

6. The method for collaborative obstacle avoidance of subway pit robot swarms based on hydraulic body perception according to claim 5, characterized in that, when ≥ hour, =1, the equipment stops operating; with Decrease Decrease As the volume increases, the equipment's operating speed gradually recovers.

7. A collaborative control system for underground robot swarms, employing the collaborative and obstacle avoidance method for subway pit robot swarms based on hydraulic body perception as described in any one of claims 1-6, characterized in that, include: The sensing module, including a pressure sensor, an inertial measurement unit, an angle sensor, and a UWB positioning tag, is used to collect information on hydraulic pressure, machine vibration, joint angles, and equipment position. The on-board controller, connected to the sensing module, is used to execute steps S1 to S4 of the method for collaborative obstacle avoidance of a group of subway pit robots based on hydraulic body perception as described in any one of claims 1-6, and to realize data calculation, potential energy field construction, conflict arbitration and generate control commands. The execution module includes a proportional solenoid valve and a hydraulic master pump connected to the vehicle controller, used to receive control commands and adjust the swashplate tilt angle to control the hydraulic flow output.

8. The downhole robot swarm collaborative control system according to claim 7, characterized in that, The control cycle of the vehicle controller is 20ms, and the data transmission delay of the sensing module is no more than 5ms.

9. An underground engineering machinery, characterized in that, It is equipped with a collaborative control system for downhole robot swarms as described in claim 7 or 8.

Images