FPGA Implementation of Closed-Loop FPGA Implementation of Closed-Loop Control System for Small-Scale RobotControl System for Small-Scale Robot

2

Small-scale robot for urban search and rescueSmall-scale robot for urban search and rescue

Bore Hole

Search Occluded Spaces with Tethered Robot Dropped Through Bore Hole

Deployed Configuration

Stowed Configuration

3

IntroductionIntroduction Small-Scale Robot

– Beneficial for urban search and rescue, military surveillance and countermeasure, planetary exploration

– Resource-constrained issues

– Mechanical design and controller design

CRAWLER functional architecture

AIcomputation

MotorSensorInterface

MotorClosed-loop

Control

PowerAmplifier

Motors

Encoders

Force SensorInterface

VisionInterface

ForceProcessing

WirelessCom.

CCD

Camera

Force/TorqueSensor

Host Computer

WirelessCom.

DigitalController

TiltSensor

ImageProcessingVib SignalProcessing

AIcomputation

MotorSensorInterface

MotorClosed-loop

Control

PowerAmplifier

Motors

Encoders

Force SensorInterface

VisionInterface

ForceProcessing

WirelessCom.

CCD

Camera

CCD

Camera

Host Computer

WirelessCom.

DigitalController

TiltSensor

TiltSensor

ImageProcessingVib SignalProcessingAdaptationProcessing

4

Introduction (contIntroduction (cont’’d)d)

Tasks of this project– Investigate FPGA implementation of different designs

for PID control system to provide performance and resource requirements to Run Time Reconfiguration (RTR) and HW/SW codesign algorithms

– Different designs• One-channel parallel and serial designs• Multiple-channel designs

– These designs are evaluated in terms of• Area• Power• Speed

5

Related workRelated work Weiss et al. analyzed different RTR methods on the XC6000

architecture [2]. Shirazi described a framework and tools for RTR [5]. Noguera and Badia proposed a HW/SW codesign algorithm

for dynamically reconfiguration [3].

Chen et al. implemented a complete wheelchair controller on an FPGA with parallel PID design [13].

Samet et al. designed three PID architectures for FPGA implementation – parallel, serial and mixed [14].

Chan et al. implemented power-efficient design of PID controller using FPGA [Chan04].

– [Chan04]: Y.F. Chan, M. Moallem, and W. Wang, “Efficient implementation of PID control algorithm using FPGA technology,” in 43rd IEEE Conference on Decision and Control, Vol. 5, Dec. 2004, pp. 4885 – 4890.

6

PID control algorithmPID control algorithm

Differential equation

dt

tdeTdtte

TteKtu d

t

