A bucket chain unloader digging part anti-collision system and method based on virtual flexible body and admittance control

Abstract

This invention belongs to the field of anti-collision control technology for bucket unloaders, and discloses an anti-collision system and method for the excavation section of a bucket unloader based on virtual flexible body and admittance control. The system includes a multi-degree-of-freedom torque sensing module, a contact state classification module, a virtual impedance parameter scheduling module, a force-position hybrid admittance control module, and a joint torque servo module. External contact equivalent torques are obtained through joint torque sensors, and time-frequency domain features are extracted to identify the contact object type. Based on this, virtual inertia, damping, and stiffness matrices are dynamically scheduled, and the force-position admittance equation is used to correct joint position commands in real time, making the excavation section exhibit a set compliant characteristic in external contact forces. This method virtualizes the excavation section as a flexible body, enabling continuous material removal with minimal force against the bulkhead in dead-end areas of the ship's hold where space avoidance is impossible. This significantly improves the cleaning rate and the continuity of unmanned operations. The sensing is unaffected by harsh environments such as high dust and water mist, and it provides safety protection for emergency compliant retreat.

Description

Technical Field

[0001] This invention relates to the field of anti-collision control technology for bucket chain unloaders, and in particular to an anti-collision system and method for the excavation section of a bucket chain unloader based on virtual flexible body and admittance control. Background Technology

[0002] Bucket unloaders are the core continuous unloading equipment in bulk cargo terminals. Their digging section (including the bucket lifting mechanism and the digging section itself) is the direct material-collecting component, requiring it to penetrate deep into the ship's hold during operation. This close proximity to bulkheads, hatch edges, and internal beams poses a collision risk. As bulk cargo terminals transition towards unmanned and intelligent operations, unmanned and automated operation of bucket unloaders has become an industry trend. However, the collision avoidance control of the digging section remains a key factor hindering its full unmanned implementation.

[0003] Currently, the following solutions are mainly used for collision avoidance control of the excavation section of chain bucket unloaders:

[0004] One approach is the mechanical contact protection scheme. This scheme involves installing rope-operated limit switches or collision sensors around the hoisting mechanism of the excavator. When a structural component makes physical contact with an obstacle, a limit signal is triggered, initiating a shutdown protection. However, this scheme can only detect contact in the direction of the rope's installation, resulting in a limited protection range and blind spots. Furthermore, it only triggers a response after an actual collision, making it a passive protection method that cannot meet the requirements of unmanned continuous operation.

[0005] The second approach is a single-sensor perception and control scheme. This scheme uses an ultrasonic sensor or a single radar to sense the distance in a certain direction of the excavation section and judges the collision risk based on a preset distance threshold. For example, Chinese utility model patent CN208265626U discloses an ultrasonic anti-collision system for a chain bucket unloader. This system uses an ultrasonic sensor installed at the head of the unloader to detect obstacles. However, due to the high concentration of dust, dim light, water mist, and irregular swaying of the hull inside the ship, the ultrasonic signal is severely attenuated. The perception range of a single sensor is limited, the accuracy is reduced, and it is prone to misjudgment or missed judgment, resulting in insufficient reliability of the anti-collision control.

[0006] Thirdly, there is the multi-sensor fusion obstacle avoidance scheme. This scheme employs multi-sensor fusion technology, such as lidar and point cloud data acquisition, to obtain distance information between the excavator and obstacles by performing 3D modeling of the environment surrounding the excavator, and then planning an avoidance path. For example, Chinese invention patent application CN119218884A discloses a bucket elevator, an anti-collision control method, and electronic equipment. This scheme obtains the relative position information between the lifting mechanism and obstacles through coordinate acquisition devices and point cloud acquisition devices, and combines this with the operating speed of the excavator to perform anti-collision control. However, the above schemes share an unstated technical premise—that is, assuming that there is a spatial path between the excavator and the obstacle that can be safely avoided. When the excavation section of the chain bucket unloader penetrates deep into the bottom of the hull, near the base of the bulkhead, or below the crossbeams, the geometry of the excavation section determines that there is a physical boundary between it and the hull structure that cannot be completely avoided. At this time, the anti-collision system based on the principle of spatial avoidance can only choose to stop the machine, sound an alarm, and wait for manual intervention, or leave too much operating margin to ensure safety. As a result, a large amount of edge material cannot be removed, the hull cleaning rate is low, and the continuity of unmanned operation cannot be guaranteed.

[0007] In summary, existing technologies for collision avoidance control in the excavation section of bucket unloaders all rely on the basic control concept of "spatial avoidance," which involves sensing distance and planning paths to avoid all contact. This approach has inherent limitations in physically confined blind spots. How to ensure the equipment can operate safely and continuously in situations where avoidance is impossible is a pressing technical problem that needs to be solved. Summary of the Invention

