Tracking Control Using the Strip-wise Affine Transformation: An - - PowerPoint PPT Presentation

Tracking Control Using the Strip-wise Affine Transformation: An Experimental SoC Design

George P. Moustris, Kyriakos M. Deliparaschos, Spyros G. Tzafestas

School of Electrical and Computer Engineering National Technical University of Athens Intelligent Automation Systems Research Group e-mails: gmoustri@central.ntua.gr kdelip@mail.ntua.gr tzafesta@softlab.ntua.gr

2

Overview of the System



Robot platform: Khepera II



0.08 mm resolution for the position counter of each wheel



Kinematics emulated to a bounded curvature vehicle



SoC FPGA: Xilinx XC3S1500-4FG676C Spartan-3 FPGA



Parameterized Digital Fuzzy Logic processor (DFLP) Intellectual Property (IP) core implementing the Fuzzy Tracking algorithm



Xilinx Microblaze™ soft processor core as the top- level flow controller



Transformation calculation



Matlab GUI:



Visualization/Monitoring running



Relaying



Odometry



Digital CamCoder:



Overhead camera recording the activity terrain in 720p resolution (1280x720 pixels)



Calibrated using Camera Calibration Toolbox in Matlab (J.Y. Bouguet)

3

 Let F be a nonlinear system of the form

where p is the state vector and u the input. If pref is a feasible reference path in the state space which corresponds to a feasible reference input uref then find an appropriate state feedback law u(p,pref,uref,t) such that

Path Tracking

( , , ) p f t p u

lim( )

ref t

p p

4



Former problem known as trajectory tracking because reference trajectory is parameterized by time



If pref is a geometric reference path (no temporal parameterization) then we get the “path tracking problem” {i.e. track the image if the reference path}



For the Dubin’s Car model: pref=(xref ,yref ), where (x,y) is the middle point of the robot axis.



Speed is constant



Control input is the curvature κ



Drift term is non-vanishing

Path Tracking

cos Σ: sin x v y v v

5

 Variations regarding the reference path:  Path is piece-wise linear (polygonal)

 Not feasible under some models (e.g. Dubins

Car or any other with continuous θ )

 Path is a set of points (waypoints)

 Not feasible under any model

Path tracking (cont’d)

6

 Applies to polygonal paths (polygonal chains)  Main idea is to straighten the polygonal path and track a

straight line in the transformed domain

 Piece-wise Linear Homeomorphism (PLH)

 Original domain  Physical  Transformed domain Canonical

Strip-wise Affine transformation

7

Strip-wise Affine transformation (cont’d)

 Polygonal Chain is described by: A={w1,w2,…,wn}, wi ∈ℂ  Each vertex wi of the chain is projected to a point ai on the

real axis in the canonical domain according to: where

 Segment [ak-1,ak] is linearly projected onto edge from wk-1 to

wk :

 Each interval [ak-1,ak] is transformed by a respective

transformation f1,f2,…,fn-1.

 Complex variable on canonical space is z=x+jy 1

, i 1,2,3,..., n

i k i k

S a S

1

,

1

n k k k k

S w wk

S S

1

arg( ) 1 1 1

( ) ( )

k k

j w w k k k

f x w S x a e

8

Strip-wise Affine transformation (cont’d)

 Using complex pulse:

and summing over all partial maps, the interval [0,1) is linearly projected to the polygonal chain

 Extension to the entire real line is done by appending two

• branches. One starting from infinity and ending to w1 and one

starting from wn and escaping to infinity.

1

1 , [ , ) 0 , elsewhere

k k k

x a a

1 1

( ) ( ) ( )

n k k k

f x f x

1 1 ( )

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

j n k k j k n n n

f x w S x a e f x f x f x w S x a e

9

Strip-wise Affine transformation (cont’d)

 Extension to the entire plane is done using:  Parameter θs is called the “shifting angle”  Φ(z) is the direct strip-wise affine transformation and

produces a linear displacement of the polygon along the direction θs

 We identify complex variable w=u+jv in the physical domain,

with the transformation Φ(z), i.e. w=u+jv= Φ(z)

 Essentially divides the planes into strips and applies an affine

map between them, hence “strip-wise affine”.

( ) ( )

s

j

Φ z y S e f x

10

Strip-wise Affine transformation (cont’d)

11

Strip-wise Affine transformation (cont’d)

 Invertibility depends on θs and actual path geometry.  Only monotone polygonal chains allow for a bijective

mapping. and

1

/ (+) /

k

x u C S a y v C S

1

sin cos sin( sin cos

/ ) sin cos sin( )

s s n n n s k k k k k k k k k R I n k k k k k k s k

S

w w C

12

Strip-wise Affine transformation (cont’d)

 (+) has a problem. Pulses ψi are ψi(z=x+jy) thus not the

inverse equations.

 First find the active pulse then use (+) to compute the

inverse system Binary search problem in O(logn)

13

Feedback equivalence

 How is the robot’s model mapped to the canonical domain?  Denote all physical states with subscript p and all canonical

states with c, then Φ(z) maps

 State θp is unaffected  Include map of θ,

and introduce the extended map

 Ψ sends the canonical state space to the physical state

• space. The canonical state is,

[ , , ] to [ , , ]

T T c c p p p p

x y x y x x

1

tan( ) cos sin tan ( ) sin( ) tan( )cos( )

n n p c s p s

( ) x x

[ , , ]T

c c c

x y x

14

Feedback equivalence

The system equations in the canonical space are:

where, J is the Jacobian of Φ. Notice that input is not mapped. The two systems are “state space equivalent”. Extend again the transformation to include an input mapping where is the mapping that sends the control from the canonical input space to the physical input space.

3 3 1

cos : sin

c c c c c c p c c

x v y v S J v

( ) 1 sin 2 cos( ) ,

n c c s

2

sin( )

n s k k

J S ( , )

( , ) u u x

15

Feedback equivalence (cont’d)

 is the canonical input and, The canonical system is, The two systems are said to be “feedback equivalent”. The

• riginal physical system and the canonical system have the

same state transition equations. Thus, the Dubins Car in the physical domain is mapped to a Dubins Car to the canonical domain.

cos : sin

c c c c c c c c c

x v y v v

c

u

3 3 p c

S J

16

Feedback equivalence (cont’d)

Results:

 Reduce tracking of monotone polygonal paths to straight line

tracking which is easier

 Use existing tracking controllers and optimize them for straight

lines  simplification of control

17

Control Strategy



Path sampled under fixed sampling spacing Δs



Inputs: Angle error φ1 Heading error φ2



Output: Curvature κ

physical canonical

1 2 2 1 2 2

, [ ,0] [0, ] [ , ] 2 , [ , ]

18

Fuzzy input



Fix look-ahead distance S or fix angle φ



Original fuzzy controller has 91 rules



Input in canonical space is constrained



We have fixed S, thus reducing the rules to 35 (61.5% reduction)

19

Experiments



Path drawn by hand in MATLAB and d/l to FPGA



Fuzzy controller and SWAM run on FPGA



Odometry implemented on MATLAB



Camera calibrated using Camera Calibration Toolbox



Video in 720p



Video post processing tracked a red LED on the khepera



Accuracy around 2.4mm ( 2 pixels)

20

Experiments

21

Experiments

22

Experiments

23

Experiments

24

Experiments

25

Experiments

26

SoC Hardware Architecture

27

SoC Hardware Architecture (cont.)

• The parameterized zero-order TSK type Fuzzy Logic

Controller (FLC) core exchanges data with the MicroBlaze processor via the FSL bus.

• The scope of the FLC core is to serve as high-speed fuzzy

inference co-processor to the Microblaze.

• The FLC core was implemented with the following

parameters.

28

FLC Core - Parameters

• The FLC core chosen parameters are summarized below

Inputs - Input resolution 2 - 12 bit Outputs - Output resolution 1 - 12 bit Antecedent Membership Functions (MF’s) - Degree of Truth resolution 9 Triangular MF’s - 8 bit Consequent MF’s - MF resolution 5 Singletons - 8 bit Number of fuzzy inference rules 81 Rule activation method MIN Aggregation method SUM Implication method PROD MF overlapping degree 2 Defuzzification method Weighted average

