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
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) 2
Path Tracking Let F be a nonlinear system of the form p f t p u ( , , ) where p is the state vector and u the input. If p ref is a feasible reference path in the state space which corresponds to a feasible reference input u ref then find an appropriate state feedback law u(p,p ref ,u ref ,t) such that lim( p p ) 0 ref t 3
Path Tracking Former problem known as trajectory tracking because reference trajectory is parameterized by time If p ref 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: x v cos 0 Σ: sin 0 y v 0 v p ref =(x ref ,y ref ), where (x,y) is the middle point of the robot axis. Speed is constant Control input is the curvature κ Drift term is non-vanishing 4
Path tracking (cont’d) 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 5
Strip-wise Affine transformation 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 6
Strip- wise Affine transformation (cont’d) Polygonal Chain is described by: A ={ w 1 ,w 2 ,…,w n }, w i ∈ℂ Each vertex w i of the chain is projected to a point a i on the real axis in the canonical domain according to: S i k a , i 1,2,3,..., n i S k 1 n where S S S w wk , 1 k k k k 1 Segment [ a k-1 ,a k ] is linearly projected onto edge from w k-1 to w k : j arg( w w ) f ( ) x w S ( x a ) e k k 1 k 1 k 1 k 1 Each interval [a k-1 ,a k ] is transformed by a respective transformation f 1 ,f 2 ,…,f n-1 . Complex variable on canonical space is z=x+jy 7
Strip- wise Affine transformation (cont’d) Using complex pulse: 1 , x [ a a , ) k k 1 k 0 , elsewhere and summing over all partial maps, n 1 f x ( ) f ( ) x ( ) k k k 1 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 w 1 and one starting from w n and escaping to infinity. j f ( ) x ( w S ( x a ) e ) , n 1 1 0 ( ) f x ( ) f ( ) x k k j f ( ) x ( w S ( x a ) e ) k 0 n n n 8
Strip- wise Affine transformation (cont’d) Extension to the entire plane is done using: Φ z y S e f x j ( ) ( ) s 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”. 9
Strip- wise Affine transformation (cont’d) 10
Strip- wise Affine transformation (cont’d) Invertibility depends on θ s and actual path geometry. Only monotone polygonal chains allow for a bijective mapping. x u C S / a k 1 (+) y v C S / and sin cos s s n 1 / ) n n S sin( s k k sin cos k 0 k k k k k 0 k 0 R I n w sin w cos k k k k C k sin( ) k 0 s k 11
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) 12
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 T T [ x , y , ] to [ x , y , ] x x c c p p p p State θ p is unaffected Include map of θ , n n tan( ) cos sin p 1 0 0 tan ( ) c sin( ) tan( )cos( ) s p s and introduce the extended map ( ) x x Ψ sends the canonical state space to the physical state space. The canonical state is, ] T [ x y , , x c c c 13
Feedback equivalence The system equations in the canonical space are: x v cos 0 c c c : y v sin 0 c c c p 3 3 1 0 S J v c c where, n n 2 ( ) 1 sin 2 cos( ) , J S sin( ) c c s s k 0 k 0 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 u ( , ) u x from the canonical input space to the physical input space. 14
Feedback equivalence (cont’d) is the canonical input and, u c p 3 3 S J c The canonical system is, x v cos 0 c c c : y v sin 0 c c c c v 0 c c The two systems are said to be “feedback equivalent”. The original 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. 15
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 16
Control Strategy Path sampled under fixed sampling spacing Δ s Inputs: Angle error φ 1 Heading error φ 2 Output: Curvature κ physical canonical , [ ,0] [0, ] [ , ] 1 2 2 2 , [ , ] 1 2 2 17
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) 18
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) 19
Experiments 20
Experiments 21
Experiments 22
Experiments 23
Experiments 24
Experiments 25
SoC Hardware Architecture 26
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. 27
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 9 Triangular MF’s - 8 bit resolution 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 28
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 2 n 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 occupies 4174 out of 13312 slices of the Xilinx Spartan 3 FPGA chip (XC3S1500-4FG676) 29
Recommend
More recommend
