WO2023173677A1

WO2023173677A1 - Trajectory fusion method and apparatus, and device and storage medium

Info

Publication number: WO2023173677A1
Application number: PCT/CN2022/113053
Authority: WO
Inventors: 杨跞; 杨锋; 汪宇星; 李法设
Original assignee: 中科新松有限公司
Priority date: 2022-03-18
Filing date: 2022-08-17
Publication date: 2023-09-21
Also published as: CN114474072B; CN114474072A

Abstract

Disclosed in the present application are a trajectory fusion method and apparatus, and a device and a storage medium. The method comprises: according to a first trajectory and a first fusion radius associated with the first trajectory, determining Cartesian motion information of a fusion start point in a Cartesian space; according to a second trajectory and the first fusion radius, determining Cartesian motion information of a fusion end point in the Cartesian space, wherein a space where the first trajectory is located is different from a space where the second trajectory is located; and on the basis of the Cartesian motion information of the fusion start point and the Cartesian motion information of the fusion end point, performing trajectory planning on a fusion section between the first trajectory and the second trajectory.

Description

Trajectory fusion method, device, equipment and storage medium

This application claims priority to the Chinese patent application with application number 202210270671.4, which was submitted to the China Patent Office on March 18, 2022. The entire content of this application is incorporated into this application by reference.

Technical field

This application relates to the field of robot motion control technology, for example, to a trajectory fusion method, device, equipment and storage medium.

Background technique

With the continuous development of robotics technology, the application scenarios of robotic arms are becoming more and more extensive. For example, scenarios such as precision assembly, inspection and picking, gluing and oiling, and loading and unloading. In order to ensure the accuracy of the movement of the robotic arm when performing the above operations, the robotic arm needs to be precisely controlled.

When a robotic arm performs a task, it usually moves along one or more pre-planned trajectories. When it is necessary to move along multiple trajectories, the robot needs to travel from the starting point of the current trajectory, to the middle point where the two trajectories meet, and finally to the end point of the next trajectory. During the driving process, the robot's speed starts to accelerate from 0, reaches 0 when it reaches the middle point, accelerates again when it reaches the next trajectory, and returns to 0 when it reaches the end point. This continuous start-stop motion will not only affect the efficiency of the robot in performing tasks, but will also have a certain impact on the life of the robot.

Contents of the invention

This application provides a trajectory fusion method, device, equipment and storage medium to improve the stability of the robot's operation when passing through the transition point between two trajectories.

According to one aspect of the present application, a trajectory fusion method is provided, including:

Determine the Cartesian motion information of the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory;

According to the second trajectory and the first fusion radius, determine the Cartesian motion information of the fusion end point in the Cartesian space; the space where the first trajectory is located and the space where the second trajectory is located are different;

Trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point.

According to another aspect of the present application, a trajectory fusion device is provided, including:

The first motion information determination module is configured to determine the Cartesian motion information of the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory;

The second motion information determination module is configured to determine the Cartesian motion information of the fusion end point in Cartesian space based on the second trajectory and the first fusion radius; the space where the first trajectory is located is different from the space where the second trajectory is located;

The fusion segment trajectory planning module is configured to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point.

According to another aspect of the present application, an electronic device is provided, the electronic device including:

at least one processor; and

a memory communicatively connected to the at least one processor; wherein,

The memory stores a computer program that can be executed by the at least one processor, and the computer program is executed by the at least one processor, so that the at least one processor can execute the method described in any embodiment of the present application. Trajectory fusion method.

According to another aspect of the present application, a computer-readable storage medium is provided. The computer-readable storage medium stores computer instructions. The computer instructions are configured to enable the processor to implement any embodiment of the present application when executed. trajectory fusion method.

Description of the drawings

Figure 1a is a flow chart of a trajectory fusion method provided according to an embodiment of the present application;

Figure 1b is a schematic diagram of Cartesian motion information for determining the starting point of fusion provided according to an embodiment of the present application;

Figure 1c is a schematic diagram of Cartesian motion information for determining the fusion endpoint provided according to an embodiment of the present application;

Figure 2 is a flow chart of a trajectory fusion method provided according to another embodiment of the present application;

Figure 3 is a flow chart of a trajectory fusion method provided according to another embodiment of the present application;

Figure 4 is a flow chart of a trajectory fusion method provided according to another embodiment of the present application;

Figure 5 is a schematic structural diagram of a trajectory fusion device provided according to an embodiment of the present application;

Figure 6 is a schematic structural diagram of an electronic device that implements the trajectory fusion method according to an embodiment of the present application.

Detailed ways

It should be noted that the terms "first", "second", etc. in the description and claims of this application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It is to be understood that the data so used are interchangeable under appropriate circumstances so that the embodiments of the application described herein can be practiced in sequences other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, e.g., a process, method, system, product, or apparatus that encompasses a series of steps or units and need not be limited to those explicitly listed. Those steps or elements may instead include other steps or elements not expressly listed or inherent to the process, method, product or apparatus.

