CN107116542B - A control method and system for a six-joint industrial robot passing a posture singularity - Google Patents

Abstract

The invention belongs to the field of motion control of industrial robots, and specifically discloses a control method and system for a six-joint industrial robot passing through a singular point of posture, including the following steps: 1) intercepting the position in the planned path preset by the industrial robot where the singular point of posture is located A section of trajectory, obtain the position value of the starting point of the trajectory and the last three joint variable values and the position value of the end point and the last three joint variable values; 2) Obtain the last three joint variable values of the pose singularity position by interpolation; 3) Find the first three joint variable values corresponding to the attitude singularity position; 4) take the last three joint variable values corresponding to the attitude singularity position and the first three joint variable values as the motion of the industrial robot corresponding to the attitude singularity position Control parameters to realize the motion control of industrial robots, so that the six-joint industrial robot can pass through the singularity of posture smoothly. The invention enables the robot to pass through the attitude singularity smoothly under the premise of not changing the original planned path, which is simple and efficient.

Description

A control method and system for a six-joint industrial robot passing a posture singularity

technical field

The invention belongs to the field of motion control of industrial robots, and more specifically relates to a control method and system for a six-joint industrial robot passing a singular point of posture.

Background technique

The motion control command is to move the tool from one position to another specified position at a specified speed, specific route mode, etc. When using the motion command, it is necessary to specify what motion method to use to control the motion path to the specified position. There are three types of robot motion. Types: joint motion (J), linear motion (L), circular motion (C). The axes of the last three joints of the six-joint industrial robot intersect at one point. When the axes of the fourth joint and the sixth joint of the robot coincide, the motions cancel each other out, and the robot will lose one degree of freedom. The internal singularity of the robot can be called the posture singularity of the robot. When it is at the pose singularity, the motion type of the robot is linear motion or arc motion. At this time, the motion controller does not know how to select the values of the fourth joint and the sixth joint, which will cause the mechanical arm to lose control and issue an alarm, so that the robot cannot follow the The planned path passes through the attitude singularity, which often occurs in practical applications, which not only reduces the movement space of the robot, but also brings great inconvenience to production.

At present, there are three main processing methods for the attitude singularity of industrial robots: 1) avoiding the attitude singularity of the robot: when encountering the attitude singularity, change the motion path of the robot so as to avoid the singularity; 2) path planning method: The method of path planning for the attitude singularity is used to make the robot pass through the attitude singularity and obtain the analytical solution of the joint variable value through iterative operations; 3) Adjust the attitude method: when it is in the attitude singularity, the joints of the fourth joint and the sixth joint The sum or difference of the variables is a fixed value, and the robot passes through the pose singularity by coordinating the two values.

However, further studies have shown that the above method still has the following problems: Although the method of avoiding the singularity of attitude makes the robot pass the path near the singularity of attitude and avoids the singularity of attitude, it changes the original path of the robot and has blindness. , it is necessary to avoid all the attitude singularities of the robot, the operability is not strong, and it also reduces the space range of the robot; the path planning method is to re-plan the path where the attitude singularities are located, and use complex iterative calculations to obtain The analytical solution of the robot joint variables cannot obtain an accurate numerical solution, so that the robot cannot pass the attitude singularity accurately, and the calculation is cumbersome, and the workload is large; although the attitude adjustment method can make the robot accurately pass the attitude singularity, but in coordination During the movement, the robot is in a stop state, which reduces the efficiency of the robot and may cause discontinuous movement speed after coordination, which brings inconvenience to actual production.

Contents of the invention

Aiming at the above defects or improvement needs of the prior art, the present invention provides a control method and system for a six-joint industrial robot passing a singular point of posture, which adopts the method of interpolation to replace the posture of the robot end, without changing the original planned path. Under the premise, the robot can pass through the attitude singularity smoothly, thereby solving the current technical problem that the robot's range of motion is limited due to the attitude singularity, and has the characteristics of simplicity, high computing efficiency, and strong applicability.

In order to achieve the above object, according to one aspect of the present invention, a control method for a six-joint industrial robot passing a singular point of attitude is proposed, which includes the following steps:

(1) Intercept a segment of the trajectory in the preset planning path of the industrial robot where the attitude singularity is located, and obtain the position value corresponding to the starting point of the trajectory and the last three joint variable values, as well as the position value corresponding to the end point of the trajectory and the last three joint variables value;

(2) According to the last three joint variable values of the starting point and the last three joint variable values of the end point, obtain the last three joint variable values corresponding to the pose singularity position by interpolation;

(3) Establish the kinematics inverse solution equation to obtain the first three joint variable values corresponding to the posture singularity position;

(4) Use the last three joint variable values and the first three joint variable values corresponding to the posture singularity position as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, so that the six The articulated industrial robot passed the pose singularity smoothly.

As further preferably, said step (3) includes the following sub-steps:

(3.1) First establish the following matrix equation:

Among them, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ =sin(θ 2 ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ , θ ₂ , θ ₃ are the first joints of the industrial robot Joint variables to the third joint, a ₁ and a ₂ represent the lengths of the first and second links respectively, d ₂ represents the offset between the first and second links, x ₃ , y ₃ , z ₃ is the three coordinate components of the origin of the tool coordinate system relative to the third link coordinate system;

(3.2) Multiply the two ends of the matrix equation established in step (3.1) by the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at the same time, establish the kinematics inverse solution equation, and obtain the first joint and the joint variable values θ ₁ and θ ₃ of the third joint:

Wherein, a ₁ and a ₂ represent the lengths of the first connecting rod and the second connecting rod respectively, and d ₂ represents the offset between the first connecting rod and the second connecting rod;

(3.3) To the inverse matrix of the homogeneous transformation matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at both ends of the matrix equation established in step (3.1), establish the kinematics inverse solution equation, and obtain the second joint The joint variable value θ ₂ of :

Wherein, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ).

As a further preference, the total number of interpolation points N is preferably determined in the following manner:

Among them: x ₁ , y ₁ , z ₁ are the position values of the starting point of the track, x ₂ , y ₂ , z ₂ are the position values of the end point of the track, V is the interpolation speed, and T is the interpolation period.

As a further preference, the interpolation speed is preferably 10 mm/s, and the interpolation cycle is preferably 1 ms.

As a further preference, the step (1) also includes the following steps before: first, establish a link coordinate system according to the six-joint industrial robot, and perform kinematic inverse solution to the preset planned path to obtain the joint variable values of the industrial robot, Then it is judged whether the six-joint industrial robot is in the pose singularity, if so, enter step (1), if not, realize the motion control of the industrial robot with the variable values of each joint of the industrial robot obtained from the inverse solution.

According to another aspect of the present invention, there is provided a control system for a six-joint industrial robot passing a singular point of posture, which includes:

The data acquisition module is used to intercept a section of the trajectory in the preset planning path of the industrial robot where the attitude singularity is located, and obtain the position value corresponding to the starting point of the trajectory and the last three joint variable values, as well as the position value corresponding to the end point of the trajectory and the last three joint variables. joint variable value;

An interpolation module, configured to obtain the last three joint variable values corresponding to the pose singularity position by interpolation according to the last three joint variable values of the starting point and the last three joint variable values of the end point;

The data processing module is used to establish the kinematics inverse solution equation and obtain the first three joint variable values corresponding to the attitude singularity position;

The motion control module is used to use the last three joint variable values corresponding to the posture singularity position and the first three joint variable values as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, to This makes the six-joint industrial robot pass through the pose singularity smoothly.

As further preferred, the data processing module includes the following submodules:

The matrix equation establishment sub-module is used to establish the following equations:

Among them, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ =sin(θ 2 ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ , θ ₂ , θ ₃ are the first joints of the industrial robot to the joint variable value of the third joint, a ₁ and a ₂ represent the lengths of the first link and the second link respectively, d ₂ represents the offset between the first link and the second link, x ₃ , y ₃ , z ₃ is the three coordinate components of the origin of the tool coordinate system relative to the coordinate system of the third link;

The first joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module to establish a kinematic inverse Solve the equation to obtain the joint variable values θ ₁ and θ ₃ of the first joint and the third joint:

The second joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module to establish a kinematic inverse Solve the equation to obtain the joint variable value θ ₂ of the second joint:

Wherein, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ).

As a further preference, when the interpolation module performs interpolation, the total number of interpolation points N is preferably determined in the following manner:

Among them: x ₁ , y ₁ , z ₁ are the position values of the starting point of the track, x ₂ , y ₂ , z ₂ are the position values of the end point of the track, V is the interpolation speed, and T is the interpolation period.

As a further preference, the interpolation speed is preferably 10 mm/s, and the interpolation cycle is preferably 1 ms.

As a further preference, the control system also includes a judging module, which is used to establish a link coordinate system according to the six-joint industrial robot, and perform kinematic inverse solution to the preset planned path to obtain the joint variable values of the industrial robot, and then judge Whether the six-joint industrial robot is in the pose singularity.

Generally speaking, compared with the prior art, the above technical solution conceived by the present invention mainly has the following technical advantages:

1. The present invention adopts the method of keeping the original position of the end of the industrial robot unchanged and obtaining the attitude of the end of the industrial robot through interpolation, so that the industrial robot can pass through the singularity of the posture smoothly without changing the original planned path, which greatly reduces the need to pass through the external position. The production cost of the equipment passing through the attitude singularity avoids the complex iterative calculation of the analytical solution obtained by path planning, and can directly obtain the numerical solution of the joint position, which is simple and easy to operate and has high calculation efficiency. It is suitable for the last three joints intersecting at one point industrial robots.

2. The present invention starts from the field of industrial robot motion control, so that the industrial robot can pass through the posture singularity smoothly, expand the range of motion control instructions of the industrial robot, make the motion space of the industrial robot more flexible and intelligent, and avoid the Attitude singularity causes problems such as reduced range of motion or alarms. The program can be directly implanted into the motion controller of the industrial robot, so that the industrial robot can pass through the attitude singularity without stopping, which brings great convenience to actual production.

3. The present invention can also be applied to path planning without attitude singularity, and the joint variable values of a series of points in the working path are not obtained through kinematics inverse solution, thereby avoiding that the joint variable value of the fourth joint cannot be calculated in the process of inverse solution The obtained phenomenon can directly obtain the joint variable value through interpolation, which is simple and practical.

Description of drawings

Figure 1 is a flow chart of a six-joint industrial robot passing through a singular point of posture;

Figure 2 is the configuration diagram of Huazhong CNC 6012 industrial robot;

Figure 3 is the coordinate system of each link of Huazhong CNC 6012 industrial robot.

