Motion trajectory planning method and device, computer device and storage medium

Abstract

The application discloses a motion trajectory planning method and device of a multi-degree-of-freedom robot, computer equipment and a storage medium. The method comprises the following steps: constructing a position spline curve and an attitude spline curve in a motion trajectory of the multi-degree-of-freedom robot; determining the motion priority of the translational motion and the rotational motion of the multi-degree-of-freedom robot according to the displacement and the maximum reachable speed of the translational motion and the rotational motion of the multi-degree-of-freedom robot; in the case that the priority of the translational motion of the multi-degree-of-freedom robot is greater than that of the rotational motion, taking the position motion of the multi-degree-of-freedom robot as the main motion to generate a first cooperative planning spline curve; and in the case that the priority of the translational motion of the multi-degree-of-freedom robot is less than that of the rotational motion, taking the attitude motion of the multi-degree-of-freedom robot as the main motion to generate a second cooperative planning spline curve. The method can guarantee the independence of the position spline curve and the attitude spline curve, and significantly reduce the fluctuation rate of the feed speed.

Description

Technical Field

[0001] This application relates to the field of robotics, and in particular to a motion trajectory planning method, apparatus, computer device, and storage medium. Background Technology

[0002] For high-degree-of-freedom robots, applications such as gluing, cutting, welding, and handling require executing complex motion trajectories in Cartesian space. Generally, the motion trajectory of a robot in Cartesian space consists of two parts: the position trajectory and the attitude trajectory. The position and attitude trajectories of the robot at multiple points can be expressed using spline curves. Therefore, how to achieve synchronous planning of position and attitude even when the changes in position and attitude are uneven becomes a problem to be solved. Summary of the Invention

[0003] This application provides a method for planning the motion trajectory of a multi-degree-of-freedom robot, a device for planning the motion trajectory of a multi-degree-of-freedom robot, a computer device, and a storage medium.

[0004] The motion trajectory planning method for a multi-degree-of-freedom robot implemented in this application includes:

[0005] Construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of a multi-degree-of-freedom robot;

[0006] Based on the translational and rotational displacements and the maximum achievable velocity of the multi-degree-of-freedom robot, the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot is determined.

[0007] When the translational motion of the multi-degree-of-freedom robot has a higher priority than the rotational motion, the positional motion of the multi-degree-of-freedom robot is taken as the main motion, and a first cooperative planning spline curve is generated. The first cooperative planning spline curve is a cooperative planning spline curve from the position spline curve node vector to the attitude spline curve node vector.

[0008] When the translational motion of the multi-degree-of-freedom robot has a lower priority than the rotational motion, the attitude motion of the multi-degree-of-freedom robot is taken as the main motion, and a second collaborative planning spline curve is generated. The second collaborative planning spline curve is a collaborative planning spline curve from the attitude spline curve node vector to the position spline curve node vector.

[0009] In some implementations, the position spline curves and attitude spline curves in the spline curves used to construct the motion trajectory of the multi-degree-of-freedom robot include:

[0010] The position spline curve is obtained by summing the products of a predetermined number of control vertices of B-spline curves and their corresponding cubic B-spline basis functions.

[0011] In some implementations, the position spline curve is obtained according to the following relationship:

[0012]

[0013] Where p(u) is the position spline curve, d i N is the control vertex of the B-spline curve. i,k (u) is a k-th degree B-spline basis function, and when k=3 it is the cubic B-spline basis function.

[0014] In some implementations, the attitude spline curve is obtained from the following relationship:

[0015]

[0016] Where, q i (u) is the attitude spline curve, Q j (j=i-1,…,i+2) are attitude control vertices represented by unit quaternions. It is a fifth-degree polynomial function.

[0017] In some embodiments, the position spline curve and attitude spline curve in the motion trajectory of the multi-degree-of-freedom robot are both second-order continuous and differentiable at the splice point.

[0018] In some embodiments, the displacement of the translational motion of the multi-degree-of-freedom robot is denoted as translational displacement, the displacement of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational displacement, the velocity of the translational motion of the multi-degree-of-freedom robot is denoted as translational velocity, and the velocity of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational velocity. Determining the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot includes:

[0019] The translational running time is calculated based on the translational displacement and the translational velocity;

[0020] The rotational running time is calculated based on the rotational displacement and the rotational speed.

[0021] If the translational running time is greater than or equal to the rotational running time, the priority of the translational motion is determined to be higher than the priority of the rotational motion.

[0022] If the translational running time is less than the rotational running time, the priority of the translational motion is determined to be lower than the priority of the rotational motion.

[0023] In some implementations, the first collaborative programming spline curve is obtained according to the following formula:

[0024]

[0025] Where y(x) is the first collaborative planning spline curve, x is the node vector of the position spline curve, y is the node vector of the attitude spline curve, and h i-1 =x i -x i-1 (i = 2, ..., N), N is the number of nodes within the domain of the positional spline curve, and d i (i = 1, ..., N) represents the first derivative of the first collaborative programming spline curve at each node.

[0026] In some implementations, the second collaborative programming spline curve is obtained according to the following formula:

[0027]

[0028] Where y(x) is the second collaborative planning spline curve, x is the node vector of the attitude spline curve, y is the node vector of the position spline curve, and h i-1 =x i -x i-1 (i = 2, ..., N), θ=(xx i-1 ) / h i-1 (i = 2, ..., N), where N is the number of nodes in the domain of the attitude spline curve, and d i (i = 1, ..., N) represents the first derivative of the second collaborative programming spline curve at each node.

[0029] In some implementations, both the first and second co-programming spline curves are monotonically increasing spline curves that are second-order continuously differentiable at the splice points.