Figure 1a is a flow chart of a trajectory fusion method provided by an embodiment of the present application. This embodiment can be applied to trajectory fusion of trajectories in different spaces. This method can be executed by a trajectory fusion device. The trajectory fusion device It can be implemented in the form of hardware and/or software, and the trajectory fusion device can be configured in a variety of general computing devices. As shown in Figure 1a, the method includes:

S110. Determine the Cartesian motion information of the fusion starting point in the Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory.

In order to control the robot to move along the trajectory desired by the user, the user needs to input one or more trajectories in advance to form a trajectory pool. During the movement of the robot, the trajectory planner will sequentially take out trajectories from the trajectory pool for speed planning according to the trajectory execution sequence, so that the robot moves along the trajectory pre-inputted by the user. When the trajectory pool contains multiple trajectories, the robot needs to move along multiple trajectories in sequence. In the transition section between the two trajectories, the speed or acceleration is prone to be unstable, which in turn causes the path of the transition section to be non-unique and affects The robot works normally.

In order to avoid the above-mentioned unstable path speed or acceleration in the transition section, when users input multiple trajectories, they need to set the fusion radius corresponding to the trajectory at the same time, so that the trajectory planner determines the fusion starting point and fusion end point through the fusion radius, and then based on the fusion starting point and fusion endpoint to fuse two adjacent trajectories so that the robot can smoothly transition from the previous trajectory to the next trajectory.

The first trajectory is the trajectory that the robot needs to run input by the user. The fusion radius associated with the first trajectory is used to determine the fusion segment between the first trajectory and a subsequent trajectory (second trajectory) adjacent to the first trajectory. Cartesian motion information refers to posture, velocity and acceleration information in Cartesian space.

In the embodiment of the present disclosure, in order to fuse trajectories in different spaces, after determining the motion information of the fusion starting point and the fusion end point, the motion information can be uniformly mapped to the Cartesian space to achieve trajectory fusion in different spaces. After extracting the first trajectory from the trajectory pool, the trajectory planner will perform motion planning on the first trajectory, and at the same time determine whether the trajectory pool contains the first fusion radius associated with the first trajectory. If the trajectory pool does not contain the first fusion radius associated with the first trajectory, it means that the first trajectory does not need to be fused with other trajectories and only needs to continue motion planning for the first trajectory. If the trajectory pool contains the first fusion radius associated with the first trajectory, it means that the first trajectory needs to be fused with other trajectories. At this time, the fusion starting point can be determined based on the first trajectory and the fusion radius associated with the first trajectory. Cartesian motion information in space.

For example, when the first trajectory is a Cartesian trajectory in Cartesian space, the Cartesian motion information of the fusion starting point in the Cartesian space can be determined directly based on the first trajectory and the first fusion radius associated with the first trajectory, where, Cartesian motion information is the fusion of the position, velocity and acceleration information of the starting point in Cartesian space.

When the first trajectory is a joint trajectory in the joint space, first, the joint motion information of the fusion starting point in the joint space can be determined in the first trajectory based on the first trajectory and the first fusion radius associated with the first trajectory, where the joint The motion information is the pose, velocity and acceleration information of the fusion starting point in the joint space. For example, with the fusion starting point as the center, a set number of first trajectory points are obtained on both sides of the fusion starting point in the first trajectory, the fusion starting point and the acquired first trajectory points are mapped to Cartesian space, and The fusion starting point and the first trajectory point mapped to the Cartesian space are fitted with a spline curve to obtain a fitting curve in the Cartesian space. Finally, based on the fitting curve, the Cartesian motion information of the fusion starting point in Cartesian space is determined, where the Cartesian motion information includes the posture, velocity and acceleration of the Cartesian space. Among them, spline curve fitting can include but is not limited to quintic spline, B-spline, non-uniform relational B-spline (Non Uniform Relational B Spline, NURBS), etc.

In one example, the starting point of the first trajectory is A and the end point is B. In the case where the first trajectory is a Cartesian trajectory in Cartesian space, the Cartesian motion information of the fusion starting point X in the Cartesian space is determined directly based on the fusion radius associated with the first trajectory.

In another example, as shown in Figure 1b, the starting point of the first trajectory is A and the end point is B. When the first trajectory is a joint trajectory in joint space, first determine the joint motion information of the fusion starting point X in the joint space based on the fusion radius r associated with the first trajectory. For example, with the fusion starting point as the center, a set number of first trajectory points are determined in the first trajectory. For example, first trajectory points M1, N1, P1, and Q1 are respectively determined on both sides of the fusion starting point in the first trajectory. For example, you can map the fusion starting point curve. Finally, based on the fitting curve, the Cartesian motion information of the fusion starting point X in Cartesian space is determined.

S120. Based on the second trajectory and the first fusion radius, determine the Cartesian motion information of the fusion end point in the Cartesian space; the space where the first trajectory is located is different from the space where the second trajectory is located.

