JPH048490A - Articular angle measuring method for multiarticulated type manipulator - Google Patents

Abstract

PURPOSE:To simplify a linear optimization technique and an induction procedure for a coupling solution by regulating an equation of motion, inclusive of multiple variables among plural pieces of articulations, into a system consisting of only multiple independent variables, while performing such a repetitive calculation as bringing a distance between a target point and an existing point into minimum or zero, compensating this repetitive range each time, and determining the degree of articulation. CONSTITUTION:After a six-variable system is replaced with an evaluation function being composed of a three-variable of deviation sum-squares, each displacement of articulations 21-26 is determined as a solution of simultaneous equations by such optimum operation that has this function linearly approximated. In calculation of this level, however, since linearization is partially applied so far, a search range of the solution from the initial estimate is limited to a very minute range. Accordingly, with a repetitive calculation each time, this repetitive range (displacement of an articular angle) is directly compensated and ameliorated, thus it is stably solvable even against the large displacement from the initial estimate, unattainable in calculation based on an ordinary linear theory.

Description

DETAILED DESCRIPTION OF THE INVENTION (Field of Industrial Application) The present invention accurately determines the amount of rotation of each joint required when the fingertip of a six-joint manipulator is moved along a specified trajectory in a workspace. The present invention also relates to an improvement in a method for measuring joint angles of a multi-joint manipulator that can be quickly measured.

(Prior Art) In order to direct the tip of the manipulator in any direction at any position in three-dimensional space, the manipulator must have at least six degrees of freedom. That is, three independent variables are specified for position and three independent variables are specified for direction. The six-jointed manipulator, which is composed of six joints, is the most frequently used, and the independent movement of each joint allows the fingertip to reach a desired position and posture. There exists a nonlinear functional relationship defined by the kinematic equation 2 between the independent movements of each joint and the position and posture of the fingertips. Once each joint angle is given, the position and posture of the manipulator's fingertips can be easily derived using the concept of a coordinate transformation matrix. (This is called a forward problem.) On the other hand, determining the joint angles that realize a given position and posture of the manipulator is (
This is called an inverse problem. ) Corresponds to finding the roots of a system of nonlinear equations, and is often impossible to use for analytical display. Currently, the mainstream solution method is to linearize the kinematic equations near each joint angle and derive the joint angles from the Jacobian inverse matrix using a successive iterative method.

(Problem to be solved by the invention) However, in the conventional method as described above, first the Jacobian matrix (
Since the calculation (normally 6×6) has 36 matrix elements, the calculation becomes quite complicated, and a solution cannot be obtained in the case of so-called singularity occurrence where the determinant becomes zero. Furthermore, due to the basic nature of linearization, there is a problem in that unless the initial estimate for the exact solution is given with some accuracy, the number of iterations will increase and the credibility of the solution itself will become doubtful. In addition, even if the inverse matrix is calculated fairly efficiently, the calculation time becomes a large I\iff problem when calculating a matrix of order 6 or higher.

SUMMARY OF THE INVENTION An object of the present invention is to provide a method for measuring joint angles of a multi-joint manipulator that overcomes the drawbacks of the prior art as described above.

(Means for Solving the Problems) The present invention proposes a method of calculating an inverse problem for all types of six-jointed manipulators in which a rotation joint is used in the drive section. Some manipulators with six degrees of freedom have six joint variables θ, ~θ6, the first three variables determine the fingertip (wrist) position of the manipulator, and the remaining three joint variables determine the direction of the fingertip. There is something designed. This means a mechanism that allows the position and posture of the wrist to be determined independently. Compared to a mechanism that determines the fingertip position and posture using six joint variables that interact in a complex manner, not only the structure but also the operation for guiding the joint group is extremely simple. However, in reality, this type of mechanism is rare, and this leads to difficulties in calculating the manipulator inverse problem.

In the present invention, in contrast to the general structure in which these six joints cannot be simply separated into a position determining mechanism and a posture determining mechanism,
For convenience in numerical solution, we focus on only three independent variables among the six joint variables and rearrange them into three relational expressions. Compared to the Jacob method, which handles six variables at the same time, this relational expression described by three joint variables is much easier to handle. For this relational expression, the joint group is derived by minimizing the deviation between the calculated value and the actual value and bringing it to zero. In other words, after replacing the six-variable system with an evaluation function composed of the sum of squared deviations of three variables, this function is approximated by a linear approximation (optimal operation) to determine the joint displacement as a solution to the simultaneous equations. In the level calculation, since linearization is applied in part, the search range for the solution from the initial estimate is limited to an extremely small range.Therefore, in the present invention, this iterative width is (
By directly correcting and improving the displacement of joint angles, we have provided an effective method to obtain stable solutions based on ordinary linear theory (even for large displacements from the initial estimate that cannot be obtained by calculation. The published minimization method (see Reference 2) focuses on the root structure and is formulated as a four-variable system, but this method can be applied to a general six-joint manipulator as a model that can be processed with fewer variables. It has possible features.

(Example) An example in which the measurement method of the present invention is applied to a six-joint manipulator will be described in detail.

First, referring to FIG. 1, a six-jointed manipulator [0] to which the present invention is applied is shown. This manipulator is connected to the heath 10 and has six links]], 12,
]3, ]4, ]5.16 and six joints 21.22.23
．． 24, 25, and 26. These joints are rotatable or pivotable.

