2016 Swarmathon RSS Workshop Technical Tutorial Joshua Hecker & Matthew Fricke 1
Tutorial Outline • Introduction ( 20 min) – Physical Platform: Capabilities and Limitations – Virtual Platform + Demo • “Localization is Hard” ( 30 min) – Recalibration walkthrough – Extended Kalman Filters: Theory and Practice – Alternate strategies • Search Algorithms ( 10 min) • Virtual Enhancements ( 15 min) • Physical Gripper ( 10 min) 2
Introduction 3
Physical Platform • Computation and Control: – Arduino Leonardo-like microcontroller – Intel NUC mini PC – 8 GB RAM, 32 GB SSD, WiFi/Bluetooth • Actuation: – 12V 131:1 DC brushed motors – Hitec servo motors • Sensing: – 10-axis Alt IMU: accelerometer, gyroscope, magnetometer – Ping))) ultrasounds – U-blox GPS – Magnetic quadrature wheel encoders – Logitech C170 webcam • Power: – 14.8V, 13.6 Ah Li-Ion battery Introduction 4
Physical Platform: Computation & Control • Microcontroller: – Receives IMU, ultrasound, encoder data – Actuates wheels and servos – Serial communication w/ PC • PC: – Receives camera, GPS data via USB – Runs Ubuntu Linux 14.04, ROS Indigo Igloo – Provides high-level autonomous control Introduction
Physical Platform: Actuation • DC Motors: – 8-bit PWM control – Includes built-in encoders – Provides skid steer control • Gripper: – “Fingers” and “wrist” – DC-DC converter for current – Maintains position over time Introduction
Physical Platform: Sensing • IMU provides: – x/y/z acceleration – noisy – x/y/z velocity – accurate, drifts over time – x/y/z gravity – accurate, needs calibration • Ultrasound provides: – Distance – highly accurate, false positives • GPS provides: – Lat/long – very noisy, mitigates drift • Encoders provide: – x/y/ ω position and velocity – highly accurate, rapidly accumulates drift • Camera provides: – 640 x 480 RGB image – sensitive to blur Introduction
Virtual Platform • Gazebo: – High-fidelity physics simulator – Plugin interface replicates physical sensors and actuators • Models: – 3D mesh representation of real Swarmie – Includes joints that provide for realistic movement – Also represents walls, ground, and tags/blocks • Model Descriptions: – XML-like description of physical characteristics – Sensor/actuator plugins implement noise Introduction 8
Virtual Platform: Demo Introduction 9
Overview of ROS Framework • Robot functionality encapsulated in ROS “nodes” and “packages” • ROS Master facilitates peer-to-peer network communication between nodes • Publish-subscribe system allows C++ programs to share information on relevant topics Introduction
Overview of ROS Framework • Robot functionality encapsulated in ROS “nodes” and “packages” • ROS Master facilitates peer-to-peer network communication between nodes • Publish-subscribe system allows C++ programs to share information on relevant topics Introduction
Overview of ROS Framework • Robot functionality encapsulated in ROS “nodes” and “packages” • ROS Master facilitates peer-to-peer network communication between nodes • Publish-subscribe system allows C++ programs to share information on relevant topics Introduction
Overview of ROS Framework • Robot functionality encapsulated in ROS “nodes” and “packages” • ROS Master facilitates peer-to-peer network communication between nodes • Publish-subscribe system allows C++ programs to share information on relevant topics Introduction
Overview of ROS Framework • Robot functionality encapsulated in ROS “nodes” and “packages” • ROS Master facilitates peer-to-peer network communication between nodes • Publish-subscribe system allows C++ programs to share information on relevant topics Introduction
Online Resources • Offical Website: http://www.NasaSwarmathon.com • Swarmathon Codebase: https://github.com/BCLab-UNM/Swarmathon-ROS • Install: http://wiki.ros.org/indigo/Installation • Tutorials: http://wiki.ros.org/ROS/Tutorials • Resources: – ROS: http://wiki.ros.org – Git: http://git-scm.com/doc Introduction
Tutorial Outline • Introduction ( 20 min) – Physical Platform: Capabilities and Limitations – Virtual Platform + Demo • “Localization is Hard” ( 30 min) – Recalibration walkthrough – Extended Kalman Filters: Theory and Practice – Alternate strategies • Search Algorithms ( 10 min) • Virtual Enhancements ( 15 min) • Physical Gripper ( 10 min) 16
Localization is Hard
Recalibration • Intuition: – The collection zone represents a known, fixed position in space – When a robot detects the zone, the robot’s position becomes known as well – The robot should exploit this spatial relationship to correct for drift in its sensors Localization 18
Recalibration • Implementation: 1. Initialize collection zone position at (0,0) 2. Search for resources 3. When resource is found, set goal position equal to current collection zone position 4. When collection zone is detected, update collection zone position by setting it equal to current position 5. Return to Step 2 Localization 19
Recalibration Live Exercise: Try it Yourself! Localization 20
Recalibration 1. Initialize collection zone position 2. Set goal position after finding resource Localization 21
Recalibration 3. Update collection zone position after returning Localization 22
Using Sensor Data Effectively • A major challenge during the competition was accurate positioning. • Swarmies have the following sensors: – Inertial Measurement Unit (IMU) – Camera – Wheel Encoders – Global Positioning • Each have different noise properties. Localization
Sensor Noise No noise, perfect signal 5% noise State 20% noise 15% noise Time Localization
Kalman Filter True State Noisy Sensor Reading EKF Filtered State Time Localization
Theory: Kalman Filter P(noise) • Provably optimal when noise is Gaussian distributed with zero mean. noise • And the control and measurement models are linear. Localization
Linear Control and Observation Control (transition) Model: Observation Model: Localization
Relating Control, Measurement and Noise Localization
Relating Control, Measurement and Noise Localization
Extended Kalman Filter • The Kalman filter assumed linear relationships. • In real systems the relationship may be non- linear. • But we can take the derivative of the signal to linearize it. • This is the extended Kalman filter. No longer optimal but works well. Localization
Sensor Fusion with Multiple States • Recall the sensor measurement is: • We would like to generalise this to multiple sensors with different kinds of states: – For example, states might be yaw-angle, velocity, or position. – Sensors might be IMU, encoders, or GPS. Localization
Sensor Fusion with Multiple States • Recall the sensor measurement is: • If we have three sensors with 2 different states we can write: With each row corresponding to a sensor and each column to a type of state. Localization
The EKF Algorithm Localization
Input previous The EKF Algorithm state estimate Localization
Input previous The EKF Algorithm state estimate Output an estimate for the current state Localization
Previous The EKF Algorithm covariance Localization
Previous The EKF Algorithm covariance … and the updated covariance matrix. Localization
Current The EKF Algorithm Observation Localization
The Kalman gain, , The EKF Algorithm weights the sensor measurements according to, , the prediction covariance (noisiness). Localization
The EKF Algorithm The new observation coefficient matrix depends on how good the previous one was. Localization
The EKF Algorithm Prediction Step Localization
The EKF Algorithm Prediction Step Correction Step Localization
Sensor Fusion Sensor 1: Higher Covariance Ground Truth Sensor 2: Lower Covariance EKF Fusion The EKF estimate is closer to the sensor with less noise. Localization home.wlu.edu/~levys/kalman_tutorial/
In Practice • The EKF is a recursive function that calls itself at each time step to improve the robot state estimate. • It weights sensor data by the variance they contribute to the state prediction. • ROS has an EKF package to do all this for us* *Moore, Thomas, and Daniel Stouch . 2016. “A Generalized Extended Kalman Filter Implementation for the Robot Operating System.” In Intelligent Autonomous Systems 13 , 335 – 48. Springer. Localization
EKF in ROS • Two nodes: – ekf_localization : EKF implementation – Navsat node : sensor processing. Transforms latitude and longitude to the robots local frame. Localization
Topic: fix Type: navsat/fix Topic: imu Navsat transform node Type: imu Topic: odom/navsat Type: odom with covariance Topic: odom EKF localization node Type: odom Wheel encoders Topic: odom/ekf Type: odometry with covariance Localization
Recommend
More recommend
