FPGA Realization of Forward Kinematics and Inverse Kinematics for FiveAxis Articulated Robot Arm

This dissertation presents a study of the forward and inverse kinematics for a axis articulated robot arm based on Field Programmable Gate Array FPGA technology.Some trigonometric functi

Southern Taiwan University of Science

and Technology Graduate School of Electrical Engineering

I would like to express my deepest gratitude to all of my teachers in Department ofElectrical Engineering, Southern Taiwan University of Technology and Science, especiallyProf Ying-Shieh Kung for his patience and guidance throughout my research since 2009when I studied the Master degree until now when I have studied the Doctoral degree Hisguidance and inspiration have provided an invaluable experience that will help me in mycareer

I would also like to thank my lab-mates for their help and advice They are also thepeople who make me have unforgettable time and sweet memories in Taiwan

Finally, I am grateful to my family for their constant love and support, andencouragement

This dissertation presents a study of the forward and inverse kinematics for a axis articulated robot arm based on Field Programmable Gate Array (FPGA) technology.Some trigonometric functions using Look-Up Table (LUT) and Taylor series method areused in hardware implementation to speed up of tracking the motion trajectoriesapplied forforward kinematics and invers kinematics for five-axis articulated robot arm Firstly, theforward kinematics and inverse kinematics of five-axis articulated robot arm are derived.Secondly, the computations algorithms and its hardware implementation are described.Thirdly, Very high speed integrated circuits Hardware Description Language (VHDL) isapplied to describe the overall hardware behavior of forward and inverse kinematics.Additionally, Finite State Machine (FSM) is applied for reducing the hardware resourceusage Further, to verify the correctness of the forward and inverse kinematics for five-axisarticulated robot arm, a co-simulation work is constructed by Modelsim and MatlabSimulink The forward and inverse kinematics hardware is run by Modelsim and a testbench which generates stimulus to Modelsim and displays the output response that is taken

five-in Simulfive-ink Under this design, the combfive-ination of the forward and five-inverse kfive-inematicssimulation for tracking the motion trajectories is adopted Fourthly, the design of forwardand inverse kinematics IPs for five-axis robot arm is implemented by a single FPGA.Additionally, a Nios II processor can be embedded into FPGA to construct a System on aProgrammable Chip (SoPC) developing environment Programs in Nios II processor arecoded in C language and IPs digital hardware is described by VHDL The Man-MachineInterface (MMI) developed by Visual Basic language which displays the results ofcomputations kinematics in FPGA into decimal number for easy checking the correctness

of results Therefore, the digital hardware/software co-design based on the SoPCis suitablefor the development of the forward and inverse kinematics for five-axis articulated robotarm Finally, an experiment system has been built up as well as some experimental resultshave been demonstrated to verify the effectiveness and correctness of computations forforward and inverse kinematic which is applied to the real five-axis articulated robot arm

軸模組化機械臂之研究。首先，推導五軸關節型機械手臂的前向運動學和逆向運動學。其次，針對演算法和硬體實現進行了描述。第三，超高速積體電路硬體描述語言（VHDL）被用於描述前向和反向運動學的整體硬體行為。此外，運用有限狀態機器（FSM）以減少硬體資源的使用。為了驗證五軸關節型機械手臂的前向和逆向運

Table of Content

Acknowledgments i

Abstract iii

摘要 iv

Table of Content v

List of Figures viii

List of Tables xi

List of Symbols xii

Chapter 1 Introduction 1

1.1 Research background & literature survey 1

1.2 The motivation of the study 6

1.3 The structure of thesis 7

Chapter 2 Mathematical description of kinematics and motion trajectories planning 8

2.1 Introduction of fiveaxis articulated robot arm 8

2.2 Review of kinematics 10

2.2.1 Rotating coordinate system 12

2.2.2 Homogeneous coordinates 13

2.2.3 Coordinates architecture 15

2.2.4 Rotary joint coordinates architecture 16

2.3 Robot kinematics of fiveaxis articulated robot arm 18

2.3.1 Forward kinematics 20

2.3.2 Inverse kinematics 24

2.4 The computation of pointtopoint motion control 29

2.4.1 Five axes trajectory planning 32

2.4.2 The formulas of motion trajectories 33

2.4.2.1 Linear motion trajectory 34

2.4.2.2 Circular motion trajectory 34

2.4.2.3 Star motion trajectory 35

2.4.2.4 Window motion trajectory 36

Chapter 3 Hardware implementation of forward kinematics and inverse kinematics 38

3.1 Introduction and literature review 38

3.2 Review of VHDL and Qformat design 42

3.3 An example of Sum of Product 44

3.4 Trigonometric functions 49

3.4.1 Computation algorithm of Sine and Cosine functions 49

3.4.2 Computation algorithm of Arctangent function using Taylor series expansion 50

3.4.3 Computation of Arccosine function using Taylor series expansion method 54

3.5 Design of hardware implementation for forward kinematics and inverse kinematics 57

3.5.1 Forward kinematics and inverse kinematics design in VHDL using Qformat 57

3.5.2 FSM for forward kinematics and inverse kinematics 58

-Chapter 4 Modelsim/Simulink co-simulation of forward/inverse kinematics for five-axis articulated robot arm 64

4.1 Introduction of Modelsim/Simulink cosimulation 64

4.2 CoSimulation cases using Modelsim/ Simulink 68

4.2.1 Sum of Product simulation results 69

4.2.2 Sine and cosine functions cosimulation results 70

4.2.3 Arctangent and arccosine functions cosimulation results 73

4.3 Modelsim/Simulink cosimulation of forward/inverse kinematics 75

4.4 Simulation results in Modelsim/Simulink of tracking motion trajectories 80

4.4.1 Linear motion trajectory 81

