FPGA Implementation of a CORDIC-Based Joint Angle Processor for a Climbing Robot

Abstract

This paper presents a novel architecture of a joint angle processor for a robot arm. The objective of the proposed coordinate, rotation, digital computer (CORDIC)-based joint angle processor is to provide a hardware solution for computing the inverse kinematic for a robot arm control system. The complicated trigonometry operation is computed by the famous CORDIC algorithm. Simulation results show that the proposed joint angle processor achieves high precision. Moreover, an efficient pipelined architecture for very large scale integration (VLSI) and field programmable gate array (FPGA) implementation is also proposed, this architecture has the advantage of saving hardware cost and power consumption. As a result, the proposed CORDIC-based joint angle processor provides a high speed inverse kinematic computation that assists the main micro-control-unit (MCU) to operate the robot arm in real time. Therefore, the motion of the robot will be very smooth, capable of powering multiple joints at same time and provide smooth walking or climbing motions.

1. Introduction

In recent years, robotic researchers have focused on improving the functionality and flexibility of robot manipulators [1–6]. The commonly used robotic motion algorithms are presented in [7–10]. The kinematics of robot arms deals with the computation of the position, orientation and equivalent joint variables. These computations are carried out with respect to a fixed reference coordinate system regardless of the forces and moments that are responsible for the motion.

The kinematics of a robot arm consists of the forward and the inverse kinematics. The goal of the forward kinematic solution is to find the position and the orientation of an end effector of a manipulator in a reference coordinate system. The inverse kinematic, on the other hand, has to determine the joint angle vector by giving the position and orientation of the end effector. Since the trajectories to be traced are usually given in Cartesian coordinates, the computations of the inverse kinematics are highly complicated and required high speed implementation to generate smooth and rapid movements.

The well-known CORDIC algorithm [11–14], which has been applied with great success to hardware implementations of many signal processing tasks, such as sine and cosine generation, vector rotation, coordinate transformation and linear system solving, is suitable for the implementation of the inverse kinematic computation [15]. In the CORDIC algorithm, only simple shifters and adders are needed which can be implemented by the use of reconfigurable hardware platforms, especially by FPGA.

The objective of the proposed CORDIC-based joint angle processor is to provide a hardware solution for computing the inverse kinematic for a robot arm control system. The rest of the paper is organized as follows. In section 2, the configuration of a robot arm and its inverse kinematic is presented. In section 3, the VLSI architecture of the proposed CORDIC-based joint angle processor is proposed. In section 4, the FPGA implementation and the performance evaluation are given. Conclusion can be found in section 5.

2. Configuration, Kinematic and CORDIC algorithm

The configuration of the robot arm and its inverse kinematic is presented in this section. Figure 1 shows the mechanical structure of the robot arm, this robot arm consists of four smaller arms “A”, “B”, “C” and “D”. The joints are labelled as “f”, “g” and “h”. The rotatable base is labelled as “e”. The arm can be regarded as a planar four-link arm with three joints. The movements of the three joints allow the robot arm to do curling or extension motions.

Figure 1.

The mechanical structure of the robot arm

Figure 2 illustrates the link coordinate systems of the robot arm. ${\mathrm{L}}_{\mathrm{b}}$ , ${\mathrm{L}}_{\mathrm{c}}$ and ${\mathrm{L}}_{\mathrm{d}}$ denote the lengths of the smaller arms. ${\theta }_{\mathrm{f}}$ , ${\theta }_{\mathrm{g}}$ and ${\theta }_{\mathrm{h}}$ denote the angles of the joints “f”, “g” and “h”, respectively. ϕ denotes the angle of the rotatable base.

Figure 2.

The link coordinate system of the robot arm

Denoted by $\mathrm{\Theta }={[{\theta }_{\mathrm{f}},{\theta }_{\mathrm{g}},{\theta }_{\mathrm{h}},\varphi ]}^{\mathrm{T}}$ is the vector of angles of the joints and rotatable base and by ${\mathrm{P}}_0={[{\mathrm{X}}_0,{\mathrm{Y}}_0,{\mathrm{Z}}_0]}^{\mathrm{T}}$ is the vector of the end effector position in Cartesian coordinates. Then, the forward kinematic is given by:

${\mathrm{P}}_0=\mathrm{F}(\mathrm{\Theta })=\left[\begin{array}{c}-({\mathrm{L}}_{\mathrm{b}}\cos {\theta }_{\mathrm{f}}+{\mathrm{L}}_{\mathrm{c}}\cos ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}})+{\mathrm{L}}_{\mathrm{d}}\cos ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}}+{\theta }_{\mathrm{h}}))\cos \varphi \\ -({\mathrm{L}}_{\mathrm{b}}\cos {\theta }_{\mathrm{f}}+{\mathrm{L}}_{\mathrm{c}}\cos ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}})+{\mathrm{L}}_{\mathrm{d}}\cos ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}}+{\theta }_{\mathrm{h}}))\sin \varphi \\ {\mathrm{L}}_{\mathrm{b}}\sin {\theta }_{\mathrm{f}}+{\mathrm{L}}_{\mathrm{c}}\sin ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}})+{\mathrm{L}}_{\mathrm{d}}\sin ({\theta }_{\mathrm{f}}+{\theta }_{\mathrm{g}}+{\theta }_{\mathrm{h}})\end{array}\right]$

