阅读说明：本技术 基于实时运动的笛卡尔空间轨迹规划方法和系统 (Real-time motion-based Cartesian space trajectory planning method and system ) 是由 廖志祥 郭震 于 2021-09-10 设计创作，主要内容包括：本发明提供了一种基于实时运动的笛卡尔空间轨迹规划方法和系统,包括：步骤1：获取笛卡尔空间中的路径点；步骤2：根据笛卡尔空间中路径点的个数规划,生成第一段轨迹规划器；步骤3：根据运动周期,通过第一段轨迹规划器生成带有时刻的位置信息,并下发至被控对象；步骤4：获取当前轨迹规划器的规划时间,除以运动周期后取整再乘以运动周期,得到时间基座,若当前轨迹不是最后一段轨迹且运动时间比时间基座少一个运动周期时,生成下一段轨迹规划器；步骤5：判断当前轨迹为最后一段轨迹,且运动时间等于轨迹规划器的规划时间时,完成轨迹规划。本发明采用圆弧过渡匀速规划的方式,避免了被控对象出现速度突跳现象,保证被控对象运动顺滑性。(The invention provides a Cartesian space trajectory planning method and a Cartesian space trajectory planning system based on real-time motion, which comprise the following steps of: step 1: acquiring path points in a Cartesian space; step 2: planning according to the number of path points in the Cartesian space to generate a first section of track planner; and step 3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object; and 4, step 4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner; and 5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning. The invention adopts a mode of circular arc transition constant speed planning, avoids the phenomenon of sudden speed jump of the controlled object and ensures the smoothness of the movement of the controlled object.)

1. A Cartesian space trajectory planning method based on real-time motion is characterized by comprising the following steps:

step 1: acquiring path points in a Cartesian space through user input or automatic identification;

step 2: performing linear planning and arc transition constant speed planning according to the number of path points in the Cartesian space to generate a first section of track planner;

and step 3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object;

and 4, step 4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner;

and 5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning.

2. The real-time motion based cartesian space trajectory planning method according to claim 1, wherein the step 1 comprises: if the distance between two adjacent path points is not within the preset range, returning information which cannot be planned; and if the distance between two adjacent path points is within a preset range, storing all path points in the Cartesian space.

3. The real-time motion-based cartesian space trajectory planning method according to claim 1, characterized in that when there are only two cartesian space path points a and B, a straight line planning is adopted to calculate the straight line distance between the two path points, and then the maximum linear velocity v in cartesian space is set_maxAnd maximum linear acceleration a_maxObtaining all the tracks of the two path points by utilizing the trapezoidal speed track, and planning the track P formula from the point A to the point B as follows:

the speeds of the point A and the point B are both zero, OP is a track point at any moment, OA is a coordinate of the point A, AB is a line segment vector of the point A and the point B, and u is a planning value of the trapezoidal speed track planner.

4. The real-time motion-based cartesian space trajectory planning method according to claim 3, wherein when the number of cartesian space path points is greater than two, the second path point is taken as a circle center to be used as a spherical surface, an intersection point of a line segment AB formed by the spherical surface and the first and second path points and an intersection point of a line segment BC formed by the spherical surface and the second and third path points are obtained, and the radius of the circular arc and the angle of the B 'C' circular arc are obtained by calculation according to a condition that the circular arc segment is tangent to the line segment AB and the line segment BC, wherein the circular arc transition adopts a uniform velocity planning mode, and the linear velocity of the circular arc transition is:

wherein r is the radius of the sphere, points A and B 'are the path points of the first section of the trajectory planner, the velocity at point A is zero, and the velocity at point B' is v_lim。

5. The real-time motion based cartesian space trajectory planning method according to claim 1, wherein the step 4 comprises:

if the next section of path is an arc, obtaining the radius, the angle and the limiting speed of arc transition, wherein the position of the path planner in the previous section of path planner when the planning time is a time base is the initial position of the path planner in the next section of path;

if the next section of path is a straight line, the end point and the limiting speed of the next section of path are obtained, and then the Cartesian space straight line path planner is generated.