4.4.2 Circular motion trajectory 83

4.4.3 Star motion trajectory 83

4.4.4 Window motion trajectory 86

Chapter 5 FPGA realization of forward/inverse kinematics for fiveaxis articulated robot arm 89

5.1 Introduction 89

5.2 Description of SoPC builder design 90

5.2.1 DE2 115 board 90

5.2.2 Nios II embedded processor 93

5.3 FPGA implementation of forward kinematics and inverse kinematics 96

5.4 Applying to real robot arm 99

5.4.1 Hardware implementation system 101

5.4.1.1 CAN bus interface 101

5.4.1.2 Overall hardware system 103

5.4.2 Experimental results 105

5.4.2.1 Linear motion trajectory 106

5.4.2.2 Circular motion trajectory 107

5.4.2.3 Star motion trajectory 107

5.4.2.4 Window motion trajectory 108

Chapter 6 Conclusion and future works 113

6.1 Conclusion 113

6.2 Future works 114

References 115

Biography 123

Academic Publications 124

-List of Figures

Figure 2.1 Electrical · Rotary Actuators · Universal Rotary Actuators 9

Figure 2.2 The sectional diagram 9

Figure 2.3 The fiveaxis articulated robot arm 10

Figure 2.4 The definition of standard DenavitHartenberg link parameters [37] 11

Figure 2.5 The endeffector 13

Figure 2.6 The coordinate architecture 16

Figure 2.7 The location of threevectors n, o, a 16

Figure 2.8 Robot coordinates indicate 17

Figure 2.9The relationship coordinates between two joints 17

Figure 2.10 The schematic representation of forward and inverse kinematics 19

Figure 2.11 The link coordinate system of a fiveaxis articulated robot arm (general) 19

Figure 2.12 The link coordinate system of a fiveaxis articulated robot arm (details) 20

Figure 2.13 The base and first link coordinate schematic 21

Figure 2.14 The first link and the second link coordinate schematic 21

Figure 2.15The sencond link and the third link coordinates schematic 22

Figure 2.16The third link and the fouth link coordinates schematic 22

Figure 2.17 The fifth link and the fouth link coordinates schematic 23

Figure 2.18 LFPB trajectory (a) velocity; (b) acceleration 31

Figure 2.19 Minimumtime trajectory (a) velocity; (b) acceleration 32

Figure 2.20 Circular motion trajectory tracking 34

Figure 2.21 Star motion trajectory tracking 36

Figure 2.22 Window motion trajectory tracking 37

Figure 3.1 Parallel processing using three multipliers and two adders execute by one step 47

Figure 3.2Sequential processing using one multiplier and one adder execute by five step 47

Figure 3.3VHDL code for computing the sum of product 48

Figure 3.4Three cases for computing the sum of product 48

Figure 3.5FSM for computing the sine function 50

Figure 3.6 FSM for computing the cosine function 50

-Figure 3.7Compute a tan2( y / x )of each region in XY coordinates 53

-Figure 3.8The diagram to compute atan2(y/x) function 54

-Figure 3.9FSM to compute tan 1(x) function 54

-Figure 3.10Computing range of  cos 1(x) 55

Figure 3.11 Hardware implementation diagram for arccosine function 56

-Figure 3.12 FSM for computing the cos 1(x) function (range from 450to 900) 57

Figure 3.13Block diagram for (a) forward kinematics and (b) inverse kinematics 58

Figure 3.14 FSM for computing the Forward kinematics 61

Figure 3.15 FSM for computing the Inverse kinematics 62

Figure 3.16 Forward kinematics computations time in FPGA 63

Figure 3.17 Inverse kinematics computations time in FPGA 63

Figure 4.1Project flow 67

Figure 4.2Simulation flow working library 68

Figure 4.3 Modelsim cosimulation of Sum of Product with 16bit Q0 69

Figure 4.4 Modelsim cosimulation of Sum of Product with 16bit Q15 70

Figure 4.5 Modelsim cosimulation of Sum of Product with 16bit Q24 70

Figure 4.6 The cosimulation in Modelsim/Simulink of Sine function 72

Figure 4.7 The cosimulation in Modelsim/Simulink of Cosine function 72

Figure 4.8The cosimulation in Modelsim/Simulink of Arctangent function 74

Figure 4.9The cosimulation in Modelsim/Simulink of Arccosine function 74

Figure 4.10 Setting complier 76

Figure 4.11 Setting the “220pack.vhd” and “220model.vhd” compile to library “lpm” 76

Figure 4.12 Compiled all files successfully and run Modelsim 77

Figure 4.13Cosimulation architecture of forward kinematics using Modelsim/Simulink 77

Figure 4.14Cosimulation architecture of inverse kinematics using Modelsim/Simulink 78

Figure 4.15 Forward kinematics and Inverse kinematics cosimulation in Modelsim/Simulink 81

Figure 4.16The results of linear motion trajectory cosimulation in Modelsim/Simulink 82

Figure 4.17Error of linear motion trajectory cosimulation in Modelsim/Simulink 82

Figure 4.18 The results of circular motion trajectory cosimulation in Modelsim/Simulink 84

Figure 4.19 Error of circular motion trajectory cosimulation in Modelsim/Simulink 84

Figure 4.20 The results of star motion trajectory cosimulation in Modelsim/Simulink 85

Figure 4.21 Error of star motion trajectory cosimulation in Modelsim/Simulink 85

Figure 4.22 The results of window motion trajectory simulation in Modelsim/Simulink 86

Figure 4.23 Error of window motion trajectory simulation in Modelsim/Simulink 87

Figure 5.1 The architecture system of hardware implementation 90

Figure 5.2 The DE2115 board (top view) [76] 92

