Robot rotation external shaft normal vector priority control strategy and inverse solution method

Abstract

The invention belongs to the field of intelligent production, and particularly relates to a robot rotation external axis normal vector priority control strategy and inverse solution method.The method comprises the following steps that a target track is dispersed and then converted to a rotating platform coordinate system; adopting a normal priority strategy; inversely solving a rotation angle corresponding to each target point; calculating the pose of the target point in a robot coordinate system; robot inverse solution; and outputting inverse solutions of the robot and the rotating shaft. The robot and the external shaft move synchronously, the movement track of the tail end of the robot is smooth, and the robot is more suitable for situations such as wire placement and spraying needing continuous smooth track movement. And the inverse solution of the robot with the rotating external shaft is iteratively completed by setting the initial angle and the termination angle of deflection corresponding to the target track point.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent production, and specifically relates to a normal vector priority control strategy and an inverse solution method for rotating an external axis of a robot. Background Art

[0002] In the prior art, there is a Chinese invention patent with patent number CN202211275681.3 and the name of "Fully automatic motion planning method for industrial robot processing system with additional external axis", which discloses the following content: A fully automatic motion planning method for industrial robot processing system with additional external axis, comprising the following steps: Step 1, obtain a processing path point set; Step 2, select a station mode; including two optional modes of automatic station setting and manual station setting, and the automatic station setting mode is further divided into intelligent planning station and fixed allocation station; Step 3, each path executes a collision-free motion planning algorithm; delayed collision and path cutting are integrated into the PRM* algorithm to achieve the purpose of reducing time and shortening the path; Step 4, integrate the robot motion paths of all stations to allow the robot to perform processing tasks.

[0003] The above patent uses a station-based planning of robot trajectories, that is, the robot's external axis moves to a certain station, and plans certain processing path point sets corresponding to the robot at this station. Then the robot's external axis moves to the next station, and the corresponding processing path point set is planned again. The external axis and the robot move one after the other, and no linkage control is achieved. In the case of some continuous motion trajectories, this planning method has disadvantages. Summary of the invention

[0004] In order to solve the above problems existing in the prior art and to realize the inverse solution of a robot system with a rotating external axis, the present application designs a normal vector priority control strategy and an inverse solution method for a robot rotating external axis to realize the iterative inverse solution of a robot system with a rotating external axis.

[0005] In order to achieve the above-mentioned invention object, the technical solution provided by this application is as follows:

[0006] A normal vector priority control strategy and inverse solution method for a robot rotating external axis, comprising the following steps:

[0007] Step S1. Discretize the target trajectory and then convert it to the rotating platform coordinate system;

[0008] Step S2. Adopt the normal priority strategy: determine the normal vector priority control strategy of the rotating external axis and determine the initial deflection angle α start and the end deflection angle α end , and use the linear interpolation strategy to calculate the intermediate deflection angle value corresponding to each target trajectory point;

[0009] Step S3. Calculate the normal vector angle corresponding to the target point: calculate the new coordinate pose corresponding to the target trajectory point after the workpiece rotates around the external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

[0010] Step S4. Inversely solve the rotation angle corresponding to each target point: calculate the final rotation axis angle according to the rotation axis control strategy, and use the angle relationship in step S3 and the deflection angle value in step S2 to inversely solve the normal priority angle of the rotating external axis;

[0011] Step S5. Calculate the pose of the target point in the robot coordinate system: Calculate the pose of the target trajectory point in the robot coordinate system according to the normal priority angle calculated in step S4 and the assembly relationship of the workpiece;

[0012] Step S6. Robot inverse solution: Inversely solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle and jump to step S4 to calculate again until the robot inverse solution meets the requirements;

[0013] Step S7: Output the inverse solution of the robot and the rotation axis.

[0014] Furthermore, step S1 is specifically as follows: the known target trajectory O G (O 1 ,O 2 ,…,O n ), where O i (i∈1,2,…,n) represents the discrete pose points on the trajectory curve, and the pose information of each point is O i ( o x i , o y i , o z i , o p i , o q i , o r i ),in o x i , o y i , o z i is the position coordinate of the i-th trajectory point, o p i,o q i,o r i is the normal vector of the ith trajectory point, and its target trajectory point coordinate system is denoted by T G .

[0015] Furthermore, in step S2, for the intermediate trajectory points, linear interpolation is performed with the path length of each trajectory point as the interpolation node to calculate the deflection angle α corresponding to the intermediate trajectory point. i

[0016]

