#### Abstract

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 () to Cartesian space (). Conversely, the inverse kinematics of the articulated robot arm will transform the coordinates of robot manipulator from Cartesian space () to the joint space (). 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 is given bywhere 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: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 can be written asThe () 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: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:Comparing the element (3,3) in (18) with (12), we obtainedTherefore, we can get Further, comparing the element (1,1) in (18) with (4), we obtained Therefore, we can getLet us assume thatthen substituting (19)~(22) into (13)~(15), we can get the sequence for computations inverse kinematics as follows:From (24) and (25), we can getFrom (23) and (26), we can getOnce is obtained, substitute it to (23) and (26) to get From (29), solve the linear equation in order to find the and asTherefore, can be derived as follows:Further, from (17) and (20), 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.

*Step 1. *Consider

*Step 2. *Consider

*Step 3. *ConsiderIn the previous steps, , , and are the position of end point which are the same as , , and in (18).

For computing the inverse kinematics, consider the following steps.

*Step 1. *Consider

*Step 2. *Consider

*Step 3. *Consider

*Step 4. *Consider

*Step 5. *Consider

*Step 6. *Consider

*Step 7. *Consider

The parameters of robot arm , , , 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 and 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 , , , and functions. The LUT for and functions stores 360 pieces of data with 24-bit Q23 format and that for and 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.

##### 3.2. 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.

*(1) First Step. * is computed by using Taylor series expansion and the input values are defined within (or the output value: ). The third-order polynomial is considered and the expression within the vicinity of is shown as follows: with 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 . To solve this problem, combining the LUT technique and Taylor series expanse technique is considered. To set up the LUT, several specific values for within the range are firstly chosen; then the parameters from to in (46) are computed. Those data included and to will be stored to LUT. Following that, when it needs to compute in (45), the which is the most approximate to input and its related to will be selected from LUT and then perform the computing task.

*(2) Second Step.* After completing the computation of , we can evaluate further and let the output suitable to the range be within . 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 16-bit Q15 format, respectively. It consists of one main circuit with FSM architecture and two components for computing the divider and functions. However, the design and implementation of the function is a major task. The input and output values of component all belong to 16-bit Q15 format. The FSM is employed to model the computation of 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 . The steps execute to look up 5 tables and perform the computation of polynomial in (45). Further, according to the computation logic shown in Figure 3, it uses 16 steps to complete the 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.

#### 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 , , and which is relative to (), as well as the mechanical parameters by , , , and . The outputs are mechanical angles . 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.

**(a)**

**(b)**

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 bySimulation 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 , , and (mm) and the outputs are five joint angles presented by , , , , (degree).

In the first case, the inputs , , and are chosen asIn the second case, the inputs , , and are chosen asSimulation 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 , , , , (degree) and the outputs are end-effectors presented by , , and (mm).

In the first case, the inputs , , , , and are chosen as In the second case, the inputs , , , , and are chosen asFrom these two cases, the absolute difference of end-effectors , , and which are calculated from ModelSim and from Matlab is less than 0.05 mm, and to 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 , , and is less than 0.05 mm and the error for 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.