The second trajectory is the subsequent trajectory connected to the first trajectory, that is, after the robot finishes moving along the first trajectory, it enters the second trajectory and continues to move along the second trajectory. It is worth noting that the space where the first trajectory is located is different from the space where the second trajectory is located. For example, the first trajectory is a joint trajectory in joint space, and the second trajectory is a Cartesian trajectory in Cartesian space. Or, the first trajectory is a Cartesian trajectory in Cartesian space, and the second trajectory is in joint space. joint trajectories.

In the embodiment of the present disclosure, during the motion planning process along the first trajectory, when it is detected that the motion planning has been executed to half of the first trajectory, the next trajectory adjacent to the first trajectory can be obtained from the trajectory pool. That is the second trajectory. In order to fuse the first trajectory and the second trajectory, Cartesian motion information of the fusion end point in Cartesian space is determined on the second trajectory based on the second trajectory and the first fusion radius.

For example, when the second trajectory is a Cartesian trajectory in Cartesian space, the Cartesian motion information of the fusion end point in the Cartesian space can be determined directly based on the second trajectory and the first fusion radius, where the Cartesian motion information is Fusion of the pose, velocity and acceleration information of the starting point in Cartesian space.

When the second trajectory is a joint trajectory in the joint space, first, the joint motion information of the fusion end point in the joint space can be determined in the second trajectory based on the second trajectory and the first fusion radius, where the joint motion information is the fusion end point. Position, velocity and acceleration information in joint space. For example, with the fusion end point as the center, a set number of second trajectory points are obtained in the second trajectory, the fusion end point and the obtained second trajectory points are mapped to the Cartesian space, and the points mapped to the Cartesian space are Fusion of the end point and the second trajectory point performs spline curve fitting to obtain a fitting curve in Cartesian space. Finally, based on the fitting curve, the Cartesian motion information of the fusion endpoint in Cartesian space is determined, where the Cartesian motion information includes the posture, velocity and acceleration of the Cartesian space. Among them, spline curve fitting can include quintic spline, B-spline, and Non-Uniform Relational B-Spline (NURBS), etc.

In one example, the starting point of the second trajectory is B and the end point is C. In the case where the second trajectory is a Cartesian trajectory in Cartesian space, the Cartesian motion information of the fusion end point Y in the Cartesian space is determined directly based on the first fusion radius.

In another example, as shown in Figure 1c, the starting point of the second trajectory is the end point B of the first trajectory, and the end point of the second trajectory is C. When the second trajectory is a joint trajectory in the joint space, first determine the joint motion information of the fusion end point Y in the joint space based on the first fusion radius. For example, with the fusion endpoint as the center, a set number of second trajectory points are determined in the second trajectory. For example, second trajectory points M2, N2, P2, and Q2 are respectively determined on both sides of the fusion endpoint in the second trajectory. For example, the fusion end point Y and the second trajectory points M2, N2, P2 and Q2 can be mapped to the Cartesian space, and the above trajectory points mapped to the Cartesian space can be fitted with a spline curve to obtain a fitting of the Cartesian space. curve. Finally, and the obtained fitting curve, the Cartesian motion information of the fusion end point Y in Cartesian space is determined.

S130. Based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, perform trajectory planning for the fusion segment between the first trajectory and the second trajectory.

In the embodiment of the present disclosure, after obtaining the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, the first trajectory and the second trajectory are calculated based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point. Fusion segments between trajectories perform trajectory specification.

For example, after obtaining the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, the planning of the first trajectory can be continued with the currently planned trajectory point in the first trajectory as the starting point and the fusion starting point as the end point. For example, after completing the planning of the first trajectory, that is, when the planning has reached the fusion section of the first trajectory and the second trajectory, you can continue to use the currently planned trajectory point (in the fusion section) as the starting point and the fusion end point as the end point, Carry out trajectory planning for the fusion segment until the trajectory planning for the fusion segment is completed. Of course, after completing the trajectory planning of the fusion section between the first trajectory and the second trajectory, trajectory planning of the second trajectory can be continued to achieve a smooth transition of the robot between the first trajectory and the second trajectory.

The technical solution of the embodiment of the present application determines the Cartesian motion information of the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory, and then determines the fusion end point based on the second trajectory and the first fusion radius. The Cartesian motion information in Cartesian space is finally based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, and the trajectory planning of the fusion segment between the first trajectory and the second trajectory is performed by combining the fusion starting point and the Cartesian motion information. The fusion endpoints are all mapped to Cartesian space, which avoids the inability to smoothly transition trajectories in different spaces and improves the stability of the robot's operation when passing through the transition point between two trajectories.

Figure 2 is a flow chart of a trajectory fusion method provided by another embodiment of the present application. Based on the above embodiment, it is further refined and provides a method to determine the fusion based on the first trajectory and the first fusion radius associated with the first trajectory. Steps of Cartesian motion information starting in Cartesian space. A trajectory fusion method provided by the embodiment of the present application will be described below with reference to Figure 2, including the following:

S210. When the space where the first trajectory is located is the joint space, determine the joint motion information of the fusion starting point in the joint space based on the first trajectory and the first fusion radius associated with the first trajectory.

In the embodiment of the present disclosure, when the space where the first trajectory is located is the joint space, the joint motion information of the fusion starting point in the joint space is first determined based on the first trajectory and the first fusion radius associated with the first trajectory. Wherein, the fusion starting point is located on the first trajectory, and the straight-line distance between the fusion starting point and the first trajectory is equal to the first fusion radius. The joint motion information of the fusion starting point includes the pose, velocity and acceleration information of the fusion starting point in the joint space.

S220: With the fusion starting point as the center, select a set number of first trajectory points in the first trajectory.

In the embodiment of the present disclosure, in order to map the joint motion information of the fusion starting point to the Cartesian space, the Cartesian motion information of the fusion starting point in the Cartesian space is obtained. First, with the fusion starting point as the center, select the first trajectory points of the set data on both sides of the fusion starting point in the first trajectory.

For example, the starting point of the first trajectory is A and the end point is B, and the fusion starting point is the trajectory point X located on the first trajectory and close to the end point B. Taking the fusion starting point X as the center, five trajectory points are obtained along the XA direction and the XB direction in the first trajectory as the first trajectory points.

S230. Map the fusion starting point and the first trajectory point to a Cartesian space, and perform spline fitting on the fusion starting point and the first trajectory point mapped to the Cartesian space to obtain a fitting curve in the Cartesian space.

In this embodiment, in order to map the motion information of the fusion starting point to the Cartesian space, the fusion starting point and a set number of first trajectory points are mapped to the Cartesian space to obtain the pose of the trajectory points in the Cartesian space. For example, perform spline fitting on the fusion starting point and the first trajectory point mapped to Cartesian space to obtain a fitting curve in Cartesian space.

For example, map the fusion starting point X and the first trajectory points on both sides of the fusion starting point X to the Cartesian space to obtain the pose of the trajectory point in the Cartesian space. For example, through B-spline fitting, the first trajectory point and the fusion starting point are fitted to obtain a fitting curve in Cartesian space.

S240. According to the fitting curve, determine the Cartesian motion information of the fusion starting point in the Cartesian space; the Cartesian motion information includes the posture, velocity and acceleration of the Cartesian space.

In this embodiment, based on the fitting curve, the Cartesian motion information of the fusion starting point in the Cartesian space is determined. For example, based on the fitting curve, the velocity and acceleration information of the fusion starting point in the Cartesian space can be calculated. At this point, the pose, velocity and acceleration information of the fusion starting point in Cartesian space have been obtained.

S250. Determine the Cartesian motion information of the fusion end point in Cartesian space based on the second trajectory and the first fusion radius.

S260. Based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, perform trajectory planning for the fusion segment between the first trajectory and the second trajectory.

For example, during the trajectory planning process, the interpolation period in the trajectory planning process is adjusted based on the speed control information input by the user.

In the embodiment of the present application, when the trajectory planning process enters the fusion section of the first trajectory and the second trajectory, if the speed control information input by the user is received, the trajectory of the fusion section may be deformed due to changes in speed. At this time, the interpolation period during the trajectory planning process of the fusion segment needs to be adjusted based on the speed control information input by the user to ensure path consistency. For example, during the fusion segment trajectory planning process, the user adjusts the robot movement speed from 100% to 10%. At this time, the interpolation period can be adjusted to 1/10 of the original interpolation period.

For example, adjusting the interpolation period in the trajectory planning process based on the speed control information input by the user includes:

According to the speed control information input by the user, at least one of the nonlinear interpolation algorithm, the proportional integral differential PID algorithm and the mean filter algorithm is used to adjust the interpolation period of the trajectory planning process.

In the embodiment of this application, a method of adjusting the interpolation period in the trajectory planning process based on the speed control information input by the user is provided: based on the speed control information input by the user, a nonlinear interpolation algorithm, a PID algorithm or a mean filter algorithm is used to adjust The interpolation period of the trajectory planning process is to ensure that the consistency of the fusion segment trajectory is maintained when the user adjusts the speed.

For example, during the fusion segment trajectory planning process, the user adjusts the robot movement speed from 100% to 10%. At this time, the interpolation period can be adjusted to 1/10 of the original interpolation period through the PID algorithm.

The technical solution of the embodiment of the present application is to obtain the fitting curve of the fusion starting point in Cartesian space through spline curve fitting when the space where the first trajectory is located is joint space, and obtain the motion of the fusion end point in Cartesian space. information, performing trajectory planning on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point. On the one hand, the joint trajectories are mapped to Cartesian space through spline curve fitting, allowing the robot to fuse trajectories in different spaces. On the other hand, during the trajectory planning process, the trajectory is adjusted based on the speed control information input by the user. The interpolation cycle in the planning process can avoid path changes caused by speed adjustment.