Next, we make the following conventions regarding the symbols used below.

Of the symbols written with double subscripts, for partial derivatives (a
FJ/aθk)-p, (aθ,/aθ,)-θ, a,
(θP, /θθ4)・PX. Abbreviated as, etc. others,
5H=sinθi, C1=CO5θi+sij"5ln
(θ, +θ,)ICi j=cO5(θ−θJ): n
? (n+++ny, nz)', O・(0,I□Oy+0
2) “a・(ax+ay, az)”; is a three-way identical unit vector (T is a transposition symbol) that determines the fingertip posture of the manipulator. :P, py, p, represents the position coordinates of the fingertip in the reference coordinate system.

Now, the kinematics of a general manipulator assuming six degrees of freedom is expressed by the following system of equations.

F8・Fi (θ, θ2.-1θ6) (1・L-, 6)
(1) Among these, 3 for 6 independent variables θ, ~θ6
Three equations F+, Fx, and Fs determine the posture of the fingertip, and the remaining three equations determine the position of the fingertip. Now, extract three variables from among the independent variables included in the three equations in (1), consider the remaining variables as dependent variables, and then relate these to the remaining three equations in (1) to calculate the minimum of the evaluation function. Determine the distance between joints from the value. In the summer planning process, the first. Second. Third joint variable 01. θ2. θ
, (hereinafter referred to as reference variables) are fixed, and the remaining joint variables are expressed by these three. That is, assuming that the left side of (1) is known, 32F, ,F,,F3 for the posture
Organize the implicit function 04. θ.

θ, θa=H1(θ1.θ2.θ,) θ5−Flz(θ1.θ2.θ3)
(2) θh=Hscθ8. θ2. θ3) An explicit display is performed. (Assume the existence of an implicit function.)
If this is related to FA, FS, and FA in (1), the fingertip position can be expressed using only the reference variables. The solution to the device equation composed of these three variables is derived by searching for the extreme value of the evaluation function described below, focusing on the deviation between the current position and the target value.

j7, (FJ(θ1.θ2.θ8.θ4.θ4.θ,
)-M)2=Σ (ΦP θ1.θ7.θ5)-FJ)
” (However, L is the target value at your fingertips.) First, (
3) The joints that minimize the sum of squared deviations, that is, zero, are the initial values θ, °2θ2 of the joint variables that are estimated to be sufficiently close to the fingertip position given each factor ΦJ (θ5.θ2.θ,). °
, θ, and ° are derived using linear approximation as follows.

