When robot arm performs a motion control, it needs to calculate a complicated algorithm of forward and inverse kinematics which consumes much CPU time and certainty slows down the motion speed of robot arm. Therefore, to solve this issue, the development of a hardware realization of forward and inverse kinematics for an articulated robot arm is investigated. In this paper, the formulation of the forward and inverse kinematics for a five-axis articulated robot arm is derived firstly. Then, the computations algorithm and its hardware implementation are described. Further, very high speed integrated circuits hardware description language (VHDL) is applied to describe the overall hardware behavior of forward and inverse kinematics. Additionally, finite state machine (FSM) is applied for reducing the hardware resource usage. Finally, for verifying the correctness of forward and inverse kinematics for the five-axis articulated robot arm, a cosimulation work is constructed by ModelSim and Simulink. The hardware of the forward and inverse kinematics is run by ModelSim and a test bench which generates stimulus to ModelSim and displays the output response is taken in Simulink. Under this design, the forward and inverse kinematics algorithms can be completed within one microsecond.
1. Introduction
The kinematics problem is an important study in the robotic motion control. The mapping from joint space to Cartesian task space is referred to as direct kinematics and mapping from Cartesian task space to joint space is referred to as inverse kinematics [1]. Because of the complexity of inverse kinematics, it is usually more difficult than forward kinematics to find the solutions [2–5]. In addition, when robot manipulator executes a motion control, the complicated inverse kinematics computation consumes much CPU time and it certainty slows down the motion performance of robot manipulator. Therefore, solving this problem becomes an important issue.
For the progress of very large scale integration (VLSI) technology, the field programmable gate arrays (FPGAs) have been widely investigated due to their programmable hard-wired feature, fast time to market, shorter design cycle, embedding processor, low power consumption, and higher density for the implementation of the digital system. FPGA provides a compromise between the special-purpose application specified integrated circuit (ASIC) hardware and general-purpose processors. Hence, many practical applications in industrial control [6], multiaxis motion control [7], and robotic control [8–10] have been studied. Therefore, for speeding up the computational power, the forward and inverse kinematics based on VHDL are studied in this paper. And the VHDL is applied to describe the overall behavior of the forward and inverse kinematics.
In recent years, an electronic design automation (EDA) simulator link, which can provide a cosimulation interface between MALTAB/Simulink [11] and HDL simulators-ModelSim [12], has been developed and applied in the design of the control system [13]. Using it, you can verify a VHDL, Verilog, or mixed-language implementation against your Simulink model or MATLAB algorithm. In MATLAB/Simulink environment, it can generate stimuli to ModelSim and analyze the simulation’s responses [11]. In this paper, a cosimulation by EDA simulator link is applied to the proposed forward kinematics and inverse kinematics hardware. Some simulation results based on EDA simulator link will demonstrate the correctness and effectiveness of the forward and inverse kinematics.
2. Description of the Forward and Inverse Kinematics
A typical five-axis articulated robot arm is studied in this paper. Figure 1 shows its link coordinate system by Denavit-Hartenberg convention. Table 1 illustrates the values of the kinematics parameters. The forward kinematics of the articulated robot arm is the transformation of joint space R5 (θ1,θ2,θ3,θ4,θ5) to Cartesian space R3 (x,y,z). Conversely, the inverse kinematics of the articulated robot arm will transform the coordinates of robot manipulator from Cartesian space R3 (x,y,z) to the joint space R5 (θ1,θ2,θ3,θ4,θ5). The computational procedure of forward and inverse kinematics is shown in Figure 1 and Table 1.
Denavit-Hartenberg parameters for robot arm in Figure 1.
Link i
di (mm)
ai (mm)
αi
θi
1
d1 = 275
0
α1=-π/2
θ1
2
0
a2 = 275
0
θ2
3
0
a3 = 255
0
θ3
4
0
0
α4=-π/2
θ4
5
d5 = 195
0
0
θ5
The link coordinates system of a five-axis articulated robot arm.
A coordinate frame is assigned to each link based on Denavit-Hartenberg notation. The transformation matrix for each link from frame i to i-1 is given by(1)Aii-1=TZ,dTZ,θTX,aTX,α=10000100001di0001cosθi-sinθi00sinθicosθi0000100001×100ai01000010000110000cosαi-sinαi00sinαicosαi00001=cosθi-cosαisinθisinαisinθiaicosθisinθicosαicosθi-sinαicosθiaisinθi0sinαicosαidi0001,where TZ,θ and TX,α present rotation and the TZ,d and TX,α denote translation. Substituting the parameters in Table 1 into (1), the coordinate five matrixes respected with five axes of robot arm are shown as follows:(2)A10=T(Z,d1)T(Z,θ1)T(X,0)T(X,-π2)=cosθ10-sinθ10sinθ10cosθ100-10d10001,A21=T(Z,0)T(Z,θ2)T(X,a2)T(X,0)=cosθ2-sinθ20a2cosθ2sinθ2cosθ20a2sinθ200100001,A32=T(Z,0)T(Z,θ3)T(X,a3)T(X,0)=cosθ3-sinθ30a3cosθ3sinθ3cosθ30a3sinθ300100001,A43=T(Z,0)T(Z,θ4)T(X,0)T(X,-π2)=cosθ40-sinθ40sinθ40cosθ400-1000001,A54=T(Z,d5)T(Z,θ5)T(X,0)T(X,0)=cosθ5-sinθ500sinθ5cosθ500001d50001.The forward kinematics of the end-effector with respect to the base frame is determined by multiplying five matrices from (2) as given above. An alternative representation of A05 can be written as(3)THR=A50=A10·A21·A32·A43·A54Δ__nxoxaxpxnyoyaypynzozazpz0001.The (n,o,a) are the orientation in the Cartesian coordinate system which is attached to the end-effector. Using the homogeneous transformation matrix to solve the kinematics problems, its transformation specifies the location (position and orientation) of the end-effector and the vector p presents the position of end-effector of robot arm. By multiplying five matrices and substituting into (3) and then comparing all the components of both sides after that we can solve the forward kinematics of the five-axis articulated robot arm as follows:(4)nx=cosθ1cosθ234cosθ5+sinθ1sinθ5,(5)ny=sinθ1cosθ234cosθ5-cosθ1sinθ5,(6)nz=-sinθ234cosθ5,(7)ox=-cosθ1cosθ234sinθ5+sinθ1cosθ5,(8)oy=-sinθ1cosθ234sinθ5-cosθ1cosθ5,(9)oz=sinθ234sinθ5,(10)ax=-cosθ1sinθ234,(11)ay=-sinθ1sinθ234,(12)az=-cosθ234,(13)px=cosθ1a2cosθ2+a3cosθ23-d5sinθ234,(14)py=sinθ1a2cosθ2+a3cosθ23-d5sinθ234,(15)pz=d1-a2sinθ2-a3sinθ23-d5cosθ234,where(16)θ23=θ2+θ3,(17)θ234=θ2+θ3+θ4.
The position vector p directs the location of the origin of the (n,o,a) frame which is defined to let the end-effector of robot arm by always gripping from a top down position. Therefore, the matrix in (3) is set by the following form:(18)TRH=100x0-10y00-1z0001.Comparing the element (3,3) in (18) with (12), we obtained(19)cosθ234=1.Therefore, we can get (20)θ234=0.Further, comparing the element (1,1) in (18) with (4), we obtained (21)cosθ1-θ5=1.Therefore, we can get(22)θ1-θ5=0orθ5=θ1.Let us assume that(23)b=a2cosθ2+a3cosθ23;then substituting (19)~(22) into (13)~(15), we can get the sequence for computations inverse kinematics as follows:(24)x=cosθ1·b(25)y=sinθ1·b(26)z=d1-a2sinθ2-a3sinθ23-d5.From (24) and (25), we can get(27)b=±(x2+y2),θ1=θ5=atan2yx.From (23) and (26), we can get(28)θ3=arccosb2+d1-d5-z2-a22-a322a2a3.Once θ3 is obtained, substitute it to (23) and (26) to get (29)b=a2+a3cosθ3cosθ2+a3sinθ3sinθ2,d1-d5-z=a2+a3cosθ3sinθ2+a3sinθ3cosθ2.From (29), solve the linear equation in order to find the sinθ2 and cosθ2 as(30)sinθ2=a2+a3cosθ3d1-d5-z-a3bsinθ3b2+d1-d5-z2,cosθ2=a2+a3cosθ3b+a3sinθ3d1-d5-zb2+d1-d5-z2.Therefore, θ2 can be derived as follows:(31)θ2=atan2a2+a3cosθ3d1-d5-z-a3bsinθ3a2+a3cosθ3b+a3sinθ3d1-d5-z.Further, from (17) and (20), θ4 is obtained as(32)θ4=-θ2-θ3.
Finally, the forward kinematics and inverse kinematics of the five-axis articulated robot arm, based on the assumption in (18) in which the end-effector of the robot arm is always toward the top down direction, can be summarized as follows.
For computing the forward kinematics, consider the following steps.
Consider(35)endz=d1-a2sinθ2-a3sinθ23-d5cosθ234.In the previous steps, endx, endy, and endz are the position of end point which are the same as x, y, and z in (18).
For computing the inverse kinematics, consider the following steps.
Step 1.
Consider(36)θ1=θ5=atan2endy,endx.
Step 2.
Consider(37)b=±endx2+endy2.
Step 3.
Consider(38)θ3=acosb2+d1-d5-endz2-a22-a322a2a3.
Step 4.
Consider(39)S2=a2+a3cosθ3d1-d5-endz-a3bsinθ3.
Step 5.
Consider(40)C2=a2+a3cosθ3b+a3sinθ3d1-d5-endz.
Step 6.
Consider(41)θ2=atan2S2,C2.
Step 7.
Consider(42)θ4=-θ2-θ3.
The parameters of robot arm a2, a3, d1, d5 are shown in Table 1.
3. Computations of Trigonometric Function and Its Hardware Implementation
Before performing the computation of the forward and inverse kinematics for five-axis articulated robot arm, some key trigonometric functions need to be built up as a component for being applied, and those are sine function, cosine function, arctangent function, and arccosine function. To increase the computing accuracy, LUT (look-up table) technique and Taylor series expanse technique are used to the computational algorithm design of the arctangent function and arccosine function. However, the computation algorithm used in these two functions is very similar; therefore, the detailed design methods, only sine/cosine function, and arctangent function are described as follows.
3.1. Computation Algorithm and Hardware Realization of Sine Function and Cosine Function
To compute sin(θ) and cos(θ) functions, the θ=θI+θF is firstly defined, in which θI and θF represented the integer part and fraction part of the θ, respectively. Then the formulation of these two functions is expanded as follows: (43)sin(θI+θF)=sin(θI)cos(θF)+cos(θI)sin(θF)cosθI+θF=cosθIcosθF-sinθIsinθF.In the hardware design, θ is adopted as 16-bit Q7 format. Therefore, if θ is 0000001001000000, it represents 4.5 degree. In addition, four LUTs (look-up tables) are built up to store the values of sin(θI), cos(θI), sin(θF), and cos(θF) functions. The LUT for sin(θI) and cos(θI) functions stores 360 pieces of data with 24-bit Q23 format and that for sin(θF) and cos(θF) functions stores 128 pieces of data with the same 24-bit Q23 format. Therefore, according to (43), the results of sine and cosine with 16-bit Q15 format can be computed after looking up four tables. In the realization, finite state machine (FSM) is adopted and the example to compute the cosine function is shown in Figure 2. It presents four LUTs, two multipliers, and one adder in hardware which manipulates 6 steps to complete the overall computation. Due to the fact that the operation of each step is 20 ns (50 MHz) in FPGA, a total of 6 steps only need 120 ns operation time. In addition, the FPGA (Altera Cyclone IV) resource usage for the realization of the sine or cosine function needs 232 logic elements (LEs) and 30,720 RAM bits.
FSM for computing the cosine function.
3.2. Computation Algorithm and Hardware Realization of Arctangent Function
The equation of arctangent function is shown as follows: (44)θ=atan2yx,where the inputs are x and y and the output is θ. Herein, there are two steps to evaluate the arctangent function.
(1) First Step. θ1=f(x)=tan-1(x) is computed by using Taylor series expansion and the input values are defined within 1≤x≤0 (or the output value: 45°≤θ1≤0°). The third-order polynomial is considered and the expression within the vicinity of x0 is shown as follows: (45)fx≅a0+a1x-x0+a2x-x02+a3x-x03,with (46)a0=fx0=tan-1x0,a1=f′x0=11+x02,a2=f′′x0=-2x01+x022,a3=f3x0=-2+6x021+x023.Actually, in realization, only third-order expansion in (44) is not enough to obtain an accuracy approximation due to the reason that the large error will occur when the input x is far from x0. To solve this problem, combining the LUT technique and Taylor series expanse technique is considered. To set up the LUT, several specific values for x0 within the range 1≤x≤0 are firstly chosen; then the parameters from a0 to a3 in (46) are computed. Those data included x0 and a0 to a3 will be stored to LUT. Following that, when it needs to compute tan-1(x) in (45), the x0 which is the most approximate to input x and its related a0 to a3 will be selected from LUT and then perform the computing task.
(2) Second Step. After completing the computation of tan-1(x), we can evaluate θ=atan2(y/x) further and let the output suitable to the range be within -180°≤θ≤180°. The formulation for each region in X-Y coordinate is shown in Figure 3.
Compute θ=atan2(y/x) of each region in X-Y coordinates.
In hardware implementation, the inputs and output of the arctangent function are designed with 32-bit Q15 and 16-bit Q15 format, respectively. It consists of one main circuit with FSM architecture and two components for computing the divider and tan-1(x) functions. However, the design and implementation of the tan-1(x) function is a major task. The input and output values of component tan-1(x) all belong to 16-bit Q15 format. The FSM is employed to model the computation of tan-1(x) and is shown in Figure 4 which uses two multipliers and one adder in the design. The multiplier and adder apply Altera LPM (library parameterized modules) standard. In Figure 4, it manipulates 7-step machine to carry out the overall computations of tan-1(x). The steps s0~s1 execute to look up 5 tables and s2~s6 perform the computation of polynomial in (45). Further, according to the computation logic shown in Figure 3, it uses 16 steps to complete the atan2(y/x) function. Due to the fact that the operation of each step is 20 ns (50 MHz) in FPGA, a total of 16 steps only need 320 ns operation time. In addition, the FPGA (Altera Cyclone IV) resource usage for the realization of the arctangent function needs 4,143 LEs and 1,280 RAM bits.
FSM to compute tan-1(x) function.
4. Design and Hardware Implementation of Forward/Inverse Kinematics and Its Simulation Results
The block diagram of kinematics for five-axis articulated robot arm is shown in Figure 5. The inputs are the end-point position by endx, endy, and endz which is relative to (x,y,z), as well as the mechanical parameters by a2, a3, d1, and d5. The outputs are mechanical angles θ1~θ5. In Figure 5, the parameters of mechanical length and the end-point position are designed with 32-bit Q15 data format and the parameters of mechanical angle are designed with 16-bit Q7 data format. According to the formulations of forward and inverse kinematics which are described in Section 2, the finite state machine (FSM) method is applied to design the hardware for reducing the usage of hardware resource. Herein, the FSM designs to compute the forward kinematics and inverse kinematics are, respectively, shown in Figures 6 and 7. The FSM will generate sequential signals and will step-by-step compute the forward kinematics and inverse kinematics. The implementation of inverse kinematics is developed by VHDL. In Figure 6, there are 34 steps to present the computations of forward kinematics, and the circuit includes two multipliers, two adders, one component for sine function, and one component for cosine function. In Figure 7, there are 47 steps to perform the inverse kinematics, and the circuit needs three multipliers, two dividers, two adders, one square root function, one component for arctangent function, one component for arccosine function, one component for sine function, and one component for cosine function. The notation “COM” in Figures 6~7 is represented with “component.” For example, the “COM for cos” is the component of cosine function. The designs of the component regarded as trigonometric function are described in previous section. Due to the fact that the operation of each step is 20 ns (50 MHz) in FPGA, the executing time for the computation of forward and inverse kinematics is 680 ns and 940 ns, respectively. In addition, the FPGA (Altera Cyclone IV) resource usage for the realization of the forward kinematics IP and the inverse kinematics IP is 1,575 LEs and 30,720 RAM bits and 9,400 LEs and 84,224 RAM bits, respectively.
Block diagram for (a) forward kinematics and (b) inverse kinematics.
FSM for computing the forward kinematics.
FSM for computing the inverse kinematics.
The cosimulation architecture by ModelSim/SimuLink for the proposed forward kinematics is shown in Figure 8 and that for the inverse kinematics is shown in Figure 9, whose works in ModelSim execute the function of the computing the forward and inverse kinematics. The input values to ModelSim are provided from Simulink and, through the computation working in ModelSim, output responses are displayed to Simulink. To confirm the correctness, an embedded Matlab function for computing the forward and inverse kinematics is considered. Under the cosimulation of ModelSim and Simulink architecture, the simulation results are presented as follows.
Cosimulation architecture of forward kinematics using ModelSim and SimuLink.
Cosimulation architecture of inverse kinematics using ModelSim and SimuLink.
Mechanical parameters of the five-axis articulated robot arm are chosen by(47)a2=275,a3=255,d1=275,d5=195mm.Simulation results of inverse kinematics with the following two cases are listed in Table 2. The inputs of inverse kinematics are the end-effectors of robot arm presented by endx, endy, and endz (mm) and the outputs are five joint angles presented by θ1, θ2, θ3, θ4, θ5 (degree).
Two cases’ simulation results of inverse kinematics from ModelSim and Matlab.
Output
Input
The 1st case (endx,endy,endz)(1)
The 2nd case (endx,endy,endz)(2)
ModelSim
Matlab
Error
ModelSim
Matlab
Error
θ1
44.8984375
44.9043477
0.0059702
71.5703125
71.5650511
−0.0052613
θ2
4.8906250
4.8948685
0.0042435
−99.4843750
−99.4939347
0.0095597
θ3
49.1953125
49.1952721
−0.0000400
76.7060765
76.7031250
0.0029515
θ4
−54.0859375
−54.0901472
0.0042032
22.7878581
22.7812500
0.0066081
θ5
44.8984375
44.9043477
0.0059102
71.5703125
71.5650511
0.0052613
(1) denotes the case 1 shown in (48).
(2) denotes the case 2 shown in (49).
In the first case, the inputs endx, endy, and endz are chosen as(48)endx=300,endy=299,endz=-150.In the second case, the inputs endx, endy, and endz are chosen as(49)endx=60,endy=180,endz=450.Simulation results of forward kinematics with the following two cases are listed in Table 3. The inputs of forward kinematics are five joint angles presented by θ1, θ2, θ3, θ4, θ5 (degree) and the outputs are end-effectors presented by endx, endy, and endz (mm).
Two cases’ simulation results of forward kinematics from ModelSim and Matlab.
Output
Input
The 1st case (θ1, θ2, θ3, θ4, θ5)(1)
The 2nd case (θ1, θ2, θ3, θ4, θ5)(2)
ModelSim
Matlab
Error
ModelSim
Matlab
Error
end x
300.0321650
300.0000160
−0.0321490
59.9913940
60.0000000
0.0086063
end y
299.0496215
299.0000160
−0.0496054
179.9915466
180.0000000
0.0084537
end z
−149.9700920
−149.9999999
−0.0299906
450.0378417
450.0000000
−0.0378416
(1) denotes the case 1 shown in (50).
(2) denotes the case 2 shown in (51).
In the first case, the inputs θ1, θ2, θ3, θ4, and θ5 are chosen as (50)θ1=44.9043477,θ2=4.8948685,θ3=49.1952721,θ4=-54.0901472,θ5=44.9043477.In the second case, the inputs θ1, θ2, θ3, θ4, and θ5 are chosen as(51)θ1=71.5650511,θ2=-99.4939347,θ3=76.703125,θ4=22.7812500,θ5=71.5650511.From these two cases, the absolute difference of end-effectors endx, endy, and endz which are calculated from ModelSim and from Matlab is less than 0.05 mm, and θ1 to θ5 are less than 0.01°. It shows that the proposed forward and inverse kinematics for the five-axis articulated robot arm are correct and effective.
5. Conclusions
The forward kinematics and inverse kinematics of five-axis articulated robot arm, in which the end-effector of the robot arm is always toward the top down direction, have been successfully demonstrated in this paper. Through the cosimulation of ModelSim and Simulink, the accuracy under two examples of forward and inverse kinematics with the results of error of the end-effectors endx, endy, and endz is less than 0.05 mm and the error for θ1~θ5 is less than 0.01°. Further, the executing times for the computations of forward and inverse kinematics in FPGA are only 680 ns and 940 ns, respectively. The high speed computational power and reasonable accuracy apparently increase the motion performance of the five-axis articulated robot arm.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgment
This work was supported by National Science Council of the R.O.C. under Grant no. NSC 100-2632-E-218 -001-MY3.
CraigJ. J.ShenW.GuJ.MiliosE. E.Self-configuration fuzzy system for inverse kinematics of robot manipulatorsProceedings of the Annual Meeting of the North American Fuzzy Information Processing Society (NAFIPS '06)June 2006414510.1109/NAFIPS.2006.3658562-s2.0-40649113189FalcoP.NataleC.On the stability of closed-loop inverse kinematics algorithms for redundant robotsParkS.-W.OhJ.-H.Hardware realization of inverse kinematics for robot manipulatorsHuangG.-S.TungC.-K.LinH.-C.HsiaoS.-H.Inverse kinematics analysis trajectory planning for a robot armProceedings of the 8th Asian Control Conference (ASCC '11)May 2011twn9659702-s2.0-80052002466MonmassonE.IdkhajineL.CirsteaM. N.BahriI.TisanA.NaouarM. W.FPGAs in industrial control applicationsChoJ. U.LeQ. N.JeonJ. W.An FPGA-based multiple-axis motion control chipKungY.-S.TsengK.-H.ChenC.-S.SzeH.-Z.WangA.-P.FPGA-implementation of inverse kinematics and servo controller for robot manipulatorProceedings of the IEEE International Conference on Robotics and Biomimetics (ROBIO '06)December 20061163116810.1109/ROBIO.2006.3400932-s2.0-46249120413Sánchez-SolanoS.CabreraA. J.BaturoneI.Moreno-VeloF. J.BroxM.FPGA implementation of embedded fuzzy controllers for robotic applicationsWongC. C.LiuC. C.FPGA realisation of inverse kinematics for biped robot based on CORDICThe MathworksModeltechKungY.-S.QuynhN. V.HuangC.-C.HuangL.-C.Simulink/ModelSim co-simulation of sensorless PMSM speed controllerProceedings of the IEEE Symposium on Industrial Electronics and Applications (ISIEA '11)September 2011242910.1109/ISIEA.2011.61087092-s2.0-84862915069