Industrial robot speed accurate planning method and control system

Abstract

The present application relates to the technical field of industrial robots, in particular to the speed planning of industrial robots. According to a given path, the present application firstly calculates the maximum allowable speed at each path point under the constraints of joint speed, acceleration, jerk and torque. Subsequently, two polynomial curves are defined to achieve smooth speed transition between adjacent path points, which are used to simulate the acceleration and deceleration process. On this basis, a conservative speed curve consisting of three stages of acceleration, constant speed and deceleration is constructed. The speed of the constant speed stage and the number of path points involved in the acceleration and deceleration stages are determined by bisection method. Finally, the constant speed stage in the existing speed curve is iteratively improved adaptively using the same method. The present application improves the working efficiency of the robot, enhances its adaptability in complex environment, and can simultaneously meet the kinematic and dynamic constraints, ensuring the stability and safety of the robot during high-speed operation, while improving the overall performance.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial robots, in particular to an industrial robot speed accurate planning method and a control system. BACKGROUND

[0002] In the field of large and complex parts processing such as aerospace, the application of industrial robots has attracted widespread attention. Compared with computer numerical control (CNC) machine tools, industrial robots have significant advantages, mainly in larger working space, higher motion flexibility, and lower equipment cost, etc. When industrial robots perform polishing, milling, grinding and deburring and other processing tasks, speed planning is a core technical link, and its essence is to parameterize the given geometric path as a function of time. In order to achieve the stability and efficiency of the processing process, the time-optimal speed planning method is usually used, while ensuring that the motion of the industrial robot meets the multiple physical constraints of speed, acceleration, jerk (derivative of acceleration) and torque, etc.

[0003] However, the speed planning of industrial robots faces unique challenges: first, due to the highly nonlinear kinematic mapping relationship between the joint space of industrial robots and the operating space of the end effector, this complex coupling characteristic makes it difficult to accurately satisfy the kinematic constraints; secondly, compared with CNC machine tools, the structural stiffness of industrial robots is relatively low, so the torque constraint becomes an important factor that cannot be ignored in speed planning. SUMMARY

[0004] The present application aims to at least solve the technical problems existing in the prior art. To this end, the present application proposes an industrial robot speed accurate planning method, which satisfies the kinematic and dynamic constraints simultaneously for a random complex geometric path of the industrial robot.

[0005] According to the industrial robot speed accurate planning method according to some embodiments of the first aspect of the present application, the speed planning method comprises the following steps:

[0006] S110, if the given path is composed of N straight line segments, there are N+1 path points, and the parameter of the path point is the cumulative length s of the path point on the path i (i = 0, 1, …, N), the path point is converted to the position in the mechanical arm joint space through the inverse kinematics model: q i ∈ R n where n is the number of joints of the mechanical arm;

[0007] S120, the first derivative of the position in the mechanical arm joint space with respect to the cumulative length q' i , the second derivative q" i and the third derivative q" i are calculated by discrete approximation;

[0008] S130, the maximum allowable speed at each path point under joint acceleration constraint is calculated as The maximum allowable speed at each path point under joint acceleration constraint is calculated as The maximum allowable speed at each path point under joint jerk constraint is calculated as The maximum allowable speed at each path point under joint torque constraint is calculated as v i τ ;

[0009] S140, the maximum allowable speed at each path point under uniform motion is calculated

[0010]

[0011] The industrial robot speed accurate planning method according to some embodiments of the first aspect of the present application has at least the following beneficial effects:

[0012] The present application optimizes the speed planning method, so that the industrial robot can be more flexible and efficient when executing a complex geometric path, improves the working efficiency of the robot, and also enhances its adaptability in complex environments, and can simultaneously meet the kinematic and dynamic constraints, including multiple physical constraints such as speed, acceleration, jerk (i.e. derivative of acceleration) and torque, etc., to ensure the stability and safety of the robot when running at high speed, while improving the overall performance.

[0013] The industrial robot speed accurate planning method according to some embodiments of the first aspect of the present application, the first derivative of the position in the joint space of the mechanical arm with respect to the cumulative length q' i , the second derivative q" i and the third derivative q" i Discrete approximation calculation is as follows:

[0014]

[0015] The industrial robot speed accurate planning method according to some embodiments of the first aspect of the present application, the S140 is specifically:

[0016] Let the given maximum tangential speed be V max , and the maximum speed of each joint be Let the maximum allowable speed at each path point under joint speed constraint be The calculation formula is as follows:

[0017]

[0018] Let the given maximum acceleration of each joint be Let the maximum allowable velocity at each path point under the joint acceleration constraint be The calculation formula is as follows:

[0019]

[0020] Let the given maximum jerk of each joint be Let the maximum allowable velocity at each path point under the joint jerk constraint be The calculation formula is as follows:

[0021]

[0022] Let the given maximum and minimum torque of each joint be τ max,j and τ min,j Let the maximum allowable velocity at each path point under the joint torque constraint when uniform motion is allowed be v i τ The calculation formula is as follows:

[0023]

[0024] Wherein M i , C i , c i are the inertia matrix, Coriolis force and centrifugal force terms, gravity and Coulomb friction force terms at the path point q i .

[0025] According to the industrial robot speed accurate planning method of some embodiments of the first aspect of the present application, after the S140, the method further comprises the step S200 of generating a conservative speed curve based on a quadratic polynomial curve interpolation.

[0026] According to the industrial robot speed accurate planning method of some embodiments of the first aspect of the present application, the S200 comprises:

[0027] S210, a quadratic polynomial curve v(ω) = C 3,2 ω 2 +C 3,1 ω+C 3,0 is given, where ω ∈ [-1, 0], assuming that the given interpolation boundary conditions are the initial speed v(-1) = v1, the terminal speed v(0) = v2 and the terminal acceleration a(0) = 0, then the parameters of the quadratic polynomial can be obtained as C 3,2 = v1-v2, C 3,1 = 0 and C 3,0 = v2, obtaining v(ω) = (v1-v2)ω 2 +v2;

[0028] S220, mapping the parameter of the path point into [-1, 0] according to the following formula:

[0029]

[0030] According to the above quadratic polynomial curve interpolation, the speed of n vc path points involved is as follows:

[0031] v r = (v1-v2)ω r 2 + v2(r = k, …, k+n vc -1);

[0032] The acceleration is as follows:

[0033]

[0034] The jerk is as follows:

[0035]

[0036] S230, according to the joint position differential about arc length calculated by the above speed, acceleration, jerk and first-order derivative q' i , second-order derivative q' i ' and third-order derivative q' i , the joint speed can be obtained as follows:

[0037]

[0038] The joint acceleration is as follows:

[0039]

[0040] The joint jerk is as follows:

[0041]

[0042] The joint torque is as follows:

[0043]

[0044] S240, judging whether the quadratic polynomial speed curve meets the conditions of kinematics and dynamics constraints, if not, a conservative speed curve is given based on the quadratic polynomial curve, the conservative speed curve includes that the speed increases from the initial speed v s to the target speed v goal , and then maintains the target speed v goalThe robot moves at a constant speed, and finally the speed is reduced from the target speed v goal to the final speed v e , where v s = v e = 0, the target speed

[0045] According to the speed planning method of the industrial robot according to some embodiments of the first aspect of the present application, the S240 further comprises: judging whether the target speed v goal can be reached under given kinematic and dynamic constraints, and outputting the number of path points corresponding to the target speed that can be reached; wherein if the target speed cannot be reached, determining the actual speed that can be reached If the target speed v goal can be reached, determining the actual speed that can be reached in the acceleration process

[0046] According to the speed planning method of the industrial robot according to some embodiments of the first aspect of the present application, the S240 further comprises: judging whether the target speed v goal can be reached under given kinematic and dynamic constraints, and outputting the number of path points corresponding to the target speed that can be reached n ac Specifically comprises:

[0047] S241a, given initial speed v s and target speed v goal , let where represents the maximum integer not exceeding N / 2, n max represents the maximum value of the number of path points involved in the acceleration process;

