Trajectory smooth transition method, device and storage medium

Abstract

The present invention discloses a trajectory smooth transition method, device and storage medium, which relates to the field of automation control, wherein the method comprises: obtaining preset initial time data, linear distance data corresponding to the motion path to be processed, target coordinate data and unit direction vector data according to preset speed parameter data; performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data according to the preset initial time data to obtain current one-dimensional position trajectory data; inputting the current one-dimensional position trajectory data, target coordinate data and unit direction vector data into the corresponding trapezoidal speed superposition algorithm model to obtain current three-dimensional position trajectory data; obtaining a trajectory smooth transition path corresponding to the motion path to be processed according to the current position vector data and the current speed vector data. The trajectory smooth transition method of the present invention realizes the smooth transition of the position and speed of the machine equipment at the junction of the continuous straight line trajectory, and improves the working efficiency of the machine equipment.

Description

Technical Field

[0001] The present invention relates to the field of automation control, and in particular to a trajectory smooth transition method, device and storage medium. Background Art

[0002] In the field of automated control, machines and equipment such as robots often use a combination of multiple segments and paths when performing tasks. If no processing is done at the junction of continuous straight lines, the robot's speed needs to be reduced to zero. If the robot's speed is not reduced to zero, the robot will experience violent vibrations due to the discontinuity of the speed, and it is very likely to damage the robot. However, if the robot's speed is reduced to zero, it will affect the robot's rhythm, especially when there are many path segments.

[0003] At present, in order to enable the robot to move continuously on different paths, a trajectory planning algorithm is used to control the robot's movement to avoid frequent start and stop of the robot. However, in the related art, the robot movement path obtained by the trajectory planning algorithm has a speed jump at the transition section of the continuous straight line and the connection of the trajectory, which affects the smoothness of the trajectory and can seriously cause mechanical vibration, resulting in low working efficiency of the robot and failure to achieve smooth transition. Summary of the invention

[0004] The present invention aims to solve at least one of the technical problems existing in the prior art. To this end, the present invention proposes a trajectory smooth transition method, device and storage medium, which realizes the smooth transition of the position and speed of the machine equipment at the junction of continuous straight line trajectories, thereby improving the working efficiency of the machine equipment.

[0005] According to a first aspect of the present invention, a trajectory smoothing method includes:

[0006] According to the preset speed parameter data, the preset initial time data, the linear distance data corresponding to the motion path to be processed, the target coordinate data and the unit direction vector data are obtained;

[0007] According to the preset initial time data, the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing to obtain current one-dimensional position trajectory data, wherein the current one-dimensional position trajectory data includes current one-dimensional position data and current one-dimensional speed data;

[0008] Inputting the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data into a corresponding trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data, wherein the current three-dimensional position trajectory data includes current position vector data and current velocity vector data;

[0009] Obtaining a trajectory smooth transition path corresponding to the motion path to be processed according to the current position vector data and the current velocity vector data;

[0010] The preset initial time data is the time data corresponding to the preset connection point, the trajectory smooth transition path includes a first trajectory and a second trajectory, and the first trajectory intersects with the second trajectory at the preset connection point.

[0011] According to one or more technical solutions provided in the embodiments of the present invention, at least the following beneficial effects are achieved: the present invention obtains the current position vector data and the current velocity vector data through a trapezoidal velocity superposition algorithm model, obtains the trajectory smooth transition path corresponding to the motion path to be processed according to the current position vector data and the current velocity vector data, and controls the machine equipment to perform the trajectory motion corresponding to the trajectory smooth transition path according to the trajectory smooth transition path. This trajectory smooth transition method realizes the smooth transition of the position and velocity of the machine equipment at the junction of continuous straight line trajectories, thereby improving the working efficiency of the machine equipment.

[0012] According to some embodiments of the present invention, the trapezoidal trajectory processing includes trapezoidal trajectory planning processing and trajectory generation processing, and the trapezoidal trajectory processing is performed on the linear distance data and the preset speed parameter data to obtain the current one-dimensional position trajectory data, including:

[0013] Performing trapezoidal trajectory planning processing on the linear distance data and the preset speed parameter data to obtain current trajectory coefficient data;

[0014] Perform trajectory generation processing on the current trajectory coefficient data to obtain current one-dimensional position trajectory data.

[0015] According to some embodiments of the present invention, the motion path to be processed includes a first path, and the first trajectory has the same starting point as the first path, and the step of performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data according to the preset initial time data to obtain current one-dimensional position trajectory data includes:

[0016] When the current time data corresponding to the motion path to be processed is less than or equal to the preset initial time data, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path;

[0017] Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, including:

[0018] The first current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a first trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

[0019] According to some embodiments of the present invention, the motion path to be processed includes a first path and a second path, the first path and the second path intersect at a junction point, and the first trajectory is the same as the starting point of the first path, and the second trajectory is the same as the end point of the second path, and the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing according to the preset initial time data to obtain current one-dimensional position trajectory data, including:

[0020] When the current time data corresponding to the motion path to be processed is between the preset initial time data and the end time data of the first path, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path and second current one-dimensional position trajectory data corresponding to the second path;

[0021] Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, including:

[0022] The first current one-dimensional position trajectory data, the second current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a second trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

[0023] According to some embodiments of the present invention, the motion path to be processed includes a second path, and the second trajectory is the same as the end point of the second path, and the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing according to the preset initial time data to obtain current one-dimensional position trajectory data, including:

[0024] When the current time data corresponding to the motion path to be processed is between the end time data of the first path and the end time data of the second path, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain second current one-dimensional position trajectory data corresponding to the second path;

[0025] Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, further comprising:

[0026] The second current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a third trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

[0027] According to some embodiments of the present invention, the calculation formula of the first trapezoidal velocity superposition algorithm model is as follows:

[0028]

[0029] in, represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional speed data corresponding to the first path, represents the coordinate data of the starting point corresponding to the first path, represents a first unit direction vector, the target coordinate data includes the starting point coordinate data, and the unit direction vector data includes the first unit direction vector.

[0030] According to some embodiments of the present invention, the calculation formula of the second trapezoidal velocity superposition algorithm model is as follows:

[0031]

[0032] in, represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional speed data corresponding to the first path, p 2·pos represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, represents the coordinate data of the starting point corresponding to the first path, represents the first unit direction vector, represents a second unit direction vector, the unit direction vector data includes the first unit direction vector and the second unit direction vector, and the target coordinate data includes the starting point coordinate data.

[0033] According to some embodiments of the present invention, the calculation formula of the third trapezoidal velocity superposition algorithm model is as follows:

[0034]

[0035] in, represents the current position vector data, Represents the current velocity vector data, p 2·pos represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, represents the second unit direction vector, Represents the middle point coordinate data corresponding to the motion path to be processed, the target coordinate data includes the middle point coordinate data, and the unit direction vector data includes the second unit direction vector.

[0036] According to the second aspect of the present invention, a trajectory smoothing transition device comprises: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the trajectory smoothing transition method as described in the first aspect above when executing the computer program.

[0037] According to the computer-readable storage medium of the embodiment of the third aspect of the present invention, the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the trajectory smoothing transition method as described in the first aspect above.

[0038] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the description, claims and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The accompanying drawings are used to provide a further understanding of the technical solution of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the technical solution of the present invention and do not constitute a limitation to the technical solution of the present invention.

[0040] Figure 1 is a schematic flow chart of a trajectory smoothing transition method provided by an embodiment of the present invention;

[0041] Figure 2 is a schematic diagram of a process flow of a trapezoidal trajectory processing provided by an embodiment of the present invention;

[0042] Figure 3 It is a flow chart of the case where the current time data provided by the embodiment of the present invention is less than or equal to the preset initial time data;

[0043] Figure 4 is a flow chart of the current time data provided by an embodiment of the present invention when it is between the preset initial time data and the end time data of the first path;

[0044] Figure 5 is a flow chart of the case where the current time data provided by an embodiment of the present invention is between the end time data of the first path and the end time data of the second path;

[0045] Figure 6 It is a schematic diagram of two trajectories provided in an embodiment of the present invention without trapezoidal speed superposition;

[0046] Figure 7 It is a schematic diagram of trapezoidal speed superposition of two trajectories provided by an embodiment of the present invention;

[0047] Figure 8 is a schematic diagram of a smoothing series provided by an embodiment of the present invention;

[0048] Fig. 9 is a schematic diagram of comparison before and after the smooth transition of the trajectory provided by an embodiment of the present invention;

[0049] Fig.10 It is a schematic diagram of a three-stage speed trajectory provided by an embodiment of the present invention;

[0050] Fig.11 is a schematic diagram of a two-stage speed trajectory provided by an embodiment of the present invention;

