WO2024041646A1 - Trajectory planning method and apparatus for joint space of multi-shaft device - Google Patents

Abstract

A trajectory planning method for a joint space of a multi-shaft device. The method comprises: acquiring the coordinates of each joint node of a multi-shaft device in a joint space; under a kinematic constraint condition of each shaft, performing first trajectory planning on each shaft according to the coordinates, so as to obtain a planned time of when each shaft reaches each joint node, and determining a synchronization time of joint nodes according to the planned time; and under the kinematic constraint condition, performing second trajectory planning on each shaft according to the synchronization time and the coordinates, so as to obtain a trajectory of each shaft in the joint space. The planning method is suitable for trajectory planning of various general shafts in a joint space, and under a constraint condition that each shaft of a multi-shaft device conforms to kinematics, each shaft operates synchronously, thereby improving the overall operation performance of the multi-shaft device. Further provided are a trajectory planning apparatus for a joint space of a multi-shaft device, and a computing device and a computer-readable storage medium.

Description

A planning method and device for joint space trajectories of multi-axis equipment

This application claims the priority of the prior country application. The prior country application is China. The application number of the prior country application is 202211035176.1 and the filing date is August 26, 2022. The entire content of the prior country application is incorporated herein by reference.

Technical field

The present application relates to the technical field related to motion control, and in particular to a planning method and device for joint space trajectories of multi-axis equipment.

Background technique

The robot is a multi-axis device that is often divided into several axes for separate feeds. The actual motion trajectory is the combined motion of several axes. During the trajectory planning process, it must be ensured that each axis reaches the designated position at the same time point.

