Robotic trajectory buffering, resampling, and water level controlled stoppage method and system

Abstract

This invention provides a method and system for robot trajectory buffering, resampling, and water level-controlled shutdown, comprising: establishing a communication session with the upper layer, determining the nominal input frequency and the pre-filled lower water level threshold, and determining the output control frequency of the drive layer; receiving target waypoints submitted by the upper layer in an asynchronous timing sequence and storing them in a waypoint cache unit; sequentially reading the target waypoints in the waypoint cache unit and reconstructing the intra-segment time reference according to the nominal input frequency for each target waypoint in the current motion segment; when the motion segment is inactive, if the pipeline buffer water level reaches the pre-filled lower water level threshold, activating the motion segment after completing the drive-side running state cleanup, and anchoring the start time of the intra-segment time reference; based on the intra-segment time reference, using the nominal input period corresponding to adjacent target waypoints as a fixed planning duration, performing resampling under the condition of satisfying joint kinematic constraints, and generating dense control commands aligned with the output control frequency.

Description

Technical Field

[0001] This invention relates to the field of robot motion control and real-time drive technology, specifically to a robot trajectory buffering, resampling, and water level-controlled shutdown method and system, and more specifically to a robot trajectory buffering, resampling, and water level-controlled shutdown method and system for asynchronous trajectory input. Background Technology

[0002] In real-time robot control, the output rhythm of the upper-level planning and the timing of the lower-level hardware control are often inconsistent in terms of frequency and phase. Frequency inconsistency (low frequency due to high computational load): Online trajectory planners and other upper-level planning modules have high single-step computational load and long solution time, often only able to operate at lower frequencies such as 50Hz and 100Hz; offline trajectories also often provide critical waypoints at lower frequencies. However, servo buses (such as EtherCAT) require fixed-cycle commands of 1000Hz (or other), and the frequencies of the two differ by more than an order of magnitude, requiring the low-frequency sparse waypoints to be increased to high-frequency control cycles; Inconsistent rhythm (frame delays caused by non-hard real-time): The aforementioned upper-layer controllers mostly operate in non-hard real-time environments, and the time consumed by single-step solution fluctuates with the scene. The actual output interval of some frames may exceed the control cycle corresponding to the nominal frequency (frame delay), causing jitter in the output frame interval; offline trajectories may be sent in batches within a very short time; and there is network jitter in the teleoperation link. If there is no buffer margin on the driver side, frame delay will cause consumption starvation.

[0003] Existing robot drive solutions typically suffer from the following problems: 1. Strongly Coupled Fixed-Frequency Assumption: Most drivers require the upper layer to issue commands strictly according to a fixed period. Once the upper layer experiences jitter or interruption, it can lead to reduced tracking accuracy and command jumps, or even trigger errors in the servo synchronization watchdog. As a result, the upper layer has to introduce complex real-time threads and hard real-time scheduling, increasing the development and deployment complexity of upper-layer applications.

[0004] 2. Lack of kinematic constraints in resampling: Linear interpolation or general spline resampling without explicit application of cross-segment continuity and velocity, acceleration and jerk constraints cannot simultaneously guarantee trajectory continuity and kinematic envelope. Sudden changes in velocity or acceleration may occur at waypoint switching points, increasing the risk of mechanical shock, jitter and actuator overcurrent.

[0005] 3. Lack of tiered buffering and water level management: Either there is no buffering, with zero tolerance for upper-level fluctuations; or an infinite buffer is used, causing the robot to continuously track severely lagging, expired trajectories when a high-frequency re-issue command suddenly occurs after the upper-level solver lags, creating safety hazards. Both extremes lack deterministic criteria for "how much pre-filling is needed for startup" and "how much backlog triggers protection."

[0006] 4. Crude interruption handling: When the upper layer temporarily stops sending path points (data starvation within the segment), the system either rigidly holds the last frame command (which may continue to move in a non-zero motion state, posing a risk of loss of control), or triggers shutdown protection indiscriminately (generating a large number of false triggers in static holding scenarios, resulting in poor availability).

[0007] 5. Insufficient real-time performance and determinism: If buffering, interpolation, and state acquisition are shared locks or dynamically allocated, uncertain delay jitter will be introduced, making it difficult to meet the hard real-time jitter budget; end-to-end delay cannot be parsed, modeled, and calibrated.

[0008] Therefore, how to deterministically convert asynchronous and sparse upper-layer waypoints into high-frequency, smooth, and kinematically feasible actuator commands, while only requiring the upper layer to declare a nominal frequency, not requiring the upper layer to submit at a strict time according to that frequency, and not requiring the upper layer to be in hard real-time, has become an urgent problem to be solved in the field of robot real-time drive. Summary of the Invention

[0009] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method and system for robot trajectory buffering, resampling, and water level-controlled shutdown.

[0010] A robot trajectory buffering, resampling, and water level controlled shutdown method provided by the present invention includes: Step S1: Establish a communication session with the upper layer to determine the nominal input frequency and the pre-filled lower water level threshold. , water level threshold and waypoint buffer unit effective capacity The output control frequency of the driving layer is determined, and the output control frequency is a positive integer multiple of the nominal input frequency; wherein, ; Step S2: Receive the target waypoints submitted by the upper layer in asynchronous timing order and store them in the waypoint buffer unit; Step S3: Read the target waypoints in the waypoint buffer unit in sequence. After completing the validity check, reconstruct the time base within the current motion segment according to the nominal input frequency of each target waypoint in the current motion segment, so that the execution time of the waypoint is decoupled from the physical submission time, and the current pipeline buffer level is updated synchronously. Step S4: When the motion segment is inactive, if the flow line buffer water level reaches the pre-filled lower water level threshold, the motion segment is activated after the drive side operation status is cleared, and the start time of the time reference within the segment is anchored. Step S5: Based on the time reference within the segment, with the nominal input period corresponding to adjacent target waypoints as the fixed planning duration, resampling is performed under the condition of satisfying joint kinematic constraints to generate dense control commands aligned with the output control frequency; if fixed duration resampling is not feasible, controlled shutdown is triggered. Step S6: According to the fixed period corresponding to the output control frequency, send the dense control command to the actuator and publish the operating status; Step S7: During the movement, if the buffer water level of the pipeline exceeds the water level threshold of the accumulation, a controlled shutdown due to accumulation over-limit is triggered; if a waypoint flow interruption occurs, the flow is diverted according to the current command state movement state: the current position is maintained at zero speed only when the command state speed and command state acceleration of each joint are within their respective tolerance ranges, and a controlled shutdown is triggered when the command state speed or command state acceleration of any joint exceeds the corresponding tolerance range.