[0051] Fig.12 It is a schematic diagram of three-stage speed trajectory planning for all motion paths to be processed in the present invention;

[0052] Fig.13 It is a schematic diagram of sequentially performing three-stage and two-stage speed trajectory planning on the motion path to be processed in the present invention;

[0053] Fig.14 It is a schematic diagram of sequentially performing two-stage and three-stage speed trajectory planning on the motion path to be processed in the present invention;

[0054] Fig.15 It is a schematic diagram of two-stage speed trajectory planning for both trajectories provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0056] It should be noted that, although the functional modules are divided in the device schematic diagram and the logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the device or the order in the flowchart. The terms "first", "second", etc. in the specification, claims and the above drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence.

[0057] In the field of automated control, machines and equipment such as robots often use a combination of multiple segments and paths when performing tasks. If no processing is done at the junction of continuous straight lines, the robot's speed needs to be reduced to zero. If the robot's speed is not reduced to zero, the robot will experience violent vibrations due to the discontinuity of the speed, and it is very likely to damage the robot. However, if the robot's speed is reduced to zero, it will affect the robot's rhythm, especially when there are many path segments.

[0058] At present, in order to enable the robot to move continuously on different paths, a trajectory planning algorithm is used to control the robot's movement to avoid frequent start and stop of the robot. However, in the related art, the robot movement path obtained by the trajectory planning algorithm has a speed jump at the transition section of the continuous straight line and the connection of the trajectory, which affects the smoothness of the trajectory and can seriously cause mechanical vibration, resulting in low working efficiency of the robot and failure to achieve smooth transition.

[0059] Based on this, an embodiment of the present invention provides a trajectory smooth transition method, device and storage medium, which realizes the smooth transition of the position and speed of the machine equipment at the transition section of the continuous straight line and the connection point of the trajectory, thereby improving the working efficiency of the machine equipment.

[0060] The embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0061] The first aspect of the present invention specifically provides a trajectory smooth transition method, such as Figure 1 As shown, Figure 1 A schematic diagram of a flow chart of a trajectory smoothing transition method provided by an embodiment of the present invention. The trajectory smoothing transition method of an embodiment of the present invention includes but is not limited to the following steps:

[0062] Step S100, obtaining preset initial time data, linear distance data corresponding to the motion path to be processed, target coordinate data and unit direction vector data according to preset speed parameter data;

[0063] Step S200, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data according to the preset initial time data to obtain current one-dimensional position trajectory data, wherein the current one-dimensional position trajectory data includes current one-dimensional position data and current one-dimensional speed data;

[0064] Step S300, inputting the current one-dimensional position trajectory data, target coordinate data and unit direction vector data into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, wherein the current three-dimensional position trajectory data includes the current position vector data and the current velocity vector data;

[0065] Step S400, obtaining a trajectory smooth transition path corresponding to the motion path to be processed according to the current position vector data and the current velocity vector data;

[0066] Among them, the preset initial time data is the time data corresponding to the preset connection point, the trajectory smooth transition path includes the first trajectory and the second trajectory, and the first trajectory intersects the second trajectory at the preset connection point, and the preset speed parameter data includes the limit linear speed and the limit acceleration.

[0067] The present invention obtains current position vector data and current velocity vector data through a trapezoidal velocity superposition algorithm model, obtains a trajectory smooth transition path corresponding to a motion path to be processed according to the current position vector data and the current velocity vector data, and controls the machine equipment to perform trajectory motion corresponding to the trajectory smooth transition path according to the trajectory smooth transition path. This trajectory smooth transition method realizes the smooth transition of the position and velocity of the machine equipment at the junction of continuous straight line trajectories, thereby improving the working efficiency of the machine equipment.

[0068] It should be noted that the target coordinate data includes the starting point coordinate data corresponding to the first path, the intermediate point coordinate data corresponding to the motion path to be processed, and the end point coordinate data corresponding to the second path. The linear distance data is obtained by the following steps:

[0069] Obtain the starting point coordinate data, the middle point coordinate data and the end point coordinate data;

[0070] According to the starting point coordinate data, the middle point coordinate data and the end point coordinate data, the linear distance data is calculated. The calculation formula of the linear distance data is as follows:

[0071]

[0072] Among them, s1 represents the linear distance data between the starting point and the middle point, and s2 represents the linear distance data between the middle point and the end point. represents the starting point coordinate data corresponding to the starting point of the first path, Indicates the coordinate data of the middle point corresponding to the middle point of the motion path to be processed, It represents the endpoint coordinate data corresponding to the endpoint of the second path, and norm() represents the modulus function.