(1)

The inverse kinematic of the robot arm is the problem of solving joint angles when given the end effector position, which can be defined as

$\mathrm{\Theta }={\mathrm{F}}^{-1}({\mathrm{P}}_0)$

(2)

In the following context an approximate solution for the inverse kinematics is given. The inverse kinematic computation is simplified and conditioned in order to achieve high speed computation. First, the vector of the robot arm “C” is defined to be in parallel with the vector $\text{[}{\mathrm{X}}_0,{\mathrm{Y}}_0,{\mathrm{Z}}_0\text{]}$ . Second, the lengths of the arm “B”, “C” and “D” are defined to be equal ( ${\mathrm{L}}_{\mathrm{b}}={\mathrm{L}}_{\mathrm{c}}={\mathrm{L}}_{\mathrm{d}}=\mathrm{L}$ ). Therefore, the angles of the elbows “g” and “h” are equal ( ${\theta }_{\mathrm{g}}={\theta }_{\mathrm{h}}={\theta }_{gh}$ ). According to equation (1), we obtain:

$\mathrm{\Theta }=\left[\begin{array}{c}{\cos }^{-1}\left(\frac{{\mathrm{Z}}_0}{\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}\right)-{\cos }^{-1}\left(\frac{\mathrm{L}-\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}{2\cdot \mathrm{L}}\right)\\ {\cos }^{-1}\left(\frac{\mathrm{L}-\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}{2\cdot \mathrm{L}}\right)\\ {\cos }^{-1}\left(\frac{\mathrm{L}-\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}{2\cdot \mathrm{L}}\right)\\ {\tan }^{-1}\left(\frac{{\mathrm{Y}}_0}{{\mathrm{X}}_0}\right)\end{array}\right]$

(3)

Let the Cartesian coordinates, $\text{(}{\mathrm{X}}_0,{\mathrm{Y}}_0,{\mathrm{Z}}_0\text{)\hspace{0.17em}}$ and $\text{\hspace{0.17em}(}{\mathrm{X}}_{\mathrm{t}},{\mathrm{Y}}_{\mathrm{t}},{\mathrm{Z}}_{\mathrm{t}}\text{)}$ , represent the initial position and the target position of the end effector, respectively. Based on equation (3), the rotation angles of the joints and rotatable base can be derived as:

$\mathrm{\Delta }{\theta }_{gh}={\cos }^{-1}\left(\frac{\mathrm{L}-\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}{2\cdot \mathrm{L}}\right)-{\cos }^{-1}\left(\frac{\mathrm{L}-\sqrt{{\mathrm{X}}_{\mathrm{t}}^2+{\mathrm{Y}}_{\mathrm{t}}^2+{\mathrm{Z}}_{\mathrm{t}}^2}}{2\cdot \mathrm{L}}\right)$

(4)

$\mathrm{\Delta }{\theta }_{\mathrm{f}}={\cos }^{-1}\left(\frac{{\mathrm{Z}}_0}{\sqrt{{\mathrm{X}}_0^2+{\mathrm{Y}}_0^2+{\mathrm{Z}}_0^2}}\right)-{\cos }^{-1}\left(\frac{{\mathrm{Z}}_{\mathrm{t}}}{\sqrt{{\mathrm{X}}_{\mathrm{t}}^2+{\mathrm{Y}}_{\mathrm{t}}^2+{\mathrm{Z}}_{\mathrm{t}}^2}}\right)-\mathrm{\Delta }{\theta }_{gh}$

(5)

$\mathrm{\Delta }\phi ={\tan }^{-1}\left(\frac{{\mathrm{Y}}_0}{{\mathrm{X}}_0}\right)-{\tan }^{-1}\left(\frac{{\mathrm{Y}}_{\mathrm{t}}}{{\mathrm{X}}_{\mathrm{t}}}\right)$

(6)

When $\mathrm{\Delta }\varphi$ is greater than π, the rotatable base can rotate in the opposite direction.

$\mathrm{\Delta }\phi =\left(2\pi -\left|\mathrm{\Delta }\phi \right|\right)\cdot \left(\frac{-\mathrm{\Delta }\phi }{\left|\mathrm{\Delta }\phi \right|}\right)$

(7)