Figure 5.3 The Nios embedded processor 93

Figure 5.4 The Nios Development Tool Flow 94

Figure 5.5 A Nios II system implemented on the DE2 board [76] 95

Figure 5.6 Embedded components information flow 96

Figure 5.7 Architecture of forward kinematics IP and inverse kinematics IP base on FPGA 97

Figure 5.8 Design of forward kinematics IP, inverse kinematics IP and Nios II processor 98

Figure 5.9 Nios II IDE environments 98

Figure 5.10 The manmachine interface programmed in Visual Basic 99

Figure 5.11 Fiveaxis articulated robot arm by PowerCube 100

Figure 5.12 CANbus adaptor 101

Figure 5.13 Blockcircuit diagram of CANUSBMini module 102

Figure 5.14 Physical Connection for CAN bus [77] 103

Figure 5.15 Block of system architecture 104

Figure 5.16 Fiveaxis articulated robot arm and MMI 105

Figure 5.17 The feedback of window motion trajectory on MMI 105

Figure 5.18 The tracking response of linear motion trajectory by PowerCube robot arm 106

Figure 5.19 Tracking error of linear motion trajectory 107

Figure 5.20 The tracking response of circular motion trajectory by PowerCube robot arm 108

Figure 5.21 Tracking error of circular motion trajectory 109

Figure 5.22 The tracking response of star motion trajectory by PowerCube robot arm 109

Figure 5.23 Tracking error of star motion trajectory 110

Figure 5.24 The tracking response of window motion trajectory by PowerCube robot arm 110

Figure 5.25 Tracking error of window motion trajectory 111

-List of Tables

Table 2.1DenavitHartenberg parameters;their physical meaning, symbol and formaldefinition 12

Table 2.2 DenavitHartenberg parameters 20

Table 4 1The cosimulation Modelsim and Matlab Simulink of Sine function 73

Table 4.2The cosimulation Modelsim/Simulink of Cosine function 73

Table 4.3 The evaluation results for arctangent function 75

Table 4.4 The evaluation results for arccosine function 75

Table 4.5 Two cases simulation results of inverse kinematics from Modelsim and Matlab 79

Table 4.6 Two cases simulation results of forward kinematics from Modelsim and Matlab 79

Table 4.7 Mean square error of simulation results 88

Table 5.1 The datasheet of PowerCube modules type 100

Table 5.2 Mean square error of experimental results 111

-List of Symbols

a i The distance between the zi-1 and ziaxes along the xiaxis

 i The angle from zi-1axis to the ziaxis about the xiaxis

d i The distance from the origin of frame i-1 to the xi axis along the zi-1axis

t Period sampling time

W i The maximum velocity

Chapter 1Introduction

1.1 Research background&literature survey

Robotics is the branch of mechanical engineering, electrical engineering andcomputer science that deals with the design, construction, operation, and application ofrobotsas well as computer systems for their control, sensory feedback, and informationprocessing [1].Robotic control is currently an exciting and highly challenging researchfocus Several solutions for implementing the control architecture for robots have beenproposed in literature The common industrial manipulator is often referred to as a robotarm, with links and joints described in similar terms Manipulators which emulate thecharacteristics of a human arm are called articulated arms [2]

An articulated robot is a robot which is fitted with rotary joints Rotary joints allow afull range of motion, as they rotate through multiple planes, and they increase thecapabilities of the robot considerably An articulated robot can have one or more rotaryjoints, and other types of joints may be used as well, depending on the design of the robotand its intended function.With rotary joints, a robot can be engagedin very precisemovements Articulated robots commonly show up on manufacturing lines, where theyutilize their flexibility to bend in a variety of directions Multiple arms can be used forgreater control or to conduct multiple tasks at once, for example, and rotary joints allowrobots to do things like turning back and forth between different work areas

These robots can also be seen at work in labs and in numerous other settings.Researchers developing robots often work with articulated robots when they want to beengaged in activities like teaching robots to walk and developing robotic arms The joints inthe robot can be programmed to interact with each other in addition to activatingindependently, allowing the robot to have an even higher degree of control Many nextgeneration robots are articulated because this allows for a high level of functionality

Basic articulated robots are sometimes available in robotics kits, allowing people whoare just starting to explore robotics to play with rotary joints and to get a feel for how theyoperate More sophisticated robots may be built for a special purpose by robotics experts

who design every component of the articulated robot from the ground up Robots used onassembly lines and in similar settings are produced in various quantities by companieswhich develop manufacturing [3].

Modular robot arms are kinematic chains consisting of identical modules which can

be easily assembled or disassembled This gives the users control of the number of joints(orthe number of degrees of freedom) in the arm by simply adding or subtracting modules.Most modular robot arms are also reconfigurable, meaning that the user also has flexibility

in the orientation of each module This gives the user the ability to change the shape of thearm and the workspace (or reach) of the end-effector A modular robot arm that is notreconfigurable would be restricted to movement in a single plane, which would make itunsuitable for most applications of modular robot arms Modular, reconfigurable robotshave found applications wherever a robot arm with many degrees of freedom is needed,such as when the robot arm is expected to reach around an object or manoeuvre itself into aconfined space such as a pipe, duct, or small compartment Currently, there are threesuppliers for such robots: Amtec, Schunk, and OC Robotics Amtec offers its “PowerCube”line of modules for modular and reconfigurable robots These modules consist of two cubes,one cube being a servo motor while the other being a gearbox Schunk also offers modularand reconfigurable robots In Schunk’s robots, the modules are not the same size (e.g thelower modules are larger than the upper ones) OC Robotics is the only commercialsupplier of snake-arm robots, which are capable of continuous curvature Their robots work

by attaching three cables tothe top of each module and adjusting the tension in the cables inorder to position the modules