[0017] Where: l min is the shortest length of the trajectory, l max is the maximum length of the trajectory, l i is the trajectory length value of the i-th target trajectory point.

[0018] Further, step S3 is specifically as follows: calculating the rotation angle θ of the external axis i After that, the new coordinate pose T corresponding to the target trajectory point new ; and calculate the normal vector angle corresponding to the target trajectory point at this time N α i ; where θ i is the normal priority angle value of the rotation external axis corresponding to the i-th target trajectory point;

[0019]

[0020] Where: cθ i is cos(θ i ), sθ i is sin(θ i ), o x i,o y i,o z i is the position coordinate of the i-th trajectory point, o p i,o q i,o r is the normal vector of the i-th trajectory point, and its target trajectory point coordinate system is denoted by T G ;

[0021] Normal vector angle N α i is the angle between the normal vector of the i-th position point after rotation and the X-axis,

[0022]

[0023] Further, in step S4, according to the normal vector angle relationship of step S3 and N α i The value of the reverse solution is the normal priority angle θ of the external axis of rotation i .

[0024] Furthermore, from the normal vector angle N α i The calculation formula is:

[0025]

[0026] Convert the above formula to:

[0027] -k 1 c 2 +k 2 s 2 =k 3

[0028] in

[0029] k 2 = o p io q i

[0030]

[0031] Available

[0032]

[0033] Furthermore, in step S5, the coordinate system of the robot system with rotating external axis is denoted as T B , transform the target trajectory point into the robot coordinate system, in, Indicates the coordinate system G T conversion to coordinate system B The transformation matrix of T, R z (θ) represents the rotation matrix corresponding to the rotation angle θ around the external axis. For the normal priority strategy, θ is the position priority angle. N θ i .

[0034] Further, Among them, cθ is the abbreviation of cos(θ), sθ is the abbreviation of sin(θ), and the target trajectory point is transformed into:

[0035]

[0036] Where: r 11 , r 12 ,…,r 33 is the posture description parameter, x i ,y i , z i It is the position coordinate of the target trajectory point in the robot coordinate system.

[0037] Further, step S6 is specifically as follows: if the robot has no solution or produces a singular solution, set the external axis step size of each change to Δα i , then update the position of the external axis to αi =α i +Δα i , modify the rotation axis angle, jump to step S4 and calculate again until the robot inverse solution meets the requirements.

[0038] The beneficial effects of the present invention are:

[0039] 1. In this application, the robot moves synchronously with the external axis, and the motion trajectory of the robot end is smooth, which is more suitable for situations such as wire laying and spraying that require continuous smooth trajectory motion.

[0040] 2. This application designs a control strategy for the robot's external rotation axis. By setting the initial angle and the end angle of the deflection corresponding to the target trajectory point, the inverse solution of the robot with a rotating external axis is iteratively completed.

[0041] 3. This application first converts the target trajectory into a rotating coordinate system, then formulates a control strategy for the robot's rotating external axis, and adopts a normal vector priority strategy to obtain the position and posture of the target trajectory point in the robot coordinate system. Then, the robot is inversely solved. If a robot singularity is encountered, the position angle or normal vector angle of the external axis is iteratively modified until the robot singularity is avoided. Finally, the inverse solution of the motion of the robot with a rotating external axis is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is the normal vector priority control strategy and solution process with rotating external axis.

[0043] Figure 2 Schematic diagram of redundant rotation axis trajectory.

[0044] Figure 3 Schematic diagram of the normal angle of the rotation axis trajectory point.

[0045] Figure 4 Inverse solution diagram for rotating the external axis of the robot. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is to explain the present invention rather than to limit the present invention. Based on the embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0047] The specific implementation method of the present invention is described below with reference to the accompanying drawings and examples, but the present invention is not limited to this embodiment.

[0048] Example 1

[0049] like Figure 1As shown, a normal vector priority control strategy and inverse solution method for a robot rotating external axis include the following steps:

[0050] Step S1. Discretize the target trajectory and then convert it to the rotating platform coordinate system;

[0051] Step S2. Adopt the normal priority strategy: determine the normal vector priority control strategy of the rotating external axis and determine the initial deflection angle α start and the end deflection angle α end , and use the linear interpolation strategy to calculate the intermediate deflection angle value corresponding to each target trajectory point;

[0052] Step S3. Calculate the normal vector angle corresponding to the target point: calculate the new coordinate pose corresponding to the target trajectory point after the workpiece rotates around the external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