[0030] This application provides a motion trajectory planning device for a multi-degree-of-freedom robot, the motion trajectory planning device comprising:

[0031] The construction module is used to construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of a multi-degree-of-freedom robot;

[0032] The determination module is used to determine the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot based on the translational and rotational displacements and the maximum achievable speed of the multi-degree-of-freedom robot.

[0033] The first generation module is used to generate a first cooperative planning spline curve, with the position motion of the multi-degree-of-freedom robot as the main motion when the priority of the translational motion of the multi-degree-of-freedom robot is greater than that of the rotational motion. The first cooperative planning spline curve is a cooperative planning spline curve from the position spline curve node vector to the attitude spline curve node vector.

[0034] The second generation module is used to generate a second collaborative planning spline curve, where the priority of the translational motion of the multi-degree-of-freedom robot is lower than that of the rotational motion, taking the attitude motion of the multi-degree-of-freedom robot as the main motion. The second collaborative planning spline curve is a collaborative planning spline curve from the attitude spline curve node vector to the position spline curve node vector.

[0035] This application provides a computer device, which includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the motion trajectory planning method for a multi-degree-of-freedom robot in any of the above embodiments.

[0036] This application provides a non-volatile computer-readable storage medium with computer-executable instructions. When the computer-executable instructions are executed by one or more processors, the processors perform the motion trajectory planning method for a multi-degree-of-freedom robot as described in any of the above embodiments.

[0037] In the motion trajectory planning method of this application, position spline curves and attitude spline curves are constructed. Then, based on the priority levels of translational and rotational motions of the multi-degree-of-freedom robot, translational motion or rotational motion is selected as the main motion. This generates collaborative planning spline curves from position spline curve node vectors to attitude spline curve node vectors and from attitude spline curve node vectors to position spline curve node vectors, respectively. This not only ensures the independence of position spline curves and attitude spline curves but also significantly reduces the fluctuation rate of feed speed. It can also achieve synchronous planning of position and attitude, even in cases where the position change is large and the attitude change is small or the position change is small and the attitude change is large.

[0038] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0039] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, wherein:

[0040] Figure 1 This is a flowchart illustrating the motion trajectory planning method in the embodiments of this application;

[0041] Figure 2 This is a schematic diagram of the motion trajectory planning device in the embodiments of this application;

[0042] Figure 3 This is a schematic diagram of the structure of the computer device in the embodiments of this application;

[0043] Figure 4 This is a flowchart illustrating the motion trajectory planning method in the embodiments of this application;

[0044] Figure 5 This is a flowchart illustrating the motion trajectory planning method in the embodiments of this application;

[0045] Figure 6 It is a position spline curve obtained by applying the motion trajectory planning method in the embodiments of this application;

[0046] Figure 7 It is an attitude spline curve obtained by applying the motion trajectory planning method in the embodiments of this application;

[0047] Figure 8 It is the second collaborative planning spline curve obtained by applying the motion trajectory planning method in the embodiments of this application.

[0048] Explanation of key component symbols:

[0049] Computer device 100, processor 11, memory 12, motion trajectory planning device 200, construction module 21, determination module 22, first generation module 23, second generation module 24. Detailed Implementation

[0050] The embodiments of this application are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain this application, and should not be construed as limiting this application.

[0051] In the description of this application, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicating orientation or positional relationships based on the orientation or positional relationships shown in the accompanying drawings, are used only for the convenience of describing this application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this application. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more of the stated features. In the description of this application, "a plurality of" means two or more, unless otherwise explicitly specified.

[0052] In this application, unless otherwise expressly specified and limited, "above" or "below" the second feature can include direct contact between the first and second features, or contact between the first and second features through another feature between them. Furthermore, "above," "over," and "on top" of the second feature includes the first feature being directly above or diagonally above the second feature, or simply indicates that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature includes the first feature being directly below or diagonally below the second feature, or simply indicates that the first feature is at a lower horizontal level than the second feature.

[0053] The following disclosure provides many different embodiments or examples for implementing different structures of this application. To simplify the disclosure, specific examples of components and arrangements are described below. Of course, these are merely examples and are not intended to limit the scope of this application. Furthermore, reference numerals and / or letters may be repeated in different examples; such repetition is for simplification and clarity and does not in itself indicate a relationship between the various embodiments and / or arrangements discussed. In addition, various specific examples of processes and materials are provided in this application, but those skilled in the art will recognize the application of other processes and / or the use of other materials.

[0054] Please see Figure 1 This application provides a motion trajectory planning method for a multi-degree-of-freedom robot, wherein the motion trajectory planning method includes:

[0055] Step S10: Construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of the multi-degree-of-freedom robot;

[0056] Step S20: Determine the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot based on the translational and rotational displacements and the maximum achievable velocity.

[0057] Step S30: When the translational motion of the multi-degree-of-freedom robot has a higher priority than the rotational motion, the position motion of the multi-degree-of-freedom robot is taken as the main motion, and a first cooperative planning spline curve is generated. The first cooperative planning spline curve is a cooperative planning spline curve from the position spline curve node vector to the attitude spline curve node vector.

[0058] Step S40: When the translational motion of the multi-degree-of-freedom robot has a lower priority than the rotational motion, the attitude motion of the multi-degree-of-freedom robot is taken as the main motion, and a second collaborative planning spline curve is generated. The second collaborative planning spline curve is a collaborative planning spline curve from the attitude spline curve node vector to the position spline curve node vector.

[0059] Please see Figure 2 This application provides a motion trajectory planning device 200 for a multi-degree-of-freedom robot. The motion trajectory planning device 200 includes a construction module 21, a determination module 22, a first generation module 23, and a second generation module 24. The motion trajectory planning method in this application can be implemented by the motion trajectory planning device 200.