6. A cartesian space trajectory planning system based on real-time motion, comprising:

module M1: acquiring path points in a Cartesian space through user input or automatic identification;

module M2: performing linear planning and arc transition constant speed planning according to the number of path points in the Cartesian space to generate a first section of track planner;

module M3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object;

module M4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner;

module M5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning.

7. The real-time motion based cartesian space trajectory planning system according to claim 6, wherein said module M1 comprises: if the distance between two adjacent path points is not within the preset range, returning information which cannot be planned; and if the distance between two adjacent path points is within a preset range, storing all path points in the Cartesian space.

8. The real-time motion-based cartesian space trajectory planning system according to claim 6, wherein when there are only two cartesian space path points a and B, straight line planning is adopted to calculate the straight line distance between the two path points, and then the maximum linear velocity v in cartesian space is set_maxAnd maximum linear acceleration a_maxObtaining all the tracks of the two path points by utilizing the trapezoidal speed track, and planning the track P formula from the point A to the point B as follows:

the speeds of the point A and the point B are both zero, OP is a track point at any moment, OA is a coordinate of the point A, AB is a line segment vector of the point A and the point B, and u is a planning value of the trapezoidal speed track planner.

9. The real-time motion-based cartesian space trajectory planning system according to claim 8, wherein when the number of cartesian space path points is greater than two, the second path point is taken as a circle center to be used as a spherical surface, an intersection point of a line segment AB formed by the spherical surface and the first and second path points is obtained, an intersection point of a line segment BC formed by the spherical surface and the second and third path points is obtained, and an angle between a radius of the circular arc and a B 'C' circular arc is obtained by calculation according to a condition that the circular arc segment is tangent to the line segment AB and the line segment BC, wherein the circular arc transition adopts a uniform velocity planning mode, and a linear velocity of the circular arc transition is:

wherein r is the radius of the sphere, points A and B 'are the path points of the first section of the trajectory planner, the velocity at point A is zero, and the velocity at point B' is v_lim。

10. The real-time motion based cartesian space trajectory planning system according to claim 6, wherein said module M4 comprises:

if the next section of path is an arc, obtaining the radius, the angle and the limiting speed of arc transition, wherein the position of the path planner in the previous section of path planner when the planning time is a time base is the initial position of the path planner in the next section of path;

if the next section of path is a straight line, the end point and the limiting speed of the next section of path are obtained, and then the Cartesian space straight line path planner is generated.

Technical Field

The invention relates to the technical field of trajectory planning, in particular to a Cartesian space trajectory planning method and system based on real-time motion.

Background

In most cases, cartesian space trajectory planning is mainly based on a linear path, the tail end of the mechanical arm is interpolated linearly between different path points, the tail end of the mechanical arm decelerates to zero when moving to a certain position, then accelerates to the next position, ensures that the tail end also decelerates to zero when moving to the next position, and repeats the above actions continuously. However, for the situations that the number of path points is large and the distance between adjacent path points is small, the trajectory planning method has a large problem, frequent starting and stopping does not fully utilize the working capacity of the mechanical arm, the working efficiency of the mechanical arm is reduced, and the service life of a transmission system of the mechanical arm is seriously influenced.

Under some conditions, circular arc transition is added to a linear path in the Cartesian space trajectory planning, the sum of the distances between the linear path and the circular arc is used as the total distance of the trajectory planning, and then kinematics constraint is used for trajectory planning.

Patent document CN106313047B (application number: CN201610860966.1) discloses a real-time corner transition method for a robot based on Bezier splines, which includes the following steps: recording a joint space starting point, a coordinate of a transition point, a Cartesian space target point, a coordinate of the transition point, a transition radius R of a Cartesian space and maximum error constraint; calculating coordinates of a starting point and an end point of the Cartesian space transition curve and coordinates of a starting point of the joint space transition curve; calculating speed direction unit vectors of a starting point and an end point of a Cartesian space transition curve; calculating the coordinates of the middle control point of the Cartesian space transition curve, and solving the constructor of the Bezier spline curve; and finally, speed planning and interpolation are carried out.

Disclosure of Invention

