CN107756400B - A geometric solution method for inverse kinematics of 6R robot based on screw theory - Google Patents

Abstract

The invention discloses a 6R robot inverse kinematics geometric solution method based on screw theory, and belongs to the research field of robot kinematics inverse solution methods. The base coordinate system and the tool coordinate system are established, the kinematic parameters of the 6R robot are determined through the base coordinate system and the tool coordinate system, and the positive kinematics model is established. The inverse kinematics of the first three joints of the 6R robot are decomposed and described and a quadratic system of six elements is established. The target position q _e1 corresponding to the initial position q _s1 is solved based on the screw positive kinematics model. The method combines geometric description and screw theory, and the geometric meaning is more clear. By simplifying the inverse solution algorithm to solve the algebraic equations, the calculation efficiency is effectively improved, and a new inverse solution processing method can be provided for the real-time control of robot motion.

Description

A geometric solution method for inverse kinematics of 6R robot based on screw theory

technical field

The invention belongs to the research field of inverse kinematics solution methods of robots, in particular to a geometric solution method of inverse kinematics of 6R robots based on screw theory.

Background technique

Kinematics analysis is the basis for realizing motion control, which mainly establishes the mapping model of joint variables and end poses. Among them, the inverse solution problem, that is, the problem of solving joint variables with known end poses, is one of the research hotspots in the field of robotics, and its solution efficiency directly affects the real-time performance of robot motion control. At present, the inverse solution modeling of robot kinematics is mainly based on the D-H method and the screw theory. The researchers compared the two methods and found that the application of the latter has the following advantages: it can avoid the establishment of a local coordinate system, simplify the calculation model and overcome the problems caused by local parameters. Singularity; its geometric meaning is clear, and the conditions and number of multiple solutions can be easily determined. At present, the Paden-Kahan subproblem based on the spinor description is widely used in the reverse solution of robots, but this method is not suitable for any configuration of six-degree-of-freedom robots. Therefore, most scholars have extended and proposed based on three basic subproblems. Some new subproblem models, such as Tan improved subproblem two and established a mathematical model for the subproblem "rotational motion about two disjoint axes"; Chen described a "rotational motion about three axes, The sub-problem where the two axes are parallel and out of plane with the third axis is described in detail for its inverse solution. Based on these sub-problems, the model has been widely used in the inverse solution of the 6-DOF series robot. The research shows that the inverse kinematics problem of the 6R robot has a closed solution when it satisfies the Piper criterion. Sariyildi et al. realized the inverse kinematics solution of the 6R robot based on three sub-problems; Lv Shizeng et al. reduced the dependence on the sub-problems by introducing the Wu method into the screw method, and transformed the inverse solution problem of the 6R robot into a six-element eight-fold The problem of solving the system of equations. In order to further simplify the inverse solution model of the 6R robot, this method transforms the inverse solution problem of the 6R robot into the solution of a six-element quadratic equation system and a three-element quadratic equation system through the geometric description method, so that its geometric meaning is clearer. The solution process is simpler.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a geometric solution method of inverse kinematics of 6R robot based on screw theory. The main feature of this method is to propose an inverse kinematics model with clear geometric meaning and simple solution process for 6R robot kinematics inverse solution problem by combining geometric description method and screw theory.

In order to achieve the above object, the technical means adopted in the present invention is a 6R robot inverse kinematics geometric solution method based on the screw theory, and the realization process of the method is as follows:

S1. Establish a base coordinate system and a tool coordinate system, determine the kinematic parameters of the 6R robot through the base coordinate system and the tool coordinate system, and establish a positive kinematics model.

S2. As shown in Figure 1, the inverse solution motion of the first three joints of the 6R robot is decomposed and described: from point q _e to point c ₂ =[x ₂ y ₂ z ₂ ] is the rotational motion around joint 1; point c ₂ to point c ₁ =[x ₁ y ₁ z ₁ ] is the rotational movement around joint 2; the rotational movement from point c ₁ to point q _s around joint 3. Among them, q _s and q _e are the initial and target positions of the robot end, respectively. According to the description of the geometric relationship shown in Figure 1, a six-element binary system with x ₁ , y ₁ , z ₁ , x ₂ , y ₂ and z ₂ as variables is established. secondary equations.

S3. Use the "SOLVE" function in MATLAB to solve the above equation system, and obtain the coordinate vectors of c ₁ and c ₂ .