[0073] The unit direction vector data includes a first unit direction vector and a second unit direction vector, and the unit direction vector data is obtained by the following steps:

[0074] Based on the starting point coordinate data, intermediate point coordinate data, end point coordinate data, and linear distance data, the unit direction vector data is calculated. Among them, the calculation formula for the unit direction vector data is as follows:

[0075]

[0076] Among them, represents the first unit direction vector, represents the second unit direction vector. Referring to Fig. 9 , the first unit direction vector is the unit direction vector of , and the second unit direction vector is the unit direction vector of .

[0077] It should be noted that the preset speed parameter data includes a limit linear speed and a limit acceleration, and the preset speed parameter data is the preset speed parameter data for one-dimensional trajectory planning set in advance.

[0078] It should be noted that referring to Figure 6 , Figure 7 and Fig. 9 , the to-be-processed motion path includes a set of to-be-processed path combinations. The to-be-processed path combination includes a first path and a second path. For example, in Fig. 9 , the to-be-processed motion path is a straight-line path AC, the first path is a straight-line path AB, and the second path is a straight-line path BC. The trajectory smoothing transition method is used for the to-be-processed motion path to obtain a trajectory smoothing transition path corresponding to the to-be-processed path. When the trapezoidal planning of the first path enters the deceleration stage but the speed has not yet decreased to zero, that is, as shown in Figure 7 , when t2 ≤ t < t3, the trajectory smoothing transition of the second trajectory of the trajectory smoothing transition path is started, which can ensure that the speed vector data does not exceed the set preset speed parameter data.

[0079] In this embodiment, the trajectory smoothing method is applied to a robot arm. One of the main tasks of the robot arm is to execute the motion instruction of the end of the robot arm, that is, given the starting point and the end point of the workspace of the end of the robot arm, the end of the robot arm is allowed to move in a straight path or other shapes in the workspace. The trajectory smoothing method of this embodiment obtains a trajectory smoothing path corresponding to the motion path to be processed of the end of the robot arm. According to the trajectory smoothing path, the end of the robot arm is controlled to perform a trajectory motion corresponding to the trajectory smoothing path, so as to achieve a smooth transition of the position and speed of the end of the robot arm at the junction of continuous straight line trajectories, thereby improving the working efficiency of the end of the robot arm. The trajectory smoothing method can also be applied to robots or other machines and equipment.

[0080] It is understandable that, referring to Figure 2 The trapezoidal trajectory processing includes trapezoidal trajectory planning processing and trajectory generation processing, step S200, including but not limited to the following steps:

[0081] Step S201, performing trapezoidal trajectory planning processing on the linear distance data and the preset speed parameter data according to the preset initial time data to obtain the current trajectory coefficient data;

[0082] Step S202, performing trajectory generation processing on the current trajectory coefficient data to obtain current one-dimensional position trajectory data.

[0083] It should be noted that, refer to Fig.10 and Fig.11 , the trapezoidal trajectory speed data is obtained through trapezoidal trajectory planning and processing, and the trapezoidal trajectory speed data includes the maximum speed and the maximum acceleration. Then, the acceleration path end time data, the uniform speed path end time data, the deceleration path end time data and the current trajectory coefficient data corresponding to the first path or the second path in the motion path to be processed are obtained through the trapezoidal trajectory speed data. The speed trajectory type can also be obtained through the trapezoidal trajectory speed data. The speed trajectory type indicates whether the speed trajectory is a two-stage speed trajectory or a three-stage speed trajectory.

[0084] Reference Figures 12 to 15 ,like Fig.15 As shown, both the first path and the second path adopt two-stage velocity trajectory planning to obtain corresponding trajectory smooth transition paths, wherein both the first path and the second path include an acceleration path and a deceleration path; Fig.12 As shown, the first path and the second path both adopt three-stage velocity trajectory planning to obtain corresponding trajectory smooth transition paths, wherein the first path and the second path both include an acceleration path, a uniform speed path and a deceleration path; Fig.13As shown, the first path adopts three-stage speed trajectory planning and the second path adopts two-stage speed trajectory planning to obtain the corresponding trajectory smooth transition path, wherein the first path includes an acceleration path, a uniform speed path and a deceleration path, and the second path includes an acceleration path and a deceleration path; Fig.14 As shown, the first path adopts two-stage speed trajectory planning and the second path adopts three-stage speed trajectory planning to obtain the corresponding trajectory smooth transition path, wherein the first path includes an acceleration path and a deceleration path, the second path includes an acceleration path, a uniform speed path and a deceleration path, and the first path and the second path are compared at the connection point. The first path is the path section before the connection point, and the second path is the path section after the connection point.

[0085] When the first path or the second path adopts three-stage velocity trajectory planning, the calculation formula of the current trajectory coefficient data is as follows:

[0086] When the current time data corresponding to the motion path to be processed is less than or equal to the end time data of the acceleration path, When the current time data corresponding to the motion path to be processed is between the end time data of the acceleration path and the end time data of the uniform speed path, When the current time data corresponding to the motion path to be processed is between the end time data of the uniform speed path and the end time data of the deceleration path, When the first path or the second path adopts a two-stage speed trajectory planning, that is, when t1=t2, skip the uniform speed path and enter the deceleration path from the acceleration path. The calculation formulas of S1 and S3 are also applicable to the two-stage speed trajectory. For S1, S2 and S3, they can be expressed as the following calculation formula: S=m1+m2t+m3t 2 . Among them, S1 represents the displacement data corresponding to the acceleration path, S2 represents the displacement data corresponding to the uniform speed path, S3 represents the displacement data corresponding to the deceleration path, t represents the current time data, t1 represents the end time data of the acceleration path, t2 represents the end time data of the uniform speed path, t3 represents the end time data of the deceleration path, a represents the maximum acceleration, that is, the limit acceleration data in the preset speed parameter data, and m1, m2 and m3 are all parameter data. The current trajectory coefficient data is the parameter data corresponding to the displacement expression of the acceleration path, uniform speed path and deceleration path. For example, the current trajectory coefficient data includes a1, a2, a3, b1, b2, b3, c1, c2, c3, a1, a2 and a3 correspond to m1, m2 and m3 in S1 respectively, b1, b2 and b3 correspond to m1, m2 and m3 in S2 respectively, and c1, c2 and c3 correspond to m1, m2 and m3 in S3 respectively.

[0087] It should be noted that the current trajectory coefficient data is subjected to trajectory generation processing in each time period, that is, the calculation formula of the current one-dimensional position trajectory data is as follows:

[0088] When the current time data corresponding to the motion path to be processed is less than or equal to the end time data of the acceleration path, p ·pos =a1+a2×t+a3×t 2 , p ·vel =a2+2×a3×t; when the current time data corresponding to the motion path to be processed is between the end time data of the acceleration path and the end time data of the uniform speed path, p ·pos =b1+b2×t+b3×t 2 , p ·vel =b2+2×b3×t; when the current time data corresponding to the motion path to be processed is between the end time data of the uniform speed path and the end time data of the deceleration path, p ·pos =c1+c2×t+c3×t 2 , p ·vel =c2+2×c3×t; where p ·pos Represents the current one-dimensional position data, p ·vel Represents the current one-dimensional velocity data.

[0089] For example, refer to Fig. 9 , perform trapezoidal trajectory planning and trajectory generation processing on the straight path AB and the straight path BC respectively, input the current trajectory coefficient data and the current time data corresponding to the motion path to be processed into the calculation formula of the trajectory generation processing, and obtain the first current one-dimensional position data p corresponding to the first path 1·pos and the second current one-dimensional position data p corresponding to the second path 2·pos , and the first current one-dimensional velocity data p corresponding to the first path is also obtained 1·vel and the second current one-dimensional velocity data p corresponding to the second path 2·vel .

[0090] It is understandable that, referring to Figure 3 , the motion path to be processed includes a first path, and the first trajectory has the same starting point as the first path; step S200 includes but is not limited to the following steps:

[0091] Step S210, when the current time data corresponding to the motion path to be processed is less than or equal to the preset initial time data, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path;

[0092] Correspondingly, step S300 includes but is not limited to the following steps:

[0093] Step S310, inputting the first current one-dimensional position trajectory data, target coordinate data and unit direction vector data into a first trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

[0094] It should be noted that the trapezoidal velocity superposition algorithm model includes a first trapezoidal velocity superposition algorithm model; the first current one-dimensional position trajectory data includes first current one-dimensional position data and first current one-dimensional velocity data; the current three-dimensional position trajectory data obtained by the first trapezoidal velocity superposition algorithm model is the current three-dimensional position trajectory data corresponding to the first trajectory.

