Accurate planning method and control system for speed of industrial robot

Abstract

The invention relates to the technical field of industrial robots, in particular to speed planning of an industrial robot. According to a given path, firstly, joint speed, acceleration, jerk and allowable maximum speed on each path point under torque constraint are calculated; then, two polynomial curves are defined to achieve smooth speed transition between adjacent path points, and the smooth speed transition is used for simulating the acceleration and deceleration process; on the basis, a conservative speed curve composed 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 stage are determined through a bisection method. And finally, adopting the same method to adaptively iteratively improve the constant speed stage in the existing speed curve. The working efficiency of the robot is improved, the adaptive capacity of the robot in a complex environment is enhanced, kinematics and dynamics constraints can be met at the same time, the stability and safety of the robot during high-speed operation are ensured, and meanwhile the overall performance is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robots, and in particular to an industrial robot speed precision planning method and a control system. Background Art

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

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

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

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

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

[0007] S120, the first-order derivative q' of the position in the joint space of the robot arm with respect to the cumulative length i , second-order derivative q″ i and the third-order derivative q″′ i Discrete approximate computing;

[0008] S130, calculate the maximum allowable speed at each path point under the joint speed constraint: The maximum allowable speed at each path point under the joint acceleration constraint is calculated as The maximum allowable speed at each path point under the joint acceleration constraint is calculated as Calculate the maximum allowable speed v for uniform motion at each path point under joint torque constraints i τ ;

[0009] S140, calculate the maximum allowable speed for uniform motion at each path point

[0010]

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

[0012] The present invention optimizes the speed planning method so that the industrial robot can be more flexible and efficient when executing complex geometric paths, thereby improving the robot's work efficiency and enhancing its adaptability in complex environments. It can also simultaneously meet kinematic and dynamic constraints, including multiple physical constraints such as speed, acceleration, jerk (i.e., the derivative of acceleration) and torque, thereby ensuring the stability and safety of the robot when running at high speed and improving its overall performance.

[0013] According to the industrial robot speed precision planning method of some embodiments of the first aspect of the present invention, the first-order derivative q' of the position in the joint space of the robot arm with respect to the cumulative length i , second-order derivative q″ i and the third-order derivative q″′ i The discrete approximation is calculated as:

[0014]

[0015] According to the industrial robot speed precision planning method in some embodiments of the first aspect of the present invention, the S140 is specifically:

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

[0017]

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

[0019]

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

[0021]

[0022] The maximum and minimum torques of each joint are given as τ max,j and τ min,j , the maximum allowable speed for uniform motion at each path point under joint torque constraint is v i τ , its calculation formula is:

[0023]

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

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

[0026] According to the industrial robot speed precision planning method in some embodiments of the first aspect of the present invention, the S200 includes:

[0027] S210, giving a quadratic polynomial curve v(ω)=C with ω as parameter 3,2 ω 2 +C 3,1 ω+C 3,0 , where ω∈[-1,0], assuming that the given interpolation boundary conditions are the head velocity v(-1)=v1, the terminal velocity v(0)=v2 and the terminal acceleration a(0)=0, the parameters of the quadratic polynomial are C 3,2 =v1-v2, C 3,1 =0 and C 3,0 =v2, we get v(ω)=(v1-v2)ω 2 +v2;

[0028] S220, map the path point parameters to [-1,0] according to the following formula:

[0029]

[0030] When each path point uses v(ω) to interpolate the velocity curve, the corresponding parameters in [-1,0] are obtained. When the quadratic polynomial curve is interpolated as above, the n involved vc The speeds of the waypoints are 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, based on the above speed, acceleration, jerk and first-order derivative q' i , second-order derivative q' i ' and the third-order derivative q' i The differential of the joint position with respect to the arc length obtained by the discrete approximate calculation formula can give the joint velocity as follows:

[0037]

[0038] The joint accelerations are as follows:

[0039]

[0040] The joint jerk is as follows:

[0041]

[0042] The joint torques are as follows:

[0043]

[0044] S240, judging whether the quadratic polynomial velocity curve satisfies the conditions of various kinematic and dynamic constraints. If not, a conservative velocity curve is given based on the quadratic polynomial curve, wherein the conservative velocity curve includes the velocity from the initial velocity v s Increase to target speed v goal , then maintain the target speed v goalMove at a uniform speed, and finally reduce the speed from the target speed v goal Reduce to final speed v e , where v s =v e =0, target speed

[0045] According to the industrial robot speed precision planning method of some embodiments of the first aspect of the present invention, the S240 further includes: under given kinematic and dynamic constraints, determining the target speed v goal Can it be reached? Output the number of path points corresponding to the target speed. If it cannot be reached, determine the actual speed that can be reached. If the target speed v goal can be achieved, then the actual speed that can be achieved during the acceleration process

[0046] According to some embodiments of the first aspect of the present invention, the industrial robot speed precision planning method, under given kinematic and dynamic constraints, determines the target speed v goal Can it be reached? Output the number of path points n corresponding to when the target speed can be reached ac Specifically include:

[0047] S241a, given initial velocity v s and target speed v goal ,make in Represents the largest integer not exceeding N / 2, n max Indicates the maximum number of path points involved in the acceleration process;

[0048] S242a, let n current =2 indicates the number of path points involved. Check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies the kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal If it can be reached, it ends; otherwise, it means that the number of path points needs to be increased, and the increment is n add ,make

[0049] S243a, if n current +n add <n max , then 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 whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal can be reached, end; otherwise, let n add =2n add , execute S243a.

[0051] S245a, check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal can be reached; otherwise, output n ac =0, indicating the target speed v goal Unable to reach.

[0052] According to the industrial robot speed precision planning method of some embodiments of the first aspect of the present invention, if the target speed v goal If it cannot be reached, then the binary method is used in the interval [v s ,v goal ] to determine the actual speed And use function The result of the dichotomy is represented by the dichotomy in the interval [v s ,v goal ] to determine the actual speed And use function The results of this dichotomy include:

[0053] S241b, set the left speed boundary to v l =v s , the right boundary is v r =v goal , the speed threshold for dichotomy stopping is Δv = 0.001;

[0054] S242b, if v r -v l >Δv, execute S243b; otherwise Finish;

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

[0056] According to some embodiments of the first aspect of the present invention, the industrial robot speed precision planning method further includes step S300 after step S200, iteratively improving the speed curve based on cubic polynomial curve interpolation; step S300 includes:

[0057] S310, giving a cubic polynomial curve v(ω)=C with ω as a parameter 4,3 ω 3 +C 4,2 ω 2 +C 4,1 ω+C 4,0 , where ω∈[-1,0], assuming that the given interpolation boundary conditions are the head velocity v(-1)=v1, the head acceleration a(-1)=0, the terminal velocity v(0)=v2 and the terminal acceleration a(0)=0, then the parameters of the cubic polynomial are 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, the parameters of the path points are Mapped to [-1,0], we get the corresponding parameters in [-1,0] when these path points use v(ω) interpolation velocity curve;

[0059] When interpolating according to the above cubic polynomial curve, the n involved vc The speeds of the waypoints 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, based on the above speed, acceleration, jerk and first-order derivative q' i , second-order derivative q' i ' and the third-order derivative q' i The differential of the joint position with respect to the arc length obtained by the discrete approximate calculation formula can give the joint velocity as follows:

[0066]

[0067] The joint accelerations are as follows:

[0068]

[0069] The joint jerk is as follows:

[0070]

[0071] The joint torques are as follows:

[0072]

[0073] S330, iteratively improving the existing speed curve based on the cubic polynomial curve.

[0074] According to some embodiments of the second aspect of the present invention, the speed control system of an industrial robot adopts the speed planning method of the industrial robot described in the embodiments of the first aspect.

[0075] The beneficial effects of the speed control system of the industrial robot according to some embodiments of the second aspect of the present invention are similar to those of the speed planning method of some embodiments of the first aspect, and are not described in detail here.

[0076] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

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

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

[0080] Figure 3It is the direction of the end of the industrial robot in the embodiment of the present invention.

[0081] Figure 4 is the maximum speed allowed at each path point in the embodiment of the present invention.

[0082] Figure 5 The conservative speed curve generated based on the quadratic polynomial curve interpolation according to the embodiment of the present invention is a speed curve that has been iterated once, twice and three times based on the cubic polynomial curve interpolation. DETAILED DESCRIPTION

[0083] Embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0084] In the description of the present invention, it should be understood that the descriptions involving orientations, such as up, down, left, right, front, back, etc., and the orientations or positional relationships indicated are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the modules or components referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present invention.

[0085] In the description of the present invention, if there is a description of first and second, it is only for the purpose of distinguishing the technical features, and cannot be understood as indicating or implying the relative importance or implicitly indicating the number of the indicated technical features or implicitly indicating the order of the indicated technical features.

[0086] In the description of the present invention, unless otherwise clearly defined, terms such as setting, installing, connecting, etc. should be understood in a broad sense, and technicians in the relevant technical field can reasonably determine the specific meanings of the above terms in the present invention based on the specific content of the technical solution.

[0087] An embodiment of the present invention provides a method for accurate speed planning of an industrial robot.

[0088] An industrial robot speed precision planning method, the speed planning method comprising the following steps:

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

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

[0091]

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

[0093]

[0094] The given maximum tangential velocity is V max , the maximum speed of each joint is The maximum allowable speed at each path point under the joint speed constraint is Its calculation formula is as follows:

[0095]

[0096] The maximum acceleration of each joint is given as The maximum allowable speed at each path point under the joint acceleration constraint is Its calculation formula is as follows:

[0097]

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

[0099]

[0100] The maximum and minimum torques of each joint are given as τ max,j and τ min,j , the maximum allowable speed for uniform motion at each path point under joint torque constraint is Its calculation formula is:

[0101]

[0102] in M i , C i , c i are the path points q i Inertia matrix, Coriolis force and centrifugal force, gravity and Coulomb friction. In summary, under various kinematic and dynamic constraints, the maximum speed allowed for uniform motion at each path point is for:

[0103]

[0104] After the above steps, the conservative speed curve is generated based on the interpolation of the quadratic polynomial curve. First, a quadratic polynomial curve v(ω)=C is given with ω as the parameter. 3,2 ω 2 +C 3,1 ω+C 3,0 , where ω∈[-1,0]. Assuming the given interpolation boundary conditions are the head velocity v(-1)=v1, the terminal velocity v(0)=v2 and the terminal acceleration a(0)=0, 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. Taking the acceleration process as an example, suppose that the acceleration involves n vc path points, the sequence numbers of these path points are {k, k+1,…, k+n vc -1}, the path point parameters are Map the path point parameters to [-1,0] according to the following formula:

[0105]

[0106] Then we get the corresponding parameters in [-1,0] when these path points use v(ω) to interpolate the velocity curve.

[0107] When interpolating according to the above quadratic polynomial curve, the n involved vc The speeds of the waypoints are as follows:

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

[0109] The acceleration is as follows:

[0110]

[0111] The jerk is as follows:

[0112]

[0113] 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, the joint velocity can be obtained as follows:

[0114]

[0115] The joint accelerations are as follows:

[0116]

[0117] The joint jerk is as follows:

[0118]

[0119] The joint torques are as follows:

[0120]

[0121] The conditions for the quadratic polynomial velocity curve to satisfy various kinematic and dynamic constraints are as follows:

[0122]

[0123] in Represents the n-dimensional vector obtained by combining the maximum velocities of each joint, that is Represents the n-dimensional vector obtained by combining the maximum acceleration of each joint, that is, Represents the n-dimensional vector obtained by combining the maximum jerk of each joint, that is, τ max Represents the n-dimensional vector obtained by combining the maximum torques of each joint, that is, τ max =[τ max,1 …τ max,n ] T , τ min Represents the n-dimensional vector obtained by combining the maximum torques of each joint, that is, τ 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 determined by the initial velocity v s Increase to target speed v goal ; Second, maintain the target speed v goal Move at a uniform speed; third, the speed is determined by the target speed v goal Reduce to final speed v e In order to ensure that the industrial robot starts moving from rest and eventually stops, let v s =v e =0, target speed

[0126] Taking the acceleration phase as an example, it is necessary to determine the target velocity v under given kinematic and dynamic constraints. goal If it is not achievable, then you need to determine the actual speed that can be achieved. In addition, the number of path points n involved in the acceleration phase in these two cases must be determined ac .

[0127] The present invention determines the target speed v according to the following process: goal Can it be achieved and output the target speed v goal The number of path points n that can be reached ac , and use the function n ac =Num(v s ,v goal )express.

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

[0129] S242a, let n current =2 indicates the number of path points involved. Check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies the kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal If it can be reached, it ends; otherwise, it means that the number of path points needs to be increased, and the increment is n add ,make Go to S243a.

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

[0131] S244a, check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current, at this time the target speed v goal can be reached, end; otherwise, let n add =2n add , go to S243a.

[0132] S245a, check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal Can be reached, end; otherwise, output n ac =0, indicating the target speed v goal Unreachable, end.

[0133] If the target speed v goal can be achieved, then the actual speed v that can be achieved during the acceleration process a a c ctual =v goal ; If the target speed v goal Cannot be achieved, the present invention adopts dichotomy in the interval [v s ,v goal ] to determine the actual speed And use function Represents the result of the dichotomy. The specific process is as follows:

[0134] S241b, set the left speed boundary to v l =v s , the right boundary is v r =v goal , the speed threshold for dichotomy stopping is Δv=0.001.

[0135] S242b, if v r -v l >Δv, then go 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 , go to S242b; otherwise, let v l =v m , go to S242b.

[0137] The deceleration phase is similar to the acceleration phase, the main difference is that the target speed becomes but like The acceleration phase needs to be re-determined. And calculate

[0138] So far, the speed values ​​at each path point have been obtained, as follows:

[0139] The sequence numbers of the path points involved in the acceleration phase are {0,1,…,n ac -1}, with an initial velocity of v s The final speed is By polynomial Interpolate the speed at each path point; (2) The sequence number of the path points involved in the uniform speed stage is {n ac ,n ac +1,…,Nn dec}, the speed is (3) The sequence number of the path points involved in the deceleration phase is {Nn dec +1,Nn dec +2…,N}, the initial velocity is The final velocity is v e , by the polynomial Interpolate the velocity at each path point.

[0140] The present invention optimizes the speed planning method so that the industrial robot can be more flexible and efficient when executing complex geometric paths, thereby improving the robot's work efficiency and enhancing its adaptability in complex environments. It can also simultaneously meet kinematic and dynamic constraints, including multiple physical constraints such as speed, acceleration, jerk (i.e., the derivative of acceleration) and torque, thereby ensuring the stability and safety of the robot when running at high speed and improving its overall performance.

[0141] The above steps include iteratively improving the speed curve based on cubic polynomial curve interpolation.

[0142] First, we give a cubic polynomial curve v(ω)=C with ω as parameter: 4,3 ω 3 +C 4,2 ω 2 +C 4,1 ω+C 4,0 , where ω∈[-1,0]. Assuming the given interpolation boundary conditions are the head velocity v(-1)=v1, the head acceleration a(-1)=0, the terminal velocity v(0)=v2 and the terminal 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 that the acceleration involves n vc path points, the sequence numbers of these path points are {k, k+1,…, k+n vc -1}, the path point parameters are The parameters of the path points are mapped to [-1, 0] according to formula (9), and the corresponding parameters in [-1, 0] are obtained when these path points use v(ω) to interpolate the velocity curve.

[0143] When interpolating according to the above cubic polynomial curve, the n involved vc The speeds of the waypoints 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 formulas of various kinematic and dynamic constraints and is consistent with formula (13-17).

[0149] Next, the existing speed curve is iterated based on the cubic polynomial curve. Specifically, when the number of path points involved in the uniform speed stage is n uni =N+1-n ac -n dec Exceeding a given threshold N uni = 2, the speed at these path points will increase further. Similar to the generation process of the conservative solution in the previous step, the sequence number of the path points corresponding to the uniform speed stage is {n ac -1,n ac ,…,Nn dec}, then the target speed is The initial and final velocities are The speeds at these path points are divided into three stages: acceleration stage, uniform speed stage and deceleration stage, and replanned. The specific process is similar to the previous step of the present invention, with the main differences being: (1) a cubic polynomial curve is used to interpolate the speed curve to ensure that the acceleration at the beginning and end is 0; (2) n max Change to This process is repeated until the number of path points without a uniform speed phase exceeds a given threshold N. uni Stop when =2.

[0150] For example, a path consisting of 400 straight line segments, the position of each path point in the joint space is as follows: Figure 1-5 As shown:

[0151] Figure 1 is the position of the path point in the joint space, the horizontal axis is the serial number of the path point, and the vertical axis is the radian of each joint.

[0152] Taking the UR5 robot with six degrees of freedom as an example, after the forward kinematics model transformation, the position and direction of the end of the robot are obtained as follows: Figure 2 and Figure 3 shown.

[0153] According to the kinematic and dynamic constraints, the maximum speed allowed at each path point is calculated as follows: Figure 4 As shown, the horizontal axis is the number of the path point, and the vertical axis is the square of the speed, in meters. 2 / s 2 .

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

[0155]

[0156] A conservative speed curve generated based on quadratic polynomial curve interpolation, and a speed curve after the conservative speed curve is iterated once, twice and three times based on cubic polynomial curve interpolation, as shown in FIG. Figure 5 shown.

[0157] It can be understood that the final speed curve is obtained because there is no uniform speed stage involving enough path points.

[0158] Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims

1. A method for accurately planning the speed of an industrial robot, characterized in that: The speed planning method comprises the following steps: S110, assuming that the given path is composed of N straight line segments, there are N+1 path points in total, and the parameter of the path point is the cumulative length s of the path point on the path i (i=0,1,…,N), convert the path points through the inverse kinematics model to obtain the position in the robot arm joint space: q i ∈R n , where n is the number of joints of the robot arm; S120, the first-order derivative q' of the position in the joint space of the robot arm with respect to the cumulative length i , second-order derivative q" i and the third-order derivative q"' i Discrete approximate computing; S130, calculate the maximum allowable speed at each path point under the joint speed constraint: The maximum allowable speed at each path point under the joint acceleration constraint is calculated as The maximum allowable speed at each path point under the joint acceleration constraint is calculated as Calculate the maximum allowable speed v for uniform motion at each path point under joint torque constraints i τ ; S140, calculate the maximum allowable speed for uniform motion at each path point 2. The industrial robot speed precision planning method according to claim 1, characterized in that: The first-order derivative q' of the position in the joint space of the manipulator with respect to the cumulative length i , second-order derivative q" i and the third-order derivative q"' i The discrete approximation is calculated as:

3. The industrial robot speed precision planning method according to claim 2, characterized in that: The S140 is specifically: The given maximum tangential velocity is V max , the maximum speed of each joint is The maximum allowable speed at each path point under the joint speed constraint is The calculation formula is as follows: The maximum acceleration of each joint is given as The maximum allowable speed at each path point under the joint acceleration constraint is The calculation formula is as follows: The maximum jerk of each joint is given as The maximum allowable velocity at each path point under the joint jerk constraint is The calculation formula is as follows: The maximum and minimum torques of each joint are given as τ max,j and τ min,j , the maximum allowable speed for uniform motion at each path point under joint torque constraint is v i τ , its calculation formula is: in M i , C i , c i are the path points q i The inertia matrix, Coriolis and centrifugal force terms, gravity, and Coulomb friction terms at .

4. The industrial robot speed precision planning method according to claim 1, characterized in that: After S140, the method further includes step S200 of generating a conservative speed curve based on quadratic polynomial curve interpolation.