[0011] Preferably, in step S2, the waypoint buffer unit is a single-producer, single-consumer lock-free circular buffer, and the backlog of waypoints in the buffer is calculated by the difference between the monotonically increasing write logic sequence number and the read logic sequence number; the backlog of waypoints in the buffer is used to calculate the pipeline buffer level. The waypoint caching unit is implemented based on shared memory and deployed between the upper layer and the driver layer; the upper layer writes target waypoints in a non-blocking, non-real-time manner, and the driver layer consumes target waypoints in a polling manner.

[0012] Preferably, step S3 includes: Step S3.1: Read the target waypoints in the waypoint cache unit in sequence, and perform session validity verification and control mode consistency verification on the target waypoints; Step S3.2: After completing the validity verification, reconstruct the time reference within the current motion segment according to the nominal input frequency for each target waypoint within the current motion segment;

[0013] in, The segment number assigned to the target waypoint within the current motion segment according to the receiving order; For nominal input frequency; Step S3.3: For each frame of target waypoint read, the current pipeline buffer level is updated synchronously.

[0014] Preferably, step S5 includes: based on the time reference within the deterministic segment, using the nominal input period corresponding to two adjacent target waypoints as a fixed planning duration, performing double-S velocity planning under the constraints of velocity, acceleration, and jerk upper and lower limits of each joint, generating a smooth motion trajectory with continuous position, velocity, and acceleration; wherein the jerk is segmentally constant within each stage, allows jumps at stage boundaries, and always satisfies the upper and lower limits; sampling the smooth motion trajectory at equal intervals according to the output control frequency to obtain dense control commands; The planning duration of the current trajectory segment is fixed to the fixed planning duration and solved directly. Only when the solution is successful, the absolute value of the difference between the obtained trajectory duration and the fixed planning duration does not exceed the numerical tolerance, and the obtained trajectory satisfies the boundary conditions and kinematic constraints, is it determined that fixed duration resampling is feasible; otherwise, it is determined to be infeasible and controlled shutdown is triggered. The shortest duration of the time-optimal solution is only used for infeasibility prediction before fixed duration solution.

[0015] Preferably, the buffer water level of the production line includes:

[0016] in,

[0017] in, For complete cross-process logic backlog; This refers to the depth that has already crossed the shared memory boundary and is still driving the internal pipeline. , These represent the cumulative number of waypoint frames written to the waypoint buffer unit and the number of waypoint frames retrieved from it by the driver side within this communication session, respectively. Both are initialized to 0 and monotonically increased when the session is established; the difference between the two represents the number of waypoint frames that have not yet been retrieved.

[0018] Preferably, zero-speed holding is performed only when the command-state velocity and command-state acceleration of each joint are within their respective tolerance ranges. This includes: freezing the current joint position, outputting a zero-speed control command, and resetting the velocity and acceleration of the internal resampled state to zero. After a new target waypoint is reached, resampling is performed again starting from the current zero-speed state. When the command-state velocity or command-state acceleration of any joint exceeds the corresponding tolerance range, controlled shutdown is performed. This includes isolating normal trajectory commands that have not yet been executed, using the most recently issued command-state position, velocity, and acceleration as the initial state, and using zero endpoint velocity and zero endpoint acceleration as termination constraints. A stop trajectory is generated within the upper and lower limits of velocity, acceleration, and jerk, and is continuously issued according to the output control frequency. After both velocity and acceleration reach their respective tolerance ranges, the system switches to position holding. If a stop trajectory cannot be generated or a safety control command cannot be issued, the system is upgraded to perform an emergency stop or safety torque shutdown by an independent hardware safety link.

[0019] A robot trajectory buffering, resampling, and water level controlled shutdown system provided by the present invention includes: Module M1: Establishes a communication session with the upper layer to determine the nominal input frequency and pre-filled lower water level threshold. , water level threshold and waypoint buffer unit effective capacity The output control frequency of the driving layer is determined, and the output control frequency is a positive integer multiple of the nominal input frequency; wherein, ; Module M2: Receives target waypoints submitted by the upper layer in an asynchronous time sequence and stores them in the waypoint buffer unit; Module M3: Reads the target waypoints in the waypoint cache unit in sequence, completes the validity verification, and reconstructs the time base within the current motion segment according to the nominal input frequency of each target waypoint, so as to decouple the execution time of the waypoint from the physical submission time, and synchronously updates the current pipeline buffer level. Module M4: When the motion segment is inactive, if the flow line buffer water level reaches the pre-filled lower water level threshold, the motion segment is activated after the drive side running status is cleared, and the start time of the time reference within the segment is anchored. Module M5: Based on the time base within the segment, with the nominal input period corresponding to adjacent target waypoints as the fixed planning duration, resampling is performed under the condition of satisfying joint kinematic constraints to generate dense control commands aligned with the output control frequency; if fixed-duration resampling is not feasible, controlled shutdown is triggered. Module M6: Issues dense control commands to the actuator according to the fixed period corresponding to the output control frequency, and publishes the operating status; Module M7: During the movement, if the water level of the pipeline buffer exceeds the water level threshold of the accumulation, a controlled shutdown due to accumulation over-limit is triggered; if a waypoint flow interruption occurs, the flow is diverted according to the current command state: the current position is maintained at zero speed only when the command state speed and command state acceleration of each joint are within their respective tolerance ranges, and a controlled shutdown is triggered when the command state speed or command state acceleration of any joint exceeds the corresponding tolerance range.