FJ (θ1°+80l + '-"-θ, ゛+Δθh)
−FJ”=F j IΔθl”Fj2Δθz+FJff
Δθ, +F, n (θ, 1Δθ, 1θ4□Δθ2+θ
4. Δθ, ) 10FjS (θ9.Δθ1+θ, 2Δθ2
+θ2. Δθ3)+p=h (θ6.Δθ, +θ62Δ
θ2+θ6. Δθ, )−(F, ++FJnθ414Fj
SθSl+F'j6θ, 1) Δθ, +('jz+FJ4
θ 42+FjS θ52+FJ6 θ, 2) Δ
θ, +(Fiff+FJ4 θa3+F; 5 θS
ff+Fj6 θ8. ) Δ θ3= ΦJ(θ 1
°10Δ θ 1. θ 2° + Δ θ 2. θ, °+Δ
θ , )−ΦJ′″=Φ4.Δθ1+ΦJ2Δθ2+
Φ4. Δθ3 (j・4,5.6) (4) However, F, °・FJ(θ1°, −1θ6−−ΦJ(θ1°
, θ2°, θ, -2ΦJ (θ.)8Φ1° (j=4.5
．． 6) Therefore, Δθr (i=L2.3) that minimizes the sum of squared deviations between (4) and the target value is determined by repeated operations as a solution to the simultaneous equations, and as a result, the distance between the joints is determined.

The solution method for the manipulator shown in FIG. 1 will be explained below. This kinematic equation step 2 is described in (5) to o[i).

nx=-(s+5tyc5+5asscl+5Issc
tsca>cb+5b(c+ca-s+5acz-1)
(5)ny;(SZ:1clc&-3l54S
S+5SCI Cz3Ca) Ch+Sb (CrCa-
SaC+ C2:l) (6)nt-(Cz3Cs
-3z:+5sC4)eh-Szko54S6
(7)
Qx= (C1c4-sl S 4C23) Cb+s
b (s 1s23c5+54sscl +s l55
C23C4) (8)oy=(s+ca-sac+
cz3)cb-s*(sz3c+cs-s+54ss+
ssc+czzca) (9)OtH-(s
z3s4)C6+s6 (Sz3SsCm-Cz*C5
) 0o) als 1SzaS
s-5+CzsCnCs-3aC+Cs
(
II) ay=c+cz3c4cs-5z3SsC+-3
ISaCs 02) az
=-(SziC4C5+5sCz3)
Q3) Px=-aa(S+Sz+
Cs÷54S5C1+51SsCz3Ca)C6+ah
Sa(CrC4-5+5aCz3)as(S+5z3C
s+54Ss(++S+5sCzsC4)-(a:++
a4) S+Sz+-azs+sz (l Py
ni a-(Sz:+C+Cs-5ls4ss+S5C+C
z3Ca)C6+ahSa(S ICa+SaC+Cz
i) +as (SzzC+Cs-3IS4SS+5S
CI C23C4)+ (a3+am)St3CI+a
tStCPz=86 (Cz xC5-32355C4
) C6-a6s6 (Sz 3541as (C2cc
5- Sz 355C4)+(as+aa)czx+
azcz child a106) First, (If), 02) to 5
The common terms organized for 1+CI are described using reference variables as shown below.

C2ffC4C5-3Z3SS"ayCl-aXs l
”Wl (θ 11 o zl θ Ko) 0 force 5
aCs□-(ays++axc+)□wz(θ1.θ2
．． θ, )(・χ,) (18) Also, 03), 07)
From CaC5=H1Cts−azszz (”χz)
09) Sz・-axis χC2co+en IS2yu)
(・χ3) C! tD
is obtained, and the joint angle θ1. The relationship between θ and the reference variable can be derived. That is, side, 09), from 12G, however, ×2
"WlC23-axs!3≠O(22)stL/L Once established, X1 knee (ays+ +a, c+)≠0 can be found immediately. What remains is θ6, and as an expression including this, for example, the coefficient of 06+56 in (5) ^=S1S23C5+5aS5C1+S+5sCz3C
4(25)ToC+Ca-5+54Cz3(26). This ^, B can be calculated from the value of the joint angle found above, but when related to (8), A = -nxca + o
xsa (27)B=nxs
Since it can also be expressed as b+owcb (2B), it can be expressed as χ5-Box-8n, l≠O(30).

In this way, from the attitude relational expression, θ4. θ6. Since θ6 is expressed as a reference variable, next, according to (4), the fingertip positions P, , P, , P of the manipulator are set to the initial value θ. Linear approximation is performed in the vicinity of . That is, PX(θ.+Δ θ)=P, (θ.) 10L1Δ θ1+
L2Δ θ2+L3Δ θ3P, (θ.+Δ θ)−P
y(θ.) 10−4Δ θ1+N2Δ θ2+tΔ θ.

