FPGA-Realization of a Motion Control IC for Robot Manipulator

Robotic control is currently an exciting and highly challenging research focus. Several solutions for implementing the control architecture for robots have been proposed (Kabuka et al., 1988; Yasuda, 2000; Li et al., 2003; Oh et al., 2003). Kabuka et al. (1998) apply two highperformance floating-point signal processors and a set of dedicated motion controllers to build a control system for a six-joint robots arm. Yasuda (2000) adopts a PC-based microcomputer and several PIC microcomputers to construct a distributed motion controller for mobile robots. Li et al. (2003) utilize an FPGA (Field Programmable Gate Array) to implement autonomous fuzzy behavior control on mobile robot. Oh et al. (2003) present a DSP (Digital Signal Processor) and a FPGA to design the overall hardware system in controlling the motion of biped robots. However, these methods can only adopt PC-based microcomputer or the DSP chip to realize the software part or adopt the FPGA chip to implement the hardware part of the robotic control system. They do not provide an overall hardware/software solution by a single chip in implementing the motion control architecture of robot system. For the progress of 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 implementing digital system. FPGA provides a compromise between the special-purpose ASIC (application specified integrated circuit) hardware and general-purpose processors (Wei et al., 2005). Hence, many practical applications in motor control (Zhou et al., 2004; Yang et al., 2006; Monmasson & Cirstea, 2007) and multi-axis motion control (Shao & Sun, 2005) have been studied, using the FPGA to realize the hardware component of the overall system. The novel FPGA (Field Programmable Gate Array) technology is able to combine an embedded processor IP (Intellectual Property) and an application IP to be an SoPC (Systemon-a-Programmable-Chip) developing environment (Xu et al., 2003; Altera, 2004; Hall & Hamblen, 2004), allowing a user to design an SoPC module by mixing hardware and software. The circuits required with fast processing but simple computation are suitable to be implemented by hardware in FPGA, and the highly complicated control algorithm with heavy computation can be realized by software in FPGA. Results that the software/hardware codesign function increase the programmable, flexibility of the designed digital system and reduce the development time. Additionally, software/hardware parallel processing enhances the controller performance. Our previous works (Kung & Shu, 2005; Kung et al., 2006; Kung &


Important
The typical architecture of the conventional motion control system for robot manipulator is shown in Fig. 1(b), which consists of a central controller, five sets of servo drivers and one robot manipulator. The central controller, which usually adopts a float-pointed processor, performs the function of motion trajectory, inverse kinematics, data communication with servo drivers and the external device. Each servo driver uses a fixed-pointed processor, some specific ICs and an inverter to perform the functions of position control at each axis of robot manipulator and the data communication with the central controller. Data communication media between them may be an analog signal, a bus signal or a serial asynchronous signal. Therefore, once the central controller receives a motion command from external device. It will execute the computation of motion trajectory and inverse kinematics, generate five position commands and send those signals to five servo drivers. Each servo driver received the position command from central controller will execute the position controller to control the servo motor of manipulator. As the result, the motion trajectory of robot manipulator will follow the prescribed motion command. However, the motion control system in Fig .1 (b) has some drawbacks, such as large volume, easy effected by the noise, expensive cost, inflexible, etc. In addition, data communication and handshake protocol between the central controller and servo drivers slow down the system executing speed. To improve aforementioned drawbacks, based on the novel FPGA technology, the central controller and the controller part of five servo drivers in Fig. 1 (b) are integrated into a motion control IC in this study, which is shown in Fig.  1(a). The proposed motion control IC has two IPs. One IP performs the functions of the motion trajectory by software. The other IP performs the functions of inverse kinematics and five axes position controllers by hardware. Hence, hardware/software co-design technology can make the motion controller of robot manipulator more compact, flexible, better performance and less cost. The detailed design methodology is described in the following sections, and the contributions of this study will be illustrated in the conclusion section.  The five-axis articulated robot manipulator

Motion control system
Motion command (b) Figure 1. (a) The proposed architecture of an FPGA-based motion control system for robot manipulator (b) the conventional motion control system for robot manipulator www.intechopen.com

Mathematical model of a robot manipulator with actuator
The dynamic equation of the n-link manipulator is given by (Lewis et al., 1993):  (2) is small and can be ignored. Then, (2) to (4) can be arranged and simplified by the following equation.
Therefore, the dynamics of the dc motors that drive the arm links are given by the n decoupled equations The dynamic equations including the robot manipulator and DC motor can be obtained by combining (1) and (6). The gear ratio of the coupling from the motor i to arm link i is given by i r , which is defined as Hence, substituting (8) into (6), then into (1), it can be obtained by A five-axis motion and servo controller th 1 DC motor and driver of arm Since the gear ratio i r is small in a commercial robot manipulator, the 2 i r term in (10) can be ignored, and (10) can be simplified as If the gear ratio is small, then the control block diagram combining arm link, motor actuator, position loop P controller, speed loop PI controller, inverse kinematics and trajectory planning for the servo and motion controller in a five-axis robot manipulator can be represented in Fig. 2. In Fig.2, the digital PI controllers in the speed loop are formulated as follows.
Where the p k , i k are P-controller gain and I-controller gain, the ) (n e is the error between command and sensor value and the ) (n v is the PI controller output. Figure 3 shows the link coordinate system of the five-axis articulated robot manipulator using the Denavit-Hartenberg convention. Table 1 illustrates the values of the kinematics parameters. The inverse kinematics of the articulated robot manipulator will transform the coordinates of robot manipulator from Cartesian space R 3 (x,y,z) to the joint space R 5 (   5  4  3  2  1 , , , , θ θ θ θ θ ) and it is described in detail by author of (Schilling, 1998). The computational procedure is as follows. Step2:

3.3.1The computation of the point-to-point motion trajectory
To consider the smooth motion when manipulating the robot, the point-to-point motion scheme with constant acceleration/deceleration trapezoid velocity profile is applied in Fig.2.
In this scheme, the designed parameters at each axis of robot are the overall displacement * i θ Δ (degree), the maximum velocity W i (degree/s), the acceleration or deceleration period T acc and the position loop sampling interval t d . Therefore, the instantaneous position command q at each sampling interval, based on the trapezoid velocity profile, can be computed as follows: Step 1: Compute the overall running time. First, compute the maximum running time without considering the acceleration and deceleration: This T 1 must exceed acceleration time T acc . The acceleration and deceleration design is then considered, and the overall running time is T= Max (T 1 , T acc ) + T acc (25) Step 2: Adjust the overall running time to meet the condition of a multiple of the sampling interval.
Where N represents the interpolation number and [.] refers to the Gauss function with integer output value. Hence, Step 3: Modify the maximum velocity Step 4 : Calculate the acceleration/deceleration value Step 5 : Calculate the position command, , at the mid-position a. Acceleration region: where t =n* t d , 0<n≤ N ′ , and 0 r q denotes the initial position. b. Constant speed region: where t =n* t d , 0<n≤N1, 1 r q denotes the final position in the acceleration region and c. Deceleration region: where t =n* t d , 0<n≤ N ′ and 2 r q denotes the final position in the constant speed region.
www.intechopen.com Therefore, applying this point-to-point motion control scheme, all joins of robot will rotate with simultaneous starting and stopping.

The computation of the linear and circular motion trajectory
Typical motion trajectories have linear and circular trajectory. a. Linear motion trajectory The formulation of linear motion trajectory is as follows:

angle between z-axis and path
vector, angle between x-axis and the vector that path vector projects to x-y plane, angle between y-axis and the vector that path vector projects to x-y plane, x-axis, y-axis and zaxis trajectory command, respectively. The running speed of the linear motion is determined by s Δ . b. Circular motion trajectory with constant value in Z-axis The formulation of circular motion trajectory with constant value in Z-axis is as follows: x , i y and i z are angle, angle increment, radius, x-axis, y-axis and z-axis trajectory command, respectively. The running speed of the circle motion is determined by ϕ Δ . Figure 4 illustrates the internal architecture of the proposed FPGA-based motion control IC for a five-axis robot manipulator. The motion control IC, which comprises a Nios II embedded processor IP and a motion control IP, is designed based on the SoPC technology, which is developed by Altera Cooperation.  Fig. 4 performs the function of motion command, computation of point-to-point motion trajectory as well as linear and circular motion trajectory in software. The clock frequency of the Nios II processor of is 50MHz. Figure 5 illustrates the flow charts of the main program, subroutine of point-to-point trajectory planning and the interrupt service routine (ISR), where the interrupt interval is designed with 10ms. However, point-to-point motion and trajectory tracking motion are run by different branch program. Because the inverse kinematics module is realized by hardware in FPGA, the computation in Fig. 5 needed to compute the inverse kinematics has to firstly send the x, y, z value from I/O pin of Nios II processor to the inverse kinematics module, wait 2µs then read back the * 5 * 1~θ θ . All of the controller programs in Fig. 5 are coded in the C programming language; then, through the complier and linker operation in the Nios II IDE (Integrated Development Environment), the execution code is produced and can be downloaded to the external Flash or SDRAM via JTAG interface. Finally, this execution code can be read by Nios II processor IP via bus interface in Fig. 4. Using the C language to develop the control algorithm not only has the portable merit but also is easier to transfer the mature code, which has been well-developed in other devices such as DSP device or PC-based system, to the Nios II processor.   The motion control IP implemented by hardware, as depicted in Fig. 4, is composed of a frequency divider, an inverse kinematics module, a five-axis position controller module, a five-axis speed estimated and speed controller module, five sets of QEP (Quadrature www.intechopen.com Encoder Pulse) module and five sets of PWM (Pulse Width Modulation) module. The frequency divider generates 50MHz (clk), 762Hz (clk_ctrp), 1525Hz (clk_ctrs) and 50MHz (clk_sp) to supply all module circuits of the motion control IP in Fig.4. To reduce the usage in FPGA, the FSM method is proposed to design the circuit in the position/speed controller module and the inverse kinematics module, as well as the VHDL is used to describe the behaviors. Figure 6(a) and 6(b) respectively describe the behavior of the single axis P controller in the position controller module and the single axis PI controller in the speed controller module. There are 4 steps and 12 steps to complete the computation of the single axis P controller and the single axis PI controller, respectively. And only one multiplier, one adder and one comparator circuits are used in Fig.6. The data type is 16-bit length with Q15 format and 2's complement operation. To prevent numerical overflow and alleviate windup phenomenon, the output values of I controller and PI controller are both limited within a specific range. The step-by-step computation of single axis controller is extended to five axes controller and illustrated in Fig.7. We can find that, it needs 20 steps to complete the computation of overall five axes P controllers in position control in Fig. 7(a), and 60 steps to complete the computation of overall five axes PI controllers in speed control in Fig. 7(b). However, in Fig.7(a) and Fig.7(b), it still need only one multiplier, one adder and one comparator circuits to complete the overall circuit, respectively. Furthermore, because the execution time of per step machine is designed with 20ns (clk_sp : 50MHz clock), the total execution time of five axes controller needs 0.4µs and 1.2 µs in position and speed loop control, respectively. And, the sampling frequency in position and speed loop control are respectively designed with 762 Hz (1.31ms) and 1525 Hz (0.655ms) in our proposed servo system of robot manipulator. It apparently reveals that it is enough time to complete the five axes controller during the control interval. Designed with FSM method, although five axes controllers need much computation time than the single axis controller, but the resource usage in FPGA is the same. The circuit of the QEP module is shown in Fig.8(a), which consists of two digital filters, a decoder and an up-down counter. The filter is used for reducing the noise effect of the input signals PA and PB. The pulse count signal PLS and the rotating direction signal DIR are obtained using the filtered signals through the decoder circuit. The PLS signal is a four times frequency pulses of the input signals PA or PB. The Qep value can be obtained using PLS and DIR signals through a directional up-down counter. Figure 8(b) and (c) are the simulation results for QEP module. Figure 8(b) shows the count-up mode where PA signal leads to PB signal, and Fig. 8(c) shows the count-down mode while PA signal lags to PB phase signal. The circuit of the PWM module is shown in Fig.9(a), which consists of two up-down counters, two comparators, a frequency divider, a dead-band generating circuit, four AND logic gates and a NOR logic gate. The first up-down counter generates an 18 kHz frequency symmetrical triangle wave. The counter value will continue comparing with the input signal u and generating a PWM signal through the comparator circuit. To prevent the short circuit occurred while both gates of the inverter are triggered-on at the same time; a circuit with 1.28µs dead-band value is designed. Finally, after the output signals of PWM comparator and dead-band comparator through AND and NOR logic gates circuit, four PWM signals with 18 kHz frequency and 1.28µs dead-band values are generated and sent out to trigger the IGBT device. Figure 9 Step2
are temporary variables. The computation of inverse kinematics in (41)~(54) by parallel operation method is resource consumption in FPGA; therefore, an FSM is adopted to model the inverse kinematics and illustrated in Fig. 10, then the VHDL is adopted to describe the circuit of FSM. The data type is 16-bit length with Q15 format and 2's complement operation. There are total 42 steps with 20ns/step to perform the overall computation of inverse kinematics. The circuits in Fig.10 need two multipliers, one divider, two adders, one component for square root function, one component for arctan function, one component for arcos function, one look-up-table for sin function and some comparators for atan2 function. The multiplier, adder, divider and square root circuit are Altera LPM (Library Parameterized Modules) standard component but the arcos and arctan function are our developed component. The divider and square root circuits respectively need 5 steps executing time in Fig. 10; the arcos and arctan component need 9 steps executing time; others circuit, such as adder, multiplier, LUT etc., needs only one step executing time. Furthermore, to realize the overall computation of the inverse kinematics needs 840ns (20ns/step* 42 steps) computation time. In our previous experience, the computation for inverse kinematics in Nios II processor using C language by software will take 5.6ms. Therefore, the computation of the inverse kinematics realized by hardware in FPGA is 6,666 times faster than those in software by Nios II processor. Finally, to test the correctness of the computation of inverse kinematics in Fig.10, three cases at Cartesian space (0,0,0), (200,100,300) and (300,300,300) are simulated and transformed to the joint space. The simulation results are shown in Fig. 11 that com_x, com_y, com_z denote input commands at Cartesian space, and those com_1 to com_5 denote output position commands at joint space. The simulation result in Figure 11 demonstrates the correctness of the proposed design method of the inverse kinematics. Finally, the FPGA utility of the motion control IC for robot manipulator in Fig. 4 is evaluated and the result is listed in Table 3. The overall circuits included a Nios II embedded processor (5.1%, 2,468 ALUTs) and a motion control IP (14.4%, 6,948 ALUTs) in Fig. 4, use 19.5% (9,416 ALUTs) utility of the Stratix II EP2S60. Nevertheless, for the cost consideration, the less expensive device -Stratix II EP2S15 (12,480 ALUTs and 419,329 RAM bits) is a better choice. In Fig. 4, the software/hardware program in parallel processing enhances the controller performance of the motion system for the robot manipulator.   Table 3. Utility evaluation of the motion control IC for robot manipulator in FPGA Figure 12 presents the overall experimental system which includes an FPGA experimental board, five sets of inverter, five sets rectifier, a power supplier system and a Mitsubishi Movemaster RV-M1 micro articulated robot. The micro articulated robot has five servo axes www.intechopen.com (excluding the hand) and its specification is shown in Fig.13. Each axis is driven by a 24V DC servo motor with a reduction gear. The operation ranges of the articulated robot are wrist roll ±180 degrees (J5-axis), wrist pitch ±90 degrees (J4-axis), elbow rotation 110 degrees (J3-axis), shoulder rotation 130 degrees (J2-axis) and waist rotation 300 degrees (J1-axis). The gear ratios for J1 to J5 axis of the robot are 1:100, 1:170, 1:110, 1:180 and 1:110, respectively. Each DC motor is attached an optical encoder. Through four times frequency circuit, the encoder pulses generate 800pulses/cycle at J1 to J3 axis and 380pulses/cycle at J4 and J5 axis. The maximum path velocity is 1000mm/s and the lifting capacity is 1.2kg including the hand. The total weight of this robot is 19 kg. The inverter has 4 sets of IGBT type power transistors. The collector-emitter voltage of the IGBT is rating 600V, the gate-emitter voltage is rating ±12V, and the collector current in DC is rating 25A and in short time (1ms) is 50A. The photo-IC, Toshiba TLP250, is used for gate driving circuit of IGBT. Input signals of the inverter are PWM signals from FPGA chip. The FPGA-Altera Stratix II EP2S60F672C5ES in Fig. 1(a) is used to develop a full digital motion controller for robot manipulator. A Nios II embedded processor can be download to this FPGA chip.  Fig.12 or Fig.4, the realization in PWM switching frequency, dead-band of inverter, position and speed control sampling frequency are set at 18k Hz, 1.28µs, 762 Hz and 1525 Hz, respectively. Moreover, in the position loop P controller design, the controller parameters at each axis of robot manipulator are selected with identical values by P-gain with 2.4. However, in the speed loop PI controller design, the controller parameters at each J1~J5 axis are selected with different values by [3.17, 0.05], [3.05, 0.12], [2.68, 0.07], [2.68, 0.12] and [2.44, 0.06], respectively. To confirm the effectiveness of the proposed motion control IC, the square-wave position command with ±3 degrees amplitude and 2 seconds period is firstly adopted to test the dynamic response performance. At the beginning of the step response testing, the robot manipulator is moved to a specified attitude for joints J1-J5 rotating at the [9 o , 40 o , 60 o , 45 o , 10 o ] position. Figure 14 shows the experimental results of the step response under these design www.intechopen.com conditions, where the rise time of the step responses are with 124ms, 81ms, 80ms, 151ms and 127 ms for axis J1-J5, respectively. The results also indicate that these step responses have almost zero steady-state error and no oscillation. Next, to test the performance of a point-topoint motion control for the robot manipulator, a specified path is run where the robot moves from the point 1 position, (94.3, 303.5, 403.9) mm to the point 2 position, (299.8, 0, 199.6) mm, then back to point 1. After through inverse kinematics computation in (17) 173 Pulses), respectively.

Experimental system and results
Additionally, a point-to-point control scheme with constant acceleration/deceleration trapezoid velocity profile adopts to smooth the robot manipulator movement. Applying this motion control scheme, all joins of robot rotate with simultaneous starting and simultaneous stopping time. The acceleration/deceleration time and overall running time are set to 256ms and 1s, and the computation procedure in paragraph 3.3.1 is applied. Figure 15 shows the tracking results of using this design condition at each link. The results indicate that the motion of each robot link produces perfect tracking with the target command in the position or the velocity response. Furthermore, the path trajectory among the position command, actual position trajectory and point-to-point straight line in Cartesian space R 3 (x,y,z) are compared, with the results shown in Fig. 16. Analytical results indicate that the actual position trajectory can precisely track the position command, but that the middle path between two points can not be specified in advanced. Next, the performance of the linear trajectory tracking of the proposed motion control IC is tested, revealing that the robot can be precisely controlled at the specified path trajectory or not. The linear trajectory path is specified where the robot manipulator moves from the starting position, (200, 100, 400) mm to the ending position, (300, 0, 300) mm, then back to starting position. The linear trajectory command is generated 100 equidistant segmented points from the starting to the ending position. Each middle position at Cartesian space R 3 (x,y,z) will be transformed to joint space (

Conclusion
This study presents a motion control IC for robot manipulator based on novel FPGA technology. The main contributions herein are summarized as follows. 1. The functionalities required to build a fully digital motion controller of a five-axis robot manipulator, such as the function of a motion trajectory planning, an inverse kinematics, five axes position and speed controller, five sets of PWM and five sets of QEP circuits have been integrated and realized in one FPGA chip. 2. The function of inverse kinematics is successfully implemented by hardware in FPGA; as the result, it diminishes the computation time from 5.6ms using Nios II processor to 840ns using FPGA hardware, and increases the system performance. 3. The software/hardware co-design technology under SoPC environment has been successfully applied to the motion controller of robot manipulator. Finally, the experimental results by the step response, the point-to-point motion trajectory response and the linear and circular motion trajectory response, have been revealed that based on the novel FPGA technology, the software/hardware co-design method with parallel operation ensures a good performance in the motion control system of robot manipulator. Compared with DSP, using FPGA in the proposed control architecture has the following benefits.