In the past, a major obstacle in the development of service robots suitable for seriesproduction was the price Robot solutions were too expensive even for high-end users Aclear potential for development is currently emerging particularly in the Schunk’s modularmechatronic systems In contrast to initial series production trials in Asia and the USA,which used cheap components for the mass market, Schunk’s industrial-quality componentsmake it a partner in the development of production-ready service robotics solutions in thecommercial research area An enormous store of modules permits high-quality solutions to

be constructed in modular form at comparative costs.Schunk’s PRL servo-electric rotary

actuator enables the creation of reconfigurable modular robot structures The individualPRL modules can be assembled with complete freedom and flexibility using connectingparts to produce an individual lightweight arm.The rotary actuator is set in motion by abrushless servo-motor with Harmonic Drive transmission, already incorporated with thecomplete power and control electronics It is capable of positioning moves with rampcontrol and features monitoring of the end positions, voltage, current and temperature.Thanks to the use of lightweight, high-strength materials, the compact rotary actuatorsachieve a weight/payload ratio exceeding 2:1 The power supply, control elements anduniversal communication interfaces are already integrated.Integrated and open: thanks tothe use of quill drives in the interior of the arm, all wiring as far as the pan-tilt unit, or

“wrist”, is done internally There are no visible cables and no interfering contours Theinternally routed CAN bus and the open software architecture means that every kind ofterminal device can be connected to and operated from the pan-tilt unit of the arm.Formaximum flexibility, the light-weight arm can also be battery-operated Control is donedirectly via PC or notebook (PCI or USB interface) Moreover, an external robot controller

is not required.The rotary module product line from Schunk offers a complete spectrum ofcompact swivel and rotary units for every handling task – and for fast and easy integration.Internal media feed-through guarantees reliable performance and minimizes interferencecontours Various sensor monitoring capabilities and multiple mounting options for allmodules increases the flexibility during plant engineering.Modular joint design andprecision design problems which aims for mapping the dual-arm robot to realize highmaneuverability and precision, has been a research topic in recent years A typical modularjoint design is the third generation of the DLR (German Aerospace Center in Germany)robot arm , it make the motor, brake, harmonic reducer and various sensors integrated inthe joint, and the development of new motor greatly improved motor performance and withimproved brake reduce the weight of the joints [4]

Robot control system has three categories, including distributed control using upperand lower machine two distributed architecture Distributed control systems of the varioussubsystems are independent and have the same functionality It has outstanding advantages:modularity, substitutability, timeliness, scalability These features are consistent with the

requirements of modular joints, so we chose a distributed control A distributed controlsystem, CAN bus is a kind of effective support distributed serial communication network,

so joint control interface using CAN bus

Kinematics studies the motion of bodies without consideration of the forcesormoments that cause the motion Robot kinematics refers to the analytical study ofthemotion of a robot arm Formulating the suitable kinematics modelsfor a robot mechanism isvery crucial for analyzing the behavior of industrial robot arms There are mainly twodifferent spaces used in kinematicsmodeling of robot arm namely, Cartesian space andQuaternion space Thetransformation between two Cartesian coordinate systems can bedecomposedinto a rotation and a translation.The robot kinematics can be divided intoforward kinematics and inversekinematics Forward kinematics problem is straightforwardand there is nocomplexity deriving the equations Hence, there is always a forwardkinematicssolution of anarm Inverse kinematics is a much more difficult problemthanforward kinematics [5] The solution of the inverse kinematics problemis computationallyexpansive and generally takes a very long time in the realtime control of robot arm.Singularities and nonlinearities that make theproblem more difficult to solve A.Bajo et al.[16] proposed a novel kinematic-based framework for collision detection and estimation ofcontact location along multisegment continuum robots in 2012 L Sciavicco [17] presented

a solution algorithm for the inverse kinematic problem for robotic manipulators whosethree end effector revolute joint axes intersect two-by-two in 1986 In 2009,G Antonellietal.[8] provided a convergence analysis of classical inverse kinematics algorithms forredundant robots S Kucuk et al [5] proposed an Inverse Kinematics Solutions ofFundamental Robot Manipulators with Offset Wrist in 2006 In 2008, V Ruiz de Angulo[9]adopted a technique to speed up the learning of the inverse kinematics of a robotmanipulator by decomposing it into two or more virtual robot arms

For the progress of Very Large Scale Integration (VLSI) technology, the FieldProgrammable Gate Arrays (FPGAs) has been widely investigated due to theirprogrammable hard-wired feature,fast time-to-market, shorter design cycle, embeddingprocessor, low power consumption and higher density for the implementation of the digitalsystem FPGA provides a compromise between the special-purpose Application Specified

Integrated Circuit (ASIC) hardware and general-purpose processors In 1998, Kabuka [10]applied two high performance floating-point signal processors and a set of dedicatedmotion controllers to build a control system for a six-joint robots manipulator Yasuda [11]adopts a PC-based microcomputer and several PIC microcomputers to construct adistributed motion controller for mobile robots Li et al [12] in 2003 utilized an FPGA(Field Programmable Gate Array) to implement autonomous fuzzy behavior control onmobile robot Oh et al [13] (2003) present a DSP (Digital Signal Processor) and a FPGA todesign the overall hardware system in controlling the motion of biped robots C.C Wong[14] presents a FPGA realization structure based on CORDIC is proposed to implementinverse kinematics for a biped robot in 2013.Sanchez, D.F et al [15] adopted an FPGAImplementation for Direct Kinematics of a Spherical Robot Manipulator in 2009.A neuralnetwork (NN)-based inverse kinematics problem of redundant manipulators subject to jointlimits is presented by Assal, S.F.M et al [16] in 2006 Tchon, K et al.[17] proposed anOptimal Extended Jacobian Inverse Kinematics Algorithms for Robotic Manipulators in