S4. Since then, the starting and ending positions of the inverse motion of the first three joints are known. Based on the Paden-Kahan sub-problem 1, the explicit solution models of the first three joint angle variables θ ₁ , θ ₂ and θ ₃ of the 6R robot are established respectively. and solve.

S5. Take a point on the axis of the joint 6 as the initial position q _s1 in the inverse solution motion of the last three joints, and solve the target position q _e1 corresponding to the initial position q _s1 based on the positive screw kinematics model.

S6. As shown in Figure 2, the inverse solution motion of joints 4 and 5 is decomposed and described: the rotational motion from point q _e1 to point c ₃ =[x ₃ y ₃ z ₃ ]; the rotational motion from point c ₃ to point q _s1 . According to Fig. 2, a ternary quadratic equation system with x ₃ , y ₃ and z ₃ as variables is established according to the geometric relationship, and the "SOLVE" function is used to solve it, and the coordinate vector of c ₃ is obtained.

S7. Since then, the starting and ending positions of the inverse solution motion of joint 4 and joint 5 are known. Based on Paden-Kahan sub-problem 1, the joint angle variables θ ₄ and θ ₅ of joint 4 and joint 5 are established to solve the model explicitly, and the solution is carried out. .

S8. Take any point q _s2 that is not on the axis of the joint 6, and solve the target position q _e2 corresponding to q _s2 based on the positive screw kinematics model, then the starting and ending positions of the inverse motion of the joint 6 are known. Similarly, θ ₆ is solved based on sub-problem 1.

The invention is characterized in that the kinematic inverse solution problem of the 6R robot is transformed into the solution of a six-element quadratic equation system and a three-element quadratic equation system based on the geometric description method, the inverse solution model is simplified, and the geometric meaning is more clear, It provides certain method support for the real-time motion control of 6R robot. The method combines geometric description and screw theory, and the geometric meaning is more clear. By simplifying the inverse solution algorithm to solve the algebraic equations, the calculation efficiency is effectively improved, and a new inverse solution processing method can be provided for the real-time control of robot motion.

Description of drawings

Figure 1 Geometric description of the inverse solution kinematics of joints 1, 2 and 3;

Fig. 2 Geometric description of the inverse solution kinematics of joints 4 and 5;

Figure 3 A 6R robot parameter coordinate system.

Detailed ways

The present invention will be described in detail below with reference to the accompanying drawings 1-3.

S1 determines the kinematic parameters of the 6R robot and establishes a positive kinematics model

As shown in Figure 3, it is known that the position vector and rotation vector of each joint in the initial state of the 6R robot are as follows:

where r _i , 1≤i≤6 represents the position vector of the i joint in the base coordinate system, and ω _i represents the rotation vector of the i joint.

Based on the screw theory, the forward kinematics model of the 6R robot is expressed as,

表示i关节旋转运动的指数积形式，where g _st (θ), g _st (0) represent the initial pose and target pose of the robot end, respectively,

represents the exponential product form of the rotational motion of the i joint,

是i关节旋转矢量ω_i的另一种表示形式，由ω_i＝[ω₁ ω₂ ω₃]定义为

则

ν_i是i关节运动的旋转线速度，ν_i＝-ω_i×r_i。where θ _i is the angular displacement of the i-th joint;

is another representation of the i-joint rotation vector ω _i , defined by ω _i =[ω ₁ ω ₂ ω ₃ ] as

but

ν _i is the rotational linear velocity of the i joint motion, ν _i =-ω _i × _ri .

Given the target pose,

S2 establishes a six-element quadratic equation system for the first three joints

The rotational motion of the first three joints based on the screw theory is described as,

where q′ _s and q′ _e represent the initial position and target position of the 6R robot end in the rotational motion, respectively. It can be seen from S1 that

q' _s = [x _s y _s z _s 1] = [0 744 940 1]

q' _e = [x _e y _e z _e 1] = [-936.6611 631.7859 570.0752 1]

Assuming that the motion passes through points c ₁ and c ₂ , the following relational expressions are established according to the geometric description shown in Fig. 1,

where q ₁ , q ₂ and q ₃ are any points on the rotation axis of joint 1, joint 2 and joint 3, respectively. For the simplified model, q ₁ =[0 0 0], q ₂ =[0 150 250] and q ₃ =[0 150 800], then formula (7) is expressed as the following equation system,