29

FLC Core - Features

• The FLC is a fully parameterized IP Core.
• The selected architecture assumes overlap of two fuzzy

sets between adjacent fuzzy sets.

• It requires 2n clock cycles (input data processing rate),

where n is the number of inputs, since it processes one active rule per clock cycle.

• The FLC core runs at the same speed as the OPB and

MicroBlaze, which is 50 MHz at the present SoC.

• Based on the place and route report, the SoC design
• ccupies 4174 out of 13312 slices of the Xilinx Spartan 3

FPGA chip (XC3S1500-4FG676)

30

FLC - Data Flow Path

Input Register Active Rule Selector 12 bit Input Register Active Rule Selector 12 bit MF Trapezoidal Generator MF Memory Bank Address Generator 3 bit 3 bit MF Trapezoidal Generator 3 bit MF Type MF Memory Bank 3 bit MF Type 40 bit 2 bit 40 bit 2 bit 12 bit Memory Mapper 6 bit Consequents Memory Bank 12 bit Rule Sellector 8 bit Min or Prod Integrator 8 bit Prod 8 bit 8 bit 16 bit Integrator 8 bit 16 bit Divider 12 bit Output x0 x1 2 bit 8 bit 8 bit 12 bit 12 bit Delay Register 12 bit Delay Register 12 bit

31

SoC Architecture - Detailed View

U_fpga_fc

rst_n clk ip0 ip1

• p

fpga_fc State Machine FSL_S_Data 32 FSL_Clk FSL_M_Full FSL_Rst FSL_S_Exists FSL_S_Data_r 32 G FSL_Clk G FSL_Rst_n G FSL_S_Data_r FSL_Rst_n FSL_Rst_n FSL_Clk FSL_S_Data_r : (12:23) FSL_S_Data_r : (0:11) FSL_M_Data_i FSL_M_Data 12 12 FSL_M_Write_sync_proc FSL_M_Write_i FSL_Clk FSL_M_Write_cnt 32 FSL_M_Write FSL_S_Read FSL_S_Control FSL_M_Clk FSL_M_Control FSL_S_Clk

Top Wrapper (flc_ip)

Process NC NC NC NC G NC Block magnified below (U_fpga_fc)

U2

ip_data sel

• p_data

U7

alpha_val ip_data mf_param

PSR2 CPR3 CPR4 U3

ip_data

• p_data

U0

ip_data fs_start_addr addr_gen_p ip_data rst_n clk int_zer

U4

addr_in addr_out

CPR1 PSR1 PSR3

cons_map_p

U1

ars_p trap_gen_p rule_sel_p andor_meth_p

CPR2 U5

data addr s_rom_p mf_rom_p data addr

U6

gen_addr

CPR5 Fuzzyfication Area Inference Engine Area R1

ip0 ip1 24 6 8 8 80 24 16 16 7 7 16 16 2 (MSBs) 12 2 8 clkx rst_n

Top Structural Parameterized DFLC Soft IP Core Design (U_fpga_fc) U8

x_signed x_unsigned mult y

CPR6 U9

y clk x rst_n clear int_uns

CPR7 CPR8 U10

y clk x rst_n clear int_sig

U12

• p

Y X div_array

U11

• p

divs divd div_ppa

IF* Defuzzyfication Area R2

• p

clkx rst_n *IF GENERATE Statement

CPR9

8 21 21 23 23 10 10 12 12 CPR{1,9}, PSR{1,3} Register delays (R{1,2}), clocked by clkx. All reset by rst_n. Divider Type Selection Truncated to 12 bits Global Connection No Connection Combinatorial Logic 14 12 12 12

32

Conclusions

• Strip-wise affine map reduces the control complexity

and can be incorporated in control schemes

• FLC IP Core provides very fast fuzzy control
• Odometry degrades the tracking quality
• If better localization is used, results will be much

better