Figure 3 is a flow chart of a trajectory fusion method provided by another embodiment of the present application. Based on the above embodiment, it is further refined to provide trajectory planning for the fusion segment between the first trajectory and the second trajectory. Previously and based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, the step of trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory. A trajectory fusion method provided by the embodiment of the present application will be described below with reference to Figure 3, including the following:

S310. Determine the Cartesian motion information of the fusion starting point in the Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory.

S320. Determine the Cartesian motion information of the fusion end point in Cartesian space based on the second trajectory and the first fusion radius.

S330. Based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, perform forward planning on the fusion road section and determine the speed and acceleration of multiple fusion trajectory points in the fusion section.

In this embodiment, in order to avoid the robot speed exceeding the upper speed limit during the trajectory planning process, causing task execution failure or even damage to the robot, pre-planning of the fusion road section can be performed after obtaining the Cartesian motion information of the fusion start point and the fusion end point. , to obtain the velocity and acceleration of multiple fusion trajectory points in the fusion segment.

Pre-forward planning means that before actually planning the trajectory of the fusion segment, in order to avoid speed exceeding the limit in direct motion planning, a trajectory planning is performed first. For example, if the first trajectory and/or the second trajectory belong to joint trajectories in joint space, you can use a fifth degree polynomial for pre-forward planning; if the first trajectory and the second trajectory do not involve joint space, they are both in Cartesian space. For Cartesian trajectories, you can use online trajectory planning (On-Line Trajectory Generation, OTG) trajectory planning or S-shaped trajectory planning for pre-forward planning.

S340. When the speed of at least one fused trajectory point in the fusion segment reaches the speed threshold and/or the acceleration of at least one trajectory point reaches the acceleration threshold, determine the speed scaling factor based on the speed and acceleration.

Among them, the speed scaling factor is used to scale the speed during the actual trajectory fusion process to avoid speed exceeding the limit.

In this embodiment, during the pre-planning process of the fusion segment, if the speed of at least one fusion trajectory point reaches the speed threshold and/or the acceleration of at least one trajectory point reaches the acceleration threshold, the speed scaling can be determined based on the speed and acceleration. factor. For example, the velocity threshold is Vmax and the acceleration threshold is Amax. During the forward planning process of the fusion segment, the speed v of one or more fusion trajectory points is greater than Vmax, which is called an overspeed trajectory point. Then the difference between the speed v and Vmax of multiple overspeed trajectory points can be calculated separately, and based on the difference The value determines the speed scaling factor. For example, the difference values corresponding to multiple speeding trajectory points can be obtained, and the speed scaling factor can be calculated based on the maximum difference value.

In the same way, during the forward planning process of the fusion segment, the acceleration a of one or more fusion trajectory points is greater than Amax, which is called a super-acceleration trajectory point. Then the difference between the acceleration a and Amax of multiple super-acceleration trajectory points can be calculated separately. value, and determine the speed scaling factor based on the difference. For example, the difference values corresponding to multiple hyperacceleration trajectory points can be obtained, and the speed scaling factor can be calculated based on the maximum difference value.

S350: Perform trajectory planning for the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point, the Cartesian motion information of the fusion end point, and the speed scaling factor.

In this embodiment, after the speed scaling factor is calculated, trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point, the Cartesian motion information of the fusion end point, and the speed scaling factor. . For example, when planning the trajectory, the planned trajectory point speed can be multiplied by the speed scaling factor to avoid robot task execution failure due to speed exceeding the limit.

For example, trajectory planning for the fusion segment between the first trajectory and the second trajectory includes: when at least one of the first trajectory and the second trajectory is located in a joint space, using a polynomial to calculate the sum of the first trajectory and the second trajectory. The fusion segment between the second trajectories is used for trajectory planning.

In the embodiment of the present application, a method of trajectory planning for the fusion segment between the first trajectory and the second trajectory is provided: when the space where at least one of the first trajectory and the second trajectory is located is the joint space, A polynomial is used to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory. As an example, a fifth-order polynomial is used for trajectory planning.

For example, trajectory planning for the fusion segment between the first trajectory and the second trajectory also includes: when the spaces where the first trajectory and the second trajectory are located are both Cartesian spaces, using online trajectory planning OTG to map the first trajectory Trajectory planning is performed on the fusion segment between the second trajectory and the second trajectory.

In the embodiment of the present application, a method of trajectory planning for the fusion segment between the first trajectory and the second trajectory is provided: when the spaces where the first trajectory and the second trajectory are located are both Cartesian spaces, OTG is used to Trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory.

The technical solution of the embodiment of the present application first performs forward planning on the fusion path based on the Cartesian motion information of the fusion starting point and the fusion end point, determines the speed and acceleration of multiple fusion trajectory points in the fusion segment, and then fuses the Cartesian motion information of the starting point. , fuse the Cartesian motion information of the end point and the speed scaling factor, and perform trajectory planning for the fusion segment between the first trajectory and the second trajectory, which can avoid excessive speed in the fusion segment and improve the stability of the robot's motion.