5. The industrial robot speed precision planning method according to claim 4, characterized in that: The S200 includes: S210, giving a quadratic polynomial curve v(ω)=C with ω as parameter 3,2 ω 2 +C 3,1 ω+C 3,0 , where ω∈[-1,0], assuming that the given interpolation boundary conditions are the head velocity v(-1)=v1, the terminal velocity v(0)=v2 and the terminal acceleration a(0)=0, the parameters of the quadratic polynomial are C 3,2 =v1-v2, C 3,1 =0 and C 3,0 =v2, we get v(ω)=(v1-v2)ω 2 +v2; S220, map the path point parameters to ]-1,0] according to the following formula: When each path point uses v(ω) to interpolate the velocity curve, the corresponding parameters in [-1,0] are obtained. When the quadratic polynomial curve is interpolated as above, the n involved vc The speeds of the waypoints are as follows: v r =(v1-v2)ω r 2 +v2(r=k,…,k+n vc -1); The acceleration is as follows: The jerk is as follows: S230, based on the above speed, acceleration, jerk and first-order derivative q' i , second-order derivative q" i and the third-order derivative q"' i The differential of the joint position with respect to the arc length calculated by the discrete approximate calculation formula can give the joint velocity as follows: The joint accelerations are as follows: The joint jerk is as follows: The joint torques are as follows: S240, judging whether the quadratic polynomial velocity curve satisfies the conditions of various kinematic and dynamic constraints. If not, a conservative velocity curve is given based on the quadratic polynomial curve, wherein the conservative velocity curve includes the velocity from the initial velocity v s Increase to target speed v goal , then maintain the target speed v goal Move at a uniform speed, and finally reduce the speed from the target speed v goal Reduce to final speed v e , where v s =v e =0, target speed 6. The industrial robot speed precision planning method according to claim 5, characterized in that: The S240 further includes: determining the target speed v under given kinematic and dynamic constraints. goal Can it be reached? Output the number of path points corresponding to the target speed. If it cannot be reached, determine the actual speed that can be reached. If the target speed v goal can be achieved, then the actual speed that can be achieved during the acceleration process 7. The industrial robot speed precision planning method according to claim 6, characterized in that: Under given kinematic and dynamic constraints, the target velocity v is determined goal Can it be reached? Output the number of path points n corresponding to when the target speed can be reached ac Specifically include: S241a, given initial velocity v s and target speed v goal ,make in Represents the largest integer not exceeding N / 2, n max Indicates the maximum number of path points involved in the acceleration process; S242a, let n current =2 indicates the number of path points involved. Check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies the kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal If it can be reached, it ends; otherwise, it means that the number of path points needs to be increased, and the increment is n add ,make S243a, if n current +n add <n max , then let n current =n current +n add , execute S244a; otherwise, let n add =n max -n current and n current =n max , execute S245a; S244a, check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal can be reached, end; otherwise, let n add =2n add , execute S243a; S245a, check whether the corresponding quadratic polynomial velocity curve v(ω) satisfies various kinematic and dynamic constraints. If so, then n ac =n current , at this time the target speed v goal can be reached; otherwise, output n ac =0, indicating the target speed v goal Unable to reach.

8. The industrial robot speed precision planning method according to claim 6, characterized in that: If the target speed v goal If it cannot be reached, then the binary method is used in the interval [v s ,v goal ] to determine the actual speed And use function The result of the dichotomy is represented by the dichotomy in the interval [v s ,v goal ] to determine the actual speed And use function The results of this dichotomy include: S241b, set the left speed boundary to v l =v s , the right boundary is v r =v goal , the speed threshold for dichotomy stopping is Δv = 0.001; S242b, if v r -v l >Δv, execute S243b; otherwise Finish; 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 , execute S242b; otherwise, let v l =v m , execute S242b.

9. The industrial robot speed precision planning method according to claim 4, characterized in that: After S200, the method further includes step S300, iteratively improving the speed curve based on cubic polynomial curve interpolation; S300 includes: S310, giving a cubic polynomial curve v(ω)=C with ω as a parameter 4,3 ω 3 +C 4,2 ω 2 +C 4,1 ω+C 4,0 , where ω∈[-1,0], assuming that the given interpolation boundary conditions are the head velocity v(-1)=v1, the head acceleration a(-1)=0, the terminal velocity v(0)=v2 and the terminal acceleration a(0)=0, then the parameters of the cubic polynomial are 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; S320, the parameters of the path points are Mapped to [-1,0], we get the corresponding parameters in [-1,0] when these path points use v(ω) interpolation velocity curve; When interpolating according to the above cubic polynomial curve, the n involved vc The speeds of the waypoints are as follows: v r =2(v1-v2)ω r 3 +3(v1-v2)ω r 2 +v2(r=k,…,k+n vc -1); The acceleration is as follows: The jerk is as follows: S320, based on the above speed, acceleration, jerk and first-order derivative q' i , second-order derivative q" i and the third-order derivative q"' i The differential of the joint position with respect to the arc length calculated by the discrete approximate calculation formula can give the joint velocity as follows: The joint accelerations are as follows: The joint jerk is as follows: The joint torques are as follows: S330, iteratively improving the existing speed curve based on the cubic polynomial curve.

10. Industrial robot control system, characterized in that: The industrial robot control system adopts the industrial robot speed precision planning method described in any one of claims 1-9.

Images