CN102566593A - Central-axis traversing method for inverse solution of displacement of joints of joint type industrial robot with six degrees of freedom - Google Patents

Abstract

The invention provides an inverse solution method of an industrial robot with six degrees of freedom, namely a central-axis traversing method, which is as shown in the attached drawing of the abstract. Central-axis traversing is realized in that a tail-end actuator and a joint L are respectively positioned on two sides of a joint S during the motion process of the industrial robot with the six degrees of freedom. An algorithm is applied for solving the solution problem of angular displacement values of all the joints during the motion of the industrial robot with the six degrees of freedom in a working space. Inverse solution is of a sequential solution method, namely the solution of the angular displacement values is performed according to the sequence of S-L-U-B-R-T, so that the solution of the angular displacement value of a joint S is the key of the whole inverse solution algorithm. When central-axis traversing occurs, a position parameter of the tail-end actuator is utilized for working out the difference phi between the angular displacement value of the joint S and the actual value, so that whether or not a central-axis traversing condition is of a central task of the inverse solution algorithm can be judged. The algorithm gives out a central-axis traversing discriminating formula, and the actual angular displacement value of the joint S can be determined by utilizing the formula, so that the calculation of the displacement values of other joints further becomes possible.

Description

Axis passes through method against separating the displacement of six degree of freedom joint type industrial robot joint

Technical field

The present invention relates to the trajectory planning algorithm of industrial robot control, especially to the mechanical component movement control of using the six degree of freedom industrial robot always and having same structure.

Background technology

Industrial robot is the key equipment of production run; Can be used for production links such as manufacturing, installation, detection, logistics; And being widely used in numerous industries such as vehicle complete vehicle and auto parts and components, Aeronautics and Astronautics, engineering machinery, IC equipment, military project, application is very extensive.Compare with manual work, adopt industrial robot not only can improve the quality of product, raise labour productivity; Save material and consume and reduce production costs; And, improve work situation to guaranteeing personal safety, reducing labor intensity also has crucial meaning.

Industrial robot controller is the robot core parts, is the key factor of decision robot function and performance.Trajectory planning is that motion path and track are converted into the control sequence that robot controller can be accepted, and is the core of robot controller.It is the committed step that solves the robot spatial pose that the related industrial robot motion of trajectory planning is learned contrary separating.Contrary the separating of kinematics is at known end effector---under the situation of wrist point (intersection point of joint R, B, T axis) spatial pose, solving joint variable, is to the robot necessary function that control effectively.Kinematics is contrary separates the problem that has multiple solution, and control system can only select wherein one group to separate, and how selecting to separate is the key of system's control.Six degree of freedom industrial robot end effector as shown in Figure 1 when cartesian coordinate space is moved, each position of end effector corresponding eight groups of joint variables, but can only confirm that in fact finally a class value separates as the contrary of robot.

Summary of the invention

The purpose of this invention is to provide that a kind of six degree of freedom industrial robot with architectural feature as shown in Figure 1 is contrary to resolve that principle---axis passes through method, to confirm the joint position of six degree of freedom industrial robot.The characteristics of this method are for any given pose among the robot working space, through judging whether wrist point passes through the axis and confirm one group of joint variable.

This algorithm is applicable to the problem of resolving of each joint variable when six degree of freedom joint type industrial robot as shown in Figure 1 moves in work space, the industrial robot structure that is suitable for has following characteristics: the axis coplane of joint S, R, T; The axes intersect of joint R, B, T is in a bit; Joint L, U are parallel to each other and perpendicular to the determined plane of the axis of joint S, R, T.

As shown in Figure 1 six from spending the joint type industrial robot when in work space, moving, and the position of robot and attitude can be divided into two types: the first kind is shown in Fig. 2-1, and end effector is inboard in the axis, representes that axis has taken place to be passed through; Second type is shown in Fig. 2-2, and end effector is in the outside, axis, and expression axis does not take place passes through.Whether the contrary key of separating selection of six degree of freedom industrial robot is exactly to judge that axis passes through to take place, and then resolves and select six joint variables.

The advantage of this algorithm is:

1. applied widely.All robots with structure as shown in Figure 1 can use this against separating algorithm position in the work space and attitude to be resolved.

2. algorithm structure is simple.Because this algorithm can be divided into the inboard and outside, axis, axis with robot according to the position of end effector of robot in the position of work space, as long as know the position of end effector, can confirm joint variable one by one.

Description of drawings

Fig. 1 axis passes through six degree of freedom joint type industrial robot structure, joint coordinate system, the joint variable define method that method is suitable for.

The industrial robot structure that this algorithm was suitable for has following characteristics: have six joint freedom degrees; Three rotary freedom concurrents of wrist, promptly the axes intersect of joint R, B, T is in a bit---the wrist point.

Definition coordinate system C ₀Be reference frame, C ₁～C ₆Be joint coordinate system, θ ₁～θ ₆Be respectively the joint variable of joint S, L, U, R, B, T; Variable θ as shown in Figure 1 ₁～θ ₆Value be respectively 0 ,-pi/2,0,0,0,0.

Fig. 2 end effector is positioned at the define method that medial and lateral, axis and axis pass through.