ip

)()(

1)()(

0

Kp : proportional gain, Ti : integral time constant, Td : derivative time constant

))1()(()()()(0

neneKieKneKnu d

n

iip : Position algorithm

: Incremental algorithm

)2()1()()1()()( 210 neKneKneKnununu

)2()1()()1()()1()( 210 neKneKneKnunununu

dip KKKK 0

dp KKK 21

where

dKK 2

Difference equation

7

One-channel parallel designOne-channel parallel design

Incremental algorithm)( PPe dn

neKp *0 0

11 *1 neKp

22 *2 neKp

101 pps

122 nups

21 ssun -

Pd

EncdCnt

*

*

*

Bounder

UpBound

LowBound

K0

K2

K1

OvFl0

+

+

+

+

OvFl2

OvFl1

OvFl3

OvFl0

OvFl2

OvFl1

OvFl3

OvFl

OutputREG

Clk

REG

Clk

REG

Clk

Clk

REG

Reset

REG0

REG3

REG2

REG1

ADD0

ADD1

ADD2

ADD3

MPL0

MPL1

MPL2

Bounder0

P P_neg

e(n)

e(n)

e(n-1)

e(n-2)

P0

P2

P1

S1

S2

u(n)

u(n-1)

24

24

16

16

16

16

16

16

32

32

32

32

32

16

16 16

4 additions and 3 multiplications

8

Department of Electrical Engineering, University of Minnesota

One-channel serial designOne-channel serial design

-

Pd

EncdCnt

*

Bounder

UpBound

LowBound

K0

K2

K1

OvFl

+

Output

Reset

REG0

REG2

ADD0MPL0

Bounder0

P P_neg

P0

P2

un

24

32

16

16 16

MUX4_1

32-bit

Clk

REG

Clk

REG

load(0)

load(2)

Product

Product

MUX3_1

32-bit

Clk

REGload(8

)

REG1

P1

Clk

REGload(1)

Product

REG3S1

Clk

REGload(3

)

Sum

REG8

Sel0

2

Sel1

2

MUX0

MUX1

24

Sum

MUX3_1

16-bit

Clk

REGload(4)

REG5en- 1

Clk

REGload(5

)

REG6

Clk

REGload(6

)

REG4

Sel3

2

MUX3

MUX3_1

16-bit

Sel2

2

MUX2

Clk

REGload(9

)

REG9

Disable

Clk

REGload(7

)

REG7

Product

ControlUnitClk

Reset

Req Ack

load(9:0)

sel0sel1sel2sel3

ControlUnit0

Flip_Flop0

Clk

Reset

Start

Sel0

Sel2Sel1

Sel3

load(9:0)

32

32

32

MUX_OUT0

MUX_OUT1

MUX_OUT2

MUX_OUT3

en

en- 2

16

16

32

Datapath

Control Unit

16

16

16

16

Req

Ack

0

1

2

3

0

1

2

0

1

2

0

1

232

32

32

32

32

un

9

Multiple-channel designsMultiple-channel designs Channel-level parallel (CLP)

designs• Each channel has its own

PID unit• Large area, proportional to

number of channels

Channel-level serial (CLS) designs

• All channels share one PID unit

• Small area independent of each channel

• More complex control unit• Context switching

Motor(1)Encoder(1)

Motor(0)PIDEncoder(0)

Motor(15)Encoder(15)

.

.

.

PID

PID

.

.

.

.

.

.

PIDMUX

Encoder(0)

Encoder(1)

Encoder(15)

.

.

.

Motor(0)

Motor(1)

Motor(15)

.

.

.

DE-MUX

10

Multiple-channel designsMultiple-channel designs Channel-level parallel (CLP)

designs• Each channel has its own

PID unit• Large area, proportional to

number of channels

Channel-level serial (CLS) designs

• All channels share one PID unit

• Small area independent of each channel

• More complex control unit• Context switching

Motor(1)Encoder(1)

Motor(0)PIDEncoder(0)

Motor(15)Encoder(15)

.

.

.

PID

PID

.

.

.

.

.

.

PIDMUX

Encoder(0)

Encoder(1)

Encoder(15)

.

.

.

Motor(0)

Motor(1)

Motor(15)

.

.

.

DE-MUX

-

Pd

EncdCnt

*

K0

K2

K1

+

Output

P

P0

P2

un

MUX4_1

REG

REG

Product

Product

REG

P1REG

Product

S1REG

Sum

Sum

REG

en- 1REG

REG

REG

Product

en

en- 2

un

MUX3_1

MUX3_1

MUX3_1

-

Pd

EncdCnt

*

*

*

K0

K2

K1

+

+

+

+Output

REG

REG

REG

REG

P

e(n)

u(n)

Arithmetic-level parallel (ALP) design

Arithmetic-level serial (ALS) design

11

Experiment system platformExperiment system platform

FPGA board (Xport)– Spartan II XC2S150

FPGA

Microprocessor module (GBA)– ARM7TDMI

processor, 32-bit RISC

Cport

CPLD

Flash

SDRAM

User expansion conectors

ARM7TDMImicroprocessor

<S/W PID><Trajectory Generator>

Game BoyAdvanced

Xport 2.0

PowerAmp

Motor

ShaftEncoder

Host Computer

< H/W PID >

PWMLogic

EncoderCounter

Pd

P

u(t)

FPGASpartan IIXC2S150

12

Department of Electrical Engineering, University of Minnesota

Function testFunction test

0 20 40 60 80 100 120 140 160 180 200

1000

1050

1100

1150

1200

1250

Pos

ition

(en

code

r co

unt)

Time (ms)

Step Response

Desired position

Software PID

One-channel parallel designOne-channel serial design

Multiple-channel parallel design

Multiple-channel serial design

Step response control– All designs perform correct function.– Fast response, small overshoot and high accuracy.

13

Performance test: areaPerformance test: area

Area test– Test tool

• Xilinx ISE, Place & Route Report– Test metric

CLB slices Logic gates

Device resources utilization of designs

Resources Avail. Used Utili. Used Utili. Used Utili. Used Utili.

# ExternalGCLKIOBs

4 2 50% 3 75% 4 100% 4 100%

# External IOBs 140 65 46% 65 46% 78 55% 86 61%

# GCLKs 4 2 50% 3 75% 4 100% 4 100%Area (# slices) 1728 615 35% 466 26% 1536 88% 1412 81%

Designs One-Channel Multiple-Channel (CLS)Arithmetic-level parallel Arithmetic-level serial Arithmetic-level parallel Arithmetic-level serial

14

Performance test: area (contPerformance test: area (cont’’d)d) Area

– CLS design smaller than CLP design for large number of channels– But, CLP design smaller for small number of channels– Arithmetic-level serial design always smaller than arithmetic-level

parallel, whatever CLP or CLS

Area of multiple-channel designs

0

2000

4000

6000

8000

10000

12000

1 3 5 7 9 11 13 15

number of channels

nu

mb

er o

f sl

ices

CLP+ALP

CLS+ALP

CLP+ALS

CLS+ALS

15

Performance test: speedPerformance test: speed

Speed test– Test tool

• Xilinx Timing Analyzer

Execution time of multiple-channel designs

0

500

1000

1500

2000

2500

3000

3500

1 3 5 7 9 11 13 15

number of channels

ex

ec

uti

on

tim

e (

ns

)

CLP+ALP

CLS+ALP

CLP+ALS

CLS+ALS

16

Performance test: powerPerformance test: power

Power test– Test tool

• Xilinx XPower– Input signals

Set input signal frequency and activity rate: far from accurate

Simulation data file (VCD file): accurate– Simulation tool: ModelSim– Simulation input: real step response experiment data

– Two situations • Stable state

– Control error = 0• Dynamic state

– Control error 0

17

Performance test: power (contPerformance test: power (cont’’d)d)

Power of one-channel serial and parallel design

0

20

40

60

80

100

0.0012 0.012 0.12 0.25 0.5 1.25

Sampling frequency (MHz)

Po

we

r (m

w)

ALS

ALP

18

Performance test: power (contPerformance test: power (cont’’d)d) Power dissipation

– For the same sampling frequency, CLP+ALS design consumes least power.

– But area of CLP is too large for large number of channels. CLS+ALS design has smallest area.

Power of multiple-channel design

0

100

200

300

400

500

600

700

800

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16

Number of channels

Po

wer

(m

W) CLP+ALP

CLS+ALP

CLP+ALS

CLS+ALS

19

ConclusionsConclusions Preliminary work exploring resources-constrained robot

control system design using FPGA. One-channel serial and parallel architectures, and CLS

multiple-channel designs for PID closed-loop control. Functional correctness verified. For one channel design increasing sampling frequency,

– ALS shows less power than ALP. For small number of channels

– CLP+ALS design has smallest area and least power. For large number of channels

– CLP+ALS design still least power, but design is too large.– CLS+ALS design has smallest area.

Performance test methodologies and metrics discussed.

20

Future workFuture work

Implement and test on Virtex-II Pro FPGA. Run time reconfiguration: reconfigure structure to

adapt different situation and terrain. Use Virtex-II FPGA clock gating structure to

improve power efficiency. Decrease motor power consumption. Current work: develop FPGA Interface to C3088

camera using Xilinx ML310 board (Virtex-II pro).