Detailed ways

In order to make the object, technical solution and advantages of the present invention more clear, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not constitute a conflict with each other.

A control method for a six-joint industrial robot of the present invention through a posture singularity, specifically comprises the following steps:

(1) Intercept a section of trajectory in the preset planning path of the industrial robot where the attitude singularity is located, and obtain the position value of the starting point of the trajectory and the last three joint variable values as well as the position value of the trajectory end point and the last three joint variable values;

(2) According to the last three joint variable values of the starting point and the last three joint variable values of the end point, obtain the last three joint variable values corresponding to the pose singularity position by interpolation;

(3) Obtain the first three joint variable values corresponding to the position of the attitude singularity;

(4) Use the last three joint variable values and the first three joint variable values corresponding to the posture singularity position as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, so that the six The articulated industrial robot passed the pose singularity smoothly.

In order to facilitate the calculation and calculation of each variable, the following preparatory work needs to be done before step (1): According to the six-joint industrial robot (including six joints and six connecting rods, respectively, the first joint, the second joint, and the third joint , fourth joint, fifth joint, sixth joint, first link, second link, third link, fourth link, fifth link, sixth link) to establish link coordinates The coordinate system can be established in many existing ways. The present invention uses the D-H method to establish the coordinate system as an example for illustration, which is not intended to limit the present invention.

According to the structural characteristics of industrial robots, use the DH method to establish the coordinate system of the link: take the axis direction of joint i as the z-axis z _i of the coordinate system {i}, and take the common vertical line direction of the axes of joints i and i+1 as the coordinate system { The x-axis x _i of i} points from joint i to joint i+1, and the y-axis y _i of the coordinate system {i} is stipulated according to the right-hand rule, and the intersection point of x _i and y _i is taken as the origin o _i of the coordinate system {i} , the base coordinate system is arbitrarily selected. For the sake of simplicity and convenience, the preferred base coordinate system {0} coincides with the coordinate system {1} of the first link 1, and the intersection point of the three rear joint axes of the industrial robot is selected as the link coordinates The origin of the systems {4}, {5} and {6}, so far establish the coordinate system of each link of the industrial robot.

According to the connecting rod coordinate system set above, the corresponding connecting rod parameters can be defined as follows:

a _i-1 = distance from z _i-1 to z _i measured along x _i-1 ;

α _i-1 = from z _i-1 to the angle that z _i rotates around x _i-1 ;

d _i = the distance measured along z _i from x _i-1 to x _i ;

θ _i = the angle from _xi-1 to the rotation of _xi around _zi .

List the corresponding connecting rod parameters according to the established connecting rod coordinate system, and use the listed connecting rod parameters to calculate the transformation matrix of each connecting rod Deduce the equivalent secondary transformation matrix of the third link coordinate system relative to the base coordinate system and the homogeneous transformation matrix of the sixth link coordinate system relative to the third link coordinate system

Among them, i=0,1...,6, θ ₁ , θ ₂ , θ ₃ are the joint variables from the first joint to the third joint, {i} represents the coordinate system fixed to the i-th connecting rod, is the homogeneous transformation matrix of the industrial robot linkage coordinate system {i} relative to {i+1}.

Specifically, as shown in Figure 3, the axis of the first joint is in the vertical direction, which is taken as the z-axis z ₁ of the coordinate system {1}, preferably pointing upward, and the axes of the first joint and the second joint are vertical The line direction is the x-axis x ₁ of the coordinate system {1}, pointing from the first joint to the second joint, then the intersection point of x ₁ and z ₁ is the origin o ₁ of the coordinate system {1}; the second joint and the third joint The axes of the joints are in the horizontal direction, which are the z axes z ₂ and z ₃ of the coordinate systems {2} and {3} respectively, and z ₂ and z ₃ are parallel to each other, and the axes of the second joint and the third joint are in the same vertical direction As the x-axis x ₂ of the coordinate system {2}, the intersection point of x ₂ and z ₂ is the origin o ₂ of the coordinate system {2}; the common vertical line direction of the axes of the third joint and the fourth joint is taken as the coordinate system {3 }'s x-axis x ₃ , the intersection of x ₃ and z ₃ is the origin o ₃ of the coordinate system {3}; the base coordinate system can be selected arbitrarily, for the sake of simplicity and convenience, the preferred base coordinate system {0} and the first The coordinate system {1} of the connecting rod coincides; the axes of the fourth joint, the fifth joint and the sixth joint intersect at one point, and this point is selected as the origin o of the connecting rod coordinate system {4}, {5} and {6} _4,5,6 , the axis direction of the fourth joint is taken as the z-axis z ₄ of the coordinate system {4}, the direction of the x-axis x ₄ of the preferred coordinate system {4} is consistent with x ₃ , and the axis of the fifth joint Orientation as the z-axis z ₅ of the coordinate system {5}, the direction of the z-axis z ₆ of the preferred coordinate system {6} is consistent with z ₄ , the x-axis x ₅ of the preferred coordinate system {5} and the coordinate system {6} The direction of the x-axis x ₆ of the tool is consistent with x ₄ ; the preferred center of the tool end is used as the origin o _T of the tool coordinate system {T}, and the direction of the x-axis x _T of the preferred tool coordinate system {T} is consistent with x ₆ , preferably The direction of the z-axis z _T of the tool coordinate system {T} is consistent with z ₆ . _The distance along x1 from z1 to _z2 is a1, the distance along _z2 from _x1 to _x2 is _d2 , the distance along _x2 from _z2 to _z3 is _a2 , and the _distance from _z3 to _{z 4} has a distance a ₃ along x ₃ and a distance d ₄ from x ₃ to x ₄ along z ₄ . List the corresponding connecting rod parameters according to the established connecting rod coordinate system, and use the listed connecting rod parameters to calculate the transformation matrix of each connecting rod Deduce the equivalent secondary transformation matrix of the third link coordinate system relative to the base coordinate system and the homogeneous transformation matrix of the sixth link coordinate system relative to the third link coordinate system