The axis of definition joint S is the axis; Joint L coordinate system C ₁True origin and joint R coordinate system C ₄(perhaps joint B coordinate system C ₅, joint T coordinate system C ₆) true origin be that end effector is referred to as axis in the inboard of axis and passes through when being positioned at the both sides of axis, otherwise axis passes through for the outside is referred to as not.

The diverse location of pairing joint L of Fig. 3 wrist point same position and joint U.

Embodiment

Step 1: calculate the homogeneous transformation matrix between joint coordinates.

$T_1^0=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_1& -\sin {\mathrm{\θ}}_1& 0& 0\\ \sin {\mathrm{\θ}}_1& \cos {\mathrm{\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ $T_2^1=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_2& -\sin {\mathrm{\θ}}_2& 0& a_1\\ 0& 0& -1& 0\\ \sin {\mathrm{\θ}}_2& \cos {\mathrm{\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right];$

$T_3^2=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_3& -\sin {\mathrm{\θ}}_3& 0& a_2\\ \sin {\mathrm{\θ}}_3& \cos {\mathrm{\θ}}_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ $T_4^3=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_4& -\sin {\mathrm{\θ}}_4& 0& a_3\\ 0& 0& 1& d_4\\ -\sin {\mathrm{\θ}}_4& -\cos {\mathrm{\θ}}_4& 0& 0\\ 0& 0& 0& 1\end{array}\right];$

$T_5^4=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_5& -\sin {\mathrm{\θ}}_5& 0& 0\\ 0& 0& -1& 0\\ \sin {\mathrm{\θ}}_5& \cos {\mathrm{\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right];$ $T_6^5=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_6& -\sin {\mathrm{\θ}}_6& 0& 0\\ 0& 0& 1& 0\\ \sin {\mathrm{\θ}}_6& \cos {\mathrm{\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right];$

If the locus of end effector and attitude matrix do

$P=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$

So

⁰T ₁ ¹T ₂ ²T ₃ ³T ₄ ⁴T ₅ ⁵T ₆＝P (1)

Set up.

Step 2: the variable θ that asks joint S ₁

The axes intersect of six degree of freedom joint type industrial robot joint R shown in Figure 1, B, T is in a bit, and the position of wrist point projects to the x in the fixedly cartesian coordinate system shown in Figure 1 ₀C ₀y ₀On the plane be on the straight line of crossing initial point a bit.This straight line is exactly that the plane at joint S, R, T place is at x ₀C ₀y ₀Projection on the plane.Therefore the location parameter according to wrist point obtains joint S shift value θ ₁Tangent value be:

$\tan {\mathrm{\θ}}_1=\frac{\sin {\mathrm{\θ}}_1}{\cos {\mathrm{\θ}}_1}=\frac{p_y}{p_x}$

If

$(0,a_1)\·(\sqrt{p_x^2+p_y^2},p_z)\≥0$

Axis does not then take place pass through, then θ ₁For:

θ ₁＝Atan2p _y，p _x

Otherwise

θ ₁＝Atan2-p _y，-p _x

Step 3: the variable θ that asks joint L ₂

As shown in Figure 3, joint L and joint U that the same position of wrist point can corresponding two kinds of different structure positions, joint S, U and wrist point have been formed the rotational symmetry triangle about joint L and wrist point line.Formula (1) both sides are premultiplication simultaneously

Obtain the wrist point at coordinate system C ₁In the pose matrix

Utilize the cosine law can obtain Δ θ ₂

$\mathrm{\Δ}{\mathrm{\θ}}_2=\arccos \frac{{a_2}^2+{(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2-({a_3}^2+{d_4}^2)}{2a_2\sqrt{{(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2}}$

θ then ₂For,

θ ₂＝±Δθ ₂+tan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)

Confirm Δ θ according to the minimum principle of change in displacement ₂Symbol.The minimum principle (nearby principle) of change in displacement: { θ _i, θ ' _iRepresent joint of robot to begin and end position respectively, Δ θ _i=| θ _i-θ ' _i| represent displacement variable, can make Δ θ _iChange minimum θ _iBeing institute asks;

Step 4: ask joint U variable θ ₃

In like manner, utilize the triangle of step 2 and the cosine law to calculate Δ θ ₃

$\mathrm{\Δ}{\mathrm{\θ}}_3=\arccos \frac{{a_2}^2+({a_3}^2+{d_4}^2)-({(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2)}{2a_2\sqrt{{a_3}^2+{d_4}^2}}$

If

θ ₂＝Atan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)-Δθ ₂

Then

${\mathrm{\θ}}_3=\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3-A\tan \frac{d_4}{a_3}$

Otherwise

θ ₂＝Atan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)+Δθ ₂

So

${\mathrm{\θ}}_3=-(\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3+A\tan \frac{d_4}{a_3})$

Step 5: the variate-value θ that confirms joint B ₅

Formula (1) both sides premultiplication

simultaneously obtain

θ ₅=± arccos (a _xCos θ ₁Sin (θ ₂+ θ ₃)-a _ySin θ ₁Sin (θ ₂+ θ ₃)+a _zCos (θ ₂+ θ ₃)) confirm θ according to the minimum principle of change in displacement ₅Symbol.

Step 6: the variate-value θ that confirms joint T ₆

Formula (1) both sides premultiplication simultaneously obtain

${\mathrm{\θ}}_6=\±\arccos \left(\frac{-n_x\cos {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)-n_y\sin {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)+n_z\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)}{\sin {\mathrm{\θ}}_5}\right)$

Confirm θ according to the minimum principle of change in displacement ₆Symbol.

Step 7: the variate-value θ that confirms joint R ₄

Formula (1) both sides premultiplication

simultaneously obtain

${\mathrm{\θ}}_4=\±\arccos \left(\frac{-n_x\cos {\mathrm{\θ}}_1\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)-n_y\cos {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)+n_z\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)}{\sin {\mathrm{\θ}}_5}\right)$

Confirm θ according to the minimum principle of change in displacement ₄Symbol.

Claims (1)

1. a 6DOF joint type industrial robot is against resolving scheme;

It is characterized by: fixing or mobile six degree of freedom joint type industrial robot; Through judging whether wrist point (intersection point of joint R, B, T axis) passes through axis (joint S axis); Confirm the variate-value of joint S, and then obtain the variate-value of joint L, U, B, R and T; End effector is at reference frame C ₀In position and the attitude parameter variate-value θ that calculates joint S ₁And then order computation goes out the variate-value θ of joint L, U, B, R or T ₂, θ ₃, θ ₅, θ ₄Or θ ₆

Provide example below: the long a of being of bar that supposes connecting rod 1 ₁, end effector is at reference frame C ₀In location parameter be p (p _x, p _y, p _z),

$(0,a_1)\·(\sqrt{p_x^2+p_y^2},p_z)\≥0---\left(1\right)$

If formula (1) is set up,

θ ₁＝Atan2p _y，p _x (2)

Otherwise,

θ ₁＝Atan2-p _y，-p _x (3)

Utilization formula (2) or (3) gained θ ₁And the location parameter of end effector, obtain formula

θ ₂＝Atan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)±Δθ ₂ (4)

Calculate the value of angular displacement θ of joint L ₂, wherein,

$\mathrm{\Δ}{\mathrm{\θ}}_2=\arccos \frac{{a_2}^2+{(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2-({a_3}^2+{d_4}^2)}{2a_2\sqrt{{(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2}}---\left(5\right)$

θ ₂Symbol select according to nearby principle;

Utilization formula (2) or (3) gained θ ₁And the location parameter of wrist point, obtain formula:

${\mathrm{\θ}}_3=\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3-A\mathrm{Tan}\frac{d_4}{a_3}$ Or ${\mathrm{\θ}}_3=-(\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3+A\mathrm{Tan}\frac{d_4}{a_3})---\left(6\right)$

Calculate the value of angular displacement θ of joint U ₃, wherein,

$\mathrm{\Δ}{\mathrm{\θ}}_3=\arccos \frac{{a_2}^2+({a_3}^2+{d_4}^2)-({(p_x\cos {\mathrm{\θ}}_1+p_y\sin {\mathrm{\θ}}_1-a_1)}^2+{p_z}^2)}{2a_2\sqrt{{a_3}^2+{d_4}^2}}---\left(7\right)$

θ ₃Selection according to Δ θ ₂Symbol, if

θ ₂＝Atan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)+Δθ ₂ (8)

So

${\mathrm{\θ}}_3=\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3-A\tan \frac{d_4}{a_3}---\left(9\right)$

If

θ ₂＝Atan2(p _xcosθ ₁+p _ysinθ ₁-a ₁，p _z)-Δθ ₂ (10)

So

${\mathrm{\θ}}_3=-(\mathrm{\π}-\mathrm{\Δ}{\mathrm{\θ}}_3+A\tan \frac{d_4}{a_3})---\left(11\right)$

Utilize θ ₁, θ ₂, θ ₃And the location parameter of wrist point, obtain formula

θ ₅＝±arccos(-a _xcosθ ₁sin(θ ₂+θ ₃)-a _ysinθ ₁sin(θ ₂+θ ₃)+a _zcos(θ ₂+θ ₃)) (12)

θ ₅Symbol select according to nearby principle;

Utilize θ ₁, θ ₂, θ ₃, θ ₅And the attitude parameter of wrist point, obtain formula

${\mathrm{\θ}}_6=\±\arccos \left(\frac{-n_x\cos {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)-n_y\sin {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)+n_z\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)}{\sin {\mathrm{\θ}}_5}\right)---\left(13\right)$

θ ₆Symbol select according to nearby principle;

Utilize θ ₁, θ ₂, θ ₃, θ ₅And the attitude parameter of wrist point, obtain formula

${\mathrm{\θ}}_4=\±\arccos \left(\frac{-n_x\cos {\mathrm{\θ}}_1\cos ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)-n_y\cos {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)+n_z\sin ({\mathrm{\θ}}_2+{\mathrm{\θ}}_3)}{\sin {\mathrm{\θ}}_5}\right)---\left(14\right)$

θ ₄Symbol select according to nearby principle.

Images