2008 Ben-Gharbia, K.M presented a method for identifying all the kinematic designs ofspatial positioning manipulators that are optimally fault tolerant in a local sense et al.[18] in

2013 Yi Min Zhao et al.[19] proposed an algorithm for the accurate path tracking ofindustrial robots in 2015 Ying-Shieh Kung et al [20] adopted an implementation ofinverse kinematics and servo controller for robot manipulator using FPGA in 2006.Based

on ARM Processor and FPGA Co-processor, kinematics control for a 6-DOF (Degree ofFreedom) space manipulator is realized by Zheng Yili et al [21] in 2008 Gac, K et al.[22]presented an application of field programmable gate arrays (FPGA) to support thecalculation of the inverse kinematics problem of a parallel robot in 2012 Boyin Ding etal.[23] adopted a hexapod robotic test system has been developed to enable complex sixdegree of freedom (6-DOF) testing of bones, joints, soft tissues, artificial joints and othermedical and surgical devices in 2011 Ching-Chih Tsai et al [24] presented a coarse-grainparallel deoxyribonucleic acid (PDNA) algorithm for optimal configurations of anomnidirectional mobile robot with a five-link robotic arm in 2011.However, these methodsonly adopt PC-based microcomputer or the DSP chip to realize the software part or adoptthe 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 motioncontrol architecture of robot system.

Hence, many practical applications in industrial control [25], multi-axis motioncontrol [26] and robotic control [14-20] have been studied Therefore, for speeding up thecomputational power, the forward and inverse kinematics based on VHDL is studied in thisdissertation And the VHDL is applied to describe the overall behavior of the forward andinverse kinematics

1.2 The motivation of the study

The kinematics problem is an important study in the robotic motion control Themapping from joint space to Cartesian task space is referred to as forward kinematics andmapping from Cartesian task space to joint space is referred to as inverse kinematics[2].Because of the complexity of inverse kinematics, it is usually more difficult than forwardkinematics to find the solutions [27-30].In addition, when the robot armexecutes a motioncontrol, the complicated inverse kinematics computation consumes much CPU time and itcertainty slows down the motion performance of robot arms Therefore, solving theproblem of implementation of forward kinematics and inverse kinematics becomes animportant issue

For the progress of Very Large Scale Integration (VLSI) technology, the FieldProgrammable Gate Arrays (FPGAs) has been widely investigated due to theirprogrammable hard-wired feature, fast time-to-market, shorter design cycle, embeddingprocessor, low power consumption and higher density for the implementation of the digitalsystem FPGA provides a compromise between the special-purpose Application SpecifiedIntegrated Circuit (ASIC) hardware and general-purpose processors In recent years, anElectronic Design Automation (EDA) Simulator Link, which can provide a co-simulationinterface between MALTAB/Simulink [31] and HDL simulators-Modelsim [32], has beendeveloped and applied of design the control system [32-33] Using it you can verify aVHDL, Verilog, or mixed-language implementation against your Simulink model orMATLAB algorithm In MATLAB/Simulink environment, it can generate stimuli toModelsim and analyze the simulation’s responses [31]

In this dissertation, a co-simulation by EDA Simulator Link is applied to theproposed forward kinematics and inverse kinematics hardware Some simulation resultsbased on EDA Simulator Link will demonstrate the correctness and effectiveness of theforward and inverse kinematics.Furthermore, an experiment system has been set up asexpectto some simulations and experimental results; it has demonstrated to verify theeffectiveness and correctness of the proposed FPGA-based on computations of forwardkinematics and inverse kinematicsIPs which isapplied to five-axis articulatedrobot arm byPower Cubevia CAN-bus interface to display the results on MMIsuccessfully.

1.3 The structure of thesis

The dissertation is divided into six chapters The outlines of chapters are as follow

Chapter 1: Introduction

Chapter 2:The mathematical description of the kinematics and motion trajectory planning Chapter 3: Hardware implementation of forward kinematics and inverse kinematics

Chapter 4:Modelsim/Simulink co-simulation of forward/inverse kinematics for five-axis

articulated robot arm

Chapter 5:FPGA-realization of forward/inverse kinematic for five-axis articulated robot

arm

Chapter 6: Conclusion and future works.

Chapter2Mathematicaldescription ofkinematicsandmotiontrajectoriesplanning

andmotiontrajectoriesplanning

This chapter presents the mathematicaldescription of the forwardkinematics andinverse kinematics and its motion trajectory planningfor five-axis articulated robotarm

2.1 Introductionof five-axis articulated robot arm

A robot manipulator is an electronically controlled mechanism, consisting ofmultiple segments, that performs tasks by interacting with its environment They are alsocommonly referred to as robotic arms All their joints are rotary (or revolute) Arepresentative articulated robot is Power Cube Modular robot

Simulation and Control of Reconfigurable Modular Robot Arm Based onCloseLoop Real-Time Feedback by Jun Zhou [34] in 2010 R.Wei et al [35] adopted aDistributed Hardware-in-the-Loop Simulation for Space Robot on CAN Bus in 2006

The rotary module product line from SCHUNK offers a complete spectrum ofcompact swivel and rotary units for every handling task – and for fast and easy integration.Internal media feed-through guarantees reliable performance and minimizes interferencecontours Various sensor monitoring capabilities and multiple mounting options for allmodules increases the flexibility during plant engineering Strong arguments for rotarymodules from SCHUNK:Short swiveling times, continuously adjustable end positions,Lockable intermediate position, Fast and easy integration, Pneumatic, electric or hydraulic[4]