[0095] In some embodiments, the motion path to be processed includes a group of path combinations to be processed, the path combination to be processed includes a first path and a second path, the first path and the second path intersect at a junction, and the corresponding trajectory smooth transition path includes a group of trajectory combinations, the trajectory combination includes a first trajectory and a second trajectory, and the first trajectory intersects with the second trajectory at a preset junction point, and the first trajectory has the same starting point as the first path, and the second trajectory has the same end point as the second path; in other embodiments, the motion path to be processed includes at least two groups of path combinations to be processed, each of which includes a first path and a second path, and the first path and the second path intersect at a junction. It can be understood that each of the two paths intersecting at a junction in the motion path to be processed can be used as a group of path combinations to be processed, and the motion path to be processed uses a trajectory smooth transition method so that each path combination to be processed in the motion path to be processed corresponds to a group of trajectory combinations, that is, the path combination to be processed corresponds to the trajectory combination one by one.

[0096] It is understandable that, referring to Figure 4 The motion path to be processed includes a first path and a second path, the first path and the second path intersect at a junction point, and the first trajectory has the same starting point as the first path, and the second trajectory has the same end point as the second path, step S200 includes but is not limited to the following steps:

[0097] Step S220, when the current time data corresponding to the motion path to be processed is between the preset initial time data and the end time data of the first path, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path and second current one-dimensional position trajectory data corresponding to the second path;

[0098] Correspondingly, step S300 includes but is not limited to the following steps:

[0099] Step S320, inputting the first current one-dimensional position trajectory data, the second current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data into the second trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data.

[0100] It should be noted that the trapezoidal velocity superposition algorithm model includes the second trapezoidal velocity superposition algorithm model; the first current one-dimensional position trajectory data includes the first current one-dimensional position data and the first current one-dimensional velocity data, and the second current one-dimensional position trajectory data includes the second current one-dimensional position data and the second current one-dimensional velocity data; the current three-dimensional position trajectory data obtained through the second trapezoidal velocity superposition algorithm model is the current three-dimensional position trajectory data corresponding to the second trajectory.

[0101] It can be understood that with reference to Figure 5 , the motion path to be processed includes a second path, and the end points of the second trajectory and the second path are the same. Step S200 includes, but is not limited to, the following steps:

[0102] Step S230, when the current time data corresponding to the motion path to be processed is between the end time data of the first path and the end time data of the second path, perform trapezoidal trajectory processing on the linear distance data and the preset velocity parameter data to obtain the second current one-dimensional position trajectory data corresponding to the second path;

[0103] Correspondingly, step S300 includes, but is not limited to, the following steps:

[0104] Step S330, input the second current one-dimensional position trajectory data, the target coordinate data, and the unit direction vector data into the third trapezoidal velocity superposition algorithm model to obtain the third current three-dimensional position trajectory data corresponding to the second trajectory.

[0105] It should be noted that the trapezoidal velocity superposition algorithm model includes the third trapezoidal velocity superposition algorithm model; the second current one-dimensional position trajectory data includes the second current one-dimensional position data and the second current one-dimensional velocity data; the current three-dimensional position trajectory data obtained through the third trapezoidal velocity superposition algorithm model is the current three-dimensional position trajectory data corresponding to the second trajectory.

[0106] It should be noted that with reference to Figure 8 , T1 represents the preset initial time data, T2 represents the end time data of the first path, and the selection rule for the preset initial time data is: when the trapezoidal planning of the first path is in the deceleration stage, that is, when t1 < t < t2, divide the deceleration stage time into N equal parts. If N = 5, when T1 is at the position where 5 is located in Figure 8 , it is called five-level smoothing, or full-level smoothing; when T1 is at the position where 0 is located in the figure, it is called zero-level smoothing, that is, no trapezoidal velocity superposition is performed; for example, as Figure 8As shown, T1 is located at the position of 4, which is called level 4 smoothing; the level of smoothing is actually the percentage of the length from T1 to T2 to the length from t1 to t2. If percent = 4 / 5, it can be seen that the length from t1 to t2 is divided into five parts, which is called level 4 smoothing; if percent = 8 / 10, it can be seen that the length from t1 to t2 is divided into ten parts, which is called level 8 smoothing. It can be understood that the level 4 smoothing and level 8 smoothing here are equivalent, because T1 is at the same time point. Therefore, the level of smoothing is relative and mainly depends on the choice of N.