[0020] Preferably, the waypoint cache unit in module M2 is a lock-free circular buffer with a single producer and a single consumer. The backlog of waypoints in the cache is calculated by the difference between the monotonically increasing write logic sequence number and the read logic sequence number. The backlog of waypoints in the cache is used to calculate the pipeline buffer level. The waypoint caching unit is implemented based on shared memory and deployed between the upper layer and the driver layer; the upper layer writes target waypoints in a non-blocking, non-real-time manner, and the driver layer consumes target waypoints in a polling manner. The module M3 includes: Module M3.1: Reads the target waypoints in the waypoint cache unit sequentially, and performs session validity verification and control mode consistency verification on the target waypoints; Module M3.2: After completing the validity verification, reconstruct the time reference within the current motion segment according to the nominal input frequency for each target waypoint within the current motion segment;

[0021] in, The segment number assigned to the target waypoint within the current motion segment according to the receiving order; For nominal input frequency; Module M3.3: For each frame of target waypoint read, the current pipeline buffer level is updated synchronously; The module M5 includes: based on a deterministic intra-segment time reference, using the nominal input period corresponding to two adjacent target waypoints as a fixed planning duration, performing dual-S velocity planning under the constraints of velocity, acceleration, and jerk upper and lower limits of each joint, generating a smooth motion trajectory with continuous position, velocity, and acceleration; wherein the jerk is segmentally constant within each stage, allows jumps at stage boundaries, and always satisfies the upper and lower limits; and sampling the smooth motion trajectory at equal intervals according to the output control frequency to obtain dense control commands; The planning duration of the current trajectory segment is fixed to the fixed planning duration and solved directly. Only when the solution is successful, the absolute value of the difference between the obtained trajectory duration and the fixed planning duration does not exceed the numerical tolerance, and the obtained trajectory satisfies the boundary conditions and kinematic constraints, is it determined that fixed duration resampling is feasible; otherwise, it is determined to be infeasible and controlled shutdown is triggered. The shortest duration of the time-optimal solution is only used for infeasibility prediction before fixed duration solution.

[0022] According to an electronic device provided by the present invention, the electronic device includes a memory and at least one processor, wherein the memory stores instructions; The at least one processor invokes the instructions in the memory to cause the electronic device to perform the steps of the robot trajectory buffering, resampling, and water level controlled shutdown method as described above.

[0023] According to a computer-readable storage medium provided by the present invention, the computer-readable storage medium stores instructions that, when executed by a processor, implement the various steps of the robot trajectory buffering, resampling, and water level controlled shutdown method described above.

[0024] Compared with the prior art, the present invention has the following beneficial effects: 1. Upper-layer applications only need to declare the nominal input frequency and submit waypoints sequentially in a non-blocking, non-real-time manner. Without hard real-time scheduling or strict timed submission according to the nominal frequency, the driver layer can deterministically convert them into high-frequency commands. The sparse waypoints output by upper-layer planning modules such as online trajectory planners are encrypted to the control frequency by double-S resampling. The frame delays caused by non-hard real-time are absorbed by the lower water level buffer. Offline trajectories can be submitted in batches according to the buffer capacity, and online trajectory planners can submit asynchronously within the range allowed by the buffer capacity and water level constraints, reducing the real-time requirements and engineering complexity of upper-layer applications. 2. Automatically generate internal timestamps based on nominal input frequency and segment sequence number. This makes the execution timeline independent of the upper-level physical clock jitter, eliminating command phase drift and speed jump caused by commit jitter; 3. Fixed-duration double S-sampling can generate smooth motion trajectories with continuous position, velocity, and acceleration. The jerk is bounded, segmented and constant within each stage, and allows jumps at the stage boundaries. For segments that are not feasible within a fixed duration, the machine is controlled to stop instead of silently degrading. Compared with linear interpolation or general splines that do not explicitly apply cross-segment continuity and velocity, acceleration, and jerk constraints, it can reduce the risk of mechanical shock, jitter, and actuator overcurrent at waypoint switching points. 4. Using pipeline depth as the sole source of fact, the lower water level forms a resolvable nominal buffer look-ahead time domain and an approximate delay budget, while the upper water level prevents backlog of trajectories that have expired. The two constitute an elastic constraint interval, taking into account both jitter absorption capability and safety. The actual pre-filling waiting time also depends on the physical commit time of the waypoint and the consumption delay of the entry thread. 5. When data is starved within a segment, the flow is split according to the command state speed and acceleration. When both are within their respective tolerance ranges, zero speed is maintained and enabled, and operation continues after the new frame arrives. When either exceeds the corresponding tolerance range, a controlled shutdown is triggered according to the fault semantics. This is safer than rigidly holding the last command and also reduces unnecessary emergency stop triggering in static holding scenarios. 6. The hard real-time data path is lock-free and has no runtime dynamic allocation. The real-time thread uses absolute time to suppress accumulated drift, which is conducive to meeting the jitter budget of high-frequency control cycles. The calculation jitter is absorbed by the lock-free queue, reducing the risk of it being transmitted to the actuator. 7. This invention is applied to the real-time drive adaptation layer of multi-degree-of-freedom robots. It is independent of the specific robot configuration and is used to deterministically convert target waypoints into high-frequency, smooth, and kinematically feasible actuator control commands under the condition that the nominal input frequency is declared by the upper-level planning module such as the online trajectory planner and the target waypoints are submitted asynchronously within the allowable range of buffer capacity and water level constraints. The invention also realizes start control, stacking protection and controlled shutdown through the buffer water level. Offline trajectories can be submitted in batches according to the buffer capacity. Attached Figure Description

[0025] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 Flowchart of robot trajectory buffering, resampling, and water level controlled shutdown method.

[0026] Figure 2 A schematic diagram of a robot trajectory buffering, resampling, and water level-controlled shutdown system. Detailed Implementation