[0008] The present invention aims to solve the problem that the existing anti-collision control technology of the excavation section of the chain bucket unloader, which adopts the "space avoidance" as the basic control concept, makes it impossible for the excavation section to safely and continuously carry out the cleaning operation against the hull wall in the case of limited physical space such as the dead corner area of ​​the hull.

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

[0010] In a first aspect, the present invention provides a collision avoidance system for the excavation section of a bucket unloader based on virtual flexible body and admittance control, including a multi-degree-of-freedom torque sensing module, a contact state classification module, a virtual impedance parameter scheduling module, a force-position hybrid admittance control module, and a joint torque servo module.

[0011] The multi-degree-of-freedom torque sensing module is installed at each active joint of the excavator's suspension mechanism to detect the equivalent torque generated by external contact forces on each active joint in real time. The contact state classification module communicates with the multi-degree-of-freedom torque sensing module to receive the equivalent torque signal, extract its time-domain and frequency-domain features, and identify the type of contact object based on these features. The virtual impedance parameter scheduling module communicates with the contact state classification module to dynamically generate the target virtual inertia matrix M_d, the target virtual damping matrix D_d, and the target virtual stiffness matrix K_d according to the type of contact object and the current operating mode. The force-position hybrid admittance control module communicates with the virtual impedance parameter scheduling module to receive M_d, D_d, and K_d, as well as the desired joint position θ_d output by the path planner, where θ_d is the desired joint position vector. It calculates the joint position correction Δθ in real time based on the force-position admittance relationship, where Δθ is the joint position correction vector, and outputs the admittance-corrected joint position command θ_cmd, where θ_cmd = θ_d + Δθ. The joint torque servo module communicates with the force-position hybrid admittance control module to receive θ_cmd. It drives each joint actuator through the inner loop torque controller, ensuring that the excavator exhibits compliant mechanical characteristics conforming to M_d, D_d, and K_d in response to external contact forces.

[0012] As a further description of the above technical solution, the force-potential hybrid admittance control module is based on the following second-order impedance dynamic equation: M_d(θ_2-θ_2d)+D_d(θ_1-θ_1d)+K_d(θ-θ_d)=-τ_ext. Where θ is the actual joint position, θ_1 is the actual joint angular velocity, θ_2 is the actual joint angular acceleration, θ_2d is the desired joint angular acceleration, θ_1d is the desired joint angular velocity, and τ_ext is the measured external equivalent torque.

[0013] As a further description of the above technical solution, the time-domain features extracted by the contact state classification module include peak torque, force rise rate, and decay time constant; the extracted frequency-domain features include high-frequency energy ratio and low-frequency energy ratio, wherein the high-frequency energy ratio is the ratio of energy in the frequency band above 50Hz to the total energy, and the low-frequency energy ratio is the ratio of energy in the frequency band below 30Hz to the total energy. The contact state classification module uses the force rise rate, the high-frequency energy ratio, the peak torque, and the decay time constant as input variables, and the contact object type as the output variable. The contact object types include rigid bulkheads, rigid beams, and large pieces of material. When the force rise rate exceeds a first preset threshold and the high-frequency energy ratio exceeds a second preset threshold, the contact object is identified as a rigid component; when the force rise rate is below a third preset threshold and the low-frequency energy ratio exceeds a fourth preset threshold, the contact object is identified as a large piece of material.

[0014] As a further description of the above technical solution, the current operating modes include free material handling mode, wall sliding mode, and emergency compliant retreat mode. In free material handling mode, the virtual impedance parameter scheduling module sets K_d to the preset maximum stiffness value K_max and D_d to the preset normal damping value D_normal. In wall sliding mode, K_d is set to the preset minimum stiffness value K_min and D_d is set to the preset high damping value D_high, with the target contact torque controlled within the range of 5 N·m to 10 N·m; where K_max and K_min satisfy K_max ≥ 20 × K_min. The virtual impedance parameter scheduling module performs parameter transitions according to the following formula when switching operating modes:

[0015] K_d(k) = α·K_d(k-1)+(1-α)·K_d_target, D_d(k) = α·D_d(k-1)+(1-α)·D_d_target. Where K_d_target is the preset virtual stiffness target value corresponding to the current operating mode, D_d_target is the preset virtual damping target value corresponding to the current operating mode, and α is a smoothing coefficient, ranging from 0.90 to 0.95.

[0016] As a further description of the above technical solution, a safety monitoring coprocessor independent of the main control processor is also included. The safety monitoring coprocessor independently monitors the raw signals of the torque sensors of each active joint with a control cycle of no more than 0.5ms. If any torque sensor signal exceeds the preset absolute stop threshold τ_max_stop, the power supply is cut off and the brake is triggered via hardware circuitry. The safety monitoring coprocessor is also used to monitor the communication heartbeat signal with the main control processor. If the communication heartbeat signal is interrupted for more than 50ms, the safety brake is automatically triggered.

[0017] Secondly, the present invention provides a collision avoidance method for the excavation section of a bucket unloader based on virtual flexible body and admittance control, comprising the following steps.

[0018] S1: The equivalent torque signal τ_ext generated by the external contact force on each active joint is acquired in real time by the torque sensors installed at each active joint of the excavator suspension mechanism.

[0019] S2: Perform time-domain and frequency-domain feature extraction on the equivalent torque signal τ_ext. Based on the extracted time-domain and frequency-domain features, identify the type of contact object, which includes rigid bulkheads, rigid beams, and large pieces of material. The extracted time-domain features include peak torque, force rise rate, and decay time constant. The extracted frequency-domain features include the proportion of high-frequency energy and the proportion of low-frequency energy. When the force rise rate exceeds a first preset threshold and the proportion of high-frequency energy exceeds a second preset threshold, the contact object is identified as a rigid component; when the force rise rate is lower than a third preset threshold and the proportion of low-frequency energy exceeds a fourth preset threshold, the contact object is identified as a large piece of material.

[0020] S3: Based on the type of contact object identified in step S2 and the current operating mode, dynamically schedule the virtual impedance parameters. The virtual impedance parameters include the virtual inertia matrix M_d, the virtual damping matrix D_d, and the virtual stiffness matrix K_d. The rules for dynamically scheduling the virtual stiffness matrix K_d are as follows: in the free material handling mode, K_d is set to the preset maximum stiffness value K_max; in the wall sliding mode and the emergency compliant retreat mode, K_d is set to the preset minimum stiffness value K_min; a first-order low-pass filter is used for smooth transition when switching parameters.

[0021] S4: Based on the following force-position admittance relationship, the joint position correction Δθ is calculated in real time from the equivalent torque signal τ_ext, and the admittance-corrected joint position command θ_cmd = θ_d + Δθ is generated: M_d(θ_2 - θ_2d) + D_d(θ_1 - θ_1d) + K_d(θ - θ_d) = -τ_ext. Where θ_d is the desired joint position output by the path planner, θ is the actual joint position, θ_1 is the actual joint angular velocity, θ_2 is the actual joint angular acceleration, θ_2d is the desired joint angular acceleration, and θ_1d is the desired joint angular velocity.

[0022] S5: Using the joint position command θ_cmd output in step S4 as the input to the inner ring torque controller, drive each joint actuator to make the excavator exhibit compliant mechanical characteristics that conform to the virtual impedance parameters in response to external contact forces.

[0023] As a further description of the above technical solution, step S6 is also included: when the amplitude of the equivalent torque signal τ_ext exceeds the preset emergency response threshold, an emergency compliant retreat is triggered. The direction of the retreating joint velocity is determined by the following formula: θ_1_escape=-k_e·τ_ext / ‖τ_ext‖, k_e=(‖τ_ext‖-τ_safe) / (τ_max_stop-τ_safe)·θ_1_max. Wherein, τ_safe is the safe contact force threshold, τ_max_stop is the maximum stopping threshold, θ_1_max is the maximum permissible speed of the joint, and k_e is the escape velocity gain.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] The present invention discloses a collision avoidance system and method for the excavation section of a chain bucket unloader based on virtual flexible body and admittance control. This system breaks through the traditional "spatial avoidance" collision avoidance control concept. Through virtual flexible body modeling and force-position hybrid admittance control, the excavation section can continuously clean the material by adhering to the hull wall with controlled small contact force. This solves the problem in the prior art that when physical space is limited and safe avoidance is not possible, the machine can only be stopped or an excessive safety margin can be maintained. This significantly improves the cleaning rate and the continuity of unmanned operation.

[0026] The present invention discloses a collision avoidance system and method for the excavation section of a bucket unloader based on virtual flexible body and admittance control. The contact sensing method uses a torque sensor embedded in the joint. The type of contact object is identified by analyzing the time-frequency domain characteristics of the contact torque signal itself. It does not rely on external environmental sensing devices such as lidar and ultrasonic sensors. Therefore, it is not affected by harsh environmental factors such as high concentration of dust, dim light and water mist in the ship's cabin, and the sensing reliability is significantly improved.

[0027] The present invention discloses a collision avoidance system and method for the excavator section of a bucket unloader based on virtual flexible body and admittance control. Through force-position hybrid admittance control, the excavator section produces a continuous and smooth compliant response to external contact forces, rather than a switch-type emergency stop response. When accidentally contacting high-risk rigid components such as crossbeams, the system can sense the contact torque and execute a yielding action within milliseconds, effectively reducing the risk of collision damage.

[0028] The present invention discloses a collision avoidance system and method for the excavation section of a bucket unloader based on virtual flexible body and admittance control. The system dynamically adjusts virtual impedance parameters according to the operation scenario, maintains high stiffness in the safe area to ensure operation accuracy and efficiency, and reduces stiffness in the critical area and contact area to achieve compliant protection, thus achieving a dynamic balance between cleaning efficiency and equipment safety. Attached Figure Description

[0029] Figure 1 This is a block diagram of the overall architecture of the anti-collision system for the excavation section of a chain bucket unloader based on virtual flexible body and admittance control.

[0030] Figure 2 This is a principle block diagram of the force-potential hybrid admittance control of the present invention;

[0031] Figure 3 This is a flowchart of the contact state classifier of the present invention;

[0032] Figure 4 This is the finite state machine state transition diagram for switching operating modes in this invention;

[0033] Figure 5 This is the main control loop flowchart of the method of the present invention. Detailed Implementation

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

[0035] Example 1

[0036] System Overall Architecture

[0037] Reference Figure 1 This invention provides a collision avoidance system for the excavation section of a bucket unloader based on virtual flexible body and admittance control. The system includes a multi-degree-of-freedom torque sensing module, a contact state classification module, a virtual impedance parameter scheduling module, a force-position hybrid admittance control module, and a joint torque servo module. This system works in conjunction with a host path planner to achieve collision avoidance control of the excavation section.

[0038] Multi-degree-of-freedom torque sensing modules are installed at each active joint of the excavator's suspension mechanism. In this embodiment, the excavator's suspension mechanism includes four active joints: pitch joint (J1), slewing joint (J2), telescopic joint (J3), and lateral joint (J4). Each active joint is driven by an AC servo motor and equipped with a planetary reducer. A strain gauge six-dimensional torque sensor is installed between the reducer output end of each active joint and the boom connection flange to detect the equivalent torque generated by external contact forces on each active joint in real time. The torque sensor has a range of ±200 N·m, a resolution of 0.05 N·m, a nonlinearity of no more than 0.5%FS, and a sampling frequency of 2000 Hz. The stator side of the torque sensor is rigidly connected to the joint output flange, and the rotor side is rigidly connected to the boom structure to ensure that the external contact force reaches the sensor through a complete force transmission path.

[0039] The contact state classification module communicates with the multi-degree-of-freedom torque sensing module, receiving the equivalent torque signals output by each torque sensor. The virtual impedance parameter scheduling module communicates with the contact state classification module. The force-position hybrid admittance control module communicates with the virtual impedance parameter scheduling module and simultaneously receives the desired joint position θ_d output by the host path planner. The joint torque servo module communicates with the force-position hybrid admittance control module, driving each joint actuator according to the joint position commands output by the latter.

[0040] The system's main control processor adopts a heterogeneous embedded controller based on ARM Cortex-A72 and FPGA. The FPGA part is responsible for the synchronous acquisition of torque sensor data at a frequency of 2000Hz and the decoding of encoder signals; the ARM processor core runs Linux real-time extension, which performs contact state classification, impedance parameter scheduling and admittance control calculation in real-time tasks, with a control cycle set to 1ms.

[0041] The system also includes a safety monitoring coprocessor independent of the main control processor. This coprocessor uses an independent ARM Cortex-M4 microcontroller to independently monitor the raw signals from the torque sensors of each active joint with a control cycle of 0.5ms. If any torque sensor signal exceeds the preset absolute stop threshold τ_max_stop (set to 150 N·m in this embodiment), the power supply is cut off via a hardware relay, triggering the brake. The safety monitoring coprocessor also monitors the communication heartbeat signal with the main control processor. If the communication heartbeat signal is interrupted for more than 50ms, the safety brake is automatically triggered. This safety monitoring coprocessor operates independently of the main control processor, providing ultimate-level safety protection even in the event of a main control program malfunction or crash.

[0042] Contact state classification module

[0043] The contact state classification module is used to receive the equivalent torque signal τ_ext, extract its time-domain and frequency-domain features, and identify the type of contact object based on these features.

[0044] Reference Figure 3 The workflow of the contact state classification module in this embodiment is as follows:

[0045] First, within a sliding time window (window length set to 80ms), features are extracted from the equivalent torque signal τ_ext. The extracted time-domain features include the peak torque τ_peak, the rate of force rise dτ / dt_peak, and the decay time constant τ_decay. The extracted frequency-domain features include the high-frequency energy proportion R_high and the low-frequency energy proportion R_low. The high-frequency energy proportion R_high is defined as the ratio of energy in the frequency band above 50Hz to the total energy, and the low-frequency energy proportion R_low is defined as the ratio of energy in the frequency band below 30Hz to the total energy. These frequency-domain features are obtained by performing a Fast Fourier Transform on the torque signal within the sliding window.

[0046] Then, the contact state classification module uses the force rise rate dτ / dt_peak, the proportion of high-frequency energy R_high, and the peak torque τ_peak as input linguistic variables, and employs a Mamdani-type fuzzy logic inference system to output the contact object type and confidence level. Contact object types include rigid bulkheads, rigid beams, large pieces of material, and normal excavation resistance.

[0047] The inference rules of the fuzzy inference system include: IF force rise rate IS extremely high AND high frequency percentage IS high THEN contact object IS crossbeam; IF force rise rate IS high AND high frequency percentage IS high AND decay time IS short THEN contact object IS bulkhead; IF force rise rate IS low AND low frequency percentage IS high AND decay time IS long THEN contact object IS large material; IF peak torque IS low AND force rise rate IS low THEN contact object IS normal digging resistance. The membership function of each linguistic variable is defined using a trigonometric function, and the specific parameters are determined through cluster analysis based on actual dock measurement data.

[0048] In actual operation, when the force rise rate exceeds the first preset threshold (300 N·m / s in this embodiment) and the proportion of high-frequency energy exceeds the second preset threshold (70% in this embodiment), the contact state classification module identifies the contact object as a rigid component (including rigid bulkheads and rigid beams). When the force rise rate is lower than the third preset threshold (100 N·m / s in this embodiment) and the proportion of low-frequency energy exceeds the fourth preset threshold (65% in this embodiment), the contact state classification module identifies the contact object as a large piece of material. When the confidence level output by the classifier is lower than 0.80, the system follows the most conservative safety strategy, treating the contact object as a high-risk rigid component.

[0049] Virtual impedance parameter scheduling module

[0050] The virtual impedance parameter scheduling module is used to dynamically generate the target virtual inertia matrix M_d, the target virtual damping matrix D_d, and the target virtual stiffness matrix K_d based on the contact object type output by the contact state classification module and the current working mode.

[0051] Reference Figure 4 The operating modes defined in this embodiment include free material handling mode, proximity warning mode, wall sliding mode, auxiliary crushing mode, and emergency compliant retreat mode. The transition between each mode is managed by a finite state machine.

[0052] In free-access mode, the excavator is positioned away from obstacles, and the system maintains high rigidity to ensure operational accuracy. The virtual impedance parameter scheduling module sets K_d to the preset maximum stiffness value K_max and D_d to the preset normal damping value D_normal. In this embodiment, the typical diagonal element of K_max for each joint is 500 N·m / rad, and the typical diagonal element of D_normal is 50 N·m·s / rad.

[0053] When the path planner indicates that the excavator is about to enter the critical zone, the system switches from free material handling mode to proximity warning mode. In this mode, K_d decreases linearly from K_max to the preset intermediate stiffness value K_mid, gradually "softening" the equipment.

[0054] Once the contact state classification module identifies stable bulkhead contact characteristics, the system enters the wall-sliding mode. In this mode, K_d is set to the preset minimum stiffness value K_min (typical value of diagonal element is 20 N·m / rad), and D_d is set to the preset high damping value D_high (typical value of diagonal element is 100 N·m·s / rad), with the target contact torque controlled within the range of 5 N·m to 10 N·m. K_max and K_min satisfy the relationship K_max ≥ 20 × K_min to ensure a sufficiently significant stiffness difference between the free-feeding mode and the wall-sliding mode, giving the equipment adequate compliance under contact conditions.

[0055] When the contact state classification module detects contact with large pieces of material, the system enters the auxiliary crushing mode. In this mode, K_d is set to K_mid and D_d is set to D_normal to maintain a certain rigidity for material crushing, and the target contact torque is controlled to not exceed 30 N·m.

[0056] When the contact status classification module detects that the beam contact or the equivalent moment amplitude exceeds the preset emergency response threshold, the system enters the emergency compliant yielding mode. In this mode, K_d is set to K_min and D_d is set to D_high, so that the equipment has the lowest stiffness to achieve the maximum degree of compliant yielding.

[0057] The virtual impedance parameter scheduling module employs a first-order low-pass filter for smooth parameter transition during operation mode switching to avoid motion jitter caused by sudden parameter changes. The transition formula is as follows: K_d(k)=α·K_d(k-1)+(1-α)·K_d_target, D_d(k)=α·D_d(k-1)+(1-α)·D_d_target. Where K_d_target is the preset virtual stiffness target value corresponding to the current operation mode, D_d_target is the preset virtual damping target value corresponding to the current operation mode, and α is the smoothing coefficient, set to 0.92 in this embodiment, so that the parameter transition is completed within approximately 10 to 15 control cycles. The virtual inertia matrix M_d remains a fixed diagonal matrix in this embodiment, with a typical value of 5 kg·m² for diagonal elements, adjusted to 3 kg·m² in emergency compliant retreat mode to improve response speed.

[0058] Force-potential hybrid admittance control module

[0059] The force-position hybrid admittance control module is the core algorithm module of the system of this invention. Its basic idea is not to directly control the joint position, but to define the dynamic relationship between the position and the external force, so that the excavator exhibits the desired compliance characteristics to the external force.

[0060] Reference Figure 2 The force-position hybrid admittance control module receives M_d, D_d, and K_d output by the virtual impedance parameter scheduling module, as well as the desired joint position θ_d output by the path planner. Based on the force-position admittance relationship, it calculates the joint position correction amount Δθ in real time and outputs the admittance-corrected joint position command θ_cmd = θ_d + Δθ.