The correct kinematics solution refers to calculating the end pose of the robot given the coordinates of each axis in their respective joint spaces. The inverse solution of kinematics refers to knowing the end pose of the robot and solving the position of each axis of the robot in their respective joint spaces. It is the basis for robot motion planning and trajectory control. There are three inverse solution methods for the kinematic model. One is to find the coordinates of each axis in their respective joint spaces analytically, and the other is to use numerical iteration (which is essentially an optimization process to find a special solution to the equation. It cannot (Find all solutions), the last one is the geometric method, which is only suitable for some robots with relatively simple structures.

In the existing technology, when planning the trajectory of multi-axis equipment, some technical solutions respectively specify the acceleration time, uniform speed time and deceleration time of the speed planner, and each axis accelerates, uniformly speeds and decelerates according to this given time to achieve multi-axis tracking. The purpose of axis synchronization, but this method does not consider motion constraints. Therefore, it often results in some axes failing to meet the motion constraints or meeting the constraints but resulting in a waste of motion time; some technical solutions select the axis with the longest time as the reference axis. , the other axes will accelerate, uniformly and decelerate according to the time of the reference axis to achieve multi-axis synchronization. However, this method can only ensure that the reference axis meets the motion constraints and cannot guarantee whether the other axes do; there are other technical solutions. Select the longest acceleration time, the longest constant speed time, and the longest deceleration time among all acceleration times, all constant speed times, and all deceleration times as the final acceleration time, constant speed time, and deceleration time of each axis, and perform acceleration, constant speed, and deceleration according to this time. Decelerating motion is used to achieve multi-axis synchronization, but this method often causes a waste of motion time and reduces the working efficiency of the robot. In short, when performing trajectory planning for multi-axis equipment, the existing technology cannot both satisfy the motion constraints and maximize the performance of the multi-axis equipment.

Therefore, there is a need for a trajectory planning method that can not only satisfy the motion constraints but also enable all axes of the entire multi-axis equipment to run synchronously when planning the trajectory of multi-axis equipment, thereby improving the operating performance of the multi-axis equipment.

Contents of the invention

In view of this, embodiments of the present application provide a method and device for planning joint space trajectories of multi-axis equipment.

In a first aspect, embodiments of the present application provide a method for planning joint space trajectories of a multi-axis device, including: obtaining the coordinates of each joint node of the multi-axis device in the joint space, and each axis of the multi-axis device is the joint. space one dimension; under the kinematic constraints of each axis, conduct the first trajectory planning for each axis according to the coordinates, obtain the planning time for each axis to reach each joint node, and The synchronization time of each joint node is determined based on this, where the synchronization time of a joint node is the time when each axis reaches the joint node at the same time; under the kinematic constraints, according to the synchronization time and the The coordinates perform a second trajectory planning for each axis separately to obtain the trajectory of each axis in the joint space.

From the above, compared to the trajectory planning in the prior art that only synchronizes the overall running time or the time of each speed change stage, the technical solution of the embodiment of the present application obeys kinematic constraints during the operation of each axis of the multi-axis equipment. Under certain conditions, each axis operates synchronously at each joint point, which improves the operating performance of multi-axis equipment and is suitable for trajectory planning of various general axes in joint space.

In a possible implementation of the first aspect, the first trajectory planning includes: obtaining the inflection point of each axis according to the coordinates, and the inflection point of an axis is the position of the axis in the joint space. The corresponding joint node with a speed of 0 in the coordinate dimension; under the kinematic constraints, plan the trajectory of each axis between every two adjacent inflection points through speed planning to obtain the planning time.

From the above, the first trajectory planning allows each axis to plan itself under kinematic constraints, so that the motion performance of each axis is optimal.

In a possible implementation of the first aspect, obtaining the synchronization time of each joint node includes: according to the planning time, obtaining the time of each axis in the trajectory of the first trajectory planning. The running time between two adjacent joint nodes; for any two adjacent joint nodes, compare the running time of each axis between the two adjacent joint nodes, and select the longest running time among them. As the synchronization duration between the two adjacent joint nodes; the synchronization time is obtained according to the synchronization duration, wherein the synchronization time of a joint node is on the synchronization time of the previous adjacent joint node of the joint node The synchronization duration superimposed between these two adjacent joint nodes.

From the above, for any two adjacent joint nodes, select the longest running time from the running time of each axis as the synchronization duration between the two adjacent joint nodes. This synchronization duration makes each axis satisfy the kinematics. constraints, and achieve the best overall operating performance of the multi-axis equipment by optimizing the motion performance of the most constrained axis (that is, the axis with the longest running time).

In a possible implementation of the first aspect, the second trajectory planning includes: under the kinematic constraints, calculating each axis of each axis according to the synchronization time and the coordinate through speed planning. The trajectory between two adjacent inflection points is planned, and during the planning process, the continuity of the speed, acceleration or jerk of each axis at each joint node is the convergence condition.

From the above, compared with the trajectory planning of the prior art that only achieves continuity of velocity and acceleration, the second trajectory planning of this application increases the continuity of both acceleration and acceleration, reducing the vibration of each axis.

In a possible implementation of the first aspect, the second trajectory planning also includes: when the second trajectory planning of the set time has passed, there is still a speed or acceleration of a certain axis at a certain joint node or When the jerk is discontinuous, smoothing filtering is used to make the velocity, acceleration and jerk of each axis at each joint node continuous.

From the above, in the second trajectory planning, the combination of planning and filtering at set time not only achieves the accuracy of planning, but also improves the continuity of the speed, acceleration and jerk of each axis at each joint node. At the same time Improved planning timeliness.

In a possible implementation of the first aspect, time cosine is used as the basis function in the speed planning The expression represents the relationship between the coordinates of each axis in the joint space and time.

From the above, speed planning uses the expression of time cosine as the basis function to express the speed, without delineating the time of each speed change node, which improves the accuracy of planning.

In a possible implementation of the first aspect, the kinematic constraints include a maximum allowable speed, a maximum allowable acceleration, or a maximum allowable jerk of each axis.

From the above, in the first trajectory planning, each axis obeys its own kinematic constraints, so that the trajectory of each axis in the planning can achieve its best performance. In the second trajectory planning, each axis is synchronized in time. Complying with the respective kinematic constraint planning, the overall operating performance of the multi-axis equipment can be improved.

In the second aspect, embodiments of the present application provide a planning device for joint space trajectories of multi-axis equipment, including: an acquisition module, a time synchronization module and a speed matching module; the acquisition module is used to obtain each joint node of the multi-axis equipment In the coordinates of the joint space, each axis of the multi-axis device is one dimension of the joint space; the time synchronization module is used to calculate the coordinates of each axis according to the coordinates under the kinematic constraints of each axis. Conduct the first trajectory planning for each axis respectively, obtain the planning time for each axis to reach each joint node, and determine the synchronization time of each joint node accordingly, where the synchronization time of a joint node is the synchronization time of each axis The time to reach the joint node at the same time; the speed matching module is used to perform a second trajectory planning for each axis according to the synchronization time and the coordinates under the kinematic constraints to obtain the The trajectory of each axis in the joint space.

From the above, compared to the trajectory planning in the prior art, which only synchronizes the overall running time or the time of each speed change stage, the technical solution of the embodiment of the present application obeys the kinematic constraints during the operation of each axis of the multi-axis equipment. Under certain conditions, each axis operates synchronously at each joint point, which improves the operating performance of multi-axis equipment and is suitable for trajectory planning of various general axes in joint space.

In a possible implementation of the second aspect, when the time synchronization module performs the first trajectory planning, it specifically includes: obtaining the inflection point of each axis according to the coordinates, and the inflection point of an axis is at the joint. The joint node in the space with a speed of 0 in the coordinate dimension corresponding to the axis; under the kinematic constraints, the trajectory of each axis between every two adjacent inflection points is planned through speed planning to obtain the above Planning time.

From the above, the first trajectory planning allows each axis to plan itself under kinematic constraints, so that the motion performance of each axis is optimal.

In a possible implementation of the second aspect, when the time synchronization module obtains the synchronization time of each joint node, it specifically includes: according to the planning time, obtaining each axis in the trajectory of the first trajectory planning. The running time between each two adjacent joint nodes; for any two adjacent joint nodes, compare the running time of each axis between the two adjacent joint nodes, and select the longest one The running time is used as the synchronization time between the two adjacent joint nodes; the synchronization time is obtained according to the synchronization time, wherein the synchronization time of a joint node is the synchronization of the previous adjacent joint node of the joint node The synchronization duration between these two adjacent joint nodes is superimposed in time.

From the above, for any two adjacent joint nodes, select the longest running time from the running time of each axis as the synchronization duration between the two adjacent joint nodes. This synchronization duration makes each axis satisfy the kinematics. constraints, and achieve the best overall operating performance of the multi-axis equipment by optimizing the motion performance of the most constrained axis (that is, the axis with the longest running time).

In a possible implementation of the second aspect, when the speed matching module performs the second trajectory planning, it specifically includes: under the kinematic constraints, according to the synchronization time and the coordinates, respectively through the speed planning The trajectory between every two adjacent inflection points of each axis is planned, and during the planning process, the continuity of the speed, acceleration or jerk of each axis at each joint node is the convergence condition.

From the above, compared with the trajectory planning of the prior art that only achieves continuity of velocity and acceleration, the second trajectory planning of this application increases the continuity of both acceleration and acceleration, reducing the vibration of each axis.

From the above, compared with the trajectory planning of the prior art that only achieves continuity of velocity and acceleration, the second trajectory planning of this application increases the continuity of both acceleration and acceleration, reducing the vibration of each axis.

In a possible implementation of the second aspect, when the speed matching module performs the second trajectory planning, it also specifically includes: when the second trajectory planning of the set time has passed, there is still a certain axis at a certain joint. When the velocity, acceleration, or jerk of a node is discontinuous, smoothing filtering is used to make the velocity, acceleration, and jerk of each axis at each joint node continuous.

From the above, in the second trajectory planning, the combination of planning and filtering at set time not only achieves the accuracy of planning, but also improves the continuity of the speed, acceleration and jerk of each axis at each joint node. At the same time Improved planning timeliness.

In a possible implementation of the second aspect, during the speed planning, an expression using time cosine as a basis function is used to represent the relationship between the coordinates of each axis in the joint space and time.

From the above, speed planning uses the expression of time cosine as the basis function to express the speed, without delineating the time of each speed change node, which improves the accuracy of planning.

In a possible implementation of the second aspect, the kinematic constraints include a maximum allowable speed, a maximum allowable acceleration, or a maximum allowable jerk of each axis.

From the above, in the first trajectory planning, each axis obeys its own kinematic constraints, so that the trajectory of each axis in the planning can achieve its best performance. In the second trajectory planning, each axis is synchronized in time. Complying with the respective kinematic constraint planning, the overall operating performance of the multi-axis equipment can be improved.

In a third aspect, embodiments of the present application provide a computing device, including:

bus;

A communication interface connected to the bus;

at least one processor connected to the bus; and

At least one memory is connected to the bus and stores program instructions. When executed by the at least one processor, the program instructions cause the at least one processor to execute any one of the embodiments of the first aspect of the present application.

In the fourth aspect, embodiments of the present application provide a computer-readable storage medium on which program instructions are stored. When executed by a computer, the program instructions cause the computer to execute any one of the embodiments of the first aspect of the application. .

Description of drawings

In order to explain the technical solutions of the embodiments of the present application more clearly, the appendices required to be used in the embodiments will be described below. The drawings are briefly introduced, and it should be understood that the following drawings only show certain embodiments of the present application, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, without exerting creative efforts, Under the premise, other related drawings can also be obtained based on these drawings.

Figure 1 is a schematic flowchart of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device according to the present application;

Figure 2 is a schematic flowchart of Embodiment 2 of a planning method for a joint space trajectory of a multi-axis device according to the present application;

Figure 3 is a schematic flowchart of a synchronization time determination method in Embodiment 2 of a joint space trajectory planning method for a multi-axis device of the present application;

Figure 4 is a schematic flowchart of the speed matching method in Embodiment 2 of the joint space trajectory planning method of a multi-axis device of the present application;

Figure 5 is a schematic structural diagram of Embodiment 1 of a planning device for joint space trajectories of multi-axis equipment according to the present application;

Figure 6 is a schematic structural diagram of Embodiment 2 of a planning device for joint space trajectories of multi-axis equipment according to the present application;

Figure 7 is a schematic structural diagram of a computing device according to various embodiments of the present application.

Detailed ways

In the following description, reference is made to "some embodiments" which describe a subset of all possible embodiments, but it is understood that "some embodiments" may be the same subset or a different subset of all possible embodiments, and Can be combined with each other without conflict.

In the following description, the terms "first\second\third, etc." or module A, module B, module C, etc. are only used to distinguish similar objects or to distinguish different embodiments. Representing a specific ordering of objects, it will be understood that the specific order or sequence may be interchanged where permitted so that the embodiments of the application described herein can be practiced in an order other than that illustrated or described herein.

In the following description, the labels indicating steps involved, such as S110, S120, etc., do not necessarily mean that this step will be executed. If permitted, the order of the preceding and following steps can be interchanged, or executed at the same time.

Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the technical field to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of the present application and are not intended to limit the present application.

Cartesian space, Cartesian space coordinates: Descartes space is a rectangular coordinate space, and the coordinates of the Cartesian space are poses, including position coordinates and attitude coordinates. The position coordinates are three-dimensional rectangular coordinates, and the attitude coordinates are the current position of the object relative to the three-dimensional rectangular coordinates. The rotation angle of the coordinates, schematically, can be represented by three angular coordinates of Euler angles.

Joint space, joint space coordinates: Joint space is the motion space of each axis of a multi-axis device. Each axis is one dimension of the joint space. Each axis is a universal axis and can be translation or rotation. The joint space coordinate is the displacement or rotation angle of the axis.

Trajectory: It is the relationship between the coordinates and time of the moving object. The Cartesian space trajectory is the relationship between the Cartesian space coordinates and time. The joint space trajectory is the relationship between the joint space coordinates and time.

Kinematic constraints: including maximum allowable speed, maximum allowable acceleration, and maximum allowable jerk. Kinematic constraints are constraints on the joint space of the axis for a specific axis. When an axis is a translation axis, its kinematic constraints are the maximum allowable speed, maximum allowable acceleration, and maximum allowable jerk in terms of displacement; when an axis is a rotation axis, its kinematic constraints are the maximum allowable speed, maximum allowable acceleration, and maximum allowable jerk in terms of rotation. Allowable acceleration, maximum allowable jerk.

Speed planner: A device for planning the trajectory and speed of moving objects. It plans the speed and trajectory of objects through trajectory interpolation. During speed planning, the speed and acceleration of each point are maintained continuously and obey kinematic constraints. The velocity planner can plan trajectories in both Cartesian space and joint space. When planning, describe velocity as an expression in time, for example, a polynomial based on time.

The technical solution of the embodiment of the present application includes: obtaining the coordinates of each joint node of the multi-axis device in the joint space, and performing secondary trajectory planning for each axis according to the coordinates under the kinematic constraints of each axis, The first trajectory planning determines the synchronization time of each joint node, and the second trajectory planning implements the movement speed of each axis matching the synchronization time and obtains the final trajectory in the joint space. The technical solutions of each embodiment of the present application are suitable for trajectory planning of various universal axes in joint space, and allow each axis of the multi-axis device to operate synchronously under the condition that each axis obeys kinematic constraints, thereby improving the overall efficiency of the multi-axis device. Operational performance. In some embodiments, the vibration of the drive motor of each axis is also suppressed to reduce the vibration of each axis through the continuous acceleration of each axis at the joint node.

The following describes the method embodiments, device embodiments and other related embodiments of the present application for planning a joint space trajectory of a multi-axis device with reference to the accompanying drawings.

The multi-axis equipment in this application has multiple axes and can be a robot or a machine tool. The trajectory node is the key point of the end of the multi-axis equipment in Cartesian space; the end of the multi-axis equipment can be the external operating part of the multi-axis equipment, such as the fingers of the robot, the cutter of the machine tool, the mechanical arm of the tower crane, etc.; the joint nodes of the multi-axis equipment It is the point reached by each axis of the multi-axis device in the joint space when the end of the multi-axis device reaches the trajectory node.

The following first introduces various planning method embodiments of a joint space trajectory of a multi-axis device with reference to the accompanying drawings 1 to 4.

Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device includes: obtaining the coordinates of each joint node of the multi-axis device in the joint space; under the kinematic constraints of each axis, calculating each axis according to the coordinates Carry out the first trajectory planning respectively, obtain the planning time for each axis to reach each joint node, and determine the synchronization time of each joint node accordingly; under the kinematic constraints, according to the synchronization time and the coordinates Perform a second trajectory planning for each axis to obtain the trajectory of each axis in the joint space.

Figure 1 shows the process of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device, including steps S110 to S130.

S110: Obtain the coordinates of each joint node in the joint space. Each axis of the multi-axis device is one dimension of the joint space.

Schematically, the joint node coordinates in the joint space form an N*M-dimensional matrix, where N represents N joint nodes and M represents M axes.

Among them, the coordinates of the joint nodes in the joint space can be obtained through the inverse kinematics solution based on the posture of the end of the multi-axis device. In some embodiments, the pose of the end of the multi-axis device is obtained through trajectory planning of the end of the multi-axis device.

Among them, the axes of the multi-axis device are universal axes, including translation axes and/or rotation axes. Schematically, the translation axis, as The node coordinates of the axis of the cylinder are the translational displacement, and the rotation axis, such as the axis of the motor, its node coordinates are the angle of rotation.

Among them, kinematic constraints are used to constrain the trajectory of each axis, and each axis has corresponding kinematic constraints. Each embodiment of the present application has kinematic constraints in the subsequent first trajectory planning and second trajectory planning.

The sampling period is the granularity of time described in each embodiment of this application. For example, the axis of the sampling period drives the pulse period.

S120: Under the kinematic constraints of each axis of the multi-axis device, perform the first trajectory planning for each axis according to the coordinates of each joint node, and obtain the planning time for each axis to reach each joint node, and Based on this, the synchronization time of each joint node is determined.

Among them, the synchronization time of a joint node is the time when each axis reaches the joint node at the same time, that is, the coordinates of each axis arriving at the joint node synchronously at the synchronization time of a joint node represent the position of each axis.

Among them, kinematic constraints are used to constrain the trajectory of each axis, and each axis has corresponding kinematic constraints. In each embodiment of the present application, the subsequent first trajectory planning and second trajectory planning all comply with the kinematic constraints.

Among them, during the first trajectory planning, the velocity planner is used to perform velocity planning to obtain the relationship between the coordinates of each joint node and time, thereby completing the first trajectory planning. The velocity planner describes the relationship between velocity and time in each dimension using The S-shaped curve, that is, S-shaped acceleration and deceleration, is used to plan the speed. The speed can also be planned based on other shape lines such as trapezoidal lines. The speed planner can use a time-based polynomial to describe the relationship between speed and time, or it can use an expression based on the cosine function of time to describe the relationship between speed and time.

Among them, because the trajectory describes the relationship between the coordinates of the joint space and time, based on the trajectory obtained from the first trajectory planning, the planning time for each axis to reach each joint node can be obtained. For any two adjacent joint nodes, the planning time for an axis to reach the latter of the two adjacent joint nodes is subtracted from the planning time for the axis to reach the previous joint node of the two adjacent joint nodes to obtain The length of time the axis runs between these two adjacent joint nodes.

Among them, for any two adjacent joint nodes, compare the running time of each axis between the two adjacent joint nodes, and select the longest running time for each axis of the multi-axis equipment between the two adjacent joint nodes. synchronization duration between. The synchronization time of the multi-axis equipment end reaching each joint node is obtained according to the synchronization duration. The synchronization time of a joint node is the synchronization time of the previous adjacent joint node of the joint node superimposed on the two adjacent joint nodes. synchronization time between.

S130: Under the kinematic constraints of each axis of the multi-axis device, perform a second trajectory planning for each axis based on the coordinates and synchronization time of each joint node to obtain the trajectory of each axis in the joint space.

Among them, the trajectory of each axis in the joint space constitutes the trajectory of the multi-axis device in the joint space.

Among them, the second trajectory planning uses the speed planner introduced in step S120 to perform the second trajectory planning on the joint space trajectory of each axis. The planning also complies with the following conditions: 1) Each axis reaches each joint during the planning process. The time of the node is the corresponding synchronization time; 2) The speed, acceleration or jerk of each axis at each joint node is continuous.

Among them, in order to realize the continuous velocity, acceleration or jerk of each axis at each joint node, in some embodiments, this is achieved by increasing the planning time and going through a sufficient number of iterations of the forward velocity planning. In other embodiments, In the embodiment, this is achieved through multiple iterations of forward speed planning and filter synthesis.

In summary, Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device performs a first step on each axis according to the coordinates of each joint node of the multi-axis device in the joint space under the kinematic constraints of each axis. Secondary trajectory planning, obtain the planning time for each axis to reach each joint node, and determine the synchronization time of each joint node accordingly, and conduct the second trajectory planning for each axis based on the synchronization time and the coordinates, Obtain the trajectory of each axis in joint space. Embodiment 1 of this method is suitable for trajectory planning of various general axes in joint space. Under the condition that each axis of the multi-axis device obeys kinematic constraints, each axis runs synchronously at each node, thereby improving the overall operation of the multi-axis device. performance.

Embodiment 2 of a planning method for a joint space trajectory of a multi-axis device is first introduced below with reference to Figures 2 to 4 of the accompanying drawings.

Embodiment 2 of a planning method for joint space trajectories of multi-axis equipment. Based on Embodiment 1 of a planning method for joint space trajectories of multi-axis equipment, the first trajectory planning is performed by selecting between every two adjacent nodes. The running time of the axis with the longest running time is selected as the synchronization time to ensure that at least one axis has the best motion performance. In the second trajectory planning, by setting the second trajectory planning with a duration and adding smoothing filtering, not only can each The speed, acceleration or jerk of the axis when it reaches each joint node is continuous, and the planning efficiency is high. In the two trajectory planning processes, expressions based on time cosine are chosen to describe the speed, which further improves the accuracy of planning.