[0060] Specifically, the construction module 21 is used to construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of the multi-degree-of-freedom robot; the determination module 22 is used to determine the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot based on the translational and rotational displacements and the maximum achievable velocity of the multi-degree-of-freedom robot; the first generation module 23 is used to generate a first co-planning spline curve when the priority of the translational motion of the multi-degree-of-freedom robot is greater than that of the rotational motion, taking the positional motion of the multi-degree-of-freedom robot as the main motion, and the first co-planning spline curve is a co-planning spline curve from the node vector of the position spline curve to the node vector of the attitude spline curve; the second generation module 24 is used to generate a second co-planning spline curve when the priority of the translational motion of the multi-degree-of-freedom robot is less than that of the rotational motion, taking the attitudeal motion of the multi-degree-of-freedom robot as the main motion, and the second co-planning spline curve is a co-planning spline curve from the node vector of the attitude spline curve to the node vector of the position spline curve.

[0061] Please see Figure 3 This application provides a computer device 100, which includes a memory 12 and a processor 11. The memory 12 stores computer programs. The processor 11 executes the computer programs to implement the motion trajectory planning method described in steps S10, S20, S30, and S40.

[0062] In the motion trajectory planning method of this application, position spline curves and attitude spline curves are constructed. Then, based on the priority levels of translational and rotational motions of the multi-degree-of-freedom robot, translational motion or rotational motion is selected as the main motion. This generates collaborative planning spline curves from position spline curve node vectors to attitude spline curve node vectors and from attitude spline curve node vectors to position spline curve node vectors, respectively. This not only ensures the independence of position spline curves and attitude spline curves but also significantly reduces the fluctuation rate of feed speed. It can also achieve synchronous planning of position and attitude, even in cases where the position change is large and the attitude change is small or the position change is small and the attitude change is large.

[0063] Specifically, the computer device 100 in the embodiments of this application may be a calculator, programmable controller, desktop computer, laptop computer, tablet computer, server, etc. The computer device 100 may include a processor 11, memory 12, communication interface, display screen and input device connected through a system bus.

[0064] The processor 11 of the computer device 100 provides computing and control capabilities, and the memory 12 includes a non-volatile storage medium and an internal memory 12. The non-volatile storage medium stores the operating system and computer programs, and the internal memory 12 provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium.

[0065] The communication interface of the computer device 100 is used to communicate with external terminals via wired or wireless means. The wireless means can be achieved through WIFI, carrier networks, NFC (near field communication) or other technologies. When the computer program is executed by the processor 11, it implements a motion trajectory planning method for a high-degree-of-freedom robot, which is designed for more complex application scenarios.

[0066] The display screen of the computer device 100 can be an LCD screen or an e-ink screen. The input device of the computer device 100 can be a touch layer covering the display screen, or buttons, trackballs or touchpads provided on the casing of the computer device 100, or external keyboards, touchpads or mice, etc.

[0067] In one embodiment, the memory 12 and processor 11 can be implemented using a general-purpose personal computer or a robot computer mounted on the robot. The robot has been equipped with necessary components such as sensors and actuators according to existing technology, enabling it to acquire the necessary data for the processor to process, and the actuators to execute the processor's processing results.

[0068] For five-axis CNC machine tools and six-axis or more degrees of freedom robots, complex motion trajectories need to be executed in Cartesian space for applications such as gluing, cutting, welding, and material handling. Generally, a trajectory in Cartesian space consists of two parts: the position component and the attitude component. Implementing complex trajectory planning in Cartesian space typically requires solving the following problems: how to represent the position component, how to represent the attitude component, and how to achieve simultaneous planning of position and attitude.

[0069] Typically, the position and attitude interpolation curves for multiple points are expressed using spline curves, and their implementation is relatively mature. However, there are few readily available solutions to address the problem of simultaneous planning of the position and attitude of multiple points, i.e., ensuring that the position and attitude of the points traversed are consistent with the given position and attitude of the waypoints at the same moment of motion.

[0070] The existing spline curve position and attitude synchronization planning scheme ensures that the node vectors of the position spline curve and the attitude spline curve are consistent.

[0071] This approach yields good results for situations with uniform changes in position and orientation. However, it may cause fluctuations in the machine tool feed rate when faced with large position changes and small orientation changes, or vice versa. This means fluctuations in the speed of the part of the CNC machine tool that holds the workpiece. It's understandable that the feed rate not only determines the machining efficiency but also significantly impacts surface quality. Drastic feed rate fluctuations will severely affect the lifespan of both the cutting tool and the machine tool. Therefore, maintaining a high and stable feed rate is crucial for CNC machining. Effectively controlling this feed rate is one of the key issues in post-processing technology for five-axis CNC machining.

[0072] Therefore, fluctuations in feed rate can lead to poor motion performance, making it difficult to achieve satisfactory results in this synchronous planning scheme.

[0073] In view of this, the motion trajectory planning method for multi-degree-of-freedom robots in this application establishes a one-to-one correspondence between the node vectors of the position spline curve and the node vectors of the attitude spline curve. This ensures the independence of the position spline curve and the attitude spline curve while significantly reducing the fluctuation rate of the feed speed. Thus, even when the position and attitude change unevenly, synchronous planning of position and attitude can be achieved.

[0074] Degrees of freedom (DOF) are a crucial technical indicator for robots, determined by their structure and directly impacting their mobility. In steps S10, S20, S30, and S40, the multi-DOF robot refers to a robot with five or more degrees of freedom. Multi-DOF robots can be five-axis CNC machine tools or robots with six or more axes of freedom. Generally, the position and orientation of robots, such as robotic arms, can be represented using three methods: actuator space description, joint space description, and Cartesian space description. The motion trajectory of the multi-DOF robot discussed in this application is a complex motion trajectory executed in Cartesian space.