The rotary actuator is equipped with a Harmonic Drive precision gear, which isdriven directly by a brushless DC servo-motor.The PRL rotary actuator is electricallyactuated by the fully integrated control and power electronics In this way, the module doesnot require any additional external control units A varied range of interfaces, such asProfibus DP, CAN-Bus or RS-232 are available as methods of communication Thisenables you to create industrial bus networks, and ensures easy integration in controlsystems You can make use of our hybrid cables for conveying the supply voltage and forcommunication If you wish to create combined systems (e.g a rotary gripping module),

various other modules from our PowerCube series are at your disposal.The position of themotor shaft is always shown in the drawing in the zero position (0°) From here, it can berotated clockwise and anti-clockwise The turning range may exceed 360° several times,depending on the type of application After switching on the module reports its position as

an absolute value (last position before switch off).Swiveling times are purely the times ofthe output cube to rotate from rest position to rest position Relay switching times or SPCreaction times are not included in the above times and must be taken into considerationwhen determining cycle times Load-dependent rest periods may have to be included in thecycle time [4]

Figure 2.1Electrical · Rotary Actuators · Universal Rotary Actuators

Figure 2.2 Thesectional diagramFig.2.1 shows the five-axisarticulated robot arm PowerCubebySchunk Company,Germany The sectional diagram in the Fig.2.2 shows the modular contains of parts inside.The five-axis articulatedrobot arm applied by five modules is shown in the Fig.2.3

○1 Complete performanceand control electronics

○2 Integrated brake

○3 Brushless servo-motor

○4 Harmonic Drive® gear

Figure 2.3 The five-axisarticulated robot arm

2.2 Review of kinematics

The study of robot arm involves dealing with the positions and orientations of theseveral segments that make up the arm This module introduces the basic concepts that arerequired to describe these positions andorientations of rigid bodies in space and performcoordinate transformations.In order to make the end-effector of robot arm can be moreaccurate to reach the target, firstly, carry on forward kinematics and inverse kinematics tofind out the solution It is an important issue to control the robot arm using thekinematics.Forward kinematics is the method for determining the orientation and position

of the end effector, given the joint angles and link lengths of the robot arm.Inversekinematics is the opposite of forward kinematics This is when you have a desired endeffector position, but need to know the joint angles required to achieve it [36]

There are many ways to represent rotation, including the following: Euler angles,Gibbs vector, Cayley-Klein parameters, Pauli spin matrices, axis and angle, orthonormalmatrices, and Hamilton’s quaternions Of these representations, homogenoustransformations based on 4x4 real matrices (orthonormal matrices) have been used mostoften in robot kinematics [5] In this dissertation, we use the Denavit-Hartenbergconvention to solve the kinematics problems

Figure 2.3 The five-axisarticulated robot arm

2.2 Review of kinematics

The study of robot arm involves dealing with the positions and orientations of theseveral segments that make up the arm This module introduces the basic concepts that arerequired to describe these positions andorientations of rigid bodies in space and performcoordinate transformations.In order to make the end-effector of robot arm can be moreaccurate to reach the target, firstly, carry on forward kinematics and inverse kinematics tofind out the solution It is an important issue to control the robot arm using thekinematics.Forward kinematics is the method for determining the orientation and position

of the end effector, given the joint angles and link lengths of the robot arm.Inversekinematics is the opposite of forward kinematics This is when you have a desired endeffector position, but need to know the joint angles required to achieve it [36]

There are many ways to represent rotation, including the following: Euler angles,Gibbs vector, Cayley-Klein parameters, Pauli spin matrices, axis and angle, orthonormalmatrices, and Hamilton’s quaternions Of these representations, homogenoustransformations based on 4x4 real matrices (orthonormal matrices) have been used mostoften in robot kinematics [5] In this dissertation, we use the Denavit-Hartenbergconvention to solve the kinematics problems

Figure 2.3 The five-axisarticulated robot arm

2.2 Review of kinematics

The study of robot arm involves dealing with the positions and orientations of theseveral segments that make up the arm This module introduces the basic concepts that arerequired to describe these positions andorientations of rigid bodies in space and performcoordinate transformations.In order to make the end-effector of robot arm can be moreaccurate to reach the target, firstly, carry on forward kinematics and inverse kinematics tofind out the solution It is an important issue to control the robot arm using thekinematics.Forward kinematics is the method for determining the orientation and position

of the end effector, given the joint angles and link lengths of the robot arm.Inversekinematics is the opposite of forward kinematics This is when you have a desired endeffector position, but need to know the joint angles required to achieve it [36]

There are many ways to represent rotation, including the following: Euler angles,Gibbs vector, Cayley-Klein parameters, Pauli spin matrices, axis and angle, orthonormalmatrices, and Hamilton’s quaternions Of these representations, homogenoustransformations based on 4x4 real matrices (orthonormal matrices) have been used mostoften in robot kinematics [5] In this dissertation, we use the Denavit-Hartenbergconvention to solve the kinematics problems

A serial-link manipulator comprises a set of bodies, called links, in a chain andconnected by joints Each joint has one degree of freedom, either translational (a sliding orprismatic joint) or rotational (a revolute joint) Motion of the joint changes the relativeangle or position of its neighbouring links.

For a manipulator with N joints numbered from 1 to N, there are N +1 links, numbered from 0 to N Link 0 is the base of the manipulator and link N carries the end- effector or tool Joint i connects link i −1 to link i and therefore joint i moves link i A link is

considered a rigid body that defines the spatial relationship between two neighbouring joint

axes A link can be specified by two parameters, its length ai and its twist αi Joints are also described by two parameters The link offset di is the distance from one link coordinate frame to the next along the axis of the joint The joint angle θi is the rotation of one link

with respect to the next about the joint axis [37]