Figure 2 shows the process of Embodiment 2 of a planning method for a joint space trajectory of a multi-axis device, including steps S210 to S240.

S210: Obtain the pose, kinematic constraints and calculation time granularity of the path node at the end of the multi-axis device.

Among them, the path nodes are several key points in the running path of the multi-axis equipment end in Cartesian space, which are used to plan the trajectory of the multi-axis equipment end in Cartesian space. The path nodes are obtained based on the key positions of the end of the multi-axis equipment in the actual scene.

Among them, the calculation time granularity is the minimum time interval used in trajectory planning. The smaller the granularity, the smoother the calculated trajectory, but the greater the calculation amount. Generally, the driving pulse period of the axis is used as the sampling period.

For the kinematic constraints, please refer to the description in step S110 of Embodiment 1 of a joint space trajectory planning method for a multi-axis device.

S220: Plan the trajectory of the end of the multi-axis device in Cartesian space based on the pose of the path node, and obtain the coordinates of the joint nodes of the multi-axis device in the joint space through the inverse kinematic solution.

Among them, the trajectory of the end of the multi-axis equipment in Cartesian space is planned according to the pose of the path node at the end of the multi-axis equipment. In some embodiments, it is implemented through two steps of path planning and trajectory planning in Cartesian space. In other embodiments, It is realized in one step through trajectory planning in Cartesian space, which is not limited in this embodiment.