[0075] Taking a multi-degree-of-freedom robotic arm as an example, the trajectory of the robotic arm is the history of the position, velocity, and acceleration of the end effector / operation point over time. The trajectory planning of the robotic arm mainly includes the following processes: First, determine the key points that the robotic arm must pass through to complete a specific task, and record the position and posture information of each key point. Then, determine the path type between each key point (straight line, circular arc, spline curve, and free curve, etc.). Finally, determine the motion law, that is, determine the correspondence between position and posture, velocity and acceleration, etc., and time, so that the robotic arm can execute it.

[0076] It is understandable that trajectory planning plays a crucial role in determining the smoothness and high speed of robot motion. Robot trajectory planning includes position planning and pose planning. Both position planning and pose planning have mature existing algorithms, and the interpolation curves of position and pose at multiple key points are expressed by spline curves. Therefore, step S10 can be used to construct the position spline curve and pose spline curve in Cartesian space.

[0077] A spline curve is a smooth curve that passes through a series of given points. A spline curve not only passes through each ordered point, but also has continuous first and second derivatives at each point, meaning that the curve has the characteristics of continuity and uniform curvature change.

[0078] Specifically, the order of spline curves mentioned below is related to their degree; the order of a spline curve is its degree plus one. A higher order means more control points. A quadratic spline has an order of three, and its segment is determined by three control points; a cubic spline has an order of four, and its segment is determined by four control points.

[0079] In other words, a spline curve is a curve obtained given a set of control points. The approximate shape of the curve is controlled by these points. Generally, splines can be divided into interpolation splines and approximation splines. The position spline curve and attitude spline curve in this application can be interpolation splines. Interpolation means interpolating a continuous function based on discrete data, so that the continuous curve passes through all discrete points, and at the same time, the approximate value of the function at other points can be estimated.

[0080] Spline interpolation is a mathematical method for constructing a smooth curve passing through a series of points using variable splines. Interpolation splines consist of polynomials, each determined by two adjacent data points, such that any two adjacent polynomials and their derivatives are continuous at the connection point.

[0081] Then, in step S20, the priority of the robot's joint motion types can be determined, that is, the priority of translational motion and rotational motion can be determined. Specifically, the priority of one motion type can be determined based on the robot's translational displacement, rotational displacement, and the maximum achievable speed.

[0082] Translation, also known as translational or parallel movement, is a special form of mechanical motion and a fundamental type of motion for rigid bodies. A straight line connecting any two points on a moving object remains parallel throughout the entire motion; this is called "translation." At any given moment, the velocity and acceleration of all points on the moving object are the same. Therefore, when studying the translation of an object, its size and shape can be disregarded, and it can be treated as a point mass. Correspondingly, rotation, as a fundamental form of mechanical motion, involves all points on a moving object, except those on the axis of rotation, moving in circles of varying sizes around the same axis. Furthermore, the maximum speed a robot can achieve during translational or rotational motion can be specified by the user.

[0083] After determining the priority of motion forms, the first or second collaborative planning spline curve can be determined according to steps S30 and S40, depending on the specific circumstances. It is understood that in some embodiments, ensuring the consistency of the node vectors of the position spline curve and the attitude spline curve when the position and attitude change uniformly can achieve a better synchronous planning effect for position and attitude.

[0084] In other embodiments, for example, when there are uneven changes in position and attitude, such as large changes in position and small changes in attitude, or small changes in position and large changes in attitude, the one-to-one correspondence between the node vectors of the position spline curve established in this application can be used to achieve a good synchronous planning effect for position and attitude.

[0085] Please see Figure 4 In some implementations, the position spline curve and attitude spline curve in the spline curve for constructing the motion trajectory of a multi-degree-of-freedom robot include:

[0086] Step S11: Calculate the sum of the products of the control vertices of a predetermined number of B-spline curves and the corresponding cubic B-spline basis functions to obtain the position spline curves.

[0087] In some implementations, the construction module 21 can be used to calculate the sum of the products of a predetermined number of control vertices of B-spline curves and their corresponding cubic B-spline basis functions to obtain position spline curves.

[0088] In some implementations, the processor 11 is used to execute a computer program to implement the motion trajectory planning method as described in step S11.

[0089] Specifically, a B-spline curve is a special representation in numerical analysis, a sub-discipline of mathematics. A B-spline curve is a linear combination of B-spline basis functions, a generalization of Bézier curves. B-spline surfaces possess many excellent properties, including geometric invariance, convex hull property, convexity preservation, variation reduction, and local support. Given several control points and a node vector, a B-spline curve can be defined by the control points and the node vector.

[0090] In one embodiment, in step S11, the control vertices included in the B-spline curve can be set by the actual robot motion trajectory to be planned. The control vertices are the control points mentioned above, and the B-spline basis functions can be defined by the node vectors and the degrees of the basis functions. In this embodiment, the final curve position, i.e., the position spline curve, can be obtained by summing the products of a predetermined number of control vertices included in the B-spline curve and the corresponding cubic B-spline basis functions.

[0091] Specifically, the degree of the B-spline basis function is three to ensure its C2 continuity (mathematically defined as second-order continuous differentiability at the splice). That is to say, in other embodiments, the degree of the B-spline basis function can be higher, thereby constructing a higher-order continuous B-spline curve. In other words, it is also possible to construct a monotonically increasing B-spline curve with third-order continuous differentiability or even higher-order continuous at the splice. This application does not impose any inherent limitations on this.

