Robot trajectory optimization methods, systems, electronic devices and storage media

Abstract

This invention relates to the field of trajectory planning technology, and discloses a robot trajectory optimization method, system, electronic device, and storage medium. The method includes: obtaining initial planned trajectories for different joints of the robot based on preset robot actions; mapping different joints of the robot and preset environmental obstacles into parameterized geometric primitives; constructing a cost functional for optimizing the initial planned trajectory; iteratively updating the trajectory using gradient descent on the cost functional until a preset termination condition is met, and controlling the robot's movement based on the updated trajectory. This invention maps robot joints and environmental obstacles into parameterized geometric primitives, transforming the calculation of the shortest distance and gradient from the reference trajectory of each point on the robot in the current frame to all obstacles in the environment into calculating the collision distance between parameterized geometric primitives using analytical distance formulas. This reduces the computational load and thus improves the efficiency of robot motion optimization.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning technology, specifically to robot trajectory optimization methods, systems, electronic devices, and storage media. Background Technology

[0002] In the field of robot motion planning, to prevent collisions between the robot and obstacles or other parts of its body during movement, path planning is required for different joints of the robot. Each joint of the robot is equipped with motors to control its movements. To optimize these paths, the Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm is typically used. The specific principles of this algorithm include: An initial trajectory (usually a straight line interpolation in configuration space) is generated based on a preset start and end point. The trajectory is discretized into a multi-frame joint angle sequence. A cost functional is constructed, which includes a smoothing term (used to penalize the trajectory's acceleration or higher-order derivatives to ensure smooth motion dynamics) and an obstacle cost term. The obstacle cost term is usually calculated based on the workspace symbolic distance field (SDF) or dense body point sampling. The gradient of the cost functional is calculated, and a smoothness prior matrix is ​​introduced for covariant preconditioning. The trajectory is iteratively updated using gradient descent until the collision-free or convergence conditions are met.

[0003] In related technologies, in order to update the trajectory using the gradient descent method, it is necessary to calculate the shortest distance and gradient from the reference trajectory of each point on the robot to all obstacles in the environment in the current frame. This not only requires a lot of time to calculate but is also very difficult, resulting in low efficiency in robot motion optimization. Summary of the Invention

[0004] This invention provides a robot trajectory optimization method, system, electronic device, and storage medium to solve the problem of low efficiency in robot motion optimization.

[0005] In a first aspect, the present invention provides a robot trajectory optimization method, wherein the robot has at least two joints, and the method includes: The initial planned trajectory of different joints of the robot is obtained based on the preset robot motion, and the total frame rate and the joint angle vector corresponding to different joints of the robot in each frame are set based on the initial planned trajectory. Map the robot's different joints and preset environmental obstacles into parametric geometric primitives; Construct a cost functional for optimizing the initial planned trajectory. The cost functional includes at least the potential energy function of the trajectory smoothing term, the potential energy function of the trajectory attraction term, and the potential energy function of the collision cost term. Based on the potential field of all collision pairs and the determined collision cost term, a collision pair includes two parameterized geometric primitives, and at least one of them is a parameterized geometric primitive of the robot's joint mapping. If the collision distance between the collision pairs is less than zero, the potential field of the collision pairs is calculated based on the penetration depth cutoff and the preset gradient activation record. If the collision distance of the collision pair is greater than or equal to zero and less than the preset safety distance, the potential field of the collision pair is calculated based on the collision distance of the collision pair, the preset gradient activation radius, and the preset decay coefficient. If the collision distance of the collision pair is greater than the preset safety distance, the potential field of the collision pair is set to zero. The cost functional is calculated based on the trajectory smoothing term and its corresponding smoothing weight, the trajectory attraction term and its corresponding attraction weight, and the collision cost term and its corresponding collision weight. If the collision distance of at least one collision pair is less than zero, the attraction weight is set to the preset minimum value and the preset safety distance is set to the preset maximum value. The cost functional is iterated over, and the iteration operations include: The collision distance between parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. The gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term in the current frame are calculated based on the collision distance. The gradient of the cost functional is calculated based on the gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term in the current frame. The trajectory is updated using gradient descent until a preset termination condition is met, and the robot's movement is controlled based on the updated trajectory.

[0006] This application maps robot joints and environmental obstacles into parameterized geometric primitives. It transforms the calculation of the shortest distance and gradient from the reference trajectory of each part of the robot in the current frame to all obstacles in the environment into the calculation of the collision distance between parameterized geometric primitives through analytical distance formulas. This reduces the amount of computation and thus improves the efficiency of robot motion optimization. This application sets a three-segment piecewise function for the potential field of the collision cost term based on the relationship between the collision distance of the collision pair and the preset safety distance. This ensures that the collision cost term is differentiable throughout the trajectory optimization process. This avoids the problem of gradient breakage at the boundary in traditional methods, which causes the robot to be unable to optimize the path and thus get stuck. It also improves the stability of the robot's obstacle avoidance capability.

[0007] In one alternative implementation, constructing the cost functional for optimizing the initial planned trajectory includes: The static basic limits are obtained based on the relationship between the minimum collision distance of the initial planned trajectory in each frame and the preset safety distance. The dynamic environment boundary is obtained based on the relationship between the minimum collision distance of the updated trajectory in each frame and the preset safety distance; Frame-by-frame adaptive dynamic weights are set based on the minimum value between the static basic limit and the dynamic environment limit. The potential energy function of the trajectory attraction term is calculated based on the joint angle vector of the updated trajectory in the current frame, the joint angle vector of the initial planned trajectory in the current frame, and the frame-by-frame adaptive dynamic weights.

[0008] This application introduces frame-by-frame adaptive dynamic weights. During the calculation of the trajectory attraction term in each frame, the static baseline limit corresponding to the initial planned trajectory and the dynamic environment limit of the updated trajectory are calculated separately. The smaller value of the static baseline limit and the dynamic environment limit is taken as the frame-by-frame adaptive dynamic weight. This ensures that the new trajectory is safe, but when the initial planned trajectory is unsafe, the gravity term will not act with full intensity, which can lead to faster convergence, improve the computational efficiency of the cost functional, and ultimately improve the motion optimization efficiency of the robot.

[0009] In one alternative implementation, constructing the cost functional for optimizing the initial planned trajectory includes: This application directly locks the static safety weight to a preset minimum value when the initial planned trajectory is located in the collision zone. Consequently, the trajectory attraction term is forcibly locked to a low value, reducing the possibility of the robot colliding with the obstacle when the robot moves according to the updated trajectory and the initial planned trajectory attracts the robot to the obstacle.

[0010] In one alternative implementation, if all collision distances are greater than zero, the preset safety distance is set to a preset minimum value, and the attraction weight is set to a preset maximum value.

[0011] When no collision occurs, the attractiveness of the initial planned trajectory is increased, while the safe distance between the obstacle and the optimized trajectory is reduced, so that the optimized trajectory is as close as possible to the initial planned trajectory without collision.

[0012] In one alternative implementation, calculating the gradient of the collision cost term for the current frame based on the collision distance includes: The total frame rate is set based on the initial planned trajectory, and the gradient of the collision cost term for all frames is obtained based on the total frame rate. The collision cost term gradient time series of all frames is convolved based on a one-dimensional filter kernel to generate a diffused gradient component after temporal diffusion processing. Temporal diffusion processing is used to diffuse the collision gradient information of local time points to their adjacent time frames. The one-dimensional filter kernel is a linear smoothing filter kernel.

[0013] This application introduces a one-dimensional filter kernel convolution to perform temporal diffusion processing on the collision gradient components of each frame. This can diffuse the sharp gradients generated by local collision frames to adjacent frames on the time axis, preventing local breaks or numerical oscillations in the trajectory, and greatly improving the stability and executability of the robot's updated trajectory.

[0014] In one alternative implementation, controlling robot movement based on the updated trajectory includes: The updated trajectory is subjected to time parameterization to obtain the actual updated trajectory. The time parameterization includes at least assigning timestamps based on the updated trajectory and applying predetermined kinematic constraints based on the robot's dynamic performance. The predetermined kinematic constraints include joint velocity limits and joint acceleration limits. The robot's movement is controlled based on the actual updated trajectory.

[0015] This application performs time parameterization on the updated trajectory to add timestamps, joint velocity limits, and joint acceleration limits, ensuring that the updated trajectory is compatible with the motors controlling the robot.

[0016] Secondly, the present invention provides a robot trajectory optimization system, the system comprising: The initial trajectory planning module is used to obtain the initial planned trajectory of different joints of the robot based on the preset robot action, and to set the joint angle vector corresponding to different joints of the robot in each frame based on the initial planned trajectory. The geometric primitive mapping module is used to map different joints of the robot and preset environmental obstacles into parametric geometric primitives; The cost functional construction module is used to construct a cost functional for optimizing the initial planned trajectory, wherein the cost functional includes at least the potential energy function of the trajectory smoothing term, the potential energy function of the trajectory attraction term, and the potential energy function of the collision cost term. The cost functional iteration module is used to perform iterative operations on the cost functional, the iterative operations including: The collision distance between the parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. The gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame are calculated based on the collision distance. The gradient of the cost functional is calculated based on the gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame. The robot trajectory update module is used to update the trajectory using the gradient descent method until a preset termination condition is met, and then control the robot's movement based on the updated trajectory.

[0017] Thirdly, the present invention provides an electronic device, comprising: a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the computer instructions to perform the robot trajectory optimization method of the first aspect or any corresponding embodiment described above.

[0018] Fourthly, the present invention provides a computer-readable storage medium storing computer instructions for causing a computer to execute the robot trajectory optimization method of the first aspect or any corresponding embodiment thereof. Attached Figure Description

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

[0020] Figure 1 This is a schematic diagram of an application scenario according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the first process of the robot trajectory optimization method according to an embodiment of the present invention; Figure 3 This is a schematic diagram of a second process for a robot trajectory optimization method according to an embodiment of the present invention; Figure 4 This is a comparison diagram of the collision potential field of the robot trajectory optimization method according to an embodiment of the present invention; Figure 5 This is a comparison chart of the Gaussian filtering effect of the robot trajectory optimization method according to an embodiment of the present invention; Figure 6 This is a dynamic weighting effect diagram of the robot trajectory optimization method according to an embodiment of the present invention; Figure 7 This is a grouping strategy optimization diagram of the robot trajectory optimization method according to an embodiment of the present invention; Figure 8 This is a parameter hierarchy constraint diagram of the robot trajectory optimization method according to an embodiment of the present invention; Figure 9 This is a diagram showing the effect of comparing different weights in the robot trajectory optimization method according to an embodiment of the present invention; Figure 10 This is a schematic diagram of the parameterized geometric primitives of the robot trajectory optimization method according to an embodiment of the present invention; Figure 11 This is a structural block diagram of a robot trajectory optimization system according to an embodiment of the present invention; Figure 12 This is a schematic diagram of the hardware structure of an electronic device according to an embodiment of the present invention. Detailed Implementation

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

[0022] It is understood that before using the technical solutions disclosed in the various embodiments of the present invention, users should be informed of the types, scope of use, and usage scenarios of the personal information involved in the present invention and their authorization should be obtained in accordance with relevant laws and regulations through appropriate means.

[0023] In related technologies, the Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm relies heavily on discrete signed distance fields or dense geometric point sampling to approximate the distance between the robot and obstacles when calculating the gradient of obstacle terms. This discretization not only requires massive high-dimensional spatial distance queries, resulting in huge computational and storage overhead, but also easily leads to abrupt changes in the distance gradient at spatial boundaries. In addition, traditional obstacle potential field functions often suffer from non-differentiability or abrupt repulsive force changes at safety threshold boundaries, which can easily form a "potential trap" during the optimization process, causing the gradient descent to get stuck in local minima or triggering trajectory numerical oscillations, severely restricting the robot's motion optimization efficiency and obstacle avoidance stability.

[0024] As an optional application scenario of this invention, such as Figure 1 As shown, the computer trajectory optimization system may include at least one terminal device and at least one server. Figure 1 The system is illustrated in the example, which includes a computer 101, a mobile terminal 102, and a server 103, and the terminal devices such as the computer 101 and the mobile terminal 102 are connected to the server 103 through a network 110.

[0025] Specifically, the terminal device can be a multi-degree-of-freedom robotic arm, a humanoid robot, a multi-joint robot, or other multi-joint motion systems. Server 103 can be a standalone physical server, a server cluster, a distributed system, or a cloud server providing cloud services. Network 110 can be a wired or wireless network, examples of which include, but are not limited to, the Internet, corporate intranets, local area networks, wide area networks, mobile communication networks, and combinations thereof.

[0026] This invention provides a robot trajectory optimization method. By mapping robot joints and environmental obstacles to parametric geometric primitives, the method transforms the calculation of the shortest distance and gradient from the reference trajectory of each part of the robot in the current frame to all obstacles in the environment into calculating the collision distance between parametric geometric primitives through analytical distance formulas. This reduces the amount of computation and thus improves the efficiency of robot motion optimization.

[0027] According to an embodiment of the present invention, a robot trajectory optimization method embodiment is provided. It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions. Furthermore, although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in a different order than that shown here.

[0028] This embodiment provides a robot trajectory optimization method, which can be applied to robot systems with at least two joints, such as multi-degree-of-freedom robotic arms, humanoid robots, and multi-joint robots. This embodiment uses a dual-arm (14 degrees of freedom) humanoid robot as an example. Each joint of this robot is driven by an independent motor and encoder. Figure 2 This is a flowchart of a robot trajectory optimization method according to an embodiment of the present invention, such as... Figure 2 and Figure 8 As shown, Figure 8 This is used to display the hierarchical relationships and constraint inequalities of parameters at each stage using number lines and piecewise annotations, ensuring that gradients do not vanish at safe boundaries during optimization. The process includes the following steps: Step S201: Obtain the initial planned trajectory of different joints of the robot based on the preset robot action, and set the joint angle vector corresponding to different joints of the robot in each frame based on the initial planned trajectory.

[0029] The preset robot actions are specific movements that the robot is expected to imitate or perform. These actions can be data from live performances captured by a motion capture system, computer animation (CG) sequences designed by professional animators, or rough path points generated by an upper-level task planner (such as grasping or assembly task planning). In this embodiment, the preset robot actions are a sequence of gestures extracted from a live video and mapped onto the robot body using GVHMR technology.

[0030] The initial planned trajectory is a raw motion path that may contain collisions or be dynamically infeasible. It can be a path randomly generated from a user-preset start and end point, typically consisting of a series of robot joint angle vectors arranged in chronological order. In this embodiment, the initial planned trajectory is a sequence of joint angles obtained by remapping video motion to the robot's configuration space using GMR technology. Because the motion remapping process does not consider robot self-collision, the initial planned trajectory suffers from issues such as arm clipping (e.g., the left forearm penetrating the right upper arm) and interference from its own links.

[0031] The joint angle vector is a set of values ​​used to describe the overall configuration of the robot at a certain moment (i.e., one frame). For a robot with D degrees of freedom, its joint angle vector is a D-dimensional vector. For example, the dual-arm humanoid robot in this embodiment has 14 degrees of freedom, therefore the joint angle vector for each frame is... , represents the angle values ​​(in radians or degrees) of the robot's 14 joints at frame k.

[0032] In this application, the processor reads an action file containing 458 frames of data from local storage, where each row corresponds to one frame and each column corresponds to the encoder reading of a joint's motor. After conversion, a joint angle sequence is obtained. The acquired initial planned trajectory is preprocessed to ensure that the preprocessed initial planned trajectory meets the standard data structure that can be directly used by subsequent optimization algorithms. To accelerate the optimization calculation, this embodiment first downsamples the original 458-frame trajectory into a sparse trajectory of 100 frames, with each frame corresponding to a 14-dimensional joint angle vector.

[0033] Step S202: Map the robot's different joints and preset environmental obstacles into parametric geometric primitives.

[0034] Parametric geometric primitives are simple 3D geometries that are precisely described using a finite set of geometric parameters (such as the coordinates and radius of a sphere's center, and the endpoints and radius of a cylinder's axis). Commonly used primitives include spheres and capsules. Compared to traditional triangular mesh models, parametric primitives have the advantages of faster computation speed and differentiable distance analytical expressions.

[0035] In this embodiment, a capsule is preferably used as the basic primitive because it can better fit the slender shape of the robot link, and its surface distance calculation has a closed analytical solution.

[0036] The preset environmental obstacles can be other objects in the robot's workspace besides itself, such as workpieces on the workbench, walls, other collaborative robots, etc. In the self-collision scenario of the dual-arm robot in this embodiment, environmental obstacles mainly refer to other linkages of the robot itself. However, this method is also applicable to the external environment, such as mapping the desktop as an infinitely large planar primitive or a half-space capsule.

[0037] In this embodiment, as Figure 10 As shown, Figure 10 The left image shows the robot capsule and its main body. Figure 10 The right image shows the visualization of the capsule body; this is achieved by establishing the correspondence between the link geometry and parametric primitives in the robot's kinematic model. Specifically, for the dual-armed humanoid robot in this embodiment, the "upper arm" link of its left arm can be mapped to a capsule body: the endpoints of the capsule body's axis are located at the shoulder and elbow joints, respectively, and its radius is set to be slightly larger than the maximum envelope radius of the upper arm link (e.g., 5 cm). The entire left arm is mapped to 16 capsule bodies, the right arm to 16, and the torso and legs to 9. In this way, the originally complex self-collision detection problem is simplified to calculating the shortest surface distance between these 41 capsule bodies (a total of 79 effective collision pairs).

[0038] Step S203: Construct a cost functional for optimizing the initial planned trajectory.

[0039] The cost functional is a function that takes a complete trajectory (i.e., a function) as input and outputs a scalar value that quantifies the "goodness" of the trajectory. A smaller value indicates a better trajectory. The optimization objective of this application is to iteratively find a trajectory that minimizes the cost functional.

[0040] In this application, the robot trajectory is set as a discrete sequence in the configuration space. ,in For the first The joint angle vector of the frame, The total frame rate is set based on the initial planned trajectory. The optimization objective of this invention is to minimize the trajectory cost functional: ; in, This is a trajectory smoothing term used to penalize trajectory jitter or unevenness, encouraging the generation of a smooth-moving trajectory. This is a trajectory attraction term used to guide the optimized trajectory so that it does not deviate too far from the initial planned trajectory (taught trajectory), thus preserving the intent of the original action. This is a collision cost term used to penalize the robot for approaching or penetrating obstacles (including self-collisions), driving the robot away from the collision area. The weighting coefficients for the preset trajectory smoothing term. The weighting coefficients for the preset trajectory attraction terms. As the weighting coefficients of the preset collision cost term, this application constructs a smooth potential field that is continuously differentiable throughout the entire process based on the analytical capsule distance.

[0041] Step S204: Perform an iterative operation on the cost functional.

[0042] Specifically, the iterative operation includes: The collision distance between parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. Wherein, the i-th capsule body is defined as being from the endpoint of the axis. and radius Sure.

[0043] For any two capsules i and j, the shortest distance between their surfaces is... Calculated using analytical geometry: ; in, To find the shortest center-to-center distance between the i-th and j-th capsules, the Clamped Parametric Segment-to-Segment algorithm can be used to calculate the shortest center-to-center distance between the axes. Let be the radius of the i-th capsule. Let be the radius of the j-th capsule. The Clamped Parametric Segment-to-Segment algorithm ensures that the calculated shortest distance is the true closest distance between two finite line segments, rather than the distance between infinite straight lines, by limiting the parameters to the range of line segments. In this embodiment, the axis of the capsule is precisely such a finite line segment. Therefore, the algorithm can accurately and differentiably calculate the shortest center distance between the two capsules, and thus obtain the surface distance.

[0044] Calculate the gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term for the current frame based on the collision distance, and then calculate the gradient of the cost functional based on the gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term for the current frame.

[0045] Step S205: Update the trajectory using gradient descent until the preset termination condition is met, and control the robot to move based on the updated trajectory.

[0046] The preset termination condition can be one of the following or a combination thereof: the minimum collision distance of all frames in the trajectory is greater than a preset safety threshold; the number of iterations reaches a preset upper limit (e.g., 50 iterations in the coarse-push stage and 300 iterations in the fine-push stage). In multiple consecutive iterations, the decrease in the cost functional is less than a preset minimum threshold.

[0047] The optimized and dynamically feasible final trajectory is converted into a sequence of instructions executable by the underlying motors and sent to the robot controller. For example, the final trajectory includes a timestamp, target angle, target velocity, and target acceleration for each joint. Based on these instructions, the robot's servo drives control the motors in real time, driving the robot's arm to perform a gesture that is collision-free and retains the original movement style.

[0048] This embodiment maps robot links and environmental obstacles to parameterized geometric primitives and uses analytical distance formulas to calculate collision distances. This avoids the problems of discrete sampling and gradient jumps in the traditional Signed Distance Field (SDF) mesh method, significantly improving computational efficiency and optimization stability, thereby solving the technical problem of low efficiency in robot motion optimization.

[0049] This embodiment refines a preferred implementation of the "constructing the cost functional" step S203 based on the above embodiments, particularly focusing on a frame-by-frame adaptive dynamic weight construction method for the trajectory attraction term. Please refer to [link to relevant documentation]. Figure 3 The method includes the following steps in step S203: Step S301: Obtain the static basic boundary based on the relationship between the minimum collision distance of the initial planned trajectory in each frame and the preset safety distance.

[0050] The static baseline limit is calculated based on the minimum collision distance of each frame of the initial reference trajectory using the following formula: ; in, For Hermite smoothing function, The preset minimum weight threshold is 0.15 in this embodiment. Let k be the static base bound of the k-th frame. Let be the minimum collision distance in the k-th frame. The preset safety distance, which is the activation radius of the collision potential field, is also a preset distance threshold for determining whether a trajectory point is safe.

[0051] Specifically, the static baseline is a pre-computed, time-frame-dependent weight limit determined by the safety of the initially planned trajectory itself. The static baseline is calculated once before optimization begins and remains constant throughout the optimization process; its function is to pre-lock the gravitational intensity of unsafe regions in the reference trajectory, preventing the optimized, safe trajectory from re-entering a collision.

[0052] Step S302: Obtain the dynamic environment boundary based on the relationship between the minimum collision distance of the updated trajectory in each frame and the preset safety distance.

[0053] The dynamic environment boundary is calculated based on the real-time collision distance of each frame of the current optimized trajectory, using the following formula: ; in, For Hermite smoothing function, The preset minimum weight threshold is 0.15 in this embodiment. Let k be the dynamic environment boundary of the k-th frame. Let be the real-time collision distance in the k-th frame. The preset safety distance, which is the activation radius of the collision potential field, is also a preset distance threshold for determining whether a trajectory point is safe.

[0054] Specifically, the dynamic environment limit is a weight cap that is tied to the current optimization process in real time. In each iteration, the dynamic environment limit is calculated based on the real-time collision distances of each frame of the current optimization trajectory. The purpose of the dynamic environment limit is to immediately reduce the gravity weight when an intermediate trajectory point unexpectedly approaches an obstacle during the optimization process, thereby reducing the possibility of collisions still occurring during trajectory updates.

[0055] Step S303: Set frame-by-frame adaptive dynamic weights based on the minimum value between the static basic limit and the dynamic environment limit.

[0056] Among them, such as Figure 6 and Figure 9 As shown, Figure 6 The left figure shows the adaptive function of the reference weight for each frame as a function of distance. Figure 6 The right figure shows the dual constraints of dynamic weights and static upper bounds, which prevent the trajectory from being attracted into the collision zone of the reference trajectory, thus improving safety and convergence. Figure 9 The top image shows the collision distance per frame, and the bottom image shows the distribution of static weights, dynamic weights, and the final weights, visually demonstrating the role of the dual constraint mechanism in actual trajectory optimization. The formula for the adaptive dynamic weights is as follows: ; in, The adaptive dynamic weights for the k-th frame are used to calculate the weights of the trajectory attraction term. Let k be the static base bound of the k-th frame. Let k be the dynamic environment boundary of the k-th frame. This is for calculating the minimum value.

[0057] Specifically, by using a truncation threshold operation, which combines the preventative constraints of static base limits with the reactive constraints of dynamic environmental limits, and by setting adaptive dynamic weights, the current trajectory is made safe. However, since the initial planned trajectory itself has collisions, the frame-by-frame adaptive dynamic weights are minimized, which makes the final trajectory attraction term also smaller, reducing the attraction of the initial planned trajectory to the updated trajectory.

[0058] Based on the above embodiments, this embodiment refines another preferred implementation of the "construction of cost functional" in step S203, particularly the method for constructing a piecewise smooth potential field for the collision cost term.

[0059] Based on the potential field of all collision pairs and the determined collision cost term, a collision pair includes two parameterized geometric primitives, at least one of which is a parameterized geometric primitive of the robot's joint mapping.

[0060] A collision pair is a combination of two parametric geometric primitives (such as capsules) that may collide. For example, a capsule in the left arm and a capsule in the right arm constitute a collision pair. By pre-establishing a collision pair grouping matrix (e.g., group A: mutual collision between arms, group B: left arm-torso), unnecessary computations can be significantly reduced. Figure 7 As shown, Figure 7 The left figure shows the number of collision pairs in different groups in the statistical practice example. Figure 7 The right figure illustrates the gradient allocation and dimensionality reduction optimization process after grouping, demonstrating the efficiency improvement of the grouping strategy for high-dimensional optimization.

[0061] A smooth potential field is constructed based on the analytical capsule distance. For a single-frame configuration, the collision cost is defined as the sum of the potential fields of all collision pairs, which can be specifically expressed as: ; in, The distance between the capsule surfaces. Configured for a single frame. For those belonging to the preset collision pair The i-th capsule and the j-th capsule, The potential field sum of the collision cost term, The potential field function is a piecewise smoothed potential field function; It is worth mentioning that the piecewise smooth potential field function is specifically as follows: ; in, Let be a piecewise smooth potential field function. The preset attenuation coefficient for the nearest neighbor region. This is the cutoff value for penetration depth. The distance between the capsule surfaces. The maximum penetration distance is preset. By locking the maximum penetration distance, the potential field of the collision pair will not increase indefinitely as the penetration distance increases. This is a preset safe distance.

[0062] Specifically, if the collision distance is less than zero (the penetration zone), it indicates that the two parametric geometric primitives have penetrated each other. In this case, the potential field function is: , This region, where the gradient is constant and the maximum thrust is used to limit the maximum penetration depth, separates the two. If the collision distance is greater than or equal to zero and less than the preset safety distance (nearest neighbor zone), it means that two parameterized geometric primitives are close but have not yet made contact. At this time, the potential field function is... This invention reveals for the first time that this function is equivalent to a cubic potential field. Since the potential field is a force when differentiated with respect to time, by analyzing its mechanical properties at d=0, it is found that... The value of determines whether a "potential well" exists. For example, in the warmup phase, setting At this point, the repulsive force at d=0 deflects upwards, eliminating the potential well, and the optimizer can successfully push the trajectory away from the surface.

[0063] It is worth mentioning that, such as Figure 4 As shown, the repulsive force (the negative derivative of the cost with respect to distance) is analyzed: ; At point d=0 on the object's surface, the repulsive force on the through-mold side is 2r, and the repulsive force on the nearest neighbor side is 3r. r, the difference between the two determines the mechanical behavior of the optimizer at that point: when (Typical value for simplification in traditional engineering, such as 0.3): ΔF<0, the repulsive force steps downward at d=0, which is equivalent to the existence of a potential well on the outside of the object surface - once the optimizer approaches the surface, it is attracted and stagnated, and can only escape with the help of random perturbation. when (Values ​​for the Warmup stage in this scheme): ΔF=r>0, the repulsive force turns upward at d=0, completely eliminating the potential well trap. The optimizer feels a stronger repulsive force than the mold penetration area on the outer side of the surface and is actively pushed away from the danger zone. when ΔF=0, the force is strictly continuous at d=0, suitable for scenarios with extremely high requirements for numerical stability; reference Figure 4, Figure 4 The left figure compares the energy curves of a traditional quadratic potential field and the smoothed cubic potential field of this invention. Figure 4 The right figure shows the corresponding repulsive force curve; by using third-order potential energy, the potential well problem at d=0 is solved.

[0064] If the collision distance is greater than the preset safe distance (safe zone), it means that the distance between the two parameterized geometric primitives is far enough that collision avoidance is not required. At this time, the potential field value is zero and no collision gradient is generated.

[0065] The cost functional is calculated based on the trajectory smoothing term and its corresponding smoothing weight, the trajectory attraction term and its corresponding attraction weight, and the collision cost term and its corresponding collision weight. If the collision distance of at least one collision pair is less than zero, the attraction weight is set to a preset minimum value, and the preset safety distance is set to a preset maximum value. If all collision distances are greater than zero, the preset safety distance is set to a preset minimum value, and the attraction weight is set to a preset maximum value.

[0066] If the collision distance of at least one collision pair is less than zero, meaning the current trajectory is in a deep clipping state, the optimizer will automatically adjust the weight coefficients of the preset trajectory attraction term. Set it to a preset minimum value (e.g., 5.0, much smaller than the fine-grained stage's 30.0), and set a preset safety distance. Set it to a large value (e.g., 4.0 cm). This setting amplifies the range of the collision gradient, allowing the robot to break free forcefully, with almost no consideration for deviation from the reference trajectory. If all collision distances are greater than zero, meaning the current trajectory is essentially safe, the optimizer will adjust the preset safety distance. Set it to a small value (e.g., 1.5cm), and adjust the weighting coefficient of the preset trajectory attraction term. Set it to a large value (e.g., 30.0). With this setting, the robot only needs to avoid very close obstacles while staying as close as possible to the preset initial trajectory under the influence of strong gravity.

[0067] Based on the above embodiments, this embodiment refines a preferred implementation of the step S204 of "calculating the gradient of the collision cost term in the current frame".

[0068] The gradient of the collision cost term for all frames is obtained based on the total frame rate.

[0069] The total frame rate is the total number of discrete control points set based on the initial planned trajectory.

[0070] It's worth noting that the collision cost gradient for each frame of the current trajectory is calculated. This collision cost gradient sequence can be very "sharp," meaning only a few frames where collisions occur have large gradient values, while adjacent, safe frames have zero gradients. Directly using this gradient update can cause the trajectory to "break" or exhibit severe local jitter on the timeline.

[0071] A one-dimensional filter kernel is used to convolve the time series of the gradients of the collision cost terms of all frames to generate the diffusion gradient components after temporal diffusion processing.

[0072] The one-dimensional filter kernel is a weighted window that moves along the time axis (frame number). For example... Figure 5 As shown, this application preferably uses a one-dimensional Gaussian kernel, whose weights are distributed in a bell-shaped curve, with the largest weight in the center frame and the weights of adjacent frames decreasing exponentially with increasing distance. Applying a Gaussian filter to the original gradient sequence yields the diffused gradient, mathematically expressed as: ; in, Let be the original gradient sequence of the k-th frame, and N be the total frame rate. This operation is equivalent to performing the gradient on the trajectory space. Preconditioning ( (This is the trajectory smoothness prior matrix), which can diffuse the sharp gradients generated by local collision frames to adjacent frames on the time axis, preventing local breaks or numerical oscillations in the trajectory, and greatly improving the stability of optimization convergence and the physical executability of the trajectory. Figure 5 The left image shows the smoothing effect of Gaussian kernel diffusion on the collision gradient. Figure 5 The right figure in the image compares the smoothness of the trajectory with and without Gaussian preconditioning.

[0073] Specifically, assuming a collision occurs in frame 50, its original gradient =100, while the gradients in frames 49 and 51 are 0. (Using standard deviation) After convolution with a Gaussian kernel of 2.0, , , Each of these will result in a non-zero gradient value (e.g., 20, 100, 20 respectively). This will also "push" frames 49 and 51, causing the collision avoidance maneuver of the trajectory to form a smooth arc on the timeline instead of a sharp "peak." This significantly improves the physical smoothness and executability of the optimized trajectory.

[0074] It is worth mentioning that the original paper on the CHOMP method in related technologies uses a trajectory smoothness prior matrix. The reverse Global preconditioning of gradients is used, but this method incurs significant computational and storage overhead for high-dimensional, long trajectories, making practical engineering implementation complex. This invention innovatively employs a one-dimensional Gaussian kernel to temporally diffuse the collision gradient, as... The efficient approximation not only significantly reduces computational complexity but also allows for flexible adjustment of the diffusion range, thereby significantly improving the engineering practicality and convergence stability of multi-degree-of-freedom robot trajectory optimization.

[0075] Based on the above embodiments, this embodiment refines a preferred implementation of "controlling robot movement based on the updated trajectory" in step S205, especially focusing on the dynamic feasibility.

[0076] The updated trajectory is processed by time parameterization to obtain the actual updated trajectory, and the robot movement is controlled based on the actual updated trajectory.

[0077] The time parameterization process involves creating a trajectory that contains only geometric path points (joint angle sequences), assigning time information to it, and planning the velocity and acceleration curves from the starting point to the ending point. Its inputs are the geometric path and joint physical limits, and its output is a timestamped trajectory.

[0078] The predetermined kinematic constraints are physical limitations determined by the robot's hardware performance, including: joint velocity limits, i.e., the maximum rotational speed (radians / second) that each joint motor can withstand; and joint acceleration limits, i.e., the maximum rotational acceleration (radians / second²) that each joint motor can withstand.

[0079] In this embodiment, by adding a dynamic feasibility sealing step, this application ensures that the optimized trajectory is not only geometrically collision-free and smooth, but also physically executable, avoiding the risk of trajectory tracking failure or hardware damage due to exceeding motor performance.

[0080] It is worth mentioning that using GVHMR technology to generate robot motion from video motion and then remapping it to the robot body using GMR technology can solve the shape mismatch problem. However, during the motion execution process, self-collision and clipping by the robot body are inevitable. For the scenario of reproducing video motion with a dual-arm robot that requires no self-collision, the operation objects include multi-degree-of-freedom joints (applicable to various robotic arms such as dual-arm humanoids and six-axis robotic arms; this case is a dual 7-degree-of-freedom robotic arm humanoid robot). Each joint is equipped with a motor and encoder for trajectory optimization.

[0081] Obtain the original reference trajectory (Include Frame), downsampled to a sparse trajectory T0 (containing Frames (e.g., 100 frames) are used to accelerate optimization calculations. After optimization, the original frame number is restored using cubic spline interpolation.

[0082] Instead of the traditional mesh-based SDF, the robot links are mapped as a finite set of capsules. Each capsule is defined by its axis endpoints. and radius By definition, for a certain dual-arm humanoid robot, the left arm is mapped to 16 capsules, the right arm is mapped to 16 capsules, and the torso and legs are mapped to 9 capsules. The number and distribution of capsules can be configured according to the specific robot configuration and are applicable to any multi-joint robot and environmental obstacles.

[0083] To improve computational efficiency, collision detection pairs are divided into several groups, and only the gradients of the relevant joints are calculated.

[0084] Group A (mutual contact of both arms): Left arm capsule assembly × Right arm capsule assembly; Group B (Left Arm - Trunk): Capsule body of left forearm and hand × capsule body of trunk; Group C (Right Arm - Trunk): Capsule body of right forearm and hand × capsule body of trunk; Group D (Environmental Collision): Robot Capsule × Environmental Obstacle Capsule (optional); Gradient dimensionality reduction: When calculating the gradient, for collisions in group A, only the joints of the corresponding arms are perturbed; for groups B / C, only the arm joints are perturbed, forcibly skipping the Jacobian calculation of unrelated trunk and leg joints, reducing the computational load of high-dimensional redundant systems.

[0085] The shortest centerline distance between any two capsule axes is calculated using the Clamped Parametric Segment-to-Segment algorithm. The formula for calculating surface distance is: ; in Let be the radii of the two capsules. This formula is continuously differentiable throughout. (Continuous), eliminating gradient jumps caused by the SDF grid.

[0086] Construct the collision cost function Uobs. To avoid the moment step at d=0 in the traditional quadratic potential field, this invention adopts a piecewise smooth potential field: When d < 0 (through-mold region): the potential field slope remains at a constant maximum value, ensuring a strong push-away; When 0≤d<ρ (nearest neighbor region): the potential field gradient decays linearly, achieving a seamless bridge from the mold-piercing region to the safe region; When d ≥ ρ: the potential field is zero; Furthermore, a one-dimensional Gaussian filter is applied to the solved repulsive gradient components to simulate the covariance matrix inversion operation (A... (1 precondition) smoothly diffuses the local collision force to adjacent frames, suppressing trajectory knotting.

[0087] This invention decouples the optimization process into two stages, thus solving the parameter deadlock problem.

[0088] Phase 1: Coarse-scale optimization (Warmup) Objective: Escape by force, regardless of whether the trajectory closely follows the reference. Parameter configuration: Repulsion activation radius Larger diameter (e.g., 4.0cm) ensures that the repulsive force can be felt even when deeply penetrating an object; Reference Gravitational Weights Smaller values ​​(e.g., 5.0) allow for significant deviations from the reference path; Safety boundary Relatively lenient; Execution: Iterative optimization is performed based on the initial trajectory until the trajectory is collision-free or the maximum number of iterations is reached.

[0089] Phase Two: Refinement Objective: To closely approximate the original teaching movements while ensuring safety, and to guarantee smoothness; Parameter configuration: Repulsion activation radius Smaller (e.g., 1.5cm) to avoid excessive avoidance that could lead to distorted movements; Reference Gravitational Weights Larger values ​​(e.g., 30.0) indicate strong constraints that are close to the reference value. Safety boundary Strict (e.g., 0.5cm); Execution: Optimize using the first-stage output as the initial value and the original reference trajectory as the target.

[0090] To address the problem of reference trajectory pull-and-collision, this invention introduces frame-by-frame adaptive weights into the reference gravity term of the objective function.

[0091] Static base limits ( ): The minimum collision distance is pre-calculated based on each frame of the initial reference trajectory. If a frame of the reference trajectory is in the collision zone, the upper limit of the weight for that frame is locked at a low value (e.g., 0.15); if a frame of the reference trajectory is safe, the upper limit of the weight is 1.0; the transition zone uses cubic Hermite smoothing to ensure the continuity of the derivative. To prevent an already erroneous reference trajectory from being pulled back to the collision zone with full force in the later stages of optimization.

[0092] Dynamic environmental boundaries ( ): The collision distance is calculated based on the real-time collision distance of each frame of the current optimized trajectory. If a frame of the current trajectory is close to a collision, the reference gravity weight of that frame is reduced and replaced by repulsive force.

[0093] Truncating threshold (final weight): Each control frame final tensile coefficient The calculation is as follows: ; This formula ensures that any high-risk gravitational forces arising from a disordered prior reference trajectory are locked by a static boundary. Even if the current trajectory is safe, if the reference trajectory itself is not safe, gravity will not act at its full strength, thus mathematically preventing the extreme cycle oscillation of "pulling-collision-pushing away".

[0094] The optimized sparse trajectory is restored to the original frame number through cubic spline interpolation, and the collision distance is checked point by point to prevent new collision points from being generated during the interpolation process.

[0095] Import nonlinear parametric programming constraints (such as the TOPPRA algorithm). Limit the upper limit of joint velocities. Upper limit of joint acceleration Generates the final execution trajectory containing timestamps, velocity, and acceleration information; resolves the safety hazard of in-situ pose optimization not being able to directly interface with the lower-level machine motor, and ensures that the trajectory conforms to rigid body dynamics constraints.

[0096] To verify the effectiveness and engineering applicability of the method of this invention, a systematic test was conducted on a dual-arm (14-DOF) humanoid robot platform (fixed base). The test scenario was collision-free optimization of complex gesture trajectories, with the initial trajectory exhibiting depth clipping collisions. The optimization results of the reference trajectory are shown in Table 1:

[0097] Deep collision escape capability: The minimum surface distance of the initial trajectory is -8.82cm (geometric penetration), which is rapidly increased to 3.06cm after 50 iterations in the coarse push stage. The number of collision frames dropped from 125 to 0, which verifies the effective escape capability of the "large repulsion radius + weak reference gravity" parameter strategy in the two-stage architecture for deep collisions.

[0098] Dynamic weight truncation eliminates collisions: In the fine-tuning stage, the reference trajectory weight λ ref Under the strong attraction condition of 30.0, the minimum trajectory distance was stably maintained at 1.78cm > 1.0cm (safety threshold), and the "pull-collision" oscillation phenomenon common in traditional algorithms was not observed, proving that... The effectiveness of the dynamic weighting mechanism.

[0099] Engineering feasibility assurance: After TOPPRA time parameterization, the output trajectory fully meets the physical constraints of joint velocity / acceleration, and the safety check of the entire frame after interpolation passes, so it can be directly used for robot controller execution.

[0100] Computational efficiency is scalable: In this embodiment, the total time for 458 frames × 79 pairs of collision detection × 350 iterations is 399.65s. After linear scaling estimation, the time for conventional 100-frame trajectory optimization is approximately 87.2s, which meets the requirements for offline planning. With GPU parallel computing acceleration, the time is reduced to 1 / 5 of the original, i.e., 17.44s.

[0101] This embodiment also provides a robot trajectory optimization system for implementing the above embodiments and preferred embodiments; details already described will not be repeated. As used below, the term "module" can refer to a combination of software and / or hardware that performs a predetermined function. Although the systems described in the following embodiments are preferably implemented in software, hardware implementations, or a combination of software and hardware, are also possible and contemplated.

[0102] This embodiment provides a robot trajectory optimization system, such as Figure 11 As shown, it includes: The initial trajectory planning module 1101 is used to obtain the initial planned trajectory of different joints of the robot based on the preset robot action, and to set the joint angle vector corresponding to different joints of the robot in each frame based on the initial planned trajectory. The geometric primitive mapping module 1102 is used to map different joints of the robot and preset environmental obstacles into parametric geometric primitives. The cost functional construction module 1103 is used to construct a cost functional for optimizing the initial planned trajectory. The cost functional includes at least the potential energy function of the trajectory smoothing term, the potential energy function of the trajectory attraction term, and the potential energy function of the collision cost term. Cost functional iteration module 1104 is used to perform iterative operations on the cost functional. The iterative operations include: The collision distance between parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. The gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term in the current frame are calculated based on the collision distance. The gradient of the cost functional is calculated based on the gradients of the trajectory smoothing term, collision cost term, and trajectory attraction term in the current frame. The robot trajectory update module 1105 is used to update the trajectory using the gradient descent method until a preset termination condition is met, and to control the robot's movement based on the updated trajectory.

[0103] In some alternative implementations, the cost functional construction module 1103 includes: The static basic boundary is obtained based on the relationship between the minimum collision distance of the initial planned trajectory in each frame and the preset safety distance; the dynamic environment boundary is obtained based on the relationship between the minimum collision distance of the updated trajectory in each frame and the preset safety distance; the frame-by-frame adaptive dynamic weight is set based on the minimum value of the static basic boundary and the dynamic environment boundary; the potential energy function of the trajectory attraction term is calculated based on the joint angle vector of the updated trajectory in the current frame, the joint angle vector of the initial planned trajectory in the current frame, and the frame-by-frame adaptive dynamic weight.

[0104] In some alternative implementations, the cost functional construction module 1103 includes: Based on the potential fields of all collision pairs and the determined collision cost term, a collision pair includes two parameterized geometric primitives, at least one of which is a parameterized geometric primitive of the robot's joint mapping. If the collision distance of the collision pair is less than zero, the potential field of the collision pair is calculated based on the penetration depth cutoff and the preset gradient activation record. If the collision distance of the collision pair is greater than or equal to zero and less than the preset safety distance, the potential field of the collision pair is calculated based on the collision distance of the collision pair, the preset gradient activation radius, and the preset attenuation coefficient. If the collision distance of the collision pair is greater than the preset safety distance, the potential field of the collision pair is set to zero.

[0105] In some alternative implementations, the cost functional construction module 1103 includes: The cost functional is calculated based on the trajectory smoothing term and its corresponding smoothing weight, the trajectory attraction term and its corresponding attraction weight, and the collision cost term and its corresponding collision weight. If the collision distance of at least one collision pair is less than zero, the attraction weight is set to a preset minimum value, and the preset safety distance is set to a preset maximum value.

[0106] In some alternative implementations, the cost functional construction module 1103 includes: If the collision distance is greater than zero, the preset safety distance is set to the preset minimum value, and the attraction weight is set to the preset maximum value.

[0107] In some alternative implementations, the cost functional construction module 1103 includes: The gradients of the collision cost terms for all frames are obtained based on the total frame rate. The time series of the gradients of the collision cost terms for all frames are convolved based on a one-dimensional filter kernel to generate the diffused gradient components after temporal diffusion processing. Temporal diffusion processing is used to diffuse the collision gradient information of local time points to their adjacent time frames. The one-dimensional filter kernel is a linear smoothing filter kernel.

[0108] In some alternative implementations, the robot trajectory update module 1105 includes: The updated trajectory is processed by time parameterization to obtain the actual updated trajectory. The time parameterization process includes at least assigning a timestamp based on the updated trajectory and applying predetermined kinematic constraints based on the robot's dynamic performance. The predetermined kinematic constraints include joint velocity limits and joint acceleration limits. The robot's movement is controlled based on the actual updated trajectory.

[0109] The robot trajectory optimization system provided in this embodiment of the invention can execute the robot trajectory optimization method provided in any embodiment of the invention, and has the corresponding functional modules and beneficial effects of the method. Further functional descriptions of the various modules and units are the same as in the corresponding embodiments described above, and will not be repeated here.

[0110] Figure 12 This is a schematic diagram of the structure of an electronic device provided in an embodiment of the present invention.

[0111] The following is a detailed reference. Figure 12 This diagram illustrates a structural schematic suitable for implementing an electronic device according to embodiments of the present invention. The electronic device may include a processor (e.g., a central processing unit, a graphics processing unit, etc.) 1201, which can perform various appropriate actions and processes according to a program stored in ROM 1202 or a program loaded from memory 1208 into RAM 1203. RAM 1203 also stores various programs and data required for the operation of the electronic device. The processor 1201, ROM 1202, and RAM 1203 are interconnected via bus 1204. An input / output (I / O) interface 1205 is also connected to bus 1204; wherein ROM is a read-only memory and RAM is a random access memory.

[0112] Typically, the following devices can be connected to I / O interface 1205: input devices 1206 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 1207 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; memory devices 1208 including, for example, magnetic tapes, hard disks, etc.; and communication devices 1209. Communication device 1209 allows electronic devices to communicate wirelessly or wiredly with other devices to exchange data. Although Figure 12 Electronic devices with various devices are shown, but it should be understood that it is not required to implement or have all of the devices shown, and more or fewer devices may be implemented or have instead.

[0113] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a non-transitory computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device 1209, or installed from a memory 1208, or installed from a ROM 1202. When the computer program is executed by the processor 1201, it performs the functions defined in the robot trajectory optimization method of the embodiments of the present invention.

[0114] Figure 12 The electronic device shown is merely an example and should not be construed as limiting the functionality and scope of use of the embodiments of the present invention.

[0115] This invention also provides a computer-readable storage medium. The methods described above according to embodiments of the invention can be implemented in hardware or firmware, or implemented as computer code that can be recorded on a storage medium, or implemented as computer code downloaded via a network and originally stored on a remote storage medium or a non-transitory machine-readable storage medium and then stored on a local storage medium. Thus, the methods described herein can be processed by software stored on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, optical disk, read-only memory, random access memory, flash memory, hard disk, or solid-state drive, etc.; further, the storage medium can also include combinations of the above types of memory. It is understood that computers, processors, microprocessor controllers, or programmable hardware include storage components capable of storing or receiving software or computer code. When the software or computer code is accessed and executed by the computer, processor, or hardware, the robot trajectory optimization method shown in the above embodiments is implemented.

[0116] A portion of this invention can be applied as a computer program product, such as computer program instructions, which, when executed by a computer, can invoke or provide the methods and / or technical solutions according to the invention through the operation of the computer. Those skilled in the art will understand that the forms in which computer program instructions exist in a computer-readable medium include, but are not limited to, source files, executable files, installation package files, etc. Correspondingly, the ways in which computer program instructions are executed by a computer include, but are not limited to: the computer directly executing the instructions, or the computer compiling the instructions and then executing the corresponding compiled program, or the computer reading and executing the instructions, or the computer reading and installing the instructions and then executing the corresponding installed program. Here, the computer-readable medium can be any available computer-readable storage medium or communication medium accessible to a computer.

[0117] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A robot trajectory optimization method, wherein the robot has at least two joints, characterized in that, The method includes: The initial planned trajectory of different joints of the robot is obtained based on the preset robot motion, and the joint angle vector corresponding to different joints of the robot in each frame is set based on the initial planned trajectory. Map the robot's different joints and preset environmental obstacles into parametric geometric primitives; Construct a cost functional for optimizing the initial planned trajectory, the cost functional including at least the potential energy function of the trajectory smoothing term, the potential energy function of the trajectory attraction term, and the potential energy function of the collision cost term; Based on the potential fields of all collision pairs and the determination of the collision cost term, the collision pair includes two parameters. Parametric geometric primitives, wherein at least one of them is a parametric geometric primitive of the robot's joint mapping; If the collision distance of the collision pair is less than zero, the potential field of the collision pair is calculated based on the penetration depth cutoff and the preset gradient activation record. If the collision distance of the collision pair is greater than or equal to zero and less than a preset safety distance, then the potential field of the collision pair is calculated based on the collision distance of the collision pair, the preset gradient activation radius and the preset attenuation coefficient. If the collision distance of the collision pair is greater than the preset safety distance, the potential field of the collision pair is set to zero. The cost functional is iterated over, and the iterative operation includes: The collision distance between the parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. The gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame are calculated based on the collision distance. The gradient of the cost functional is calculated based on the gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame. The trajectory is updated using gradient descent until a preset termination condition is met, and the robot's movement is controlled based on the updated trajectory.

2. The method according to claim 1, characterized in that, The construction of the cost functional for optimizing the initial planned trajectory includes: The static basic limit is obtained based on the relationship between the minimum collision distance of the initial planned trajectory in each frame and the preset safety distance; The dynamic environment boundary is obtained based on the relationship between the minimum collision distance of the updated trajectory in each frame and the preset safety distance; Frame-by-frame adaptive dynamic weights are set based on the minimum value between the static basic limit and the dynamic environment limit. The potential energy function of the trajectory attraction term is calculated based on the joint angle vector of the updated trajectory in the current frame, the joint angle vector of the initial planned trajectory in the current frame, and the frame-by-frame adaptive dynamic weight.

3. The method according to claim 1, characterized in that, The construction of the cost functional for optimizing the initial planned trajectory includes: The cost functional is calculated based on the trajectory smoothing term and its corresponding smoothing weight, the trajectory attraction term and its corresponding attraction weight, and the collision cost term and its corresponding collision weight. If the collision distance of at least one collision pair is less than zero, the attraction weight is set to a preset minimum value, and the preset safety distance is set to a preset maximum value.

4. The method according to claim 3, characterized in that, The method further includes: If the collision distance is greater than zero, the preset safety distance is set to the preset minimum value, and the attraction weight is set to the preset maximum value.

5. The method according to claim 1, characterized in that, The gradient of the collision cost term for the current frame is calculated based on the collision distance, including: The total frame rate is set based on the initial planned trajectory, and the gradient of the collision cost term for all frames is obtained based on the total frame rate. A convolution operation is performed on the time series of the gradients of the collision cost terms of all frames based on a one-dimensional filter kernel to generate a diffused gradient component after temporal diffusion processing. The temporal diffusion processing is used to diffuse the collision gradient information of local time points to their adjacent time frames. The one-dimensional filter kernel is a linear smoothing filter kernel.

6. The method according to claim 1, characterized in that, The robot movement based on the updated trajectory control includes: The updated trajectory is subjected to time parameterization processing to obtain the actual updated trajectory. The time parameterization processing includes at least allocating timestamps based on the updated trajectory and applying predetermined kinematic constraints based on the robot's dynamic performance. The predetermined kinematic constraints include joint velocity limits and joint acceleration limits. The robot's movement is controlled based on the actual updated trajectory.

7. A robot trajectory optimization system, characterized in that, The system includes: The initial trajectory planning module is used to obtain the initial planned trajectory of different joints of the robot based on the preset robot action, and to set the joint angle vector corresponding to different joints of the robot in each frame based on the initial planned trajectory. The geometric primitive mapping module is used to map different joints of the robot and preset environmental obstacles into parametric geometric primitives; The cost functional construction module is used to construct a cost functional for optimizing the initial planned trajectory, wherein the cost functional includes at least the potential energy function of the trajectory smoothing term, the potential energy function of the trajectory attraction term, and the potential energy function of the collision cost term. The cost functional iteration module is used to perform iterative operations on the cost functional, the iterative operations including: The collision distance between the parameterized geometric primitives in the current frame is calculated based on the analytical distance formula and the joint angle vector of the current frame. The gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame are calculated based on the collision distance. The gradient of the cost functional is calculated based on the gradients of the trajectory smoothing term, collision cost term and trajectory attraction term of the current frame. The robot trajectory update module is used to update the trajectory using the gradient descent method until a preset termination condition is met, and then control the robot's movement based on the updated trajectory.

8. An electronic device, characterized in that, include: A memory and a processor are interconnected, the memory storing computer instructions, and the processor executing the computer instructions to perform the robot trajectory optimization method according to any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing the computer to execute the robot trajectory optimization method according to any one of claims 1 to 6.

Images