Among them, the joint nodes in the joint space correspond to the trajectory nodes on the Cartesian space trajectory, and the trajectory nodes are key points on the Cartesian space trajectory, which determine the trend of the Cartesian space trajectory.

Among them, the coordinates of each joint node in the joint space are obtained through the inverse kinematics solution. The inverse kinematics solution methods include analytical methods, iterative methods and geometric methods, which are not limited in this embodiment.

The coordinates of each joint node in the joint space obtained at this time may be obtained in step S110 of the first embodiment of a joint space trajectory planning method for a multi-axis device.

S230: Under the kinematic constraints of each axis of the multi-axis device, calculate each joint node based on the coordinates of each joint node. The axes perform first trajectory planning respectively, and the planning time for each axis to reach each joint node is obtained, and the synchronization time of each joint node is determined accordingly.

Among them, this step is based on step S120 of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device, and performs segment planning by keeping the inflection point speed of each axis at 0 to improve the planning accuracy of each axis, while speed planning The device plans the speed based on the S-shaped curve, that is, S-shaped acceleration and deceleration, and uses an expression based on the cosine function of time to describe the relationship between speed and time to further improve the planning accuracy.

For the detailed method of this step, please refer to the synchronization time determination method in Embodiment 2 of a joint space trajectory planning method for a multi-axis device, which will not be described in detail here.

S240: Under the kinematic constraints of each axis of the multi-axis device, perform a second trajectory planning for each axis based on the coordinates and synchronization time of each joint node to obtain the trajectory of each axis in the joint space.

Among them, this step is based on step S120 of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device, and also improves the continuity of the speed, acceleration and jerk of each axis at each joint node through filtering, and at the same time Use the expression based on the cosine function of time to describe the relationship between speed and time.

For the detailed method of this step, please refer to the speed matching method in Embodiment 2 of a joint space trajectory planning method for a multi-axis device, which will not be described in detail here.

Figure 3 shows the flow of the synchronization time determination method in Embodiment 2 of a planning method for a joint space trajectory of a multi-axis device, that is, step 230 includes steps S2310 to S2350.

S2310: Obtain the inflection point of each axis based on the coordinates of each joint node.

Among them, the inflection point of an axis is the point where the axis's velocity in the joint space is zero. It is the turning point of the forward and reverse motion of the axis, also called the anchor point. By analyzing the coordinate changes between the joint nodes of each axis, the inflection point of the axis can be obtained. The methods used include differential method, contrast method, etc.

Among them, the inflection point of an axis also includes the first and last joint nodes of the axis. At these two joint nodes, the speed of the joint space is also 0.

S2320: Use the speed planner to perform the first trajectory planning of the joint space trajectory between every two adjacent inflection points of each axis under kinematic constraints, and obtain the planning time for each axis to reach each joint node.