[0107] It can be understood that when the current time data corresponding to the motion path to be processed is less than or equal to the preset initial time data, the calculation formula of the first trapezoidal velocity superposition algorithm model is as follows:

[0108]

[0109] in, Represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional speed data corresponding to the first path, Indicates the coordinate data of the starting point corresponding to the first path, represents a first unit direction vector, the target coordinate data includes the starting point coordinate data, and the unit direction vector data includes the first unit direction vector.

[0110] It can be understood that when the current time data corresponding to the motion path to be processed is between the preset initial time data and the end time data of the first path, the calculation formula of the second trapezoidal velocity superposition algorithm model is as follows:

[0111]

[0112] in, Represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional velocity data corresponding to the first path, p 2·pos represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, Indicates the coordinate data of the starting point corresponding to the first path, represents the first unit direction vector, Represents a second unit direction vector, the unit direction vector data includes the first unit direction vector and the second unit direction vector, and the target coordinate data includes the starting point coordinate data.

[0113] It can be understood that when the current time data corresponding to the motion path to be processed is between the end time data of the first path and the end time data of the second path, the calculation formula of the third trapezoidal velocity superposition algorithm model is as follows:

[0114]

[0115] in, Represents the current position vector data, represents the current velocity vector data, represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, represents the second unit direction vector, It represents the coordinate data of the middle point corresponding to the motion path to be processed, the target coordinate data includes the coordinate data of the middle point, and the unit direction vector data includes the second unit direction vector.

[0116] In addition, the second aspect of the present invention further provides a trajectory smooth transition device, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor.

[0117] The processor and the memory may be connected via a bus or other means.

[0118] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely disposed relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0119] The non-transient software program and instructions required to implement the trajectory smooth transition method of the first aspect of the embodiment are stored in the memory. When executed by the processor, the trajectory smooth transition method in the above embodiment is executed, for example, the above described Figure 1 The method steps S100 to S400, Figure 2 Steps S201 to S202 of the method, Figure 3 Steps S210 and S310 of the method, Figure 4Steps S220 and S320 of the method, Figure 5 Method steps S230 and S330 in.

[0120] The device embodiments described above are merely illustrative, and the units described as separate components may or may not be physically separated, that is, they may fall into one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0121] In addition, an embodiment of the present invention further provides a computer-readable storage medium, which stores computer-executable instructions. The computer-executable instructions are executed by a processor or a controller, for example, by a processor in the above-mentioned device embodiment, so that the above-mentioned processor can execute the trajectory smoothing transition method in the above-mentioned embodiment, for example, execute the above-mentioned Figure 1 The method steps S100 to S400, Figure 2 Steps S201 to S202 of the method, Figure 3 Steps S210 and S310 of the method, Figure 4 Steps S220 and S320 of the method, Figure 5 Method steps S230 and S330 in.

[0122] It will be appreciated by those skilled in the art that all or some of the steps and systems in the disclosed method above may be implemented as software, firmware, hardware and appropriate combinations thereof. Some physical components or all physical components may be implemented as software executed by a processor, such as a central processing unit, a digital signal processor or a microprocessor, or may be implemented as hardware, or may be implemented as an integrated circuit, such as an application specific integrated circuit. Such software may be distributed on a computer-readable medium, which may include a computer storage medium (or a non-transitory medium) and a communication medium (or a temporary medium). As known to those skilled in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technologies, CD-ROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tapes, disk storage or other magnetic storage devices, or any other medium that may be used to store desired information and may be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0123] The above is a specific description of the preferred implementation of the present invention, but the present invention is not limited to the above-mentioned implementation mode. Technical personnel familiar with the field can also make various equivalent deformations or substitutions without violating the spirit of the present invention. These equivalent deformations or substitutions are all included in the scope defined by the claims of the present invention.

Claims

1. A trajectory smooth transition method, characterized in that: include: According to the preset speed parameter data, the preset initial time data, the linear distance data corresponding to the motion path to be processed, the target coordinate data and the unit direction vector data are obtained; According to the preset initial time data, the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing to obtain current one-dimensional position trajectory data, wherein the current one-dimensional position trajectory data includes current one-dimensional position data and current one-dimensional speed data; Inputting the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data into a corresponding trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data, wherein the current three-dimensional position trajectory data includes current position vector data and current velocity vector data; Obtaining a trajectory smooth transition path corresponding to the motion path to be processed according to the current position vector data and the current velocity vector data; The preset initial time data is the time data corresponding to the preset connection point, the trajectory smooth transition path includes a first trajectory and a second trajectory, and the first trajectory intersects with the second trajectory at the preset connection point.

