Scholarly article on topic 'Improved Inverse Kinematics Algorithm Using Screw Theory for a Six-DOF Robot Manipulator'

Improved Inverse Kinematics Algorithm Using Screw Theory for a Six-DOF Robot Manipulator Academic research paper on "Mechanical engineering"

Share paper
Academic journal
Int J Adv Robotic Sy
OECD Field of science

Academic research paper on topic "Improved Inverse Kinematics Algorithm Using Screw Theory for a Six-DOF Robot Manipulator"

International Journal of Advanced Robotic Systems


Improved Inverse Kinematics Algorithm Using Screw Theory for a Six-DOF Robot Manipulator

Regular Paper

Qingcheng Chen1*, Shiqiang Zhu1 and Xuequn Zhang1

1 Zhejiang University, Hangzhou, Zhejiang, China Corresponding author(s) E-mail:

Received 08 August 2014; Accepted 21 April 2015

DOI: 10.5772/60834

© 2015 Author(s). Licensee InTech. This is an open access article distributed under the terms of the Creative Commons Attribution License (, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.


Based on screw theory, a novel improved inverse-kinematics approach for a type of six-DOF serial robot, "Qianjiang I", is proposed in this paper. The common kinematics model of the robot is based on the Denavit-Hartenberg (D-H) notation method while its inverse kinematics has inefficient calculation and complicated solution, which cannot meet the demands of online real-time application. To solve this problem, this paper presents a new method to improve the efficiency of the inverse kinematics solution by introducing the screw theory. Unlike other methods, the proposed method only establishes two coordinates, namely the inertial coordinate and the tool coordinate; the screw motion of each link is carried out based on the inertial coordinate, ensuring definite geometric meaning. Furthermore, we adopt a new inverse kinematics algorithm, developing an improved sub-problem method along with Paden-Kahan sub-problems. This method has high efficiency and can be applied in real-time industrial operation. It is convenient to select the desired solutions directly from among multiple solutions by examining clear geometric meaning. Finally, the effectiveness and reliability performance of the new algorithm are analysed and verified in comparative experiments carried out on the six-DOF serial robot "Qianjiang I".

Keywords Inverse Kinematics, Screw Theory, Paden-Kahan Sub-problems, Serial Robot

Symbol List

CO angular velocity of rotation

V linear velocity of translation

z unit motion twist

e twist exponential form

CO skew symmetric matrix

qi intermediate point

gi configuration of rigid

u, v , z variable

reference point

a , ß , y coefficient

1. Introduction

The most popular method used in robot kinematics is based on the Denavit-Hartenberg (D-H) notation with the homogeneous transformation of points [1]. In this method, the kinematic formula of each joint is described by one

coordinate system in relation to another, resulting in some singularity problems. The inverse kinematics based on the D-H method has inefficient calculation and complicated solution, which will make real-time online operation less effective; this may not be suitable for real-time circumstances. As a traditional algorithm, the D-H-based inverse transformation method [1] is widely used for its intuition, but the inverse of each 4*4 homogeneous transformation matrix needs to be worked out, which results in a complex and time-consuming solution process. Thus, the effectiveness of the inverse-kinematics solution process directly affects the results of real-time online motion control [2,3], for example as in the inverse dynamics controller in [4] and the remote robot operation in [5].

In recent years, another method based on screw theory has been increasingly adopted in kinematics modelling both for serial robots and parallel robots [6-8]. A screw motion is represented as a linear motion along an axis with a rotation motion by an angle about the same axis in relation to the inertial frame [6]. In screw theory every transformation of rigid body with respect to the inertial coordinate system can be expressed by a screw displacement [9].

In [10] a comparative analysis between the screw theory method and the D-H method for kinematics is presented, which shows that the screw-based method has some striking advantages over the D-H method. There are two main advantages of using the screw method to describe rigidbody kinematics.Thefirst isthatit allows a global description of yfid body moyoa that docs nof snffdr from singularity preMemswithoutysmglycalcoordinates.K needs aefy toon cooe&nates, af wMsh one cs theinerti a. foame al: the bast send tfte other thu tool frame at tfte end-effoctors. ylljoint motionsaredcrcribed m tfiebtfe caordinare.yhe seaond tynaafokeor .hemethed ^tica.^ tan ^catiyeimpHfytirtan clysicofmechaniemsayosmda gtomcirin descrintiohoirigidmotien nndaasiiyOefine Che geemetrical significance of the inoerro klycmeiics rolu-ilono.Heecu, il tre asytoim]tlamenl itiemaoci^^yt oilhe sobel icngwiny nechjoint]з obvtaus nacme1^rli:m^ao^nr^