Among them, in the first trajectory planning process, the planning of each axis is carried out separately, and the speed between every two adjacent inflection points of each axis is planned through the rate planner, so as to obtain every two The trajectory between adjacent inflection points. When planning by inflection points, keep the speed of each axis at each inflection point at 0.

Among them, in the process of speed planning, an S-shaped curve is used for planning under kinematic constraints, and the relationship between speed and time is expressed by an expression with time cosine as the basis function. This expression can be time cosine as the basis function. The polynomial of the function can also be in other forms.

From the above, the speed planner using the expression of time cosine as the basis function automatically plans according to the S-shaped curve. There is no need to manually set the time-based segmentation of the S-shaped curve. For example, there is no need to manually set increasing acceleration, uniformity, etc. Accelerate, reduce acceleration, constant speed, increase and increase deceleration, uniform deceleration, decrease deceleration and other time periods, the planning trajectory is more accurate, and the motion performance of each axis is optimal under kinematic constraints.

S2330: According to the planning time for each axis to reach each joint node, obtain the running time of each axis between every two adjacent joint nodes.

Among them, the motion performance of each axis in a trajectory planning is optimal under kinematic constraints. According to each axis The running time of each axis between each two adjacent joint nodes determined by the time to reach each joint node is also the optimal running time of each axis.

S2340: For any two adjacent joint nodes, compare the running time of each axis between the two adjacent joint nodes, and select the longest running time as the synchronization time between the two adjacent joint nodes.

From the above, the synchronization duration between any two adjacent joint nodes is determined by the above method, so that at least one axis has the best performance between the two adjacent joint nodes, so that the entire system can be operated under kinematic constraints. The multi-axis device performs best between the two adjacent joint nodes, because at this time, shortening the running time of the entire multi-axis device between the two adjacent joint nodes will cause one of the axes to exceed its Kinematic constraints.

S2350: Obtain the synchronization time of each joint node in sequence according to the determined synchronization duration.

Among them, the synchronization time of a joint node is the synchronization time between the two adjacent joint nodes superimposed on the synchronization time of the previous adjacent joint node of the joint node.

Schematically, let the time of the first joint node, which is the starting point, be t. Since the starting time of each axis is the same, t can also be considered as the synchronization time of the first joint node. Subsequently, it will be superimposed with the next joint on the basis of t. The synchronization time between nodes, so as to obtain the synchronization time of each joint node in turn.

From the above, according to the determined synchronization time, each axis of the multi-axis device reaches the designated position (joint node) at the same time, and obeys the kinematic constraints, and at the same time, the operating performance reaches the optimal level.

Figure 4 shows the flow of the speed matching method in Embodiment 2 of a joint space trajectory planning method for a multi-axis device, that is, step 240 includes steps S2410 to S2440.

Among them, each axis of the multi-axis device runs each step of this embodiment for planning. For the convenience of description, the axis currently undergoing planning is called the planning axis.

S2410: Under kinematic constraints, according to the coordinates and synchronization time of each joint node, use the speed planner to perform a second trajectory planning on the joint space trajectory between each two adjacent inflection points of the planning axis, and obtain the planning axis Velocity, acceleration and jerk at each joint node.

Among them, in the second trajectory planning, the time when the planned axis reaches any joint node is the synchronization time of the joint node, so as to realize the matching of the speed of each axis of the multi-axis equipment, that is, according to the matched speed of each axis (that is, the first The speed after quadratic trajectory planning) each axis reaches each joint node synchronously. A more detailed description is that each axis synchronously reaches the coordinates corresponding to the respective dimensions of each axis in each joint node. And in the second trajectory planning, the velocity, acceleration or jerk of the planning axis at each joint node in the joint space is continuous as the planning convergence condition.

Among them, during the planning process, the speed of the planning axis inflection point in the joint space is also kept as 0, and the S-shaped curve is used for planning under kinematic constraints.

Among them, the relationship between speed and time during planning is expressed by an expression with time cosine as the basis function. This expression can be a polynomial with time cosine as the basis function, or it can be in other forms.

From the above, the speed planner that uses the expression of time cosine as the basis function automatically plans according to the S-shaped curve. It does not need to manually set the time-based segmentation of the S-shaped curve in advance, which improves the accuracy of the second trajectory planning.

Based on the above, the existing technology only realizes the continuity of velocity and acceleration. This embodiment adds the continuity of acceleration and acceleration, thus suppressing the vibration problem of each axis.

S2420: After the set secondary trajectory planning time, determine whether there is a jump joint node on the planning axis.

If it exists, step S2430 is executed; otherwise, step S2440 is executed.

Among them, the jump joint node of an axis is the velocity or acceleration or jerk discontinuity of the axis at the joint node.

Among them, the second trajectory planning includes multiple rounds of forward speed planning. Each round of forward speed planning selects multiple forward interpolation points and uses forward and/or reverse search methods to perform complex speed planning calculations, which requires a large amount of calculation. In order to maintain the timeliness of the operation, it is necessary to set the duration of the second trajectory planning.

S2430: Make the speed, acceleration and jerk of the jumping joint node of the planning axis continuous through filtering.

Among them, filtering methods include polynomial filtering or average filtering.

From the above, the duration of the second trajectory planning is controlled by adding filtering to meet the timeliness requirements of the actual scene and improve the continuity of the speed, acceleration and jerk of each axis at the joint nodes.

S2440: Output the currently planned trajectory as the joint space trajectory of the planned axis of the multi-axis device.

Among them, after completing the joint space trajectory planning of the current planning axis of the multi-axis device, the next unplanned axis is selected as the planning axis to continue planning.

