Robot end effector trajectory planning method and apparatus

Abstract

This application discloses a method and apparatus for trajectory planning of a robot end effector, relating to the field of robotics. The method includes: acquiring the first initial joint state, pre-grasping pose, and grasping pose of the robot end effector; obtaining a feasible preliminary trajectory based on preset first constraints, the first initial joint state, and the pre-grasping pose; obtaining a feasible grasping trajectory based on preset first constraints, second constraints, the final joint state of the feasible preliminary trajectory, and the grasping pose, wherein the second constraint includes directional constraints and linear path constraints; and obtaining the final trajectory based on the feasible preliminary trajectory and the feasible grasping trajectory. This method can achieve directional-linear joint constraints on some trajectories in grasping tasks, breaking through the efficiency bottleneck of traditional two-step methods and improving trajectory planning efficiency. It can ensure the dynamic consistency of the two trajectory segments, resulting in a smoother final trajectory. It can achieve true optimization of global time and smoothness, improving trajectory quality.

Description

Technical Field

[0001] This application relates to the field of robotics technology, specifically to a robot end effector trajectory planning method, a robot end effector trajectory planning device, a machine-readable storage medium, and an electronic device. Background Technology

[0002] In robotic automated grasping tasks, precise, efficient, and safe trajectory control is the core technology for achieving stable operation. This type of control typically requires the robot's end effector (such as a gripper or suction cup) to stably contact and grasp the target workpiece from an initial pose, ultimately achieving a specific position and orientation (i.e., the grasping pose). To meet the stringent requirements for position and orientation at the grasping point, a classic two-step trajectory planning strategy has long been widely adopted. This traditional strategy first plans a trajectory that moves the robot's end effector to a "pre-grasping pose." This pre-grasping pose is usually set at a fixed offset above the final grasping pose, and its orientation (i.e., tool orientation) is adjusted to perfectly match the final grasping pose. Subsequently, the robot controls the end effector to move linearly along a fixed axis of its tool coordinate system (usually the Z-axis, i.e., the tool pointing direction), either descending or advancing linearly from the pre-grasping pose to the final grasping pose. While the above two-step strategy is widely adopted due to its clear logic and intuitive programming, its inherent planning paradigm has the following significant drawbacks:

[0003] Motion discontinuity and efficiency loss: By artificially dividing the complete grasping trajectory into two independent planning stages (first reaching the pre-grasp point, then grasping in a straight line), an inherent kinematic discontinuity exists in the synthesized overall trajectory at the connection point (i.e., the pre-grasp pose). The robot typically needs to stop completely or decelerate significantly at the pre-grasp point to prepare for the precise linear motion of the next stage, thus introducing unnecessary pauses and time delays, limiting further reduction in the cycle time.

[0004] The strategy suffers from separate and uncoordinated constraint handling: it treats the "direction alignment" constraint (completed upon reaching the pre-grab point) and the "precise linear path" constraint (executed in the second phase) in isolation. The trajectory planner cannot perform unified, coordinated optimization of these two interrelated constraint objectives. For example, it cannot plan a less-than-optimal approach path in the first phase, but one more favorable for linear motion in the second phase, in pursuit of a faster overall completion time, thus sacrificing the possibility of optimal global dynamic performance.

[0005] Collision risks in segmented planning: Two-step planning only explicitly guarantees the safety of discrete poses such as the starting point, pre-grab point, and gripping point. Collision avoidance in the middle process of the two-stage trajectory, especially the interference check between the robot body and the arm during the complex path motion in the first stage, often relies on the engineer's experience or additional post-processing, which has the risk of omission. The safety and reliability of the system need to be improved.

[0006] Existing two-step planning strategies are insufficient in terms of motion coherence, constraint coordination, and global safety, making it difficult to efficiently generate high-quality trajectories that are globally optimal in time, have smooth motion, and are absolutely safe in multi-stage, multi-constraint tasks such as robot grasping. Summary of the Invention

[0007] The purpose of this application is to provide a robot end effector trajectory planning method, a robot end effector trajectory planning device, a machine-readable storage medium, and an electronic device to solve the problem that the existing two-step planning strategy is insufficient in terms of motion coherence, constraint coordination, and global safety, which makes it difficult to efficiently generate high-quality trajectories that are globally optimal in time, have smooth motion, and are absolutely safe in multi-stage and multi-constraint tasks such as robot grasping.

[0008] To achieve the above objectives, the first aspect of this application provides a robot end effector trajectory planning method, comprising:

[0009] Obtain the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector;

[0010] Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, a feasible pre-trajectory is obtained;

[0011] Based on the preset first constraint, second constraint, the final joint state of the feasible pre-trajectory and the grasping pose, a feasible grasping trajectory is obtained. The second constraint includes directional constraint and linear path constraint.

[0012] Based on the feasible preliminary trajectory and the feasible capture trajectory, the final trajectory is obtained.

[0013] In this embodiment of the application, obtaining a feasible preliminary trajectory based on a preset first constraint, a first initial joint state, and a pre-grabbing pose includes:

[0014] Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, an initial feasible pre-trajectory is determined;

[0015] Based on the joint states corresponding to each time step in the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is optimized to obtain a feasible preliminary trajectory.

[0016] In this embodiment of the application, optimizing the initial feasible preliminary trajectory based on the joint states corresponding to each time step in the initial feasible preliminary trajectory to obtain a feasible preliminary trajectory includes:

[0017] Based on the joint states corresponding to each time step in the initial feasible preparatory trajectory, the pose corresponding to each time step in the initial feasible preparatory trajectory is obtained;

[0018] According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible pre-trajectory satisfies the pre-grabbing pose, so as to determine the end time step of the initial feasible pre-trajectory. The end time step of the initial feasible pre-trajectory is the time step when the corresponding pose is first determined to satisfy the pre-grabbing pose.

[0019] Based on the endpoint time step of the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is intercepted to obtain a feasible preliminary trajectory.

[0020] In this embodiment of the application, a feasible grasping trajectory is obtained based on a preset first constraint, a second constraint, the final joint state of the feasible preliminary trajectory, and the grasping pose. The second constraint includes a directional constraint and a linear path constraint, including:

[0021] The final joint state in the feasible preliminary trajectory is used as the initial joint state to obtain the second initial joint state;

[0022] Based on the preset first constraint, second constraint, second initial joint state and the grasping pose, an initial feasible grasping trajectory is determined.