Figure 4 is a flow chart of a trajectory fusion method provided by another embodiment of the present application. The specific trajectory planning process includes the following:

S401. Obtain the first trajectory in the trajectory pool, and perform trajectory planning on the first trajectory.

S402. Determine whether the first trajectory is associated with the first fusion radius. Based on the determination result that the first trajectory is associated with the first fusion radius, execute S403. Based on the determination result that the first trajectory is not associated with the first fusion radius, continue to execute the The first trajectory performs trajectory planning operations.

S403. Calculate the motion information of the fusion starting point according to the first trajectory and the first fusion radius.

S404. Determine whether the first trajectory is in the joint space. Based on the determination result that the first trajectory is in the joint space, execute S405. Based on the determination result that the first trajectory is not in the joint space, execute S406.

S405. Map the part of the first trajectory including the fusion starting point to Cartesian space through spline fitting.

S406. Obtain the Cartesian motion information of the fusion starting point in Cartesian space.

S407. Determine whether the first trajectory is more than half planned. Based on the determination result that the first trajectory is more than half planned, execute S408. Based on the determination result that the first trajectory is not planned more than half way, continue to perform the operation of determining whether the first trajectory is more than half planned.

S408: Obtain the second trajectory from the trajectory pool, and calculate the motion information of the fusion end point according to the second trajectory and the first fusion radius.

S409. Determine whether the second trajectory is in the joint space. Based on the determination result that the second trajectory is in the joint space, execute S410. Based on the determination result that the second trajectory is not in the joint space, execute S411.

S410. Map the part of the second trajectory including the fusion endpoint to Cartesian space through spline fitting.

S411. Obtain the Cartesian motion information of the fusion end point in Cartesian space.

S412. Carry out forward planning for the fusion segment based on the Cartesian motion information of the fusion start point and the fusion end point.

S413. Determine whether the speed exceeds the limit during the pre-forward planning process. Based on the judgment result that the speed exceeds the limit, execute S414. Based on the judgment result that the speed does not exceed the limit, execute S415.

S414. Determine the speed scaling coefficient based on the overspeed information.

S415. Perform trajectory planning for the remaining part of the first trajectory according to the speed scaling coefficient and the Cartesian motion information of the fusion starting point (if there is no overspeed, the speed scaling coefficient is set to 1).

S416. Determine whether S415 is executed successfully. Based on the judgment result that S415 is executed successfully, execute S417. Based on the judgment result that S415 is not executed successfully, cancel the trajectory fusion.

S417. Continue to execute the current trajectory planning.

S418. Determine whether to enter the fusion segment. Based on the judgment result of entering the fusion segment, execute S419. Based on the judgment result of not entering the fusion segment, continue to execute S417.

S419. Perform fusion segment trajectory planning based on the Cartesian motion information of the current planned trajectory point and the Cartesian motion information of the fusion end point.

S420. In response to the user's speed adjustment control in the fusion section, use the PID algorithm to adjust the interpolation period.

S421. Determine whether the trajectory planning process leaves the fusion section. Based on the determination result that the trajectory planning process leaves the fusion section, execute S422. Based on the determination result that the trajectory planning process does not leave the fusion section, return to execution S419.

S422. Use the second trajectory as the new first trajectory, take the currently planned trajectory point as the starting point, and the end point of the new first trajectory as the end point, perform trajectory planning on the new first trajectory, and return to execution S402.

Figure 5 is a schematic structural diagram of a trajectory fusion device provided by an embodiment of the present application. This embodiment can be applied to the case of trajectory fusion of robot trajectories, especially to the case of trajectory fusion of trajectories in different spaces. The trajectory fusion device can be implemented in the form of hardware and/or software, and can be integrated into an electronic device carrying the trajectory fusion function, such as a server. As shown in Figure 5, the device includes:

The first motion information determination module 510 is configured to determine the Cartesian motion information with the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory;

The second motion information determination module 520 is configured to determine the Cartesian motion information of the fusion end point in Cartesian space based on the second trajectory and the first fusion radius; the space where the first trajectory is located is different from the space where the second trajectory is located;

The fusion segment trajectory planning module 530 is configured to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point.

For example, the first motion information determination module 510 is set to:

When the space where the first trajectory is located is a joint space, determine the joint motion information of the fusion starting point in the joint space based on the first trajectory and the first fusion radius associated with the first trajectory;

Taking the fusion starting point as the center, select a set number of first trajectory points in the first trajectory;

Map the fusion starting point and the first trajectory point to a Cartesian space, and perform spline fitting on the fusion starting point and the first trajectory point mapped to the Cartesian space to obtain a fitting curve in the Cartesian space;