[0092] In some implementations, the position spline curve is obtained according to the following relationship:

[0093]

[0094] Where p(u) is the position spline curve, d i N is the control vertex of the B-spline curve. i,k(u) is a k-th degree B-spline basis function, where k=3 is the cubic B-spline basis function. In particular, the degree of the B-spline basis function is three to ensure its C2 continuity (mathematically defined as being twice continuously differentiable at the splice). That is to say, in other embodiments, the degree of the B-spline basis function can be higher, thereby constructing a higher-order continuous B-spline curve. This application does not inherently limit this.

[0095] Thus, the final curve position, i.e., the position spline curve p(u), is the weight value N of all elements in the third order of the B-spline basis function. i,3 (u) and the corresponding control vertex, i.e., d i The sum is obtained by multiplying the products of the products and then adding them together.

[0096] In some implementations, the attitude spline curve can be obtained from the following relationship:

[0097]

[0098] Where, q i (u) is the attitude spline curve, Q j (j=i-1,…,i+2) are attitude control vertices represented by unit quaternions. It is a fifth-degree polynomial function.

[0099] Specifically, as mentioned above, there are already mature existing algorithms for both position planning and attitude planning in robot trajectory planning. For example, position planning includes velocity planning and interpolation planning; attitude planning, when done independently, includes Euler angle representation, rotation matrix representation, and quaternion representation. Due to the singularities of Euler angles (e.g., the result of "roll 90 degrees and then yaw 90 degrees" and "pitch 90 degrees and then roll 90 degrees" can yield the same result) and the orthogonality of rotation matrices, these two methods are difficult to apply when facing random paths. Quaternion attitude, on the other hand, has the characteristics of no singularities and easy interpolation. At the same time, quaternions are superior to Euler angles in terms of both accuracy of representation and convenience of calculation. Especially for the attitude solution process, quaternion multiplication is the attitude update process. Therefore, quaternions are currently commonly used as the attitude description method.

[0100] Quaternions can solve the gimbal lock problem of Euler angles and the problem that the nine values ​​of the rotation matrix are not independent. In a quaternion, the four variables are independent and the gimbal lock problem is avoided. Using quaternions to describe attitude is called attitude quaternion, which is widely used in the field of attitude calculation.

[0101] Compared to the two-dimensional space of complex numbers, complex numbers were generalized to solve the problem of rotational transformations in three-dimensional space, resulting in quaternions. Quaternions can be understood as four-dimensional space, with the three imaginary parts representing three orthogonal bases of the three-dimensional space, while the real parts are perpendicular to the three-dimensional space.

[0102] In one embodiment, Q j (j = i-1, ..., i+2) can be used as attitude control vertices represented by unit quaternions, q i (u) is the attitude spline curve. Given a fifth-order polynomial function, a quaternion interpolation spline curve based on the parametric fifth-order polynomial can be calculated using the above relationship.

[0103] In some implementations, the position spline curves and attitude spline curves in the spline curves of the motion trajectory of a multi-degree-of-freedom robot are both second-order continuous and differentiable at the splice points. Specifically, when constructing position spline curves based on multiple B-spline basis functions and using B-spline curves passing through vertices, and when constructing attitude spline curves based on polynomial quaternion spline curves, in order to satisfy that the spline curves are both second-order continuous and differentiable at the splice points, i.e., C2 continuous, the B-spline basis functions can be cubic basis functions, and the multiple polynomials can be fifth-order polynomials.

[0104] Please see Figure 5 In some implementations, the displacement of the translational motion of the multi-degree-of-freedom robot is denoted as translational displacement, the displacement of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational displacement, the velocity of the translational motion of the multi-degree-of-freedom robot is denoted as translational velocity, and the velocity of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational velocity. Determining the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot (step S20) includes:

[0105] Step S21: Calculate the translational running time based on the translational displacement and translational velocity;

[0106] Step S22: Calculate the rotational running time based on the rotational displacement and rotational speed;

[0107] Step S23: If the translational running time is greater than or equal to the rotational running time, determine that the priority of the translational motion is higher than the priority of the rotational motion;

[0108] Step S24: If the translational running time is less than the rotational running time, determine that the priority of the translational motion is lower than the priority of the rotational motion.

[0109] In some embodiments, the determining module 22 can be used to calculate the translational running time based on the translational displacement and translational velocity, and to calculate the rotational running time based on the rotational displacement and rotational velocity, and to determine that the priority of the translational motion is higher than the priority of the rotational motion when the translational running time is greater than or equal to the rotational running time, and to determine that the priority of the translational motion is lower than the priority of the rotational motion when the translational running time is less than the rotational running time.

[0110] In some implementations, the processor 11 may be used to execute a computer program to implement the motion trajectory planning method as described in steps S21, S22, S23 and S24.

[0111] Thus, the method for determining the priority of a robot's motion patterns is simple, reliable, and easy to implement.

[0112] Specifically, translation, also known as translational motion or parallel movement, is a special form of mechanical motion and a fundamental type of motion for rigid bodies. The straight line connecting any two points on a moving object remains parallel throughout the entire motion; this type of motion is called "translation." At any given moment, the velocity and acceleration of all points on a moving object are the same. Therefore, when studying the translation of an object, its size and shape can be disregarded, and it can be treated as a point mass. Correspondingly, rotation, as a fundamental form of mechanical motion, involves all points on a moving object, except those on the axis of rotation, moving in circles of varying sizes around the same axis of rotation; this type of motion is called "rotation."

[0113] In one embodiment, let the translational displacement of the robot's translational motion in spline motion within Cartesian space be l. trans Let the rotational displacement of the rotational motion be l. ori Meanwhile, let the translational velocity of the robot during its translational motion be v. trans The rotational speed when it is in rotational motion is v ori Among them, the translational velocity v trans With rotational speed v ori This can be specified by the user.

