Track replay method, apparatus, device, readable storage medium and program product
By integrating adaptive calculation of duration and trajectory fusion processing, the problems of low trajectory replay efficiency and stability of robots in interrupted scenarios are solved, and the continuity and efficient replay of robot trajectories are achieved.
Patent Information
- Application Number
- CN202611571202.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-12
- Publication Date
- 2026-11-10
AI Technical Summary
In existing technologies, when a robot receives an interruption command, the trajectory replay efficiency is low, resulting in discontinuous jumps in joint angles, joint velocities, and joint accelerations, which affects the robot's stability or causes the trajectory execution progress to be lost.
By acquiring relevant information about the original trajectory and the target trajectory segment, and using adaptive calculation of fusion duration and trajectory fusion processing, the fusion duration and fusion trajectory segment are determined, and splicing and fusion processing is performed to ensure the continuity of joint angles, velocities, and accelerations, avoid jumps, and restore the remaining trajectory of the original trajectory.
It improves the efficiency of trajectory replay in interrupted scenarios, ensures the stability of the robot and the continuity of trajectory execution, avoids discontinuous jumps in joint angles, speed and acceleration, and does not lose the original trajectory progress.
Smart Images

Figure CN123060718A_ABST
Abstract
Description
Technical Field
[0001] This disclosure relates to the field of computer technology, and more specifically, to a trajectory replay method, apparatus, device, readable storage medium, and program product. Background Technology
[0002] In existing technologies, when robots receive interruption commands (such as pause, trajectory switching, or recovery after an emergency stop), they typically employ strategies A and B. For example, strategy A involves a direct jump, where the control commands are switched directly to the sampled value of the target trajectory at the current moment upon recovery. This results in discontinuous jumps in the robot's joint angles, joint velocities, and joint accelerations. When these jumps exceed the physical limits of the joints, an emergency stop or hardware protection is triggered, affecting the robot's stability. Strategy B, on the other hand, involves stopping, resetting, and restarting. After an interruption, the robot completely stops, returns to its initial state or safe posture, and replays the original trajectory, thus losing the execution progress of the original trajectory and wasting time. Existing technologies often result in low trajectory replay efficiency for robots in interrupted scenarios. Summary of the Invention
[0003] This disclosure addresses the shortcomings of existing methods by proposing a trajectory replay method, apparatus, device, computer-readable storage medium, and computer program product to solve the problem of how to improve the trajectory replay efficiency of robots in interrupted scenarios.
[0004] In a first aspect, this disclosure provides a trajectory replay method, executed by a robot, comprising: In response to the detection of an interruption command during the robot's trajectory replay based on the original trajectory, relevant information about the original trajectory and relevant information about the target trajectory segment are obtained. The relevant information about the original trajectory includes the robot's current state information, and the relevant information about the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach. Based on the current state information and relevant information of the target trajectory segment, the fusion duration is determined through adaptive calculation of the fusion duration; Based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion duration, the fusion trajectory segment corresponding to the fusion duration is determined through trajectory fusion processing; Based on the fused trajectory segment and the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; The robot executes the stitched and merged trajectory until it reaches the endpoint, in order to restore the remaining trajectory of the original trajectory.
[0005] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the starting position of each joint of the robot at the starting point of the target trajectory segment, the speed limit information of each joint, the minimum fusion time, and the speed limit safety factor. Based on the current state information and relevant information of the target trajectory segment, the fusion duration is determined through adaptive calculation, including: Based on the current position of each joint, the starting position of each joint, the velocity limit information of each joint, the minimum fusion time, and the velocity limit safety factor, the fusion time is determined by adaptive calculation. The fusion time is used to characterize the duration of the fused trajectory segment.
[0006] In one embodiment, the speed limit safety factor ranges from 0.05 to 0.5, and the minimum fusion time ranges from 0.5 seconds to 3 seconds.
[0007] In one embodiment, based on relevant information of the original trajectory, relevant information of the target trajectory segment, and fusion duration, trajectory fusion processing is used to determine the fused trajectory segment corresponding to the fusion duration, including: Based on the fusion duration and the preset control cycle, sampling processing is performed within the fusion time window corresponding to the fusion duration to obtain multiple sampling time points; Determine the fusion coefficient for each sampling time point. The fusion coefficient is used to ensure at least one of the following: continuous joint position, continuous joint velocity, continuous joint acceleration, and continuous joint jerk. Based on relevant information from the original trajectory, the fusion mode is determined; Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode. Based on the position information of each joint at each sampling time point, the interpolator for each joint is determined; Based on the interpolators of each joint, construct the fused trajectory segment and the interpolator of the fused trajectory segment.
[0008] In one embodiment, the fusion coefficient is an S-type fusion coefficient, which supports configuring the continuity order, including zero-order, first-order, second-order, and third-order. The zeroth order is used to ensure the continuity of joint positions, and the corresponding S-shaped fusion coefficient is a linear function. The first order is used to ensure the continuity of joint position and joint velocity, and the corresponding S-shaped fusion coefficient is a cubic polynomial; The second order is used to ensure the continuity of joint position, joint velocity, and joint acceleration, and the corresponding S-shaped fusion coefficient is a fifth-order polynomial; The third order is used to ensure the continuity of joint position, joint velocity, joint acceleration, and joint jerk. The corresponding S-shaped fusion coefficients are seventh-order polynomials.
[0009] In one embodiment, the relevant information of the original trajectory includes the interpolator of the original trajectory, and the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment. Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: If the fusion mode is trajectory-to-trajectory fusion mode, then a weighted fusion process is performed based on the fusion coefficient, the interpolator of the original trajectory, and the interpolator of the target trajectory segment to obtain the position information of each joint at each sampling time point; If the fusion mode is a point-to-trajectory fusion mode, then an interpolator for the transition trajectory is constructed based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment. Based on the fusion coefficient, the interpolator for the transition trajectory, and the interpolator for the target trajectory segment, a weighted fusion process is performed to obtain the position information of each joint at each sampling time point.
[0010] In one embodiment, the type of interpolator for each joint includes any one of cubic spline interpolator, quintic spline interpolator, piecewise cubic Hermite interpolator, B-spline interpolator, or interpolator constructed based on the minimum jerk criterion.
[0011] In one embodiment, the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; The process of stitching and fusing the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory includes: Based on the interpolator of the fused trajectory segment and the interpolator of the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; The interpolator for the fused trajectory segment corresponds to the first time period, and the interpolator for the target trajectory segment corresponds to the second time period. In the first time period, each time point is less than the fusion duration, and in the second time period, each time point is greater than or equal to the fusion duration and less than or equal to the end time point. The total duration of the target trajectory segment is equal to the end time point.
[0012] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the end position of each joint of the robot at the end of the target trajectory segment. Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: Based on the current position and the endpoint position of each joint, forward kinematics calculations are performed to obtain the pose corresponding to the current position and the pose corresponding to the endpoint position of each joint. The pose includes position components and attitude quaternion components. Based on the fusion coefficients, the poses corresponding to the current positions of each joint and the poses corresponding to the end positions of each joint, interpolation is performed to determine the position information of each joint at each sampling time point.
[0013] Secondly, this disclosure provides a trajectory replay device for use in robots, comprising: The acquisition module is used to respond to the interruption command detected when the robot is replaying the trajectory based on the original trajectory, and to acquire relevant information about the original trajectory and the target trajectory segment. The relevant information about the original trajectory includes the robot's current state information, and the relevant information about the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach. The adaptive fusion duration calculation module is used to determine the fusion duration based on the current state information and relevant information of the target trajectory segment through adaptive fusion duration calculation. The trajectory fusion processing module is used to determine the fusion trajectory segment corresponding to the fusion time based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion time through trajectory fusion processing. The stitching and fusion processing module is used to perform stitching and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory; The execution module is used to execute the stitched and merged trajectory by the robot until the endpoint is reached, so as to restore the remaining trajectory of the original trajectory.
[0014] Thirdly, this disclosure provides an electronic device, including: a processor, a memory, and a bus; A bus is used to connect the processor and memory; Memory, used to store operation instructions; A processor is configured to execute the trajectory replay method of the first aspect of this disclosure by invoking operation instructions.
[0015] Fourthly, this disclosure provides a computer-readable storage medium storing a computer program that is used to execute the trajectory replay method of the first aspect of this disclosure.
[0016] Fifthly, this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the trajectory replay method in the first aspect of this disclosure.
[0017] The technical solutions provided in this disclosure have at least the following beneficial effects: The robot is replaying its original trajectory when it receives an interrupt command. In response, the robot determines the fusion duration based on its current state information and the target trajectory segment's information, through adaptive calculation. Then, based on the original trajectory's information, the target trajectory segment's information, and the fusion duration, the robot performs trajectory fusion processing to determine the fused trajectory segment corresponding to the fusion duration. This fused trajectory segment prevents discontinuous jumps in the robot's joint angles, joint velocities, and joint accelerations, thus ensuring the robot's stability. The robot then performs stitching and fusion processing on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory. After the robot executes the stitched and fused trajectory until it reaches the endpoint, it resumes executing the remaining portion of the original trajectory, thus preserving the original trajectory's progress and remaining trajectory. This improves the robot's trajectory replay efficiency in interrupted scenarios. Attached Figure Description
[0018] To more clearly illustrate the technical solutions in the embodiments of this disclosure, the accompanying drawings used in the description of the embodiments of this disclosure will be briefly introduced below.
[0019] Figure 1 A schematic flowchart of a trajectory replay method provided in an embodiment of this disclosure; Figure 2 A schematic diagram of a trajectory replay system provided in an embodiment of this disclosure; Figure 3 A schematic diagram of the trajectory fusion algorithm provided in the embodiments of this disclosure; Figure 4 A schematic diagram illustrating the continuity constraint guarantee mechanism provided in the embodiments of this disclosure; Figure 5 A schematic diagram of the timeline provided for embodiments of this disclosure; Figure 6 A schematic flowchart of a trajectory replay method provided in an embodiment of this disclosure; Figure 7 This is a schematic diagram of the structure of a trajectory replay device provided in an embodiment of the present disclosure; Figure 8 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Detailed Implementation
[0020] The embodiments of this disclosure are described below with reference to the accompanying drawings. It should be understood that the embodiments described below with reference to the accompanying drawings are exemplary descriptions for explaining the technical solutions of the embodiments of this disclosure, and do not constitute a limitation on the technical solutions of the embodiments of this disclosure.
[0021] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the terms “comprising” and “including” as used in embodiments of this disclosure mean that the corresponding feature can be implemented as the presented feature, information, data, step, operation, element, and / or component, but do not exclude implementation as other features, information, data, step, operation, element, component, and / or combinations thereof supported by the art. It should be understood that when we say that an element is “connected” or “coupled” to another element, the one element can be directly connected or coupled to the other element, or it can mean that the one element and the other element are connected through an intermediate element. Furthermore, “connected” or “coupled” as used herein can include wireless connection or wireless coupling. The term “and / or” as used herein indicates at least one of the items defined by the term; for example, “A and / or B” indicates implementation as “A,” or implementation as “B,” or implementation as “A and B.”
[0022] It is understood that in the specific embodiments of this disclosure, data related to trajectory replay is involved. When the above embodiments of this disclosure are applied to specific products or technologies, user permission or consent is required, and the collection, use and processing of related data must comply with the relevant laws, regulations and standards of the relevant countries and regions.
[0023] To make the objectives, technical solutions, and advantages of this disclosure clearer, the embodiments of this disclosure will be described in further detail below with reference to the accompanying drawings.
[0024] This disclosure provides a trajectory replay method for a trajectory replay system, which relates to the fields of trajectory replay.
[0025] To better understand and explain the solutions of the embodiments of this disclosure, some technical terms involved in the embodiments of this disclosure will be briefly explained below.
[0026] Trajectory replay: The process by which a robot performs motion according to pre-recorded or planned trajectory data (joint angle sequence and timestamp).
[0027] Interruption commands (control commands): include PAUSE, STOP, RESUME, and BLEND; PAUSE and STOP trigger controlled stop or hold procedures; RESUME and BLEND trigger the fusion execution procedure of this embodiment when the execution conditions are met; emergency stop is handled by the safety control mechanism, and motion can only be replanned after the emergency stop is lifted and a resume command is received.
[0028] Fusion time window: The local time interval after the newly generated trajectory begins execution. , The fusion duration is in seconds; the fused trajectory segment and the target trajectory segment use the same local time reference.
[0029] Fusion coefficient: A function that weights the reference trajectory and the target trajectory within the fusion window. , Its satisfaction , To achieve The order is continuously connected, corresponding to 1 to The first derivative is zero at both ends, and the reference trajectory, state boundary, and interpolator should also satisfy the following conditions. Zero-order configuration uses linear weights and does not require an S-shaped appearance.
[0030] Smootherstep: Fifth-order polynomial fusion weights Its first and second derivatives are zero, and it is used to achieve [something] when the reference trajectory and interpolator meet the corresponding conditions. Connection.
[0031] Continuous: The function and its first derivative (velocity) and second derivative (acceleration) are continuous, without abrupt changes.
[0032] Cubic spline interpolation: A piecewise cubic polynomial interpolation method that ensures the continuity of the second derivative at the connection between adjacent segments.
[0033] Track-to-track blending: weighted blending between the currently playing track (e.g., the original track) and the target track segment.
[0034] Point-to-trajectory fusion: Construct a transition trajectory that satisfies the constraints of starting position, velocity, and acceleration from the current state, and then fuse it with the target trajectory segment using weighted fusion; the starting velocity and acceleration are only set to zero when the robot is confirmed to be stationary.
[0035] State checkpoint: A data structure that records the robot's state at the moment of interruption, including the current joint position, trajectory playback timestamp, and cumulative pause duration; a data structure that saves the target trajectory identifier, version, logic progress, wall clock reference, cumulative pause duration, and current state; and a data structure that rereads the actual state upon resumption and checks it against the trajectory information in the checkpoint.
[0036] Joint speed limits (joint speed limit information): The maximum permissible movement speed of each joint of the robot, denoted as... The unit is rad / s or m / s.
[0037] Safety factor: A reduction factor multiplied to avoid reaching the physical limits of the joint, denoted as , The value is usually taken as 0.05 to 0.5.
[0038] Cartesian space fusion: Trajectory fusion is performed in the pose space (position and orientation) of the robot's end effector, rather than in the joint angle space.
[0039] SLERP: SLERP (Spherical Linear Interpolation) is used for smooth interpolation of quaternion poses.
[0040] FK: Forward Kinematics (FK) is a mapping relationship for calculating the pose of the end effector based on joint angles.
[0041] HDF5: A hierarchical data format used to store trajectory data files.
[0042] Control cycle: The time interval between the robot control system sending motion commands, denoted as . For example, 0.004 seconds (250Hz).
[0043] Cumulative pause duration: The sum of all pause periods during a single trajectory replay.
[0044] Jerk: The derivative of acceleration with respect to time; the degree of freedom index is still denoted as... The meanings of the two can be distinguished by the context.
[0045] Minimum Jerk Trajectory: This is a trajectory that is typically solved under given endpoint states and motion constraints with the objective of minimizing the sum of the square integrals of the jerks of each degree of freedom within the motion range.
[0046] The solutions provided in this disclosure can be applied to trajectory replay scenarios. For example, in a product launch demonstration: the presenter pauses the robot's movements to explain details, and resumes playback afterward. For example, in assembly line operation trajectory recording and playback: the operator can seamlessly switch to a new trajectory after correcting the previous one. For example, in remote teleoperation: control is restored smoothly after network latency or packet loss, seamlessly connecting to the current trajectory.
[0047] Based on this, the present disclosure provides a trajectory replay method, apparatus, device, computer-readable storage medium, and program product, and the specific technical solutions will be described in detail below.
[0048] The solutions provided in this disclosure relate to trajectory replay technology. Specific embodiments are described in detail below. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of this disclosure will now be described with reference to the accompanying drawings.
[0049] See Figure 1 , Figure 1 This diagram illustrates a flowchart of a trajectory replay method provided in an embodiment of this disclosure. This method can be executed by any electronic device, such as a robot. As an optional implementation, the method can be executed by a robot. For ease of description, the following description of some optional embodiments will use the robot as the execution subject of the method. Figure 1 As shown, the trajectory replay method provided in this embodiment includes the following steps: S201, in response to detecting an interruption command during trajectory replay based on the original trajectory, obtain relevant information about the original trajectory and relevant information about the target trajectory segment. The relevant information about the original trajectory includes the robot's current state information, and the relevant information about the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach.
[0050] Specifically, interruption commands include PAUSE, STOP, RESUME, BLEND, etc.
[0051] For example, information related to the original trajectory includes the robot's current state information and the original trajectory interpolator; the current state information includes the current position of each joint of the robot. For example, when an interrupt command is received, the current joint state is immediately recorded. ∈ 25 (25 degrees of freedom including 3 chassis, 3 torsos, 2×7 arms, 2×2 claws, and 3 heads), current joint state This represents the angle values of the robot's 25 joints (degrees of freedom) at the moment of interruption; where the angle value of each joint is the current position of that joint.
[0052] For example, relevant information about the target trajectory segment includes the starting position of each joint of the robot at the beginning of the target trajectory segment, the ending position of each joint of the robot at the end of the target trajectory segment, the speed limit information of each joint, the minimum fusion time, the speed limit safety factor, and the interpolator of the target trajectory segment.
[0053] For example, PAUSE and STOP do not directly trigger continued motion after fusion; PAUSE causes the controller to enter controlled deceleration and remain in the paused state, while freezing or updating the logical progress according to the defined deceleration strategy and saving the checkpoint; STOP enters the controlled stop process; fusion calculation is only started when RESUME or BLEND is received and the safety state allows execution.
[0054] For example, let The number of independent degrees of freedom involved in trajectory control in the current robot configuration, and the current state includes... , and The meaning, coordinate system, and units of each component are defined according to the configuration: rotational degrees of freedom use angular units, and translational degrees of freedom use length units; the chassis generalized coordinates are not equivalent to a single joint motor; the trajectory data dimensions, motion constraint dimensions, and number of interpolators are all consistent with... Consistent.
[0055] For example, relevant information for the target trajectory segment includes trajectory identifier and version, selected start and end time, and total duration. Starting and ending states, interpolator, motion constraints for each degree of freedom, and In the recovery scenario, let the saved original trajectory logic progress be... The total duration of the original trajectory is ,but , Switching scenes involves performing the same local time transformation based on the specified trajectory and starting point.
[0056] S202, based on the current state information and relevant information of the target trajectory segment, determines the fusion duration through adaptive calculation of the fusion duration.
[0057] Specifically, for example, the fusion time window is , Indicates the duration of fusion.
[0058] For example, the fusion time window is ,in Validation before fusion generation Furthermore, the current reference in the trajectory-to-trajectory mode is valid throughout the entire window; if this condition is not met, out-of-bounds sampling or blind extrapolation is prohibited. Explicit temporal reparameterization of the target trajectory segment can be performed and updated, provided the task allows. After calculating all derivatives, re-plan and verify; otherwise, reject the fusion and stop or maintain it under controlled conditions. There is no remaining segment with positive duration; if it is necessary to maintain the target endpoint after reaching it, the target endpoint must meet the holding conditions such as zero velocity and zero acceleration, otherwise it should be connected to the verified subsequent motion or controlled deceleration segment.
[0059] S203, based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion duration, the fusion trajectory segment corresponding to the fusion duration is determined through trajectory fusion processing.
[0060] Specifically, for example, fusion trajectory segment splines This represents the fusion trajectory segment corresponding to the fusion duration, where, , Indicates the duration of fusion.
[0061] S204. Based on the fused trajectory segment and the target trajectory segment, perform splicing and fusion processing to determine the spliced and fused trajectory.
[0062] Specifically, for example, Represents the spliced and merged trajectory; fused trajectory segment spline Indicates the fused trajectory segment. ; Target trajectory spline Indicates the target trajectory segment. The remaining segments corresponding to the target trajectory segment (from the target trajectory segment) arrive (trajectory) This represents the total duration of the target trajectory segment, i.e. The time point at which the target trajectory segment ends; ; .
[0063] S205, the robot executes the spliced and merged trajectory until it reaches the endpoint, in order to restore the remaining trajectory of the original trajectory.
[0064] Specifically, for example, the robot executes the stitched and merged trajectory. Mid-fusion trajectory segment During the fusion time window [0, After completion, switch to the stitched and merged trajectory. Middle target trajectory segment The remaining segment (from the target trajectory segment) arrive The trajectory, that is Continue execution; the robot will reach the end of the target trajectory segment. Then, resume executing the remaining trajectory of the original trajectory.
[0065] For example, the robot first executes the fused trajectory segment, and then... The remaining portion of the target trajectory segment is then integrated. For example, in a recovery scenario, the fusion window simultaneously advances the logical progress of the original trajectory. correspond Once the target trajectory segment ends, the remaining part of the original trajectory is executed, without needing to replay from the old breakpoint. For example, in scene switching, if a specified trajectory is used as the new execution target, the original trajectory is not automatically returned. For example, motion within the fusion window is allowed to deviate from the original path; tasks requiring point-by-point execution or strict adherence to the original path should use a separately validated path-preserving scheme.
[0066] For example, in response to receiving an execution command to restore the original trajectory or switch to a specified trajectory, the robot acquires its current state information, relevant information about the current trajectory, and relevant information about the target trajectory segment. In the restoration scenario, the target trajectory segment is the remaining trajectory of the original trajectory from the saved restoration time point; in the switching scenario, it is the trajectory segment of the specified trajectory from the specified start time point, with the local time of the target trajectory segment starting from zero. Based on the current state information and the relevant information of the target trajectory segment, the fusion duration is determined through adaptive calculation. Based on the current state information, relevant information about the current trajectory, relevant information about the target trajectory segment, and the fusion duration, the fused trajectory segment corresponding to the fusion duration is determined through trajectory fusion processing. Based on the fused trajectory segment and the target trajectory segment, splicing and fusion processing is performed to determine the spliced and fused trajectory. The robot executes the spliced and fused trajectory, which has passed continuity and motion constraint verification. After the fusion time window ends, the remaining part of the target trajectory segment is executed until the endpoint of the target trajectory segment is reached.
[0067] In this embodiment, the robot receives an interruption command while replaying the original trajectory. In response to the interruption command, the robot determines the fusion duration through adaptive calculation based on the current state information and relevant information of the target trajectory segment. The robot then determines the fused trajectory segment corresponding to the fusion duration through trajectory fusion processing, based on relevant information of the original trajectory, relevant information of the target trajectory segment, and the fusion duration. The fused trajectory segment avoids discontinuous jumps in the robot's joint angles, joint velocities, and joint accelerations, thus ensuring the robot's stability. The robot performs splicing and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the spliced and fused trajectory. After the robot executes the spliced and fused trajectory until it reaches the endpoint, the robot resumes executing the remaining trajectory of the original trajectory, thus not losing the progress of the original trajectory, i.e., not losing the remaining trajectory of the original trajectory. This improves the trajectory replay efficiency of the robot in interrupted scenarios.
[0068] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the starting position of each joint of the robot at the starting point of the target trajectory segment, the speed limit information of each joint, the minimum fusion time, and the speed limit safety factor. Based on the current state information and relevant information of the target trajectory segment, the fusion duration is determined through adaptive calculation, including: Based on the current position of each joint, the starting position of each joint, the velocity limit information of each joint, the minimum fusion time, and the velocity limit safety factor, the fusion time is determined by adaptive calculation. The fusion time is used to characterize the duration of the fused trajectory segment.
[0069] Specifically, for example, a trajectory fusion algorithm includes steps 1-7: Step 1: Detect interruption and record the current joint state, that is, process control commands and read the current state before fusion execution.
[0070] For example, during PAUSE, the wall clock time, the logic progress after freezing, and the trajectory version are recorded, and the speed is reduced and maintained according to the control strategy; during RESUME, the checkpoint is read, and the actual value is remeasured or estimated. , , To avoid using outdated states before the pause; record the target to be switched and its starting point during BLEND; check the starting state again from the completion of planning to the actual switch; if the state change exceeds the preset tolerance, replan and do not execute outdated fusion trajectory segments.
[0071] Step 2: Determine the fusion mode.
[0072] For example, if the current trajectory (e.g., the original trajectory) is still playing (e.g., PLAYING or BLENDING) when the interruption occurs, and the current trajectory data is still valid, then trajectory-to-trajectory fusion is selected, which allows the inertial reference of the original trajectory to be utilized; if only the current joint position point is available when the interruption occurs (e.g., resuming from IDLE state or PAUSED state), then point-to-trajectory fusion is selected.
[0073] For example, if the current trajectory is still being executed, the data is valid within the entire candidate fusion window, and its starting state matches the actual state at the time of switching, trajectory-to-trajectory fusion is selected; in the BLENDING state, the reference should be the currently executed composite trajectory, not the previous old trajectory; otherwise, point-to-trajectory fusion with constrained state is selected; if the state cannot be reliably obtained, no new motion is started.
[0074] Step 3: Adaptively calculate the fusion time.
[0075] For example, first calculate the fusion time of the candidates. Formula (1) provides an initial value estimate, rather than a sufficient condition for a speed limit across the entire interval: Formula (1) in, ; The first point of the target trajectory segment One degree of freedom position, To switch the actual position before execution, To accommodate the corresponding speed limits, the position difference and speed limit shall use the same unit; The initial velocity safety factor (safety factor) is estimated, for example, 0.15; It can be configured to 1.5 seconds; the continuous rotational degrees of freedom are first unfolded according to the selected motion direction, and then the position difference is calculated.
[0076] For example, with As candidate values, a fused trajectory is generated, an interpolator is constructed, and full-range validation is performed; if it fails, the next candidate duration can be set to the previous value. times, for example The candidate trajectory is regenerated and verified; the maximum number of attempts and the maximum allowed duration are set. If the requirements are not met after exceeding the boundaries, the candidate trajectory is rejected and the controlled stop or hold process is initiated.
[0077] For example, the verification includes the range of degrees of freedom, the upper limit of velocity, and the upper limit of configured acceleration and jerk, as well as the matching of the fused endpoint state with the current state and the target trajectory state; for each polynomial, the boundaries of each state quantity are determined by finding the real roots of the derivative in the interval and checking the endpoints and internal extrema; a conservative interval method with error bounds can also be used; checking only the discrete sampled values of the control cycle is insufficient to prove that the entire interval does not exceed the limits.
[0078] For example, based on the current position of each joint. The starting position of each joint Speed limit information for each joint Minimum fusion time and speed limit safety factor The fusion duration is determined through adaptive calculation. .
[0079] It should be noted that the fusion duration is adaptively calculated based on joint velocity constraints. The fusion duration is not fixed by humans, but is based on the current position of the joints. and the origin of the joint The difference and the speed limit of each joint are automatically calculated. This mechanism ensures that the fusion process does not violate the physical limitations of the joints, nor is it too slow to affect the response.
[0080] Step 4: Generate the sampling sequence of the fused trajectory segment.
[0081] For example, in Internal Order Number of sampling points , , This explicitly includes two endpoints, with the last interval allowed to be shorter than... If the controller only allows whole-cycle switching, then the candidate duration is first upquantized. Multiples of the specified integers are then regenerated and verified; for each... Calculate the fusion coefficient Formula (2) is shown below: Formula (2) in, ;For example The second corresponds to 250Hz. The sampling density and control frequency are selected according to the controller configuration, and the interval constraints are still verified using the final continuous interpolation function.
[0082] For example, based on the fusion pattern, the fused joint position is calculated; based on the trajectory-to-trajectory fusion pattern, each sampling time point is obtained. Position information of each joint That is, the joint position after fusion As shown in formula (3): Formula (3) in, The trajectory interpolator that is currently being executed and is valid throughout the entire window before the switch (e.g., the interpolator of the original trajectory). To switch the execution time within the logical time of the trajectory (e.g., the logical playback timestamp of the original trajectory when an interruption occurs); when switching again during the fusion process, Indicates the current composite trajectory; For the local time interpolator of the target trajectory segment; for Generalized position value, This is the fusion coefficient. For example... (·) Sample from the current trajectory spline interpolator, i.e. (·) indicates the interpolator for the original trajectory; Indicates a call (·), returns the result; (·) Sample from the target trajectory spline interpolator, i.e. (·) represents the interpolator for the target trajectory segment; Indicates a call (·), returns the result; Indicates in Discrete sampling points at time (25-dimensional angle values); This indicates the logical playback timestamp of the original trajectory when the interruption occurred; This represents the S-shaped fusion coefficient.
[0083] It should be noted that the trajectory-to-trajectory fusion mode is as follows: when the current trajectory at the interruption time is valid, a weighted average is performed on the sampled value of the current trajectory at the corresponding time and the sampled value of the target trajectory segment at the corresponding time.
[0084] For example, based on the fusion mode, the joint position after fusion is calculated; based on the point-to-trajectory fusion mode, a transition trajectory is first constructed (for example, linearly moving from the current point to the starting point of the target trajectory segment); in the point-to-trajectory fusion mode, a fifth-order transition polynomial that satisfies the current state boundary is adopted, as shown in formula (4): Formula (4) Among them, each coefficient is dimensional vector, by , , , , , These six boundary conditions are solved one degree of freedom at a time. As a fusion reference only, its terminal zero derivative does not require the target trajectory to be stationary; if it is actually stationary, i.e. ,but The third-order configuration is changed to a seventh-order transition polynomial, and boundary conditions of starting jerk matching and ending jerk being zero are added.
[0085] For example, by weighted fusion of the transition trajectory and the target trajectory segment, the results at each sampling time point can be obtained. Position information of each joint That is, the joint position after fusion As shown in formula (5): Formula (5) in, An interpolator representing the transition trajectory; Indicates a call The returned result; An interpolator representing the target trajectory segment; Indicates a call The returned result; This represents the S-shaped fusion coefficient.
[0086] It should be noted that point-to-trajectory fusion uses a transition polynomial that satisfies the starting point state constraints; the fusion weights are zero at the starting point and the derivative of the required order is zero, therefore the analytical fusion trajectory segment inheritance... The starting state; inheritance at the end point. and its corresponding derivative.
[0087] Step 5: Construct a fusion trajectory segment interpolator with node state constraints (e.g., construct a cubic spline interpolator for the fusion trajectory segment).
[0088] For example, using Configuration: Position, velocity, and acceleration are calculated by an analytical fusion function at each sampling node; a fifth-order Hermite polynomial is constructed for each degree of freedom using the six scalar boundary conditions of two adjacent nodes, and a piecewise interpolator is formed, as shown in formula (6): Formula (6) in, , ; Indicates joint At any moment The location information is used, and each segment uses the position, velocity, and acceleration of adjacent nodes as endpoint constraints; common nodes use the same state values, therefore the internal... Continuous; the first and last nodes respectively adopt the current state and The corresponding state.
[0089] For example, the endpoint properties of the fifth-order weights are used to derive the boundary states of the analytical fusion function; these states must be explicitly preserved during reinterpolation, and the endpoint derivatives cannot be assumed to be consistent simply by fitting ordinary cubic splines to the position sampling points; interpolation may change the extrema within the interval, so the final interpolator must be verified, rather than just the original analytical function.
[0090] Step 6: Piece together the remaining segments of the target trajectory segment to obtain the fused complete trajectory.
[0091] For example, This represents the splined trajectory, i.e., the complete trajectory after fusion; the fused trajectory segment spline. Indicates the fused trajectory segment. ; Target trajectory spline Indicates the target trajectory segment. The remaining segments corresponding to the target trajectory segment (from the target trajectory segment) arrive (trajectory) This represents the total duration of the target trajectory segment, i.e. The time point at which the target trajectory segment ends; ; .
[0092] For example, fusion interpolators Interpolator with target Store separately and execute according to local time; The place requires Its first and second derivatives are respectively equal to and its corresponding derivative; Configure and match the third derivative; the target trajectory segment should meet the same smoothness requirements at the access point and the remaining segment; if the original data does not meet the requirements, the target trajectory should be reconstructed and verified within the allowable error.
[0093] Step 7: Output the stitched and merged trajectory.
[0094] For example, the data structure of the spliced and fused trajectory includes: a list of cubic spline interpolators for the fused trajectory segment (one cubic spline interpolator for each joint, and 25 cubic spline interpolators for 25 joints), a reference to the target trajectory (without copying, directly referencing the original data), the fusion duration, the total duration of the target trajectory segment, and the flag bits of the fused trajectory segment, etc.
[0095] It should be noted that during the robot trajectory replay process, when an interruption command is received at any time and it is necessary to return to the specified trajectory point for execution, the trajectory fusion algorithm is used to achieve a smooth, seamless transition between the current motion state and the target trajectory segment.
[0096] For example, the dual fusion mode adaptive selection adaptively selects two fusion modes based on whether the current trajectory is still valid at the time of interruption: trajectory-to-trajectory fusion, when the current trajectory is still valid, directly weighted and fused the current trajectory sample value and the target trajectory segment sample value; point-to-trajectory fusion, when there is only the current state point (e.g., recovering from a static or paused state), first make a linear transition from the current state point to the starting point of the target trajectory segment, and then weighted and fused with the target trajectory segment.
[0097] For example, the output data structure includes The program includes an interpolator with each degree of freedom, a target trajectory identifier and version, a target interpolator reference, a fusion duration, a total duration of the target trajectory segment, a continuity order, a recovery time offset, and constraint verification results. During execution, an immutable trajectory version is used to avoid changes in target data during planning.
[0098] For example, when a recovery or switch execution instruction is received and the state and security conditions are met, a candidate trajectory that matches the current state and is connected to the specified target trajectory segment is generated through a trajectory fusion algorithm; candidate trajectories that fail the verification are not sent for execution.
[0099] For example, let the reference trajectory for or Target trajectory segment Then the analytical fusion function .when , At least for And when the first and second derivatives at both ends of the fifth weight are zero, Inheritance at the starting point The state is inherited at the endpoint. The starting point must match the actual state, and boundary constraints must still be applied when re-interpolating.
[0100] For example, the dual fusion mode selects based on the validity of the current reference trajectory and the actual state matching: the trajectory-to-trajectory mode uses the currently valid trajectory; the point-to-trajectory mode uses the transition polynomial that matches the current position, velocity, and acceleration; both modes use the same endpoint matching and full-range constraint verification.
[0101] In one embodiment, the speed limit safety factor ranges from 0.05 to 0.5, and the minimum fusion time ranges from 0.5 seconds to 3 seconds.
[0102] Specifically, for example, speed limit safety factor The value ranges from 0.05 to 0.5, and the minimum fusion time is... The value ranges from 0.5 seconds to 3 seconds.
[0103] In one embodiment, motion continuity is ensured, for example, through the following three-layer mechanism: Continuous (positionally continuous): The analytical fusion function and the final interpolator satisfy... , The trajectory-to-trajectory pattern still needs to be verified. When tracking deviations exist, a transitional programming approach with state constraints should be adopted.
[0104] Continuous (speed continuous): The starting speed of the fusion is equal to the actual speed. The final speed is equal to Neither of them is required to be zero; weighted derivative Used to eliminate boundary additions introduced by the weighting process; this condition is not equal to the joint velocity being zero.
[0105] Continuous (acceleration continuity): The initial acceleration equals the actual acceleration. The final acceleration is equal to The weights are required to have zero first and second derivatives at both ends, and the boundary states and internal states are preserved through five-fold Hermite node state constraints. Continuity.
[0106] For example, speed verification should include a weighted additional term; differentiated with respect to actual time. Indicating the rate of change of weights, then The initial value estimate of the location difference was not covered. , and the entire interval ,therefore Only initial margin is provided; the velocity limit conclusion is based on the full-range verification of the final interpolator, and acceleration and jerk are handled in the same way.
[0107] For example, joint velocity limit verification, in the calculation of fusion duration, fusion duration As shown in formula (1), through ≥ | | / ( Ensure that the maximum speed of each joint during the fusion process does not exceed the physical limits. , Provides additional security margin.
[0108] In one embodiment, for example, timeline management, state checkpoint storage, and recovery processes can be used to support any number of interruptions and recoveries during a single trajectory replay.
[0109] For example, in the case where no trajectory switching occurs, no time scaling is performed, and the motion segment advances at a unit rate, the original trajectory logical time is calculated by formula (7): Formula (7) in, The elapsed time from the starting point of this trajectory timing to the current time on the wall clock, including periods of pause and waiting for planning; This is the sum of the durations of all time intervals where logical time is frozen; during the actual execution period, the target local time progresses synchronously with the original trajectory logical progress, and is no longer deducted. Whether progress is frozen during the planning period must be consistent with the status checkpoint records.
[0110] For example, the cumulative processing of multiple interruptions during a single trajectory replay includes: Record the duration of each interruption and maintain the cumulative pause duration; In the above-described scenario without switching or time scaling, the first... The logical progress corresponding to the next recovery time is calculated according to formula (8): Formula (8) in, Including each logical freeze interval, It is the first The duration of the wall clock in the second freeze interval cannot be deducted again from the effective motion time that has already been deducted for the pause. When a trajectory switch or time scaling occurs, the progress is calculated using the trajectory identifier, the saved logical progress, and the explicit time mapping, and formula (7) or (8) is not applied again.
[0111] For example, the status checkpoint saves the current execution trajectory identifier and version, logical progress, wall clock reference, cumulative freeze duration, current composite trajectory reference, and measured status; the recovery determines the starting point of the target trajectory segment based on the saved progress, and determines the fusion starting point based on the status remeasured during recovery.
[0112] For example, checkpoints can be stored in memory or persistent storage, or updated periodically; a monotonic clock is used to record the duration of the current run; when resuming across processes, the wall clock reference is re-established based on the persistent logical progress, without directly reusing the monotonic clock readings of the old process; this mechanism helps to avoid repeated deduction of pauses or erroneous replays; the actual error is also affected by timing accuracy, state estimation, and execution tracing errors.
[0113] For example, during each recovery, checkpoints and trajectory versions are verified, the actual state is read, and replanning is performed. After the new fused trajectory segment is completed, the remaining part of its corresponding target trajectory segment is continued. If the process is paused again during fusion, the reference to the current composite trajectory and the corresponding target logic progress are saved. Subsequent recovery will still recalculate based on the actual state and will not directly jump back to the old instructions.
[0114] For example, in a recovery process, the current joint position is read from the status checkpoint each time recovery is performed. Playback time of the original track Current trajectory reference, cumulative pause duration The system uses information such as trajectory fusion algorithm to generate a new fused trajectory segment and starts execution from the new fused trajectory segment. After the new fused trajectory segment finishes playing, it automatically switches to the remaining segment of the target trajectory segment without losing progress.
[0115] In one embodiment, for example, the sampling and execution of the fused trajectory segment are both performed under a 250Hz control cycle (Δt=0.004 seconds) to ensure real-time performance; the fused trajectory segment generation process (e.g., trajectory fusion algorithm) is usually completed within tens of milliseconds without blocking the control loop.
[0116] For example, the controller executes the verified trajectory at a 250Hz cycle; planning and verification are completed in separate tasks, and the trajectory is switched through double buffering or equivalent mechanisms; the actual planning time needs to be measured on the target hardware; when the planning timeout or the starting state has changed, the established safety control strategy is maintained and the current instruction is not replaced by an unverified trajectory.
[0117] In one embodiment, for example, the fifth-order weights and other weights listed in Table 1 are used for analytical fusion of the corresponding orders; in Table 1, continuity refers to the continuity that can be achieved when the reference trajectory, endpoint state, and reconstruction interpolator all meet the corresponding orders; the weight function itself cannot guarantee physical constraints or actual tracking accuracy.
[0118] Table 1: Integration Weights and Alternative Programming Methods
[0119] In one embodiment, for example, an alternative strategy for merging duration calculations includes: Fixed candidate duration: Manually set values can be used as initial candidate values, but the same endpoint and full range constraint verification must still be performed; Candidate initial values based on acceleration constraints: For a simplified model considering only symmetrical acceleration and deceleration in a one-dimensional rest-to-rest transition, refer to... Estimating the time scale; this estimation is not applicable to directly proving that general dual-track fusion satisfies acceleration constraints, and the final duration is still determined by the complete trajectory verification. Time-Optimal Programming: Simultaneously considering velocity, acceleration, jerk constraints, etc., to solve the time-optimal transition trajectory, which has the shortest fusion time but is computationally complex; User-specified and automatic verification: The user's expected duration is used as a candidate value. If the verification fails, the duration is adjusted or the plan is re-planned. If the constraints are still not met or the effective time domain is exceeded after adjustment, failure is returned. It is not guaranteed that simply extending the duration will always yield a feasible solution.
[0120] In one embodiment, for example, an alternative implementation of the interruption detection mechanism includes: Command queue polling: The control thread periodically checks the command queue. It is simple to implement and has a delay of one control cycle. Interrupt / signal driven: interrupting instructions triggers signals, immediately preempting the control loop, resulting in lower latency but more complex implementation; Shared memory flag: In a multi-process architecture, interruption requests are passed through the shared memory flag, which is suitable for cross-process deployment.
[0121] In one embodiment, for example, in addition to weighted fusion, a time scaling strategy can also be used to achieve smooth switching; the time scaling strategy includes: Time scaling is achieved through monotonic time mapping. Reparameterized path, speed is Acceleration also includes Item; only if the current position is on the target path and the required current velocity can be calculated from the path tangent vector and the same scalar. During matching, speed transition can only be achieved by time scaling alone; otherwise, other transition planning should be used; after reparameterization, all state constraints should be re-validated and the progress mapping updated.
[0122] In one embodiment, for example, a state checkpoint storage strategy includes: Memory storage: Checkpoints are stored in the process's memory, which allows for fast read and write operations, but they are lost if the process crashes. Persistent storage: Checkpoints are written to local files or databases, supporting recovery after process restart, but increasing I / O overhead; Periodic checkpoints: Instead of saving when interrupted, checkpoints are saved at fixed intervals (e.g., every second), supporting recovery from any point in time, but with increased storage overhead.
[0123] In one embodiment, based on relevant information of the original trajectory, relevant information of the target trajectory segment, and fusion duration, trajectory fusion processing is used to determine the fused trajectory segment corresponding to the fusion duration, including: Based on the fusion duration and the preset control cycle, sampling processing is performed within the fusion time window corresponding to the fusion duration to obtain multiple sampling time points; Determine the fusion coefficient for each sampling time point. The fusion coefficient is used to ensure at least one of the following: continuous joint position, continuous joint velocity, continuous joint acceleration, and continuous joint jerk. Based on relevant information from the original trajectory, the fusion mode is determined; Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode. Based on the position information of each joint at each sampling time point, the interpolator for each joint is determined; Based on the interpolators of each joint, construct the fused trajectory segment and the interpolator of the fused trajectory segment.
[0124] Specifically, for example, based on current state information, relevant information of the current trajectory, relevant information of the target trajectory segment, and fusion duration, the fusion trajectory segment corresponding to the fusion duration is determined through trajectory fusion processing, including: Based on the fusion duration and the preset control cycle, sampling is performed within the fusion time window, which includes time zero and the end of fusion, to obtain multiple sampling time points; The fusion coefficient for each sampling time point is determined according to the required continuity order; the fusion coefficient, together with the reference trajectory, endpoint state constraints and interpolator that satisfy the corresponding order, is used to achieve the continuity of position, velocity, acceleration or jerk. Based on relevant information about the current trajectory, determine the fusion mode; Based on the fusion coefficient, current state information, relevant information of the current trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode. Based on the location of each degree of freedom at each sampling time point and the derivative information corresponding to the required continuity order, an interpolator for each degree of freedom that satisfies the state constraints of the sampling nodes and the fusion endpoints is constructed. The fused trajectory segment is constructed based on the interpolator of each degree of freedom, and its endpoint connection conditions and motion constraints within the entire interval are verified.
[0125] For example, in Internal Order Number of sampling points , , This explicitly includes two endpoints, with the last interval allowed to be shorter than... If the controller only allows whole-cycle switching, then the candidate duration is first upquantized. Multiples of the specified integers are then regenerated and validated. For each... Calculate the fusion coefficient : Formula (2) in, ;For example The second corresponds to 250Hz. The sampling density and control frequency are selected according to the controller configuration, and the interval constraints are still verified using the final continuous interpolation function.
[0126] For example, based on the fusion pattern, the fused joint position is calculated; based on the trajectory-to-trajectory fusion pattern, each sampling time point is obtained. Position information of each joint That is, the joint position after fusion As shown in formula (3): Formula (3) in, The trajectory interpolator that is currently being executed and is valid throughout the entire window before the switch. To switch the execution time within the logical timeframe of this trajectory; when switching again during the fusion process, This indicates the current composite trajectory. For the local time interpolator of the target trajectory segment; for Generalized position value, This is the fusion coefficient.
[0127] For example, in the point-to-trajectory fusion mode, a fifth-order transition polynomial that satisfies the current state boundary is used, as shown in formula (4): Formula (4) Among them, each coefficient is dimensional vector, by , , , , , These six boundary conditions are solved one degree of freedom at a time. As a fusion reference only, its terminal zero derivative does not require the target trajectory to be stationary. If it is actually stationary, i.e. ,but The third-order configuration is changed to a seventh-order transition polynomial, and boundary conditions of starting jerk matching and ending jerk being zero are added.
[0128] For example, by weighted fusion of the transition trajectory and the target trajectory segment, the results at each sampling time point can be obtained. Position information of each joint That is, the joint position after fusion As shown in formula (5): Formula (5) in, An interpolator representing the transition trajectory; Indicates a call The returned result; An interpolator representing the target trajectory segment; Indicates a call The returned result; This represents the S-shaped fusion coefficient.
[0129] For example, using Configuration: Position, velocity, and acceleration are calculated by an analytical fusion function at each sampling node; a fifth-order Hermite polynomial is constructed for each degree of freedom using the six scalar boundary conditions of two adjacent nodes, and a piecewise interpolator is formed, as shown in formula (6): Formula (6) In this context, the position, first-order, and second-order derivatives take the same value at common nodes in adjacent segments; the first node uses the current state, and the last node uses the target trajectory at... The state of time is thus constructed .
[0130] In one embodiment, for example, the fusion coefficient is an S-shaped fusion coefficient, which supports configuring the continuity order, including zero-order, first-order, second-order and third-order. The zeroth order is used to ensure the continuity of joint positions, and the corresponding S-shaped fusion coefficient is a linear function. The first order is used to ensure the continuity of joint position and joint velocity, and the corresponding S-shaped fusion coefficient is a cubic polynomial. The second order is used to ensure the continuity of joint position, joint velocity, and joint acceleration, and the corresponding S-shaped fusion coefficient is a fifth-order polynomial; The third order is used to ensure the continuity of joint position, joint velocity, joint acceleration, and joint jerk. The corresponding S-shaped fusion coefficients are seventh-order polynomials.
[0131] For example, the fusion coefficients support zero-order, first-order, second-order, and third-order continuity configurations; when the reference trajectory, boundary conditions, and interpolator all satisfy the configured order, the fusion coefficients for each order are selected as follows: Zero-order fusion is used to ensure joint position continuity, and the corresponding fusion coefficients are linear functions. The first order is used to ensure the continuity of joint position and joint velocity, and the corresponding S-shaped fusion coefficient is a cubic polynomial. The second order is used to ensure the continuity of joint position, joint velocity and joint acceleration, and the corresponding S-shaped fusion coefficient is a fifth-order polynomial. The third order is used to achieve continuity of position, velocity, acceleration and jerk, with the corresponding fusion coefficients being seventh-order polynomials, and an interpolator that satisfies the continuity of the third derivative and the endpoint matching constraint is used.
[0132] For example, the following configurations all assume that the reference trajectory meets the corresponding smoothness requirements and that the interpolator explicitly preserves the required node and endpoint derivatives: Configuration: Linear weights It is only used for position continuity requirements; it cannot be used to claim that velocity or acceleration is continuous. Whether it is allowed to be executed is still determined by the corresponding control mode and motion constraints.
[0133] Configuration: Uses cubic smoothstep weights and selects an interpolator that satisfies position, velocity node, and endpoint constraints.
[0134] Configuration: The main configuration of this embodiment is a piecewise quintic Hermite interpolator with quintic smootherstep weights and node constraints for position, velocity, and acceleration.
[0135] Configuration: Seventh-order weights and a piecewise seventh-order Hermite interpolator are used, and jerk is matched at each node and both ends; the current state, reference trajectory and target trajectory must also provide corresponding third-order information, and ordinary cubic splines cannot generally meet this requirement.
[0136] For example, it supports configuring the continuity order, which includes: The order (zero order) only guarantees positional continuity, and the fusion coefficient is a linear function; First order (first order), ensuring continuity of position and velocity, the S-shaped fusion coefficients are cubic polynomials. ; The second order ensures the continuity of position, velocity, and acceleration, and the S-shaped fusion coefficient is a fifth-order polynomial. The third order further ensures the continuity of the accelerator, and the S-shaped fusion coefficient is a seventh-order polynomial.
[0137] In one embodiment, the relevant information of the original trajectory includes the interpolator of the original trajectory, and the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment. Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: If the fusion mode is trajectory-to-trajectory fusion mode, then a weighted fusion process is performed based on the fusion coefficient, the interpolator of the original trajectory, and the interpolator of the target trajectory segment to obtain the position information of each joint at each sampling time point; If the fusion mode is a point-to-trajectory fusion mode, then an interpolator for the transition trajectory is constructed based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment. Based on the fusion coefficient, the interpolator for the transition trajectory, and the interpolator for the target trajectory segment, a weighted fusion process is performed to obtain the position information of each joint at each sampling time point.
[0138] Specifically, for example, the relevant information of the current trajectory includes a validity flag, and its interpolator when the current trajectory is valid; the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; Based on the fusion coefficient, current state information, relevant information of the current trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: If the fusion mode is trajectory-to-trajectory fusion mode, then a weighted fusion process is performed based on the fusion coefficient, the interpolator of the current valid trajectory, and the interpolator of the target trajectory segment to obtain the position information of each joint at each sampling time point; If the fusion mode is a point-to-trajectory fusion mode, then a transition trajectory with endpoint constraints is constructed based on the current state information, so that its starting position, velocity and acceleration match the current state; the transition trajectory and the target trajectory segment are weighted and fused with the fusion coefficient to obtain the state at each sampling time point; when the third-order continuity configuration is adopted, the starting acceleration is further constrained.
[0139] For example, based on the fusion pattern, the fused joint position is calculated; based on the trajectory-to-trajectory fusion pattern, each sampling time point is obtained. Position information of each joint That is, the joint position after fusion As shown in formula (3): Formula (3) in, The trajectory interpolator that is currently being executed and is valid throughout the entire window before the switch. To switch the execution time within the logical timeframe of this trajectory; when switching again during the fusion process, This indicates the current composite trajectory. For the local time interpolator of the target trajectory segment; for Generalized position value, This is the fusion coefficient.
[0140] For example, in the point-to-trajectory fusion mode, a fifth-order transition polynomial that satisfies the current state boundary is used, as shown in formula (4): Formula (4) Among them, each coefficient is dimensional vector, by , , , , , These six boundary conditions are solved one degree of freedom at a time. As a fusion reference only, its terminal zero derivative does not require the target trajectory to be stationary. If it is actually stationary, i.e. ,but The third-order configuration is changed to a seventh-order transition polynomial, and boundary conditions of starting jerk matching and ending jerk being zero are added.
[0141] For example, by weighted fusion of the transition trajectory and the target trajectory segment, the results at each sampling time point can be obtained. Position information of each joint That is, the joint position after fusion As shown in formula (5): Formula (5) in, An interpolator representing the transition trajectory; Indicates a call The returned result; An interpolator representing the target trajectory segment; Indicates a call The returned result; This represents the S-shaped fusion coefficient.
[0142] In one embodiment, for example, the type of interpolator for each joint includes any one of cubic spline interpolator, quintic spline interpolator, piecewise cubic Hermite interpolator, B-spline interpolator, or interpolator constructed based on the minimum jerk criterion.
[0143] For example, interpolators for each degree of freedom are selected according to continuity requirements: a piecewise fifth Hermite interpolator with position, velocity and acceleration node constraints is used for second-order continuity configurations; a piecewise seventh Hermite interpolator with further accelerometer node constraints is used for third-order continuity configurations; other interpolators are used only when the configured internal continuity and endpoint connection constraints are met.
[0144] For example, other interpolation implementations should be selected based on the continuity and boundary conditions they can satisfy: Fifth-order Hermite interpolation: Position, velocity, and acceleration are explicitly given at both ends of each segment, and the node states are shared. continuous; B-splines: Continuity depends on the degree and the number of nodes. When using them, it is also necessary to apply start and end state constraints and check for overshoot within the interval. PCHIP piecewise cubic Hermite interpolation: generally guarantees Shape preservation is not guaranteed within continuous and monotonic intervals of data. Continuous acceleration cannot directly replace the main implementation method that requires continuous acceleration. Ordinary cubic splines: generally guarantee internal... Continuous, but simply inputting position sampling points cannot guarantee simultaneous matching of velocities and accelerations at both ends; it should only be used when the required endpoint constraints have been verified to be met. Linear interpolation with low-pass filtering also needs to take into account the filter state and phase delay, and cannot be directly used as a strict method. ensure.
[0145] In one embodiment, the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; Based on the fused trajectory segment and the target trajectory segment, a stitching and fusion process is performed to determine the stitched and fused trajectory, including: Based on the interpolator of the fused trajectory segment and the interpolator of the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; The interpolator for the fused trajectory segment corresponds to the first time period, and the interpolator for the target trajectory segment corresponds to the second time period. In the first time period, each time point is less than the fusion duration, and in the second time period, each time point is greater than or equal to the fusion duration and less than or equal to the end time point. The total duration of the target trajectory segment is equal to the end time point.
[0146] Specifically, for example, This represents the splined trajectory, i.e., the complete trajectory after fusion; the fused trajectory segment spline. Indicates the fused trajectory segment. ; Target trajectory spline Indicates the target trajectory segment. The remaining segments corresponding to the target trajectory segment (from the target trajectory segment) arrive (trajectory) This represents the total duration of the target trajectory segment, i.e. The time point at which the target trajectory segment ends; ; The interpolator for the fused trajectory segment corresponds to the first time period as follows: The interpolator for the target trajectory segment corresponds to the second time period. The fusion time is The time of the finish line is .
[0147] For example, the target trajectory segment uses its selected starting point as the local time zero point, and the total duration is... ; fusion duration satisfy ;exist When using a fused trajectory segment interpolator, in Use the target trajectory segment interpolator; At this point, the positions of the two interpolators and the derivatives of their respective orders are matched.
[0148] For example, ( ), ( ) Verification before fusion generation Furthermore, the current reference in the trajectory-to-trajectory mode is valid throughout the entire window; if this condition is not met, out-of-bounds sampling or blind extrapolation is prohibited. Explicit temporal reparameterization of the target trajectory segment can be performed and updated, provided the task allows. After calculating all derivatives, re-plan and verify; otherwise, reject the fusion and stop or maintain it under controlled conditions. There is no remaining segment with positive duration; if it is necessary to maintain the target endpoint after reaching it, the target endpoint must meet the holding conditions such as zero velocity and zero acceleration, otherwise it should be connected to the verified subsequent motion or controlled deceleration segment.
[0149] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the end position of each joint of the robot at the end of the target trajectory segment. Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: Based on the current position and the endpoint position of each joint, forward kinematics calculations are performed to obtain the pose corresponding to the current position and the pose corresponding to the endpoint position of each joint. The pose includes position components and attitude quaternion components. Based on the fusion coefficients, the poses corresponding to the current positions of each joint and the poses corresponding to the end positions of each joint, interpolation is performed to determine the position information of each joint at each sampling time point.
[0150] Specifically, for example, the current state information includes the robot's current degree of freedom state, and the relevant information of the target trajectory segment includes the target degree of freedom state corresponding to each sampling time within the fusion window; Based on the fusion coefficient, current state information, relevant information of the current trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode, including: For the reference state of the current valid trajectory or the transition trajectory that satisfies the starting state constraint at each sampling time, and the target state of the target trajectory segment at the same time, forward kinematics calculations are performed to obtain the reference end pose and the target end pose, wherein the pose includes position components and attitude quaternion components. Interpolate the reference end pose and the target end pose based on the fusion coefficient, and perform inverse kinematics solution with adjacent solution continuity constraints and endpoint degree-of-freedom state constraints to obtain the degree-of-freedom state at each sampling time point; verify the continuity and motion constraints of the joint trajectory obtained by the solution, and replan or switch to controlled stopping state when the solution or verification fails.
[0151] For example, for applications requiring precise control of the end-effector pose, a Cartesian space fusion option is provided; fusion is performed in Cartesian space instead of joint angle space; fusion in Cartesian space includes (1)-(4): (1) For the reference trajectory and target trajectory Perform forward kinematics calculations at each time step to obtain and . To match the valid current trajectory or constrained transition trajectory of the actual starting state; At the endpoint of fusion Instead of the final moment of the target trajectory segment. The state.
[0152] (2) Use the fusion weight to perform weighted interpolation on two position references at the same time, as shown in formula (9): Formula (9) in, The selected weight order should be consistent with the required continuity, and the position reference and its derivative should meet the corresponding smoothness conditions.
[0153] (3) Perform SLERP interpolation (spherical linear interpolation) on the attitude quaternion, as shown in formula (10): Formula (10) (4) Perform inverse kinematics on the interpolated end pose, using the solutions at adjacent time points as initial values, and constrain the matching of the starting state with the actual state and the matching of the ending state with the actual state. Matching; for redundant degrees of freedom, continuous redundant parameters or null space references are used, and jumps between different inverse kinematic branches are not allowed. For degrees of freedom that do not use this end-effector pose control, joint space fusion can be used, and the final overall trajectory is uniformly verified.
[0154] For example, forward kinematics (FK) calculations are performed on the current joint angle (the current position of each joint) and the target joint angle (the endpoint position of each joint) to obtain the end pose corresponding to the current joint angle. End pose corresponding to the target joint angle ;in, Indicates the end position (position component) at the moment of interruption. The terminal attitude (attitude quaternion component) represents the moment of interruption. This indicates the end position (position component) corresponding to the end of the target trajectory segment. This represents the end attitude (attitude quaternion component) corresponding to the end of the target trajectory segment.
[0155] For example, quaternions should first be normalized, and the signs of adjacent quaternions should be continuous. When the relative rotation is close to 180 degrees, the inverse kinematics is close to a singular configuration, or there is an unreachable pose, the joint continuity should not be directly estimated by SLERP and point-by-point inverse kinematics; a new reference or replanning should be selected. The position sequence generated by the inverse kinematics must also construct a joint interpolator with the required nodal derivative constraints to verify the end-effector error, start and end-effector states, continuity, and motion constraints of each degree of freedom; if verification fails, the trajectory should not be executed.
[0156] For example, trajectory fusion algorithms support fusion in Cartesian space, including: right and Forward kinematics calculations are performed on the degrees of freedom at the same local time to obtain the corresponding end pose; Linear interpolation is performed on the position components using S-shaped fusion coefficients as weights; Spherical linear interpolation is performed on the attitude quaternion components using the S-shaped fusion coefficient as a parameter; The joint space is returned by solving the inverse kinematics continuously and satisfying the endpoint constraints. The other degrees of freedom are then fused using the joint space and the whole machine constraint is checked uniformly.
[0157] For example, error recovery mechanisms include: If the Cartesian pattern, inverse kinematics reachability, or continuous branching conditions are not met, the joint space candidate trajectory can be regenerated and verified when the task allows; otherwise, the motion request is rejected, and a controlled stop or hold is performed according to the current state.
[0158] If the candidate fusion duration exceeds the target effective duration, maximum allowed duration, or calculation limit, the current planning attempt will be stopped; it will only be retried after the task allows and the target time mapping is explicitly updated, without processing through infinite delay or out-of-bounds extrapolation.
[0159] If the target fails to load or the status is unavailable, no new fusion trajectory segment will be issued; robots in motion will be stopped under control according to existing safety policies, and robots that have been stationary will remain stationary.
[0160] Applying the embodiments of this disclosure has at least the following beneficial effects: (1) Improve trajectory connection during interruption and recovery.
[0161] Under the conditions that the reference trajectory satisfies the corresponding smoothness, the actual starting state matches, and the interpolation boundary constraints are valid, position, velocity, and acceleration continuity are achieved at the command trajectory level. Direct switching may produce finite state jump variables; its ideal derivative may exhibit impulses, and the velocity jump variable itself cannot be generally described as infinite. The actual tracking error, mechanical shock, and reduction of emergency stops need to be evaluated in conjunction with controller and actual machine testing.
[0162] (2) Supports interruption and resumption during trajectory execution.
[0163] Upon receiving the recovery command, the system plans and continues the target trajectory segment from the saved logical progress, without needing to replay the entire original trajectory from the beginning. Recovery waiting time includes planning, verification, and necessary state preparation time; the time required to access the target trajectory also includes... . The value is set according to the configuration, such as 1.5 seconds. This example cannot be regarded as a fixed lower bound for all implementations.
[0164] (3) Adaptively select fusion duration based on state and constraints.
[0165] The initial value of the fusion time is estimated based on the state difference and speed limit. Then, the time is adjusted or replanned by verifying the interval constraints of the final trajectory to reduce the need to manually set the time for each switch. The safety factor, motion limit, tolerance and maximum number of attempts still need to be configured according to the robot and the task.
[0166] (4) Maintain the logical progress after multiple interruptions through checkpoints.
[0167] Cumulative freeze duration, trajectory version, and explicit time mapping are used to maintain the progress of multiple recovery cycles, reducing repeated time deductions and error replays. The time management mechanism does not eliminate sensor errors, numerical errors, or execution tracking errors; each recovery requires re-verification of the actual state and validation of the new trajectory.
[0168] (5) Dual-space fusion adapts to different control requirements.
[0169] Joint space fusion directly constrains the states of each degree of freedom; Cartesian space fusion takes into account end-effector path requirements through time-corresponding end-effector pose interpolation and continuous inverse kinematics. Both modes are executed on the condition that the endpoint states of the final joint trajectory, motion constraints, and task allowable error verification pass.
[0170] (6) Improve the user experience and security of the demonstration scene.
[0171] In scenarios such as product demonstrations and exhibitions, fusion trajectories that meet the above conditions help improve the motion perception during recovery and reduce the impact caused by sudden changes in instructions; the effect depends on the specific robot, controller, and task.
[0172] To better understand the methods provided in the embodiments of this disclosure, the solutions of the embodiments of this disclosure will be further explained below with reference to specific application scenarios.
[0173] In one embodiment, the method provided in this disclosure is applied to a robot trajectory replay system, which is used to automatically repeat the trajectory in a demonstration scenario. The demonstrator can issue commands such as pause, resume, switch trajectories, and emergency stop at any time during the trajectory playback through the control panel. This ensures that after each interruption and resumption, the robot smoothly transitions from its current posture to the designated point on the target trajectory, with continuous and jitter-free movement, thereby improving the demonstration effect and safety.
[0174] For example, in a demonstration trajectory replay system, operators can issue pause, resume, switch, and stop requests. Pause and stop follow the corresponding control flow; resume and switch are only executed when the robot's state allows it and the candidate trajectory has passed verification; emergency stop is handled by an independent safety mechanism and cannot be automatically deactivated by the fusion algorithm.
[0175] Typical application scenarios of the methods provided in this disclosure include: Product launch demonstration: The presenter pauses the robot's movements to explain the details, and resumes playback afterward; Production line operation trajectory recording and playback: The operator can seamlessly switch to the new trajectory after correcting the trajectory; Remote operation: Control can be restored smoothly after network latency or packet loss, and the current trajectory can be seamlessly accessed.
[0176] In one embodiment, such as Figure 2 As shown, the trajectory replay system includes: Track replay request: A user-issued command to "start executing a certain track"; for example, a command to restore the original track; Track loading module: Reads pre-recorded track data (joint angle sequence and timestamp sequence) from HDF5 file. Cubic spline interpolator (target trajectory interpolator): interpolates or reconstructs recorded data according to the required order, clarifies the effective duration and endpoint states, and verifies its internal continuity and motion constraints; The scenario can use cubic splines if the requirements are met. The scenario must be selected with corresponding continuity; a cubic spline interpolator is constructed for the discrete points of the original trajectory to ensure that the original trajectory is within a certain range. continuous.
[0177] Fusion Decision Module: Verifies the execution command, trajectory validity, and actual starting point status, and selects trajectory-to-trajectory or point-to-trajectory mode with constrained status; that is, it determines whether the current trajectory is valid and selects "trajectory-to-trajectory fusion" or "point-to-trajectory fusion".
[0178] The adaptive calculation module for fusion duration estimates the candidate duration (fusion duration), adjusts the duration based on the final trajectory constraint verification results, or returns a planning failure.
[0179] Trajectory fusion module: Generates an interpolator for fused trajectory segments and splices the fused trajectory segments with the target trajectory segments; S-shaped weighted fusion: For example, step 4 in the trajectory fusion algorithm; Fusion trajectory segment spline resampling (fusion trajectory segment interpolation and verification): Construct an interpolator with node state constraints and verify the motion constraints and access point status of the entire interval; Motion command generation module: Samples from the fused trajectory at a control cycle of 250Hz to generate joint angle or Cartesian pose commands; Actuator: Sends the periodic motion command corresponding to the verified trajectory to the robot body to execute the actual motion; Robot body: It has independent controlled degrees of freedom and corresponding execution and sensing mechanisms determined by the actual configuration; its state dimension is consistent with the trajectory, motion constraints and the number of interpolators; Status feedback module: Provides real-time feedback on the robot's status for use by the interruption detection module; Interruption detection module: Real-time detection of the command queue, detecting interruption commands such as PAUSE / STOP / RESUME / BLEND; Status checkpoint saving module: Saves track identifier, version, logical progress, clock reference, cumulative freeze duration and current status, etc.; that is, records the joint position at the time of interruption, track playback timestamp, cumulative pause duration, etc. Interrupt command input: External control commands from the user (PAUSE / RESUME / BLEND / STOP).
[0180] In one embodiment, such as Figure 3 As shown, the trajectory fusion algorithm includes the following steps: starting from the detection of an interruption (interruption command), recording the current joint state, determining the fusion mode (trajectory-to-trajectory fusion / point-to-trajectory fusion), sampling the current trajectory position and the sampled target trajectory position, linear transition interpolation, calculating the S-shaped fusion coefficient, calculating the fusion position by weighted average, cyclic sampling until the fusion trajectory segment is completed, constructing a cubic spline interpolator for the fusion trajectory segment, splicing the remaining segments of the target trajectory, and outputting the spliced and fused trajectory.
[0181] For example, after the execution instruction undergoes a status check, it enters mode selection, generates a currently valid reference or a constrained transition reference, samples and fuses the data, and constructs an interpolator with node state constraints. If the endpoint or interval constraint verification fails, the duration is adjusted and recalculated within the valid time domain and number of attempts. After passing the verification, the remaining segments of the target trajectory segment are spliced together and output. If a feasible solution cannot be obtained, it enters controlled stop or hold.
[0182] In one embodiment, such as Figure 4 The continuity constraint guarantee mechanism for velocity / acceleration shown includes three levels of constraints: C0 position continuity, C1 velocity continuity, and C2 acceleration continuity, as well as their corresponding implementation methods: the fusion starting point is equal to the current actual position, the endpoint of the first derivative of the S-curve is zero, and the endpoint of the second derivative of the S-curve is zero. The relationship between the Smootherstep weighting function, the smooth monotonicity of the fusion coefficients, the cubic spline interpolator, the joint velocity limit check, the safety factor scaling, and the non-jump output trajectory is also shown.
[0183] For example, continuity comes from the combined effect of current state matching, weight endpoint properties, and interpolation node state constraints; motion constraints are verified by interval extreme value verification of the final interpolator; the trajectory is output only after both types of conditions are met, and the absence of jumps and limits cannot be inferred solely from S-shaped weights or safety factors.
[0184] In one embodiment, such as Figure 5 The timeline shown includes: the original playback track, the breakpoint (t... int ), pause state retention, resume command (t) resume ), fusion time window ([0, T) b ]), fusion trajectory segment execution, target point arrival (t) target The temporal relationships formed by the continued playback of the remaining segments of the original trajectory, the completion of playback, etc., as well as the input effect of cumulative pause duration compensation and checkpoint state quantity storage on the fusion time window.
[0185] For example, during a pause, the logical progress is frozen and checkpoints are saved; upon resumption, the actual state is reread, planning and verification are performed, and the target logical time is advanced when the fusion window is executed. Access the remaining segments of the target trajectory segment; restore the original trajectory corresponding to the scene. At this point, switch the scene to the local area corresponding to the newly specified trajectory. At the point where the target trajectory segment ends, playback will not repeat from the old breakpoint.
[0186] In a specific application scenario, such as a trajectory replay scenario, see [link to relevant documentation]. Figure 6 This illustrates the processing flow of a trajectory replay method, such as... Figure 6 As shown, the processing flow of the trajectory replay method provided in this embodiment includes the following steps: S601, during the robot's trajectory replay based on the original trajectory, the terminal sends an interrupt command to the robot.
[0187] S602, in response to receiving an interruption command, the robot obtains relevant information about the original trajectory and relevant information about the target trajectory segment. The relevant information about the original trajectory includes the robot's current state information, and the relevant information about the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach.
[0188] S603: Based on the current state information and relevant information of the target trajectory segment, the robot determines the fusion duration through adaptive calculation of the fusion duration.
[0189] S604, the robot determines the fusion trajectory segment corresponding to the fusion duration through trajectory fusion processing based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion duration.
[0190] S605, the robot performs splicing and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the spliced and fused trajectory.
[0191] S606, the robot executes the stitched and merged trajectory until it reaches the end of the target trajectory segment.
[0192] S607, the robot resumes executing the remaining trajectory of the original trajectory.
[0193] For example, the terminal sends a command to resume or switch execution; pause, stop, and emergency stop requests are handled by their respective state control flows; the robot checks the execution conditions, reads the actual position, velocity, acceleration, and trajectory checkpoints, and determines the target trajectory segment and local time reference; it estimates the fusion duration of candidates and verifies that the target and reference trajectories are valid within the candidate window; it constructs a fusion reference matching the current state and a fusion interpolator with node state constraints, and verifies continuity and interval motion limits; if it fails, it adjusts and recalculates within the allowable range; it splices the verified fusion trajectory segment with the remaining part of the target trajectory segment, and checks the actual starting point state again before switching execution; it executes the fusion trajectory segment first, and then... The remaining part of the target trajectory segment is then executed; execution continues until the end of the target trajectory segment, and the execution progress is saved, depending on whether the end-point status is maintained, stopped under control, or continued from the verified subsequent task.
[0194] Applying the embodiments of this disclosure has at least the following beneficial effects: The target trajectory segment is determined based on the recovery or switching command. An executable trajectory is generated through current state matching, trajectory fusion with endpoint constraints, interpolator construction, and motion constraint verification. Under the condition that the reference trajectory meets the required smoothness and the boundary and constraint verifications pass, continuous connection of a set order can be achieved. The recovered scene continues playback along the saved original trajectory progress, and the switched scene continues the specified trajectory; there is no need to replay from the original trajectory starting point, which helps improve the efficiency of trajectory replay after interruption.
[0195] This disclosure also provides a trajectory replay device for use in a robot, and a schematic diagram of the trajectory replay device is shown below. Figure 7 As shown, the trajectory replay device 70 includes an acquisition module 701, a fusion duration adaptive calculation module 702, a trajectory fusion processing module 703, a splicing fusion processing module 704, and an execution module 705.
[0196] The acquisition module 701 is used to respond to the detection of an interruption command in the robot's trajectory replay based on the original trajectory, and to acquire relevant information of the original trajectory and relevant information of the target trajectory segment. The relevant information of the original trajectory includes the robot's current state information, and the relevant information of the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach. The fusion duration adaptive calculation module 702 is used to determine the fusion duration based on the current state information and relevant information of the target trajectory segment through adaptive calculation of the fusion duration; The trajectory fusion processing module 703 is used to determine the fusion trajectory segment corresponding to the fusion time based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion time through trajectory fusion processing. The stitching and fusion processing module 704 is used to perform stitching and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory. The execution module 705 is used to execute the spliced and fused trajectory by the robot until the endpoint is reached, so as to restore the remaining trajectory of the original trajectory.
[0197] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the starting position of each joint of the robot at the starting point of the target trajectory segment, the speed limit information of each joint, the minimum fusion time, and the speed limit safety factor. The adaptive fusion duration calculation module 702, based on the current state information and relevant information of the target trajectory segment, determines the fusion duration through adaptive fusion duration calculation, specifically for: Based on the current position of each joint, the starting position of each joint, the velocity limit information of each joint, the minimum fusion time, and the velocity limit safety factor, the fusion time is determined by adaptive calculation. The fusion time is used to characterize the duration of the fused trajectory segment.
[0198] In one embodiment, the speed limit safety factor ranges from 0.05 to 0.5, and the minimum fusion time ranges from 0.5 seconds to 3 seconds.
[0199] In one embodiment, when the trajectory fusion processing module 703 determines the fusion trajectory segment corresponding to the fusion duration based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion duration through trajectory fusion processing, it is specifically used for: Based on the fusion duration and the preset control cycle, sampling processing is performed within the fusion time window corresponding to the fusion duration to obtain multiple sampling time points; Determine the fusion coefficient for each sampling time point. The fusion coefficient is used to ensure at least one of the following: continuous joint position, continuous joint velocity, continuous joint acceleration, and continuous joint jerk. Based on relevant information from the original trajectory, the fusion mode is determined; Based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode. Based on the position information of each joint at each sampling time point, the interpolator for each joint is determined; Based on the interpolators of each joint, construct the fused trajectory segment and the interpolator of the fused trajectory segment.
[0200] In one embodiment, the fusion coefficient is an S-type fusion coefficient, which supports configuring the continuity order, including zero-order, first-order, second-order, and third-order. The zeroth order is used to ensure the continuity of joint positions, and the corresponding S-shaped fusion coefficient is a linear function. The first order is used to ensure the continuity of joint position and joint velocity, and the corresponding S-shaped fusion coefficient is a cubic polynomial; The second order is used to ensure the continuity of joint position, joint velocity, and joint acceleration, and the corresponding S-shaped fusion coefficient is a fifth-order polynomial; The third order is used to ensure the continuity of joint position, joint velocity, joint acceleration, and joint jerk. The corresponding S-shaped fusion coefficients are seventh-order polynomials.
[0201] In one embodiment, the relevant information of the original trajectory includes the interpolator of the original trajectory, and the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment. The trajectory fusion processing module 703, based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, obtains the position information of each joint at each sampling time point through the trajectory fusion algorithm corresponding to the fusion mode. Specifically, it is used for: If the fusion mode is trajectory-to-trajectory fusion mode, then a weighted fusion process is performed based on the fusion coefficient, the interpolator of the original trajectory, and the interpolator of the target trajectory segment to obtain the position information of each joint at each sampling time point; If the fusion mode is a point-to-trajectory fusion mode, then an interpolator for the transition trajectory is constructed based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment. Based on the fusion coefficient, the interpolator for the transition trajectory, and the interpolator for the target trajectory segment, a weighted fusion process is performed to obtain the position information of each joint at each sampling time point.
[0202] In one embodiment, the type of interpolator for each joint includes any one of cubic spline interpolator, quintic spline interpolator, piecewise cubic Hermite interpolator, B-spline interpolator, or interpolator constructed based on the minimum jerk criterion.
[0203] In one embodiment, the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; When the stitching and fusion processing module 704 performs stitching and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory, it is specifically used for: Based on the interpolator of the fused trajectory segment and the interpolator of the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; The interpolator for the fused trajectory segment corresponds to the first time period, and the interpolator for the target trajectory segment corresponds to the second time period. In the first time period, each time point is less than the fusion duration, and in the second time period, each time point is greater than or equal to the fusion duration and less than or equal to the end time point. The total duration of the target trajectory segment is equal to the end time point.
[0204] In one embodiment, the current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the end position of each joint of the robot at the end of the target trajectory segment. The trajectory fusion processing module 703, based on the fusion coefficient, relevant information of the original trajectory, and relevant information of the target trajectory segment, obtains the position information of each joint at each sampling time point through the trajectory fusion algorithm corresponding to the fusion mode. Specifically, it is used for: Based on the current position and the endpoint position of each joint, forward kinematics calculations are performed to obtain the pose corresponding to the current position and the pose corresponding to the endpoint position of each joint. The pose includes position components and attitude quaternion components. Based on the fusion coefficients, the poses corresponding to the current positions of each joint and the poses corresponding to the end positions of each joint, interpolation is performed to determine the position information of each joint at each sampling time point.
[0205] Applying the embodiments of this disclosure has at least the following beneficial effects: The robot is replaying its original trajectory when it receives an interrupt command. In response, the robot determines the fusion duration based on its current state information and the target trajectory segment's information, through adaptive calculation. Then, based on the original trajectory's information, the target trajectory segment's information, and the fusion duration, the robot performs trajectory fusion processing to determine the fused trajectory segment corresponding to the fusion duration. This fused trajectory segment prevents discontinuous jumps in the robot's joint angles, joint velocities, and joint accelerations, thus ensuring the robot's stability. The robot then performs stitching and fusion processing on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory. After the robot executes the stitched and fused trajectory until it reaches the endpoint, it resumes executing the remaining portion of the original trajectory, thus preserving the original trajectory's progress and remaining trajectory. This improves the robot's trajectory replay efficiency in interrupted scenarios.
[0206] This disclosure also provides an electronic device, the structural schematic diagram of which is shown below. Figure 8 As shown, Figure 8 The illustrated electronic device 4000 includes a processor 4001 and a memory 4003. The processor 4001 and the memory 4003 are connected, for example, via a bus 4002. Optionally, the electronic device 4000 may further include a transceiver 4004, which can be used for data interaction between the electronic device and other electronic devices, such as sending and / or receiving data. It should be noted that in practical applications, the transceiver 4004 is not limited to one type, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of this disclosure.
[0207] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with this disclosure. Processor 4001 may also be a combination that implements computational functions, such as including one or more microprocessor combinations, a combination of a DSP and a microprocessor, etc.
[0208] Bus 4002 may include a pathway for transmitting information between the aforementioned components. Bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus, etc. Bus 4002 can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 8 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.
[0209] The memory 4003 may be ROM (Read Only Memory) or other types of static storage devices capable of storing static information and instructions, RAM (Random Access Memory) or other types of dynamic storage devices capable of storing information and instructions, or EEPROM (Electrically Erasable Programmable Read Only Memory), CD-ROM (Compact Disc Read Only Memory) or other optical disc storage, optical disc storage (including compressed optical discs, laser discs, optical discs, digital universal optical discs, Blu-ray discs, etc.), magnetic disk storage media, other magnetic storage devices, or any other medium capable of carrying or storing computer programs and capable of being read by a computer, without limitation herein.
[0210] The memory 4003 is used to store computer programs that execute embodiments of the present disclosure, and is controlled by the processor 4001 to execute them. The processor 4001 is used to execute the computer programs stored in the memory 4003 to implement the steps shown in the foregoing method embodiments.
[0211] Electronic devices include, but are not limited to, robots.
[0212] Applying the embodiments of this disclosure has at least the following beneficial effects: The robot is replaying its original trajectory when it receives an interrupt command. In response, the robot determines the fusion duration based on its current state information and the target trajectory segment's information, through adaptive calculation. Then, based on the original trajectory's information, the target trajectory segment's information, and the fusion duration, the robot performs trajectory fusion processing to determine the fused trajectory segment corresponding to the fusion duration. This fused trajectory segment prevents discontinuous jumps in the robot's joint angles, joint velocities, and joint accelerations, thus ensuring the robot's stability. The robot then performs stitching and fusion processing on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory. After the robot executes the stitched and fused trajectory until it reaches the endpoint, it resumes executing the remaining portion of the original trajectory, thus preserving the original trajectory's progress and remaining trajectory. This improves the robot's trajectory replay efficiency in interrupted scenarios.
[0213] This disclosure provides a computer-readable storage medium storing a computer program, which, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.
[0214] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, can implement the steps and corresponding content of the aforementioned method embodiments.
[0215] It should be understood that although arrows indicate various operation steps in the flowcharts of the embodiments of this disclosure, the order in which these steps are implemented is not limited to the order indicated by the arrows. Unless explicitly stated herein, in some implementation scenarios of the embodiments of this disclosure, the implementation steps in each flowchart can be executed in other orders as required. Furthermore, some or all of the steps in each flowchart may include multiple sub-steps or multiple stages based on the actual implementation scenario. Some or all of these sub-steps or stages can be executed at the same time, and each sub-step or stage can also be executed at different times. In scenarios where execution times differ, the execution order of these sub-steps or stages can be flexibly configured as required, and the embodiments of this disclosure do not limit this.
[0216] The above embodiments are used to illustrate the technical solutions of this disclosure; any equivalent modifications made without departing from the technical concept of this disclosure shall have their protection scope determined in accordance with the claims.
Claims
1. A trajectory replay method, characterized in that, Performed by robots, including: In response to detecting an interruption command during trajectory replay based on the original trajectory, the robot obtains relevant information about the original trajectory and relevant information about the target trajectory segment. The relevant information about the original trajectory includes the robot's current state information, and the relevant information about the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach. Based on the current state information and the relevant information of the target trajectory segment, the fusion duration is determined through adaptive calculation of the fusion duration; Based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion duration, the fused trajectory segment corresponding to the fusion duration is determined through trajectory fusion processing; Based on the fused trajectory segment and the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; The robot executes the spliced and merged trajectory until it reaches the endpoint, thereby resuming the execution of the remaining trajectory of the original trajectory.
2. The method according to claim 1, characterized in that, The current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the starting position of each joint of the robot at the starting point of the target trajectory segment, the speed limit information of each joint, the minimum fusion time, and the speed limit safety factor. The determination of the fusion duration based on the current state information and relevant information of the target trajectory segment, through adaptive calculation of the fusion duration, includes: Based on the current position of each joint, the starting position of each joint, the speed limit information of each joint, the minimum fusion time, and the speed limit safety factor, the fusion time is determined by adaptive calculation, and the fusion time is used to characterize the duration of the fused trajectory segment.
3. The method according to claim 2, characterized in that, The speed limit safety factor ranges from 0.05 to 0.5, and the minimum fusion time ranges from 0.5 seconds to 3 seconds.
4. The method according to claim 1, characterized in that, The process of determining the fused trajectory segment corresponding to the fusion duration through trajectory fusion processing, based on relevant information of the original trajectory, relevant information of the target trajectory segment, and the fusion duration, includes: Based on the fusion duration and the preset control cycle, sampling processing is performed within the fusion time window corresponding to the fusion duration to obtain multiple sampling time points; Determine the fusion coefficient for each sampling time point, the fusion coefficient being used to ensure at least one of the following: continuous joint position, continuous joint velocity, continuous joint acceleration, and continuous joint jerk of the robot; Based on the relevant information of the original trajectory, the fusion mode is determined; Based on the fusion coefficient, the relevant information of the original trajectory, and the relevant information of the target trajectory segment, the position information of each joint at each sampling time point is obtained through the trajectory fusion algorithm corresponding to the fusion mode. Based on the position information of each joint at each sampling time point, the interpolator for each joint is determined; Based on the interpolators of each joint, a fused trajectory segment and an interpolator for the fused trajectory segment are constructed.
5. The method according to claim 4, characterized in that, The fusion coefficient is an S-type fusion coefficient, which supports configuration of continuity order, including zero order, first order, second order and third order; The zeroth order is used to ensure the continuity of joint positions, and the corresponding S-shaped fusion coefficient is a linear function; The first order is used to ensure the continuity of joint position and joint velocity, and the corresponding S-shaped fusion coefficient is a cubic polynomial; The second order is used to ensure the continuity of joint position, joint velocity, and joint acceleration, and the corresponding S-shaped fusion coefficient is a fifth-order polynomial; The third order is used to ensure the continuity of joint position, joint velocity, joint acceleration, and joint jerk, and the corresponding S-shaped fusion coefficients are seventh-order polynomials.
6. The method according to claim 4, characterized in that, The relevant information of the original trajectory includes the interpolator of the original trajectory, and the relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; Based on the fusion coefficient, the relevant information of the original trajectory, and the relevant information of the target trajectory segment, the trajectory fusion algorithm corresponding to the fusion mode is used to obtain the position information of each joint at each sampling time point, including: If the fusion mode is a trajectory-to-trajectory fusion mode, then a weighted fusion process is performed based on the fusion coefficient, the interpolator of the original trajectory, and the interpolator of the target trajectory segment to obtain the position information of each joint at each sampling time point; If the fusion mode is a point-to-trajectory fusion mode, then based on the fusion coefficient, the relevant information of the original trajectory and the relevant information of the target trajectory segment, an interpolator for the transition trajectory is constructed. Based on the fusion coefficient, the interpolator for the transition trajectory and the interpolator for the target trajectory segment, a weighted fusion process is performed to obtain the position information of each joint at each sampling time point.
7. The method according to claim 4, characterized in that, The interpolator types for each joint include any one of the following: cubic spline interpolator, quintic spline interpolator, piecewise cubic Hermite interpolator, B-spline interpolator, and interpolator constructed based on the minimum jerk criterion.
8. The method according to claim 4, characterized in that, The relevant information of the target trajectory segment includes the interpolator of the target trajectory segment; The step of performing splicing and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the spliced and fused trajectory includes: Based on the interpolator of the fused trajectory segment and the interpolator of the target trajectory segment, a splicing and fusion process is performed to determine the spliced and fused trajectory; Wherein, the interpolator of the fused trajectory segment corresponds to a first time period, the interpolator of the target trajectory segment corresponds to a second time period, each time point in the first time period is less than the fusion duration, each time point in the second time period is greater than or equal to the fusion duration and less than or equal to the time point of the endpoint, and the total duration of the target trajectory segment is equal to the time point of the endpoint.
9. The method according to claim 4, characterized in that, The current state information includes the current position of each joint of the robot, and the relevant information of the target trajectory segment includes the end position of each joint of the robot at the end of the target trajectory segment. Based on the fusion coefficient, the relevant information of the original trajectory, and the relevant information of the target trajectory segment, the trajectory fusion algorithm corresponding to the fusion mode is used to obtain the position information of each joint at each sampling time point, including: Based on the current position and the endpoint position of each joint, forward kinematics calculations are performed to obtain the pose corresponding to the current position and the pose corresponding to the endpoint position of each joint. The pose includes position components and attitude quaternion components. Based on the fusion coefficient, the pose corresponding to the current position of each joint, and the pose corresponding to the end position of each joint, interpolation is performed to determine the position information of each joint at each sampling time point.
10. A trajectory replay device, characterized in that, Applications in robots, including: The acquisition module is used to respond to the detection of an interruption command during the robot's trajectory replay based on the original trajectory, and to acquire relevant information of the original trajectory and relevant information of the target trajectory segment. The relevant information of the original trajectory includes the robot's current state information, and the relevant information of the target trajectory segment includes the endpoint of the target trajectory segment that the robot needs to reach. The fusion duration adaptive calculation module is used to determine the fusion duration based on the current state information and the relevant information of the target trajectory segment through adaptive fusion duration calculation; The trajectory fusion processing module is used to determine the fused trajectory segment corresponding to the fusion time through trajectory fusion processing based on the relevant information of the original trajectory, the relevant information of the target trajectory segment, and the fusion time. The stitching and fusion processing module is used to perform stitching and fusion processing based on the fused trajectory segment and the target trajectory segment to determine the stitched and fused trajectory; An execution module is used to execute the spliced and fused trajectory through the robot until the endpoint is reached, so as to restore the remaining trajectory of the original trajectory.
11. An electronic device comprising a memory, a processor, and a computer program stored in the memory, characterized in that, The processor executes the computer program to implement the steps of the method according to any one of claims 1-9.
12. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1-9.
13. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1-9.