According to the fitting curve, the Cartesian motion information of the fusion starting point in the Cartesian space is determined; the Cartesian motion information includes the posture, velocity and acceleration of the Cartesian space.

For example, trajectory fusion devices also include:

A pre-forward planning module is configured to perform pre-forward planning on the fusion road section based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, and determine the location of multiple fusion trajectory points in the fusion section. speed and acceleration;

A speed scaling factor determination module configured to determine speed scaling based on the speed and acceleration when the speed of at least one fused trajectory point in the fusion segment reaches a speed threshold and/or the acceleration of at least one trajectory point reaches an acceleration threshold. factor;

The fusion segment trajectory planning module 530 is set to:

Trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point, the Cartesian motion information of the fusion end point, and the speed scaling factor.

For example, trajectory fusion devices also include:

The interpolation cycle adjustment module is set to adjust the interpolation cycle during the trajectory planning process based on the speed control information input by the user.

For example, the interpolation cycle adjustment module is set to:

For example, the fusion segment trajectory planning module 530 is also set to:

When the space in which at least one of the first trajectory and the second trajectory is located is a joint space, a polynomial is used to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory.

For example, the fusion segment trajectory planning module 530 is also set to:

When the spaces where the first trajectory and the second trajectory are located are both Cartesian spaces, online trajectory planning OTG is used to perform trajectory planning on the fusion section between the first trajectory and the second trajectory.

The trajectory fusion device provided by the embodiments of this application can execute the trajectory fusion method provided by any embodiment of this application, and has functional modules and beneficial effects corresponding to the execution method.

FIG. 6 shows a schematic structural diagram of an electronic device 10 that can be used to implement embodiments of the present application. Electronic devices are intended to refer to various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. Electronic devices may also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smart phones, wearable devices (eg, helmets, glasses, watches, etc.), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are examples only and are not intended to limit the implementation of the present application as described and/or claimed herein.

As shown in Figure 6, the electronic device 10 includes at least one processor 11, and a memory communicatively connected to the at least one processor 11, such as a read-only memory (ROM) 12, a random access memory (RAM) 13, etc., wherein the memory stores There is a computer program that can be executed by at least one processor. The processor 11 can perform the operation according to the computer program stored in the read-only memory (ROM) 12 or loaded from the storage unit 18 into the random access memory (RAM) 13. Performs a variety of appropriate actions and processes. In the RAM 13, various programs and data required for the operation of the electronic device 10 can also be stored. The processor 11, the ROM 12 and the RAM 13 are connected to each other via the bus 14. An input/output (I/O) interface 15 is also connected to bus 14 .

Multiple components in the electronic device 10 are connected to the I/O interface 15, including: an input unit 16, such as a keyboard, a mouse, etc.; an output unit 17, such as various types of displays, speakers, etc.; a storage unit 18, such as a magnetic disk, an optical disk, etc. etc.; and communication unit 19, such as network card, modem, wireless communication transceiver, etc. The communication unit 19 allows the electronic device 10 to exchange information/data with other devices through a computer network such as the Internet and/or various telecommunications networks.

Processor 11 may be a variety of general and/or special purpose processing components having processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various specialized artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, digital signal processing processor (DSP), and any appropriate processor, controller, microcontroller, etc. The processor 11 performs a plurality of methods and processes described above, such as trajectory fusion methods.

In some embodiments, the trajectory fusion method may be implemented as a computer program, which is tangibly embodied in a computer-readable storage medium, such as the storage unit 18 . In some embodiments, part or all of the computer program may be loaded and/or installed onto the electronic device 10 via the ROM 12 and/or the communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the trajectory fusion method described above may be performed. Alternatively, in other embodiments, the processor 11 may be configured to perform the trajectory fusion method in any other suitable manner (eg, by means of firmware).

Various implementations of the systems and techniques described above may be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on a chip implemented in a system (SOC), complex programmable logic device (CPLD), computer hardware, firmware, software, and/or combinations thereof. These various embodiments may include implementation in one or more computer programs executable and/or interpreted on a programmable system including at least one programmable processor, the programmable processor The processor, which may be a special purpose or general purpose programmable processor, may receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device. An output device.

Computer programs for implementing the methods of the present application may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that the computer program, when executed by the processor, causes the functions/operations specified in the flowcharts and/or block diagrams to be implemented. A computer program may execute entirely on the machine, partly on the machine, as a stand-alone software package, partly on the machine and partly on a remote machine or entirely on the remote machine or server.

In the context of this application, a computer-readable storage medium may be a tangible medium that may contain or store a computer program for use by or in connection with an instruction execution system, apparatus, or device. Computer-readable storage media may include, but are not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or devices, or any suitable combination of the foregoing. Alternatively, the computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media would include one or more wire-based electrical connections, laptop disks, hard drives, random access memory (RAM), read only memory (ROM), erasable programmable read only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination of the above.