To sum up, Embodiment 2 of a planning method for a joint space trajectory of a multi-axis device is based on Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device. The first trajectory planning is performed in every two phases. The running time of the axis with the longest running time between adjacent nodes is selected as the synchronization time so that at least one axis has the best motion performance. In the second trajectory planning, the method of setting the duration of the second trajectory planning and filtering is not only achieved The speed, acceleration or jerk of each axis reaching each joint node is continuous, and the planning efficiency is high. In the two trajectory planning processes, expressions based on time cosine are chosen to describe the speed, which further improves the accuracy of planning.

An embodiment of a planning device for a joint space trajectory of a multi-axis device according to the present application will be introduced below with reference to FIGS. 5 and 6 .

Embodiment 1 of a planning device for a joint space trajectory of a multi-axis device executes the method of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device, and has all its advantages.

Figure 5 shows the structure of Embodiment 1 of a planning device for joint space trajectories of multi-axis equipment, including: an acquisition module 510, a time synchronization module 520 and a speed matching module 530.

The acquisition module 510 is used to acquire the coordinates of each joint node in the joint space. Each axis of the multi-axis device is one dimension of the joint space. For its method and advantages, please refer to step S110 of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device.

The time synchronization module 520 is used to perform the first trajectory planning for each axis according to the coordinates of each joint node under the kinematic constraints of each axis of the multi-axis device, and obtain the time when each axis reaches each joint node. Plan the time and determine the synchronization time of each joint node accordingly. For its method and advantages, please refer to step S120 of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device.

The speed matching module 530 is used to perform a second trajectory planning for each axis according to the coordinates and synchronization time of each joint node under the kinematic constraints of each axis of the multi-axis device, and obtain the position of each axis in the joint space. trajectory. For its method and advantages, please refer to step S130 of Embodiment 1 of a planning method for a joint space trajectory of a multi-axis device.

Embodiment 2 of a planning device for joint space trajectories of multi-axis equipment executes the method of Embodiment 2 of a planning method for joint space trajectories of multi-axis equipment, and has all its advantages.

Figure 6 shows the structure of Embodiment 2 of a joint space trajectory planning device for multi-axis equipment, including: an acquisition module 610, an end trajectory planning module 620, a time synchronization module 630 and a speed matching module 640.

The acquisition module 610 is used for S210: acquiring the pose, kinematic constraints and calculation time granularity of the path node at the end of the multi-axis device. For its method and advantages, please refer to step S210 in Embodiment 2 of a joint space trajectory planning method for a multi-axis device.

The end trajectory planning module 620 is used to plan the trajectory of the end of the multi-axis device in Cartesian space according to the pose of the path node, and obtain the coordinates of the joint nodes of the multi-axis device in the joint space through the inverse kinematic solution. For its method and advantages, please refer to step S220 in Embodiment 2 of a joint space trajectory planning method for a multi-axis device.

The time synchronization module 630 is used to perform the first trajectory planning for each axis according to the coordinates of each joint node under the kinematic constraints of each axis of the multi-axis device, and obtain the time when each axis reaches each joint node. Plan the time and determine the synchronization time of each joint node accordingly. For its method and advantages, please refer to step S230 of Embodiment 2 of a joint space trajectory planning method for a multi-axis device.

The speed matching module 640 is used to perform a second trajectory planning for each axis according to the coordinates and synchronization time of each joint node under the kinematic constraints of each axis of the multi-axis device, and obtain the position of each axis in the joint space. trajectory. For its method and advantages, please refer to step S240 in Embodiment 2 of a joint space trajectory planning method for a multi-axis device.

An embodiment of the present application also provides a computing device, which will be described in detail below in conjunction with Figure 7 .

The computing device 700 includes a processor 710, a memory 720, a communication interface 730, and a bus 740.

It should be understood that the communication interface 730 in the computing device 700 shown in this figure can be used to communicate with other devices.

The processor 710 can be connected to the memory 720 . The memory 720 can be used to store the program code and data. Therefore, the memory 720 may be a storage unit internal to the processor 710 , or may be an external storage unit independent of the processor 710 , or may include a storage unit internal to the processor 710 and an external storage unit independent of the processor 710 . part.

Optionally, computing device 700 may also include bus 740. Among them, the memory 720 and the communication interface 730 can be connected to the processor 710 through the bus 740. The bus 740 may be a Peripheral Component Interconnect (PCI) bus or an Extended Industrial Standard Architecture (EFStended Industry Standard Architecture, EISA) bus, etc. The bus 740 can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one line is used in this figure, but it does not mean that there is only one bus or one type of bus.

It should be understood that in this embodiment of the present application, the processor 710 may be a central processing unit (CPU). The processor can also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), off-the-shelf programmable gate arrays (field programmable gate arrays, FPGAs) or other Programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc. Or the processor 710 uses one or more integrated circuits to execute relevant programs to implement the technical solutions provided by the embodiments of the present application.

The memory 720 may include read-only memory and random access memory and provide instructions to the processor 710 and data. A portion of processor 710 may also include non-volatile random access memory. For example, processor 710 may also store device type information.

When the computing device 700 is running, the processor 710 executes the computer execution instructions in the memory 720 to perform the operation steps of each method embodiment.

It should be understood that the computing device 700 according to the embodiments of the present application may correspond to the corresponding subject in performing the methods according to the various embodiments of the present application, and the above and other operations and/or functions of the various modules in the computing device 700 are respectively intended to implement the present application. For the sake of brevity, the corresponding processes of each method in the method embodiments will not be described again here.

Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented with electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled artisans may implement the described functionality using different methods for each specific application, but such implementations should not be considered beyond the scope of this application.

Those skilled in the art can clearly understand that for the convenience and simplicity of description, the specific working processes of the systems, devices and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be described again here.