P, (θ.+Δθ)・P, (θ.)4N1Δθ112Δ
θ, +N: lΔθ3 (however, θ.-(θ1°, θ7°,
θ, °). ) Here, each coefficient of minute displacement is,
M,, N, is determined as follows, and (P, /θ
1) etc. use (35).

L, , -P, + Σ Pxkθ3□ (i・1.
2.3) (32) A, = P,, + Σ
Pykθ2, (l・1,2.3> N, = Pst + Σ Pffikθ□(i・1,2
．． 3) Pxl ・ PX3 ・ PX4 ”
)SpH2+ azs+cz G(d+, 7,0+0 to ksss+ass6+)G(d
:+, 9.0,0,0.0) G(R6,ox,0,0,0.0) Pyup,zc+/s+ (
35) Pyt -azc+c+ G (di, kbsb+ a6si+ S l54-
'1c23clO+ 0)G(+1+, ay, 0,0,
0. O) J・Pyh:G(R5,oy,O,0,0,0)Pl・
0 Ptt = -G(d*+ka+aish+54c
z:++d++1)Pzff -Pzz + a
zsgPta = G(d3+54ss+-ais
th+cn+0+0)SziPts --G(da,
ks, 0,0.0.0)P-a-G(a*+oz,
O+O+0,0) However, the function G is defined as follows.

G(a+b+c+d+e+f)□ab + cd +
ef (36) Above, linearization regarding the reference variable of the fingertip position of the manipulator was performed. Therefore, (31)
The deviation between each calculated value and the given fingertip target value is expressed as a square norm, and this value is updated until one convergence criterion is satisfied.Then the required joint solution is derived. The objective evaluation function is (P, (θ.+Δ θ)−P−]2+f P, (θ
. + Δ θ)-Pa12+f P, (θ .+ Δ
θ), which can be expanded as follows.

R2) 2 msJw (θo) - Pg + M = ml" + 2g
2 + m33 (39) M M M 5 is the estimated position of the fingertip.

Therefore, the minute displacement of each joint variable that minimizes the function J is to set the derivative of (38) to zero (J/Δθ,).

・O(i・1,2.3) is found as the solution of the following 03-dimensional simultaneous equations.

However, −2 R,,=G(L,,Ll,Ml,Ml,N,,N1
)Lx・G(Ll, Lz, M, Mz, N3.ti
z) = R21R+s = G(Ll, R3
, Ml, phylum 3. N1. Nz) ・R11hz =
G (Lz, LzlMz, gate 2 + N 2 +~z)
(41) Rzs = G(Lz
, Ll, Mz, gate, Nz, Ni) ・R32R3
3.G(R3,R3,M3.M3.N3.N5)b+
= -G(01zL1+mz+gate++m3+N
+)b2=-G(+m,,l,2+mZ+gate 21
m z + N Z )bz = −G(m++1
3 + face 2. The solution to the 3+1l13+Nl+) equation can be easily found using the Gauss-Seidel method or the sweep method. Each calculated increment value Δθ1. Δθ2. Due to Δθ, the initial estimated value θ8° of the joint angle is θ, = θ, e+Δθr (i・1.2.3)
(42), (21), (23
), the remaining joint angles are determined from (29). At this point, the magnitude of the norm is recalculated by substituting the pre-joint into the original equation (3) without linearization. If the size is within the specified convergence radius, it is accepted as the solution; otherwise,
Return to the beginning and repeat the series of operations using 01 in (42) as the initial value. The above is an overview of the calculation procedure. When the performance of this algorithm was evaluated through several numerical experiments, it was shown that if the distance between the fingertip position and the target value was within 0cm, convergence could be achieved within four iterations at most. Of course, this is an example of stable behavior, and the convergence situation may change depending on the initial value given to the arbitrary posture of the manipulator and the nonlinear characteristics of the evaluation function. Since the solution search method based on linear theory naturally has a defined range of application, it would be useful if it could be expanded and used.