Using the "SOLVE" function in MATLAB to solve the equation system (8), the solutions of x ₁ , y ₁ , z ₁ , x ₂ , y ₂ and z ₂ are obtained.

S3 calculates joint angular displacements θ ₁ , θ ₂ and θ ₃

After obtaining the process point coordinates c ₁ =[x ₁ y ₁ z ₁ ] and c ₂ =[x ₂ y ₂ z ₂ ], the rotational motions around joint 1, joint 2 and joint 3 are described as follows,

Based on the Paden-Kahan subproblem 1, the display expression of the joint angular displacement is obtained as follows,

Step (4) Establish a ternary quadratic equation system for joint 4 and joint 5

From formula (3), we know,

在关节6的轴线上取一点q_s1＝[x_s1 y_s1 z_s1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，in

Take a point q _s1 =[x _s1 y _s1 z _s1 ]=[0 744 0] on the axis of the joint 6, the rotational motion around the joint 4 and the joint 5 is described as follows,

where q' _e1 =g ₁ q' _s1 =[x _e1 y _e1 z _e1 1].

Assuming that the coordinates of the moving passing point are c ₃ =[x ₃ y ₃ z ₃ ], the following relational expressions are established according to the geometric description shown in Figure 2,

where q ₄ =[0 744 940]. Then formula (14) is expressed as the following equation,

Using the "SOLVE" function in MATLAB to solve the equation system (15), the solutions of x ₃ , y ₃ and z ₃ are obtained.

Step (5) Calculate the joint angular displacements θ ₄ and θ ₅

After obtaining the process point coordinates c ₃ =[x ₃ y ₃ z ₃ ], the rotational motions around joint 4 and joint 5 are described as follows,

Based on the Paden-Kahan subproblem 1, the display expressions for the joint angular displacements θ ₄ and θ ₅ are obtained as follows,

Step (6) Calculate the joint angular displacement θ ₆

Taking a point q _s2 not on the axis of joint 6 =[0 750 940], its rotational motion around joint 6 is described as,

同理基于Paden-Kahan子问题1，得到关节角位移θ₆的显示表达式如下，in

Similarly, based on the Paden-Kahan subproblem 1, the display expression of the joint angular displacement θ ₆ is obtained as follows,

Through the above solutions, eight groups of kinematic inverse solutions are obtained as shown in Table 1.

Table 1 Eight groups of kinematic inverse solutions

Claims (2)

1. A6R robot inverse kinematics geometric solving method based on a momentum theory is characterized by comprising the following steps: the method comprises the following implementation processes:

s1, establishing a base coordinate system and a tool coordinate system, determining kinematic parameters of the 6R robot through the base coordinate system and the tool coordinate system, and establishing a positive kinematic model;

s2, decomposing and describing inverse solution motions of the first three joints of the 6R robot: from point q_eTo point c₂＝[x₂ y₂ z₂]A rotational movement around the joint 1; point c₂To point c₁＝[x₁ y₁ z₁]A rotational movement around the joint 2; point c₁To point q_sA rotational movement about the joint 3; wherein q is_s，q_eRespectively establishing x-based initial and target positions of the tail ends of the front three joints of the robot according to the description of the geometric relationship₁,y₁,z₁,x₂,y₂And z₂Is a six-membered quadratic system of variables;

s3, solving the equation set by adopting a 'SOLVE' function in MATLAB to obtain c₁And c₂The coordinate vector of (2);

s4, knowing the inverse solution motion starting and ending positions of the first three joints, and respectively establishing the first three joint angle variables theta of the 6R robot based on the Pasen-Kahan subproblem 1₁，θ₂And theta₃The explicit solution model of (2) and solving;

s5, taking one point on the axis of the joint 6 as the initial position q in the inverse solution movement of the last three joints_s1And solving the initial position q based on the momentum positive kinematics model_s1Corresponding target position q_e1；

S6, describing the inverse solution motion decomposition of joints 4 and 5: from point q_e1To point c₃＝[x₃ y₃ z₃]The rotational movement of (a); from point c₃To point q_s1The rotational movement of (a); establishing the relation x according to the geometric relation₃,y₃And z₃As a ternary quadratic equation of a variableGroup, and solving by using a 'SOLVE' function to obtain c₃The coordinate vector of (2);

s7, the inverse solution motion starting and ending positions of the joint 4 and the joint 5 are known, and joint angle variables theta of the joint 4 and the joint 5 are respectively established based on the Pasen-Kahan subproblem 1₄And theta₅Explicitly solving the model and solving;

s8, taking any point q not on the axis of the joint 6_s2Solving q based on a momentum positive kinematics model_s2Corresponding target position q_e2If so, the starting and ending positions of the inverse solution movement of the joint 6 are known; solving theta based on subproblem 1 in the same way₆。

2. The inverse kinematics geometric solution method of the 6R robot based on the momentum theory as claimed in claim 1, wherein: s1 determining kinematic parameters of 6R robot and establishing positive kinematic model

The position vector and the rotation vector of each joint in the initial state of the 6R robot are known as follows:

wherein r is_iI is more than or equal to 1 and less than or equal to 6, and represents the position vector of the i joint in the base coordinate system, omega_iA rotation vector representing the i-joint;

based on the rotation theory, the positive kinematics model of the 6R robot is expressed as,

wherein g is_st(0),g_st(theta) respectively representing an initial pose and a target pose of the robot end,

representing the form of the exponential product of the i-joint rotational motion,

in the formula [ theta ]_iIs the ith joint angular displacement;

is the i joint rotation vector omega_iBy ω_i＝[ω₁ω₂ ω₃]Is defined as

Then

ν_iIs the rotational linear velocity of the i-joint motion, v_i＝-ω_i×r_i；

Given the pose of the object(s),

s2 six-membered quadratic equation set established for the first three joints

The rotational motion of the first three joints is described based on the momentum theory as,

q 'in the formula'_sAnd q'_eRespectively representing the initial position and the target position of the 6R robot tail end in the rotary motion; as can be seen from S1, in this case,

q′_s＝[x_s y_s z_s 1]＝[0 744 940 1]

q′_e＝[x_e y_e z_e 1]＝[-936.6611 631.7859 570.0752 1]

let the movement pass through point c₁And c₂The following set of relationships is established from the geometric description,

wherein q is₁，q₂And q is₃At any point on the axis of rotation of joint 1, joint 2 and joint 3, respectively, and q is taken for simplifying the model₁＝[0 0 0]，q₂＝[0 150 250]And q is₃＝[0 150 800]Then, equation (7) is expressed as the following equation system,

solving an equation set (8) by adopting a 'SOLVE' function in MATLAB to obtain x₁,y₁,z₁,x₂,y₂And z₂The solution of (1);

s3 calculating angular displacement theta of joint₁，θ₂And theta₃

Obtain process point coordinates c₁＝[x₁ y₁ z₁]And c₂＝[x₂ y₂ z₂]Hereinafter, the rotational movements about the joint 1, the joint 2 and the joint 3 will be described respectively as follows,

based on the Pasen-Kahan subproblem 1, the display expression of the angular displacement of the joint is obtained as follows,

s4 sets up a system of ternary quadratic equations for joint 4 and joint 5

As can be seen from the formula (3),

wherein

Taking a point q on the axis of the joint 6_s1＝[x_s1 y_s1 z_s1]＝[0 744 0]The rotational movement thereof about the joints 4 and 5 is described below,

wherein q'_e1＝g₁q′_s1＝[x_e1 y_e1 z_e1 1]；

Let the coordinate of the passing point of motion be c₃＝[x₃ y₃ z₃]The following set of relationships is established from the geometric description,

wherein q is₄＝[0 744 940](ii) a Equation (14) is expressed as the following equation,

solving the equation set (15) by using a 'SOLVE' function in MATLAB to obtain x₃,y₃And z₃The solution of (1);

s5 calculating angular displacement theta of joint₄And theta₅

Obtain process point coordinates c₃＝[x₃ y₃ z₃]Hereinafter, the rotational movements about the joints 4 and 5 will be described respectively as follows,

obtaining angular displacement theta of joint based on problem 1 of Paden-Kahan son₄And theta₅The expression of (a) is as follows,

s6 calculating angular displacement theta of joint₆

Take a point q not on the axis of the joint 6_s2＝[0 750 940]The rotational movement of which about the joint 6 is described as,

wherein

Obtaining the angular displacement theta of the joint based on the problem 1 of the Paden-Kahan in the same way₆The expression of (a) is as follows,

eight sets of inverse kinematics solutions are obtained by the above solution.

Images