Where, i=0,1...,6, θ _i is the joint variable of joint i, s _i =sin(θ _i ), _ci =cos(θ _i ), s ₂₃ =sin(θ ₂ +θ ₃ ), c ₂₃ ＝cos(θ ₂ +θ ₃ ), {i} represents the coordinate system fixed to the i-th connecting rod, is the homogeneous transformation matrix of the industrial robot link coordinate system {i} relative to {i-1}.

After the linkage coordinate system is established, perform kinematic inverse solution to the preset planning path (the path is determined by the actual motion demand, and the present invention does not limit it) to obtain the joint variable values of the industrial robot, which is the prior art, here Without going into details, under normal circumstances, the industrial robot moves according to the preset planning path, that is, when it is not in the attitude singularity, the industrial robot runs normally. When the joint variable value of the fourth joint cannot be obtained, that is, the joint variable of the fifth joint When it is zero, the industrial robot is in a posture singularity at this time. Since the fifth joint variable is zero, it is difficult to pass smoothly. Therefore, the present invention performs the following processing on the motion pose of the industrial robot:

(1) Intercept a section of the trajectory in the planned path where the attitude singularity is located. In order to improve the calculation efficiency, the preferred trajectory is the entire arc segment or straight line segment where the attitude singularity is located, and then obtain the position value of the starting point of this trajectory and the last three joint variable values, as well as the position value of the trajectory end point and the last three joint variable values;

(2) Then the position value of the starting point and the last three joint variable values, as well as the position value of the trajectory end point and the last three joint variable values are sent to the interpolator for interpolation to obtain the last three corresponding to the attitude singularity position The joint variable value, specifically according to the start and end position values, the interpolation speed V and the interpolation period T can obtain the total number of points to be interpolated N. The interpolation speed and interpolation period of the interpolator can be based on the actual situation of the industrial robot The demand is given, considering the accuracy and efficiency of the interpolation, the optimal interpolation speed is 10mm/s, the interpolation period is 1ms, and the interpolation between the start point and the end point of the trajectory is divided into N parts according to the total number of interpolation points N, and also That is, the last three joint variable values between the trajectory start point and the trajectory end point are also interpolated into N parts, and the last three joint variable values corresponding to the posture singularity position are obtained; among them, the sum of the trajectory start point and the trajectory end point is obtained by interpolation The position value of the pose singularity and the last three joint variable values are: x ₁ , y ₁ , z ₁ , θ ₄₁ , θ ₅₁ , θ ₆₁ , x ₂ , y ₂ , z ₂ , θ ₄₂ , θ ₅₂ , θ ₆₂ , x _t , y _t , z _t , θ _4t , θ _5t , θ _6t ,

When at the attitude singularity, the motion type of the industrial robot is linear motion or circular motion, so each point obtained by the interpolator interpolation is on the original planned path.

(3) Obtain the first three joint variable values corresponding to the pose singularity position

(3.1) First establish the following matrix equation:

Among them, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ =sin(θ 2 ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ , θ ₂ , θ ₃ are the first joints of the industrial robot Joint variables to the third joint, a ₁ and a ₂ represent the lengths of the first and second links respectively, d ₂ represents the offset between the first and second links, x ₃ , y ₃ , z ₃ is the three coordinate components of the origin of the tool coordinate system relative to the third link coordinate system;

Specifically, the matrix equation is established in the following way:

Multiply the last three link transformation matrices with the homogeneous transformation matrix of the tool coordinate system relative to the sixth link coordinate system to get:

which is

in, Represents the rotation matrix of the tool coordinate system relative to the third link coordinate system, Indicates the rotation matrix of the tool coordinate system relative to the sixth link coordinate system, Indicates the position vector of the tool coordinate system relative to the third link coordinate system, Indicates the position vector of the tool coordinate system relative to the sixth link coordinate system;

Let the elements of the fourth column at both ends of the equation be equal, and the last three joint variable values of the attitude singularity have been obtained through interpolation, and the right side of the equation is known:

The first three joints of the industrial robot are fixed (that is, there is no relative motion), and the position vector of the known tool coordinate system relative to the base coordinate system is: The position vector of the tool coordinate system relative to the third link coordinate system is: The latter three joint variables have been obtained by interpolation, and the kinematic equation of the industrial robot is written as:

which is Let the elements of the fourth column at both ends of the above matrix equation be correspondingly equal, and the following equation can be established:

which is

(3.2) Multiply the two ends of the matrix equation established in step (3.1) by the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at the same time, establish the kinematics inverse solution equation, and obtain the first joint and the joint variable values θ ₁ and θ ₃ of the third joint:

Specifically: the matrix equation established in step (3.1) is multiplied by the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at the same time The kinematic inverse solution equation is established as follows:

resolved:

In the formula, available by Get the inverse.

Let the elements of the second row and the first column at both ends of the matrix equation (1) be correspondingly equal:

-s ₁ x _t +c ₁ y _t = -z ₃ -d ₂ ;

like Then get the joint variable value θ ₁ of the first joint:

After selecting one of the solutions of θ ₁ , let the elements of the first row, fourth column and third row, fourth column at both ends of the matrix equation (1) be equal to each other:

The sum of the squares of the above two formulas is:

-s ₃ y ₃ +c ₃ x ₃ =k,

like Then get the joint variable value θ ₃ of the third joint:

(3.3) To the inverse matrix of the homogeneous transformation matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at both ends of the matrix equation established in step (3.1), establish the kinematics inverse solution equation, and obtain the second joint The joint variable value θ ₂ of :

Wherein, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ).

Specifically: the matrix equation established in step (3.1) is multiplied to the left by the inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at the same time The kinematic inverse solution equation is established as follows:

resolved

In the formula, available by Get the inverse.

Let the elements of the first row, the fourth column and the second row, the fourth column at both ends of the matrix equation (2) be correspondingly equal:

Solve the above two equations simultaneously to get s ₂₃ and:

It can be seen that the denominators of the expressions of s ₂₃ and c ₂₃ are equal and positive, so the joint variable value θ ₂ of the second joint is obtained:

Wherein, s ₂ =sin(θ ₂ ); c ₂ =cos(θ ₂ ); s ₂₃ =sin(θ ₂ +θ ₃ ); c ₂₃ =cos(θ ₂ +θ ₃ ).

(4) Use the last three joint variable values and the first three joint variable values corresponding to the posture singularity position as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, so that the six The articulated industrial robot passed the pose singularity smoothly. That is to say, the first three joint variable values obtained by the above algorithm and the last three joint variable values obtained by interpolation are transmitted to the motion controller of the industrial robot, so that the six joints of the industrial robot have definite values at the attitude singularities, so that In this way, the industrial robot can successfully pass through the pose singularity.

The present invention also provides a control system for a six-joint industrial robot passing through a singular point of posture, including: a data acquisition module, which is used to intercept a segment of the trajectory in the preset planning path of the industrial robot where the singular point of posture is located, and obtain the corresponding position of the starting point of the trajectory. The position value and the last three joint variable values and the position value and the last three joint variable values corresponding to the end point of the trajectory; the interpolation module is used to base the last three joint variable values of the starting point and the last three joint variable values at the end point The variable values are interpolated to obtain the last three joint variable values corresponding to the posture singularity position; the data processing module is used to establish the kinematics inverse solution equation and obtain the first three joint variable values corresponding to the posture singularity position; The control module is used to use the last three joint variable values corresponding to the posture singularity position and the first three joint variable values as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, thereby This makes the six-joint industrial robot pass through the pose singularity smoothly.

Specifically, the data processing module includes the following submodules:

The matrix equation establishment sub-module is used to establish the following equations:

Among them, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ =sin( θ ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ , θ ₂ , θ ₃ are the first in industrial robots Joint variables from the joint to the third joint, a ₁ and a ₂ represent the lengths of the first link and the second link respectively, d ₂ represents the offset between the first link and the second link, x ₃ , y ₃ , z ₃ are the three coordinate components of the origin of the tool coordinate system relative to the third link coordinate system;

The first joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module to establish a kinematic inverse Solve the equation to obtain the joint variable values θ ₁ and θ ₃ of the first joint and the third joint:

The second joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module to establish a kinematic inverse Solve the equation to obtain the joint variable value θ ₂ of the second joint:

Wherein, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ).

Specifically, the total number of interpolation points N when the interpolation module performs interpolation is preferably determined in the following manner:

Among them: x ₁ , y ₁ , z ₁ are the position values of the starting point of the track, x ₂ , y ₂ , z ₂ are the position values of the end point of the track, V is the interpolation speed, and T is the interpolation cycle. The interpolation speed is preferably 10 mm/s, and the interpolation cycle is preferably 1 ms.

Specifically, the control system also includes a judging module, which is used to establish a link coordinate system according to the six-joint industrial robot, and perform kinematic inverse solution to the preset planned path to obtain the variable values of each joint of the industrial robot, and then judge the six-joint Whether the industrial robot is at a singular point of attitude, if so, enter step (1), if not, realize the motion control of the industrial robot with the joint variable values of the industrial robot obtained from the inverse solution.

The following are specific examples:

In this embodiment, the Huazhong CNC 6012 industrial robot is taken as an example. The second connecting rod 2, the third connecting rod 3, the fourth connecting rod 4, the fifth connecting rod 5, the sixth connecting rod 6) and 6 rotating joints (points A, B, C, D as shown in Figure 3 , E, F,), the base is fixed and called link 0, the first joint connects the base and the first link, the second joint connects the first link and the second link, and so on, the tool It is fixedly connected with the sixth connecting rod 6. The process of passing through the attitude singularity mainly includes the following steps:

Step 1): According to the structural characteristics of Huazhong CNC 6012 industrial robot and the DH method, set the linkage coordinate system {0}, {1}, {2}, {3}, {4}, {5}, {6 } and {T} are shown in Figure 3: the base coordinate system {0} coincides with the coordinate system {1} of the first link 1, and the z-axis z ₁ of the coordinate system {1} is the axis direction of the first joint, pointing to Upward, the x-axis x ₁ of the coordinate system {1} is the direction of the common vertical line of the axis of the first joint and the second connecting rod 2, pointing from the first joint to the second joint, then the origin o ₁ of the coordinate system {1} is is the intersection point of x ₁ and z ₁ ; the coordinate system {1} has been established so far, and the coordinates {2} and {3} have been established by analogy; the origin o ₄ of the linkage coordinate system {4}, {5} and {6} _,5,6 are the fourth joint, the axis intersection of the fifth link 5 and the sixth link 6, the z-axis of the coordinate system {4} z ₄ is the axis direction of the fourth joint, and the x-axis of the coordinate system {4} The direction of x ₄ is consistent with that of x ₃ , the z-axis z ₅ of the coordinate system {5} is the axis direction of the fifth joint, the direction of the z-axis z ₆ of the coordinate system {6} is consistent with z ₄ , and the z-axis of the coordinate system {5} The direction of the x-axis x ₅ and the x-axis x ₆ of the coordinate system {6} is consistent with x ₄ ; the origin o _T of the tool coordinate system {T} is the center of the tool end, and the direction of the x-axis x _T of the tool coordinate system {T} Consistent with x ₆ , the direction of the z-axis z _T of the tool coordinate system {T} is consistent with z ₆ .

According to the established coordinate system of each connecting rod, the corresponding connecting rod parameters are listed, as shown in Table 1.

Table 1 Connecting rod parameters of Huazhong CNC 6012 industrial robot

Use the listed link parameters to calculate the transformation matrix of each link, and derive the equivalent secondary transformation matrix of the third link coordinate system relative to the base coordinate system

Step 2): The position of the industrial robot at the attitude singularity is: (1323.1, 226.8, 703.8), the path where the attitude singularity is located is a straight line, intercept the entire straight line segment in the planned path where it is located, and the starting point of the straight line segment is The position value is (999.2, 450.9, 532.3), the joint variable value of the last three joint variables is (-108.5°, -30.6°, 223.8°), the position value of the end point is (1512.3, 105.6, 796.4), the last three joint variables The joint variable values of the joint variable are (36.3°, -15.1°, 77.1°).

Step 3): Send the start point, end position and the last three joint variable values to the interpolator, the interpolation speed is 10mm/s, the interpolation period is 1ms, and the total number of interpolation points N:

Interpolate the last three joint variable values between the start point and the end point of the trajectory into 68 parts to obtain the last three joint variable values corresponding to the attitude singularity position: (-11.9°, -20.2°, 119.3°).

Step 4): The position vector of the known tool coordinate system relative to the sixth link coordinate system is: Substituting the last three joint variable values θ ₄ , θ ₅ , θ ₆ obtained through interpolation into formula (2):

Therefore, the position vector of the tool coordinate system relative to the third link coordinate system for:

Connect the first three joints of the industrial robot. At the attitude singularity, the position vector of the known tool coordinate system relative to the base coordinate system is: The last three joint variables θ ₄ , θ ₅ , θ ₆ have been obtained through interpolation, and the kinematic equation of the industrial robot is written as:

Let the elements of the fourth column at both ends of the matrix equation (4) correspond to be equal, and the following equation can be established:

The inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system is multiplied to the left by both ends of the matrix equation (5) The kinematic inverse solution equation is established as follows:

Make the elements of the second row and the first column at both ends of the matrix equation (6) correspondingly equal: -1332.1s ₁ +226.8c ₁ =-346.8(7); solve the joint variable value of the first joint: Or θ ₁ =24.5°.

After selecting one of the solutions of θ ₁ , let the elements of the first row, fourth column and third row, fourth column at both ends of the matrix equation (6) be equal to each other:

The sum of the squares of the above two formulas is: -(-1254)s ₃ +(-42.1)c ₃ =k (9);

in,

When θ ₁ =174.8°, the joint variable value of the third joint: θ ₃ =-343.5° or θ ₃ =-192.6°; when θ ₁ =24.5°, the joint variable value of the third joint: θ ₃ =-9.8 ° or θ ₃ =-166.4°.

The inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system is multiplied to the left by both sides of the matrix equation (5) The kinematic inverse solution equation is established as follows:

Let the elements of the first row, the fourth column and the second row, the fourth column at both ends of the matrix equation (10) be equal to each other:

Solve the above two equations simultaneously to get s ₂₃ and c ₂₃ ,

Solve to get: θ ₂₃ =-141°, θ ₂₃ =-86.1°, θ ₂₃ =88.8°, θ ₂₃ =158.5°.

Therefore, the joint variable values of the second joint are: θ ₂ =202.5°, θ ₂ =106.5°, θ ₂ =98.6°, θ ₂ =324,9°. There are four sets of solutions for the inverse solutions of the first three joint variables, as shown in Table 2:

Table 2 Inverse solution values of the first three joint variables of Huazhong CNC 6012 industrial robot

θ₁ θ₂ θ₃ First group 174.8° 202.5° -343.5° Second Group 174.8° 106.5° -192.6° The third group 24.5° 98.6° -9.8° Fourth group 24.5° 324.9° -166.4°

Only the third group of solutions conforms to the range of motion of the joints given in Table 1, that is, the first three joint variables are: θ ₁ =24.5°, θ ₂ =98.6°, θ ₃ =-9.8°.

Step 5): Transfer the first three joint variable values obtained by the above algorithm and the last three joint variable values obtained by interpolation to the motion controller of the industrial robot, so that the six joints of the industrial robot have definite values at the posture singularity ; In this way, the industrial robot can pass through the attitude singularity smoothly.

In this example, according to the above position and kinematics inverse solution equation, the first three joint variables when the robot is at the attitude singularity are obtained, and the joint variables of the last three joints are obtained through interpolation, and the end position of the robot is obtained through kinematics positive solution checking Corresponds to the actual end position.

The invention provides a control method for a six-joint industrial robot passing through the singularity of posture, which enables the robot to pass through the singularity of posture without changing the original planned path, and greatly reduces the production cost of passing through the singularity of posture through external equipment , avoiding the complex iterative calculation from the path planning method to the analytical solution, and can directly obtain the numerical solution of the joint position when the robot is at a singular point of attitude, which is simple and easy to operate and has high calculation efficiency. The program can be directly implanted into the robot motion controller, which brings great convenience to the actual production.

It is easy for those skilled in the art to understand that the above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements and improvements made within the spirit and principles of the present invention, All should be included within the protection scope of the present invention.

Claims (8)