[0027] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0028] Example 1 The present invention provides a method for robot trajectory buffering, resampling, and water level controlled shutdown, such as... Figure 1 As shown, it includes: Step S1: Establish a communication session with the upper layer to determine the nominal input frequency and the pre-filled lower water level threshold. , water level threshold and waypoint buffer unit effective capacity The output control frequency of the driving layer is determined, and the output control frequency is a positive integer multiple of the nominal input frequency; wherein, ; Step S2: Receive the target waypoints submitted by the upper layer in asynchronous timing order and store them in the waypoint buffer unit; Specifically, in step S2, the waypoint cache unit is a lock-free circular buffer with a single producer and a single consumer. The backlog of waypoints in the cache is calculated by the difference between the monotonically increasing write logic sequence number and the read logic sequence number. The backlog of waypoints in the cache is used to calculate the pipeline buffer level. The waypoint caching unit is implemented based on shared memory and deployed between the upper layer and the driver layer; the upper layer writes target waypoints in a non-blocking, non-real-time manner, and the driver layer consumes target waypoints in a polling manner.

[0029] Step S3: Read the target waypoints in the waypoint buffer unit in sequence. After completing the validity check, reconstruct the time base within the current motion segment according to the nominal input frequency of each target waypoint in the current motion segment, so that the execution time of the waypoint is decoupled from the physical submission time, and the current pipeline buffer level is updated synchronously. Specifically, step S3 includes: Step S3.1: Read the target waypoints in the waypoint cache unit in sequence, and perform session validity verification and control mode consistency verification on the target waypoints; Step S3.2: After completing the validity verification, reconstruct the time reference within the current motion segment according to the nominal input frequency for each target waypoint within the current motion segment;

[0030] in, The segment number assigned to the target waypoint within the current motion segment according to the receiving order; For nominal input frequency; Step S3.3: For each frame of target waypoint read, the current pipeline buffer level is updated synchronously.

[0031] Step S4: When the motion segment is inactive, if the flow line buffer water level reaches the pre-filled lower water level threshold, the motion segment is activated after the drive side operation status is cleared, and the start time of the time reference within the segment is anchored. Step S5: Based on the time reference within the segment, with the nominal input period corresponding to adjacent target waypoints as the fixed planning duration, resampling is performed under the condition of satisfying joint kinematic constraints to generate dense control commands aligned with the output control frequency; if fixed duration resampling is not feasible, controlled shutdown is triggered. Specifically, step S5 includes: based on the time reference within the deterministic segment, using the nominal input period corresponding to two adjacent target waypoints as the fixed planning duration, performing double-S velocity planning under the constraints of velocity, acceleration, and jerk at each joint to generate a smooth motion trajectory with continuous position, velocity, and acceleration; and sampling the smooth motion trajectory at equal intervals according to the output control frequency to obtain dense control commands.

[0032] Step S6: According to the fixed period corresponding to the output control frequency, send the dense control command to the actuator and publish the operating status; Step S7: During the movement, if the buffer water level of the pipeline exceeds the water level threshold of the accumulation, a controlled shutdown due to accumulation over-limit is triggered; if a waypoint flow interruption occurs, the flow is diverted according to the current command state movement state: the current position is maintained at zero speed only when the command state speed and command state acceleration of each joint are within their respective tolerance ranges, and a controlled shutdown is triggered when the command state speed or command state acceleration of any joint exceeds the corresponding tolerance range.

[0033] Specifically, the flow line buffer water level includes:

[0034] in,

[0035] in, For complete cross-process logic backlog; This refers to the depth that has already crossed the shared memory boundary and is still driving the internal pipeline. , These represent the cumulative number of waypoint frames written to the waypoint buffer unit and the number of waypoint frames retrieved from it by the driver side within this communication session, respectively. Both are initialized to 0 and monotonically increased when the session is established; the difference between the two represents the number of waypoint frames that have not yet been retrieved.

[0036] The present invention also provides a robot trajectory buffering, resampling, and water level controlled shutdown system. The robot trajectory buffering, resampling, and water level controlled shutdown system can be implemented by executing the process steps of the robot trajectory buffering, resampling, and water level controlled shutdown method. That is, those skilled in the art can understand the robot trajectory buffering, resampling, and water level controlled shutdown method as a preferred embodiment of the robot trajectory buffering, resampling, and water level controlled shutdown system.

[0037] Example 2 Example 2 is a preferred example of Example 1. This invention proposes a method and system for robot trajectory buffering, resampling, and water level-controlled shutdown, such as... Figure 2 As shown, it sets up a cross-process lock-free buffer between the upper-layer application process and the hard real-time driver process to absorb the rhythm jitter of asynchronous submissions; the driver side reconstructs the deterministic time base with the nominal input frequency, and resamples and encrypts the sparse path points to the control frequency with double S fixed duration according to the integer division relationship; at the same time, it sets two levels of thresholds, the lower water level (pre-filling) and the upper water level (overflow limit), with the pipeline depth (buffer level) as the only fact source, to realize the controlled start of the motion segment and the controlled shutdown of the overflow limit respectively, and to split the flow according to the instruction state speed and acceleration when the data is starved in the segment, so as to realize zero speed maintenance or controlled shutdown.

[0038] This embodiment is divided into five functional layers that work in a top-down coordinated manner: asynchronous trajectory input access and buffering, time base reconstruction, fixed duration resampling, water level controlled segment activation and shutdown, and real-time fixed frequency transmission.

[0039] The upper-layer application process and the driver process are two independent deployment units. The data plane uses POSIX shared memory to carry a cross-process SPSC ring buffer. This SPSC ring buffer (Single-Producer-Single-Consumer ring buffer) is a lock-free ring queue with a single producer and single consumer, and its capacity is a power of 2. The upper-layer process is the sole writer, and the driver-side consumer thread is the sole reader; they are identified solely by their write sequence numbers. Reading the serial number The difference is used to determine the unlocked capacity.

