Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation...
Transcript of Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation...
![Page 1: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/1.jpg)
IROS 2016 Workshop on State Estimation and Terrain PerceptionDaejeon Convention Center - Daejeon, Korea
Multi-Sensor State Estimation on Dynamic Quadruped Robots
Marco Camurri [email protected] Legged System Lab
Istituto Italiano di Tecnologia – Genoa, Italy
![Page 2: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/2.jpg)
October 14th, 2016 Marco Camurri 2/30
Summary
1) Hydraulic Quadruped (HyQ)– Description– Characteristic motions– Sensors
2) State Estimation– Overview– Modules– Applications
3) Mapping– Definition– Applications
![Page 3: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/3.jpg)
October 14th, 2016 Marco Camurri 3/30
Hydraulic Quadruped (HyQ)
![Page 4: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/4.jpg)
October 14th, 2016 Marco Camurri 4/30
Specifications
● 12 Degrees of Freedom● ~80 kg● 1 m x 0.5 m x ~0.8 m● Fully torque controlled● Fully hydraulic● 145 Nm (at 16 MPa)
http://www.iit.it/hyq
![Page 5: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/5.jpg)
October 14th, 2016 Marco Camurri 5/30
Characteristic Motions
● Planned crawl● Trot● Flying trot● Chimney Climb
https://www.youtube.com/HydraulicQuadruped
![Page 6: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/6.jpg)
October 14th, 2016 Marco Camurri 6/30
MiniHyQ, HyQ2Max, and...
MiniHyQ HyQ2Max (source: Reuters)
MOOG @ IIT Joint LabIntegrated Servo Actuators
http://moog.iit.it
![Page 7: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/7.jpg)
October 14th, 2016 Marco Camurri 7/30
Sensors
● Microstrain GX3-25
● Optical encoders
● Load cells
● ASUS Xtion
● Multisense SL
● Hokuyo URG-04LX
![Page 8: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/8.jpg)
October 14th, 2016 Marco Camurri 8/30
State Estimation
![Page 9: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/9.jpg)
October 14th, 2016 Marco Camurri 10/30
Pronto
● Modular● EKF-based● History of meas.● Open Source*
● Used/tested for the DRC (MIT, ViGIR, …)
● LCM§ based § https://lcm-proj.github.io/Drift-free humanoid state estimation fusing kinematic, inertial and LIDAR sensing
M. Fallon, M. Antone, N. Roy and S. Teller2014 IEEE-RAS International Conference on Humanoid Robots, Madrid, 2014 *https://github.com/ipab-slmc/pronto-distro
![Page 10: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/10.jpg)
October 14th, 2016 Marco Camurri 11/30
Modules
● Proprioceptive:– IMU (prediction)– Leg Odometry
● Exteroceptive:– Visual Odometry (FOVIS)– Gaussian Particle Filter (GPF)– Fast and Robust Scan Matcher (FRSM)– Vicon (ground truth)– ...
![Page 11: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/11.jpg)
October 14th, 2016 Marco Camurri 12/30
Modules
● Proprioceptive:– IMU (prediction) → bias, drift– Leg Odometry → drift, slippage, leg compliance
● Exteroceptive:– Visual Odometry (FOVIS) → featureless areas– Gaussian Particle Filter (GPF) → pre-acquired map– Fast and Robust Scan Matcher (FRSM) → only planar– Vicon (ground truth)– ...
![Page 12: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/12.jpg)
October 14th, 2016 Marco Camurri 13/30
State Estimation Scheme
Stance Detection
Leg Odometry
IMU EKFω II , x II
~x BB
q , q , τ
ωBB , xBB , θB-
W , xB-
B , x B-
W
Controller
~s ∈B4
Ethernet~x BB , ~x BW , ~
θBW
ωBB , xBB , θB+
W , xB+
B , x B+
W
Clouds, LiDAR scans, ...
GPFFRSM VO ...
1 kHz
500 Hz
10-40 Hz
250 kHz
250 kHz
250 kHz
500 Hz
250 kHz
10-40 Hz
500 Hz
![Page 13: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/13.jpg)
October 14th, 2016 Marco Camurri 15/30
Leg Odometry
● Ground Reaction Forces estimation● Stance Detection● Velocity computation● Covariance estimation
![Page 14: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/14.jpg)
October 14th, 2016 Marco Camurri 16/30
FRSM and Trunk Control
● RCF with push recovery
● Robot controlled to stay on target position
● Hokuyo URG 04-LX
A reactive controller framework for quadrupedal locomotion on challenging terrainVictor Barasuol, Jonas Buchli, Claudio Semini, Marco Frigerio, Edson R De Pieri, Darwin G Caldwell2013 IEEE International Conference on Robotics and Automation (ICRA)
RANGE - Robust Autonomous Navigation in GPS-denied EnvironmentsAbraham Bachrach, Samuel Prentice, Ruijie He Nicholas RoyJournal of Field Robotics, 2011
![Page 15: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/15.jpg)
October 14th, 2016 Marco Camurri 17/30
Gaussian Particle Filter
● Tested on Atlas/Drones
● Suitable for aggressive motions
● High Quality map required
State estimation for aggressive flight in GPS-denied environments using onboard sensingA. Bry, A. Bachrach and N. Roy2012 IEEE International Conference on Robotics and Automation (ICRA), Saint Paul, MN, 2012
![Page 16: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/16.jpg)
October 14th, 2016 Marco Camurri 18/30
Gaussian Particle Filter
IMU+Leg Odometry
IMU+Leg Odometry+Gaussian Particle Filter
Ground truthEstimate
Ground truthEstimate
![Page 17: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/17.jpg)
October 14th, 2016 Marco Camurri 19/30
FOVIS
● Tested on Atlas/Drones
● Lightweight● Position or
velocity measure
Visual Odometry and Mapping for Autonomous Flight Using an RGB-D Camera. Albert S. Huang, Abraham Bachrach, Peter Henry, Michael Krainin, Daniel Maturana, Dieter Fox, and Nicholas Roy. Int. Symposium on Robotics Research (ISRR), Flagstaff, Arizona, USA, Aug. 2011
![Page 18: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/18.jpg)
October 14th, 2016 Marco Camurri 20/30
FOVIS
● Tested on Atlas/Drones
● Lightweight● Position or
velocity measure
Visual Odometry and Mapping for Autonomous Flight Using an RGB-D Camera. Albert S. Huang, Abraham Bachrach, Peter Henry, Michael Krainin, Daniel Maturana, Dieter Fox, and Nicholas Roy. Int. Symposium on Robotics Research (ISRR), Flagstaff, Arizona, USA, Aug. 2011
![Page 19: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/19.jpg)
October 14th, 2016 Marco Camurri 21/30
Selective ICP
● Selective ICP: register only the points in motion, geometrically relevant
● Fuse with IMU
Real-time depth and inertial fusion for local slam on dynamic legged robots.M. Camurri, S. Bazeille, C. Semini, and D. G. CaldwellIEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 2015
![Page 20: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/20.jpg)
October 14th, 2016 Marco Camurri 22/30
Selective ICP
● Frame-to-frame background subtraction
● Morphologic dilation
● Point cloud selection
● Iterative Closest Point (ICP) registration
● Black image (no edges) no →motion
t-1 t
Real-time depth and inertial fusion for local slam on dynamic legged robots.M. Camurri, S. Bazeille, C. Semini, and D. G. CaldwellIEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 2015
![Page 21: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/21.jpg)
October 14th, 2016 Marco Camurri 23/30
Mapping
![Page 22: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/22.jpg)
October 14th, 2016 Marco Camurri 24/30
Robocentric Mapping
● Current sensed cloud is the most trustworthy● Current map should accumulate drift backwards
● Current map is the newest cloud plus previous map aligned to the current cloud
● Less accurate data is the oldest, and automatically discarded when out of scope
M n=M(n−1)+ T n(n−1 ) ⋅Cn
Real-time depth and inertial fusion for local slam on dynamic legged robots.M. Camurri, S. Bazeille, C. Semini, and D. G. CaldwellIEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 2015
![Page 23: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/23.jpg)
October 14th, 2016 Marco Camurri 25/30
Visual Pattern Classification
● Local heightmap around target footholds● Each heightmap is classified to select an offset correction
on the touch down coordinateReactive trotting with foot placement corrections through visual pattern classificationV. Barasuol, M. Camurri, S. Bazeille, D. G. Caldwell and C. SeminiIntelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on, Hamburg, 2015
![Page 24: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/24.jpg)
October 14th, 2016 Marco Camurri 27/30
Octomap and planning
On-line and On-board Planning and Perception for Quadrupedal LocomotionC. Mastalli, I. Havoutis, A. W. Winkler, D. G. Caldwell and C. SeminiIEEE International Conference on Technologies for Practical Robot Applications (TEPRA) 2015
● Scan with PTU● Scan Merging with
Octomap● Feature extraction● Reward computation● Planning from reward
map
![Page 25: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/25.jpg)
October 14th, 2016 Marco Camurri 28/30
Conclusions
● State Estimation is crucial for robot control, mapping and planning
● Multiple sources help being robust against more scenarios
● Local mapping helps keeping uncertainty away from where you want to operate
![Page 26: Multi-Sensor State Estimation on Dynamic Quadruped Robots...IROS 2016 Workshop on State Estimation and Terrain Perception Daejeon Convention Center - Daejeon, Korea Multi-Sensor State](https://reader036.fdocuments.us/reader036/viewer/2022081411/60a7a5637d819d2222391910/html5/thumbnails/26.jpg)
October 14th, 2016 Marco Camurri 29/30
Alex Posatskiy Jose Colmenares
Bilal Ur Rehman Carlos Mastalli
Marco Camurri Yifu Gao
Roy Featherstone
Janne Koivumaki Elco Heijmink
Claudio Semini Marco Frigerio Victor Barasuol Michele Focchi Andreea RadulescuRomeo Orsolino
Sep Driessen
The Dynamic Legged System Lab and friends: