Mechanical arm operation safety distance dynamic control method based on radio frequency electromagnetic wave sensing and real-time decision

Abstract

This invention relates to the field of robotic arm control technology and discloses a dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making. The method includes: acquiring real-time dynamic state data of the robotic arm's end effector and the straight-line distance to the nearest obstacle; constructing a dynamic safe braking distance model based on the dynamic state data; calculating a dynamic safety threshold matching the current motion state; calculating the spatial margin between the obstacle's straight-line distance and the dynamic safety threshold; and performing hierarchical decision-making control based on the interval of the spatial margin. This invention calculates the dynamic safety threshold using a dynamic safe braking distance model, eliminating the risk of false triggering and collisions caused by fixed thresholds. Furthermore, it replaces the traditional rigid emergency stop with a multi-level compliant avoidance strategy using hierarchical decision-making control, thereby maximizing the stable operation of production equipment while balancing operational efficiency and safety.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, specifically to a dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making. Background Technology

[0002] With the continuous development of industrial automation technology, robotic arms have been widely used in modern intelligent manufacturing. In dynamic production line scenarios where human-machine collaboration is increasingly frequent, the safety of robotic arm operations has become a crucial issue in control system design, as operators and robotic arms often reside in the same workspace. Ensuring that robotic arms can operate efficiently while guaranteeing personnel safety in complex and dynamic human-machine interaction environments is a key research direction in the field of industrial robot control technology.

[0003] Currently, safety measures for robotic arm operations mainly rely on installing fixed safety light curtains or visual sensors outside the work area to monitor personnel and obstacles. Regarding safety distance control strategies, traditional control schemes typically set a fixed, conservative braking distance threshold for the system. When external sensors detect an obstacle entering this fixed distance threshold range, the system triggers the robotic arm's braking and stopping mechanism to avoid a collision.

[0004] However, the aforementioned existing technologies have some limitations in practical applications: a fixed, conservative braking distance threshold is a static control strategy, which is difficult to adapt to the dynamic changes of the robotic arm under different loads and movement speeds. Specifically, when the robotic arm operates at low speeds or light loads, due to the small system inertia, a fixed, large safety threshold can easily lead to frequent false triggering of the system, resulting in unnecessary shutdowns and reduced production efficiency. At high speeds or under heavy loads, the robotic arm contains significant kinetic energy. If the fixed safety distance threshold is insufficient, the braking process cannot exhaust the kinetic energy within the set threshold space, which can easily lead to squeezing or collision accidents, posing a significant safety hazard. Therefore, there is an urgent need for a dynamic control method for the safe distance of robotic arm operations based on radio frequency electromagnetic wave sensing and real-time decision-making to solve these problems. Summary of the Invention

[0005] To address the problems in related technologies, this invention provides a dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, thereby overcoming the aforementioned technical problems in existing related technologies.

[0006] To solve the aforementioned technical problem, the present invention is achieved through the following technical solution:

[0007] In a first aspect, embodiments of the present invention provide a dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, specifically including: acquiring in real-time dynamic state data of the robotic arm's end effector and the straight-line distance to the nearest obstacle; constructing a dynamic safe braking distance model based on the dynamic state data, calculating a dynamic safety threshold that matches the current motion state; calculating the spatial margin between the straight-line distance to the obstacle and the dynamic safety threshold, and performing hierarchical decision control based on the interval in which the spatial margin is located.

[0008] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making as described in this invention, the dynamic state data includes the real-time motion speed and real-time acceleration of the robotic arm end effector and the load mass grasped by the actuator.

[0009] As a preferred embodiment of the dynamic control method for safe working distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to the present invention, the method for obtaining the real-time motion speed and real-time acceleration of the robotic arm end effector is as follows:

[0010] The real-time motion velocity and real-time acceleration of the robotic arm's end effector are obtained by projecting the three-dimensional linear velocity vector and three-dimensional linear acceleration vector of the end effector in the base coordinate system onto the unit direction normal vector pointing from the end effector to the nearest obstacle.

[0011] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making as described in this invention, the method for obtaining the straight-line distance of the nearest obstacle to the actuator is as follows:

[0012] A flexible radio frequency sensing module conformally wrapped around the outside of the actuator transmits a frequency-modulated continuous wave signal and receives its reflected echo. The straight-line distance to the obstacle is then calculated based on the frequency difference information between the reflected echo and the transmitted signal.

[0013] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to the present invention, the method for determining the dynamic safety threshold that matches the current motion state is as follows:

[0014] The dynamic safety threshold is determined by combining the basic physical buffer distance, the delay stage displacement, and the ultimate physical displacement required to consume the system's kinetic energy.

[0015] The method for obtaining the displacement during the delay stage is as follows: combining the compensation coefficient, the motion trend of the robotic arm during the total response delay time is predicted based on the uniformly accelerated linear kinematics equation to obtain the displacement during the delay stage.

[0016] The method for obtaining the ultimate physical displacement is as follows: the total kinetic energy of the system is calculated by combining the basic inertial mass of the robotic arm with the load mass currently grasped by the actuator, and the ultimate physical displacement is determined based on the kinetic energy theorem and the maximum equivalent braking force of the system.

[0017] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making as described in this invention, the method for obtaining the compensation coefficient is as follows:

[0018] The equivalent damping ratio of the entire robotic arm is obtained, and the compensation coefficient is determined by adding one to the product of the equivalent damping ratio and the preset gain coefficient.

[0019] As a preferred embodiment of the dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to the present invention, the hierarchical decision-making control based on the interval where the space margin is located specifically includes:

[0020] The difference between the actual straight-line distance to the obstacle and the dynamic safety threshold is defined as the spatial margin; and warning grace zone, retreat judgment threshold and extreme tolerance threshold are set.

[0021] When the space margin is greater than the warning width limit, it is determined to be a normal operating area, and the robotic arm operates normally according to the original trajectory and speed plan;

[0022] When the space margin is greater than zero and less than or equal to the warning width band, it is determined to be a deceleration operation area, and the system outputs a smooth deceleration command to execute deceleration operation;

[0023] When the space margin is greater than the negative value of the back-off determination threshold but less than or equal to zero, it is determined to be a paused operation area, and the driver outputs torque to maintain the current pose.

[0024] When the space margin is greater than the negative value of the limit tolerance threshold and less than or equal to the negative value of the back-off determination threshold, it is determined to be a back-off running area, and a reverse compliant back-off command is generated.

[0025] When the space margin is less than or equal to the negative value of the limit tolerance threshold, it is determined to be an emergency stop zone, triggering the underlying hardware to cut off power and engage the brake to perform an emergency stop.

[0026] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making as described in this invention, the method for obtaining the target speed during deceleration is as follows:

[0027] The target speed is determined by multiplying the real-time motion speed by a smooth decay factor raised to a specific base.

[0028] The specific base is the quotient of the space margin divided by the warning grace band.

[0029] As a preferred embodiment of the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to the present invention, the method for obtaining the attenuation factor is as follows:

[0030] The attenuation factor is determined by the ratio of the rated continuous braking force to the maximum peak braking force of the servo motor, combined with a preset safety margin coefficient.

[0031] Secondly, embodiments of the present invention provide a dynamic control system for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, comprising: a data acquisition module for acquiring in real-time dynamic state data of the robotic arm end effector and the straight-line distance to the nearest obstacle; a dynamic safety threshold calculation module for constructing a dynamic safety braking distance model and calculating a dynamic safety threshold matching the current motion state; a space margin assessment module for calculating the space margin between the straight-line distance to the obstacle and the dynamic safety threshold; and a hierarchical decision control module for performing hierarchical decision control based on the interval in which the space margin is located.

[0032] The present invention has the following beneficial effects:

[0033] 1. This invention acquires the speed and acceleration of the robotic arm's end effector and the current load mass grasped by the actuator in real time. Taking into account the total response delay time and the ultimate physical braking capability of the system, a dynamic safe braking distance model is constructed. A dynamic safety threshold matching the current motion state is calculated, enabling the robotic arm to automatically retract the safety boundary to avoid frequent false triggers and shutdowns when operating at low speed or light load. At high speed and heavy load, it can provide sufficient braking space to prevent squeezing and collision accidents. This eliminates the false triggering and collision hazards caused by fixed thresholds, and balances work efficiency and safety.

[0034] 2. This solution calculates the spatial margin between the straight-line distance to the obstacle and the dynamic safety threshold, and sets a warning grace zone, a reverse judgment threshold, and an extreme tolerance threshold. Based on the different ranges of the spatial margin, it can perform graded decision control such as deceleration, suspension, reverse operation, or emergency stop, avoiding the traditional one-size-fits-all emergency braking. Under the premise of ensuring the bottom line of personnel safety, it maintains the stable operation of production equipment and the continuity of the process to the greatest extent.

[0035] Of course, any product implementing this invention does not necessarily need to achieve all of the advantages described above at the same time. Attached Figure Description

[0036] To more clearly illustrate the technical solutions of the embodiments of the invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For those skilled in the art, the drawings can be obtained from these drawings without creative effort.

[0037] Figure 1 The present invention provides a flowchart of a dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making.

[0038] Figure 2 This is a schematic diagram of the process S3 provided by the present invention.

[0039] Figure 3 This invention provides a schematic diagram of a dynamic control system for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making. Detailed Implementation

[0040] 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.

[0041] Example 1

[0042] Current robotic arm safety protection mainly relies on externally mounted fixed safety light curtains or vision sensors. These traditional control schemes typically set a fixed, conservative braking distance threshold, which is difficult to adapt to the dynamic changes of the robotic arm under different loads and speeds. Specifically, when the robotic arm is operating at low speed or light load, the fixed, large threshold is prone to frequent false triggering, leading to shutdown. On the other hand, under high speed and heavy load, if the threshold is not set sufficiently, crushing and collision accidents are likely to occur.

[0043] To solve the above technical problems, such as Figure 1 As shown, Embodiment 1 of the present invention provides a dynamic control method for the safe distance of a robotic arm operation based on radio frequency electromagnetic wave sensing and real-time decision-making. Specifically, Embodiment 1 takes a material grasping scenario of a six-axis collaborative robotic arm on a dynamic production line with personnel movement as an example: by wrapping a flexible skin-like structure on the side of the actuator (such as a suction cup) at the end of the robotic arm, a specially designed transmitting and receiving antenna is evenly distributed inside the flexible skin-like structure to emit radio frequency signals; the flexible skin-like structure as a whole is equivalent to a sensor, which directly detects the distance information of surrounding obstacles in real time at the closest end of the grasping point.

[0044] The specific implementation process is as follows: First, the dynamic state data of the robotic arm's end effector and the straight-line distance to the nearest obstacle are acquired in real time. This method integrates the dynamic state data of the robotic arm's internal servo system with the flexible radio frequency sensor data from the end effector to achieve real-time near-end detection without blind spots, overcoming the blind zone limitations of traditional external sensors. It also provides data parameters for the subsequent construction of a dynamic safety braking distance model. Based on the dynamic state data, a dynamic safety braking distance model is constructed, and a dynamic safety threshold matching the current motion state is calculated. This method eliminates the risk of false triggering and collisions caused by fixed thresholds, enabling the robotic arm to automatically retract its safety boundary during low-speed or light-load operations to avoid frequent false triggering and shutdowns, while providing sufficient physical braking space during high-speed, heavy-load operations to prevent squeezing and collision accidents. The spatial margin between the straight-line distance to the obstacle and the dynamic safety threshold is calculated, and hierarchical decision control is executed based on the interval where the spatial margin is located. This method breaks the rigid control logic of traditional safety control schemes that require immediate action upon stopping, avoiding the traditional one-size-fits-all emergency braking. Under the premise of ensuring the safety of personnel, it maximizes the stable operation of production equipment and the continuity of the process.

[0045] Furthermore, to better illustrate the technical solution of Embodiment 1 of the present invention, a detailed description is provided of the dynamic control method for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, specifically including the following:

[0046] S1. Real-time acquisition of dynamic state data and obstacle distance information of the robotic arm's end effector, specifically including the following sub-steps:

[0047] S11. Using the encoder of the servo system inside the robotic arm to fix the sampling period. Synchronously obtain the current time The real-time motion speed of the robotic arm's end effector in Cartesian space and real-time acceleration The specific implementation steps are as follows:

[0048] S111: At the current moment By reading the absolute encoders of the servo motors at each joint of the robotic arm, the data obtained is... Joint position vector composed of joint angles To avoid interference from high-frequency noise in the kinematics calculation, the original angle data is low-pass filtered, and a fixed sampling period is used. The joint velocity vector is solved in real time using the first-order backward differential discretization formula. With joint acceleration vector :

[0049] ;

[0050] ;

[0051] In the formula, For a moment The joint position vector of the robotic arm; For a moment The joint velocity vector; For a moment The joint acceleration vector; This is the system's fixed sampling period.

[0052] For example: In this embodiment, to avoid high-frequency noise being amplified step by step in subsequent velocity and acceleration calculations, leading to system control divergence, the specific implementation steps for low-pass filtering of the original angle data are as follows:

[0053] S111a: In each sampling period Internally, the original joint position vectors, including high-frequency quantization noise and electromagnetic interference, are extracted in real time from the communication bus of the absolute encoders of each joint servo motor. .

[0054] S111b: The system constructs a first-order discrete low-pass filter (i.e., an exponentially weighted moving average model) in the underlying driver to perform noise filtering with low latency. Its expression is as follows:

[0055] ;

[0056] Furthermore, the time constant in the above formula Based on the structural resonant frequency and encoder noise characteristics of the robotic arm under its operating conditions, the target cutoff frequency can be set. The calculation yielded the following:

[0057] ;

[0058] In the formula, For a moment The smoothed joint position vector output after low-pass filtering will be used directly as the input source for solving the Jacobian matrix and joint velocity and acceleration. For a moment The encoder directly reads the unprocessed raw joint position vector; The previous sampling time The joint position vector latched after low-pass filtering; This refers to the fixed servo sampling period of the system. The physical time constant of the first-order low-pass filter reflects the hysteresis speed of the system's response to a step input. The target cutoff frequency of the low-pass filter is usually set according to the Nyquist sampling theorem and practical control requirements. .

[0059] S111c: At the current moment After the filtering calculation is completed, the current smooth joint position vector is... Write to the circular memory buffer of the underlying controller, overwrite the oldest discarded data, and change its state to the historical state parameters of the next cycle. This completes the discrete-time axis rolling recursion of the filtering algorithm.

[0060] S112: Based on the standardized Denavit-Hartenberg (DH) parameter table for the robotic arm, establish a homogeneous transformation matrix from the base coordinate system to the end effector coordinate system. This is based on the joint position vector at the current moment. The translation components are extracted, and by taking partial derivatives with respect to the forward kinematics equations, a linear velocity Jacobian matrix mapping the joint space and Cartesian space is constructed in real time. , it is A matrix of order 1. The specific implementation steps are as follows:

[0061] S112a: For those with For a collaborative robotic arm with multiple rotary joints, extract its factory-calibrated standardized DH parameter set. At the current moment According to the real-time joint position vector obtained by S111 , construct the first The coordinate system of the first link relative to the first link coordinate system Homogeneous transformation matrix of a link coordinate system :

[0062] ;

[0063] In the formula, The joint number has a range of values. ; For a moment The joint angle, that is, around The angle of rotation of the axis; The link offset, i.e., along The distance of the axis translation; The length of the link, i.e., along The distance of the axis translation; This refers to the torsion angle of the connecting rod, i.e., the angle around the connecting rod. The angle of rotation of the axis; To describe the relative pose of two adjacent joints A homogeneous transformation matrix, where the elements in the first three rows and fourth column represent spatial translation vectors, with the top left corner... Submatrices represent spatial rotation states.

[0064] S112b: Calculate the coordinates from the base coordinate system (index 0) to the end effector coordinate system (index 1) by performing a chain multiplication using the homogeneous transformation matrices of each joint. The cumulative homogeneous transformation matrix of ) :

[0065] ;

[0066] In the formula, For the end effector relative to the base Real-time rotation matrix; The origin of the end effector is in the base coordinate system Real-time Cartesian space position vector.

[0067] In the above chain multiplication operation, the system synchronously caches and retrieves any number of multiplication operations. Transformation matrices of the link coordinate system relative to the base Extract the corresponding origin position vector from it. and Unit vector along the axis (That is, the third column of the rotatable submatrix). To maintain logical closed loop, the initial state of the absolute base is defined as follows: and .

[0068] S112c: Regarding the embodiment where... A robotic arm composed of several rotary joints uses the vector cross product method in differential kinematics to dynamically construct a mapping from joint angular velocity to end-effector linear velocity. Linear velocity Jacobian matrix Its first column vector The real-time calculation formula is as follows:

[0069] ;

[0070] The above The column vectors are concatenated in joint order to output the complete linear velocity Jacobian matrix:

[0071] ;

[0072] In the formula, The first Jacobian matrix Column component, representing the first Each joint When rotating at a unit angular velocity, the Cartesian linear velocity vector generated at the end effector; For a moment No. The absolute direction vectors of the effective rotation axes of each joint in the base coordinate system are dimensionless unit vectors. For a moment From the The displacement vector of the lever arm pointing from the origin of the joint space coordinate system to the origin of the end effector; For complete 1-order matrix.

[0073] S113: Using the calculated linear velocity Jacobian matrix and its time derivative matrix By mapping the kinematic state of the joint space to Cartesian space, the real-time three-dimensional linear velocity vector of the end effector in the base coordinate system is calculated. With real-time three-dimensional linear acceleration vector :

[0074] ;

[0075] ;

[0076] In the formula, For a moment The three-dimensional linear velocity vector of the robotic arm's end effector in Cartesian space; For a moment The three-dimensional linear acceleration vector of the robotic arm's end effector in Cartesian space; The basic linear velocity Jacobian matrix has the following dimensions for its elements: ; Let be the time derivative of the Jacobian matrix, and the dimensions of the matrix elements be... .

[0077] S114: Receives the unit direction normal vector of the obstacle relative to the end of the robotic arm, calculated by the flexible radio frequency sensing end module. The three-dimensional linear velocity vector and three-dimensional linear acceleration vector of the end effector are projected onto the collision hazard direction using a dot product to obtain the real-time motion velocity in the obstacle direction. and real-time acceleration :

[0078] ;

[0079] ;

[0080] In the formula, The unit direction normal vector pointing from the end to the nearest obstacle is dimensionless. For a moment The real-time speed of the robotic arm's end effector in the direction of obstacle approach, when This indicates that the end is approaching an obstacle; For a moment Real-time acceleration of the robotic arm end effector in the direction of obstacle approach.

[0081] S12. Combining the robotic arm's dynamics model with a six-axis force sensor located at the end flange, the current load mass grasped by the actuator is identified and acquired in real time. .

[0082] S13. A flexible radio frequency sensing end module conformally wrapped around the robotic arm actuator (such as the outside of a vacuum suction cup) emits a frequency-modulated continuous wave radio frequency signal and receives its reflected echo. Based on the frequency difference information between the reflected echo and the emitted signal, the straight-line distance to the nearest obstacle to the actuator is calculated in real time. The specific implementation steps are as follows:

[0083] S131: Controls the flexible RF sensing end module, conformally wrapped around the actuator, to emit a linear frequency modulated continuous wave. This module employs a multi-layered composite anti-interference and anti-deformation design in its physical structure: First, a flexible artificial magnetic conductor (AMC) metamaterial isolation layer is laid between the bottom layer of the flexible printed circuit (based on a polyimide PI substrate) and the metal surface of the robotic arm. This layer flips the electromagnetic wave reflection phase of the metal backplane from 180 degrees to 0 degrees, eliminating the electric field short-circuit effect caused by direct contact with the metal surface and ensuring that the antenna radiation pattern is not distorted in a metallic environment. Second, the microstrip patch antenna array on the flexible printed circuit adopts an island-bridge structure layout, placing the microstrip patch transmitting and receiving antennas in an island region locally solidified with a high Young's modulus material, ensuring constant geometric dimensions of the radiating elements; the RF feed lines connecting each antenna are arranged in a serpentine microstrip line within the ultra-thin, flexible bridge region. This structure concentrates and releases the stress generated by mechanical deformation in the bridge region, ensuring stable antenna resonant frequency.

[0084] Driven by a voltage-controlled oscillator (VCO) at the radio frequency front end, the microstrip patch transmitting antenna transmits a frequency-modulated continuous wave (Chirp signal) whose frequency varies linearly with time.

[0085] S132: The system integrates a closed-loop dynamic impedance matching network between the RF front-end voltage-controlled oscillator and the microstrip patch transmitting antenna. A varactor diode is used as a tunable element, and the reflection coefficient at the antenna port is monitored in real time via an RF directional coupler. When the flexible skin is subjected to extreme compression, causing the impedance to deviate from the rated value, the underlying controller calculates and adjusts the bias voltage of the varactor diode in real time to dynamically compensate for the equivalent distributed capacitance deviation introduced by the deformation, maintaining efficient RF energy radiation.

[0086] When the transmitted radio frequency electromagnetic waves encounter surrounding obstacles, they generate reflected echoes, which are captured by the receiving antenna. The received echo signals are amplified, mixed with the transmitted signals, and the difference frequency intermediate frequency signal is extracted through a hardware low-pass filter.

[0087] S133: The intermediate frequency signal output above is discretized and sampled using a high-speed analog-to-digital converter (ADC), converting it into a digital signal sequence. The system processor performs a fast Fourier transform (FFT) on this digital sequence in real time, transforming the time-domain signal into the frequency domain for analysis.

[0088] In the generated frequency domain amplitude spectrum, the algorithm searches in real time and extracts the effective main peak whose amplitude exceeds the preset environmental background signal-to-noise ratio threshold and is closest to zero frequency. The center frequency corresponding to this main peak is the fundamental frequency of the difference frequency representing the physical obstacle closest to the actuator. .

[0089] S134: Based on the physical laws of electromagnetic wave propagation in space and the geometric similarity mapping principle of linear frequency modulated continuous waves, the system first defines the round-trip flight time delay of electromagnetic waves. Distance from actual distance Physical connection:

[0090] ;

[0091] The difference frequency is established based on the slope of the time-frequency linear mapping of the linear frequency modulation waveform. With flight delay time Proportional relationship:

[0092] ;

[0093] By combining and simplifying the above two equations, the straight-line distance to the nearest obstacle to the actuator at the current moment can be calculated in real time. :

[0094] ;

[0095] In the formula, For a moment The straight-line distance to the nearest obstacle to the actuator; Let $\frac{ ... ; The duration of a single linear frequency modulation cycle (Chirp cycle) set for the radio frequency system; For a moment The effective difference frequency (fundamental frequency) of the intermediate frequency signal extracted by fast Fourier transform; The effective modulation sweep bandwidth of the frequency-modulated continuous wave is set for the radio frequency system; This refers to the time delay of electromagnetic waves traveling to and from the target.

[0096] It should be noted that the effective modulation sweep bandwidth of the conventional radio frequency system is set to 24GHz band, which is limited by 250MHz bandwidth, resulting in low physical distance resolution. In this embodiment, the radio frequency system adopts 60GHz industrial band or 79GHz millimeter wave high-resolution band to ensure the measurement accuracy of the straight distance to obstacles.

[0097] In this embodiment 1, by fusing the dynamic state data of the internal servo system of the robotic arm with the flexible radio frequency sensing data of the end effector, near-end real-time detection without blind spots is achieved, overcoming the blind zone limitation of traditional external sensors. Specifically, for example, when a six-axis collaborative robotic arm performs a material grasping task on a dynamic production line with personnel movement, if the operator suddenly approaches and happens to be within the physical blind zone of the external vision sensor, the microstrip patch antenna on the flexible printed circuit conformally covering the actuator (such as a vacuum suction cup) can still emit linear frequency modulated continuous electromagnetic waves without interference, detecting and calculating the straight-line distance of the operator closest to the actuator. Simultaneously, the system internally projects the three-dimensional linear velocity vector and linear acceleration vector of the robotic arm onto the direction of the collision hazard where the operator is located, and combines this with the real-time load mass identified by the force sensor. This process allows us to understand the true kinematic state and potential inertial impact force of the robotic arm in the approximation direction. This step combines internal sensing with external detection, providing data parameters for the subsequent construction of a dynamic safety braking distance model.

[0098] S2. Construct a dynamic safe braking distance model and calculate the dynamic safety threshold that matches the current motion state. This includes the following sub-steps:

[0099] S21: Set the basic physical buffer distance It is used to compensate for the physical thickness of the flexible radio frequency sensor itself, mechanical assembly tolerances, and the minimum human-machine safety clearance specified by the standard (usually set to 0.05m to 0.1m), to ensure that a physical isolation layer still exists even in the most extreme ideal static state.

[0100] S22: The total response delay time from when the system processor receives the out-of-bounds signal from the RF radar, to algorithm processing, communication bus issuing commands, and then the servo driver cutting off the current and triggering the mechanical brake. Within this time window, the robotic arm has not yet begun to decelerate and continues to move forward due to inertia, following its original trajectory.

[0101] Based on the fundamental equations of uniformly accelerated linear kinematics, the real-time velocity of the obstacle approaching in the current moment, obtained from S11, is extracted. With real-time acceleration The theoretical displacement during the delay phase is calculated. Furthermore, considering the non-rigid transmission backlash and flexible deformation amplification effect between the robotic arm joint reducer and the connecting rod, a compensation coefficient is introduced. ( Displacement error amplification compensation is performed to construct the displacement in the delayed stage. :

[0102] ;

[0103] For example: In this embodiment, the compensation coefficient The method for determining it is as follows:

[0104] S21a: During the initial deployment phase of the robotic arm, standardized experimental modal analysis (EMA) is performed on the entire robotic arm. A transient pulse excitation is applied to the end effector using a force hammer, and a high-frequency accelerometer synchronously acquires the three-dimensional free decaying oscillation signal of the end effector. Using the logarithmic decay rate method, the structural damping ratio of the robotic arm at the first dominant resonant frequency is calculated and extracted from the physical time-domain decay curve. .

[0105] S22a: The end-position forward thrust error caused by non-rigid transmission during the braking delay phase of the robotic arm is equivalent in system dynamics to the dynamic overshoot process of an underdamped second-order system on the trajectory of an ideal rigid body.

[0106] Based on the analytical solution of the maximum overshoot of the step response of a second-order linear time-invariant (LTI) system in control theory, the compensation coefficient is derived. The expression is:

[0107] ;

[0108] Furthermore, in order to accurately capture the nonlinear dynamic response of the robotic arm under different spatial poses, Expressed as the equivalent damping ratio in the end direction mapped to Cartesian space, the specific calculation formula is as follows:

[0109] ;

[0110] ;

[0111] ;

[0112] ;

[0113] In the formula, For a moment Real-time joint position vectors of the robotic arm; The real-time linear velocity Jacobian matrix; Let be the joint space inertia matrix in the current pose; The equivalent joint space stiffness matrix of each joint of the robotic arm is determined by the coupling between servo gain and mechanical structure. The equivalent joint space damping matrix for each joint of the robotic arm; The terminal equivalent operation space inertia matrix mapped to the Cartesian workspace; This is the end-effector equivalent operation space stiffness matrix; This is the end-effector equivalent operating space damping matrix; It is the unit direction normal vector pointing from the end to the nearest obstacle.

[0114] S23: When the system has elapsed the delay time Then, the robotic arm's bottom brake engages at full power. At this point, the robotic arm's initial braking speed in the obstacle approaching direction... The acceleration has changed due to previous acceleration; its value is:

[0115] ;

[0116] During the braking phase, the system extracts the load mass identified in real time in S12. Combined with the basic inertial mass of the robotic arm itself Calculate the total kinetic energy of the system at the moment of braking. :

[0117] ;

[0118] Because the mapping relationship between the joint torques and the force in a specific direction at the end effector differs significantly depending on the spatial pose of the robotic arm, the system needs to calculate in real time the maximum equivalent braking force along the obstacle approaching direction of the robotic arm in its current pose, based on the principle of static mapping. The specific calculation formula is as follows:

[0119] ;

[0120] In the formula: For a moment The maximum equivalent braking force along the direction of approach to the obstacle; The ultimate tensile or compressive strength of the physical structure that the end flange or link of the robotic arm can withstand, used to set the upper limit of mechanical strength; This represents the total number of joints in the robotic arm; For the first The maximum peak braking torque that can be provided by the joint servo motor and mechanical brake; For a moment The transpose of the linear velocity Jacobian matrix, which transmits static force from joint space to Cartesian space; The unit direction normal vector pointing from the end to the nearest obstacle; This indicates taking the first result of the vector calculation within the parentheses. Each component element; It is a very small positive real number, used to prevent calculation divergence where the denominator is zero when the robotic arm is in a singular pose or the lever arms are orthogonal.

[0121] The above formula finds the moment limit of the current collision normal vector by iterating through the moment limits of all joints. The shortest joint that first reaches torque saturation is identified as the bottleneck of dynamic braking force for the entire robotic arm system.

[0122] According to the work-energy theorem, the brake does negative work, consuming all kinetic energy to bring the robotic arm's speed back to zero. The braking distance is... Then the work equation is satisfied. Therefore, the ultimate physical displacement required for the braking phase can be derived. :

[0123] .

[0124] S24: The basic physical buffer distance of the above three physical stages Delayed stage displacement and ultimate physical displacement By performing linear superposition, a dynamic safe braking distance model covering the entire braking cycle is constructed:

[0125] ;

[0126] Substitute into the above formula:

[0127] ;

[0128] In the formula, For a moment Dynamic security threshold; Based on the basic physical buffer distance; The compensation coefficient; Real-time motion speed; For real-time acceleration; Total response delay time; For the basic inertial mass of the robotic arm; The quality of the load captured by the current actuator; This is the maximum equivalent braking force.

[0129] For example: In this embodiment, the basic inertial mass of the robotic arm The calculation formula is as follows:

[0130] ;

[0131] ;

[0132] In the formula, The unit direction normal vector pointing from the end to the nearest obstacle; For a moment The three-dimensional translational inertia matrix of the robotic arm's end effector in the Cartesian workspace; For a moment Real-time build The linear velocity Jacobian matrix of order 1; For a moment robotic arm in joint space The positive definite symmetric inertia matrix is ​​composed of the link mass, the relative position vector of the center of mass, the inertia tensor, and the real-time joint position vector of each link of the robotic arm. Determined by the Lagrange dynamics equations; Representing the Moore-Penrose generalized inverse of a matrix, when the robotic arm is in a singular pose that causes the Jacobian matrix to be reduced in rank, the pseudo-inverse can output the minimum norm solution to maintain the continuous calculation of physical dimensions.

[0133] In this embodiment 1, the basic physical buffer distance of the system itself is comprehensively considered. Inertial forward displacement during the total hardware response delay time And the ultimate physical braking displacement required for the robotic arm to consume total kinetic energy. A dynamic safe braking distance model covering the entire braking cycle was constructed to obtain a dynamic safety threshold that matches the current motion state. This operation can control the real-time motion speed of the robotic arm's end effector. Real-time acceleration and current load quality By converting dynamic physical parameters into corresponding spatial safety threshold requirements, the limitations of traditional control schemes that use fixed, conservative braking distance thresholds are eliminated. This allows the system to automatically shrink the safety boundary during low-speed or light-load operations to avoid frequent false triggers and shutdowns, while adaptively increasing the threshold during high-speed, heavy-load operations to provide sufficient physical braking space. This maximizes the effective working space of the robotic arm and ensures the efficiency of the production line while preventing crushing or collision accidents. Specifically, for example, setting the basic physical buffer distance for the six-axis collaborative robotic arm... The compensation coefficient is 0.05m. The total response latency is 1.1. The basic inertial mass is 0.02s. It weighs 5 kg and has a maximum equivalent braking force. It is 500N. At a certain moment... The system measured that the robotic arm was moving at... A speed of 1.5 m / s Moving towards the target point with an acceleration of 2 m / s², and the current grasped load mass The value is 10kg. Substituting the above parameters into the formula, the current dynamic safety threshold is calculated in real time. It is approximately 0.12m. The system uses this distance as the minimum braking boundary within this extremely short time window.

[0134] S3. Calculate the actual straight-line distance to the obstacle. With dynamic security threshold Spare space between And based on space margin The interval in which it is located implements hierarchical decision control, such as Figure 2 As shown, the specific steps include the following:

[0135] S31, Straight-line distance from obstacles With dynamic security threshold Spare space between The calculation formula is as follows:

[0136] .

[0137] S32, Setting a warning grace zone Backward judgment threshold and the limit tolerance threshold The specific implementation steps are as follows:

[0138] S321, Warning Boundary Band The theoretical displacement required for the robotic arm to exhaust its current kinetic energy using the rated continuous braking force of the motor, compared to the displacement required using the maximum peak braking force (i.e. The difference between the braking term and the displacement required to exhaust kinetic energy is determined, and its expression is:

[0139] ;

[0140] In the formula, For a moment Real-time dynamically calculated early warning bandwidth; , , These are the basic inertial mass, the real-time load mass, and the currently measured normal velocity, respectively. The rated continuous braking force specified on the servo motor's nameplate at the factory. The maximum peak braking force specified on the servo motor's nameplate at the factory, and meets the requirements. );

[0141] For example: when the robotic arm is stationary ( When ), the calculation yields This means that when the robotic arm is not moving, there is no possibility of any inertial forward movement, and the system does not need to allocate any deceleration width, directly releasing the maximum available static working space.

[0142] S322, Backwards Detection Threshold The expression used to determine whether an obstacle is actually and continuously squeezing the robotic arm is:

[0143] ;

[0144] In the formula, The threshold for determining the backward movement; It is the constant of the speed of light; The sweep bandwidth is fixed in the FMCW RF module hardware; This is the standard gravitational acceleration; The equivalent static stiffness of the robotic arm's end effector is determined by the factory mechanical tensile test.

[0145] For example: if an ideal radar with a bandwidth approaching infinity is used ( And the robotic arm is an absolutely rigid body. ),but This means that in an ideal system without any measurement or mechanical errors, the obstacle will immediately trigger a reversal once it crosses the safety boundary.

[0146] S323, Limit Tolerance Threshold This is the last line of defense that triggers a power outage and brake application at the underlying hardware level. When the system is at this stage, it means that the software-level compliant retreat is no longer sufficient to prevent the rapid approach of an obstacle (such as a human body); at this point, the only remaining physical buffer is the flexible radio frequency sensor skin on the actuator surface itself. Therefore, The maximum compressible stroke of the flexible sensing skin before it reaches its material physical failure limit, plus the maximum mechanical play error accumulated by the backlash of the gears at each joint of the robotic arm and mapped to the end effector, is expressed as follows:

[0147] ;

[0148] In the formula, The initial physical thickness of the flexible sensing skin; The maximum reversible strain rate of the flexible PI substrate material (e.g., 0.15). The first linear velocity Jacobian matrix calculated in real time in S112 is the... Column component; The maximum gear backlash angle specified at the factory for each joint reducer;

[0149] For example, when a robotic arm moves to an extreme singular pose, causing the Jacobian column vector of a joint with a large backlash to change... When it approaches the maximum value, the formula calculates The automatic widening reflects the increased physical risk of the robotic arm slipping due to uncontrolled gear backlash under this specific positioning posture. By expanding the limit tolerance range, the system prevents minor, harmless mechanical movements from being misjudged as fatal collisions and frequently triggering emergency stops, thus ensuring the robustness of the control system.

[0150] S33, allocate space margin Each with a set warning grace zone Backward judgment threshold and the limit tolerance threshold Compare and adjust based on available space. The specified interval sends a hierarchical instruction to the motion controller, the details of which are as follows:

[0151] S331: When When the area is determined to be within the normal operating range and the distance is sufficient, the robotic arm will operate normally according to the original trajectory and speed plan.

[0152] S332: When When the system determines that the area is in a deceleration zone and an obstacle is approaching the safety boundary, it will execute a deceleration operation: the system outputs a smooth deceleration command, and the target speed is [not specified]. Decrease according to a power function based on space margin:

[0153] ;

[0154] In the formula, For smoothing decay factor ( ).

[0155] For example: In this embodiment, the smoothing attenuation factor The ratio of the servo motor's rated continuous braking force to its maximum peak braking force is used to determine the braking force, and this is done in conjunction with a preset safety margin factor. The specific method is as follows:

[0156] S322a: In the deceleration zone, the target velocity at the end of the robotic arm decreases according to a set function. Assume real-time space margin. As the robotic arm approaches the obstacle, its approach speed is equivalent to... .

[0157] According to the chain rule of calculus, when the robotic arm executes this smooth deceleration curve, the underlying layer must output the theoretical control acceleration. The product of the derivative of velocity with respect to displacement and the velocity itself:

[0158] ;

[0159] The simplified model yields the real-time theoretical acceleration distribution model during the braking process:

[0160] .

[0161] S322b: To ensure a smooth deceleration process while maximizing safety, the system requires that upon entering the warning grace zone (i.e., ... At that instant, the theoretically required peak deceleration This equals the maximum absolute safe deceleration that the robotic arm can provide under the current load. .

[0162] when Substituting into the above formula, we get... .

[0163] Based on Newton's second law, the robotic arm relies on the maximum equivalent braking force. The resulting physical limit deceleration is .

[0164] By setting the two equal, we establish the dynamic boundary equation:

[0165] .

[0166] S322c: Rearrange the terms in the above equation and solve. The initial expression:

[0167] ;

[0168] At this point, the previously established objective derivation formula for the early warning grace band is invoked. Substitute into the above formula:

[0169] ;

[0170] After eliminating and simplifying the physical variables in the numerator and denominator, the velocity of motion... With quality By eliminating each other, the smooth decay factor is finally derived. The calculation formula is: .

[0171] S333: When When the area is identified as a pause zone, a pause command is issued, and the driver outputs torque to maintain the current position.

[0172] S334: When When the system determines that the area is in a backward movement zone, it executes a backward movement: the system generates a compliant backward movement command in the opposite direction based on the obstacle normal vector obtained by the radio frequency sensor.

[0173] S335: When When the area is identified as an emergency stop zone, the underlying hardware is powered off and the brake is engaged to initiate an emergency stop.

[0174] In this embodiment 1, the straight-line distance to the obstacle is calculated in real time. With dynamic security threshold Spare space between The system compares this data with multiple boundary intervals (early warning tolerance zone, retreat decision threshold, and ultimate tolerance threshold) defined based on system dynamics characteristics, enabling graded responses and multi-level compliant safety control decisions for different hazard levels. Specifically, for example, when an operator occasionally approaches the robotic arm's working area and enters the deceleration zone on a dynamic production line, the system will use a smoothing attenuation factor... The system outputs a smooth deceleration command, slowing down the robotic arm's movements without immediately cutting off power or reporting an error. If the operator continues to approach, compressing the available space to the pause or retreat range, the robotic arm will actively maintain its current posture using its actuators or perform a compliant avoidance maneuver in the opposite direction. Only when faced with extremely rapid approach and the physical space exceeds the tolerance threshold will the robotic arm be allowed to move smoothly. Only when the power is cut off and the brake is engaged in emergency stop will the underlying hardware be triggered. This operation breaks the rigid control logic of traditional safety control schemes that require either stopping or starting, avoiding the obstruction of production continuity by a single emergency stop mechanism. Under the premise of ensuring the safety of personnel, it maximizes the stable operation of production equipment and the efficiency of process flow.

[0175] Example 2

[0176] As a second embodiment of the present invention, such as Figure 3 As shown in Example 1, this example also discloses a dynamic control system for the safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, specifically including:

[0177] Data acquisition module: Used to acquire real-time dynamic state data of the robotic arm's end effector and the straight-line distance to the nearest obstacle. Specifically, the dynamic state data acquired by this module in real time includes the real-time motion velocity of the robotic arm's end effector. and real-time acceleration and the quality of the load currently captured by the actuator. And obtain the straight-line distance of the obstacle through radio frequency sensing. .

[0178] Dynamic safety threshold calculation module: Used to construct a dynamic safety braking distance model and calculate the dynamic safety threshold that matches the current motion state. This module is based on the fundamental physical buffer distance. Delayed stage displacement With limit physical displacement Calculate the current dynamic security threshold. .

[0179] Space margin assessment module: used to calculate the straight-line distance to obstacles. With dynamic security threshold Spare space between Specifically, this involves calculating the straight-line distance to the actual obstacle. With dynamic security threshold The difference between them is used to obtain the real-time space margin. .

[0180] Hierarchical decision control module: used for decision control based on space margin The module implements hierarchical decision control within its specified range. An internal warning grace period is configured within this module. Backward judgment threshold and the limit tolerance threshold By judging the space margin Real-time boundary crossings trigger corresponding low-level control commands. For example, within the deceleration zone, commands are issued based on a smoothing attenuation factor. Calculate and output the target speed .

[0181] In the specific implementation of Embodiment 2 described above, a complete data processing and logical closed loop is formed among the various modules. The data acquisition module, as the sensing front end, provides data input for the system's dynamic calculations; the dynamic safety threshold calculation module and the space margin assessment module map the physical dimension of motion state into a quantified safety space boundary; finally, the hierarchical decision control module completes the instruction issuance. This system architecture, through the collaborative operation between modules, overcomes the shortcomings of traditional equipment that can only rely on external sensors to implement a single shutdown mechanism. It can adaptively adjust the protection level in dynamic human-machine collaboration scenarios, balancing production efficiency and operational safety.

[0182] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0183] The preferred embodiments of the invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention.

Claims

1. A method for dynamic control of safety distance of a robot based on radio frequency electromagnetic wave sensing and real-time decision making, characterized in that, include: Real-time acquisition of dynamic state data of the robotic arm's end effector and the straight-line distance to the nearest obstacle; A dynamic safety braking distance model is constructed based on the aforementioned dynamic state data, and a dynamic safety threshold matching the current motion state is calculated. Calculate the spatial margin between the straight-line distance to the obstacle and the dynamic safety threshold, and perform hierarchical decision control based on the interval in which the spatial margin is located.

2. The method of claim 1, wherein the method further comprises: The dynamic state data includes the real-time motion speed and real-time acceleration of the robotic arm end effector, as well as the load mass grasped by the actuator.

3. The method of claim 2, wherein the method further comprises: The method for obtaining the real-time motion speed and real-time acceleration of the robotic arm end effector is as follows: The real-time motion velocity and real-time acceleration of the robotic arm's end effector are obtained by projecting the three-dimensional linear velocity vector and three-dimensional linear acceleration vector of the end effector in the base coordinate system onto the unit direction normal vector pointing from the end effector to the nearest obstacle.

4. The dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 1, characterized in that, The method for obtaining the straight-line distance to the nearest obstacle to the actuator is as follows: A flexible radio frequency sensing module conformally wrapped around the outside of the actuator transmits a frequency-modulated continuous wave signal and receives its reflected echo. The straight-line distance to the obstacle is then calculated based on the frequency difference information between the reflected echo and the transmitted signal.

5. The dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 3, characterized in that, The method for determining the dynamic safety threshold that matches the current motion state is as follows: The dynamic safety threshold is determined by combining the basic physical buffer distance, the delay stage displacement, and the ultimate physical displacement required to consume the system's kinetic energy. The method for obtaining the displacement during the delay stage is as follows: combining the compensation coefficient, the motion trend of the robotic arm during the total response delay time is predicted based on the uniformly accelerated linear kinematics equation to obtain the displacement during the delay stage. The method for obtaining the ultimate physical displacement is as follows: the total kinetic energy of the system is calculated by combining the basic inertial mass of the robotic arm with the load mass currently grasped by the actuator, and the ultimate physical displacement is determined based on the kinetic energy theorem and the maximum equivalent braking force of the system.

6. The dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 5, characterized in that, The method for obtaining the compensation coefficient is as follows: The equivalent damping ratio of the entire robotic arm is obtained, and the compensation coefficient is determined by adding one to the product of the equivalent damping ratio and the preset gain coefficient.

7. The method for dynamic control of the safe working distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 5, characterized in that, Hierarchical decision control is performed based on the interval in which the aforementioned space margin is located, specifically including: The difference between the actual straight-line distance to the obstacle and the dynamic safety threshold is defined as the spatial margin; and warning grace zone, retreat judgment threshold and extreme tolerance threshold are set. When the space margin is greater than the warning width limit, it is determined to be a normal operating area, and the robotic arm operates normally according to the original trajectory and speed plan; When the space margin is greater than zero and less than or equal to the warning width band, it is determined to be a deceleration operation area, and the system outputs a smooth deceleration command to execute deceleration operation; When the space margin is greater than the negative value of the back-off determination threshold but less than or equal to zero, it is determined to be a paused operation area, and the driver outputs torque to maintain the current pose. When the space margin is greater than the negative value of the limit tolerance threshold and less than or equal to the negative value of the back-off determination threshold, it is determined to be a back-off running area, and a reverse compliant back-off command is generated. When the space margin is less than or equal to the negative value of the limit tolerance threshold, it is determined to be an emergency stop zone, triggering the underlying hardware to cut off power and engage the brake to perform an emergency stop.

8. The dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 7, characterized in that, The method for obtaining the target speed during the deceleration process is as follows: The target speed is determined by multiplying the real-time motion speed by a smooth decay factor raised to a specific base. The specific base is the quotient of the space margin divided by the warning grace band.

9. The method for dynamic control of the safe working distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making according to claim 1, characterized in that, The method for obtaining the attenuation factor is as follows: The attenuation factor is determined by the ratio of the rated continuous braking force to the maximum peak braking force of the servo motor, combined with a preset safety margin coefficient.

10. A dynamic control system for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making, employing the dynamic control method for safe operating distance of a robotic arm based on radio frequency electromagnetic wave sensing and real-time decision-making as described in any one of claims 1 to 9, characterized in that, include: Data acquisition module: used to acquire real-time dynamic state data of the robotic arm end effector and the straight-line distance to the nearest obstacle; Dynamic safety threshold calculation module: used to build a dynamic safety braking distance model and calculate the dynamic safety threshold that matches the current motion state; Space margin assessment module: used to calculate the space margin between the straight-line distance to the obstacle and the dynamic safety threshold; Hierarchical decision control module: used to perform hierarchical decision control based on the interval in which the space margin is located.

Images