[0040] The robot trajectory buffering, resampling, and water level controlled shutdown method includes: determining the nominal input frequency, pre-filling the lower water level, and accumulating the upper water level when establishing a session; the upper layer then asynchronously and sequentially writes waypoints; performing session and control mode verification for each frame, reconstructing the intra-segment time base according to the nominal frequency, and updating the pipeline water level; when an inactive segment reaches the lower water level, the waypoint entry thread only issues a segment activation request, and the lifecycle worker thread completes a handshake with the real-time cleanup operation of each cycle domain before migrating to the enabled running state; the producer performs fixed-duration resampling for each relevant subgroup; the synchronization cycle thread issues commands and publishes the status according to the fixed cycle of each subgroup; when there is starvation within the segment, the pipeline water level reaches or exceeds the upper water level, or the fixed duration is not feasible, zero-speed hold or entering an error state is executed and controlled shutdown is triggered according to regulations. Therefore, what is asynchronous is the waypoint submission time, not the time base execution and hardware-issued tick on the driver side.

[0041] The operation of adding waypoints at the upper layer is a non-blocking, non-real-time call: it only checks the local session flag and the remaining capacity of the circular buffer, writes a fixed-size waypoint frame to shared memory, and advances the process. When the capacity is insufficient, the queue is immediately returned to full. The upper layer only needs to declare the nominal input frequency when the session is established. During runtime, there is no need for hard real-time scheduling or strict timed submission according to the nominal frequency. Frame delays caused by non-hard real-time (output intervals exceeding the nominal period) are absorbed by the lower water level buffer on the driver side. The stronger the real-time performance of the upper layer and the more stable the submission, the lower the required lower water level.

[0042] The driver side entry thread consumes buffered frames at a lightweight polling pace, completes session verification, time base reconstruction and water level determination, and distributes them to the driver core according to the control mode.

[0043] The driver side only submits waypoints, updates water levels, and publishes activation request flags; it does not directly migrate the lifecycle finite state machine. The lifecycle worker thread consumes this request, issues real-time cleanup requests to each lifecycle domain, waits for completion, and then executes the transition from the stopped state to the enabled running state at the safe lifecycle boundary. Therefore, the waypoint entry thread, the lifecycle worker thread, and the real-time thread each only write the states they possess.

[0044] The producer thread and the synchronization cycle thread constitute the hard real-time data path, which is responsible for resampling encryption and fixed-frequency distribution, respectively. This data path is lock-free and has no runtime dynamic allocation.

[0045] Asynchronous trajectory input access and deterministic time-based reconstruction: During session establishment, the upper layer declares the nominal input frequency and pre-fill depth (i.e., bottom water level, in waypoint frames). The driver side derives the input cycle. : ;in, The nominal input frequency declared when the session is established; Key constraint (divisibility constraint): Let The nominal input frequency declared when the session is established. For any related subgroup, the output frequency, This is the dominant frequency in the periodic domain of this subgroup. To ensure that the number of samples within each segment and the dominant beat in the periodic domain are both integers, the nominal input frequency must be divisible by every relevant frequency. and :

[0046] For each subgroup, there is

[0047] Where mod represents the modulo operation of an integer; It is the number of output sampling frames corresponding to each input segment of the subgroup, i.e., the upsampling ratio of the subgroup; Let represent the set of positive integers. The input beat ratio for the periodic domain is... This is used to definitively schedule input segment consumption on the domain master beat. This dual constraint avoids, for example... , , This refers to the non-integer case where each segment has 2.5 subgroups of sampling points. When all relevant frequencies are 1000Hz, it is valid. The possible values ​​are 1000, 500, 250, 200, 125, 100, 50, ...; non-integer divisibility rates (such as 30Hz) must be resampled to a valid frequency by the upper layer before being submitted.

[0048] Deterministic time base reconstruction: Due to the unpredictable submission rhythm of the upper layer, waypoint frames themselves do not carry timestamps. The driver automatically generates the internal target execution time for the k-th frame (within the segment index k=0,1,2,...) within the motion segment.

[0049] This time base is anchored at the start of the motion segment (first frame) and is independent of the actual submission time of the upper layer. Thus, the physical rhythm produced by the upper layer is completely decoupled from the robot's execution time base: regardless of whether the upper layer distributes data at a constant speed, jitter, or in batches, the drive side always executes at equal intervals. Reconstruct a regular target timeline. After the movement segment ends (explicit shutdown or water level anomaly), the next segment re-anchors the time base.

[0050] Definitions of cross-process buffering and buffer level: Let the total number of waypoints submitted but not yet consumed by the real-time path in the pipeline be denoted as pipeline depth D. D is used as the sole source of facts for water level determination. D consists of two parts:

[0051] The backlog of logic in the shared memory circular buffer that has not yet been read by the waypoint entry thread includes logic that has not yet been processed due to backpressure from the driver core. The frame; : The depth of a waypoint that has been accepted by the waypoint entry thread, distributed into the queues of each cycle domain of the driver core, but has not yet been consumed by the producer in segments.

[0052] and These are all monotonically increasing logical indices, not physical array indices; the logical indices are used modulo the shared ring capacity only when accessing physical slots in the SPSC circular buffer. Therefore... This indicates a complete backlog of cross-process logic, while This indicates the depth that has crossed the shared memory boundary and is still driving the internal pipeline. The sum of these two values ​​is the complete pipeline depth used in this invention for activation and water level determination.

[0053] Set two water level thresholds above D:

[0054] Shared memory waypoint circular buffers and in-process single-producer single-consumer lock-free circular buffers should not be used interchangeably in terms of capacity semantics: the former is based on... If the slot is full, all its configured physical capacity can be used; otherwise, one slot is reserved to distinguish between empty and full slots. The effective capacity is N-1 when the number of physical slots is N. The lower water level is only used for segment activation and will no longer trigger a low water level shutdown; the upper water level is used for over-limit protection against accumulation.

[0055] Fixed-duration double-S resampling: The driver organizes resampling by grouping components of the same frequency into subgroups, with each subgroup sharing a single trajectory generator. Each upstream waypoint segment is modeled as a nominal duration... The dual-S velocity planning trajectory; for the output frequency of Subgroups, producer threads are arranged according to Sampling is a dense command.

