Servo system for robots

Proceedings of 2005 CACS Automatic Control Conference Tainan, Taiwan, Nov 18-19, 2005

Design and Implementation of a Servo System for Robotic Manipulator

*Ying-Shieh Kung, Chia-Sheng Chen, Gua-Shieh Shu

Department of Electrical Engineering, Southern Taiwan University of Technology, Yung-Kang, Tainan County, 710, Taiwan, *e-mail: [email protected]

Abstract A FPGA-based servo system for robotic manipulator is presented in this paper. The servo system includes a FPGA experimental board and five sets of inverter. The FPGA is a kernel of the servo system and the internal structure of this chip is designed with five-axis (J1~J5) position/speed control circuits, five sets of the PWM (Pulse Width Modulation) circuits and capture circuits for QEP (Quadrature Encoder Pulse), etc. The position control loop adopts a P controller and speed control loop uses a PI controller at each axis of the robotic manipulator. To reduce the usage of the logic elements (LEs) in FPGA, a finite state machine (FSM) method is applied. At last, an experimental system included by the proposed servo system and a Mitsubishi RV-M1 micro robot is set up and verified its correctness and effectiveness.

Key Word: FPGA, Robotic manipulator, Finite state machine, PWM. 1. Introduction

Robotic control is an exciting and high challenge

research work in recent year. Several solutions to the implementation of digital control system for robot manipulator and mobile robots are proposed in the literatures [1-5]. But, all of those techniques use the DSP chip or FPGA chip to realize the software part or hardware part of the robot control system.

Due to the advantages of their programmable hard-wired feature, short-to-market, high speed, and higher density for the implementation, FPGA (Field Programmer Gate Array) has brought more attention and become a popular research topic on digital control. Especially, the FPGA can be embedded a processor to build up a SoPC (System on a Programmable Chip) environment [6-8], which make the software and hardware can programmable design within a single chip. In this paper, a FPGA-based servo system for robotic manipulator under this SoPC environment is presented and shown in Fig.1. The FPGA is a kernel of the servo system and the internal structure of this chip is implemented by a Nios processor, five-axis (J1~J5) position/speed control circuits, five sets of the PWM (Pulse Width Modulation) circuits and capture circuits for QEP (Quadrature Encoder Pulse), etc. In the function of the proposed FPGA-based servo control IC, it can simultaneously receives five encoder signals of robotic manipulator, compares the position

command of each axis, through the computation of position and speed controller circuit then sends to generate the PWM signal to drive DC motors of robot and to let the robotic manipulator precise moving to the target position. Therefore, All of the function needed for building up a fully digital servo control, such as point-to-point motion control, position and speed loop controller, and PWM and QEP circuits for five-axis robotic manipulator can be integrated in a single FPGA chip. The FPGA chip here adopts Altera Stratix EP1P10, which has 10,570 Les, maximum 426 user I/O pins, 6 DSP blocks, total 900,448 RAM bits, and a Nios embedded processor which has a 32-bit configurable CPU core, 1 to 20Kbytes available on chip and maximum 4G bytes off-chip memory. 2. System description and design

The architecture of the servo control system for a five-axis robotic manipulator is shown in Fig. 1. The FPGA is a kernel device and the detailed design is as follows: 2.1 The mathematical model of robot arm with actuator

The dynamic equation of the n-link robot arm is given by: [9]