[0048] S242a, let n current = 2 represent the number of path points, check whether the speed curve v(ω) in the form of a quadratic polynomial corresponding thereto satisfies each kinematic and dynamic constraint, if the constraints are satisfied, n ac = n current , at this time the target speed v goal can be reached, end; otherwise, it indicates that the number of path points needs to be increased, and the increment is n add , let

[0049] S243a, if n current +n add <n max , let n current = n current +n add , execute S244a; otherwise, let n add = n max -n current and ncurrent = n max , execute S245a;

[0050] S244a, check if the corresponding quadratic polynomial form of the velocity curve v(ω) at this time satisfies various kinematic and dynamic constraints, if the constraints are satisfied, then n ac = n current , the target velocity v goal can be reached, end; otherwise, let n add = 2n add , execute S243a.

[0051] S245a, check if the corresponding quadratic polynomial form of the velocity curve v(ω) at this time satisfies various kinematic and dynamic constraints, if the constraints are satisfied, then n ac = n current , the target velocity v goal can be reached; otherwise, output n ac = 0, indicating that the target velocity v goal cannot be reached.

[0052] According to the industrial robot speed accurate planning method according to some embodiments of the first aspect of the application, if the target velocity v goal cannot be reached, the bisection method is used to determine the actual velocity s in the interval [v goal , v s ] and the function represents the result of the bisection method, and the bisection method is used to determine the actual velocity goal in the interval [v l , v s ] and the function represents the result of the bisection method. The specific contents include:

[0053] S241b, set the left boundary of the velocity as v r = v goal , the right boundary as v r = v l , and the velocity threshold value at which the bisection method stops as Δv = 0.001;

[0054] S242b, if v m -v r > Δv, execute S243b; otherwise end;

[0055] S243b, let v l = (v ac + v s ) / 2, calculate n m = Num(v ac , vm ). If n ac = 0, let v r = v m , execute S242b; otherwise, let v l = v m , execute S242b.

[0056] According to the industrial robot speed accurate planning method of some embodiments of the first aspect of the application, after the S200, it further comprises the step S300 of improving the speed curve based on the cubic polynomial curve interpolation; the S300 comprises:

[0057] S310, give the cubic polynomial curve v(ω) = C 4,3 ω 3 +C 4,2 ω 2 +C 4,1 ω+C 4,0 , wherein ω ∈ [-1, 0], assuming that the given interpolation boundary conditions are the initial end speed v(-1) = v1, the initial end acceleration a(-1) = 0, the terminal speed v(0) = v2 and the terminal acceleration a(0) = 0, then the parameters of the cubic polynomial can be obtained as C 4,3 = 2(v1-v2), C 4,2 = 3(v1-v2), C 4,1 = 0 and C 4,0 = v2, that is, v(ω) = 2(v1-v2)ω 3 +3(v1-v2)ω 2 +v2.

[0058] S320, map the parameters of the path points according to into [-1, 0], then the corresponding parameters in [-1, 0] when the path points adopt the v(ω) interpolation speed curve are obtained;

[0059] According to the above-mentioned cubic polynomial curve interpolation, the speeds of the n vc path points involved are as follows:

[0060] v r = 2(v1-v2)ω r 3 +3(v1-v2)ω r 2 +v2(r = k, …, k + n vc -1) ;

[0061] The acceleration is as follows:

[0062]

[0063] The jerk is as follows:

[0064]

[0065] S320、According to the above speed, acceleration, jerk and first derivative q' i , second derivative q' i ' , and third derivative q' i The joint velocity can be obtained as follows:

[0066]

[0067] The joint acceleration is as follows:

[0068]

[0069] The joint jerk is as follows:

[0070]

[0071] The joint torque is as follows:

[0072]

[0073] S330, Iteratively improve the existing speed curve based on a cubic polynomial curve.

[0074] The speed control system of the industrial robot according to some embodiments of the second aspect of the present application adopts the speed planning method of the industrial robot according to the first aspect of the present application.

[0075] The speed control system of the industrial robot according to some embodiments of the second aspect of the present application has similar advantages to the speed planning method according to some embodiments of the first aspect of the present application, which will not be repeated here.