2. The trajectory smooth transition method according to claim 1, characterized in that: The trapezoidal trajectory processing includes trapezoidal trajectory planning processing and trajectory generation processing, and the trapezoidal trajectory processing is performed on the linear distance data and the preset speed parameter data to obtain current one-dimensional position trajectory data, including: Performing trapezoidal trajectory planning processing on the linear distance data and the preset speed parameter data to obtain current trajectory coefficient data; Perform trajectory generation processing on the current trajectory coefficient data to obtain current one-dimensional position trajectory data.

3. The trajectory smooth transition method according to claim 1 or 2, characterized in that: The motion path to be processed includes a first path, and the first trajectory has the same starting point as the first path, and the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing according to the preset initial time data to obtain current one-dimensional position trajectory data, including: When the current time data corresponding to the motion path to be processed is less than or equal to the preset initial time data, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path; Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, including: The first current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a first trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

4. The trajectory smooth transition method according to claim 1 or 2, characterized in that: The motion path to be processed includes a first path and a second path, the first path and the second path intersect at a junction point, and the first trajectory has the same starting point as the first path, and the second trajectory has the same end point as the second path, and the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing according to the preset initial time data to obtain current one-dimensional position trajectory data, including: When the current time data corresponding to the motion path to be processed is between the preset initial time data and the end time data of the first path, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain first current one-dimensional position trajectory data corresponding to the first path and second current one-dimensional position trajectory data corresponding to the second path; Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, including: The first current one-dimensional position trajectory data, the second current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a second trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

5. The trajectory smooth transition method according to claim 1 or 2, characterized in that: The motion path to be processed includes a second path, and the second trajectory is the same as an end point of the second path, and the linear distance data and the preset speed parameter data are subjected to trapezoidal trajectory processing according to the preset initial time data to obtain current one-dimensional position trajectory data, including: When the current time data corresponding to the motion path to be processed is between the end time data of the first path and the end time data of the second path, performing trapezoidal trajectory processing on the linear distance data and the preset speed parameter data to obtain second current one-dimensional position trajectory data corresponding to the second path; Correspondingly, the current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into the corresponding trapezoidal velocity superposition algorithm model to obtain the current three-dimensional position trajectory data, further comprising: The second current one-dimensional position trajectory data, the target coordinate data and the unit direction vector data are input into a third trapezoidal velocity superposition algorithm model to obtain current three-dimensional position trajectory data.

6. The trajectory smooth transition method according to claim 3, characterized in that: The calculation formula of the first trapezoidal velocity superposition algorithm model is as follows: in, represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional speed data corresponding to the first path, represents the coordinate data of the starting point corresponding to the first path, represents a first unit direction vector, the target coordinate data includes the starting point coordinate data, and the unit direction vector data includes the first unit direction vector.

7. The trajectory smooth transition method according to claim 4, characterized in that: The calculation formula of the second trapezoidal velocity superposition algorithm model is as follows: in, represents the current position vector data, Represents the current velocity vector data, p 1·pos represents the first current one-dimensional position data corresponding to the first path, p 1·vel represents the first current one-dimensional speed data corresponding to the first path, p 2·pos represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, represents the coordinate data of the starting point corresponding to the first path, represents the first unit direction vector, represents a second unit direction vector, the unit direction vector data includes the first unit direction vector and the second unit direction vector, and the target coordinate data includes the starting point coordinate data.

8. The trajectory smooth transition method according to claim 5, characterized in that: The calculation formula of the third trapezoidal velocity superposition algorithm model is as follows: in, represents the current position vector data, Represents the current velocity vector data, p 2·pos represents the second current one-dimensional position data corresponding to the second path, p 2·vel represents the second current one-dimensional speed data corresponding to the second path, represents the second unit direction vector, Represents the middle point coordinate data corresponding to the motion path to be processed, the target coordinate data includes the middle point coordinate data, and the unit direction vector data includes the second unit direction vector.

9. A trajectory smooth transition device, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the trajectory smoothing transition method as claimed in any one of claims 1 to 8 when executing the computer program.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a computer to execute the trajectory smoothing transition method according to any one of claims 1 to 8.

Images