[0023] Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the initial feasible grasping trajectory is optimized to obtain a feasible grasping trajectory.

[0024] In this embodiment of the application, optimizing the initial feasible grasping trajectory based on the joint states corresponding to each time step in the initial feasible grasping trajectory to obtain a feasible grasping trajectory includes:

[0025] Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the poses corresponding to each time step in the initial feasible grasping trajectory are obtained.

[0026] According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible grasping trajectory satisfies the grasping pose, so as to determine the end time step of the initial feasible grasping trajectory. The end time step of the initial feasible grasping trajectory is the time step when the corresponding pose is first determined to satisfy the grasping pose.

[0027] Based on the endpoint time step of the initial feasible crawling trajectory, the initial feasible crawling trajectory is intercepted to obtain a feasible crawling trajectory.

[0028] In this embodiment of the application, there are multiple feasible preliminary trajectories and multiple feasible grasping trajectories. The step of obtaining the final trajectory based on the feasible preliminary trajectories and the feasible grasping trajectories includes:

[0029] Multiple feasible pre-trajectories and multiple feasible grasping trajectories are spliced ​​together to obtain multiple initial trajectories;

[0030] The trajectory cost of each initial trajectory is calculated separately;

[0031] Based on the trajectory cost of each initial trajectory, the final trajectory is determined from the plurality of initial trajectories.

[0032] In this embodiment of the application, the calculation of the trajectory cost of each initial trajectory includes:

[0033] The first cost of each initial trajectory is obtained, where the first cost is the cost of each initial trajectory satisfying a preset first constraint.

[0034] The second cost of each initial trajectory is obtained respectively. The second cost is the cost of the feasible capture trajectory among the initial trajectories under the linear path constraint and the direction constraint.

[0035] Based on the first cost and the second cost, the trajectory cost of each initial trajectory is obtained.

[0036] In this embodiment of the application, obtaining the second cost of the initial trajectory includes:

[0037] Based on the initial trajectory, the basic data corresponding to multiple time steps are determined. The basic data corresponding to the multiple time steps are the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory.

[0038] Based on the aforementioned basic data, the cost corresponding to each time step in the feasible capture trajectory is calculated according to a preset calculation function.

[0039] Based on the cost corresponding to each time step in the feasible capture trajectory, the second cost of the initial trajectory is obtained.

[0040] In this embodiment of the application, the preset calculation function is:

[0041] ,

[0042] in, For linear path constraints, For directional constraints, The joint angles of the current robot end effector. The reference values ​​for the joint angles of the preset linear path. Let be the quaternion of the current robot end effector. Capture the pose quaternion for the target. The first loss coefficient, This is the second loss coefficient. This is the third loss coefficient.

[0043] A second aspect of this application provides a trajectory planning device for a robot end effector, comprising:

[0044] The acquisition module is used to acquire the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector;

[0045] The first planning module is used to obtain a feasible preliminary trajectory based on the preset first constraint, the first initial joint state and the pre-grabbing pose.

[0046] The second planning module is used to obtain a feasible grasping trajectory based on the preset first constraint, second constraint, the final joint state of the feasible pre-trajectory and the grasping pose. The second constraint includes directional constraint and linear path constraint.

[0047] The determination module is used to obtain the final trajectory based on the feasible preliminary trajectory and the feasible capture trajectory.

[0048] A third aspect of this application provides an electronic device, the electronic device comprising:

[0049] At least one processor;

[0050] A memory connected to the at least one processor;

[0051] The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the above-described robot end effector trajectory planning method by executing the instructions stored in the memory.

[0052] A fourth aspect of this application provides a machine-readable storage medium storing instructions that, when executed by a processor, configure the processor to perform the above-described robot end effector trajectory planning method.

[0053] The above technical solution involves acquiring the first initial joint state, pre-grasping pose, and grasping pose of the robot's end effector; obtaining a feasible preliminary trajectory based on a preset first constraint, the first initial joint state, and the pre-grasping pose; obtaining a feasible grasping trajectory based on a preset first constraint, a second constraint, the final joint state of the feasible preliminary trajectory, and the grasping pose, where the second constraint includes directional constraints and linear path constraints; and obtaining the final trajectory based on the feasible preliminary trajectory and the feasible grasping trajectory. By implementing directional constraints and linear path constraints during the feasible grasping trajectory construction stage, directional-linear joint constraints on some trajectories in the grasping task can be achieved. Since both directional constraints and linear paths are placed in the feasible grasping trajectory stage, the construction of the feasible preliminary trajectory is faster, and the entire trajectory planning is faster, thus breaking through the efficiency bottleneck of the traditional two-step method and improving trajectory planning efficiency. Compared to existing methods that first optimize the pre-grasp pose and then separately optimize the linear path, which cannot guarantee the dynamic consistency of the two trajectory segments, this method uses the final joint state of the feasible pre-trajectory as input for feasible grasp trajectory planning. This ensures the dynamic consistency of the two trajectory segments, resulting in a smoother final trajectory. This method utilizes all degrees of freedom to find an efficient approach path in the early stages and seamlessly transitions to precise orientation in the later stages, achieving true optimization of global time and smoothness, thus improving trajectory quality.

[0054] Other features and advantages of the embodiments of this application will be described in detail in the following detailed description section. Attached Figure Description

[0055] The accompanying drawings are provided to further illustrate the embodiments of this application and form part of the specification. They are used together with the following detailed description to explain the embodiments of this application, but do not constitute a limitation on the embodiments of this application. In the drawings:

[0056] Figure 1 The illustration shows a flowchart of a robot end effector trajectory planning method according to an embodiment of this application;

[0057] Figure 2 This illustration schematically shows a structural diagram of a robot end effector trajectory planning device according to an embodiment of this application;

[0058] Figure 3 The diagram illustrates the internal structure of a computer device according to an embodiment of this application.

[0059] Explanation of reference numerals in the attached figures

[0060] 410 - Acquisition Module; 420 - First Planning Module; 430 - Second Planning Module; 440 - Determination Module; A01 - Processor; A02 - Network Interface; A03 - Internal Memory; A04 - Display Screen; A05 - Input Device; A06 - Non-volatile Storage Medium; B01 - Operating System; B02 - Computer Program. Detailed Implementation