[0076] Additional aspects and advantages of the present application will be given, partially in the following description, partially become obvious from the following description, or be understood by practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

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

[0078] Figure 1 is the position of the path point in the joint space of the embodiment of the present application.

[0079] Figure 2 is the position of the end of the industrial robot of the embodiment of the present application.

[0080] Figure 3A direction of an end of an industrial robot according to an embodiment of the present application.

[0081] Figure 4 A maximum speed allowable on each path point according to an embodiment of the present application.

[0082] Figure 5 A conservative speed curve generated based on quadratic polynomial curve interpolation according to an embodiment of the present application successively passes through speed curves after one, two and three times of iteration based on cubic polynomial curve interpolation. DETAILED DESCRIPTION

[0083] Embodiments of the present application are described in detail below with reference to the attached drawings, wherein the same or similar components are denoted by the same or similar reference numerals throughout the drawings. The embodiments described below with reference to the drawings are exemplary and are only used to explain the present application, and cannot be understood as a limitation of the present application.

[0084] In the description of the present application, it should be understood that, in relation to the description of the orientation, for example, the orientation or position relationship indicated by up, down, left, right, front, back, etc. is based on the orientation or position relationship shown in the drawings, and is only for the purpose of facilitating the description of the present application and simplifying the description, and therefore cannot be understood as a limitation of the present application, which indicates or implies that the modules or components referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation of the present application.

[0085] In the description of the present application, if the first, second, etc. are described, it is only for the purpose of distinguishing technical features, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated or the sequence of technical features indicated.

[0086] In the description of the present application, unless otherwise explicitly limited, the words such as setting, installing, connecting, etc. should be understood in a broad sense, and the person skilled in the art can reasonably determine the specific meaning of the above words in the present application in combination with the specific content of the technical solution.

[0087] The embodiment of the present application provides an industrial robot speed accurate planning method.

[0088] The industrial robot speed accurate planning method comprises the following steps:

[0089] If a given path is composed of N straight line segments, there are N+1 path points, and the parameter of the path point is the cumulative length s of the path point on the path i (i = 0, 1, …, N), the path point is converted to the position in the joint space of the robot arm through the inverse kinematics model: q i ∈R n where n is the number of joints of the robot arm,

[0090] the first derivative of the position in joint space of the robot arm with respect to the cumulative length q' i which can be approximated discretely as:

[0091]

[0092] Similarly, the second and third derivatives of the position in joint space of the robot arm with respect to the cumulative length can be approximated discretely as:

[0093]

[0094] Let the given maximum tangential velocity be V max , and the maximum velocity of each joint be Let the maximum allowable velocity at each path point under joint velocity constraints be which is calculated as follows:

[0095]

[0096] Let the given maximum acceleration of each joint be Let the maximum allowable velocity at each path point under joint acceleration constraints be which is calculated as follows:

[0097]

[0098] Let the given maximum jerk of each joint be Let the maximum allowable velocity at each path point under joint jerk constraints be which is calculated as follows:

[0099]

[0100] Let the given maximum and minimum torque of each joint be τ max,j and τ min,j Let the maximum allowable velocity at each path point under joint torque constraints for constant velocity motion be which is calculated as follows:

[0101]

[0102] where M i , C i , c i are the inertia matrix, Coriolis and centrifugal force terms, and the gravitational and Coulomb friction force terms at path point q i . In summary, the maximum allowable velocity at each path point for constant velocity motion under all kinematic and dynamic constraints is ​

[0103]

[0104] After the above steps, further comprising generating a conservative velocity curve based on a quadratic polynomial curve interpolation, first give a quadratic polynomial curve v(ω)=C 3,2 ω 2 +C 3,1 ω+C 3,0 , where ω∈[-1,0]. Assuming that the given interpolation boundary conditions are the initial velocity v(-1)=v1, the final velocity v(0)=v2 and the final acceleration a(0)=0, then the parameters of the quadratic polynomial are C 3,2 =v1-v2, C 3,1 =0 and C 3,0 =v2, that is, v(ω)=(v1-v2)ω 2 +v2. Take the acceleration process as an example, assuming that the acceleration process involves n vc path points, the serial numbers of these path points are {k, k+1, …, k+n vc -1}, and the parameters of the path points are Map the parameters of the path points to [-1,0] according to the following formula:

[0105]

[0106] Then the corresponding parameters in [-1,0] when these path points adopt the v(ω) interpolation velocity curve are obtained.

[0107] According to the above quadratic polynomial curve interpolation, the velocities of the n vc path points are as follows:

[0108] v r =(v1-v2)ω r 2 +v2(r=k,…,k+n vc -1) #(10)

[0109] The accelerations are as follows:

[0110]

[0111] The jerk is as follows:

[0112]

[0113] According to the above velocity, acceleration and jerk, and the differential of the joint position about the arc length calculated in formulas #1-3, the joint velocity can be obtained as follows:

[0114]

[0115] Joint accelerations are as follows:

[0116]

[0117] Joint jerk is as follows:

[0118]

[0119] Joint torque is as follows:

[0120]

[0121] The conditions that the quadratic polynomial velocity curve satisfies the kinematic and dynamic constraints are as follows:

[0122]

[0123] wherein represents an n-dimensional vector obtained by combining the maximum velocities of the joints, i.e. represents an n-dimensional vector obtained by combining the maximum accelerations of the joints, i.e. represents an n-dimensional vector obtained by combining the maximum jerks of the joints, i.e. τ max represents an n-dimensional vector obtained by combining the maximum torques of the joints, i.e. max = [τ max,1 … τ max,n ] T , τ min represents an n-dimensional vector obtained by combining the maximum torques of the joints, i.e. min = [τ min,1 … τ min,n ] T .

[0124] Otherwise, the velocity curve does not satisfy the constraints.

[0125] A conservative velocity curve is given based on the quadratic polynomial curve. The velocity curve consists of three stages: first, the velocity is increased from the initial velocity v s to the target velocity v goal ; second, the target velocity v goal is maintained at a constant speed; third, the velocity is decreased from the target velocity v goal to the final velocity v e . In order to ensure that the industrial robot starts from rest and finally stops, let v s = v e = 0, and the target velocity

[0126] Taking the acceleration phase as an example, it is necessary to determine the target velocity v under given kinematic and dynamic constraints. goal Is it achievable? If not, then the actual achievable speed needs to be determined. In addition, it is necessary to determine the number of path points n involved in the acceleration phase in both cases. ac .

[0127] The present invention determines the target velocity v according to the following process. goal Can the target speed v be achieved and output? goal The number of path points n that can be reached ac And this process is implemented using function n ac =Num(v s ,v goal )express.

[0128] S241a, given initial velocity v s and target speed v goal ,make in Let n represent the largest integer not exceeding N / 2. max This represents the maximum number of path points involved in the acceleration process.

[0129] S242a, let n current =2 indicates the number of path points involved. We check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies all kinematic and dynamic constraints. If the constraints are satisfied, then n ac =n current At this time, the target velocity v goal If the goal can be achieved, the process ends; otherwise, it means the number of path points needs to be increased, denoted as n. add ,make Switch to S243a.

[0130] S243a, if n current +n add <n max Let n current =n current +n add Proceed to S244a; otherwise, let n add =n max -n current and n current =n max Switch to S245a.

[0131] S244a. Check whether the velocity curve v(ω) in the form of a quadratic polynomial satisfies all kinematic and dynamic constraints. If the constraints are satisfied, then n ac =n currentAt this time, the target velocity v goal If it can be achieved, then end; otherwise, let n add =2n add , then proceed to S243a.

[0132] S245a. Check whether the velocity curve v(ω) in the form of a quadratic polynomial satisfies all kinematic and dynamic constraints. If the constraints are satisfied, then n ac =n current At this time, the target velocity v goal If the target can be reached, end the output; otherwise, output n. ac =0 indicates the target velocity v goal Unable to reach, end.

[0133] If the target velocity is v goal If it can be achieved, then the actual speed v that can be reached during the acceleration process. a a c ctual =v goal If the target velocity v goal This cannot be achieved; the present invention employs a bisection method in the interval [v] s ,v goal Determine the actual speed in the [ ] and use functions This represents the result of the bisection method. The specific process is as follows:

[0134] S241b, Set the left boundary of velocity to v l =v s The right boundary is v r =v goal The velocity threshold for stopping the bisection method is Δv = 0.001.

[0135] S242b, if v r -v l If Δv >, then switch to S243b; otherwise... Finish.

[0136] S243b, let v m =(v r +v l ) / 2, calculate n ac =Num(v s ,v m If n ac =0, let v r =v m Proceed to S242b; otherwise, let v l =v m , then switch to S242b.

[0137] The deceleration phase is similar to the acceleration phase, with the main difference being that the target speed becomes Then If Then the acceleration phase needs to be re-determined, in which case And calculate

[0138] At this point, the speed values at each path point are obtained as follows:

[0139] The acceleration phase involves path points with serial numbers {0, 1,..., n ac -1}, the initial speed is v s , and the final speed is The speed at each path point is interpolated by the polynomial (2) The uniform speed phase involves path points with serial numbers {n ac , n ac +1,..., N-n dec}, and the speed is (3) The deceleration phase involves path points with serial numbers {N-n dec +1, N-n dec +2,..., N}, the initial speed is , and the final speed is v e , and the speed at each path point is interpolated by the polynomial .

[0140] The present application optimizes the speed planning method, so that the industrial robot can be more flexible and efficient when executing a complex geometric path, improves the working efficiency of the robot, and also enhances its adaptability in complex environments, and can simultaneously satisfy the kinematics and dynamics constraints, including speed, acceleration, jerk (i.e. the derivative of acceleration) and torque and other multiple physical constraints, ensuring the stability and safety of the robot when running at high speed, while improving the overall performance.

[0141] After the above steps, a cubic polynomial curve interpolation iteration is included to improve the speed curve.

[0142] First, a cubic polynomial curve v(ω) = C 4,3 ω 3 +C 4,2 ω 2 +C 4,1 ω+C 4,0 is given, where ω ∈ [-1, 0]. Assuming that the given interpolation boundary conditions are the initial speed v(-1) = v1, the initial acceleration a(-1) = 0, the final speed v(0) = v2 and the final acceleration a(0) = 0, the parameters of the cubic polynomial are C 4,3 = 2(v1-v2), C4,2 =3(v1-v2), C 4,1 =0 and C 4,0 = v2, that is, v(ω) = 2(v1-v2)ω 3 +3(v1-v2)ω 2 +v2. Assume this acceleration configuration involves n vc There are path points, and the path points are numbered {k,k+1,…,k+n}. vc -1}, the parameters of the path points are Mapping the parameters of the path points to [-1,0] according to formula (9) yields the parameters in [-1,0] corresponding to these path points when using v(ω) interpolation velocity curves.

[0143] When performing cubic polynomial curve interpolation as described above, the n involved... vc The velocities of the path points are as follows:

[0144] v r =2(v1-v2)ω r 3 +3(v1-v2)ω r 2 +v2(r=k,…,k+n vc -1)#(18)

[0145] The acceleration is as follows:

[0146]

[0147]

[0148] Based on the above velocity, acceleration, and jerk, as well as the differential of the joint position with respect to the arc length calculated in formula (1-3), it is verified that the current cubic polynomial velocity curve satisfies the kinematic and dynamic constraints of the formulas and formulas (13-17).

[0149] Next, the existing velocity curve is improved based on the iterative process of this cubic polynomial curve. Specifically, when the constant velocity phase involves n path points... uni =N+1-n ac -n dec Exceeding the given threshold N uni When = 2, the speed at these path points will increase further. Similar to the generation process of the conservative solution in the previous step, the path point indices corresponding to the uniform speed stage are {n}. ac -1,n ac ,…,Nn dec}, then the target speed is Initial velocity and final velocity are Again, the velocities at these waypoints are divided into three stages: acceleration stage, constant velocity stage and deceleration stage, and re-planned. The specific procedure is similar to the previous step, the main difference is that: (1) a cubic polynomial curve is used for the interpolation of the velocity curve, which is used to ensure that the acceleration at the beginning and end is 0; (2) n max is changed to The procedure is repeated until the number of waypoints without constant velocity stage exceeds a given threshold N uni = 2.