1. A method for controlling a six-joint industrial robot through a posture singularity, the six-joint industrial robot comprising successively connected bases, first joints, first connecting rods, second joints, second connecting rods, third joints, The third connecting rod, the fourth joint, the fourth connecting rod, the fifth joint, the fifth connecting rod, the sixth joint and the sixth connecting rod, the sixth connecting rod is fixedly connected with the tool, and the first, second and third joints are the front Three joints, the fourth, fifth and sixth joints are the last three joints, which are characterized in that, comprising the following steps: (1) Intercept a segment of the trajectory in the preset planning path of the industrial robot where the attitude singularity is located, and obtain the position value corresponding to the starting point of the trajectory and the last three joint variable values, as well as the position value corresponding to the end point of the trajectory and the last three joint variables value; (2) Obtain the last three joint variable values corresponding to the pose singularity position in an interpolation manner according to the last three joint variable values at the starting point of the trajectory and the last three joint variable values at the end point of the trajectory; (3) Establish the kinematics inverse solution equation to obtain the first three joint variable values corresponding to the attitude singularity position: (3.1) First establish the following matrix equation: in, is the position vector of the tool coordinate system relative to the base coordinate system, is the homogeneous transformation matrix of the third link coordinate system of the industrial robot relative to the base coordinate system, Indicates the position vector of the tool coordinate system relative to the third link coordinate system, the last three joint variable values obtained by interpolation and the position vector of the tool coordinate system relative to the sixth link coordinate system Calculated, θ ₁ , θ ₂ and θ ₃ are respectively the joint variable value of the first joint, the joint variable value of the second joint and the joint variable value of the third joint to be solved; said Calculated using the following formula: Among them, s _i =sin(θ _i ), c _i =cos(θ _i ), i=4,5,6, θ _i is the joint variable value of the i-th joint obtained by interpolation, and a ₃ is from the coordinate system { The distance from the z-axis z ₃ of 3} to the z-axis z ₄ of the coordinate system {4} along the x-axis x 3 of the coordinate system {3}, d ₄ is the distance from the x-axis x ₃ of the coordinate system { ₃ } to the coordinate system { 4}’s x-axis x ₄ is the distance along the coordinate system {4}’s z-axis z ₄ , and {i} represents the coordinate system fixed to the i-th connecting rod; (3.2) multiply the two ends of the matrix equation established in step (3.1) to the left simultaneously by the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system Establish kinematics inverse solution equation The joint variable values θ ₁ and θ ₃ of the first joint and the third joint are obtained as follows: Among them, Atan is the arctangent function, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ = sin(θ ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ ,θ ₂ ,θ ₃ is the joint variable value of the first joint to the third joint of the industrial robot, a ₁ and a ₂ represent the lengths of the first link and the second link respectively, and d ₂ represents the deflection between the first link and the second link x ₃ , y ₃ , z ₃ are the three coordinate components of the origin of the tool coordinate system relative to the third link coordinate system; (3.3) to the inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system to the matrix equation two ends that set up in step (3.1) Establish kinematics inverse solution equation The joint variable value θ ₂ of the second joint is obtained as follows: Wherein, Atan is arctangent function, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ); (4) The last three joint variable values corresponding to the attitude singularity position obtained by interpolation in step (2) and the first three joint variable values obtained by solving the step (3) are used as the position corresponding to the attitude singularity of the industrial robot The motion control parameters are used to realize the motion control of the industrial robot, so that the six-joint industrial robot can pass through the attitude singularity smoothly. 2. The six-joint industrial robot as claimed in claim 1 is characterized in that the total number of points N of said interpolation is determined in the following manner: Among them, x ₁ , y ₁ , z ₁ are the position values of the starting point of the track, x ₂ , y ₂ , z ₂ are the position values of the end point of the track, V is the interpolation speed, and T is the interpolation period. 3. The method for controlling a six-joint industrial robot passing a posture singularity according to claim 2, wherein the interpolation speed is 10 mm/s, and the interpolation period is 1 ms. 4. The six-joint industrial robot as claimed in any one of claims 1-3 through the control method of attitude singularity, is characterized in that, before the step (1), the following steps are also included: first, establish a connection according to the six-joint industrial robot. Rod coordinate system, and carry out kinematics inverse solution to the preset planning path to obtain the joint variable values of the industrial robot, and then judge whether the six-joint industrial robot is in the attitude singularity, if so, go to step (1), if not, then The motion control of the industrial robot is realized by the variable value of each joint of the industrial robot obtained from the inverse solution. 5. A control system for a six-joint industrial robot passing through a singular point of posture, the six-joint industrial robot includes a base, a first joint, a first connecting rod, a second joint, a second connecting rod, a third joint, The third connecting rod, the fourth joint, the fourth connecting rod, the fifth joint, the fifth connecting rod, the sixth joint and the sixth connecting rod, the sixth connecting rod is fixedly connected with the tool, and the first, second and third joints are the front Three joints, the fourth, fifth and sixth joints are the last three joints, which are characterized in that they include: The data acquisition module is used to intercept a section of the trajectory in the preset planning path of the industrial robot where the attitude singularity is located, and obtain the position value corresponding to the starting point of the trajectory and the last three joint variable values, as well as the position value corresponding to the end point of the trajectory and the last three joint variables. joint variable value; An interpolation module, configured to obtain the last three joint variable values corresponding to the attitude singularity position by interpolation according to the last three joint variable values at the starting point of the trajectory and the last three joint variable values at the trajectory end point; The data processing module is used to establish the kinematics inverse solution equation and obtain the first three joint variable values corresponding to the attitude singularity position; the data processing module includes the following sub-modules: The matrix equation establishment sub-module is used to establish the following equations: in, is the position vector of the tool coordinate system relative to the base coordinate system, is the homogeneous transformation matrix of the third link coordinate system of the industrial robot relative to the base coordinate system, Indicates the position vector of the tool coordinate system relative to the third link coordinate system, the last three joint variable values obtained by interpolation and the position vector of the tool coordinate system relative to the sixth link coordinate system Calculated, θ ₁ , θ ₂ and θ ₃ are respectively the joint variable value of the first joint, the joint variable value of the second joint and the joint variable value of the third joint to be solved; said Calculated using the following formula: Among them, s _i =sin(θ _i ), c _i =cos(θ _i ), i=4,5,6, θ _i is the joint variable value of the i-th joint obtained by interpolation, and a ₃ is from the coordinate system { The distance from the z-axis z ₃ of 3} to the z-axis z ₄ of the coordinate system {4} along the x-axis x 3 of the coordinate system {3}, d ₄ is the distance from the x-axis x ₃ of the coordinate system { ₃ } to the coordinate system { 4}’s x-axis x ₄ is the distance along the coordinate system {4}’s z-axis z ₄ , and {i} represents the coordinate system fixed to the i-th connecting rod; The first joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the first link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module Establish kinematics inverse solution equation The joint variable values θ ₁ and θ ₃ of the first joint and the third joint are obtained as follows: Among them, Atan is the arctangent function, x _t , y _t , z _t are the three coordinate components of the origin of the tool coordinate system relative to the base coordinate system, s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₂₃ = sin(θ ₂ +θ ₃ ), c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₂₃ =cos(θ ₂ +θ ₃ ), θ ₁ ,θ ₂ ,θ ₃ is the joint variable value of the first joint to the third joint of the industrial robot, a ₁ and a ₂ represent the lengths of the first connecting rod and the second connecting rod respectively, and d ₂ represents the deviation between the first connecting rod and the second connecting rod x ₃ , y ₃ , z ₃ are the three coordinate components of the origin of the tool coordinate system relative to the third link coordinate system; The second joint variable value calculation sub-module is used to simultaneously multiply the inverse matrix of the homogeneous transformation matrix of the third link coordinate system relative to the base coordinate system at both ends of the matrix equation established by the matrix equation establishment sub-module Establish kinematics inverse solution equation The joint variable value θ ₂ of the second joint is obtained as follows: Wherein, Atan is arctangent function, c ₃ =cos(θ ₃ ), s ₃ =sin(θ ₃ ); The motion control module is used to use the last three joint variable values corresponding to the posture singularity position and the first three joint variable values as the motion control parameters corresponding to the posture singularity position of the industrial robot to realize the motion control of the industrial robot, to This makes the six-joint industrial robot pass through the pose singularity smoothly. 6. The six-joint industrial robot as claimed in claim 5 passes through the control system of the attitude singularity, wherein the total number of interpolation points N when the interpolation module performs interpolation is determined in the following manner: Among them: x ₁ , y ₁ , z ₁ are the position values of the starting point of the track, x ₂ , y ₂ , z ₂ are the position values of the end point of the track, V is the interpolation speed, and T is the interpolation cycle. 7. The control system for a six-joint industrial robot passing a singular point of posture according to claim 6, wherein the interpolation speed is 10 mm/s, and the interpolation period is 1 ms. 8. The six-joint industrial robot as claimed in claim 5 passes through the control system of the attitude singularity, wherein the control system also includes a judging module for establishing a link coordinate system according to the six-joint industrial robot, and predicting The kinematics inverse solution is performed on the planned path to obtain the variable values of each joint of the industrial robot, and then it is judged whether the six-joint industrial robot is in the pose singularity.