[0061] The force-potential hybrid admittance control module is based on the following second-order impedance dynamic equation: M_d(θ_2-θ_2d)+D_d(θ_1-θ_1d)+K_d(θ-θ_d)=-τ_ext. Where θ is the actual joint position, θ_1 is the actual joint angular velocity, θ_2 is the actual joint angular acceleration, θ_2d is the desired joint angular acceleration, θ_1d is the desired joint angular velocity, and τ_ext is the measured external equivalent torque.

[0062] The physical meaning of this equation is: when an external contact torque τ_ext acts on the excavator, the actual joint position θ will deviate from the desired joint position θ_d. The magnitude of the deviation depends on the virtual impedance parameters M_d, D_d, and K_d. The negative sign on the right side of the equation indicates that the external contact force "pushes" the equipment away from the desired position: when subjected to a contact torque that causes the joint to deflect positively, the system produces a negative position correction, that is, it yields in the direction of the external force.

[0063] To achieve real-time solution in the digital controller, this embodiment uses the bilinear transform method to discretize the above-mentioned second-order differential equation in the continuous domain into the following difference equation: Δθ(k)=a1·Δθ(k-1)+a2·Δθ(k-2)+b0·τ_ext(k)+b1·τ_ext(k-1)+b2·τ_ext(k-2). Wherein, Δθ(k) is the joint position correction amount in the k-th sampling period, and the coefficients a1, a2, b0, b1, and b2 are calculated and determined by M_d, D_d, K_d, and the sampling period T using the bilinear transform formula.

[0064] Joint torque servo module

[0065] The joint torque servo module receives the joint position command θ_cmd output by the force-position hybrid admittance control module and drives each joint actuator through the inner loop torque controller. In this embodiment, the inner loop torque control adopts the calculated torque method, and the inner loop control law is: τ_cmd=M(θ)(θ_2cmd+K_v(θ_1cmd-θ_1)+K_p(θ_cmd-θ))+C(θ,θ_1)θ_1+g(θ). Where M(θ), C(θ,θ_1), and g(θ) are the dynamic model parameters of the excavator suspension mechanism, and K_p and K_v are the position feedback gain matrix and velocity feedback gain matrix, respectively.

[0066] Wall sliding mode

[0067] When the system determines that the excavation unit has reached the vicinity of the bulkhead (200) and the contact status classification module identifies stable bulkhead contact characteristics, the system switches from free material handling mode or proximity warning mode to wall sliding mode.

[0068] In wall-sliding mode, the virtual impedance parameter scheduling module sets K_d to K_min and D_d to D_high. The force-position hybrid admittance control module maintains a small target contact torque of 5 N·m to 10 N·m. Based on the deviation between the measured equivalent torque and the target contact torque, the admittance controller automatically adjusts the position of the excavator in the normal direction of the bulkhead: if the measured contact torque exceeds the upper limit of the target range, the excavator automatically retreats away from the bulkhead to reduce the contact force; if the measured contact torque is below the lower limit of the target range (or contact is lost), the excavator automatically moves closer to the bulkhead to restore contact. The tangential feed motion of the excavator along the bulkhead is generated by the path planner according to a preset cleaning path, and the admittance controller only adjusts the normal position to maintain the desired contact torque. During this process, the bucket continues normal excavation operation, completing the cleaning process along the bulkhead.

[0069] Emergency Compliance Mode

[0070] When the amplitude of the equivalent torque signal τ_ext exceeds the preset emergency response threshold (set to 100 N·m in this embodiment), the system will immediately trigger an emergency compliant retreat regardless of the current operating mode.

[0071] The direction of the retraction joint velocity is determined by the following formula: θ_1_escape=-k_e·τ_ext / ‖τ_ext‖, k_e=(‖τ_ext‖-τ_safe) / (τ_max_stop-τ_safe)·θ_1_max. Where τ_safe is the safe contact force threshold, set to 50 N·m in this embodiment; τ_max_stop is the maximum stopping threshold, set to 150 N·m in this embodiment; θ_1_max is the maximum permissible joint velocity; and k_e is the escape velocity gain. As can be seen from this formula, the retraction velocity direction is the opposite direction of the contact torque, and the retraction velocity amplitude is directly proportional to the contact torque amplitude; that is, the greater the contact force, the faster the retraction velocity.

[0072] Example 2

[0073] Reference Figure 5 The present invention provides a collision avoidance method for the excavation section of a bucket unloader based on virtual flexible body and admittance control, comprising the following steps:

[0074] S1: System power-on self-test, torque sensor zeroing calibration, communication link handshake confirmation. Torque sensors installed at each active joint of the excavator's suspension mechanism acquire, in real time, the equivalent torque signal τ_ext generated by external contact force on each active joint at a sampling frequency of 2000Hz.