Aiming at the defects in the prior art, the invention aims to provide a Cartesian space trajectory planning method and system based on real-time motion.

The Cartesian space trajectory planning method based on real-time motion provided by the invention comprises the following steps:

step 1: acquiring path points in a Cartesian space through user input or automatic identification;

step 2: performing linear planning and arc transition constant speed planning according to the number of path points in the Cartesian space to generate a first section of track planner;

and step 3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object;

and 4, step 4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner;

and 5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning.

Preferably, the step 1 comprises: if the distance between two adjacent path points is not within the preset range, returning information which cannot be planned; and if the distance between two adjacent path points is within a preset range, storing all path points in the Cartesian space.

Preferably, when only two Cartesian space path points A and B exist, linear planning is adopted to calculate the linear distance between the two path points, and then the maximum linear velocity v in the Cartesian space is set_maxAnd maximum linear acceleration a_maxObtaining all the tracks of the two path points by utilizing the trapezoidal speed track, and planning the track P formula from the point A to the point B as follows:

the speeds of the point A and the point B are both zero, OP is a track point at any moment, OA is a coordinate of the point A, AB is a line segment vector of the point A and the point B, and u is a planning value of the trapezoidal speed track planner.

Preferably, when the number of the cartesian space path points is greater than two, the second path point is taken as a circle center to be used as a spherical surface, an intersection point of the spherical surface and a line segment AB formed by the first and second path points and an intersection point of a line segment BC formed by the spherical surface, the second and third path points are obtained, and the radius of the circular arc and the angle of the circular arc of the B 'C' are obtained by calculating according to the condition that the circular arc is tangent to the line segment AB and the line segment BC, wherein the circular arc transition adopts a uniform speed planning mode, and the linear speed of the circular arc transition is as follows:

wherein r is the radius of the sphere, points A and B 'are the path points of the first section of the trajectory planner, the velocity at point A is zero, and the velocity at point B' is v_lim。

Preferably, the step 4 comprises:

if the next section of path is an arc, obtaining the radius, the angle and the limiting speed of arc transition, wherein the position of the path planner in the previous section of path planner when the planning time is a time base is the initial position of the path planner in the next section of path;

if the next section of path is a straight line, the end point and the limiting speed of the next section of path are obtained, and then the Cartesian space straight line path planner is generated.

The invention provides a Cartesian space trajectory planning system based on real-time motion, which comprises:

module M1: acquiring path points in a Cartesian space through user input or automatic identification;

module M2: performing linear planning and arc transition constant speed planning according to the number of path points in the Cartesian space to generate a first section of track planner;

module M3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object;

module M4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner;

module M5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning.

Preferably, the module M1 includes: if the distance between two adjacent path points is not within the preset range, returning information which cannot be planned; and if the distance between two adjacent path points is within a preset range, storing all path points in the Cartesian space.

Preferably, when only two Cartesian space path points A and B exist, linear planning is adopted to calculate the linear distance between the two path points, and then the maximum linear velocity v in the Cartesian space is set_maxAnd maximum linear acceleration a_maxObtaining all the tracks of the two path points by utilizing the trapezoidal speed track, and planning the track P formula from the point A to the point B as follows:

the speeds of the point A and the point B are both zero, OP is a track point at any moment, OA is a coordinate of the point A, AB is a line segment vector of the point A and the point B, and u is a planning value of the trapezoidal speed track planner.

Preferably, when the number of the cartesian space path points is greater than two, the second path point is taken as a circle center to be used as a spherical surface, an intersection point of the spherical surface and a line segment AB formed by the first and second path points and an intersection point of a line segment BC formed by the spherical surface, the second and third path points are obtained, and the radius of the circular arc and the angle of the circular arc of the B 'C' are obtained by calculating according to the condition that the circular arc is tangent to the line segment AB and the line segment BC, wherein the circular arc transition adopts a uniform speed planning mode, and the linear speed of the circular arc transition is as follows:

wherein r is the radius of the sphere, points A and B 'are the path points of the first section of the trajectory planner, the velocity at point A is zero, and the velocity at point B' is v_lim。

