2016 Swarmathon RSS Workshop Technical Tutorial Joshua Hecker - - PowerPoint PPT Presentation

▶
2016 swarmathon
SMART_READER_LITE
LIVE PREVIEW

2016 Swarmathon RSS Workshop Technical Tutorial Joshua Hecker - - PowerPoint PPT Presentation

Mar 10, 2023 151 likes •811 views

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

slide-1
SLIDE 1

1

2016 Swarmathon RSS Workshop

Technical Tutorial

Joshua Hecker & Matthew Fricke

slide-2
SLIDE 2
  • 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

Tutorial Outline

slide-3
SLIDE 3

3

Introduction

slide-4
SLIDE 4
  • 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 AltIMU: accelerometer, gyroscope, magnetometer – Ping))) ultrasounds – U-blox GPS – Magnetic quadrature wheel encoders – Logitech C170 webcam

  • Power:

– 14.8V, 13.6 Ah Li-Ion battery

4

Physical Platform

Introduction

slide-5
SLIDE 5

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

slide-6
SLIDE 6

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

slide-7
SLIDE 7

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

slide-8
SLIDE 8
  • 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

8

Virtual Platform

Introduction

slide-9
SLIDE 9

9

Virtual Platform: Demo

Introduction

slide-10
SLIDE 10
  • 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

Overview of ROS Framework

Introduction

slide-11
SLIDE 11
  • 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

Overview of ROS Framework

Introduction

slide-12
SLIDE 12
  • 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

Overview of ROS Framework

Introduction

slide-13
SLIDE 13
  • 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

Overview of ROS Framework

Introduction

slide-14
SLIDE 14
  • 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

Overview of ROS Framework

Introduction

slide-15
SLIDE 15

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

slide-16
SLIDE 16
  • 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

Tutorial Outline

slide-17
SLIDE 17

Localization is Hard

slide-18
SLIDE 18
  • 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

18

Recalibration

Localization

slide-19
SLIDE 19
  • 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

19

Recalibration

Localization

slide-20
SLIDE 20

20

Recalibration

Localization

Live Exercise: Try it Yourself!

slide-21
SLIDE 21

21

Recalibration

Localization

1. Initialize collection zone position 2. Set goal position after finding resource

slide-22
SLIDE 22

22

Recalibration

Localization

3. Update collection zone position after returning

slide-23
SLIDE 23

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

slide-24
SLIDE 24

No noise, perfect signal 5% noise 15% noise 20% noise

Time State Sensor Noise

Localization

slide-25
SLIDE 25

Kalman Filter State Time

True State Noisy Sensor Reading EKF Filtered

Localization

slide-26
SLIDE 26

Theory: Kalman Filter

  • Provably optimal when

noise is Gaussian distributed with zero mean.

  • And the control and

measurement models are linear.

P(noise) noise

Localization

slide-27
SLIDE 27

Linear Control and Observation

Observation Model: Control (transition) Model:

Localization

slide-28
SLIDE 28

Relating Control, Measurement and Noise

Localization

slide-29
SLIDE 29

Relating Control, Measurement and Noise

Localization

slide-30
SLIDE 30

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
  • ptimal but works well.

Localization

slide-31
SLIDE 31

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,

  • r position.

– Sensors might be IMU, encoders, or GPS.

Localization

slide-32
SLIDE 32

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

slide-33
SLIDE 33

The EKF Algorithm

Localization

slide-34
SLIDE 34

The EKF Algorithm

Input previous state estimate

Localization

slide-35
SLIDE 35

The EKF Algorithm

Output an estimate for the current state Input previous state estimate

Localization

slide-36
SLIDE 36

The EKF Algorithm

Previous covariance

Localization

slide-37
SLIDE 37

The EKF Algorithm

… and the updated covariance matrix. Previous covariance

Localization

slide-38
SLIDE 38

The EKF Algorithm

Current Observation

Localization

slide-39
SLIDE 39

The EKF Algorithm

The Kalman gain, , weights the sensor measurements according to, , the prediction covariance (noisiness).

Localization

slide-40
SLIDE 40

The EKF Algorithm

The new observation coefficient matrix depends

  • n how good the previous
  • ne was.

Localization

slide-41
SLIDE 41

The EKF Algorithm

Prediction Step

Localization

slide-42
SLIDE 42

The EKF Algorithm

Prediction Step Correction Step

Localization

slide-43
SLIDE 43

Sensor Fusion

home.wlu.edu/~levys/kalman_tutorial/ Sensor 2: Lower Covariance Sensor 1: Higher Covariance Ground Truth EKF Fusion

The EKF estimate is closer to the sensor with less noise.

Localization

slide-44
SLIDE 44

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

slide-45
SLIDE 45

EKF in ROS

  • Two nodes:

–ekf_localization: EKF implementation –Navsat node: sensor processing. Transforms latitude and longitude to the robots local frame.

Localization

slide-46
SLIDE 46

EKF localization node

Topic: odom/navsat Type: odom with covariance Topic: odom Type: odom Wheel encoders