[0056] The double-S velocity programming is characterized by piecewise constant values ​​for jerk, and its jerk... satisfy:

[0057] in, The maximum acceleration of the i-th joint; n represents the total number of n joints; in, Let be the position of the i-th joint. This is the third derivative of the position with respect to time, representing the jerk of the joint. Positive jerk corresponds to the acceleration-increasing phase, zero jerk corresponds to the acceleration-constant phase, and negative jerk corresponds to the acceleration-decreasing phase. A complete double-S trajectory moving in the positive direction sequentially experiences positive jerk, zero jerk, negative jerk, zero jerk (acceleration is zero during uniform motion), negative jerk, zero jerk, and positive jerk; the signs reverse accordingly during reverse motion. This ensures continuity of position and velocity, continuity of acceleration, and bounded jerk.

[0058] Its complete form consists of seven segments along the direction of motion: acceleration, uniform acceleration, deceleration, uniform speed, acceleration / deceleration, uniform deceleration, and deceleration. When the upper limit of velocity or acceleration is not reached, the corresponding uniform speed segment or uniform acceleration / deceleration segment degenerates and disappears. The actual number of segments can be 4, 5, 6, or 7. The mathematical model for its fixed-duration resampling is as follows.

[0059] Given the motion state at the starting point of the segment (The end point of the previous segment) and the target status of the segment (Current waypoint), kinematic envelope constraints on each axis: , , ,

[0060] Below, find a solution that satisfies the boundary conditions and whose duration is pinned. smooth trajectory , , so that: , , ; , , ; in, These represent the position, velocity, and acceleration of the endpoint of the previous segment, respectively. These represent the current waypoint's position, velocity, and acceleration, respectively. This represents the maximum velocity of the i-th joint. This represents the maximum acceleration of the i-th joint. The producer thread then samples the trajectory by subgroup output beats and produces... Frame Dense Command :

[0061] That is, a frame of sparse waypoints is encrypted in subgroup s as Frame frequency setting command completed. Upsampling; the periodic domain main frequency is only used to schedule when the subgroup expires, and does not change the number of samples in the subgroup.