[0075] S2: Perform time-domain and frequency-domain feature extraction on the equivalent torque signal τ_ext. The extracted time-domain features include the peak torque, force rise rate, and decay time constant. The extracted frequency-domain features include the proportion of high-frequency energy and the proportion of low-frequency energy. Based on the extracted features, the type of contact object is identified, including rigid bulkheads, rigid beams, and large materials. When the force rise rate exceeds 300 N·m / s and the proportion of high-frequency energy exceeds 70%, the contact object is identified as a rigid component; when the force rise rate is less than 100 N·m / s and the proportion of low-frequency energy exceeds 65%, the contact object is identified as a large material. When the amplitude of the equivalent torque signal is lower than the preset minimum sensing threshold (set to 2 N·m in this embodiment), it is determined that there is no external contact, and the system maintains or switches back to free material handling mode.

[0076] S3: Based on the contact object type identified in S2 and the current operating mode, dynamically schedule the virtual impedance parameters M_d, D_d, and K_d. The scheduling rules are as follows: In free material handling mode, K_d is set to K_max (diagonal element 500 N·m / rad), and D_d is set to D_normal (diagonal element 50 N·m·s / rad); in wall sliding mode and emergency compliant retreat mode, K_d is set to K_min (diagonal element 20 N·m / rad), and D_d is set to D_high (diagonal element 100 N·m·s / rad). A first-order low-pass filter is used for smooth transition when switching parameters, with a smoothing coefficient α = 0.92.

[0077] S4: Based on the force-position admittance relationship M_d(θ_2-θ_2d)+D_d(θ_1-θ_1d)+K_d(θ-θ_d)=-τ_ext, the joint position correction Δθ is solved in real time from the equivalent torque signal τ_ext, and the admittance-corrected joint position command θ_cmd=θ_d+Δθ is generated. The discrete domain solution is performed using the difference equation after bilinear transformation.

[0078] S5: Using the joint position command θ_cmd output by S4 as the input of the inner loop torque controller, drive the actuators of each joint to make the excavator exhibit compliant mechanical characteristics in response to external contact forces that conform to the current virtual impedance parameters.

[0079] S6: When the amplitude of the equivalent torque signal τ_ext exceeds 100 N·m, an emergency compliant retreat is triggered. The direction of the retreat joint velocity is determined by θ_1_escape = -k_e·τ_ext / ||τ_ext|, and the retreat velocity gain k_e = (||τ_ext| - 50) / (150 - 50)·θ_1_max. After the retreat is completed, the system reports the contact event to the upper-level operation management system, which then decides whether to adjust the subsequent operation path or request manual confirmation.

[0080] Finally, it should be noted that the above are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A collision avoidance system for the excavation section of a bucket unloader based on virtual flexible body and admittance control, comprising: A multi-degree-of-freedom torque sensing module is installed at each active joint of the excavator's suspension mechanism to detect in real time the equivalent torque generated by external contact forces on each active joint. The contact state classification module is communicatively connected to the multi-degree-of-freedom torque sensing module. It is used to receive the equivalent torque signal, extract the time-domain and frequency-domain features of the equivalent torque signal, and identify the type of contact object based on the time-domain and frequency-domain features. The virtual impedance parameter scheduling module is communicatively connected to the contact state classification module and is used to dynamically generate the target virtual inertia matrix M_d, the target virtual damping matrix D_d, and the target virtual stiffness matrix K_d according to the type of the contact object and the current working mode. The force-potential hybrid admittance control module is communicatively connected to the virtual impedance parameter scheduling module. It is used to receive M_d, D_d, and K_d, as well as the desired joint position θ_d output by the path planner, where θ_d is the desired joint position vector. It calculates the joint position correction amount Δθ in real time according to the force-potential admittance relationship, where Δθ is the joint position correction amount vector, and outputs the admittance-corrected joint position command θ_cmd, where θ_cmd = θ_d + Δθ. The joint torque servo module is communicatively connected to the force-position hybrid admittance control module. It is used to receive θ_cmd and drive each joint actuator through the inner loop torque controller, so that the excavator exhibits compliant mechanical characteristics that conform to M_d, D_d and K_d in response to external contact forces.

2. The anti-collision system for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 1, characterized in that, The force-potential hybrid admittance control module is based on the following second-order impedance dynamic equation: M_d(θ_2-θ_2d)+D_d(θ_1-θ_1d)+K_d(θ-θ_d)=-τ_ext Where θ is the actual joint position, θ_1 is the actual joint angular velocity, θ_2 is the actual joint angular acceleration, θ_2d is the desired joint angular acceleration, θ_1d is the desired joint angular velocity, and τ_ext is the measured external equivalent torque.

3. The anti-collision system for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 1, characterized in that, The contact state classification module extracts time-domain features including peak torque, force rise rate, and decay time constant, and frequency-domain features including high-frequency energy ratio and low-frequency energy ratio, wherein the high-frequency energy ratio is the ratio of energy in the frequency band above 50Hz to the total energy, and the low-frequency energy ratio is the ratio of energy in the frequency band below 30Hz to the total energy. The contact state classification module uses the force rise rate, the high-frequency energy ratio, the peak torque, and the decay time constant as input variables, and the contact object type as the output variable. The contact object type includes rigid bulkheads, rigid beams, and large pieces of material.