=+++ )()(),()( qGqFqqqVqqM m &&&&& (1) with )(qM inertial matrix, ),( qqVm & Coriolis /centripetal vector, )(qG gravity vector, )(qF & friction vector. The q , q& , q&& and denote the n-vector of the position, velocity, acceleration and generalized forces, respectively. The , q , q& and q&& are nR . The dynamics of the dc motors that drive the arm links are given by the n decoupled equations [9]

vKRFqBqJ MMMMM =+++ &&& (2) with

)(vec ),v(vcev

}R/K{diagK },r{diagR

}B{diag }R/KKB{diagB

}J{diagJ },q{vecq

ii

aMMi

iaMbM

MMMM

ii

iiii

ii

====

+===

(3)

where ia

R , ii , iv , ibK , iMK are resistance, current, voltage, voltage constant, current constant, respectively.


Inverter-4

Inverter-5

Inverter-2

Inverter-1

Inverter-3

J3-axis Motor

Encoder Signal

PWM Signal

J4-axis Motor

J5-axis Motor

J2-axis Motor

J1-axis Motor

PWM Signal

J1-axis

J2-axisJ3-axis

J4-axis

J5-axis

FPGA-based servo control IC

Five-axis Position control circuits using

Programmable Logic Device

Embedded Processor IP(Nios Processor)

Application IP

Figure 1. The architecture of FPGA-based servo system for a five-axis robotic manipulator

TheiM

J , iM

B , iM

, iL

are the inertial, viscous, generated torque of the motor, load torque for the ith axis of the robot arm, respectively. Combined equations (1) and (2), the dynamic equations including robot arm and dc motor can be obtained. At first, if the gear ratio of the coupling from the ith motor to the ith arm link is ir , which we can define as follows

iMiiqrq = or MqRq = (4)

Substituting equation (4) into (2), then into (1), it can obtain

vRKqGRqFRqRF qqqVRBqqMRJ

M22

Mm22

M

=++++++

)()(

)()),(())(( &&&& (5)

Due to the gear ratio ir is often small for obtaining to increase torque value on the commercial robot arm, it can help us to simply the formulations. From equation (5), the dynamic equation combining the motor and arm link is given by

iiia

MiMiiiiiiiM drvR

KrFrqBqmrJ

i

i

ii

22 )( =+++ &&& (6)

where

+++=ij kj

iikjjkijiji GFqqVqmd,

&&&& (7)

Therefore, when the gear ratioir is small, the

2ir term

in equation (6) can be neglected, equation (6) is simplified by

ia

MiMiiiiM vR

KrFrqBqJ

i

i

ii=++ &&& (8)

Substituting equation (3) and (4) into (8), the following equations can be given

ia

MLM

a

bMMMM vR

KTq

RKK

BqJi

i

ii

i

ii

iii=+++ &&& )( (9)

where iL

T is the external force or the disturbance force induced by other arm motion. Therefore, if the gear ratio is small, the control block diagram combining arm link, motor actuator and P controller in

position and PI controller in speed loop at each axis can be represented in Fig.2.

ii aaRsL +

1iM

Kii MM

BsJ +1

s1iMq&+

-

-+iMT

iLT

ii

ibK

+

-irpK

Position controller

e iMq iq

0 iaLwith

iM

q

FPGA chip thiDC motor and driver of the arm

+

-11 z

iv

Speed controller

1

1

1

+ zzKK ip

Figure 2. The block diagram of position controller for

ith robot arm

2.3 The FPGA-based servo control IC for robotic manipulator The internal architecture of the proposed servo

control IC for robotic manipulator is shown in Fig.3. The FPGA is developed by Altera Corporation and it can be embedded a Nios processor to construct a SoPC environment. The FPGA chip here adopts Altera Stratix EP1P10, which has 10,570 Les, maximum 426 user I/O pins, 6 DSP blocks, total 900,448 RAM bits, and a Nios embedded processor which has a 16-bit or 32-bit configurable CPU core, 1 to 20Kbytes available on-chip and maximum 4G bytes off-chip memory. In Fig.3, the servo control IC has two modules, and they are a Nios embedded processor and an application module. The Nios processor is used to perform the functions of the point-to-point motion control by software. The application module is used to realize the five-axis position control by hardware, and their circuits include a frequency divider, a five-axis position controller circuit, a five-axis speed controller circuit, five sets of QEP circuit, five sets of PWM circuit and a gripper control circuit. To reduce the usage of the logic elements (LEs) in FPGA, a finite state machine (FSM) method is applied to design the controller circuits of position loop and speed loop. The sampling frequency in position loop and speed loop are 762Hz and 1525Hz, respectively. At each sampling time, the step control sequence for computing the


controller output for five-axis position loop and five-axis speed loop are arranged in Fig. 4(a) and Fig. 4(b). In position loop, there are 4 steps to compute the P controller each axis and it is shown in Fig.5. In speed loop, there are 12 steps to estimate the speed value and compute the PI controller each axis, which is shown in Fig.6. Fig. 7 shows a PWM circuit and Fig. 8 is QEP circuit. The overall circuits included a Nios embedded processor (25%) and an application IP (37%) in Fig. 3 use 62% utility (LEs) of the Stratix EP1P10.

3. Experiments and results

The overall experimental system is depicted in Fig.1, and it includes a FPGA experimental board, five sets of inverter and a Mitsubishi Movemaster RV-M1 micro robot. The servo axis of this micro robot has five degrees of freedom (hand isnt included) in this arm, shown in Fig.9. Each axis is driven by a 24V dc servo motor with a reduction gear. The operation range of robot arm 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. The maximum path velocity is 1000mm/s and the lifting capacity is 1.2kg including hand. The total weight of this robot arm is 19 kg. The FPGA chip adopts Altera Cyclone EP1C20 which has 20,060 LEs, maximum 301 user I/O pins, total 294,912 RAM bits, and a Nios embedded processor can download to this FPGA chip which has a 16-bit or 32-bit configurable CPU core, 1 to 20Kbytes available on chip and maximum 4G bytes off-chip memory.

In implementation, the PWM switching frequency and dead-band of inverter, position control sampling frequency are designed with 18k Hz, 1.28s and 762

Hz, respectively. To confirm the effectiveness of the proposed servo control IC, the square-wave position command with 6 degrees amplitude and 2 seconds period is used to test the dynamic response performance of the robot arm. The robot is set to a specified attitude with positions of joint 1 to joint 5 be at [9, 40, 60, 45, 10]. In controller design, we use the same controller parameter at each axis of robot arm that the P-gain in position loop are designed with 2.4, the P-gain and I-gain in velocity loop are chosen by 3.0 and 0.1. The experimental results of step response are shown in Figs. 10~14. The rising times of step response are with 124ms, 81ms, 80ms, 151ms and 127 ms for J1~J5 axis, respectively. Figs. 10~14 also show the zero steady-state results. To test the tracking performance of a point-to-point motion control of robotic manipulator, we design a specified path which robot moves from the start position (94.3, 303.5, 403.9) mm to the end position (299.8, 0, 199.6) mm, then back to start point. The overall displacement command with acceleration and deceleration velocity profile are obtained by -72.74 (-16346 Pulses), 23.5 (8916 Pulses), 32.16 (8173 Pulses), -56.72 (-11145 Pulses) and -71.18 (-8173 Pulses), for J1~J5-axis, respectively. The tracking results corresponding with the aforementioned input commands are shown in Figs. 15~17. It can be seen that no matter what in position or the velocity tracking, the motion trajectory of the robotic manipulator can gives a perfect tracking with target command in J1~J3 axis. Furthermore, the path trajectory among the position command, actual position trajectory and point-to-point linear distance in Cartesian space R3 (x,y,z) are compared and shown in Fig. 18. It presents that the actual position trajectory can precisely tracking the position command, but the path between two points cant specified in advanced. Therefore, from Figs. 10~18, the results demonstrate that the proposed servo system for the robotic manipulator is effectiveness and correctness.

Servo control IC for Robot Arm

Five-axisPosition loopP controller

PLD of five-axis position servo controller PWM_J1Generation

QEP_J1Circuit

Five-axisSpeed estimator & Speed loopPI controller

Sita_J1~5 Speed_Com_J1~5

PWM_J1

PWM_J2Generation

PWM_J2

PA_J2PA_J2

PA_J1PB_J1

QEP_J2Circuit

QEP_J1

QEP_J5

u_J1~5 PWM_J3Generation

PWM_J3

PWM_J4Generation

PWM_J4

PWM_J5Generation

PWM_J5

PA_J3 PB_J3 QEP_J3

CircuitPA_J4 PB_J4QEP_J4

CircuitPA_J5PB_J5QEP_J5

Circuit

QEP_J3

QEP_J2

QEP_J4

5

55

5

4

4

4

4

4

LS_J1

LS_J2

LS_J3

LS_J4

LS_J5

CPU

On-chipROM

On-chipRAM

UART

PIO

Timer

SPI

Aval

on B

us

Ava

lon

Bus

Nios Embedded Processor

Figure 3. Internal architecture of the proposed servo control IC


Trigger frequency = 1525 Hz

J1-axis Jn-axis

Step1 Step2 Step3 Step4 Step5 Step6 Step6 Step7 Step8 Step9 Step10 Step12Step11 Step1

Wait

Step2 Step..

Trigger frequency = 762 Hz

J1-axis J2-axis J3-axis J4-axis J5-axis J1-axis Jn-axis ..

Step1 Step2 Step3 Step4 Step1 Step2 Step3 Step4 Step1 Step2 Step3 Step1Step4 Step..

Wait.

J1-axis J2-axis J3-axis J4-axis J5-axis

(a)

(b) Figure 4. Step control signal generated sequence at (a) position loop (b) velocity loop

*-

S1 S3S2Time

Sita-Jn

QEP_Jn

Speed_Com_JnErr

KP

S4

**--

S1 S3S2Time

Sita-Jn

QEP_Jn

Speed_Com_JnErr

KP

S4 Figure 5. P controller circuit in position loop

--

** ++--

++**

**

S1 S2 S3 S4 S6S5

Speed_Com_Jn

S7 S8 S9 S10 S11

C_Jn

S12

QEP_JnQEP_DQEP_D

QEP_DQEP_D

KI

KP

Q(13)

Ui_DUi_D

Err_DErr_D

Err_DErr_D

Ui_DUi_D

u_Jn

rpm

ui

Time

Figure 6. Speed estimation and PI controller circuit in

speed loop

Up-downCounter

PWM Generation

Comparator PWM2-Jn

PWM3-Jn

PWM4-Jn

Clk Clk

u-Jn

PWM1-JnFrequencydivider

clkEdge detect

counter

Comparatorlogic

Dead-bandvalue

Clk

Figure 7. PWM circuit

Qep-Jn

FILTER

A

PA-Jn

PB-Jn

PHA

PHB

DLA

DLB

DIR

PLS

D-type FF

D-type FF

QEP

FILTER

B

DECODERQEP

COUNTER

Clk

Clk

Clk

Clk

Clk

Figure 8. QEP circuit

Five-axis Robotic manipulator

J1-axis

J2-axisJ3-axis

J4-axis

J5-axis

Five-axis Robotic manipulator

J1-axis

J2-axisJ3-axis

J4-axis

J5-axis

1:1103801800 (20667 pulse)

J5wrist roll

1:180380900 (17676 pulse)

J4wrist pitch

1:1108001100 (27958 pulse)J3elbow

1:1708001300 (49311 pulse)J2should

1:1008003000 (67416 pulse)J1waist

Gear ratio

Encoder (ppr)

Max. Working range (degree)

No.Name of Link

1:1103801800 (20667 pulse)

J5wrist roll

1:180380900 (17676 pulse)

J4wrist pitch

1:1108001100 (27958 pulse)J3elbow

1:1708001300 (49311 pulse)J2should

1:1008003000 (67416 pulse)J1waist

Gear ratio

Encoder (ppr)

Max. Working range (degree)

No.Name of Link

Figure 9. Robotic manipulator and the related mechanical parameters


0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.04

6

8

10

12

14

Time (s)

Posi

tion

(deg

ree) J1-axisJ1-axis

command

response

Figure 10. Position step response at J1-axis

0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.034363840424446

Time (s)

Posi

tion

(deg

ree) J2-axis

0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.034363840424446

Time (s)

Posi

tion

(deg

ree) J2-axisJ2-axis


0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.054

56

58

60

62

64

66

Time (s)

Posi

tion

(deg

ree) J3-axis

0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.054

56

58

60

62

64

66

Time (s)

Posi

tion

(deg

ree) J3-axis


0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.040

42

44

46

48

50

Time (s)

Posit

ion

(deg

ree) J4-axis

0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.040

42

44

46

48

50

Time (s)

Posit

ion

(deg

ree) J4-axis


0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.04

6

8

10

12

14

16

Time (s)

Posi

tion

(deg

ree) J5-axis

0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.04

6

8

10

12

14

16

Time (s)

Posi

tion

(deg

ree) J5-axis

Figure 14. Position step response at J5-axis of

0.5 1.0 1.5 2.0 2.50

4000

8000

12000

16000

Time (s)

Posi

tion

(pul

se)

Position commandPosition Tracking

J1-axis

0.5 1.0 1.5 2.0 2.5

-20,000

-10,000

0

10,000

20,000

Time (s)

Vel

ocity

(pul

se/s

ec)

Velocity ProfileVelocity Tracking

0

0

J1-axis

Figure 15. Position and velocity response at J1 axis

with acceleration/deceleration function

0.5 1.0 1.5 2.0 2.5

-20,000

-10,000

0

10,000

20,000

Time (s)

Vel

ocity

(pul

se/s

ec) Velocity Profile

Velocity Tracking

0.5 1.0 1.5 2.0 2.5

-18,000

-16,000

-14,000

-12,000

-10,000

Time (s)

Posit

ion

(pul

se)


J2-axis

0

0

J2-axis



0.5 1.0 1.5 2.0 2.5-20,000

-10,000

0

10,000

20,000

Time (s)

Vel

ocity

(pul

se/s

ec) Velocity Profile

Velocity Tracking

0.5 1.0 1.5 2.0 2.5

15,000

17,000

19,000

21,000

23,000

Time (s)

Posi

tion

(pul

se)


J3-axis

0

0

J3-axis




0100

200300

400

0100

200300

400150200250300350400450

X-axis (mm

)Y-axis (mm)

Z-ax

is (m

m)

Position command & actual Position trajectory

(94.3 , 303.5 , 403.9 )

(299.8 , 0 , 199.6 )

Point-to-point linear distance

0100

200300

400

0100

200300

400150200250300350400450

X-axis (mm

)Y-axis (mm)

Z-ax

is (m

m)

Position command & actual Position trajectory

(94.3 , 303.5 , 403.9 )

(299.8 , 0 , 199.6 )

Point-to-point linear distance

Figure 18. Point-to-point motion trajectory

4. Conclusions

A FPGA-based servo system for robotic manipulator is successful developed in this paper. This FPGA-based motion control IC allows a fully digital motion control for a five-axis robotic manipulator, such as point-to-point motion controller, position and speed controller, PWM generation and QEP circuits are all integrated and realized in a single FPGA chip. The experiments are successfully validated, and the experimental results show a good performance. 5. References [1] M. Kabuka, P. Glaskowsky and J. Miranda,

Microcontroller-based Architecture for Control of a Six Joints Robot Arm, IEEE Trans. on Industrial Electronics, vol. 35, no. 2, 1988, pp.217-221.

[2] G. Yasuda, Microcontroller Implementation for Distributed Motion Control of Mobile Robots, in Proceeding of International workshop on Advanced Motion Control, 2000, pp. 114-119.

[3] T.S. Li, S.J. Chang and Y.X. Chen, Implementation of Human-like Driving Skills by Autonomous Fuzzy Behavior Control on an FPGA-based Car-like Mobile Robot, IEEE Trans. on Industrial Electronics, vol. 50, no. 5, 2003, pp.867-880.

[4] M.A. Hannan Bin Azhar and K.R. Dimond, Design of an FPGA Based adaptive Neural Controller for Intelligent Robot Navigation, in Proceeding of the Euromicro Symposium on Digital System Design, 2002.

[5] Y.S. Kung and G. S. Shu, Design and Implementation of a Servo Control IC for Vertical Articulated Robot Arm, ICMT 2004, Proceedings of the 8th International Conference on Mechatronics Technology, pp.297~301, November 8~12, 2004.

[6] Y.S. Kung, P.G. Huang and C.W. Chen, Development of a SOPC for PMSM Drives, in Proceeding of the 47th IEEE International Midwest Symposium on Circuits and Systems, 2004, vol. II, pp. II-329~II-332.

[7] SOPC World, Altera Corporation, 2004. [8] N. Xu, H. Liu, X. Chen and Z. Zhou,

Implementation of DVB Demultiplexer System with System-on-a-programmable-chip FPGA, in Proceeding of the 5th International Conference on ASIC, 2003, vol. 2, pp.954-957.

[9] L.W. Tsai, Robot Analysis-The Mechanics of Serial and Parallel Manipulators, John Wiley & Sons, Inc, 1999.

Acknowledgements

This work was supported by National Science Council of the R.O.C. under grant no. NSC 93-2213-E-218-041.

Documents

Servo system for robots