To provide interaction with a user, the systems and techniques described herein may be implemented on an electronic device having a display device (eg, a CRT (cathode ray tube) or LCD (liquid crystal display)) for displaying information to the user monitor); and a keyboard and pointing device (e.g., a mouse or a trackball) through which a user can provide input to the electronic device. Other kinds of devices may also be used to provide interaction with the user; for example, the feedback provided to the user may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and may be provided in any form, including Acoustic input, voice input or tactile input) to receive input from the user.

The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., A user's computer having a graphical user interface or web browser through which the user can interact with implementations of the systems and technologies described herein), or including such backend components, middleware components, or any combination of front-end components in a computing system. The components of the system may be interconnected by any form or medium of digital data communication (eg, a communications network). Examples of communication networks include: local area network (LAN), wide area network (WAN), blockchain network, and the Internet.

Computing systems may include clients and servers. Clients and servers are generally remote from each other and typically interact over a communications network. The relationship of client and server is created by computer programs running on corresponding computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host. It is a host product in the cloud computing service system, which avoids the difficult management and weak business scalability of traditional physical hosts and VPS services. Condition.

It should be understood that various forms of the process shown above may be used, with steps reordered, added or deleted. For example, multiple steps described in this application can be executed in parallel, sequentially, or in different orders. As long as the desired results of the technical solution of this application can be achieved, there is no limitation here.

Claims

A trajectory fusion method including:

Determine the Cartesian motion information of the fusion starting point in Cartesian space according to the first fusion radius associated with the first trajectory and the first trajectory;

According to the second trajectory and the first fusion radius, determine the Cartesian motion information of the fusion end point in the Cartesian space; the space where the first trajectory is located and the space where the second trajectory is located are different;

Trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point.
The method according to claim 1, wherein determining the Cartesian motion information of the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory includes:

In response to determining that the space where the first trajectory is located is the joint space, determine the joint motion information of the fusion starting point in the joint space based on the first trajectory and the first fusion radius associated with the first trajectory;

Taking the fusion starting point as the center, select a set number of first trajectory points in the first trajectory;

Map the fusion starting point and the first trajectory point to a Cartesian space, and perform spline fitting on the fusion starting point and the first trajectory point mapped to the Cartesian space to obtain a fitting curve in the Cartesian space;

According to the fitting curve, the Cartesian motion information of the fusion starting point in the Cartesian space is determined; the Cartesian motion information includes the posture, velocity and acceleration of the Cartesian space.
The method according to claim 1, before performing trajectory planning on the fusion segment between the first trajectory and the second trajectory, further comprising:

According to the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point, pre-planning is performed on the fusion road section to determine the speed and acceleration of multiple fusion trajectory points in the fusion section;

In response to determining that the speed of at least one fused trajectory point in the fusion segment reaches a speed threshold and the acceleration of at least one trajectory point reaches at least one of the acceleration thresholds, determining a speed scaling factor based on the speed and acceleration;

The trajectory planning of the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point includes:

Trajectory planning is performed on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point, the Cartesian motion information of the fusion end point, and the speed scaling factor.
The method of claim 1, further comprising:

During the trajectory planning process, the interpolation period in the trajectory planning process is adjusted based on the speed control information input by the user.
The method according to claim 4, wherein adjusting the interpolation period in the trajectory planning process according to the speed control information input by the user includes:

According to the speed control information input by the user, at least one of the nonlinear interpolation algorithm, the proportional integral differential PID algorithm and the mean filter algorithm is used to adjust the interpolation period of the trajectory planning process.
The method according to claim 1, wherein the performing trajectory planning on the fusion segment between the first trajectory and the second trajectory includes:

In response to determining that the space in which at least one of the first trajectory and the second trajectory is located is a joint space, a polynomial is used to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory.
The method according to claim 1, wherein the performing trajectory planning on the fusion segment between the first trajectory and the second trajectory further includes:

In response to determining that the spaces where the first trajectory and the second trajectory are located are respectively Cartesian spaces, online trajectory planning OTG is used to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory.
A trajectory fusion device including:

The first motion information determination module is configured to determine the Cartesian motion information of the fusion starting point in Cartesian space based on the first trajectory and the first fusion radius associated with the first trajectory;

The second motion information determination module is configured to determine the Cartesian motion information of the fusion end point in Cartesian space based on the second trajectory and the first fusion radius; the space where the first trajectory is located is different from the space where the second trajectory is located;

The fusion segment trajectory planning module is configured to perform trajectory planning on the fusion segment between the first trajectory and the second trajectory based on the Cartesian motion information of the fusion starting point and the Cartesian motion information of the fusion end point.
An electronic device including:

at least one processor; and

a memory communicatively connected to the at least one processor; wherein,

The memory stores a computer program executable by the at least one processor, the computer program being executed by the at least one processor, so that the at least one processor can execute any one of claims 1-7 The trajectory fusion method described.
A computer-readable storage medium, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are configured to enable a processor to implement the trajectory fusion method according to any one of claims 1-7 when executed.