Robot rotation external axis method vector priority control strategy and inverse solution method

Abstract

The application belongs to the field of intelligent production, and particularly relates to a robot rotation external shaft normal vector priority control strategy and inverse solution method, which comprises the following steps: discretizing a target trajectory, and then converting to a rotation platform coordinate system; adopting a normal priority strategy; inversely solving a rotation angle corresponding to each target point; calculating a pose of the target point under a robot coordinate system; robot inverse solution; and outputting the inverse solution of the robot and the rotation shaft. The application is synchronous motion of the robot and the external shaft, the end motion trajectory of the robot is smooth, and the application is more suitable for situations such as fiber laying and spraying which require continuous smooth trajectory motion. By setting an initial angle and a terminal angle of deflection corresponding to a target trajectory point, the inverse solution of the robot with the rotation external shaft is iteratively completed.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of intelligent production, and particularly relates to a robot rotating external shaft normal vector priority control strategy and inverse solution method. BACKGROUND

[0002] In the prior art, there is a Chinese invention patent with the patent number CN202211275681.3 and the name of "full-automatic motion planning method of industrial robot processing system with additional external shaft", which discloses the following content: a full-automatic motion planning method of industrial robot processing system with additional external shaft, comprising the following steps: step 1, obtaining a processing path point set; step 2, selecting a station mode; including two optional modes of automatically setting a station and manually setting a station, and the automatic station setting mode is further divided into intelligent planning station and fixed allocation station; step 3, performing collision-free motion planning algorithm for each path; the delayed collision and path cutting are fused into the PRM* algorithm to achieve the purpose of reducing time and shortening the path; step 4, integrating the robot motion path of all stations to make the robot execute a processing task.

[0003] The above patent adopts a station type planning robot trajectory, that is, the robot external shaft is moved to a certain station, and certain processing path point sets corresponding to the robot at the station are planned. Then the robot external shaft is moved to the next station, and the corresponding processing path point sets are planned again. The external shaft and the robot are moved in sequence, and the linkage control is not realized. In the case of some continuous motion trajectories, this planning method has disadvantages. SUMMARY

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

[0005] In order to achieve the above application purposes, the technical scheme provided by the application is as follows:

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

[0007] Step S1. Discretizing a target trajectory, and then converting to a rotating platform coordinate system;

[0008] Step S2. Adopting a normal priority strategy: determining a rotating external shaft normal vector priority control strategy, determining an initial deflection angle α start and a final deflection angle α end , and adopting a 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 of the target trajectory point after the workpiece rotates around the rotating external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

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

[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: inverse solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle, jump to step S4 and calculate again until the robot inverse solution meets the requirements;

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

[0014] Further, step S1 is specifically: given a target trajectory O G (O1,O2,…,O n ), wherein O i (i∈1,2,…,n) represents a discrete pose point 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 ), wherein 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 i-th trajectory point, and the coordinate system of the target trajectory point is denoted as T G .

[0015] Further, 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 pointsi

[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] Furthermore, 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 external axis of rotation 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 recorded as 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 can be obtained:

[0025]

[0026] The above formula is converted into:

[0027] -k1c2+k2s2=k3

[0028] wherein

[0029] k2= o p io q i

[0030]

[0031] The following can be obtained:

[0032]

[0033] Further, in step S5, the coordinate system of the robot system with the rotating external axis is denoted as T B , the target trajectory point is converted into the robot coordinate system, wherein, represents the conversion matrix from the coordinate system G T to the coordinate system B T, R z (θ) represents the corresponding rotation matrix after rotating the rotating external axis by an angle θ, and for the normal priority strategy, the value of θ is the position priority angle N θ i .

[0034] Further, wherein, cθ is the abbreviation of cos(θ), and sθ is the abbreviation of sin(θ), and the target trajectory point is transformed into the robot coordinate system as follows:

[0035]

[0036] In the formula: r 11 , r 12 , …, r 33 are attitude description parameters, x i , y i , z i are the position coordinates of the target trajectory point in the robot coordinate system.