Fig.2.4 illustrates this notation The coordinate frame {i} is attached to the far (distal) end of link i The axis of joint i is aligned with the z-axis These link and joint parameters

are known as Denavit-Hartenberg parameters and are summarized in Table 2.1

Figure 2.4The definition of standard Denavit-Hartenberg link parameters [37]

base

joint i

Joint i+1

link i Link i-1

Following this convention; the first joint, joint 1, connects link 0 to link 1 Link 0 is the base of the robot Commonly for the first link d1=α1=0 but we could set d1>0 to

represent the height of the shoulder joint above the base In a manufacturing system thebase is usually fixed to the environment but it could be mounted on a mobile base such as a

space shuttle, an underwater robot or a truck The final joint, joint Nconnects link N−1 to link N Link Nis the tool of the robot and the parameters dNand aNspecify the length of the tool and its x-axis offset respectively The tool is generally considered to be pointed along the z-axis The transformation from link coordinate frame {i− 1} to frame {i} is defined in

terms of elementary rotationsand translations [37]

Table 2.1Denavit-Hartenberg parameters; their physical meaning, symbol and

formaldefinition[37]

Joint angle  i The angle between the xi-1and xiaxes about the zi-1axis

Link offset d i The distance from the origin of frame i-1 to the xiaxis along

the zi-1axisLink length a i The distance between the zi-1 and ziaxes along the xiaxis; for

intersecting axes is parallel to ̂i-1x ̂i

Link twist  i The angle from zi-1axis to the ziaxis about the xiaxis

The parameters αiand aiare always constant For a revolute joint θiis the joint variable and diis constant, while for a prismatic joint diis variable, θiis constant and αi=0.

2.2.1Rotating coordinate system

The end-effector which suppose as a known coordinate system at the point (x, y, z),

if the z-axis as a rotation then after rotating of the shaft angle coordinates into α (x ', y', z '),

the system architecture is shown in Figure 2.5 The point coordinates are derived asfollowing [37-38]

Figure 2.5 Theend-effectorFirstly, according to the trigonometric functions can be learned as

Coordinate point (x, y, z) in the Z-axis as the rotation axis, after a rotation angle α, the value

of x ', y' calculated as follows

We know as cosθ = x/l and sinθ = y/l, then Eq.2.5 and Eq.2.6 can be expressed as follows:

x

y

Suppose that the definition of displacement transformation matrix, displacement

vector will be placed in the fourth column of Eq.2.12, where a represents the amount of displacement along the x-axis direction, b represents the amount of displacement along the y-axis direction, c represents the z-axis direction displacement amount The displacement

transfermatrix is expressed as follows:

1 0 0

0 1 0( , , )

0 0 1

0 0 0 1

a b Trans a b c

2.2.3Coordinates architecture

When a conversion matrix is used to describe the relationship between twocoordinate systems which their coordinate architecture intentionally ambiguity as two

coordinate systems shown as Fig.2.6 Where U is defined as the gravity coordinate system,

R is robot coordinate system, His mechanical arm gripper coordinate system, E is the tool

coordinate system, Pis a workspaces holder marking system Assuming robot gripper grab

a drill bit for drilling a workspaces, suppose that the bit is set on the gripper as the target

transformation matrix (T) for H T E, drill relative to the gravity coordinate transformation

matrix U T R R T H. H T E = U T P. P T Eand the workspaces to be the coordinates system H T E =

U T P. P T Eso that we can obtain

Figure 2.6 Thecoordinate architecture

The three-vectorn, o, a,and pare defined as in Fig.2.7 The approach vector of the end-effector is “a”;the orientation vector “o”is the directionspecifying the orientation of the hand, from fingertip to fingertip The normal vector “n”is chosen to complete the definition of a right-handed coordinatesystem so the direction of vector “n” is chosen by the multiplen = o xa.

Figure 2.7The location of three-vectors n, o, a

2.2.4Rotary joint coordinates architecture

On establishment of the mechanical arm model can be regarded as the link with thejoint are connected to each other, which is based on the relative position of the link between

the homogeneous matrix An represented In Fig.2.8, for example, which the A1 to a link

transformation matrix relative to the baseA0, and A2 is the second link with respect to the

Arm base

transformation matrix A of the link, so on, and then R T H transformation matrix by A1to A5

Figure 2.8 Robot coordinates indicate

Figure 2.9The relationship coordinates between two jointsSuppose that a mechanical arm in Fig.2.8, and coordinate between joint 1 and joint

0 in Fig.2.9, which transforms the relationship between the joint and joint is expressed asfollows [39]:

Firstly, the angle  i between the xi-1and xiaxes about the zi-1 axis; secondly, the

distance dfrom the origin of frame i-1 to the xiaxis along the zi-1axis; thirdly, the distance ai

between the zi-1 and zi axes along the xiaxis and lastly, is the angle  i from zi-1axis to the

z iaxis about the xiaxis So its conversion formula is expressed as

),(),(),(),(),

i i

i i

0 0

cos sin

0

sin cos

sin cos

cos sin

cos sin

sin sin

cos cos

1 0 0

0

0 cos sin

0

0 sin cos

0

0 0 0

1

1 0 0 0

0 1 0 0

0 0 1 0

0 0 1

1 0 0 0

0 1 0 0

0 0 cos sin

0 0 sin cos

1 0

1

0

0 0

0

1

) , ( ) , ( ) , ( )

,