Navsat transform node

Topic: imu Type: imu Topic: fix Type: navsat/fix Topic: odom/ekf Type: odometry with covariance

Localization

slide-47
SLIDE 47

EKF Node Basic Configuration

<rosparam param="imu0_config"> [false, false, false, true, true, true, false, false, false, true, true, true, true, true, true] </rosparam> <rosparam param="odom0_config"> [true, true, false, false, false, false, false, false, false, false, false, true, false, false, false] </rosparam>

x, y, z roll, pitch, yaw y velocity, y velocity, z velocity roll velocity, pitch velocity, yaw velocity x accel., y accel., z accel.

In ~/rover_workspace/launch/rover_name.launch

Localization

slide-48
SLIDE 48

<rosparam param="initial_estimate_covariance"> [0.8, 0, ..., 1e-9] </rosparam>

EKF Node Basic Configuration

<rosparam param="process_noise_covariance"> [0.04, 0, ..., 0.02] </rosparam> In ~/rover_workspace/launch/rover_name.launch

Localization

slide-49
SLIDE 49

In Practice

  • Can set initial values for, , the prediction

covariance matrix.

  • Small initial values tell the Swarmie that the

sensor has low noise.

  • High values tell ROS that the sensor is noisy.
  • Ideally the covariance is measured

experimentally for each sensor.

  • The initial value for can be the first sensor
  • value. That is .

Localization

slide-50
SLIDE 50

50

Alternatives

Localization

  • Exploit walls/corners:

– Recalibrate as with collection zone

  • Fallback to GPS:

– Outdoor error is typically < 1 m – Combine with IMU (compass)

  • Blob detection:

– Detect colors (white, black, orange) to identify tags, collection zone, walls

slide-51
SLIDE 51
  • 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)

51

Tutorial Outline

slide-52
SLIDE 52

Search in Operations Research

  • Bayesian Methods*

– Deciding when an area has been searched enough.

  • Deterministic Algorithms

– Coastguard spirals

  • Examples:

– Nuclear submarine (scorpion, sank in 1968) – atom bomb recovery (5 lost between 1950 and 1965) – Wreck survivor recovery (single Heliocopter)

*Stone, Lawrence D. 1975. Theory of Optimal Search. Academic Press New York.

Search

slide-53
SLIDE 53

Search in Ecology

  • Foraging animals:
  • e.g.
  • Correlated

Random Walks

  • Lévy walks

Méndez, Vicenç, Daniel Campos, and Frederic Bartumeus. 2013. Stochastic Foundations in Movement Ecology: Anomalous Diffusion, Front Propagation and Random Searches. Springer Science & Business Media.

Search

slide-54
SLIDE 54

The Needle in a haystack

DCs tagged with green dye. T cells with red dye. T cells must search a volume 106 times their own volume at a mean velocity of .11 um/s. To systematically explore the search zone would take 105 days. T cells are able to find cognate antigen in about 8 hours and give up after 12-24 hours.

Immunolgy

Search

Krummel, M. F., Bartumeus, F., & Gérard,

  • A. (2016). T cell migration, search

strategies and mechanisms. Nature Reviews Immunology.

slide-55
SLIDE 55

Lévy search

Step Length Distribution: Hausdorff Fractal Dimension:

Ecology Example

Search

slide-56
SLIDE 56

8 robots, 0.99 Hopkins

Getting the fractal dimension right improves performance. Lévy search with

  • ptimised fractal

dimension. Lévy search not

  • ptimised for the

target distribution. (– 26%). Brownian Search (–81%)

Search

slide-57
SLIDE 57

Deterministic Spiral Search

Archimedes Spiral

Square Spiral

  • Minimise oversampling.
  • Maximise coverage.
  • Find nearest targets first.
  • Square spiral minimises turns.
  • Minimise search extent.

To maximise search efficiency:

Search

slide-58
SLIDE 58

Distributed Spiral Search Algorithm (DDSA)

Argos3 Simulator

Black dots are targets (Power law distribution) Blue dots are robots (Simulated iAnts) Short detection diameter but applies to any distance. Continuous plane (no grid) Defined by a simple recurrence relation.

Search

slide-59
SLIDE 59

Interlocking Archimedes Spirals

Search

slide-60
SLIDE 60
  • 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)

60

Tutorial Outline

slide-61
SLIDE 61

61

Virtual Enhancements

  • Tuning model descriptions:

– Corrected mass and inertia values to match physical Swarmie robots

  • Gazebo physics solver:

– Decreased maximum step size of ODE solver to increase simulation accuracy

  • In progress:

– Tuning 3D meshes to match physical robots – Implementing simulated gripper

slide-62
SLIDE 62
  • 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)

62

Tutorial Outline

slide-63
SLIDE 63

63

Gripper

slide-64
SLIDE 64

64

Gripper

  • Kits being built for all physical teams
  • Laser-cut ABS; custom design
  • 5 x 5 x 5 cm cubes with printed AprilTag:
slide-65
SLIDE 65

65

Thank You!

Questions?