[0150] For example, a path composed of 400 straight line segments, the position of each waypoint in the joint space is shown as Figures 1-5

[0151] Figure 1 The horizontal coordinate is the serial number of the waypoint, and the vertical coordinate is the radian of each joint.

[0152] Taking a six-degree-of-freedom UR5 robot arm as an example, after the forward kinematics model transformation, the position and direction of the robot arm end are shown as Figure 2 and Figure 3

[0153] According to the kinematic constraints and dynamic constraints, the maximum allowable velocity at each waypoint is calculated as shown in Figure 4 , where the horizontal coordinate is the serial number of the waypoint, and the vertical coordinate is the square of the velocity, with the unit of m 2 / s 2 .

[0154] The kinematic and dynamic constraints are shown in the following table:

[0155]

[0156] The conservative velocity curve generated based on the quadratic polynomial curve interpolation, and the velocity curves after one, two and three times of iteration based on the cubic polynomial curve interpolation based on the conservative velocity curve are shown as Figure 5

[0157] It can be understood that due to the absence of constant velocity stage involving enough waypoints, the final velocity curve is obtained.

[0158] Although the embodiments of the present application have been shown and described, it can be understood by those skilled in the art that various changes, modifications, replacements and variations can be made to these embodiments without departing from the principles and purposes of the present application, and the scope of the present application is defined by the claims and their equivalents.​​​

Claims

1. A method for precise speed planning of industrial robots, characterized in that, The precise speed planning method includes the following steps: S110. Suppose the given path is... It consists of _ ... There are path points, and the parameter of each path point is the cumulative length of the path point on the path. The path points are transformed into their positions in the joint space of the robotic arm using an inverse kinematics model: ,in This represents the number of joints in the robotic arm. S120, the first derivative of the position of the robotic arm in joint space with respect to the cumulative length. Second derivative and third derivative Discrete approximation calculation; S130. Calculate the maximum allowable speed at each path point under the joint velocity constraint. The maximum permissible velocity at each path point under joint acceleration constraints was calculated as follows: The maximum permissible velocity at each path point under joint jerk constraints was calculated as follows: The maximum permissible speed for uniform motion at each path point under joint torque constraints is calculated as follows: ; S140. Calculate the maximum permissible speed for uniform motion at each path point. : The process also includes step S200 after step S140: generating a conservative velocity curve based on quadratic polynomial curve interpolation. S200 includes: S210, given Quadratic polynomial curve with parameter ,in Assuming the given interpolation boundary condition is the initial velocity... terminal velocity and terminal acceleration Then the parameters of the quadratic polynomial can be obtained as follows: , and ,get ; S220. Map the parameters of the path points to the following formula: middle: ； The path points are obtained using When interpolating the speed curve, the corresponding location is... The parameters involved, when interpolating using the aforementioned quadratic polynomial curve, are... The velocities of the path points are as follows: ； The acceleration is as follows: ； The jerk is as follows: ； S230, Based on the above velocity, acceleration, jerk, and first derivative... Second derivative and third derivative The differential of the joint position with respect to the arc length obtained from the discrete approximation calculation formula can be used to obtain the joint velocity as follows: ； The joint accelerations are as follows: ； The joint jerk is as follows: ； The joint torque is as follows: ； S240. Determine whether the quadratic polynomial velocity curve satisfies all kinematic and dynamic constraints. If not, provide a conservative velocity curve based on the quadratic polynomial curve. The conservative velocity curve includes the velocity obtained from the initial velocity. Increase to target speed Then maintain the target speed It moves at a constant speed, and finally changes its speed from the target speed. Decrease to terminal velocity ,in Target speed ; Determine the target velocity under given kinematic and dynamic constraints. If the target speed cannot be reached, output the number of path points corresponding to that speed; if it cannot be reached, determine the actual achievable speed. If the target speed If it can be achieved, then the actual speed that the acceleration process can reach. .