[0061] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only for illustration and explanation of the embodiments of this application and are not intended to limit the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0062] It should be noted that the acquisition, transmission, storage, use, and processing of data in the technical solution of this application all comply with relevant laws and regulations. In the embodiments of this application, certain existing industry solutions such as software, components, and models may be mentioned. These should be considered exemplary, intended only to illustrate the feasibility of implementing the technical solution of this application, and do not imply that the applicant has already used or necessarily used such solutions.

[0063] It should be noted that if the embodiments of this application involve directional indicators (such as up, down, left, right, front, back, etc.), the directional indicators are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicators will also change accordingly.

[0064] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0065] Figure 1 The illustration shows a flowchart of a robot end effector trajectory planning method according to an embodiment of this application. Figure 1As shown in the figure, this application provides a robot end effector trajectory planning method, which may include the following steps:

[0066] Step 210: Obtain the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector;

[0067] In this embodiment, the end effector can be a robot gripper, suction cup, etc., and the first initial joint state is the initial joint state of the end effector. The joint state includes joint position, joint velocity, joint acceleration, etc., which can be acquired in real time by sensors or obtained through other systems; this embodiment does not limit this. In the robot grasping task, the pre-grasping pose is a predefined intermediate transition pose preceding the final grasping pose. It can be predetermined as needed. In specific implementation, it can be obtained by applying a fixed spatial transformation based on the known final grasping pose. The final joint state corresponding to the pre-grasping pose is unknown; the same pose may correspond to multiple joint states. The grasping pose refers to the position and orientation of the robot's coordinate system relative to the world coordinate system (or target coordinate system) when the robot's end effector makes stable contact with the target and forms a reliable grip for successful grasping. It can be predetermined as needed.

[0068] It should be noted that each joint state records the joint state of the robot's end effector at the current time step. The pose of the end effector can be solved by forward kinematics for each joint state. The pose includes spatial coordinates and rotation angles (6 parameters).

[0069] Step 220: Based on the preset first constraint, the first initial joint state, and the pre-grabbing pose, obtain a feasible pre-trajectory;

[0070] In this embodiment, the trajectory is essentially a sequence of joint states consisting of multiple time steps, such as {S0, S1…Si…S30}, where each Si represents a joint state at one time step. The first initial joint state is S0. The aforementioned feasible preliminary trajectory refers to the trajectory from the initial pose of the end effector to the pre-grasp pose. The initial pose can refer to the pose corresponding to the first initial joint state, which can be obtained by solving the forward kinematics of the first initial joint state. When solving the preliminary trajectory, the initial pose and the pre-grasp pose can be input into the trajectory solver, and a preset first constraint can be given to allow the solver to generate a feasible preliminary trajectory that satisfies the task cost. The process of solving the trajectory using the trajectory solver is prior art and will not be described in detail here. The preset first constraint can be set in advance, for example, it can include the task cost. Smoothing costs Collision avoidance costs The task cost ensures the end effector reaches the target pose (position and orientation) at the final time step; the smoothing cost minimizes acceleration and jerk to ensure a smooth trajectory; and the collision avoidance cost is used for full activation, using a signed distance function to check for self-collisions and environment collisions. The task cost, smoothing cost, and collision avoidance cost can be implemented using existing techniques and will not be elaborated further. The feasible preliminary trajectories can be one or more, and the time steps contained in a feasible preliminary trajectory can be represented as q (q is a hyperparameter of the solution algorithm). A feasible preliminary trajectory can be represented as {S0, S1…Sq}.

[0071] In some embodiments, after determining a feasible pre-trajectory, the above process can be further optimized to obtain a more accurate pre-trajectory. Specifically, obtaining the feasible pre-trajectory based on a preset first constraint, the first initial joint state, and the pre-grasp pose includes:

[0072] First, based on the preset first constraint, the first initial joint state, and the pre-grabbing pose, an initial feasible pre-trajectory is determined;

[0073] In this embodiment, the number of initial feasible pre-trajectories can be multiple.

[0074] Then, based on the joint states corresponding to each time step in the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is optimized to obtain a feasible preliminary trajectory.

[0075] In this embodiment, the above optimization can be to seek the shortest time from the starting point to the ending point within the limits of dynamics, or it can be other optimization strategies, which will not be listed here. When there are multiple initial feasible preliminary trajectories, each initial feasible preliminary trajectory can be optimized separately. By optimizing each initial feasible preliminary trajectory, a more accurate feasible preliminary trajectory can be obtained.

[0076] In some embodiments, optimizing the initial feasible preliminary trajectory based on the joint states corresponding to each time step in the initial feasible preliminary trajectory to obtain a feasible preliminary trajectory includes:

[0077] The first step is to obtain the pose corresponding to each time step in the initial feasible preparatory trajectory based on the joint state corresponding to each time step in the initial feasible preparatory trajectory.

[0078] In this embodiment, the joint states corresponding to each time step can be solved by forward kinematics to obtain the corresponding pose, which is existing technology and will not be described in detail here.

[0079] The second step is to determine whether the pose corresponding to each time step in the initial feasible pre-trajectory satisfies the pre-grabbing pose in the order of time steps from front to back, so as to determine the end time step of the initial feasible pre-trajectory. The end time step of the initial feasible pre-trajectory is the time step when the corresponding pose is first determined to satisfy the pre-grabbing pose.

[0080] In this embodiment, since the trajectory is a sequence of joint states consisting of multiple time steps, the poses corresponding to the time steps can be sequentially determined according to the order from front to back to see if they satisfy the pre-grasp pose. Whether the poses corresponding to the time steps satisfy the pre-grasp pose can be compared with the pre-grasp pose to determine if they are satisfied. The time step corresponding to the first satisfied joint state is denoted as Sx, indicating that the pre-grasp pose has been reached at time step Sx. Time step Sx can be taken as the endpoint time step of the initial feasible pre-trajectory, and the determination stops.

[0081] The third step is to extract the initial feasible preparatory trajectory based on the end time step of the initial feasible preparatory trajectory to obtain the feasible preparatory trajectory.

[0082] In this embodiment, the above-mentioned truncation refers to removing the portion after the endpoint time step in the initial feasible preliminary trajectory. That is, in the above example, {S0, S1...Sx} is truncated as the optimized feasible preliminary trajectory corresponding to the initial feasible preliminary trajectory.