These kinematic solutions contain mainly trigonometric functions which have to be computed separately for each joint angle. These complicated functions are particularly suitable for CORDIC-based processors. The CORDIC algorithm is originally developed by Volder [11] for 2-D rotation and polar coordinate transformation and was later substantially improved by Walther [12]. It basically allows the computation of complicated functions using shift and add operations only and can be realized very efficiently in simple hardware. The CORDIC algorithm is described as follows: A rotation of angle β in the circular coordinate system or linear coordinate system can be obtained by performing a sequence of micro-rotations in the iterative manner. Specifically, a vector can be successively rotated by the use of a sequence of pre-determined step-angles: ${\alpha }_{\mathrm{m}}(\mathrm{i})=(1/\sqrt{\mathrm{m}}){\tan }^{-1}(\sqrt{\mathrm{m}}{2}^{-\mathrm{i}})$ . This methodology can be applied to generate many elementary functions in which only simple adders and shifters are required [11]. The conventional CORDIC algorithm is as follows:

$\mathrm{u}(\mathrm{i}+1)=\mathrm{u}(\mathrm{i})-\mathrm{m}\cdot \sigma (\mathrm{i})2^{-\mathrm{i}}\mathrm{v}(\mathrm{i})$

(8)

$\mathrm{v}(\mathrm{i}+1)=\mathrm{v}(\mathrm{i})+\sigma (\mathrm{i})2^{-\mathrm{i}}\mathrm{u}(\mathrm{i})$

(9)

$\mathrm{w}(\mathrm{i}+1)=\mathrm{w}(\mathrm{i})-\sigma (\mathrm{i})\alpha (\mathrm{i})$

(10)

${\alpha }_{\mathrm{m}}(\mathrm{i})=(1/\sqrt{\mathrm{m}}){\tan }^{-1}(\sqrt{\mathrm{m}}{2}^{-\mathrm{i}})$

(11)

where m denotes the circular (m=1) and linear (m=0) coordinate systems. $\sigma (\mathrm{i})\in \{-1,+1\}$ denotes the direction of the i-th micro-rotation, ${\sigma }_{\mathrm{i}}=sign(\mathrm{w}(\mathrm{i}))$ with $\mathrm{w}(\mathrm{i})\to 0$ in the vector rotation mode, ${\sigma }_{\mathrm{i}}=-sign(\mathrm{u}(\mathrm{i}))\cdot sign(\mathrm{v}(\mathrm{i}))$ with $\mathrm{v}(\mathrm{i})\to 0$ in the angle accumulated mode, the corresponding scale factor $\mathrm{k}(\mathrm{i})$ is equal to $\sqrt{1+{\sigma }^2(\mathrm{i})2^{-2\mathrm{i}}}$ and i=0,1,…,n-1. The product of all scale factors after n micro-rotations is given by:

${\mathrm{K}}_1={\displaystyle \prod _{\mathrm{i}=0}^{\mathrm{n}-1}\mathrm{k}(\mathrm{i})}={\displaystyle \prod _{\mathrm{i}=0}^{\mathrm{n}-1}\sqrt{1+\mathrm{m}\cdot 2^{-2\mathrm{i}}}}$

(12)

The iterations defined by equations (8)~(11) can be interpreted as rotations of a vector $[{\mathrm{u}}_{in},{\mathrm{v}}_{in}]$ on a circle (m=1) or along a line (m=0) into a new position given by $[{\mathrm{u}}_{out},{\mathrm{v}}_{out}]$ after n iterations. Another interpretation is simply rotational arithmetic which can compute various functions such as $\sin (\mathrm{x})$ , $\cos (\mathrm{x})$ , $\exp (\mathrm{x})$ , $\ln (\mathrm{x})$ , etc.

In order to compute more complicated functions such as ${\sin }^{-1}(\mathrm{x})$ and ${\cos }^{-1}(\mathrm{x})$ , the CORDIC algorithm of must be modified and it is presented as follows [16–18]:

$\mathrm{p}(\mathrm{i}+1)=\mathrm{p}(\mathrm{i})-\mathrm{s}(\mathrm{i})2^{-\mathrm{i}}\mathrm{q}(\mathrm{i})$

(13)

$\mathrm{q}(\mathrm{i}+1)=\mathrm{q}(\mathrm{i})+\mathrm{s}(\mathrm{i})2^{-\mathrm{i}}\mathrm{p}(\mathrm{i})$

(14)

$\mathrm{r}(\mathrm{i}+1)=\mathrm{r}(\mathrm{i})-2\cdot \mathrm{s}(\mathrm{i}){\tan }^{-1}{2}^{-\mathrm{i}}$

(15)

$\mathrm{t}(\mathrm{i}+1)=\mathrm{t}(\mathrm{i})+\mathrm{t}(\mathrm{i})\cdot 2^{-2\mathrm{i}}$

(16)

where $\mathrm{s}(\mathrm{i})\in \{-1,+1\}$ denotes the direction of the i-th micro-rotation, which is given by:

$\mathrm{s}(\mathrm{i})=\{\begin{array}{cc}sign(\mathrm{q}(\mathrm{i}))& if\hspace{0.17em}\mathrm{p}(\mathrm{i})\ge \mathrm{t}(\mathrm{i})\\ -sign(\mathrm{q}(\mathrm{i}))& otherwise\end{array}$

(17)

In order to evaluate the computation of ${\cos }^{-1}(\mathrm{x})$ for the proposed joint angle processor, the inputs of the modified CORDIC are ${\mathrm{p}}_{in}=1$ , ${\mathrm{q}}_{in}=0$ , ${\mathrm{r}}_{in}=0$ and ${\mathrm{t}}_{in}=\gamma$ as shown in Figure 3.

Figure 3.

The modified CORDIC arithmetic for the proposed joint angle processor

3. The Proposed Architecture of a CORDIC-Based Joint Angle Processor

In this section, the proposed CORDIC-based joint angle processor is presented. Figure 4 shows the block diagram of the proposed CORDIC-based joint angle processor, which consists of two CAU-Is (CORDIC arithmetic units type-I), four CAU-IIs (CORDIC arithmetic units type-II), an AAU (angle arithmetic unit), five adders and two shifters.

Figure 4.

The proposed CORDIC-based joint angle processor

Figure 5 shows the CAU-I consisting of two CORDIC processor As. Figure 6 shows the CORDIC processor A performing the CORDIC algorithm in the vector rotation mode of the circular coordinate system.

Figure 5.

The CAU-I (CORDIC arithmetic units type-I) consisting of two CORDIC processor As

Figure 6.

The CORDIC processor A

Figure 7 shows the CAU-II, which consists of a CORDIC processor B and a modified CORDIC processor. Figure 8 shows the CORDIC processor B performing the CORDIC algorithm in the vector rotation mode of the linear coordinate system. Figure 9 shows the modified CORDIC processor. Figure 10 shows the AAU performing the computation of equations (6) and (7).

Figure 7.

The CAU-II (CORDIC arithmetic unit type-II)

Figure 8.

The CORDIC processor B

Figure 9.

The modified CORDIC processor

Figure 10.

The AAU (angle arithmetic unit)

4. FPGA Implementation of CORDIC-based Joint Angle Processor

In this section, the proposed high accuracy architecture of the CORDIC-based joint angle processor is presented. The objective of the proposed CORDIC-based joint angle processor is to provide a hardware solution for computing the inverse kinematic for a robot control system. The functional block diagram of a robot control system is shown in Figure 11.

Figure 11.

The function block diagram of a robot control system

Figure 12 depicts the block diagram and circuit board of the architecture development and evaluation platform. In which, the microcontroller reads data and commands from a PC via a USB 2.0 bus, the field programmable gate array (FPGA) implements the proposed architecture of the joint angle processor, then sends the results of inverse kinematic computation to the servo driver. The joint angle processor stores the coordinates of the target position and uses them as the initial position of next motion.

Figure 12.

The block diagram and circuit board of the architecture development and verification platform

The hardware code in portable hardware description language runs on a PC with the logic circuit simulator [19] and FPGA compiler [20]. The throughput can be improved by using the proposed pipelined architecture while the computation accuracy is the same as that obtained by using the conventional architecture with the same word length. Thus, the proposed joint angle processor improves the power consumption and performance significantly. Moreover, all the control signals are internally generated on-chip. The proposed joint angle processor provides high accuracy inverse kinematic computations.

The estimated error is close to $2\cdot \left(2^{-\mathrm{b}}+2\cdot \mathrm{b}\cdot 2^{-\mathrm{b}}\right)$ , where b denotes the word length [21]. Figure 13 shows the estimated error versus various word lengths. Figure 14 shows the motion of the desired path (red line) and the actual path (blue line) of the end effector when driven by the proposed 12-bit CORDIC-based joint angle processor. The error between the desired and actual motion path is within the estimation.

Figure 13.

The estimated errors versus various different word lengths

Figure 14.

The motion of the desired path (red line) and the actual path (blue line) of the end effector when driven by the proposed 16-bit CORDIC-based joint angle processor

5. Conclusion

In this paper, we present a novel CORDIC-based joint angle processor with pipelined architecture. Circuit evaluation shows that the proposed high performance architecture has the advantages of high precision, a high data rate and simple hardware. The main advantage of our robot arm control system is that the joint angle processor provides a high speed inverse kinematic computation that assists the main MCU to operate the robot arm in real time. Therefore, the motion of the robot will be very smooth, capable of powering multiple joints at same time and provide smooth walking or climbing motions.