In the several embodiments provided in this application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components may be combined or can be integrated into another system, or some features can be ignored, or not implemented. On the other hand, the coupling or direct coupling or communication connection between each other shown or discussed may be through some interfaces, and the indirect coupling or communication connection of the devices or units may be in electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or they may be distributed to multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the method embodiment.

In addition, each functional unit in each embodiment of the present application can be integrated into one processing unit, each unit can exist physically alone, or two or more units can be integrated into one unit.

If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the existing technology or the part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including Several instructions are used to cause a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the decoding method described in various embodiments of this application. The aforementioned storage media include U disk, mobile hard disk, Read-Only Memory (ROM), Random Access Memory (RAM), magnetic disk or optical disk and other media that can store program code. .

Embodiments of the present application also provide a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the program is used to perform the operating steps of each method embodiment.

The computer storage medium in the embodiment of the present application may be any combination of one or more computer-readable media. The computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. computer The readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination thereof. More specific examples (non-exhaustive list) of computer readable storage media include electrical connections having one or more conductors, portable computer 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. As used herein, a computer-readable storage medium may be any tangible medium that contains or stores a program for use by or in connection with an instruction execution system, apparatus, or device.

A computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave carrying computer-readable program code therein. Such propagated data signals may take many forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device .

Program code embodied on a computer-readable medium may be transmitted using any suitable medium, including, but not limited to, wireless, wire, optical cable, RF, etc., or any suitable combination of the foregoing.

Computer program code for performing operations of the present application may be written in one or more programming languages, including object-oriented programming languages such as Java, Smalltalk, C++, and conventional Procedural programming language—such as "C" or a similar programming language. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In situations involving remote computers, the remote computer can be connected to the user's computer through any kind of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computer (such as an Internet service provider through the Internet). connect).

Note that the above are only the preferred embodiments of the present application and the technical principles used. Those skilled in the art will understand that the present application is not limited to the specific embodiments described here, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of the present application. Therefore, although the present application has been described in detail through the above embodiments, the present application is not limited to the above embodiments. Without departing from the concept of the present application, it can also include more other equivalent embodiments, all of which belong to the present application. Apply for protection scope.

Claims (10)

A planning method for joint space trajectories of multi-axis equipment, which is characterized by including: Obtain the coordinates of each joint node of the multi-axis device in the joint space. Each axis of the multi-axis device is one dimension of the joint space; Under the kinematic constraints of each axis, perform the first trajectory planning for each axis according to the coordinates, obtain the planning time for each axis to reach each joint node, and determine each axis accordingly. The synchronization time of a joint node, where the synchronization time of a joint node is the time when each axis reaches the joint node at the same time; Under the kinematic constraints, a second trajectory planning is performed on each axis according to the synchronization time and the coordinates to obtain the trajectory of each axis in the joint space. The method according to claim 1, characterized in that the first trajectory planning includes: Obtain the inflection point of each axis according to the coordinates, and the inflection point of an axis is the joint node with a speed of 0 in the coordinate dimension corresponding to the axis in the joint space; Under the kinematic constraints, the trajectory of each axis between every two adjacent inflection points is planned through speed planning to obtain the planning time. The method according to claim 1, characterized in that, determining the synchronization time of each joint node based on this includes: According to the planning time, the running time of each axis between each two adjacent joint nodes in the trajectory of the first trajectory planning is obtained; For any two adjacent joint nodes, compare the running time of each axis between the two adjacent joint nodes, and select the longest running time as the running time between the two adjacent joint nodes. Synchronization duration; The synchronization time is obtained according to the synchronization duration, wherein the synchronization time of one joint node is the synchronization time between the two adjacent joint nodes superimposed on the synchronization time of the last adjacent joint node of the joint node. The method according to claim 2, characterized in that the second trajectory planning includes: Under the kinematic constraints, the trajectory between each two adjacent inflection points of each axis is planned through speed planning according to the synchronization time and the coordinates, and during the planning process, each axis is planned at each time. The convergence condition is that the velocity, acceleration or jerk of each joint node is continuous. The method according to claim 4, characterized in that the second trajectory planning further includes: When there is still discontinuity in the velocity, acceleration or jerk of an axis at a certain joint node during the second trajectory planning after the set time, smoothing filtering is used to make the velocity, acceleration and acceleration of each axis at each joint node Continuity of jerk. The method according to any one of claims 2, 4 or 5, characterized in that during the speed planning, an expression using time cosine as a basis function is used to represent the relationship between the coordinates of each axis in the joint space and time. The method according to any one of claims 1 to 5, characterized in that the kinematic constraints include the maximum allowable speed, the maximum allowable acceleration or the maximum allowable jerk of each axis. A planning device for joint space trajectories of multi-axis equipment, characterized by including: an acquisition module, a time synchronization module and a speed matching module; The acquisition module is used to obtain the coordinates of each joint node of the multi-axis device in the joint space, and each axis of the multi-axis device is one dimension of the joint space; The time synchronization module is used to perform the first trajectory planning for each axis according to the coordinates under the kinematic constraints of each axis, and obtain the plan for each axis to reach each joint node. time, and determine the synchronization time of each joint node accordingly, where the synchronization time of a joint node is the time when each axis reaches the joint node at the same time; The speed matching module is used to perform a second trajectory planning for each axis according to the synchronization time and the coordinates under the kinematic constraints, and obtain the position of each axis in the joint space. traces of. A computing device, characterized by including: bus; A communication interface connected to the bus; at least one processor connected to the bus; and At least one memory is connected to the bus and stores program instructions, which when executed by the at least one processor cause the at least one processor to perform the method of any one of claims 1 to 7. A computer-readable storage medium on which program instructions are stored, characterized in that, when executed by a computer, the program instructions cause the computer to execute the method of any one of claims 1 to 7.