[0083] By sequentially determining whether the pose corresponding to each time step in the initial feasible pre-trajectory satisfies the pre-grabbing pose according to the time step from front to back, the initial feasible pre-trajectory is truncated based on the time step at which the first determined pose satisfies the pre-grabbing pose, thus obtaining the feasible pre-trajectory. Since the pre-grabbing pose may have already been reached at a certain time step, the total length of the time steps can be dynamically adjusted without being limited by the hyperparameter q, thereby achieving deep optimization of the execution efficiency of the feasible pre-trajectory.

[0084] It should be noted that when there are multiple initial feasible preparatory trajectories, each initial feasible preparatory trajectory can be optimized according to the above steps to obtain multiple feasible preparatory trajectories. That is, after obtaining several initial feasible preparatory trajectories, each initial feasible preparatory trajectory is optimized and checked. The joint states corresponding to the q time steps of each initial feasible preparatory trajectory are checked sequentially from front to back. The pose of the end effector of the corresponding joint state is calculated to see if it satisfies the pre-grasp pose. The time step corresponding to the joint state that satisfies the pose for the first time is recorded as Sx. {S0, S1...Sx} is extracted as the optimized feasible preparatory trajectory corresponding to the initial feasible preparatory trajectory, thus obtaining multiple feasible preparatory trajectories.

[0085] Step 230: Based on the preset first constraint, second constraint, final joint state of the feasible pre-trajectory and the grasping pose, obtain the feasible grasping trajectory. The second constraint includes directional constraint and linear path constraint.

[0086] In this embodiment, the aforementioned feasible grasping trajectory refers to the trajectory from the last pose of the feasible preliminary trajectory to the grasping pose. The final joint state of the feasible preliminary trajectory can be used as the initial joint state, and the initial pose can be obtained by solving the forward kinematics of the final joint state. When solving the preliminary trajectory, the initial pose and the grasping pose can be input into the trajectory solver, and a preset first constraint and second constraint can be given, allowing the solver to generate a feasible grasping trajectory that satisfies the constraints. The above-mentioned process of solving the trajectory using the trajectory solver is prior art and will not be described in detail here. The aforementioned directional constraint is used to constrain the motion direction of the end effector, and the aforementioned linear path constraint is used to constrain the end effector to perform linear motion. Since there can be multiple feasible preliminary trajectories, multiple feasible grasping trajectories derived from each feasible preliminary trajectory can be determined. The directional constraint can be mapped to a rotation matrix to constrain the motion degrees of freedom of the end effector on a specified axis. The specific steps are as follows: according to the input specified feasible grasping trajectory axis direction, such as the x-axis, y-axis, and z-axis, it is converted into the corresponding rotation matrix, and the end effector will move along the fixed direction.

[0087] In some embodiments, a feasible grasping trajectory is obtained based on a preset first constraint, a second constraint, the final joint state of the feasible pre-trajectory, and the grasping pose. The second constraint includes a direction constraint and a linear path constraint, including:

[0088] First, the final joint state in the feasible preliminary trajectory is used as the initial joint state to obtain the second initial joint state;

[0089] In this embodiment, the aforementioned final joint state is the joint state corresponding to the time step when the end effector reaches the pre-grasp pose. For each optimized feasible preparatory trajectory, the final joint state in the feasible preparatory trajectory is the joint state corresponding to the time step when the pre-grasp pose is first satisfied, which is Sx.

[0090] Then, based on the preset first constraint, second constraint, second initial joint state and the grasping pose, an initial feasible grasping trajectory is determined;

[0091] In this embodiment, the corresponding pose can be obtained by solving the forward kinematics of the second initial joint state. This pose, along with the grasping pose, is then input into the trajectory solver. Given a preset first constraint and a second constraint, the solver generates a feasible grasping trajectory that satisfies the constraints, thus obtaining the initial feasible grasping trajectory. For example, the pre-grasping pose (with the initial joint state Sx known) and the grasping pose (with the corresponding final joint state unknown) are input into the trajectory solver. Given the first and second constraints, the solver generates an initial feasible grasping trajectory that satisfies the task cost. The initial feasible grasping trajectory contains N time steps (N is the hyperparameter of the solution algorithm), and the initial feasible pre-grasping trajectory can be represented as {Sx…Sn}.

[0092] Finally, based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the initial feasible grasping trajectory is optimized to obtain a feasible grasping trajectory.

[0093] In this embodiment, the above optimization can be to seek the shortest time from the starting point to the ending point within the limits of dynamics, or it can be other optimization strategies, which will not be listed here. When there are multiple initial feasible grasping trajectories, each initial feasible grasping trajectory can be optimized separately. By optimizing each initial feasible grasping trajectory, a more accurate feasible grasping trajectory can be obtained.

[0094] In some embodiments, optimizing the initial feasible grasping trajectory based on the joint states corresponding to each time step in the initial feasible grasping trajectory to obtain a feasible grasping trajectory includes:

[0095] The first step is to obtain the pose corresponding to each time step in the initial feasible grasping trajectory based on the joint state corresponding to each time step in the initial feasible grasping trajectory.

[0096] In this embodiment, the joint states corresponding to each time step can be solved by forward kinematics to obtain the corresponding pose, which is existing technology and will not be described in detail here.

[0097] The second step is to determine whether the pose corresponding to each time step in the initial feasible grasping trajectory satisfies the grasping pose in the order of time steps from front to back, so as to determine the end time step of the initial feasible grasping trajectory. The end time step of the initial feasible grasping trajectory is the time step when the corresponding pose is first determined to satisfy the grasping pose.

[0098] In this embodiment, since the trajectory is a sequence of joint states consisting of multiple time steps, the poses corresponding to the time steps can be sequentially determined according to the order from front to back to see if they satisfy the pre-grasping pose. Whether the poses corresponding to the time steps satisfy the pre-grasping pose can be determined by comparing them with the grasping pose. The time step corresponding to the first satisfied joint state is denoted as Sy, indicating that the grasping pose has been reached at time step Sy. Time step Sy can be taken as the endpoint time step of the initial feasible grasping trajectory, and the determination stops.

[0099] The third step is to extract the initial feasible grasping trajectory based on the end time step of the initial feasible grasping trajectory to obtain the feasible grasping trajectory.

[0100] In this embodiment, the above-mentioned truncation refers to removing the portion after the endpoint time step in the initial feasible crawling trajectory. For example, in the above example, the initial feasible preliminary trajectory is {Sx…Sn}, and {Sx…Sy} is truncated as the optimized feasible crawling trajectory corresponding to the feasible crawling trajectory.