[0037] Further, step S6 is specifically: if the robot has no solution or produces a singular solution, assuming that the step length of the external axis is changed each time is Δα i , then the position of the external axis is updated as α i = α i + Δα i , the rotation axis angle is modified, and the step S4 is jumped to again calculate until the robot inverse solution meets the requirements.

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

[0039] 1. This application is about the robot moving 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 and final angles of 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 transforms the target trajectory into a rotating coordinate system. A control strategy for rotating the robot's external axes is then developed, employing a normal-first strategy to determine the position of the target trajectory points in the robot coordinate system. The robot is then inversely solved. If a singularity is encountered, the position angles or normal angles of the external axes are iteratively modified until the robot's singularity is eliminated. Ultimately, this achieves an inverse solution for the motion of a robot with rotating external axes. 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] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are intended to explain the present invention rather than to limit the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall 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 1 As shown, a normal vector priority control strategy and inverse solution method for robot rotation external axis include the following steps:

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

[0051] Step S2. Adopt normal priority strategy: determine the normal priority control strategy of the rotating external axis, determine the initial deflection angle a start and the end deflection angle a end , and adopt 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 of the target trajectory point after the workpiece rotates around the rotating external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

[0053] Step S4. Backward solve the rotation angle corresponding to each target point: according to the rotation axis control strategy, calculate the final rotation axis angle, and adopt the angle relationship in step S3 and the deflection angle value in step S2 to back 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: according to the normal priority angle calculated in step S4 and the assembly relationship of the workpiece, calculate the pose of the target trajectory point in the robot coordinate system;

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

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

[0057] Embodiment 2

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

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

[0060] Step S2. Adopt normal priority strategy: determine the normal priority control strategy of the rotating external axis, determine the initial deflection angle a start and the end deflection angle a end , and adopt 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 of the target trajectory point after the workpiece rotates around the rotating external axis by an angle θ, and calculate the position vector angle and the normal vector angle;

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

[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: inverse solve the robot joint posture. If the robot has no solution or produces a singular solution, modify the rotation axis angle, jump to step S4 and 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] Further, step S1 is specifically: given a target trajectory O G (O1, O2, …, On) n , wherein O i (i∈1, 2, …, n) represents a discrete pose point 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 ), wherein o x i, o y i,o z i is the position coordinate vector of the i-th trajectory point, o p i , o q i,o r i is the normal vector of the i-th trajectory point, and the coordinate system of the target trajectory point is denoted as T G .

[0067] Further, 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 a i

[0068]

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

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

[0071]