[0053] Step S4. Inversely solve the rotation angle corresponding to each target point: calculate the final rotation axis angle according to the rotation axis control strategy, and use the angle relationship in step S3 and the deflection angle value in step S2 to inversely solve the normal priority angle of the rotating external axis;

[0054] Step S5. Calculate the pose of the target point in the robot coordinate system: Calculate the pose of the target trajectory point in the robot coordinate system according to the normal priority angle calculated in step S4 and the assembly relationship of the workpiece;

[0055] Step S6. Robot inverse solution: Inversely solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle and jump to step S4 to calculate again until the robot inverse solution meets the requirements;

[0056] Step S7: Output the inverse solution of the robot and the rotation axis.

[0057] Example 2

[0058] like Figure 1 As shown, a normal vector priority control strategy and inverse solution method for a robot rotating external axis include the following steps:

[0059] Step S1. Discretize the target trajectory and then convert it to the rotating platform coordinate system;

[0060] Step S2. Adopt the normal priority strategy: determine the normal vector priority control strategy of the rotating external axis and determine the initial deflection angle α start and the end deflection angle α end , and use the linear interpolation strategy to calculate the intermediate deflection angle value corresponding to each target trajectory point;

[0061] Step S3. Calculate the normal vector angle corresponding to the target point: calculate the new coordinate pose corresponding to the target trajectory point after the workpiece rotates around the external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

[0062] Step S4. Inversely solve the rotation angle corresponding to each target point: calculate the final rotation axis angle according to the rotation axis control strategy, and use the angle relationship in step S3 and the deflection angle value in step S2 to inversely solve the normal priority angle of the rotating external axis;

[0063] Step S5. Calculate the pose of the target point in the robot coordinate system: Calculate the pose of the target trajectory point in the robot coordinate system according to the normal priority angle calculated in step S4 and the assembly relationship of the workpiece;

[0064] Step S6. Robot inverse solution: Inversely solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle and jump to step S4 to calculate again until the robot inverse solution meets the requirements;

[0065] Step S7: Output the inverse solution of the robot and the rotation axis.

[0066] Furthermore, step S1 is specifically as follows: the known target trajectory O G (O 1 ,O 2 ,…,O n ), where O i (i∈1,2,…,n) represents the discrete pose points on the trajectory curve, and the pose information of each point is O i ( o x i,o y i,o z i,o p i,o q i,o r i ),in o x i, o y i,o z i is the position coordinate of the i-th trajectory point, o p i , o q i,o r i is the normal vector of the ith trajectory point, and its target trajectory point coordinate system is denoted by T G .

[0067] Furthermore, in step S2, for the intermediate trajectory points, linear interpolation is performed with the path length of each trajectory point as the interpolation node to calculate the deflection angle α corresponding to the intermediate trajectory point. i

[0068]

[0069] Where: l min is the shortest length of the trajectory, l max is the maximum length of the trajectory, l i is the trajectory length value of the i-th target trajectory point.

[0070] Further, step S3 is specifically as follows: calculating the rotation angle θ of the external axis i After that, the new coordinate pose T corresponding to the target trajectory point new ; and calculate the normal vector angle corresponding to the target trajectory point at this time N α i ; where θ i is the normal priority angle value of the rotation external axis corresponding to the i-th target trajectory point;

[0071]

[0072] Where: cθ i is cos(θ i ), sθ i is sin(θ i ), o x i,o y i,o z i is the position coordinate of the i-th trajectory point, o p i,o q i,o r is the normal vector of the i-th trajectory point, and its target trajectory point coordinate system is denoted by T G ;

[0073] Normal vector angle N α i is the angle between the normal vector of the i-th position point after rotation and the X-axis,

[0074]

[0075] Further, in step S4, according to the normal vector angle relationship of step S3 and N α i The value of the reverse solution is the normal priority angle θ of the external axis of rotation i .

[0076] Furthermore, from the normal vector angle N α i The calculation formula is:

[0077]

[0078] Convert the above formula to:

[0079] -k 1c 2 +k 2 s 2 =k 3

[0080] in

[0081] k 2 = o p io q i

[0082]

[0083] Available

[0084]

[0085] Furthermore, in step S5, the coordinate system of the robot system with rotating external axis is denoted as T B , transform the target trajectory point into the robot coordinate system, in, Indicates the coordinate system G T conversion to coordinate system B The transformation matrix of T, R z (θ) represents the rotation matrix corresponding to the rotation angle θ around the external axis. For the normal priority strategy, θ is the position priority angle. N θ i .

[0086] Further, Among them, cθ is the abbreviation of cos(θ), sθ is the abbreviation of sin(θ), and the target trajectory point is transformed into:

[0087]

[0088] Where: r 11 , r 12 ,…,r 33 is the posture description parameter, x i ,y i , z i It is the position coordinate of the target trajectory point in the robot coordinate system.

[0089] Further, step S6 is specifically as follows: if the robot has no solution or produces a singular solution, set the external axis step size of each change to Δα i , then update the position of the external axis to α i =α i +Δα i , modify the rotation axis angle, jump to step S4 and calculate again until the robot inverse solution meets the requirements.

[0090] Example 3

[0091] 1. Known target trajectory O G (O 1 ,O 2 ,…,O n ), where O i (i∈1,2,…,n) represents the discrete pose points on the trajectory curve, and the pose information of each point is O i ( o x i,o y i,o z i , o p i , o q i , o r i ),in o x i , o y i , o z i is the position coordinate of the i-th trajectory point, o p i , o q i , o r is the normal vector of the i-th trajectory point, and its target trajectory point coordinate system is denoted by T G .

[0092] 2. Determine the normal vector priority control strategy for the rotating external axis and determine the initial deflection angle α start and the end deflection angle α end For the intermediate trajectory points, linear interpolation is performed with the path length of each trajectory point as the interpolation node to calculate the deflection angle α corresponding to the intermediate trajectory point i

[0093]

[0094] Where: l min is the shortest length of the trajectory, l max is the maximum length of the trajectory, l i is the trajectory length value of the i-th target trajectory point.

[0095] 3. Calculate the rotation angle θ of the external axis i After that, the new coordinate pose T corresponding to the target trajectory point new . And calculate the normal vector angle corresponding to the target trajectory point at this time N α i .

[0096]

[0097] Where: cθ i is cos(θ i ), sθ i is sin(θ i ). o x i , o y i , o z i is the position coordinate of the i-th trajectory point, o p i , o q i , o r is the normal vector of the i-th trajectory point, and its target trajectory point coordinate system is denoted by T G .

[0098] Normal vector angle N α i is the angle between the normal vector of the i-th position point after rotation and the X-axis,

[0099]

[0100] 4. According to the normal vector angle relationship in step 4 and N α i The value of the reverse solution is the normal priority angle θ of the external axis of rotation i .

[0101] First inversely calculate the position of the rotation axis based on the normal vector:

[0102]

[0103] From the above formula, we can get:

[0104] (( o p i cθ i - o q i sθ i ) 2 +( o p i sθ i + o q i cθ i ) 2 +( o r i ) 2 )c 2 α i =( o p i cθ i - o q i sθi ) 2

[0105] According to the trigonometric function double angle formula: cos(2α i )=2cos 2 α i -1=1-2sin 2 α i

[0106] Convert the above formula into:

[0107]

[0108] Convert the above formula to:

[0109] -k 1 c 2 +k 2 s 2 =k 3

[0110] in

[0111] k 2 = o p i o q i

[0112]

[0113] Available

[0114]

[0115] 5. According to the normal priority angle calculated in step 4 and the assembly relationship of the workpiece, calculate the position of the target trajectory point in the robot coordinate system. The coordinate system of the robot system with rotating external axis is denoted as T B . Convert the target trajectory point to the robot coordinate system. in, Indicates the coordinate system G T conversion to coordinate system B The transformation matrix of T, R z (θ) represents the rotation matrix corresponding to the rotation of θ around the external axis. For the normal priority strategy, θ is the position priority angle N θ i ;

[0116]

[0117] Among them, cθ is the abbreviation of cos(θ), sθ is the abbreviation of sin(θ),

[0118] The target trajectory point is transformed into:

[0119]

[0120] Where: r 11 , r 12 ,…,r 33 is the posture description parameter, x i ,y i , z i It is the position coordinate of the target trajectory point in the robot coordinate system.

[0121] 6. Inversely solve the robot joint posture. If the robot has no solution or produces a singular solution, set the external axis step size of each change to Δα i , then update the position of the external axis to α i =α i +Δα i , modify the rotation axis angle, jump to S4 and calculate again until the robot inverse solution meets the requirements.

[0122] Take the example of a robot rotating an external axis. Figure 2 As shown in the figure, the robot is on a linear external axis, and there is a rotating external axis next to the robot. The workpiece to be processed is on the rotating external axis. The processing target trajectory point is O 1 ,O 2 ,…,O n .

[0123] Rotating external axis normal vector priority control strategy, such as Figure 3 As shown, the normal vector angle N α i It is the angle between the normal vector of the i-th position point after rotation and the X-axis.