[0114] Therefore, after steps S21 and S22, the running time T of the robot's translational motion can be calculated using the following formulas. trans and the running time T of rotational motion ori :

[0115]

[0116]

[0117] Then, in one embodiment, if T trans ≥T ori That is, when the translational velocity is greater than or equal to the rotational velocity, the robot's spline motion in Cartesian space prioritizes translational motion, meaning the priority of translational motion is greater than the priority of rotational motion; in another embodiment, if T trans <T ori When the rotational speed is greater than the translational speed, the spline motion of the robot in Cartesian space takes priority over rotational motion.

[0118] In some implementations, the first collaborative programming spline curve can be obtained according to the following formula:

[0119]

[0120] Where y(x) is the first co-planning spline curve, x is the node vector of the position spline curve, y is the node vector of the attitude spline curve, and h i-1 =x i -x i-1 (i = 2, ..., N), N is the number of nodes in the domain of the position spline curve, and d i (i=1,…,N) represents the first derivative of the first co-programming spline curve at each node.

[0121] Specifically, in one embodiment, in the expression of the first collaboratively planned spline curve y(x), d i The approximate value of the solution to a system of linear equations can be calculated based on the principle of the Gauss-Seidel method. The Gauss-Seidel method is an iterative method in numerical linear algebra that can be used to find the approximate value of the solution to a system of linear equations. The specific calculation process is as follows:

[0122] Step 1: Let the iteration number k = 1, and calculate d. i Initial values ​​for iterations of (i = 1, ..., N):

[0123]

[0124] In (1):

[0125]

[0126]

[0127] In (3):

[0128]

[0129]

[0130] In (5):

[0131]

[0132]

[0133] Step 2: Let d k =d k-1 Gauss-Seidel iteration was used to calculate

[0134]

[0135] In (8):

[0136]

[0137]

[0138]

[0139] Step 3: Determine if the iteration conditions are met: Where em is the convergence error, which is taken as em = 10 here. -10 If the iteration condition is met, the calculation is terminated; otherwise, let k = k + 1 and proceed to step 2.

[0140] Thus, through steps 1-3, the expression for the first collaborative programming spline curve is completed, specifically the expression for d. i The calculation of (i = 1, ..., N). The above calculation method is an iterative algorithm designed based on the principle of Gauss-Seidel iterative algorithm and conforms to the scheme of this application. The selection of its boundary conditions meets the actual needs of this scheme.

[0141] Thus, we obtain the first co-planning spline curve that is monotonically increasing and C2 continuous (second-order continuous and differentiable at the splice) from the position spline curve node vector to the attitude spline curve node vector under the condition of prioritizing translational motion.

[0142] In some implementations, the second collaborative programming spline curve can be obtained according to the following formula:

[0143]

[0144] Where y(x) is the second co-programming spline curve, x is the node vector of the attitude spline curve, y is the node vector of the position spline curve, and h i-1 =x i -x i-1 (i = 2, ..., N), θ=(xx i-1 ) / h i-1 (i = 2, ..., N), where N is the number of nodes in the domain of the attitude spline curve, and d i (i=1,…,N) represents the first derivative of the second collaborative programming spline curve at each node.

[0145] Specifically, in one embodiment, in the expression of the second collaborative programming spline curve y(x), d i It can be calculated based on the principle of the Gauss-Seidel iterative algorithm, and the specific calculation process is as follows:

[0146] Step 1: Let the iteration number k = 1, and calculate d. i Initial values ​​for iterations of (i = 1, ..., N):

[0147]

[0148] In (1):

[0149]

[0150]

[0151] In (3):

[0152]

[0153]

[0154] In (5):

[0155]

[0156]

[0157] Step 2: Let d k =d k-1 Gauss-Seidel iteration was used to calculate

[0158]

[0159] In (8):

[0160]

[0161]

[0162]

[0163] Step 3: Determine if the iteration conditions are met: Where em is the convergence error, which is taken as em = 10 here. -10 If the iteration condition is met, the calculation is terminated; otherwise, let k = k + 1 and proceed to step 2.

[0164] Thus, by completing steps 1-3, the expression for the second collaborative programming spline curve is obtained by modifying d. i The calculation of (i = 1, ..., N). The above calculation method is an iterative algorithm designed based on the principle of Gauss-Seidel iterative algorithm and conforms to the scheme of this application. The selection of its boundary conditions meets the actual needs of this scheme.

[0165] It is understandable that, when generating the second co-programming spline curve from the attitude spline curve node vector to the position spline curve node vector, prioritizing rotational motion and taking attitude as the main motion, the d... i (i=1,…,N), that is, the solution of the first derivative of the second co-programming spline curve at each node is the same as the solution of the first co-programming spline curve at each node. The only difference is that in the process of constructing the second co-programming spline curve, the node vector of the attitude spline curve can be set as x and the node vector of the position spline curve can be set as y.

[0166] Thus, we obtain the second co-programming spline curve, which is monotonically increasing and C2 continuous (second-order continuous and differentiable at the splice) from the attitude spline curve node vector to the position spline curve node vector, under the condition of rotational motion priority.

[0167] Please see Figure 6 , Figure 7 and Figure 8 In some embodiments, both the first and second co-programming spline curves are monotonically increasing and second-order continuously differentiable spline curves at the splice junction. Thus, by constructing a second-order continuously differentiable monotonically increasing spline curve at the splice junction, a one-to-one correspondence is established between the node vectors of the position spline curve and the node vectors of the attitude spline curve. This not only ensures the independence of the position and attitude spline curves but also significantly reduces the fluctuation rate of the feed rate. Of course, in other embodiments, a third-order continuously differentiable or even higher-order continuously increasing monotonically increasing spline curve at the splice junction can also be constructed; this application does not inherently limit this.