2. The industrial robot speed precision planning method according to claim 1, characterized in that: The first derivative of the position in the joint space of the robotic arm with respect to the cumulative length Second derivative and third derivative Discrete approximation is calculated as follows: ； ； 。 3. The industrial robot speed precision planning method according to claim 2, characterized in that: Specifically, S140 is as follows: Let the given maximum tangential velocity be... The maximum speed of each joint is Let the maximum allowable speed at each path point under joint velocity constraints be . , The calculation formula is as follows: ； Let the maximum acceleration of each joint be given. Let the maximum allowable velocity at each path point under joint acceleration constraints be . , The calculation formula is as follows: ； Let the maximum jerk of each joint be . Let the maximum allowable velocity at each path point under the joint jerk constraint be . , The calculation formula is as follows: ； Let the maximum and minimum torques of each joint be given as and Let the maximum permissible speed for uniform motion at each path point under joint torque constraints be . Its calculation formula is: ； in , , , respectively path points The inertial matrix, Coriolis force and centrifugal force terms, gravity and Coulomb friction terms at the location.

4. The industrial robot speed precision planning method according to claim 1, characterized in that: The determination of target velocity under given kinematic and dynamic constraints. Can the target speed be achieved? This will determine the number of path points corresponding to that target speed. Specifically, it includes: S241a, given initial velocity and target speed ,make ,in Indicates no more than The largest integer, This represents the maximum number of path points involved in the acceleration process; S242a, Order This indicates the number of path points involved; check the corresponding quadratic polynomial velocity curve at this point. Does it satisfy all kinematic and dynamic constraints? If the constraints are satisfied, then... At this time, the target speed If the goal can be reached, the process ends; otherwise, it indicates that the number of path points needs to be increased, denoted as increment. ,make ; S243a, if Then let Execute S244a; otherwise, order and Execute S245a; S244a. Check the velocity curve in quadratic polynomial form corresponding to this point. Does it satisfy all kinematic and dynamic constraints? If the constraints are satisfied, then... At this time, the target speed If it can be achieved, then end; otherwise, let Execute S243a; S245a. Check the velocity curve in quadratic polynomial form corresponding to this point. Does it satisfy all kinematic and dynamic constraints? If the constraints are satisfied, then... At this time, the target speed It can be achieved; otherwise, output , indicating the target speed Unable to achieve.

5. The industrial robot speed precision planning method according to claim 1, characterized in that: If the target speed If this cannot be achieved, then the binary search method is used within the interval. Determine the actual speed and use functions This represents the result of the bisection method, where the bisection method is used in the interval... Determine the actual speed and use functions The results of this dichotomy specifically include: S241b, Set the left boundary of velocity as The right boundary is The speed threshold for stopping the bisection method is ; S242b, if If yes, then execute S243b; otherwise... ,Finish; S243b, Order ,calculate ;like ,make Execute S242b; otherwise, order Execute S242b.

6. The industrial robot speed precision planning method according to claim 1, characterized in that: Following step S200, the method further includes step S300: improving the speed curve based on cubic polynomial curve interpolation iteration; step S300 includes: S310, given cubic polynomial curve with parameter ,in Assuming the given interpolation boundary condition is the initial velocity... acceleration at the front end terminal velocity and terminal acceleration Then the parameters of the cubic polynomial can be obtained as follows: , , and ,Right now ; S320, Configure the path point parameters according to... Mapped to In the middle, these path points are obtained using When interpolating the speed curve, the corresponding location is... Parameters in; When performing interpolation using the cubic polynomial curve described above, the following are involved: The velocities of the path points are as follows: ； The acceleration is as follows: ； The jerk is as follows: ； S320, based on the above velocity, acceleration, jerk, and first derivative. Second derivative and third derivative The differential of the joint position with respect to the arc length obtained from the discrete approximation calculation formula can be used to obtain the joint velocity as follows: ； The joint accelerations are as follows: ； The joint jerk is as follows: ； The joint torque is as follows: ； S330, based on the iteration of cubic polynomial curves to improve the existing speed curve.

7. An industrial robot control system, characterized in that: The industrial robot control system adopts the industrial robot speed precision planning method according to any one of claims 1-6.

Images