4. The anti-collision system for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 1, characterized in that, The current operating modes include free material handling mode, wall sliding mode, and emergency compliant retreat mode. In the free material handling mode, the virtual impedance parameter scheduling module sets K_d to the preset maximum stiffness value K_max and D_d to the preset normal damping value D_normal. In the wall sliding mode, K_d is set to the preset minimum stiffness value K_min and D_d is set to the preset high damping value D_high, and the target contact torque is controlled within the range of 5 N·m to 10 N·m. Among them, K_max and K_min satisfy K_max ≥ 20 × K_min.

5. The anti-collision system for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 4, characterized in that, The virtual impedance parameter scheduling module performs parameter transitions according to the following formula when switching operating modes: K_d(k)=α·K_d(k-1)+(1-α)·K_d_target D_d(k)=α·D_d(k-1)+(1-α)·D_d_target Wherein, K_d_target is the preset virtual stiffness target value corresponding to the current working mode, D_d_target is the preset virtual damping target value corresponding to the current working mode, and α is the smoothing coefficient, with a value range of 0.90 to 0.

95.

6. The anti-collision system for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 1, characterized in that, It also includes a safety monitoring coprocessor independent of the main control processor. The safety monitoring coprocessor independently monitors the raw signals of the torque sensors of each active joint with a control cycle of no more than 0.5ms. If any torque sensor signal exceeds the preset absolute stop threshold τ_max_stop, the power supply is cut off and the brake is triggered through hardware circuitry. The safety monitoring coprocessor is also used to monitor the communication heartbeat signal with the main control processor. If the communication heartbeat signal is interrupted for more than 50ms, the safety brake is automatically triggered.

7. A collision avoidance method for the excavation section of a bucket unloader based on virtual flexible body and admittance control, comprising the following steps: S1, by using torque sensors installed at each active joint of the excavator suspension mechanism, the equivalent torque signal τ_ext generated by the external contact force on each active joint is acquired in real time; S2, perform time-domain feature extraction and frequency-domain feature extraction on the equivalent torque signal τ_ext, and identify the type of contact object based on the extracted time-domain features and frequency-domain features. The type of contact object includes rigid bulkheads, rigid beams and large pieces of material. S3, based on the type of contact object identified in step S2 and the current working mode, dynamically schedule the virtual impedance parameters, which include the virtual inertia matrix M_d, the virtual damping matrix D_d, and the virtual stiffness matrix K_d; S4. Based on the following force-position admittance relationship, the joint position correction Δθ is calculated in real time from the equivalent torque signal τ_ext, and the admittance-corrected joint position command θ_cmd = θ_d + Δθ is generated: M_d(θ_2-θ_2d)+D_d(θ_1-θ_1d)+K_d(θ-θ_d)=-τ_ext Where θ_d is the desired joint position output by the path planner, θ is the actual joint position, θ_1 is the actual joint angular velocity, θ_2 is the actual joint angular acceleration, θ_2d is the desired joint angular acceleration, and θ_1d is the desired joint angular velocity. S5, using the joint position command θ_cmd output in step S4 as the input to the inner ring torque controller, drive each joint actuator to make the excavating part exhibit compliant mechanical characteristics that conform to the virtual impedance parameters in response to external contact forces.

8. The anti-collision method for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 7, characterized in that, The time-domain features extracted in step S2 include peak torque, force rise rate, and decay time constant; the frequency-domain features extracted include high-frequency energy ratio and low-frequency energy ratio. In step S2, when the force rise rate exceeds a first preset threshold and the high-frequency energy ratio exceeds a second preset threshold, the contact object is identified as a rigid component; when the force rise rate is lower than a third preset threshold and the low-frequency energy ratio exceeds a fourth preset threshold, the contact object is identified as a large piece of material.

9. The anti-collision method for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 7, characterized in that, The rules for dynamically scheduling the virtual stiffness matrix K_d in step S3 are as follows: in the free material feeding mode, K_d is set to the preset maximum stiffness value K_max; in the wall sliding mode and the emergency compliant retreat mode, K_d is set to the preset minimum stiffness value K_min; and a first-order low-pass filter is used for smooth transition when switching parameters.

10. The anti-collision method for the excavation section of a bucket unloader based on virtual flexible body and admittance control according to claim 7, characterized in that, The process also includes step S6: When the amplitude of the equivalent torque signal τ_ext exceeds a preset emergency response threshold, an emergency compliant retreat is triggered, and the direction of the retreating joint velocity is determined by the following formula: θ_1_escape=-k_e·τ_ext / ‖τ_ext‖ k_e=(‖τ_ext‖-τ_safe) / (τ_max_stop-τ_safe)·θ_1_max Where τ_safe is the safe contact force threshold, τ_max_stop is the maximum stop threshold, θ_1_max is the maximum allowable speed of the joint, and k_e is the escape velocity gain.

Images