[0124] Set the initial deflection angle α start = 80° and the end deflection angle α end =85°. The total length of the target trajectory is about 2 meters. The inverse solution of a target trajectory is performed, and the normal priority strategy is adopted. The calculated rotation angle of the external rotation axis is as follows: Figure 4 shown.

Claims

1. A normal vector priority control strategy and inverse solution method for a robot rotating external axis, characterized in that: The steps include: Step S1. Discretize the target trajectory and then convert it to the rotating platform coordinate system; Step S2. Adopt the normal priority strategy: determine the normal vector priority control strategy of the rotating external axis and determine the initial deflection angle α start and the end deflection angle α end , and use the linear interpolation strategy to calculate the intermediate deflection angle value corresponding to each target trajectory point; Step S3. Calculate the normal vector angle corresponding to the target point: calculate the new coordinate pose corresponding to the target trajectory point after the workpiece rotates around the external axis by an angle θ, and calculate the position vector angle and the normal vector angle; Step S4. Inversely solve the rotation angle corresponding to each target point: calculate the final rotation axis angle according to the rotation axis control strategy, and use the angle relationship in step S3 and the deflection angle value in step S2 to inversely solve the normal priority angle of the rotating external axis; Step S5. Calculate the pose of the target point in the robot coordinate system: Calculate the pose of the target trajectory point in the robot coordinate system according to the normal priority angle calculated in step S4 and the assembly relationship of the workpiece; Step S6. Robot inverse solution: Inversely solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle and jump to step S4 to calculate again until the robot inverse solution meets the requirements; Step S7: Output the inverse solution of the robot and the rotation axis.

2. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 1, characterized in that: Step S1 is specifically as follows: the known target trajectory O G (O1,O2,…,O n ), where O i (i∈1,2,…,n) represents the discrete pose points on the trajectory curve, and the pose information of each point is O i ( o x i,o y i,o z i,o p i,o q i,o r i ),in o x i,o y i,o z i is the position coordinate of the i-th trajectory point, o p i,o q i,o r i is the normal vector of the ith trajectory point, and its target trajectory point coordinate system is denoted by T G .

3. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 2, characterized in that: In step S2, for the intermediate trajectory points, linear interpolation is performed with the path length of each trajectory point as the interpolation node to calculate the deflection angle α corresponding to the intermediate trajectory point i Where: l min is the shortest length of the trajectory, l max is the maximum length of the trajectory, l i is the trajectory length value of the i-th target trajectory point.

4. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 3, characterized in that: Step S3 is specifically: calculating the rotation angle θ of the external axis i After that, the new coordinate position T corresponding to the target trajectory point new ; and calculate the normal vector angle corresponding to the target trajectory point at this time N α i ; where θ i is the normal priority angle value of the rotation external axis corresponding to the i-th target trajectory point; Where: cθ i is cos(θ i ), sθ i is sin(θ i ), o x i,o y i,o z i is the position coordinate of the i-th trajectory point, o p i,o q i,o r is the normal vector of the i-th trajectory point, and its target trajectory point coordinate system is denoted by T G ; Normal vector angle N α i is the angle between the normal vector of the i-th position point after rotation and the X-axis, 5. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 4, characterized in that: In step S4, according to the normal vector angle relationship of step S3 and N α i The value of the reverse solution is the normal priority angle θ of the external axis of rotation i .

6. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 5, characterized in that: From the normal vector angle N α i The calculation formula is: Convert the above formula to: -k1c2+k2s2=k3 in k2= o p io q i Available 7. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 6, characterized in that: In step S5, the coordinate system of the robot system with rotating external axis is denoted as T B , transform the target trajectory point into the robot coordinate system, in, Indicates the coordinate system G T conversion to coordinate system B The transformation matrix of T, R z (θ) represents the rotation matrix corresponding to the rotation angle θ around the external axis. For the normal priority strategy, θ is the position priority angle. N θ i .

8. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 7, characterized in that: Among them, cθ is the abbreviation of cos(θ), sθ is the abbreviation of sin(θ), and the target trajectory point is transformed into: Where: r 11 , r 12 ,…,r 33 is the posture description parameter, x i ,y i , z i It is the position coordinate of the target trajectory point in the robot coordinate system.

9. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 8, characterized in that: Step S6 is as follows: If the robot has no solution or produces a singular solution, set the external axis step length of each change to Δα i , then update the position of the external axis to α i =α i +Δα i , modify the rotation axis angle, jump to step S4 and calculate again until the robot inverse solution meets the requirements.

Images