[0168] In one embodiment, an example demonstrates the effectiveness of the motion trajectory planning method provided in this application, such as... Figures 6-8 As shown, the data for the four feature points are as follows:

[0169] P1(0.881712,0.803469,0.868705,0.224697,0.858523,0.451427,0.093073);

[0170] P2(0.588026,0.157438,1.616001,0.044643,-0.931653,-0.122654,-0.339094);

[0171] P3(1.114594,-0.240028,0.953928,0.247861,-0.948919,0.192493,-0.032632);

[0172] P4(0.367706,0.485037,1.039052,0.337795,-0.859129,-0.337795,0.183539).

[0173] Among them, P1, P2, P3 and P4 are point codes. The point data of the feature points of P1-P4 are composed of position data and attitude data, respectively. In the attitude data, it is represented by a unit quaternion, that is to say, the attitude data includes four parts: w, x, y and z.

[0174] Then, the position spline curve represented by the B-spline curve obtained by applying the algorithm provided in this application is as follows: Figure 6 As shown, the attitude spline curve represented by a quaternion interpolation spline curve based on a fifth-order polynomial is obtained by applying the algorithm provided in this application. Figure 7 As shown, after determining the priority of motion forms, rotation is prioritized in Cartesian space, with attitude motion as the primary motion. The second co-planning spline curve, which generates attitude spline curve node vectors to position spline curve node vectors, is the Cartesian space spline curve as shown. Figure 8 As shown.

[0175] From Figure 8 It is easy to see that when the running time is 0.0s, 2.16s, 4.13s, and 6.30s, the positions and attitudes of the points passing through point P1 to point P4 are simultaneously observed.

[0176] That is to say, when the running time is 0.0s, Figure 8 The specific values ​​for px are 0.881712, py is 0.803469, pz is 0.868705, qw is 0.224697, qx is 0.858523, qy is 0.451427, and qz is 0.093073.

[0177] At a runtime of 2.16 seconds, Figure 8 The specific values ​​for px are 0.588026, py is 0.157438, pz is 1.616001, qw is 0.044643, qx is -0.931653, qy is -0.122654, and qz is -0.339094.

[0178] At a runtime of 4.13 seconds, Figure 8The specific values ​​for px are 1.114594, py is -0.240028, pz is 0.953928, qw is 0.247861, qx is -0.948919, qy is 0.192493, and qz is -0.032632.

[0179] At a runtime of 6.30 seconds, Figure 8 The specific values ​​for px are 0.367706, py is 0.485037, pz is 1.039052, qw is 0.337795, qx is -0.859129, qy is -0.337795, and qz is 0.183539.

[0180] This verifies the effectiveness of the method proposed in this application.

[0181] In summary, existing spline curve position and attitude synchronization planning schemes achieve this by ensuring that the node vectors of the position spline curve and the attitude spline curve are consistent.

[0182] This approach yields good results for situations with uniform changes in position and orientation. However, it may cause fluctuations in the machine tool feed rate when faced with large position changes and small orientation changes, or vice versa. This means fluctuations in the speed of the part of the CNC machine tool that holds the workpiece. It's understandable that the feed rate not only determines the machining efficiency but also significantly impacts surface quality. Drastic feed rate fluctuations will severely affect the lifespan of both the cutting tool and the machine tool. Therefore, maintaining a high and stable feed rate is crucial for CNC machining. Effectively controlling this feed rate is one of the key issues in post-processing technology for five-axis CNC machining.

[0183] Therefore, feed rate fluctuations can lead to poor motion performance, making it difficult to achieve satisfactory results in this synchronous planning scheme. Thus, although good planning results can be obtained when the pose changes uniformly, feed rate fluctuations caused by non-uniform pose changes affect the planning results.

[0184] In this application, for situations where large position changes and small attitude changes, or small position changes and large attitude changes, can cause fluctuations in the feed rate of CNC machine tools, a continuous monotonically increasing C2 spline curve is constructed to establish a one-to-one correspondence between the node vectors of the position spline curve and the node vectors of the attitude spline curve. This not only ensures the independence of the position spline curve and the attitude spline curve, but also significantly reduces the fluctuation rate of the feed rate.

[0185] The motion trajectory planning method provided in this application can be used for complex Cartesian space trajectory planning of five-axis CNC machine tools and six-axis or more degree-of-freedom robots, and can be applied to more complex application scenarios. This application establishes a one-to-one correspondence between the node vectors of the position spline curve and the node vectors of the attitude spline curve by constructing monotonically increasing C2 continuous spline curves. Subsequently, C3 or even higher-order continuous monotonically increasing spline curves can be constructed.

[0186] This application provides a non-volatile computer-readable storage medium storing a computer program, which, when executed by one or more processors 11, causes the processors 11 to perform the motion trajectory planning method of any of the above embodiments.

[0187] Specifically, the computer device 100 in the embodiments of this application may be a calculator, programmable controller, desktop computer, laptop computer, tablet computer, server, etc. The computer device 100 may include a processor 11, memory 12, communication interface, display screen and input device connected through a system bus.

[0188] In one embodiment, the computer device 100 may further include a processor 11, which may be a central processing unit (CPU). The processor 11 may also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or combinations thereof.