Preferably, the module M4 includes:

if the next section of path is an arc, obtaining the radius, the angle and the limiting speed of arc transition, wherein the position of the path planner in the previous section of path planner when the planning time is a time base is the initial position of the path planner in the next section of path;

if the next section of path is a straight line, the end point and the limiting speed of the next section of path are obtained, and then the Cartesian space straight line path planner is generated.

Compared with the prior art, the invention has the following beneficial effects:

(1) the invention avoids the frequent start and stop of the controlled object, improves the movement fluency and prolongs the service life of the controlled object;

(2) the invention gives full play to the speed performance of the controlled object and improves the working efficiency;

(3) the invention adopts a mode of circular arc transition constant speed planning, avoids the phenomenon of sudden speed jump of the controlled object and ensures the smoothness of the movement of the controlled object.

Drawings

Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:

FIG. 1 is a Cartesian space linear trajectory planning diagram;

FIG. 2 is a schematic view of a calculation of the arc transition midpoint;

FIG. 3 is a diagram of the effect of trajectory planning for two path points;

FIG. 4 is a graph of a trajectory planning speed curve for two path points;

FIG. 5 is a diagram of the effect of trajectory planning for four waypoints;

fig. 6 is a graph of trajectory planning speeds for four waypoints.

Detailed Description

The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.

Example (b):

the Cartesian space trajectory planning method based on real-time motion provided by the invention comprises the following steps of:

step 1: acquiring Cartesian space path points;

step 2: generating a first section of track planner;

and step 3: generating planning position information;

and 4, step 4: generating a next section of track planner;

and 5: and completing the trajectory planning.

The detailed content of the step 1 is as follows: the path points in the Cartesian space are obtained through user input or a system automatic identification method, and if the distance between two adjacent path points is too short, information which cannot be planned is returned; if the distance between adjacent path points is not too short, all the Cartesian space path points are saved and the step 2 is started.

Step 2 can be divided into two cases:

case 1: only two cartesian space path points are needed, and circular arc transition is not needed to be considered at the moment, and the straight line planning is directly carried out, as shown in fig. 1. Calculating the linear distance between two path points, setting the maximum linear velocity vmax and the maximum linear acceleration amax of the Cartesian space, setting the velocities of the point A and the point B to be zero, and obtaining all the paths of the two path points by utilizing the trapezoidal velocity path. The trajectory P for planning the movement from point A to point B is formulated as:

wherein, OP is a track point at any moment, OA is a point coordinate A, AB is a line segment vector, and u is a planning value of the trapezoidal speed track planner. Fig. one is a schematic diagram of cartesian space linear trajectory planning.

Case 2: the Cartesian space path points are greater than 2, and at least one circular arc transition path is included. Setting the sphere with the second path point as the center of circle and 5mm as the radius, the intersection point of the sphere and the line segment formed by the first and second path points can be obtained, and the intersection point of the sphere and the line segment formed by the second and third path points can be obtained, as shown in fig. 2.

The angle between the radius of the arc and the arc of the B 'C' can be calculated according to the condition that the arc segment is tangent to the line segment AB and the line segment BC, wherein the arc transition adopts a uniform velocity planning mode, and in order to meet the linear acceleration constraint condition, the linear velocity of the arc transition can be calculated at the moment:

at the moment, points A and B 'are taken as path points of the first section of track planner, the speed at the point A is zero, and the speed at the point B' is v_limThe first section of track planner can be generated by utilizing the Cartesian space straight-line track planning method.

The detailed content of the step 3 is as follows: and according to the running period of the real-time motion control system, the track planner generates the position information at the moment and transmits the position information to the controlled object along with the change of time.

The detailed content of the step 4 is as follows: and acquiring the planning time of the current track planner, dividing the planning time by the operation period of the real-time motion control system, rounding, multiplying the operation period by the operation period of the real-time motion control system, and calculating to obtain a time base tf _ int. When the current track is not the last track and the motion time is less than tf _ int by one running period of the real-time motion control system, a next track planner starts to be generated, and the next track planner can be divided into two conditions according to the difference of the next path:

case 1: and (3) obtaining the radius and the angle of the arc and the limiting speed of the arc transition by using the method in the step (2) when the next section of the path is the arc. The position of the last section of the path planner when the planning time is tf _ int is the starting position of the next section of the path planner.

Case 2: and (3) the next section of path is a straight line, the method in the step (2) can be used for obtaining the end point of the next section of path, limiting the speed and then generating the Cartesian space straight line path planner.

Details of step 5: and when the current track is the last section of track and the movement time is equal to the planning time of the track planner, the track planning is finished.

Specifically, the trajectory planning for two waypoints:

given any two cartesian spatial path points, a ═ 10.51]^T，B＝[-0.3 1.4 0]^TSetting the maximum linear velocity to be 0.4m/s and the maximum linear acceleration to be 0.9m/s, satisfying the condition 1 in the step 2, wherein the effect of planning the linear trajectory in the cartesian space is shown in fig. 3, and the velocity curve in the motion process is shown in fig. 4

Specifically, the trajectory of four waypoints is planned:

given any four cartesian spatial path points, a ═ 10.51]^T，B＝[-0.3 1.4 0]^T，C＝[-1.3 0.2 -0.6]^T，D＝[0.1 -0.4 0.8]^TSimilarly, the maximum linear velocity is set to be 0.4m/s, the maximum linear acceleration is set to be 0.9m/s, and the condition 2 in the step 2 is satisfied, at this time, the cartesian space trajectory planning effect is shown in fig. 5, and the velocity curve in the motion process is shown in fig. 6.

The invention provides a Cartesian space trajectory planning system based on real-time motion, which comprises: module M1: acquiring path points in a Cartesian space through user input or automatic identification; module M2: performing linear planning and arc transition constant speed planning according to the number of path points in the Cartesian space to generate a first section of track planner; module M3: generating position information with time through a first section of track planner according to the motion period, and issuing the position information to a controlled object; module M4: acquiring the planning time of the current trajectory planner, dividing the planning time by the movement period, rounding, and multiplying by the movement period to obtain a time base, and if the current trajectory is not the last trajectory and the movement time is less than the movement period of the time base, generating a next trajectory planner; module M5: and when the current track is judged to be the last section of track and the movement time is equal to the planning time of the track planner, finishing the track planning.

The module M1 includes: if the distance between two adjacent path points is not within the preset range, returning information which cannot be planned; if the distance between two adjacent path points is within the preset rangeAnd in the enclosure, all path points in the Cartesian space are saved. When only two Cartesian space path points A and B exist, linear planning is adopted to calculate the linear distance between the two path points, and then the maximum linear velocity v in the Cartesian space is set_maxAnd maximum linear acceleration a_maxObtaining all the tracks of the two path points by utilizing the trapezoidal speed track, and planning the track P formula from the point A to the point B as follows:

the speeds of the point A and the point B are both zero, OP is a track point at any moment, OA is a coordinate of the point A, AB is a line segment vector of the point A and the point B, and u is a planning value of the trapezoidal speed track planner.

When the number of the Cartesian space path points is more than two, a second path point is taken as a circle center to be used as a spherical surface, the intersection point of the spherical surface and a line segment AB formed by the first path point and the second path point and the intersection point of a line segment BC formed by the spherical surface, the second path point and the third path point are obtained, and the radius of the circular arc and the angle of the B 'C' circular arc are obtained by calculating according to the condition that the circular arc is tangent to the line segment AB and the line segment BC, wherein the circular arc transition adopts a mode of uniform speed planning, and the linear speed of the circular arc transition is as follows:

wherein r is the radius of the sphere, points A and B 'are the path points of the first section of the trajectory planner, the velocity at point A is zero, and the velocity at point B' is v_lim。

The module M4 includes: if the next section of path is an arc, obtaining the radius, the angle and the limiting speed of arc transition, wherein the position of the path planner in the previous section of path planner when the planning time is a time base is the initial position of the path planner in the next section of path; if the next section of path is a straight line, the end point and the limiting speed of the next section of path are obtained, and then the Cartesian space straight line path planner is generated.

Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.

The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.