Besides the merits mentioned above, the screw theory methodalso can avoid a large amount of matrixes multi-plymgo^rntio^whkla canoesuitmfasf cakulationaed compart tormaliootions tompar ed to n-H note tiou meehoai Mil3], Wn'smuthonlr infrodticed tolhayrrcnsc oc inverieninemesiccbk comoimn° foai^ao.characteristic sel: wrt. the screw meyhc>cL In [141 sccew tiroory ns mtple-mecited tr anotysa theiimifetionзcOinprovldergebmelric intc^tatationof thcseycible wrenrtins ofcontinuum rokots.WlShcrrewtheorn totoivethelnveccekinamftics probiemwe gonrtallyyuducethe Sullinvoзccklnemchcs kroblem o^O a ceriel roCot into fome apneopriaCu cub-problcmr wtosg rn^t^l^^i^i^oareflreaiC^^nodo^.Fi^r erodot enl^mn^^i^ii^ndoe^l^niip^i^l^^, threebasicPcden-Kahantubn pro01ema[h5] hasalen irschl thaory ereadmirtlblefcn snmecnd0igurotiansoSroУot inhocfe kinematics. i tin, on

extended solution of sub-problem 2 is developed to respond to more manipulator configurations. However, the existing methods of Paden-Kahan sub-problems cannot solve all configurations of robot inverse kinematics. Taking a particular configuration of the six-DOF serial robot "Qianjiang I" as an example, it is not suitable to apply the known Paden-Kahan sub-problems to solve the full inverse kinematic problem. Hence, in this paper a novel improved Paden-Kahan sub-problem is studied for this particular configuration of six-DOF serial robot; then, the new inverse kinematic algorithm is proposed to simplify the solution process and promote efficiency with the virtues of the screw theory.

In section II we provide the necessary notation of the kinematic description of rigid body based on the screw theory and introduce some basic Paden-Kahan subproblems. Section III presents a novel sub-problem solution with a special configuration of the series revolute joints. In section IV we implement the full inverse kinematic algorithm for the special six-DOF "Qianjiang-I" by combining our proposed new sub-problem and other basic Paden-Kahan sub-problems. In section V a numerical example and comparative experiment are illustrated to verify the correctness andeffectivenessofthis proposedmethod. SectionV! conclude sthispaper.

2.S c rew theory of rigidbody

AtwisS, S repeusentethainstantpneousmotipn of a rigid body relative to a referential axis in [15]. It is usually expressed as £ = (o>T ;vT)T eRs whereon^ tfie essgulat velocity oS rotation sbout a œstein axis enl v isthe inetanteneoueiy Hnssr velocity oi trangSatcoaalong tde samu axis, as shown in Figure 1.

Figure 1. Screw motion

We consider gst (0) as the initial configuration of the body coordinate system {B} relative to the inertial coordinate {A} at the base. After the twist motion Ç of the rigid body, the final configuration of {B} to {A} in twist exponential form is

gab =gt (q) = ^ (0) (1)

w (I-^)(9x V)+wwV 0 1 I 0V 01

^ = I +Wsin0+|>2(1-cos0)

w can correspond to w =[^3]T For a revolute joint:

For an open-chain serial robot with n revolute joints, the forward kinematics can be achieved by the product of the exponential formula in Eq.(1) as

( (q) £02...(0)

As for the inverse kinematics problem, we can develop a geometrically effective algorithm by using the known inverse kinematics sub-problems. There are some basic Paden-Kahan sub-problems [15], as follows:

1. Rotation about a single axis.

2. Rotation about two subsequent axes.

3. Rotation to a given distance.

Though the basic Paden-Kahan sub-problems are applied widely in some configurations of robot, this is still not enough to respond to the general configuration structure. This paper therefore extends a novel sub-problem to solve some special configuration structures in series robots.

3. New sub-problem solution

The new sub-problem can be illustrated as in Figure 2, where there is rotation about three non-intersecting axis; two of these are parallel and non co-planar to the third.

The point qa rotates 03 along 43 axis to point qb, then rotates 02 along 42 axis to point qc, and finally rotates 01 along 41 axis to point q. Now we solve Qv 02 and 03 with the given

q, and q.

The twists' motion can be represented as

^qa = qb, ^02qb =qc, xqc =q

X0 X0 X0

qa = q =

where pz a2and<Î3areunitmagmtude twists withzero-pétah,non-interszctingaxiswitZ<32,<t3two parallel axis.

Figure 2. New sub-problemstructure

Applying the principle that the distance between points is preserved byrigid motion:

lU01^02¿hq^ - qi)| = 11^02£