(

1

i i

i

i i i i i

i i

i i i i i

i i

i i

i i

i i

i

i i

i

i

i

d a a

a d

X T a X T Z T

2.3Robot kinematics of five-axis articulated robot arm

The robot kinematics can be divided into forward kinematics and inverse kinematics.Forward kinematics problem is straightforward and there is no complexity deriving theequations Hence, there is always a forward kinematics solution of a robot arm Inversekinematics is a much more difficult problem than forward kinematics The solution of theinverse kinematics problem is computationally expansive and generally takes a very longtime in the real time control of robot arm Singularities and nonlinearities that make theproblem more difficult to solve Hence, only for a very small class of kinematic simplemanipulators (manipulators with Euler wrist) have complete analytical solutions(Kucuk&Bingul, 2004) The relationship between forward and inverse kinematics isillustrated in Fig.2.10

Figure 2.10The schematic representation of forward and inverse kinematics

The reconfigurable modular robot herein is constructed by different type ofPowerCube module from Schunk cooperation [5] The link coordinate system of a five-axisarticulated robot arm is generally shown in Fig.2.11

Figure 2.11The link coordinate system of a five-axis articulated robot arm (general)

A typical five-axis articulated robot arm is studied in this dissertation Fig.2.12shows its link coordinate system byDenavit-Hartenberg convention [37].Table 2.2 illustratesthe values of the kinematics parameters The forward kinematics of articulated robot arm isthe transformation of joint space R5 (1,2,3,4,5 ) to Cartesian space R3(x, y,z).

Conversely, the inverse kinematics of the articulated robot arm will transform thecoordinates of robot manipulator from Cartesian space R3 (x,y,z) to the joint space

R5(1,2,3,4,5).The link coordinate system of a five-axis articulated robot arm in details

is shown in Fig.2.12

Forward kinematics

Inverse kinematics

Cartesian space

Figure 2.12 The link coordinate system of a five-axis articulated robot arm (details)

Table 2.2Denavit-Hartenberg parameters

Accordingly, the computational procedure of forward kinematics and inversekinematicsissolving step by step as follows:

2.3.1 Forward kinematics

A robot arm is composed of serial links which are affixed to each other revoluteorprismatic joints from the base frame through the end-effector Calculatingthe position andorientation of the end-effector in terms of the joint variablesis called as forward kinematics

In order to have forward kinematics for arobot mechanism in a systematic manner, oneshould use a suitable kinematicsmodel Denavit-Hartenberg method that uses fourparameters is the mostcommon method for describing the robot kinematics These

parameters ai-1,αi-1, di and θi are the link length, link twist, link offset and joint angle,

respectively.A coordinate frame is attached to each joint to determine DH parameters.Ziaxis

of the coordinate frame is pointing along the rotary or sliding directionof the joints [11]

2 / 4

i

1 2 3 4 5

a) Link 1 to link 0; using Eq2.7 substitute d=d1, a=0, α=-90ointo Eq.2.19, we have

( , ) ( , ) ( , ) ( , )

a a

c) Link 3 to link 2; using Eq.2.7substitute d=0, a=a3, α=0ointo Eq.2.19, we obtain

2

3

( , ) ( , ) ( , ) ( , )

a a

Figure 2.15The sencond link and the third link coordinates schematic

d) Link 4 to link 3; using Eq.2.7substitute d=0, a=0, α=-90ointo Eq.2.19, we have

Figure 2.16The third link and the fouth link coordinates schematic

e) Link 4 to end effector, using Eq.2.7 substituted=d5, a=0, α=0ointo Eq.2.19, we obtain

Figure 2.17The fifth link and the fouth link coordinates schematic

An alternative representation of 0A5can be written as

5

4 4

3 3

2 2

y y y y

x x x x H

R

p a o n

p a o n

p a o n A A A A

234   

The forward kinematic of five-axis articulated robot arm is summary as three steps:

Step 1: xcos1a2cos2 a3 cos23d5 sin234 (2.40)

Step 2: ysin1a2 cos2 a3 cos23d5sin234 (2.41)

Step 3: z  d1  a2sin 2  a3sin 23  d5cos 234 (2.42)wherex, y and z are calculated as Px, Py and Pz, respectively

2.3.2 Inverse kinematics

The inverse kinematics problem of the serial manipulators has been studied formany decades Solving the inverse kinematics is computationally expansive and generallytakes a very long time in the real time control of robot arms Tasks to be performed by amanipulator are in the Cartesian space, whereas actuators work in joint space Cartesianspace includes orientation matrix and position vector However, joint space is represented

by joint angles The conversion of the position and orientation of a robot arm end-effectorfrom Cartesian space to joint space is called as inverse kinematics problems

The position vector p directs the location of the origin of the (n, o, a)frame which is

defined to let the end-effector of robot arm by always down position Therefore, the matrix

in Eq.2.25 is set by the following form:

0

10

0

01

0

00

1

z y

x

T H

R

(2.43)

Comparing the element (3, 3) in Eq.2.43with Eq.2.34, thus we have

2 1 2

Eq.2.52 above can be written as

])(sin)

1

2 1 2

1 ) (sin

)

Then, applyingEq.2.54 toEq.2.53,we obtain

An alternative representation of Eq.2.51is

23 3 2 2 5

2 2

2 23 3

2 2

2 5

sin sin

(

) cos cos 2

cos cos

( ) (

23 2

3 2 23 2 2 3 2 2

2

2

23 2

3 2 23 2 2 3 2 2 2 2

2 5

a

a a a

a z

23 2

3 2 23 2

3

2

23

2 23 2 2 3 2

2 2 2 2 2

2 5

1

2

sin sin 2

cos cos

2

) sin (cos

) sin (cos

a

a a

z d

23 2

3 2 23 2

3 2

2 3

2 2

2 5

) sin sin cos

(cos 2

)

3

2 2

2 5

2 )

3

2 2

2 5

2)

3

2 2

2 5

2 3

2 2

2 5 1 2

3

2

) (

cos

a a

a a z d d

2 3

2 2

2 5 1

2

1

) (

cos

a a

a a z d d

3