[0101] By sequentially determining whether the pose corresponding to each time step in the initial feasible grasping trajectory satisfies the grasping pose according to the time step order from front to back, the time step at which the first corresponding pose is determined to satisfy the grasping pose is identified. The initial feasible grasping trajectory is then truncated to obtain the feasible grasping trajectory. Because the grasping pose may have already been reached at a certain time step, the total length of the time steps can be dynamically adjusted without being limited by the hyperparameter N, thereby achieving deep optimization of the execution efficiency of the grasping trajectory.

[0102] It should be noted that when there are multiple initial feasible grasping trajectories, each initial feasible grasping trajectory can be optimized according to the above steps to obtain multiple feasible grasping trajectories. That is, in the example above, after obtaining several feasible grasping trajectories, each grasping preparation trajectory is optimized and checked. The joint states corresponding to the N time steps of each feasible grasping trajectory are checked sequentially from front to back. The pose of the end effector of the corresponding joint state is calculated to see if it satisfies the grasping pose. The time step corresponding to the first satisfied joint state is denoted as Sy, and {Sx…Sy} is extracted as the optimized feasible grasping trajectory corresponding to that feasible grasping trajectory.

[0103] Step 240: Based on the feasible preliminary trajectory and the feasible capture trajectory, obtain the final trajectory.

[0104] In this embodiment, the final trajectory can be obtained by splicing the feasible preliminary trajectory and the feasible grasping trajectory.

[0105] In some embodiments, there are multiple feasible preliminary trajectories and multiple feasible grasping trajectories, and obtaining the final trajectory based on the feasible preliminary trajectories and the feasible grasping trajectories includes:

[0106] First, multiple feasible pre-trajectories and multiple feasible grasping trajectories are spliced ​​together to obtain multiple initial trajectories;

[0107] In this embodiment, the feasible grasping trajectories derived from each feasible pre-trajectory can be concatenated with the feasible pre-trajectory to obtain a complete trajectory, i.e., multiple initial trajectories. Alternatively, multiple optimized feasible grasping trajectories {Sx…Sy} derived from each feasible pre-trajectory can be concatenated with the feasible pre-trajectory {S0, S1…Sx} to obtain a complete trajectory {S0, S1…Sx…Sy}. Assuming there are a total of a feasible pre-trajectories, where the i-th feasible pre-trajectory corresponds to Ri derived feasible grasping trajectories, then the final complete trajectory that can be concatenated is: indivual.

[0108] Then, the trajectory cost of each initial trajectory is calculated separately;

[0109] In this embodiment, the total cost corresponding to each complete trajectory (i.e., the initial trajectory) can be calculated, thus obtaining the trajectory cost. Because the time steps of the pre-grasping pose and the grasping pose may be different for each initial trajectory, i.e., the corresponding Sx and Sy may be different, each trajectory needs to be labeled and calculated separately. Among them, the task cost, smoothing cost, and collision avoidance cost in the first constraint need to be considered in both stages of the trajectory, while the linear path constraint and direction constraint are only considered in the grasping motion of the second stage.

[0110] In some embodiments, calculating the trajectory cost of each initial trajectory includes:

[0111] First, the first cost of each initial trajectory is obtained, where the first cost is the cost of each initial trajectory satisfying a preset first constraint.

[0112] In this embodiment, for each initial trajectory, its first cost can be expressed as:

[0113] ,

[0114] in, The task cost can be obtained by summing the task costs at each time step in the initial trajectory. To smooth the cost, it can be obtained by summing the smoothing costs at each time step in the initial trajectory. The collision avoidance cost can be obtained by summing the collision avoidance costs at each time step in the initial trajectory. This is the first cost. The calculation of the first cost can be achieved using existing technology, so it will not be elaborated here.

[0115] Then, the second cost of each initial trajectory is obtained respectively. The second cost is the cost of the feasible grasping trajectory among the initial trajectories under the linear path constraint and the direction constraint.

[0116] In this embodiment, for each initial trajectory, the cost of its feasible grasping trajectory under the linear path constraint and direction constraint can be calculated. This can be obtained by calculating the sum of the costs corresponding to each time step in the feasible grasping trajectory.

[0117] In some embodiments, obtaining a second cost of the initial trajectory includes:

[0118] The first step is to determine the basic data corresponding to multiple time steps based on the initial trajectory. The basic data corresponding to multiple time steps are the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory.

[0119] In this embodiment, given that the initial trajectory is determined, the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory can also be determined.

[0120] The second step is to calculate the cost corresponding to each time step in the feasible grasping trajectory based on the basic data and according to the preset calculation function.

[0121] In this embodiment, the cost corresponding to each time step can be calculated according to a preset calculation function.

[0122] The preset calculation function is:

[0123] ,

[0124] in, For linear path constraints, For directional constraints, The joint angles of the current robot end effector. The reference values ​​for the joint angles of the preset linear path. Let be the quaternion of the current robot end effector. Capture the pose quaternion for the target, that is, capture the quaternion corresponding to the pose. The first loss coefficient, This is the second loss coefficient. This is the third loss coefficient, which can be adjusted according to the actual situation.

[0125] By substituting the basic data into the above calculation function, the cost corresponding to each time step can be quickly calculated. This cost is the cost that is superimposed with linear path constraints and directional constraints.

[0126] The third step is to obtain the second cost of the initial trajectory based on the cost corresponding to each time step in the feasible capture trajectory.

[0127] In this embodiment, the cost corresponding to each time step in the feasible capture trajectory can be summed to obtain the second cost of the initial trajectory.

[0128] By using the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory, the cost corresponding to each time step can be calculated quickly and accurately according to the preset calculation function. Then, by summing the costs corresponding to each time step in the feasible grasping trajectory, the second cost of the initial trajectory can be obtained quickly and accurately.

[0129] Finally, based on the first cost and the second cost, the trajectory cost of each initial trajectory is obtained.

[0130] In this embodiment, the trajectory cost of each initial trajectory is the sum of the first cost and the second cost. For example, the task cost, smoothing cost, and collision avoidance cost need to be considered in both stages of the trajectory, while linear path constraints and directional constraints are only considered in feasible grasping trajectories. The trajectory cost of the aforementioned initial trajectories can be expressed as:

[0131] ,