[0062] Feasibility criterion (without silent degradation): Let the shortest duration of the time-optimal solution be... .like ( If the numerical tolerance is specified, then a fixed duration T is feasible, and the trajectory generator precisely stretches the duration to T and hits the target; otherwise ( If a target cannot be hit under constraints within a given timeframe, it is deemed infeasible. Instead of silent rollback or truncation, the driver enters a software termination state, triggering a controlled shutdown. This principle of "better to stop than remain silent" ensures that the kinematic feasibility of issued commands is always strictly guaranteed.

[0063] Bypass during frequency matching: when the nominal input frequency is equal to the subgroup output frequency ( ,Right now =1) When the upstream has guaranteed the trajectory quality, the resampling of this subgroup can be bypassed as a whole, maintaining frame-by-frame pass-through at zero order, avoiding meaningless replanning overhead. Whether to bypass is determined once when the session is established and is not dynamically switched during runtime.

[0064] Determining the activation and shutdown of sections with controlled water levels: Drive a three-branch controlled logic with pipeline depth D: prefill activation, water level controlled emergency stop, and intra-segment starvation diversion; Among them, pre-fill activation: when the motion segment is not activated, it is checked every time a frame is consumed. Once satisfied, the waypoint entry thread only issues a semantically set-activation request without locking, without directly migrating the finite state machine; after consuming the request, the lifecycle worker thread issues a real-time cleanup request to each lifecycle domain, waiting for the real-time thread to clean up the command queue and delay compensation buffer at the lifecycle boundary and reset the interpolator with the measured state before finally migrating the entire machine from the stopped state to the enabled running state and anchoring the time base. Pre-filling ensures that the machine has a base at the start of the motion. The frame margin absorbs subsequent jitter. This only indicates that we are waiting for pre-filling and does not constitute a low water level shutdown condition.

[0065] The water level termination is as follows: After the motion segment is activated, a check is performed after each frame is consumed. An out-of-limit error indicates that the upper-layer reissue rate exceeds the consumption capacity. Continuing execution will trace the expired trajectory, so it immediately enters an error state, triggering a controlled shutdown, which requires explicit reset to recover.

[0066] Intra-segment data starvation speed splitting: When the resampler cannot obtain the next frame waypoint in a certain control cycle (intra-segment starvation), speed / acceleration splitting is performed according to the current command state to avoid the two extremes of rigidly holding the last command or abruptly stopping in all cases.

[0067]

[0068] in, For internal sampling state velocity; For internal sampling state acceleration; Zero-speed branches remain frozen at their current position and report a zero-speed command externally. Simultaneously, the internal sampled velocity and acceleration are reset to zero, ensuring seamless continuation from zero speed in the next iteration when a new waypoint is reached (a valid double-S programming segment from zero speed to the target). Non-zero-speed starved branches enter an error state according to the motion interruption flow, i.e., fault semantics, triggering controlled shutdown. Tolerance Used to absorb numerical noise. If a fixed duration is not feasible, it will also enter an error state, triggering a controlled shutdown, without employing silent degradation.

[0069] The error state and subsequent safe output here represent software-level termination and command gate semantics, and are not equivalent to hardware safe torque shutdown. In cases such as shared cycle bus failures, software safe frames may not be able to complete synchronous output; hardware-level safe torque shutdown still requires a separate hardware safe link to guarantee it.

[0070] The composition of real-time fixed-frequency transmission and end-to-end delay: The hard real-time path consists of two types of threads: the producer thread only performs resampling and subcommand assembly, writing dense commands into a lock-free single-producer, single-consumer lock-free circular buffer; the synchronization cycle thread is the sole real-time owner of hardware synchronous I / O, performing a fixed sequence of synchronous input → reading component status → issuing at most one expired command → synchronous output → publishing a state snapshot each control cycle. The two are decoupled via a lock-free SPSC queue, with computational jitter absorbed by the queue depth. Absolute time is used for timing to avoid cumulative cycle drift.

[0071] End-to-end latency analysis model: The total latency from adding a waypoint at the upper layer to the actual activation of the actuator can be decomposed into:

[0072] in: Delay the polling consumption time for the waypoint entry thread; : Pre-filled buffer delay, determined by the lower water level and input frequency, is the main delay term introduced by the asynchronous input buffering mechanism; The delay introduced by the resampling segment duration (approximately one second) (magnitude) Dense command SPSC queue depth delay; The depth of the dense command lock-free circular queue; : Output control cycle of the driver layer; Bus communication and actuator internal hardware latency.

[0073] Without introducing other additional buffers, the main delay term introduced by the buffering mechanism is:

[0074] That is, the buffer delay is inversely proportional to the nominal input frequency and directly proportional to the prefill level, and can be analytically predicted and calibrated.

[0075] Hard real-time guarantees at the implementation level: Cross-process communication uses a shared memory waypoint circular buffer that is filled by a monotonic logical sequence number; cross-thread command communication uses a lock-free circular buffer with a capacity that is a power of 2 and an effective capacity of N-1, consisting of a single producer and a single consumer; real-time threads are prohibited from dynamic memory allocation, locking, blocking I / O, and exceptions. The entire runtime buffer of the resampling trajectory generator is a fixed-length array on the stack, which is locked at compile time with no heap allocation and no abnormal invariants. Shared memory sessions are created and reclaimed by the driver process as the owner, and abnormal residual objects are automatically cleaned up when the process starts and a new session is established.

[0076] Those skilled in the art will understand that, in addition to implementing the system, apparatus, and their modules provided by this invention in purely computer-readable program code, the same program can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, the system, apparatus, and their modules provided by this invention can be considered a hardware component, and the modules included therein for implementing various programs can also be considered structures within the hardware component; alternatively, modules for implementing various functions can be considered both software programs implementing the method and structures within the hardware component.

[0077] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for robot trajectory buffering, resampling, and water level-controlled shutdown, characterized in that, include Step S1: Establish a communication session with the upper layer to determine the nominal input frequency and the pre-filled lower water level threshold. , water level threshold and waypoint buffer unit effective capacity The output control frequency of the driving layer is determined, and the output control frequency is a positive integer multiple of the nominal input frequency; wherein, ; Step S2: Receive the target waypoints submitted by the upper layer in asynchronous timing order and store them in the waypoint buffer unit; Step S3: Read the target waypoints in the waypoint buffer unit in sequence. After completing the validity check, reconstruct the time base within the current motion segment according to the nominal input frequency of each target waypoint in the current motion segment, so that the execution time of the waypoint is decoupled from the physical submission time, and the current pipeline buffer level is updated synchronously. Step S4: When the motion segment is inactive, if the flow line buffer water level reaches the pre-filled lower water level threshold, the motion segment is activated after the drive side operation status is cleared, and the start time of the time reference within the segment is anchored. Step S5: Based on the time reference within the segment, with the nominal input period corresponding to adjacent target waypoints as the fixed planning duration, resampling is performed under the condition of satisfying joint kinematic constraints to generate dense control commands aligned with the output control frequency; if fixed duration resampling is not feasible, controlled shutdown is triggered. Step S6: According to the fixed period corresponding to the output control frequency, send the dense control command to the actuator and publish the operating status; Step S7: During the movement, if the buffer water level of the pipeline exceeds the water level threshold of the accumulation, a controlled shutdown due to accumulation over-limit is triggered; if a waypoint flow interruption occurs, the flow is diverted according to the current command state movement state: the current position is maintained at zero speed only when the command state speed and command state acceleration of each joint are within their respective tolerance ranges, and a controlled shutdown is triggered when the command state speed or command state acceleration of any joint exceeds the corresponding tolerance range.

2. The robot trajectory buffering, resampling, and water level-controlled shutdown method according to claim 1, characterized in that, In step S2, the waypoint buffer unit is a lock-free circular buffer with a single producer and a single consumer. The backlog of waypoints in the buffer is calculated by the difference between the monotonically increasing write logic sequence number and the read logic sequence number. The backlog of waypoints in the buffer is used to calculate the pipeline buffer level. The waypoint caching unit is implemented based on shared memory and deployed between the upper layer and the driver layer; the upper layer writes target waypoints in a non-blocking, non-real-time manner, and the driver layer consumes target waypoints in a polling manner.

3. The robot trajectory buffering, resampling, and water level-controlled shutdown method according to claim 1, characterized in that, Step S3 includes: Step S3.1: Read the target waypoints in the waypoint cache unit in sequence, and perform session validity verification and control mode consistency verification on the target waypoints; Step S3.2: After completing the validity verification, reconstruct the time reference within the current motion segment according to the nominal input frequency for each target waypoint within the current motion segment; in, The segment number assigned to the target waypoint within the current motion segment according to the receiving order; For nominal input frequency; Step S3.3: For each frame of target waypoint read, the current pipeline buffer level is updated synchronously.

4. The robot trajectory buffering, resampling, and water level-controlled shutdown method according to claim 1, characterized in that, Step S5 includes: based on the deterministic intra-segment time reference, using the nominal input period corresponding to two adjacent target waypoints as the fixed planning duration, performing double-S velocity planning under the constraints of velocity, acceleration, and jerk upper and lower limits of each joint, generating a smooth motion trajectory with continuous position, velocity, and acceleration; wherein the jerk is segmentally constant within each stage, allows jumps at stage boundaries, and always satisfies the upper and lower limits; sampling the smooth motion trajectory at equal intervals according to the output control frequency to obtain dense control commands; The planning duration of the current trajectory segment is fixed to the fixed planning duration and solved directly. Only when the solution is successful, the absolute value of the difference between the obtained trajectory duration and the fixed planning duration does not exceed the numerical tolerance, and the obtained trajectory satisfies the boundary conditions and kinematic constraints, is it determined that fixed duration resampling is feasible; otherwise, it is determined to be infeasible and controlled shutdown is triggered. The shortest duration of the time-optimal solution is only used for infeasibility prediction before fixed duration solution.

5. The robot trajectory buffering, resampling, and water level-controlled shutdown method according to claim 1, characterized in that, The flow line buffer water level includes: in, in, For complete cross-process logic backlog; This refers to the depth that has already crossed the shared memory boundary and is still driving the internal pipeline. , The number of pathpoint frames accumulated in the pathpoint buffer unit and the number of pathpoint frames retrieved from it by the driver side during the current communication session are respectively initialized to 0 and monotonically increased when the session is established; the difference between the two represents the number of pathpoint frames that have not yet been retrieved.

6. The robot trajectory buffering, resampling, and water level-controlled shutdown method according to claim 1, characterized in that, Zero-speed hold is performed only when the command-state velocity and acceleration of each joint are within their respective tolerance ranges. This includes: freezing the current joint position, outputting a zero-speed control command, and resetting the velocity and acceleration of the internal resampled state to zero. After a new target waypoint is reached, resampling continues from the current zero-speed state. When the command-state velocity or acceleration of any joint exceeds the corresponding tolerance range, a controlled shutdown is performed. This includes isolating normal trajectory commands that have not yet been executed, using the most recently issued command-state position, velocity, and acceleration as the initial state, and using zero endpoint velocity and zero endpoint acceleration as termination constraints. A stop trajectory is generated within the upper and lower limits of velocity, acceleration, and jerk, and is continuously issued at the output control frequency. After both velocity and acceleration reach their respective tolerance ranges, the system switches to position hold. If a stop trajectory cannot be generated or a safety control command cannot be issued, the system is upgraded to perform an emergency stop or safety torque shutdown by an independent hardware safety link.

7. A robot trajectory buffering, resampling, and water level-controlled shutdown system, characterized in that, include Module M1: Establishes a communication session with the upper layer to determine the nominal input frequency and pre-filled lower water level threshold. , water level threshold and waypoint buffer unit effective capacity The output control frequency of the driving layer is determined, and the output control frequency is a positive integer multiple of the nominal input frequency; wherein, ; Module M2: Receives target waypoints submitted by the upper layer in an asynchronous time sequence and stores them in the waypoint buffer unit; Module M3: Reads the target waypoints in the waypoint cache unit in sequence, completes the validity verification, and reconstructs the time base within the current motion segment according to the nominal input frequency of each target waypoint, so as to decouple the execution time of the waypoint from the physical submission time, and synchronously updates the current pipeline buffer level. Module M4: When the motion segment is inactive, if the flow line buffer water level reaches the pre-filled lower water level threshold, the motion segment is activated after the drive side running status is cleared, and the start time of the time reference within the segment is anchored. Module M5: Based on the time base within the segment, with the nominal input period corresponding to adjacent target waypoints as the fixed planning duration, resampling is performed under the condition of satisfying joint kinematic constraints to generate dense control commands aligned with the output control frequency; if fixed-duration resampling is not feasible, controlled shutdown is triggered. Module M6: Issues dense control commands to the actuator according to the fixed period corresponding to the output control frequency, and publishes the operating status; Module M7: During the movement, if the water level of the pipeline buffer exceeds the water level threshold of the accumulation, a controlled shutdown due to accumulation over-limit is triggered; if a waypoint flow interruption occurs, the flow is diverted according to the current command state: the current position is maintained at zero speed only when the command state speed and command state acceleration of each joint are within their respective tolerance ranges, and a controlled shutdown is triggered when the command state speed or command state acceleration of any joint exceeds the corresponding tolerance range.

8. The robot trajectory buffering, resampling, and water level controlled shutdown system according to claim 7, characterized in that, The pathpoint cache unit in module M2 is a lock-free circular buffer with a single producer and a single consumer. The backlog of pathpoints in the cache is calculated by the difference between the monotonically increasing write logic sequence number and the read logic sequence number. The backlog of pathpoints in the cache is used to calculate the pipeline buffer level. The waypoint caching unit is implemented based on shared memory and deployed between the upper layer and the driver layer; The upper layer writes the target waypoint in a non-blocking, non-real-time manner, and the driver layer consumes the target waypoint in a polling manner. The module M3 includes: Module M3.1: Reads the target waypoints in the waypoint cache unit sequentially, and performs session validity verification and control mode consistency verification on the target waypoints; Module M3.2: After completing the validity verification, reconstruct the time reference within the current motion segment according to the nominal input frequency for each target waypoint within the current motion segment; in, The segment number assigned to the target waypoint within the current motion segment according to the receiving order; For nominal input frequency; Module M3.3: For each frame of target waypoint read, the current pipeline buffer level is updated synchronously; The module M5 includes: based on a deterministic intra-segment time reference, using the nominal input period corresponding to two adjacent target waypoints as a fixed planning duration, performing dual-S velocity planning under the constraints of velocity, acceleration, and jerk upper and lower limits of each joint, generating a smooth motion trajectory with continuous position, velocity, and acceleration; wherein the jerk is segmentally constant within each stage, allows jumps at stage boundaries, and always satisfies the upper and lower limits; and sampling the smooth motion trajectory at equal intervals according to the output control frequency to obtain dense control commands. The planning duration of the current trajectory segment is fixed to the fixed planning duration and solved directly. Only when the solution is successful, the absolute value of the difference between the obtained trajectory duration and the fixed planning duration does not exceed the numerical tolerance, and the obtained trajectory satisfies the boundary conditions and kinematic constraints, is it determined that fixed duration resampling is feasible; otherwise, it is determined to be infeasible and controlled shutdown is triggered. The shortest duration of the time-optimal solution is only used for infeasibility prediction before fixed duration solution.

9. An electronic device comprising a memory and at least one processor, wherein the memory stores instructions; The at least one processor invokes the instructions in the memory to cause the electronic device to perform the steps of the robot trajectory buffering, resampling, and water level controlled shutdown method as described in any one of claims 1-6.

10. A computer-readable storage medium storing instructions thereon, characterized in that, When the instructions are executed by the processor, they implement each step of the robot trajectory buffering, resampling, and water level controlled shutdown method as described in any one of claims 1-6.

Images