Autonomous SoC for fuzzy robot path tracking

Autonomous SoC for Fuzzy Robot Path Tracking Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas School of Electrical and Computer Engineering National Technical University of Athens Intelligent Robotics and Automation Laboratory e-mails: [email protected] [email protected] [email protected] Autonomous SoC for Fuzzy Robot Path Tracking Overview of the System ► Robot platform: ActivMedia P3-DX8  1 mm resolution for the position estimation and 1° angle resolution for the heading  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 ► Matlab GUI: Visualization/Monitoring running on Laptop SoC - FPGA Matlab GUI Pioneer 3 DX8 Robot Serial link Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas Embedded FL tracker Serial link 2 Autonomous SoC for Fuzzy Robot Path Tracking 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 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 lim( p t Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas pref ) 0 3 Autonomous SoC for Fuzzy Robot Path Tracking Path Tracking ► ► ► 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” {e.g. pref=(x,y) where (x,y) are the Cartesian coordinates of the robot} For the Dubin’s Car model: x y cos sin v pref=(xref ,yref ), where (x,y) is the middle point of the robot axis. ► ► Speed is constant Control input is the curvature κ Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 4 Autonomous SoC for Fuzzy Robot Path Tracking Constraints ► Non-holonomic: rolling without slipping ► Rigidity: robot dimensions remain fixed ► Input bounds: robot cannot turn while stopped (bounded κ) ► Quantization: robot quantizes states and inputs  No explicit command for curvature. Curvature command is issued through the control of the linear and angular velocities  Angular velocity quantized to 1 deg/sec/bit resolution  Linear Velocity quantized to 1 mm/sec/bit resolution  RESULT  Turning radius: κ-1=R=c v/ω is quantized Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 5 Autonomous SoC for Fuzzy Robot Path Tracking Quantization ► Quantization presents a problem for the control input. If the curvature is bounded to |κ|≤1 m-1, a velocity of v=100 mm/sec results to 6 available input commands ► Bounding constraint reduces further the available levels ► Performance severely degraded. Results to oscillations ► Putting R=1 and solving w.r.t ω we get the available quantization levels: L num ► 180 1000 Linear dependence on v. Obvious solution is to increase velocity but if v is large, dynamics might come into play Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 6 Autonomous SoC for Fuzzy Robot Path Tracking Setting the Speed To estimate an acceptable error level between the curvature computed by the DFLP and the actual curvature the robot follows, we draw the maximum relative error versus all available speeds over all available inputs i.e., 100 max( FLC ACTUAL )/ ACTUAL , 1 , 2 In this way one can see the maximum possible relative error for each speed between the actual and the desired curvature. An acceptable trade-off speed seems to be 1000 mm/sec (1m/sec) where the error drops below 1.745%. For this speed, the available quantization levels are Lnum=57. As a result, the robot’s speed was set to 1000 mm/sec in all field experiments. Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 7 Autonomous SoC for Fuzzy Robot Path Tracking Control Strategy ► ► ► Path sampled under fixed sampling spacing Δs Inputs: Angle error φ1 Heading error φ2 Output: Curvature κ Closest Point Path Tangent φ1 Robot Heading φ2 ► ► Spatial Window  Offset (Start at “offset” points from closest)  Order (iterate over “order” path points)  Step (skip “step” points at each iteration) step offset Closest Point order Output: Mean of curvatures 1 2 ... n n Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 8 Autonomous SoC for Fuzzy Robot Path Tracking FPGA Development Platform Specifications ► The SoC Application was implemented on a Spartan-3 Development Platform Xilinx XC3S1500-4FG676C Spartan-3 FPGA 16 M x 16 DDR memory, 2 M x 16 flash memory Platform Flash ISP PROMs 10/100 Ethernet PHY, USB 2.0 and RS232 2 7-segment LED displays 4 User LEDs, 2 Push Buttons, 8 Pos. Dip Switches On-board clock oscillator JTAG configuration port 75 MHz Clock Oscillator 2 x 16 Character LCD Two P160 expansion slots System ACE/User I/O Header LVDS tx/rx interface Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 9 Autonomous SoC for Fuzzy Robot Path Tracking Fuzzy Path Tracker (DFLP IP Core) The DFLP IP Core is fully parameterized. The selected architecture assumes overlap of two fuzzy sets between adjacent fuzzy sets and requires 2n clock cycles (input data processing rate), where n is the number of inputs, since it processes one active rule per clock cycle. Fuzzy Inference System (FIS) Takagi-Sugeno zero-order type Characteristics ► TS Zero-Order Fuzzy Logic Controller ► 2 Inputs, 1 Output ► 9 Triangular MFs per Input ► 5 Singleton Output MFs ► 81 Fuzzy Rules ► Implication method: Product ► Defuzzification method: Weighted average Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas type Inputs 2 Input resolution 12 bit Outputs 1 Output resolution 12 bit Antecedent Membership Functions (MF’s) 9 Triangular or Trapezoidal shaped per fuzzy set Antecedent MF Degree of Truth (α value) resolution width 8 bit Consequent MF’s 81 Singleton type Consequent MF resolution 8 bit Max. no. of fuzzy inference rules 81 (no. of fuzzy sets no. of inputs) AND method MIN (T-norm operator implemented by minimum) Implication method PROD (product operator) MF overlapping degree 2 Defuzzification method Weighted average 10 Autonomous SoC for Fuzzy Robot Path Tracking SoC Architecture - High Level Diagram MicroBlaze FSL0 Bus BRAM FSL_interface Parameterized Fuzzy Logic Controller Peripheral Soft-Core (VHDL Implementation) LMB Bus FSL1 Bus OPB Bus GPIO Output Ports (User LEDs, etc) GPIO Input Ports (Push Buttons, Dip SW’s, etc) Debug Module RS232 UART Module USB UART Module Ethernet MAC Microblaze™ is licensed as part of the Xilinx Embedded Development Kit (EDK) and is based on RISC architecture ► It is a soft core, meaning that it is implemented using general logic primitives rather than a hard dedicated block on the FPGA ► The DFLP IP core is connected to the Microblaze™ via the FSL bus ► The processor connects to the OPB bus for access to a wide range of different modules, and communicates via the LMB bus for a fast access to BRAM inside the FPGA ► Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 11 Autonomous SoC for Fuzzy Robot Path Tracking SoC Architecture – Detailed Diagram ► ► ► ► ► ► Component pipeline registers (CPR) Path synchronization registers (PSR) ‘U’ blocks represent the different components of the DFLP IP core U_fpga_fc component is embedded in flc_ip top structural entity wrapper Two Digital Clock Manager (DCM) Modules (DCM_0 and DCM_1) used for the different clocks production SoC achieves a system clock (DCM_0) operating frequency of 14.140ns or ~71 MHz, while DCM_1 is mainly used for clocking the external DDR RAM Top Wrapper (flc_ip) FSL_M_Write_sync_proc 32 State Machine FSL_S_Data FSL_Clk FSL_M_Write_cnt FSL_M_Write_i 32 FSL_Rst_n FSL_Clk FSL_S_Data_r : (0:11) FSL_S_Data_r : (12:23) G FSL_M_Clk NC U_fpga_fc FSL_S_Data_r FSL_M_Full FSL_Rst FSL_S_Exists FSL_Clk FSL_M_Write 32 Process FSL_Clk NC FSL_M_Control NC FSL_S_Clk FSL_M_Data_i fpga_fc rst_n clk ip0 ip1 op 12 12 FSL_M_Data Block magnified below (U_fpga_fc) FSL_S_Read FSL_Rst_n G FSL_S_Control NC FSL_Rst_n G FSL_S_Data_r G Global Connection NC No Connection Combinatorial Logic Top Structural DFLP Parameterized Soft Core IP Design (U_fpga_fc) ip0 U0 R1 U1 ars_p 12 6 fs_start_addr ip_data clkx rst_n ip1 12 addr_gen_p ip_data int_zer clk gen_addr rst_n 80 PSR1 s_rom_p 7 addr data 14 8 12 12 PSR3 CPR5 2 8 CPR3 U7 trap_gen_p mf_param alpha_val ip_data 24 U5 7 2 (MSBs) mf_rom_p data addr CPR4 U2 rule_sel_p sel op_data 16 ip_data 16 16 U3 andor_meth_p ip_data op_data 8 16 Fuzzyfication Area Inference Engine Area Divider Type Selection U10 CPR6 U8 21 mult x_signed y 21 x_unsigned int_sig clear x y clk rst_n CPR8 23 U9 8 clkx rst_n int_uns 10 clk rst_n y x clear Defuzzyfication Area Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas cons_map_p addr_in addr_out U6 24 PSR2 CPR2 U4 CPR1 23 CPR7 10 *IF GENERATE Statement Truncated to 12 bits U12 div_array X op Y U11 div_ppa divd op divs CPR9 IF* R2 op 12 12 CPR{1,9}, PSR{1,3} Register delays (R{1,2}), clocked by clkx. All reset by rst_n. 12 Autonomous SoC for Fuzzy Robot Path Tracking FPGA Design Resources ► U_fpga_fc alone was synthesized using Synplify Pro synthesizer tool ► The rest of the design components were synthesized by Xilinx Synthesis Tool (XST) through the EDK Platform Studio ► The DFLP IP core alone for the selected parameters on this application occupies 1600 (6%) LUTs, 4 Block Multipliers (MULT18X18s), 12 64x1 ROMs (ROM64X1), and 56 256x1 ROMs (ROM256X1) Resource Used Available Utilization BSCANs 1 1 100% BUFGMUXs 6 8 75% DCMs 2 4 50% External IOBs 121 487 24% LOCed IOBs 120 121 99% MULT18X18s 11 32 34% RAMB16s 16 32 50% Slices 4021 13312 30% SLICEMs 668 6656 10% 5,956 26,624 22% Total LUTs: Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 13 Autonomous SoC for Fuzzy Robot Path Tracking Top-Level Control Program Written in C and executed in the Microblaze™ soft processor core ► Implements the autonomous control logic of the P3 robot ► Receives odometry information from the robot and issues steering commands outputted by the FL tracker ► Treats synchronization and timing requirements ► For the communication with the outside world (Robot, Laptop) uses two I/O channels, one serial and one Serial2USB bridge, both having 16-byte input and output buffers Wait for path download Start program Send “bad checksum” to Matlab Send acknowledgement to Matlab yes no a Calculate steering commands Initiate robot communication Hear for incoming SIP packets Checksum valid Call DFLC and calculate curvature Encode commands and transmit them no no Discard packet Is closest point the last path point? Is whole SIP availiable? yes Stop motors and exit yes no no Extract inputs for FLC and relay info to Matlab Checksum valid b yes ► (a) Path tracking control algorithm ► (b) “fix” algorithm to avoid 16-bit integer coordinates overflow when their range is exceeded Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas Decode packet and relay to Matlab Check for encoder overflows Overflow occurred? yes Fix readings 14 Autonomous SoC for Fuzzy Robot Path Tracking Hardware/Software Co-design Flow Hardware Development Flow Software Development Flow Software Project Build Specify Processor, Bus, and Peripherals SW Configuration HW Configuration Virtual System Model Automatic SW Library Generation Automatic HW Platform Generation Software Compilation Xilinx FPGA Implementation Flow Executable Bitstream Debug GDB/ XMD Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas FPGA Platform Board Download to FPGA 15 Autonomous SoC for Fuzzy Robot Path Tracking DFLP IP Core design Flow Design Specifications Mathworks Matlab System model Development Test Vector generation VHDL RTL description Mentor Modelsim RTL Simulation Debugging NO Functional Optimization OK? Synplicity – YES Synplify Pro Logic Synthesis Synthesis Constraints Synthesis Optimization Xilinx – ISE FPGA Place & Route P&R Constraints Meets Specifications ? Area & Timing Report P&R Optimization NO YES Timing Simulation Debugging NO Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas OK? YES “.bit” File 16 Autonomous SoC for Fuzzy Robot Path Tracking Matlab GUI Application A Matlab program was developed for monitoring and initialization purposes. Matlab is connected to the FPGA through a bridged USB connection. It receives and analyzes data relayed by the SoC, mainly the SIP packets that the robot sends. The program decodes the SIP packets and extracts odometry information. It also incorporates the same routine used in the SoC for catching and fixing encoder overflows Snapshot of the GUI after an experiment. The solid line represents the desired path while the dashed line the actual path. The map illustrates part of the 2nd floor of the Electrical & Computer Engineering faculty of NTUA. All units are in millimeters. Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 17 Autonomous SoC for Fuzzy Robot Path Tracking Experiments ► Experiments took place inside the NTUA campus. The goal was to assess the overall efficiency of the system and particularly the fuzzy tracker. In order to log the actual position of the robot during the runs, a DGPS antenna and receiver were mounted onto it. The DGPS system used is the Trimble 4700 GPS receiver. The GPS was set to Kinematic Survey mode where the path is solved in post-processing. In this mode the horizontal precision is ±1cm+1ppm for a baseline under 10Km ► Localization was done through odometry, not DGPS. The DGPS was used only to get the actual position. Thus a degraded GPS solution is useless and positional data of a Q factor greater than 1 (with 1 being the best and 6 the worst) have been discarded Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 18 Autonomous SoC for Fuzzy Robot Path Tracking Showcase Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 19 Autonomous SoC for Fuzzy Robot Path Tracking Conclusions ► A novel SoC for the path following task of autonomous non-holonomic mobile robots was presented. ► The latency of the control is very small although it is bounded by the response of the controlled system i.e. the robot. ► Simulations and field experiments showed that the fuzzy tracking algorithm, introduced by the authors, and the overall system performance is satisfactory even under the limitations presented by the real system, e.g. the quantization of available steering commands. This is due to the high robustness exhibited by the fuzzy tracking algorithm along with the “smoothing” behavior of the spatial window technique inserted in the control loop. Kyriakos M. Deliparaschos, George P. Moustris, Spyros G. Tzafestas 20