[0132] in, The trajectory cost is the initial trajectory. The task cost for the initial trajectory, The smoothing cost of the initial trajectory. To avoid costs associated with collisions, The second cost is the initial trajectory.

[0133] By calculating the first and second costs of the initial trajectory separately, the total cost corresponding to each complete trajectory can be accurately calculated.

[0134] Finally, based on the trajectory cost of each initial trajectory, the final trajectory is determined from the plurality of initial trajectories.

[0135] In this embodiment, the complete trajectory with the lowest calculated total cost can be used as the final output trajectory for the robot to execute. That is, the trajectory with the lowest cost among multiple initial trajectories is determined as the final trajectory, thereby ensuring that the final trajectory is optimal.

[0136] In the above implementation process, the first initial joint state, pre-grasping pose, and grasping pose of the robot's end effector are obtained; a feasible preliminary trajectory is obtained based on a preset first constraint, the first initial joint state, and the pre-grasping pose; a feasible grasping trajectory is obtained based on a preset first constraint, a second constraint, the final joint state of the feasible preliminary trajectory, and the grasping pose, where the second constraint includes directional constraints and linear path constraints; and the final trajectory is obtained based on the feasible preliminary trajectory and the feasible grasping trajectory. By implementing directional constraints and linear path constraints during the feasible grasping trajectory construction stage, directional-linear joint constraints on some trajectories in the grasping task can be achieved. Since both directional constraints and linear paths are placed in the feasible grasping trajectory stage, the construction of the feasible preliminary trajectory is faster, and the entire trajectory planning is faster, thereby breaking through the efficiency bottleneck of the traditional two-step method and improving the efficiency of trajectory planning. Compared to existing methods that first optimize the pre-grasp pose and then separately optimize the linear path, which cannot guarantee the dynamic consistency of the two trajectory segments, this method uses the final joint state of the feasible pre-grasp trajectory as input for feasible grasp trajectory planning. This ensures the dynamic consistency of the two trajectory segments, resulting in a smoother final trajectory. This method utilizes all degrees of freedom to find an efficient approach path in the early stages and seamlessly transitions to precise orientation in the later stages, achieving true optimization of global time and smoothness, thus improving trajectory quality. By generating feasible pre-grasp trajectories and feasible grasp trajectories separately, constraints can be activated independently for trajectory segments, allowing for separate optimization of the feasible pre-grasp trajectories and feasible grasp trajectories, facilitating flexible trajectory optimization.

[0137] It should be noted that, from the perspective of system parallelism, the generation and optimization of feasible preliminary trajectories in the first stage and the generation and optimization of feasible grasping trajectories in the second stage are sequential processes. The generation of multiple feasible preliminary trajectories in the first stage can be computed in parallel, as can the optimization of these trajectories. Similarly, the generation of multiple feasible grasping trajectories for each optimized feasible preliminary trajectory in the second stage can be parallelized, as can the optimization of each feasible grasping trajectory and its concatenation with the corresponding feasible preliminary trajectory. Therefore, the total computation time of the system is T = T(average generation time of feasible preliminary trajectories) + T(average optimization time of feasible preliminary trajectories) + T(average generation time of feasible grasping trajectories) + T(average optimization time of feasible grasping trajectories) + T(average concatenation time) + T(total cost calculation time of the complete trajectory) + T(time to select the complete trajectory with the lowest total cost), further improving trajectory planning efficiency. When performing parallel optimization, the L-BFGS optimizer can be used to solve the problem with multiple parallel seeds. Collision constraints are enabled throughout the process to avoid interference in the grasping path.

[0138] For example, when the final trajectory is generated using the method of this embodiment, the robot grasps the cup according to the trajectory: t∈[0, 2.0s]: free movement to the pre-grabbing pose (position constraint only); t∈[2.0s, 3.0s]: activate Z-axis linear motion + end-effector pose constraint to ensure vertical grasping.

[0139] Please refer to Figure 2 , Figure 2 This schematically illustrates a structural diagram of a robot end effector trajectory planning device according to an embodiment of the present application. This embodiment provides a robot end effector trajectory planning device, including an acquisition module 410, a first planning module 420, a second planning module 430, and a determination module 440, wherein:

[0140] The acquisition module 410 is used to acquire the first initial joint state, pre-grasping pose, and grasping pose of the robot end effector;

[0141] The first planning module 420 is used to obtain a feasible preliminary trajectory based on the preset first constraint, the first initial joint state and the pre-grabbing pose.

[0142] The second planning module 430 is used to obtain a feasible grasping trajectory based on the preset first constraint, second constraint, the final joint state of the feasible pre-trajectory and the grasping pose, wherein the second constraint includes directional constraint and linear path constraint.

[0143] The determination module 440 is used to obtain the final trajectory based on the feasible preliminary trajectory and the feasible capture trajectory.

[0144] The robot end effector trajectory planning device includes a processor and a memory. The acquisition module 410, the first planning module 420, the second planning module 430, and the determination module 440 are all stored in the memory as program units. The processor executes the program units stored in the memory to realize the corresponding functions.

[0145] The processor contains a kernel, which retrieves the corresponding program unit from memory. One or more kernels can be configured, and the trajectory planning method for the robot's end effector can be implemented by adjusting the kernel parameters.

[0146] The memory may include non-permanent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0147] This invention provides a machine-readable storage medium storing a program that, when executed by a processor, implements the robot end effector trajectory planning method.

[0148] This invention provides a processor for running a program, wherein the program executes the robot end effector trajectory planning method during runtime.

[0149] In one embodiment, a computer device is provided, which may be a terminal, and its internal structure diagram may be as follows: Figure 3 As shown, the computer device includes a processor A01, a network interface A02, a display screen A04, an input device A05, and a memory (not shown) connected via a system bus. The processor A01 provides computing and control capabilities. The memory includes internal memory A03 and a non-volatile storage medium A06. The non-volatile storage medium A06 stores an operating system B01 and a computer program B02. The internal memory A03 provides an environment for the operation of the operating system B01 and the computer program B02 stored in the non-volatile storage medium A06. The network interface A02 is used for communication with external terminals via a network connection. When the computer program is executed by the processor A01, it implements a robot end effector trajectory planning method. The display screen A04 can be a liquid crystal display (LCD) or an e-ink display. The input device A05 can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.

