Vision Aided Inertial Navigation using Single Video Camera Measurements

1
Vision Aided Inertial Navigation using Single
Video Camera Measurements
Vasko Sazdovski
Faculty of Electrical Engineering, Goce Delcev
University, Shtip, R.Macedonia Pilot Training
Centre, ELMAK Elbit Systems of Macedonia, Airport
Petrovec, Skopje, R.Macedonia ROBOTILNICA
Intelligent Systems and Robotics, Skopje,
R.Macedonia
2
OUTLINE OF THE PRESENTATION

• INTRODUCTION
• THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING
  (SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE
  VECTOR.
• VISION SENSORS
• INERTIAL NAVIGATION AIDED BY SIMULTANEOUS
  LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION
  ALGORITHM
• INERTIAL NAVIGATION AIDED BY BEARING ONLY
  SIMULTANEOUS LOCALIZATION AND MAPPING
• CONCLUSIONS
• FUTURE WORK POSSIBILITIES

3
(No Transcript)
4
INTRODUCTION

• Integrated navigation system design requires
  selection of
• set of a sensors and
• computation power
• that provides reliable and accurate navigation
  parameters (position, velocity and attitude) with
  high update rates and bandwidth in small and cost
  effective manner.

Navigator SL Cloud Cap Technology USA
Modern INS/GPS Navigation System for UAVs Size
131 x 57 x 19 mm  Weight 80 grams
Piccolo autopilot used on UAV for collecting
meteorological or map data over long distances.
Cloud Cap Technology USA.
5
INTRODUCTION

• Integrated navigation system design requires
  selection of
• set of a sensors and
• computation power
• that provides reliable and accurate navigation
  parameters (position, velocity and attitude) with
  high update rates and bandwidth in small and cost
  effective manner.

Always Innovating Cortex-A9 SoC Module which can
run from 1.0GHz to 1.5GHz
1GB RAM, SD card, 2.4GHz/5GHz Wi-Fi and Bluetooth
connectivity.
MeCam quadrotor
6
INTRODUCTION
Many of todays operational navigation systems
rely on inertial sensor measurements.
Inertial navigation is diverging slowly from the
real measurements with time.
7
INTRODUCTION
Additional sources of aiding information to the
navigation parameters ( non interactive aiding
information)
Ching-Fang Lin Modern Navigation Guidance and
Control Processing Prentice Hall Inc. 1991
8
INTRODUCTION
Additional sources of aiding information to the
navigation parameters (interactive aiding
information)
Vision sensors are must nowadays
9
THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING
(SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE
VECTOR.
Unknown environment
Simultaneous Localization and Mapping (SLAM) uses
relative measurements (range and bearing) from
the vehicle with respect to the environment to
build a map whilst simultaneously using the
generated map computes vehicle position.
10
THEORY OF SIMULTANEOUS LOCALIZATION AND MAPPING
(SLAM). INERTIAL NAVIGATION AND AUGMENTED STATE
VECTOR.
Repeated relative measurements of a map point
(circular movement)
Mapping of an unknown map point
Localization using the map point
provide valuable informations of the spatial
relationship of the map point and the vehicle
11
VISION SENSORS
Observation models
12
INERTIAL NAVIGATION AIDED BY SIMULTANEOUS
LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION
ALGORITHM
The augmented state vector containing the vehicle
state and the map point estimates is denoted as
13
INERTIAL NAVIGATION AIDED BY SIMULTANEOUS
LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION
ALGORITHM
These equations are repeated until we augment the
state vector with sufficient number of map
point estimates. Through simulations we realized
that we need more then three map point estimates.
14
INERTIAL NAVIGATION AIDED BY SIMULTANEOUS
LOCALIZATION AND MAPPING. SLAM AS SENSOR FUSION
ALGORITHM
As the vehicle circles around the map point,
repeated measurements
are available from the stereo camera.
Using these measurements we can start updating
the augmented state vector by using each of the
map point estimates at a time. Extended Kalman
Filter (EKF) can be implemented for the update.
15
PERFORMANCES OF INERTIAL NAVIGATION AIDED BY
SIMULTANEOUS LOCALIZATION AND MAPPING
Map point estimates
True estimated and divergent vehicle trajectories
Position errors
16
PERFORMANCES OF INERTIAL NAVIGATION AIDED BY
SIMULTANEOUS LOCALIZATION AND MAPPING
Determinant of the covariance matrix of the first
map point estimate
The contour ellipsoid of the covariance matrix of
the first map point estimate
Map point estimates
17
PERFORMANCES OF INERTIAL NAVIGATION AIDED BY
SIMULTANEOUS LOCALIZATION AND MAPPING
Determinant of the covariance matrix of the
vehicle position
The relative distance between the first and
second map point estimates
The contour ellipsoid of the covariance matrix of
the vehicle position
18
INERTIAL NAVIGATION AIDED BY BEARING ONLY
SIMULTANEOUS LOCALIZATION AND MAPPING
Bearing Only Simultaneous Localization and
Mapping (BOSLAM) is very attractive these days
because it permits the use of single camera as
sensor for measuring the bearing i.e. unit
direction to the map points.
The major drawback of this solution is the
problem of map point initialization from a single
measurement.
19
INERTIAL NAVIGATION AIDED BY BEARING ONLY
SIMULTANEOUS LOCALIZATION AND MAPPING
Much of the research in BOSLAM is focused towards
the problem of initialization of the map points
from single camera measurements. In the
literature two techniques are proposed to address
the problem of map point initialization.
The first technique involves delaying the map
point initialization until a criterion is
fulfilled and sufficient baseline is available
from different vehicle positions.
1.
Both techniques have their pros and cons
The second technique tries to avoid the delay and
initialize the map point from a single
measurement The fact that after the first
observation, the map point lies along on the line
from the vehicle to the map point (the projection
ray) is used
2.
The approach presented before to augment the
state vector not only with one map point estimate
brings new parameterization of the map point in
BOSLAM. The novelty comes from the usage of the
certain number of map point estimates for update
of the whole augmented state vector together with
a combination of repeated measurements and motion
in vicinity of the map point. This approach
brings delayed initialization of the map points.
20
INERTIAL NAVIGATION AIDED BY BEARING ONLY
SIMULTANEOUS LOCALIZATION AND MAPPING
Discussion on the observability of a system
provides insights and understanding of the
fundamental limits of the estimation processes.
Since observability analysis can give the best
achievable performance even before the system is
built, it can be considered as tool for computer
analysis of many complicated estimation processes.
21
INERTIAL NAVIGATION AIDED BY BEARING ONLY
SIMULTANEOUS LOCALIZATION AND MAPPING
Inertial Navigation aided by BOSLAM as well as
many other navigation problems are nonlinear and
must be linearized (approximated) before applying
the popular Kalman-like filtering algorithms. An
EKF presents one such approximation.
The EKF is very commonly used algorithm and,
because of its simplicity, is very often chosen
as the best algorithm for implementation.
22
INERTIAL NAVIGATION AIDED BY BEARING ONLY
SIMULTANEOUS LOCALIZATION AND MAPPING
Inertial Navigation aided by BOSLAM as well as
many other navigation problems are nonlinear and
must be linearized (approximated) before applying
the popular Kalman-like filtering algorithms. An
EKF presents one such approximation.
The EKF is very commonly used algorithm and,
because of its simplicity, is very often chosen
as the best algorithm for implementation.

• Because of numbers of significant problems that
  appear when implementing the EKF, other
  algorithms such as
• Iterated Extended Kalman Filter (IEKF)
• Unscented Kalman Filter (UKF)
• Unscented Particle Filter (UPF)
• were implemented, tested and compared with the
  EKF algorithm.

23
PERFORMANCES OF INERTIAL NAVIGATION AIDED BY
BEARING ONLY SIMULTANEOUS LOCALIZATION AND
MAPPING
True estimated and divergent vehicle trajectories
24
PERFORMANCES OF INERTIAL NAVIGATION AIDED BY
BEARING ONLY SIMULTANEOUS LOCALIZATION AND
MAPPING
Map point estimates
25
CONCLUSIONS

• Aiding Inertial navigation (IN) by BOSLAM
  exhibits a high degree of nonlinearity and
  typically in these applications an EKF introduces
  large estimation errors.
• Other algorithms such as UKF and UPF demonstrate
  best performance and appear to be efficient
  estimators for the concept of IN aided by BOSLAM.
• The SLAM aided IN and BOSLAM aided IN sensor
  fusion algorithm present reliable solutions that
  provide aiding information to IN from vision
  sensors. These algorithms successfully integrate
  the inertial and vision sensors with no a priori
  knowledge of the environment.

26
QUADROTOR RESEARCH ARENA
GRASP Lab, University of Pennsylvania, extensive
use of motion capture systems
Vicon T Series motion capture System
27
QUADROTOR RESEARCH ARENA
Flying Machine arena , Institute for Dynamic
Systems and Control ETH Zurich
28
QUADROTOR RESEARCH ARENA
Localization and mapping done with two Hokuyo
lidars and a servo motor. University of
Pennsylvania
CityFlyer project, Robotics and Intelligent
Systems Lab, City College of New York, City
University of New York.
STARMAC Project in the Hybrid Systems Lab at UC
Berkeley 
29
QUADROTOR RESEARCH ARENA
Robotics Group, CSAIL MIT

• Hokuyo laser range-finder (1),
• stereo cameras (2),
• monocular color camera (3),
• laser-deflecting mirrors for altitude (4),
• 1.6GHz Intel Atom-based flight computer (5),
• Ascending Technologies internal processor and IMU
  (6).

30
QUADROTOR RESEARCH ARENA
sFly European Project Visual-Inertial SLAM for a
small hexacopter (IROS 2012 video screenshot)
Computer Vision Group at TUM Germany Autonomous
Camera-Based Navigation of a Low-Cost Quadrocopter
31
FUTURE WORK POSSIBILITIES
In our future research we are working on
developing novel approaches i.e. efficient
estimators (sensor fusion algorithms) which will
provide navigation performances and accuracy to a
level close to the Differential GPS using only
single video camera and inertial sensors.
Our research work is very much focused on the
practical experimentation and validation of the
before defined problems. Quadrotor UAV is the
chosen platform for the practical experiments.
Quadrotor Flying Frog ROBOTILNICA Intelligent
Systems and Robotics
Custom modified quadrotor from KKMulticopter
South Korea
32
FUTURE WORK POSSIBILITIES
We are fascinated by insect based navigation.
Flying insects especially inspire much of our
research on autonomous navigation systems.
We are very much interested in how the flying
insects adept at maneuvering in complex,
unconstrained, hostile and hazardous
environments.
33
FUTURE WORK POSSIBILITIES
Our approach with the practical experiments
presents a low cost approach. Low cost Inertial
Measurement Unit (IMU) from Sparkfun Electronics
provides the angular rates (gyro) measurements
and accelerometer measurements. These inertial
sensors data together with the video from a
smartphone camera are transferred over WiFi
and/or 3G to a remote server/PC for processing.
The video processing is performed on the remote
PC using OpenCV libraries, together with the
sensor fusion algorithms. The results are
transferred back to the quadrotor for guidance
and control of the vehicle for performing the
required maneuvers (motion) and the vehicle task
itself.
34
FUTURE WORK POSSIBILITIES
As with the case of singe UAV, simply passing or
flying by a map point with no manoeuvre will not
help much for autonomous navigation for swarm of
UAVs. Coordination and cooperation between the
UAVs performing manoeuvres over same environment
features (map points) are needed. This
investigation is expected to address the issues
when low accuracy vision sensors are used on
UAVs. In this scenario the UAVs can take
measurements not just of the environment but also
of each other. It is expected that these
measurements can accelerate the convergence of
the novel algorithms under experimentations
35
THANK YOU FOR YOUR ATTENTION
?
QUESTIONS ARE WELCOMED