[0189] Computer programs can be stored in memory 12. Memory 12, as a non-transitory computer-readable storage medium, can be used to store non-transitory software programs, non-transitory computer-executable programs, and modules, such as the program instructions / modules corresponding to the methods in the above method embodiments. Processor 11 executes various functional applications and data processing of processor 11 by running the non-transitory software programs, instructions, and modules stored in memory 12, thereby implementing the methods in the above method embodiments.

[0190] Any process or method described in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the function involved, as will be understood by those skilled in the art to which embodiments of this application pertain.

[0191] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processing module, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0192] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor.

[0193] It should be understood that various parts of the embodiments of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0194] Those skilled in the art will understand that all or part of the steps of the methods described in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it includes one or a combination of the steps of the method embodiments.

[0195] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0196] The storage media mentioned above can be read-only memory, disk, or optical disk, etc.

[0197] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. A method for planning the motion trajectory of a multi-degree-of-freedom robot, characterized in that, include: Construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of a multi-degree-of-freedom robot; Based on the translational and rotational displacements and the maximum achievable velocity of the multi-degree-of-freedom robot, the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot is determined. When the translational motion of the multi-degree-of-freedom robot has a higher priority than the rotational motion, the positional motion of the multi-degree-of-freedom robot is taken as the main motion, and a first cooperative planning spline curve is generated. The first cooperative planning spline curve is a cooperative planning spline curve from the position spline curve node vector to the attitude spline curve node vector. When the translational motion of the multi-degree-of-freedom robot has a lower priority than the rotational motion, the attitude motion of the multi-degree-of-freedom robot is taken as the main motion, and a second collaborative planning spline curve is generated. The second collaborative planning spline curve is a collaborative planning spline curve from the attitude spline curve node vector to the position spline curve node vector. The position spline curve and attitude spline curve in the spline curve for constructing the motion trajectory of the multi-degree-of-freedom robot include: The position spline curve is obtained by summing the products of a predetermined number of control vertices of B-spline curves and their corresponding cubic B-spline basis functions. The position spline curve is obtained according to the following relationship: in, For the position spline curve, These are the control vertices of the B-spline curve. yes B-spline basis functions The time is the cubic B-spline basis function; The attitude spline curve is obtained from the following relationship: in, The attitude spline curve, ( ) represents the attitude control vertex in terms of element quaternions. ( () is a fifth-degree polynomial function; The first collaborative planning spline curve is obtained according to the following formula: （ ） in, Let the first collaborative planning spline curve be... The node vector of the position spline curve. The node vector of the attitude spline curve. , , Define the number of nodes within the domain of the position spline curve. ( ) represents the first derivative of the first collaborative planning spline curve at each node; The second collaborative programming spline curve is obtained according to the following formula: （ ） in, For the second collaborative programming spline curve, The node vector of the attitude spline curve. The node vector of the position spline curve. , , , Define the number of nodes within the domain of the attitude spline curve. ( ) represents the first derivative of the second collaborative programming spline curve at each node.

2. The motion trajectory planning method for a multi-degree-of-freedom robot according to claim 1, characterized in that, The position spline curve and attitude spline curve in the motion trajectory of the multi-degree-of-freedom robot are both second-order continuous and differentiable at the spline junction.

3. The motion trajectory planning method for a multi-degree-of-freedom robot according to claim 1, characterized in that, The displacement of the translational motion of the multi-degree-of-freedom robot is denoted as translational displacement, the displacement of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational displacement, the velocity of the translational motion of the multi-degree-of-freedom robot is denoted as translational velocity, and the velocity of the rotational motion of the multi-degree-of-freedom robot is denoted as rotational velocity. Determining the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot includes: The translational running time is calculated based on the translational displacement and the translational velocity; The rotational running time is calculated based on the rotational displacement and the rotational speed. If the translational running time is greater than or equal to the rotational running time, the priority of the translational motion is determined to be higher than the priority of the rotational motion. If the translational running time is less than the rotational running time, the priority of the translational motion is determined to be lower than the priority of the rotational motion.

4. The motion trajectory planning method for a multi-degree-of-freedom robot according to claim 1, characterized in that, Both the first and second collaborative programming spline curves are monotonically increasing spline curves that are second-order continuously differentiable at the splice points.

5. A motion trajectory planning device for a multi-degree-of-freedom robot, characterized in that, The motion trajectory planning device is used to execute the motion trajectory planning method for a multi-degree-of-freedom robot as described in any one of claims 1-4, and includes: The construction module is used to construct the position spline curve and attitude spline curve in the spline curve of the motion trajectory of a multi-degree-of-freedom robot; The determination module is used to determine the motion priority of the translational and rotational motions of the multi-degree-of-freedom robot based on the translational and rotational displacements and the maximum achievable speed of the multi-degree-of-freedom robot. The first generation module is used to generate a first cooperative planning spline curve, with the position motion of the multi-degree-of-freedom robot as the main motion when the priority of the translational motion of the multi-degree-of-freedom robot is greater than that of the rotational motion. The first cooperative planning spline curve is a cooperative planning spline curve from the position spline curve node vector to the attitude spline curve node vector. The second generation module is used to generate a second collaborative planning spline curve, where the priority of the translational motion of the multi-degree-of-freedom robot is lower than that of the rotational motion, taking the attitude motion of the multi-degree-of-freedom robot as the main motion. The second collaborative planning spline curve is a collaborative planning spline curve from the attitude spline curve node vector to the position spline curve node vector.

6. A computer device, characterized in that, The computer device includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the motion trajectory planning method for a multi-degree-of-freedom robot as described in any one of claims 1-4.

7. A non-volatile computer-readable storage medium with computer-executable instructions, characterized in that, When the computer-executable instructions are executed by one or more processors, the processors perform the motion trajectory planning method for a multi-degree-of-freedom robot as described in any one of claims 1-4.

Images