Even with the current algorithm, if the initial displacement is large, it naturally leads to numerical instability and divergence. Therefore, we conducted an investigation to obtain stable solutions even for joint displacements that deviate from the linear region. In iterative calculations, the magnitude of Δθ in the first step holds the key to the subsequent behavior of the solution, so
We believed that it would be effective to suppress the relative size to some extent at this initial stage and make appropriate improvements and corrections. As the results show, after this operation, even if the value of Δθ is somewhat large, the minimization method works effectively and there is a tendency to quickly converge toward the target value without using the gap from the target value as an obstacle. Shown. In other words, the operation of applying a numerical brake on the tendency to diverge is significant as a breakthrough in improving the ability to search for solutions using conventional linear approximation. Specifically, calculations were performed by correcting joint displacement as follows.

1) When the initial displacement is within 300 (w) (i=1.2.
3) Δθi”Δθi /1.5 (Δθ, >(de
n)) 11) If it exceeds Δθ, 2 Δθ; /1.5 (10 (deg) > Δθ
(>1(deg)) Δθ, = Δθ*/2.0(30(
deg)>Δθr>10(deg))Δθ8−
Δθi/3.0 (Δθr >30 (deg)) The results are summarized in FIG. The leap ^,B is used as the initial estimate.
, C are three calculation categories in which one joint value is varied and the others are fixed at standard values, and are represented by X and mouth marks. The number above it indicates the number of convergence times.

For example, at point P, θ1 is 30 (deg) from the reference value,
Point Q sets θ2 to -20 (deg), and point R sets θ3 to -50
(deg) For the sample fluctuated, the convergence solution is 4 and 4.6, respectively, for the distance D from the fingertip target value shown on the vertical axis.
It represents what was obtained during the course. In all cases, the constraint frame of the linearized model is greatly improved compared to the shaded area established before correction. At the start of calculation, the gap from the target value is about 35
It was shown that if it is within 0, the deviations constituting each term of the evaluation function monotonically approach zero by this correction. Next, in a test calculation (see Table 1) in which each joint was changed simultaneously, a stable solution was obtained within 10 times. This model sufficiently withstood a considerably large angular displacement, and a normal solution was obtained when the distance between the fingertips was within 400 (am).

Note that the accuracy of the converged solution is 3 decimal places compared to the exact solution.
They matched up to the point.

As described above, a solution was successfully obtained through a series of treatments including minimization techniques, linear approximation, and correction operations. It was suggested that a normal solution could be reached as long as the magnitude of the initial displacement, which is a divergence factor, was directly controlled. As shown in the example, the accuracy is 0.1 m for a distance of 30 to 50 cm from the fingertip target value.
, it is considered that if it can be achieved within 10 iterations, it will be sufficiently useful for practical use.

(effect) The effects of the present invention are as follows.

(1) In order to avoid the complexity of handling six variables at once, the independent variables are formally reduced to three, associated with an evaluation function consisting of the squared deviation, and the solution derivation procedure is coupled with a linear optimization method. , is extremely simple.

(2) Even if the initial estimate exceeds the common sense of linearization,
By correcting Δθ, the extreme value search method can effectively contribute (without increasing the number of iterations) and a highly accurate solution can be expected.

(3) The concept is applicable in principle to all types of six-link manipulators.

(4) Processing is easier than Jacobian calculation, but it is necessary to keep in mind the conditions under which the implicit function does not hold.

It is an advantage that we were able to reach highly accurate solutions through optimization and displacement correction to solve similar problems experienced with the Jacobi method, such as initial value dependence, which is the fate of linearization, and handling in minute regions.

[Brief explanation of the drawing]

Figure 1 is a perspective view of a six-joint manipulator to which the present invention is applied, and Figure 2 is a calculation result using the method of the present invention, which shows that it is possible to solve large fluctuations by improving the repetition width of the linear model. This is a graph showing that it can be drawn out. 10- Heath, 11-16- Links 21-26- Joints.

Claims (1)

[Scope of Claims] 1. A method for measuring joint angles of a multi-joint manipulator having a plurality of rotatable or pivotable joints, wherein equations of motion including a plurality of variables between the plurality of joints are determined by a plurality of independent A joint of a multi-joint manipulator that organizes it into a system of only variables, performs repeated calculations to minimize the distance between the target point and the current point, and corrects the repetition width of the repeated calculations each time to determine the joint angle. Angle measurement method. 2. The measuring method according to claim 1, wherein there are six joints and three independent variables.