CORDIC FPGA Design for Hexapod Kinematics
CORDIC FPGA Design for Hexapod Kinematics
X w3
z z z C4:HV 0
0
w4
Figure 1. Hexapod-leg joints and systems.
𝜃1 𝜃1 w5
Equations (1), (2) and (3) provide the joints values
STAGE 4 STAGE 6
according to the end-effector Cartesian position [6] and the STAGE 5
l
length of links: l1 = 0.0275m, l2 = 0.0963m, l3 = 0.1051m [7]. w3
w4 C7:LV C8:HV C9:CV
Eq. (4), (5), (6), (7), (8), (9), (10) and (11) are variables for a 0 0 0
development by segments of the architecture.
F
(1)
𝜃2
𝜃1 = atan (𝑦𝑒 /𝑥𝑒 ) +
(2)
D
𝜃2 = 𝑎𝑡𝑎𝑛 (𝐺 ⁄√1 − 𝐺 2 ) − atan(sin 𝜃3 ⁄(𝐹 + D)) w1 iK
𝜃3 = 𝑎𝑡𝑎𝑛 (√1 − 𝐷2 ⁄𝐷) (3) w2 X C3:CV 0 C5:CR C6:CV
0 0 𝜃3 0
𝑟 = √𝑥𝑒 2 + 𝑦𝑒 2 (4) iKh
𝐴 = 2𝑙1 𝑟 (5) 𝜃3 𝜃3
𝐵 = √(𝑟 − 𝑙1 )2 + 𝑧𝑒 2 (6) w5 𝜃1 𝜃1 𝜃1
𝐶 = 𝑟 2 + 𝑧𝑒 2 + 𝑙1 2 − 𝑙2 2 − 𝑙3 2 − 𝐴 (7)
Figure 3. Fully-pipelined CORDIC-based architecture.
𝐷 = 𝐶 𝐶𝑎 = cos 𝜃3 (8)
𝐺 = 𝑧𝑒 ⁄𝐵 (9) Four types of CORDIC operators were considered: circular
𝐶𝑎 = 1⁄2𝑙2 𝑙3 (10) rotational [11], circular vectorial, hyperbolic vectorial [12] and
𝐹 = 𝑙2 ⁄𝑙3 (11) linear vectorial [13], whose functions and symbology are
represented in Table 2. In addition, circular rotational and
B. End-effector Trajectory circular vectorial can be implemented in the same CORDIC
module by changing the operation mode.
The surface of working area for an end-effector with a crab
angle 𝛼 ∈ [0,90]° for a quadruped walk [8] is showed at Figure TABLE II. CORDIC OPERATION REFERENCE
2. This is necessary because it defines the convergence range
of the CORDIC. Circular Rotational
Circular Vectorial
𝑥 𝐾√𝑥 2 + 𝑦 2
𝑦 CV 0
𝑧 CC 𝑧 + tan −1(𝑦⁄𝑥 )
100
Hyperbolic Vectorial
𝑥 𝐾′√𝑥 2 − 𝑦 2
𝑦 HV 0
𝑧 CC 𝑧 + tanh−1(𝑦⁄𝑥 )
100
Lineal Vectorial
𝑥 𝑥
𝑦 LV 0
Figure 2. Working area for hexapod-leg end-effector. 𝑧 CC 𝑧 + 𝑦⁄𝑥
100
B. Finite State Machine Architecture Figure 6 shows that Hyperbolic C8 and Linear C7 are far
In order to calculate the value of 𝑟 and 𝐷 which are not enough of the limit of their range of convergence in order not
CORDIC-dependent in (4) and (8), a FSM was designed at to be expanded. Hyperbolic C2, on the other hand, is slightly
stage two by using 2 multipliers and one adder as shown in closer to limit of its range of convergence, Hyperbolic basic
Figure 4. This FSM obtains these values in three clock cycles range of convergence is expanded in an interesting adaption
and sets outputs to the pipeline register. approach unlike Linear module which could be replaced by
another division method.
Stage 2 Reg 1
𝐶𝑎
MUX
Stage 1 Reg 0
𝑥 Stage 2 Reg 0
𝑥
MUX
+
Stage 1 Reg 1
MUX
For 𝑖 ≤ 𝑀:
𝑥𝑖+1 1 −𝛿𝑖 (1 − 2𝑖−𝑀−2 ) 𝑥𝑖
[𝑦 ] = [ ] [𝑦 ] (14)
𝑖+1 −𝛿𝑖 (1 − 2𝑖−𝑀−2 ) 1 𝑖
For 𝑖 > 𝑀:
𝑥𝑖+1 1 −𝛿𝑖 2𝑀−𝑖 𝑥𝑖
[𝑦 ] = [ ] [𝑦 ] (15)
Figure 5. Circular C3 CV Range of Convergence Analysis. 𝑖+1 −𝛿 2𝑀−𝑖
𝑖 1 𝑖
The implementation of bit-parallel iterative architecture is convergence 𝑎𝑡𝑎𝑛 parameters starting from the third
shown in Figure 7. position in memory. Since pipeline architecture makes
CORDIC modules start with the pipeline clock, all
instantiations are synchronized. As a result, all Circular
CORDIC modules shares the same look-up ROM
MUX
MUX
10
memory, thereby reducing hardware cost.
Y
0
sgn(Y) ADDR_A [4: 0] Dout_A [31: 0]
>>(i-M) 100 100 [4: 0]
ADDR_B 100 [31: 0]
Dout_B
MUX
IP CORE
100 100 ROM 100
>>(2+M-i) 𝑎𝑡𝑎𝑛
10 0 XOR
ENA
>>(i-M)
10
MUX
100
sgn(Y)
100 i<=M
100
100
>>(2+M+i)
Figure 10. RTL level for IP CORE ROM 𝑎𝑡𝑎𝑛. Total port read latency
from rising edge of read clock: 1 clock cycle.
100 XOR
sgn(Y)
100 10 Linear cordic: This is shown in Figure 11 (basic
X convergence) and requires 16 iterations/cycles for processing
MUX
MUX
iKinematics
[31:0] Wire 3
Read Enbl FIFO Full Figure 17. Test bench for timing.
Y Wire 4
Write Enbl Empty Figure 17 shows the clock signaling, described as follows:
Read_fifo: Sends the clock signal for FIFO modules to
Write Data [31:0] Read Data [31:0] read the next Inverse Parameters input data.
Wire 5
Stage_clock: Pipeline clock signal.
Read Enbl FIFO Full
Z
Wire 6 Read_clock: Clock signal reads FIFO output and records it
Write Enbl Empty in the first stage of pipeline input.
Start_clock: Cordic module starts signal clock.
Wire 7
Start
TABLE IV. IK ANGLES ERROR CALCULATION
AND
Wire 2
Wire 4 ROM Type 𝜽𝟏 𝜽𝟐 𝜽𝟑
Wire 6
ROM
LUT Percent Error (%) 0.004888 5.272004 0.001002
ROM
Data [31:0] Address [4:0] Deviation (Rads) 2.6451x10−7 3.9705x10−7 2.6451𝑥10−7
Wire 7
Figure 15. VLSI Architecture for Trajectories and IK.
A good approach to measure effective error in robot motion
is by using direct kinematics. Table 5 shows a Direct
C. Timing Analysis Kinematics parameters mean percentage error based on each
Another factor took into consideration was the clock hexapod robot gait. The architecture calculation gets great
generation. Regarding the architecture proposed, two timing accuracy on both Tripod and Pentapod gaits. On the other hand,
it gets slightly more errors in Quadruped gaits with a maximum APPENDIX
of 2.43%, which this represents a deviation of around 7 mm.
Algorithms and simulations:
TABLE V. EQUATIONS FOR OPERATOR ’S EXPANSION [Link]
nFU24oTn9zw3128cL
𝑷𝒆𝒓𝒄𝒆𝒏𝒕 𝑬𝒓𝒓𝒐𝒓 (%)
Gait
𝒙 𝒚 𝒛
Tripod 8.06451x10−4 4.62962x10−4 14.7633x10−4 ACKNOWLEDGMENT
Quadruped* 0.583768 0.583333 0.80507 This research work was supported by the Professional
−3 −3
School of Electronic Engineering of the Faculty of
Pentapod 2.98408x10 2.20439x10 3.41625x10−3
Engineering, Antenor Orrego Private University. The work
*The results for Quadruped 4+2 are the same. was performed at CIIE and was supported by rectoral
resolution N°009-2014-VIN-UPAO.
Figure 18 shows gaits accuracy, mentioned that Quadruped
gaits gets more errors, but this actually represents a maximum REFERENCES
derivation around 7 mm in the real gait, which can be
overlapped by actuators precision and may be improved in [1] D. Chávez, “Gait Optimization for Multi-Legged Walking Robots, with
Application to a Lunar Hexapod”, Ph.D. dissertation, Dept. Aeronautics
further works by approximation methods. Thus, the accuracy and Astronautics, Stanford University, USA, 2011, p. 6.
in Inverse Kinematics calculation showed is acceptable with [2] M. Cigola, A. Pelliccio, O. Salotto, G. Carbone, E. Ottaviano, M.
negligible error and deviation presented less than zero. Ceccarelli, “Application of robots for inspection and restoration of
historical sites”. In Proceedings of the International Symposium on
Automation and Robotics in Construction of the Conference, Ferrara,
Italy, 11–14 September 2005, p. 37.
[3] P. Gregorio, M. Ahmadi, M. Buehler. “Design, Control and Energetics
of an Electrically Actuated Legged Robot”. IEEE Trans. Systems, Man
and Cybernetics, vol. 27, pp. 626–634, Aug. 1997.
[4] D. Henrich, T. Höniger, “Parallel Processing Approaches in Robotics”,
IEEE International Symposium on Industrial Electronics, Guimarães,
Portugal, 7-11 July, 1997, pp. 2-6.
[5] M. Arora, R. Chauhan and L. Bagga, “FPGA Prototyping of Hardware
Implementation of CORDIC Algorithm”, International Journal of
Scientific & Engineering Research, vol. 3, Issue 1, January 2012, p1.
[6] W.C. Chen, C.S. Chen, F.C. Lee, Y.S. Kung, “Digital hardware
implementation of the forward/inverse kinematics for a SCARA robot
manipulator”, IEEE International Conference on Applied System
Invention (ICASI), Chiba, Japan, pp.54-57, April 2018.
[7] G. Evangelista, “Design and Modeling of a Mobile Research Platform
based on Hexapod Robot with Embedded System and Interactive
Control”, 23rd International Conference on Methods and Models in
Automation and Robotics, Międzyzdroje, Poland, 27-30 August, pp.
294-299, 2014.
Figure 18. Gate Accuracy. [8] G. Evangelista, D. Lázaro, “Translational Motion Analysis of a
Hexapod Walking Robot”, IEEE International Engineering Summit,
Finally, based on (16) (17) and in contrast to the previous Coatzacoalcos, Mexico, 29-31 October, pp. 167-172, 2013.
implementation in an ARM Cortex-M3, the calculation time [9] R. Sharma, P. Thakre, “A CORDIC Architecture Implementation for
Rectangular to Polar Conversion”, International Journal of Scientific
was reduced from 20 𝑚𝑠 at 96 MHz [8] to 5.04 𝑢𝑠 at 25 MHz Research in Science and Technology (IJSRST), vol. 4, no. 8, pp. 389-
for all legs. 395, Maharashtra, India, 2018.
[10] R.L. Chung, Y.Q. Zhang, S.L. Chen, “Fully pipelined CORDIC-based
𝐿𝑎𝑡𝑒𝑛𝑐𝑦 = 𝑆𝑡𝑎𝑔𝑒𝑠 × 𝑃𝑖𝑝𝑒𝑙𝑖𝑛𝑒 𝐶𝑙𝑜𝑐𝑘 = 5040 𝑛𝑠 (16) inverse kinematic FPGA design for biped robots”, Electronics Letters,
𝐶𝑎𝑙𝑐𝑢𝑙𝑎𝑡𝑖𝑜𝑛 vol. 51, no. 16, pp. 1241-1243, 2015.
𝑇ℎ𝑟𝑜𝑢𝑔ℎ𝑝𝑢𝑡 = 𝑃𝑖𝑝𝑒𝑙𝑖𝑛𝑒 𝐶𝑙𝑜𝑐𝑘 = 1 𝑐𝑎𝑙𝑐 / 840𝑛𝑠 (17) [11] S. Wang, V. Piuri, E. E. Wartzlander, “Hybrid CORDIC Algorithms”,
IEEE Transactions on Computers, vol. 46, no. 11, pp.1202-1207,
November 1997.
VII. CONCLUSIONS
[12] J. Zhou, Y. Dou, Y. Lei, J. Xu, Y. Dong, “Double Precision Hybrid-
This paper presents a high-speed, high-accuracy fully Mode Floating-Point FPGA CORDIC Co-processor”, 10th IEEE
pipelined architecture to solve inverse kinematics of an 3-DOF International Conference on High Performance Computing and
Communications, pp. 182-189, September 2008.
hexapod robot leg. The techniques used in this proposal [13] [Link], R. Harber and S. Bass, “Expanding the Range of Convergence of
presents benefits on scalability, high accuracy, high the CORDIC Algorithm”, IEEE Transactions on Computers, vol. 40,
adaptability, low latency and high throughput. no. 1, pp. 2-6, January 1991.
[14] Y. Luo, Y. Wang, H. Sun, Y. Zha, Z. Wang, H. Pan, “CORDIC-Based
Architecture for Computing Nth Root and Its Implementation”, IEEE
VIII. FUTURE WORKS Transactions on Circuits and Systems I: Regular Papers, vol. PP, no.
99, p. 1-13, June 2018.
We will work on applying the architecture developed in a [15] S. Aggarwal, P.K. Meher, “Reconfigurable CORDIC Architectures for
SoC to validate our algorithms in the hexapod platform Crixus Multi-mode and Multi-Trajectory Operations”, IEEE International
[7][8]. We will also obtain experimental data by adding control Symposium on Circuits and Systems (ISCAS), Melbourne, Australia, pp.
stages for the auto-correction of trajectory deviations based on 2490-2494, July 2014.
the 4 types of gait.