Digital Hardware Realization of Forward and Inverse Kinematics for a Five-Axis Articulated Robot Arm

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.


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][3][4][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 hardwired 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][9][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 2 Mathematical Problems in Engineering  Table 1: Denavit-Hartenberg parameters for robot arm in Figure 1.
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.

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 5 ( 1 , 2 , 3 , 4 , 5 ) to Cartesian space 3 ( , , ). Conversely, the inverse kinematics of the articulated robot arm will transform the coordinates of robot manipulator from Cartesian space 3 ( , , ) to the joint space 5 ( 1 , 2 , 3 , 4 , 5 ). The computational procedure of forward and inverse kinematics is shown in Figure 1 and Table 1.
A coordinate frame is assigned to each link based on Denavit-Hartenberg notation. The transformation matrix for each link from frame to − 1 is given by where ( , ) and ( , ) present rotation and the ( , ) and ( , ) 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) Mathematical Problems in Engineering 3 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 0 5 can be written as ] . ( The ( , , ) 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 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: = cos 1 cos 234 cos 5 + sin 1 sin 5 , = sin 1 cos 234 cos 5 − cos 1 sin 5 , = − sin 234 cos 5 , = − cos 1 cos 234 sin 5 + sin 1 cos 5 , = − sin 1 cos 234 sin 5 − cos 1 cos 5 , = sin 234 sin 5 , = − cos 1 sin 234 , = − sin 1 sin 234 , = − cos 234 , = cos 1 ( 2 cos 2 + 3 cos 23 − 5 sin 234 ) , = sin 1 ( 2 cos 2 + 3 cos 23 − 5 sin 234 ) , where The position vector directs the location of the origin of the ( , , ) 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) Comparing the element (3,3) in (18) with (12), we obtained Therefore, we can get Further, comparing the element (1,1) in (18) with (4), we obtained Therefore, we can get Let us assume that then substituting (19)∼(22) into (13)∼(15), we can get the sequence for computations inverse kinematics as follows: From (24) and (25), we can get = ± √ ( 2 + 2 ), From (23) and (26), we can get Once 3 is obtained, substitute it to (23) and (26) to get From (29), solve the linear equation in order to find the sin 2 and cos 2 as Mathematical Problems in Engineering Therefore, 2 can be derived as follows: Further, from (17) and (20), 4 is obtained as 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.
For computing the inverse kinematics, consider the following steps.

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.

Computation Algorithm and Hardware Realization of Sine Function and Cosine Function.
To compute sin( ) and cos( ) functions, the = + is firstly defined, in which and represented the integer part and fraction part of the , respectively. Then the formulation of these two functions is expanded as follows: 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 ( ), cos ( ), sin ( ), and cos ( ) functions. The LUT for sin ( ) and cos ( ) functions stores 360 pieces of data with 24-bit Q23 format and that for sin ( ) and cos ( ) functions stores 128 pieces of data with the same 24bit 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.

Computation Algorithm and Hardware
Realization of Arctangent Function. The equation of arctangent function is shown as follows: where the inputs are and and the output is . Herein, there are two steps to evaluate the arctangent function. The third-order polynomial is considered and the expression within the vicinity of 0 is shown as follows: 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 is far from 0 . To solve this problem, combining the LUT technique and Taylor series expanse technique is considered. To set up the LUT, several specific values for 0 within the range 1 ≤ ≤ 0 are firstly chosen; then the parameters from 0 to 3 in (46) are computed. Those data included 0 and 0 to 3 will be stored to LUT. Following that, when it needs to compute tan −1 ( ) in (45), the 0 which is the most approximate to input and its related 0 to 3 will be selected from LUT and then perform the computing task.
(2) Second Step. After completing the computation of tan −1 ( ), we can evaluate = tan 2( / ) further and let the output suitable to the range be within −180 ∘ ≤ ≤ 180 ∘ . The formulation for each region in -coordinate is shown in Figure 3.
In hardware implementation, the inputs and output of the arctangent function are designed with 32-bit Q15 and 16bit Q15 format, respectively. It consists of one main circuit with FSM architecture and two components for computing the divider and tan −1 ( ) functions. However, the design and implementation of the tan −1 ( ) function is a major task. The input and output values of component tan −1 ( ) all belong  to 16-bit Q15 format. The FSM is employed to model the computation of tan −1 ( ) 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 ( ). The steps 0 ∼ 1 execute to look up 5 tables and 2 ∼ 6 perform the computation of polynomial in (45). Further, according to the computation logic shown in Figure 3, it uses 16 steps to complete the tan 2( / ) 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.

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 endpoint position by end , end , and end which is relative    to ( , , ), as well as the mechanical parameters by 2 , 3 , 1 , and 5 . The outputs are mechanical angles 1 ∼ 5 . In Figure 5, the parameters of mechanical length and the endpoint 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.
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.
Mechanical parameters of the five-axis articulated robot arm are chosen by 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 end , end , and end (mm) and the outputs are five joint angles presented by 1 , 2 , 3 , 4 , 5 (degree).
In the first case, the inputs end , end , and end are chosen as end = 300, end = 299, In the second case, the inputs end , end , and end are chosen as end = 60, end = 180, 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 end , end , and end (mm).
In the first case, the inputs 1 , 2 , 3 , 4 , and 5 are chosen as From these two cases, the absolute difference of end-effectors end , end , and end 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.

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 end , end , and end 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.