3 q,- qJ

U0i X0 X

qa - ql

\£02 ¿1«

p01 S20 ^03 qa - qll = 1le*02 ^03 qa - qll

where q1 and q2 are arbitrary different points on the axis ;herewechoose thesimplest:

0 0 1

= 0 q2 = 0 W = 0

0 1 0

From Eq.(4) and Eq.(5) we can get

4 2^2 4 3^3 e e q,=qc

II qc-q1l HI q-q1l II qc-qJ HI q-q2l

(qc - )W =0

Substituting Eq.(6) into Eq.(7) gives

From Eq.(4) we have

1 qc = q

Then, 01 can be obtained by applying the Paden-Kahan subproblem 1 in [15]:

Using triangular geometry, we can find 001 and fa as follows:

6j,i = atan2(wr (u x uT r!2)

lull2 +1 IrJI2 -21 U||r23|| cosf=| |v||2

6i = a tan2(—q2 + %2 qx, q2 + %2 %) (10)

As for 02 and 03 we project the screw motion onto a plane which is perpendicular to axis £2, as shown in Figure 3.

J 4 i L 4

to 3 \ ^

is given by

62 = f ±601 = a cos(

u 2 + r || 23 | 2 V 2

2|| U\\r23 ||

Using the same method, we can find 03 as follows:

602 = a tan 2(0 (v xr23 ^ VT^,3 )

f2 = a cos(

l|v| 2 I + r II 23 | 2 u 2

2 v\\\\r llll 23II

Figure 3. (a) Two-parallel-screw motion. (b) Screw motion plane projection.

The vectors are set as

v = q — r

r = r — r

223 2 u3

63 =602 ±f2 = 602 ± a cos(

|| v | |2 + r || 231 2 u 2

2 v\\\\r llll'23 II

The results above indicate that this sub-problem may have one solution, multiple solutions or no solution, depending on whether the rotational surfaces of axis £2, £3 intersect or not. When the two circles have two intersections, as in Figure 3(b), there are two solutions for the sub-problem. When the two circles are tangential to each other, there exists only one solution. There will be no solution where the two circles are not intersecting.

4. Inverse kinematics of six-DOF series robot

4.1 Kinematic modelling

The six-DOF industrial welding serial Robot "Qianjiang I" has six revolute joints and the structure coordinates are shown as Figure 4.

As illustrated in the figure, the last three consecutive joints' axes intersect at a point while the first three joints' structure satisfies the new sub-problem situation presented in Section 3. To solve the inverse kinematics problem of the whole robot, we can reduce it into some appropriate subproblems whose solutions are already known. We can apply the new sub-problem for the inverse kinematics of

r = r — r

32 3 2

the first three joints and apply the Paden-Kahan subproblems to the inverse kinematics of the last three consecutive joints.

rife r6)i T & £

Figure4. (a)"QianjiangI".(b)Structure coordinates.

Firstly, we constructthe twist of each joint:

"0" "1" "1"

0 0 0

1 0 X2= 0 0 « = 0 0

0 d1 a2 + d1

0 _-a1 _ _ -a1 _

1= 0 a1 + d4 0 0 16= 1 0 a2 + a3 + d1 _ -a1 - d4 _

-a2 - d1 - a3 0 0

The initial configuration of Figure 4(b) is

gt (0)=

œ ioo 0 1 0 0 0 1 a2 +a3 + d1 0 0 0 1

a1 + d4

Then, the forward kinematics of the six-DOF industrial welding robot is

t (q) =X1 ^ l^A gst ( 0 ) =

x x r :

n o a p

y y y 'y

nz oz az pz

0 0 0 1

4.2 Solving for the inverse kinematics Step 1 Solving 01, 02, 03

When given the pose position and orientation of end-effectors in Eq.(17), we can solve the first three joints, 01, 02, 03, by applying the proposed sub-problem in Section 3. In thiscase,wecan settheknown parameters as follows:

e and qa = a1 +d4

d1 + a2 + a3

"0" "1" "1" "0" "1" "0"

•1 = 0 •2 = 0 •3 = 0 •4 = 1 •5 = 0 •6 = 0

1 0 0 0 0 1

"0" "0" "0" "0

n = 0 r2 = a1 r = 3 a1 r = r = r = 456 a1 + d4

0 d _ d1 + a2 d1 + a2 + a3

Finally, we substitute these parameters into Eq.(4), and solve 01, 02, 03 accordingly.

Step 2 Solving 04 and 05

From Eq.(17) and the known 01, 02, 03, we have

e®04 e^®5 e1®6 = e^ e^2 e1 gst (d) = g1 (19)

The simple point q6 is chosen on the 46 axis, but not on the axis of 444, 445 as

a1 + d4

Then, the q6 on both sides of Eq.(19) is multiplied, giving

e*04 S5®5 q6 =

= gq =

Thus, we can solve this case directly by applying the Paden-Kahan sub-problem 2 in [15], from which we have:

Eq. (24) has two solutions corresponding to the results of y in Eq. (22).

Step 3 Solving 06 From Eq.(19), we get

e*06 = e^ e^ e^2e-*01 gst (0) = g2

The reference point qt is chosen, though not on the axis,

d1 + a2 + a3



= q e - a, - d4

y6 1 4

b_mm)mTv)-mTu=0 b (mm)5-i

Then, the qt is multiplied on both sides of Eq.(25), giving

1t = glV

p u p2-«2-b2-2«bmm) 11m m 11_

= ±yj(d1 + a2 + a3 )2 - ( 1y6 - ai - d4 )2

zzam+bm+g(m xm )=

-g + d1 + a2 + a3

Then, in this case we employ the Paden-Kahan subproblem 1 in [15], giving

06 = a tan2(W (u ' x v ' ), u 'T v ' ) = a tan2(d4qtx, -d4(qt

"^V 'lty "1 4

In sum, the full inverse-kinematic closed solutions are solved by applying the new sub-problem and basic Paden-Kahan problems. In the process of solving, we can choose the most suitable and simple reference points, such as (6), (20) and (26), resulting in a simplification of the calculating process and an improvement of the computation efficiency. Additionally, the obvious geometric meaning allows us to select the desired and reasonable solutions easily.

Finally, this yields:

^ q6 = c

Then, in this case we employ the Paden-Kahan subproblem 1 in [15], giving

05 = atan2(WT(U x V),u'TV)=atan2((qy6 - a1 - d4),g) 04 = a tan2(W (U x v' ), u'V) zatan2I -gqx6,(qy6 - a1 - d4 ) + g(d1 + a2 + a3 - q*6 )

5. Example and experiment

To verify the correctness of the inverse kinematics algorithm, we choose an arbitrary group of joint angles within the bounds of workspace range ( - n, n] :

0 =pq = 0,0 =p,q4 = p,q =p,q6 =-p

1 4 ' 2 '3 2 4 8

with the structure parameters

d1 = 250mm, a1 = 150mm, a2 = 550mm, a3 = 160mm,d4 = 594mm

Applying theforward kmematics gèves Shs poseposition and orientation of the end-effectors:

-0.4619S -0.73254 -0.50000 7.07100

-0.84464 0.19134 0.50000 -7.07100

-0.27060 0.65330 -0.70710 1400

0 0 0 1

The—, 3a1ng0q.(37 )as aknown condition, we solve the inverse kinematics by applying the new sub-problem and the basic Paden-Kahan sub-problems. The solutions are shown in T-ble 1, and the configurati—nsimulation mo del of each solution ^l^^wnuil^igure 5.

No. Joint en Joint e [°]

1 45.SS0S0001 4 105.tasassas

1 2 s.soesosi2 5 W.9e9tSlSS

3 90.SS0S0000 6 -02.S999SS8S

1 45.70000001 4 4

2 t S-SOSSC^ 5 1S5.00000855

l 94.00000042 6 10oaasaooio

1 45.70000001 4 10t.eotossos

3 2 15.84111248 5 30.97777657

3 eoss/mta 6 -20.4.49.^1941

1 of.oaaoaaoi 4 0

4 2 55.89015548 5 105.42222545

3 64SS7nita 6 10t.e0t0S50S

1 854.99009058 4 0

5 2 8.15732591 5

3 4iaaaoaa55 6 -02.e060SS2S

1 -154.99009058 4

6 2 -1.95792331 5 134.88331106

3 91.84000059 6 157.50300072

1 -134.99999998 4 0

7 2 15.83992123 5 60.86405665

3 58.2963389 6 -22.496155618

1 -134.99999998 4 180.00000000

8 2 15.83992123 5 119.13594335

3 58.2963389 6 157.50384438

Table 1. Eight solutions of the inverse kinematics

From the result above, there are eight solutions of the inverse kinematics, and the first solution is perfect with the given joint in Eq.(29), which verifies the correctness and high accuracy of the presented inverse kinematics algorithm. Furthermore, in real mechanisms when considering the caging device and the actual workspace of the robot, a series of position bounds for every joint need be set. This

Figure 5. Configuration simulation models

ade4t7onal geometgivalinlormiti-nm-y heO-soloO 3he desiridandreasonable lolution duoing—heinverse bine-matiocomputTrig proœi-u—ing the progistdmothi3. In red se3o0 operotot>h,weihoulda3oo adopi optimi—ntion stri3egie s sut-s a s iht minimom enerj^ n^e^te^ee—- (or nora-^unw jooO-dinplocement methid, agi hioose a s-ritaeio soluiien for reb-0

To gir0yt0i-ittideng—and rahabiïityp—oform—-ce ih og—I conltxrtoftise prop—ed a^oriHT!0 we c±ie>oiea dose-curve irregular trajectory to track in Figure 6(a). Before applying the inverse kinematic algorithm, we sampled 681

discret epointsfromthecurv einFigure6(b) and. cakula.ed theC crteeian no ornmnteof eaeh^irtas the desiredp ore matrid of the end-effector.TheinverseHnematics of theee posesnerdddeosolved for the nextstep (te trajectorg grtnning m ining epace.

-300 -200

-100 0 X /mm

Figure 6. (a) Closed-curve irregular trajectory. (b) Sampled discrete points.

To show the correctness and efficiency of our proposed method, we compare our screw theory method ("STM") in this paper with the traditional inverse transformation method ("TIT") in [17] and the vector-dot-product method ("VDP") in [18]. Then, we implement these different algorithms to solve the inverse kinematics of the closed-curve irregular trajectory in Figure 6, respectively, in VC+ + software (Windows XP 32-bit, Intel Core i5, 2.57GHz CPU, 4GB DDR3), and record the computation time of each given pose matrix respectively 1000 times. The performance comparison results are shown in Table 2.

As shown in Table2, our proposed STM method with the geometric characteristic is more effective in determining kinematics solutions than the other two methods. To be more specific, it only requires just under 4.151 p.s to figure out the inverse kinematics solutions for one sample pose based on mean time with our STM. Comparatively, the TTM and VDP take almost 11.683 p.s and 6.457 p.s, respectively, due to their complicated matrix computation. The max time results show the performance of our proposed algorithmisbetterthan others.

6. Conclusion

Concentrating on simplifying the calculation of inverse kinematics cproblems for serial robots, this paper has presented a new sub-problem solution, which extends the application of sub-problem solutions in the inverse kinematics of general configuration robots. The proposed( method can avoid complex matrix inverse operation and make the choice among multiple solutions easier and faster due to the obvious geometric meaning. Further, the case of the six-DOF "Qianjiang I" inverse kinematics problem has been studied by reducing the full inverse kinematics into some sub-problems, namely, our novel sub-problem and the other Paden-Kahan sub-problems, which makes the formula calculation simple and the geometric meaning clear. The results verify the correctness and completeness( of the novel screw-theory-based inverse kinematics algorithm. Finally, the efficiency and reliability perform-anceofourmethod(STM) hasbeen comparedin aclosed-curve irregular trajectory tracking experiment with other two inverse kinematics algorithms, revealing that the STM( algorithm is more practical and can meet the high-performance requirements of real-time operation. In future work, the inverse algorithm for other general structural six-DOF serial robotscan be studiedbasedonthe approachpresent-ed inthis paper.

7. Acknowledgements

The authors express sincere thanks to the editors, reviewers and all the members of our research group for their beneficial comments. The research work is jointly supported by the Science Fund for Creative Research Groups of the National Natural Science Foundation of China (No. 51221004) and Zhejiang Provincial Natural Science Foundation of China (No. LY13E050001), and Hangzhou Civic Significant Technological Innovation Project of China (No. 20132111A04).

Mean time (us) Max time (us)

TIT 11.683 12.579

VDP 6.457 8.191

STM 4.151 4.913

Table 2. Performance comparisons of different algorithms

8. References

[1] Spong W, Hutchinson S, Vidyasagar M. (2006) Robot modelling and control. New York: John Wiley & Sons.

[2] Kucuk S, Bingul Z. (2004) The inverse kinematics solutions of industrial robot manipulators. Interna-

tional Conference on Mechatronics, 3-5 June, Tunis, Tunisia, IEEE: 274-279.

[3] Kucuk S, Bingul Z. (2006) Robot kinematics: forward and inverse kinematics. Industrial Robotics Theory Modelling & Control. Germany: 117-148.

[4] Matsuda Y, Tsukamoto K, Matsumoto T et al. (2014) Remote Operation System of Robot Arm with Visual Servo Mechanism by Target Selection. International Journal of Innovative Computing, Information & Control. 10 (4): 1381-1390.

[5] Chen Y, Mei G, Ma G et al. (2014) Robust Adaptive Inverse Dynamics Control for Uncertain Robot Manipulator. International Journal of Innovative Computing, Information & Control. 10 (2): 575-587.

[6] Crane C, Rico J, Duffy J. (2006) Screw theory and its application to spatial robot manipulators. London: Cambridge University Press.

[7] Sariyildiz E, Temeltas H. (2009) Solution of inverse kinematic problem for serial robot using quaterni-nons. International Conference on Mechatronics and Automation (ICMA), 9-12 Aug, Changchun, China, IEEE: 26-31.

[8] Frisoli A, Solazzi M, Pellegrinetti D. (2011) A new screw theory method for the estimation of position accuracy in spatial parallel manipulators with revolute joint clearances. Mechanism and Machine Theory. 46: 1929-1949.

[9] Gallardo-Alvarado J, Aguilar-Najera C.R, Casique-Rosas L et al. (2008) Kinematics and dynamics of 2 (3-RPS) manipulators by means of screw theory and the principle of virtual work. Mechanism and Machine Theory. 43(10): 1281-1294.

[10] Sariyildiz E, Cakiray E, Temeltas H. (2011) A comparative study of three inverse kinematic

methods of serial industrial robot manipulators in the screw theory framework. International Journal of Advanced Robotic Systems. 8: 9-24.

[11] Rocha C, Tonetto C, Dias A. (2011) A comparison between the Denavit-Hartenberg and the screw-based methods used in kinematic modelling of robot manipulators. Robotics and Computer-Integrated Manufacturing. 27(4): 723-728.

[12] Selig J. (2000) Geometrical foundations of robotics. NJ, USA: World Scientific Publishing Co., Inc.

[13] Lv S.Z., Zhang D.W., Liu H.N. (2010) Solution of screw equation for inverse kinematics of 6R robot based on Wu's method. Journal of Mechanical Engineering. 46(17): 35-41.

[14] Xu K, Simaan N. (2008) An investigation of the intrinsic force sensing capabilities of continuum robots. IEEE Transactions on Robotics. 24(3): 576-587.

[15] Murray R.M, Li Z, Sastry S.S. (1994) A mathematical introduction to robotic manipulation. Boca Raton, FL: CRC Press.

[16] Tan Y.S, Xiao A.P. (2008) Extension of the second paden-kahan sub-problem and its application in the inverse kinematics of a manipulator. International Conference on Robotics, Automation and Mecha-tronics. 21-24 Sept, Chengdu, China, IEEE: 379-381.

[17] Lenarcic J, Husty M. (2012) Latest Advances in Robot Kinematics. Springer Netherlands.

[18] Liu H.S, Zhu S.Q, Wu J.B et al. (2008) Real-time inverse kinematics algorithm based on vector dot product. Transactions of the Chinese Society for Agricultural Machinery. 6: 212-216.