[0150] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0151] In one embodiment, the robot end effector trajectory planning device provided in this application can be implemented as a computer program, and the computer program can be implemented in the form of, for example, Figure 3 The computer device shown runs on this device. The computer device's memory can store the various program modules that make up the trajectory planning device of the robot's end effector, for example... Figure 2 The diagram shows the acquisition module 410, the first planning module 420, the second planning module 430, and the determination module 440. The computer program comprised of these modules causes the processor to execute the steps in the robot end effector trajectory planning methods described in the various embodiments of this application.

[0152] Figure 3 The computer device shown can be used as follows Figure 2The acquisition module 410 in the robot end effector trajectory planning device shown executes step 210. The computer device can execute step 220 via the first planning module 420. The computer device can execute step 230 via the second planning module 430. The computer device can execute step 240 via the determination module 440.

[0153] This application provides an electronic device, comprising: at least one processor; and a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the at least one processor implements the above-described robot end effector trajectory planning method by executing the instructions stored in the memory. When the processor executes the instructions, it performs the following steps:

[0154] Obtain the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector;

[0155] Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, a feasible pre-trajectory is obtained;

[0156] Based on the preset first constraint, second constraint, the final joint state of the feasible pre-trajectory and the grasping pose, a feasible grasping trajectory is obtained. The second constraint includes directional constraint and linear path constraint.

[0157] Based on the feasible preliminary trajectory and the feasible capture trajectory, the final trajectory is obtained.

[0158] In one embodiment, obtaining a feasible preliminary trajectory based on a preset first constraint, the first initial joint state, and the pre-grasp pose includes:

[0159] Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, an initial feasible pre-trajectory is determined;

[0160] Based on the joint states corresponding to each time step in the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is optimized to obtain a feasible preliminary trajectory.

[0161] In one embodiment, optimizing the initial feasible preliminary trajectory based on the joint states corresponding to each time step in the initial feasible preliminary trajectory to obtain a feasible preliminary trajectory includes:

[0162] Based on the joint states corresponding to each time step in the initial feasible preparatory trajectory, the pose corresponding to each time step in the initial feasible preparatory trajectory is obtained;

[0163] According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible pre-trajectory satisfies the pre-grabbing pose, so as to determine the end time step of the initial feasible pre-trajectory. The end time step of the initial feasible pre-trajectory is the time step when the corresponding pose is first determined to satisfy the pre-grabbing pose.

[0164] Based on the endpoint time step of the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is intercepted to obtain a feasible preliminary trajectory.

[0165] In one embodiment, a feasible grasping trajectory is obtained based on a preset first constraint, a second constraint, the final joint state of the feasible preliminary trajectory, and the grasping pose. The second constraint includes directional constraints and linear path constraints, including:

[0166] The final joint state in the feasible preliminary trajectory is used as the initial joint state to obtain the second initial joint state;

[0167] Based on the preset first constraint, second constraint, second initial joint state and the grasping pose, an initial feasible grasping trajectory is determined.

[0168] Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the initial feasible grasping trajectory is optimized to obtain a feasible grasping trajectory.

[0169] In one embodiment, optimizing the initial feasible grasping trajectory based on the joint states corresponding to each time step in the initial feasible grasping trajectory to obtain a feasible grasping trajectory includes:

[0170] Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the poses corresponding to each time step in the initial feasible grasping trajectory are obtained.

[0171] According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible grasping trajectory satisfies the grasping pose, so as to determine the end time step of the initial feasible grasping trajectory. The end time step of the initial feasible grasping trajectory is the time step when the corresponding pose is first determined to satisfy the grasping pose.

[0172] Based on the endpoint time step of the initial feasible crawling trajectory, the initial feasible crawling trajectory is intercepted to obtain a feasible crawling trajectory.

[0173] In one embodiment, there are multiple feasible preliminary trajectories and multiple feasible grasping trajectories, and obtaining the final trajectory based on the feasible preliminary trajectories and the feasible grasping trajectories includes:

[0174] Multiple feasible pre-trajectories and multiple feasible grasping trajectories are spliced ​​together to obtain multiple initial trajectories;

[0175] The trajectory cost of each initial trajectory is calculated separately;

[0176] Based on the trajectory cost of each initial trajectory, the final trajectory is determined from the plurality of initial trajectories.

[0177] In one embodiment, calculating the trajectory cost of each initial trajectory includes:

[0178] The first cost of each initial trajectory is obtained, where the first cost is the cost of each initial trajectory satisfying a preset first constraint.

[0179] The second cost of each initial trajectory is obtained respectively. The second cost is the cost of the feasible capture trajectory among the initial trajectories under the linear path constraint and the direction constraint.

[0180] Based on the first cost and the second cost, the trajectory cost of each initial trajectory is obtained.

[0181] In one embodiment, obtaining a second cost for the initial trajectory includes:

[0182] Based on the initial trajectory, the basic data corresponding to multiple time steps are determined. The basic data corresponding to the multiple time steps are the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory.

[0183] Based on the aforementioned basic data, the cost corresponding to each time step in the feasible capture trajectory is calculated according to a preset calculation function.

[0184] Based on the cost corresponding to each time step in the feasible capture trajectory, the second cost of the initial trajectory is obtained.

[0185] In one embodiment, the preset calculation function is:

[0186] ,

[0187] in, For linear path constraints, For directional constraints, The joint angles of the current robot end effector. The reference values ​​for the joint angles of the preset linear path. Let be the quaternion of the current robot end effector. Capture the pose quaternion for the target. The first loss coefficient, This is the second loss coefficient. This is the third loss coefficient.

[0188] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0189] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0190] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0191] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0192] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0193] Memory may include non-persistent memory in computer-readable media, such as random access memory (RAM) and / or non-volatile memory, like read-only memory (ROM) or flash RAM. Memory is an example of computer-readable media.

[0194] Computer-readable media includes both permanent and non-permanent, removable and non-removable media that can store information using any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, disk storage or other magnetic storage devices, or any other non-transferable medium that can be used to store information accessible by a computing device. As defined herein, computer-readable media does not include transient computer-readable media, such as modulated data signals and carrier waves.