[0072] In the formula: cθ i is the abbreviation of cos(θ i ), sθ i is the abbreviation of sin(θ o x i,o y i,o z i is the position coordinate vector 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 the coordinate system of the target trajectory point is denoted as T G ;

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

[0074]

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

[0076] Still further, the normal vector angle N α i calculation formula can be obtained

[0077]

[0078] The above formula is converted to:

[0079] -k1c2+k2s2=k3

[0080] wherein

[0081] k2= 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 angle θ around the external axis. For the normal priority strategy, the value of θ is the position priority angle. N θ i .

[0086] Going 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 is the position coordinate of the target trajectory point in the robot coordinate system.

[0089] Furthermore, 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 (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 zi , 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 recorded as 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 ) abbreviation. 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, whose target trajectory point coordinate system is denoted as T G .

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

[0099]

[0100] 4. According to the normal vector angle relationship in the fourth step and N α i , the normal priority angle θ of the external axis is solved by inverse rotation i .

[0101] According to the normal vector priority inverse rotation axis position:

[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 double-angle formula of trigonometric function: cos(2α i )=2cos 2 α i -1=1-2sin 2 α i

[0106] The above formula is transformed into:

[0107]

[0108] Convert the above formula to:

[0109] -k1c2+k2s2=k3

[0110] where

[0111] k2= o p i o q i

[0112]

[0113] Available

[0114]

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

[0116]

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

[0118] The target trajectory point in the robot coordinate system is transformed as:

[0119]

[0120] In the formula: r 11 , r 12 , …, r 33 are attitude description parameters, x i , y i , z i are the position coordinates of the target trajectory point in the robot coordinate system.

[0121] 6. Inverse solution of the robot joint attitude, if the robot has no solution or singular solution, set the step length of the external axis changed each time as Δα i , then update the position of the external axis as αi = a i + Δa i , modify the rotation axis angle, jump to S4 to calculate again until the robot inverse solution meets the requirements.

[0122] Take a certain robot rotating external axis as an example, as shown in Figure 2 , the robot is on a straight external axis, and there is a rotating external axis beside the robot. The rotating external axis is the workpiece to be processed. The processing target track points are O1, O2, …, O n .

[0123] The rotating external axis normal vector priority control strategy, as shown in Figure 3 , the normal vector angle N a i is the included angle between the normal vector after rotation of the i-th position point and the X-axis.

[0124] The initial deflection angle a start = 80° and the end deflection angle a end = 85° are set. The total length of the target track is about 2 meters. The inverse solution of a certain target track is calculated, and the rotating angle of the external rotating axis is calculated by using the normal priority strategy, as shown in Figure 4 .

Claims

1. A normal vector priority control strategy and inverse solution method for a robot rotating external axis, characterized by: The steps include: Step S1. Discretize the target trajectory and then convert it into the rotating platform coordinate system; Step S2. Adopt normal priority strategy: determine the normal vector priority control strategy of the rotating external axis and determine the initial deflection angle and the end deflection angle , and use 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 rotation angle of the workpiece around the external axis of rotation After that, the new coordinate pose corresponding to the target trajectory point is obtained, and the position vector angle and normal vector angle are calculated; 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 external rotation 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 based on 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; Step S1 is specifically: the known target trajectory ,in Represents the discrete pose points on the trajectory curve, and the pose information of each point is ,in is the position coordinate of the i-th trajectory point, is the normal vector of the i-th trajectory point, and the target trajectory point coordinate system is recorded as ; 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 : ； Where: is the shortest length of the trajectory, is the maximum length of the trajectory, is the trajectory length value of the i-th target trajectory point; Step S3 is specifically: calculating the rotation angle of the external axis After that, the new coordinate pose corresponding to the target trajectory point ; And calculate the normal vector angle corresponding to the target trajectory point at this time ;in is the normal priority angle value of the external axis of rotation corresponding to the i-th target trajectory point; Where: yes The abbreviation of yes The abbreviation of is the position coordinate of the i-th trajectory point, is the normal vector of the i-th trajectory point, and the target trajectory point coordinate system is recorded as ; Normal vector angle is the angle between the normal vector of the i-th position point after rotation and the X-axis, 。 2. A robot rotation external axis normal vector priority control strategy and inverse solution method according to claim 1, characterized in that: In step S4, according to the normal vector angle relationship of step S3 and The normal priority angle of the external axis of the inverse rotation .

3. The robot rotation external axis normal vector priority control strategy and inverse solution method according to claim 2, characterized in that: In step S5, the coordinate system of the robot system with rotating external axis is recorded as , transform the target trajectory point into the robot coordinate system, ;in, Indicates the coordinate system Convert to coordinate system The transformation matrix, Represents rotation around an external axis The corresponding rotation matrix after the angle, for the normal priority strategy, Value is position priority angle .

4. A robot rotating external axis normal vector priority control strategy and inverse solution method according to claim 3, characterized in that: ,in, yes The abbreviation of yes The target trajectory point is transformed into: Where: , ,…, is the posture description parameter, , , is the position coordinate of the target trajectory point in the robot coordinate system.

5. A robot rotation external axis normal vector priority control strategy and inverse solution method according to claim 4, characterized in that: Step S6 is as follows: If the robot has no solution or produces a singular solution, set the external axis step size of each change to , then the updated position of the external axis is , modify the rotation axis angle, jump to step S4 and calculate again until the robot inverse solution meets the requirements.

Images