[0195] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0196] The above are merely embodiments of this application and are not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A robot end effector trajectory planning method, characterized by, include: Obtain the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector; Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, a feasible pre-trajectory is obtained; Based on the preset first constraint, second constraint, final joint state of the feasible preparatory trajectory, and grasping pose, a feasible grasping trajectory is obtained. The second constraint includes directional constraint and linear path constraint. The final joint state in the feasible preparatory trajectory is the initial joint state used to determine the feasible grasping trajectory. Based on the feasible preliminary trajectory and the feasible grasping trajectory, the final trajectory is obtained; There are multiple feasible preliminary trajectories and multiple feasible grasping trajectories. The step of obtaining the final trajectory based on the feasible preliminary trajectories and the feasible grasping trajectories includes: Multiple feasible pre-trajectories and multiple feasible grasping trajectories are spliced ​​together to obtain multiple initial trajectories; The trajectory cost of each initial trajectory is calculated separately; Based on the trajectory cost of each initial trajectory, the final trajectory is determined from the plurality of initial trajectories; The calculation of the trajectory cost for each initial trajectory includes: The first cost of each initial trajectory is obtained, where the first cost is the cost of each initial trajectory satisfying a preset first constraint. The second cost of each initial trajectory is obtained respectively. The second cost is the cost of the feasible capture trajectory among the initial trajectories under the linear path constraint and the direction constraint. Based on the first cost and the second cost, the trajectory cost of each initial trajectory is obtained.

2. The method of claim 1, wherein, The process of obtaining a feasible preliminary trajectory based on a preset first constraint, the first initial joint state, and the pre-grasp pose includes: Based on the preset first constraint, the first initial joint state and the pre-grabbing pose, an initial feasible pre-trajectory is determined; Based on the joint states corresponding to each time step in the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is optimized to obtain a feasible preliminary trajectory.

3. The method according to claim 2, characterized in that, The optimization of the initial feasible preliminary trajectory based on the joint states corresponding to each time step in the initial feasible preliminary trajectory to obtain a feasible preliminary trajectory includes: Based on the joint states corresponding to each time step in the initial feasible preparatory trajectory, the pose corresponding to each time step in the initial feasible preparatory trajectory is obtained; According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible pre-trajectory satisfies the pre-grabbing pose, so as to determine the end time step of the initial feasible pre-trajectory. The end time step of the initial feasible pre-trajectory is the time step when the corresponding pose is first determined to satisfy the pre-grabbing pose. Based on the endpoint time step of the initial feasible preliminary trajectory, the initial feasible preliminary trajectory is intercepted to obtain a feasible preliminary trajectory.

4. The method according to claim 1, characterized in that, Based on the preset first constraint, second constraint, the final joint state of the feasible pre-trajectory, and the grasping pose, a feasible grasping trajectory is obtained. The second constraint includes directional constraints and linear path constraints, including: The final joint state in the feasible preliminary trajectory is used as the initial joint state to obtain the second initial joint state; Based on the preset first constraint, second constraint, second initial joint state and the grasping pose, an initial feasible grasping trajectory is determined. Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the initial feasible grasping trajectory is optimized to obtain a feasible grasping trajectory.

5. The method according to claim 4, characterized in that, The optimization of the initial feasible grasping trajectory based on the joint states corresponding to each time step in the initial feasible grasping trajectory to obtain a feasible grasping trajectory includes: Based on the joint states corresponding to each time step in the initial feasible grasping trajectory, the poses corresponding to each time step in the initial feasible grasping trajectory are obtained. According to the time steps from front to back, it is determined whether the pose corresponding to each time step in the initial feasible grasping trajectory satisfies the grasping pose, so as to determine the end time step of the initial feasible grasping trajectory. The end time step of the initial feasible grasping trajectory is the time step when the corresponding pose is first determined to satisfy the grasping pose. Based on the endpoint time step of the initial feasible crawling trajectory, the initial feasible crawling trajectory is intercepted to obtain a feasible crawling trajectory.

6. The method according to claim 1, characterized in that, The second cost of obtaining the initial trajectory includes: Based on the initial trajectory, the basic data corresponding to multiple time steps are determined. The basic data corresponding to the multiple time steps are the joint angles and quaternions corresponding to each time step in the feasible grasping trajectory. Based on the aforementioned basic data, the cost corresponding to each time step in the feasible capture trajectory is calculated according to a preset calculation function. Based on the cost corresponding to each time step in the feasible capture trajectory, the second cost of the initial trajectory is obtained.

7. The method according to claim 6, characterized in that, The preset calculation function is: ， in, For linear path constraints, For directional constraints, The joint angles of the current robot end effector. The reference values ​​for the joint angles of the preset linear path. Let be the quaternion of the current robot end effector. Capture the pose quaternion for the target. The first loss coefficient, This is the second loss coefficient. This is the third loss coefficient.

8. A trajectory planning device for a robot end effector, characterized in that, include: The acquisition module is used to acquire the first initial joint state, pre-grasp pose, and grasp pose of the robot's end effector; The first planning module is used to obtain a feasible preliminary trajectory based on the preset first constraint, the first initial joint state and the pre-grabbing pose. The second planning module is used to obtain a feasible grasping trajectory based on the preset first constraint, second constraint, the final joint state of the feasible preparatory trajectory, and the grasping pose. The second constraint includes directional constraints and linear path constraints. The final joint state in the feasible preparatory trajectory is the initial joint state used to determine the feasible grasping trajectory. A determining module is used to obtain a final trajectory based on the feasible preliminary trajectory and the feasible grasping trajectory; wherein there are multiple feasible preliminary trajectories and multiple feasible grasping trajectories, and obtaining the final trajectory based on the feasible preliminary trajectories and the feasible grasping trajectories includes: concatenating multiple feasible preliminary trajectories and multiple feasible grasping trajectories to obtain multiple initial trajectories; calculating the trajectory cost of each initial trajectory respectively; and determining the final trajectory from the multiple initial trajectories based on the trajectory costs of each initial trajectory; wherein calculating the trajectory cost of each initial trajectory includes: obtaining a first cost for each initial trajectory, the first cost being the cost of each initial trajectory satisfying a preset first constraint; obtaining a second cost for each initial trajectory, the second cost being the cost of the feasible grasping trajectory among the initial trajectories satisfying linear path constraints and directional constraints; and obtaining the trajectory cost of each initial trajectory based on the first cost and the second cost.

9. A machine-readable storage medium storing instructions thereon, characterized in that, This instruction is used to cause the machine to perform the method according to any one of claims 1 to 7.

10. An electronic device, characterized in that, The electronic device includes: At least one processor; A memory connected to the at least one processor; The memory stores instructions executable by the at least one processor, which implements the method of any one of claims 1 to 7 by executing the instructions stored in the memory.

Images