amcl.cc

机器人仿真软件

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000 Brian Gerkey and others
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Adaptive Monte-Carlo localization
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl.cc,v 1.70.2.2 2006/07/13 17:59:43 gerkey Exp $
//
// Theory of operation:
//  TODO
//
// Requires: position (odometry)
// Provides: localization
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_amcl amcl
 * @brief Adaptive Monte Carlo localization

The @p amcl driver implements the Adaptive Monte-Carlo
Localization algorithm described by Dieter Fox.

At the conceptual level, the @p amcl driver maintains a probability
distribution over the set of all possible robot poses, and updates this
distribution using data from odometry, sonar and/or laser range-finders.
The driver also requires a pre-defined map of the environment against
which to compare observed sensor values.

At the implementation level, the @p amcl driver represents the probability
distribution using a particle filter.  The filter is "adaptive" because
it dynamically adjusts the number of particles in the filter: when the
robot's pose is highly uncertain, the number of particles is increased;
when the robot's pose is well determined, the number of particles
is decreased.  The driver is therefore able make a trade-off between
processing speed and localization accuracy.

As an example, consider the sequence of images shown below.  This sequence
shows the filter converging from an initial configuration in which the
pose of the robot is entirely unknown to a final configuration in which
the pose of the robot is well determined.  At the same time, the number
of particles in the filter decreases from 100,000 to less than 100.
Convergence in this case is relatively slow.

@image html amcl-phe200-0010.jpg "t = 1 sec, approx 100,000 particles"
@image html amcl-phe200-0400.jpg "t = 40 sec, approx 1,000 particles"
@image html amcl-phe200-0800.jpg "t = 80 sec, approx 100 particles"
@image html amcl-phe200-1200.jpg "t = 120 sec, approx 100 particles"


The @p amcl driver has the the usual features --
and failures -- associated with simple Monte-Carlo Localization
techniques:
  - If the robot's initial pose is specified as being completely
  unknown, the driver's estimate will usually converge to correct
  pose.  This assumes that the particle filter starts with a large
  number of particles (to cover the space of possible poses), and that
  the robot is driven some distance through the environment (to
  collect observations).
  - If the robot's initial pose is specified accurately, but
  incorrectly, or if the robot becomes lost (e.g., by picking it up
  and replacing it elsewhere) the driver's estimate will not converge
  on the correct pose.  Such situations require the use of more
  advanced techniques that have not yet been implemented.

The @p amcl driver also has some slightly unusual temporal
behavior:
  - When the number of particles in the filter is large, data may
  arrive from the sensors faster than it can be processed.  When this
  happens, data is queued up for later processing, but the driver
  continues to generate an up-to-date estimate for the robot pose.
  Thus, for example, at time t = 10 sec, the driver may have only
  processed sensor readings up until time t = 5 sec, but will
  nevertheless generate an estimate (prediction) of where the robot is
  at t = 10 sec.  The adaptive nature of the algorithm more-or-less
  guarantees that the driver will eventual "catch up": as more
  sensor readings are processed, the number of particles will
  generally decrease, and the sensor update step of the algorithm will
  run faster.

@par Caveats

At the time of writing, this driver is still evolving.  The sensor
models, in particular, are currently over-simplified and
under-parameterized (there are lots of magic numbers lurking about the
place).  Consequently, while this driver is known to work for certain
hardware configurations (think Pioneer2DX with a SICKLMS200 laser
range-finder), other configurations may require some refinement of the
sensor models.

@par Provides

- @ref interface_localize : this interface provides a (sort of)
  representative sample of the current pose hypotheses, weighted by likelihood.
- @ref interface_position2d : this interface provides just the
  most-likely hypothesis, formatted as position data, which you can
  (at your peril) pretend came from a perfect odometry system

@par Requires

The @p amcl driver requires the following interfaces, some of them named:

- "odometry" @ref interface_position2d : source of odometry information
- @ref interface_laser : source of laser scans
- "laser" @ref interface_map : a map in which to localize the
   robot, by fusing odometry and laser/sonar data.
- In principle supported, but currently disabled are:
    - @ref interface_fiducial
    - "imu" @ref interface_position2d
    - @ref interface_sonar
    - @ref interface_gps
    - @ref interface_wifi

@par Configuration requests

- TODO

@par Configuration file options

- Particle filter settings:
  - odom_init (integer)
    - Default: 1
    - Use the odometry device as the "action" sensor
  - pf_min_samples (integer)
    - Default: 100
    - Lower bound on the number of samples to maintain in the particle filter.
  - pf_max_samples (integer)
    - Default: 10000
    - Upper bound on the number of samples to maintain in the particle filter.
  - pf_err (float)
    - Default: 0.01
    - Control parameter for the particle set size.  See notes below.
  - pf_z (float)
    - Default: 3
    - Control parameter for the particle set size.  See notes below.
  - init_pose (tuple: [length length angle])
    - Default: [0 0 0] (m m rad)
    - Initial pose estimate (mean value) for the robot.
  - init_pose_var (tuple: [length length angle])
    - Default: [1 1 2pi] (m m rad)
    - Uncertainty in the initial pose estimate.
  - update_thresh (tuple: [length angle])
    - Default: [0.2 pi/6] (m rad)
    - Minimum change required in action sensor to force update in
      particle filter.
  - odom_drift[0-2] (float tuples)
    - Default:
      - odom_drift[0] [0.2 0.0 0.0]
      - odom_drift[1] [0.0 0.2 0.0]
      - odom_drift[2] [0.2 0.0 0.2]
    - Set the 3 rows of the covariance matrix used for odometric drift.
- Laser settings:
  - laser_pose (length tuple)
    - Default: [0 0 0]
    - Pose of the laser sensor in the robot's coordinate system
  - laser_max_beams (integer)
    - Default: 6
    - Maximum number of range readings being used
  - laser_range_max (length)
    - Default: 8.192 m
    - Maximum range returned by laser
  - laser_range_var (length)
    - Default: 0.1 m
    - Variance in range data returned by laser
  - laser_range_bad (float)
    - Default 0.1
    - ???
- Debugging:
  - enable_gui (integer)
    - Default: 0
    - Set this to 1 to enable the built-in driver GUI (useful
      for debugging).  Player must also be built with @p configure
      @p --enable-rtkgui for this option to have any effect.

@par Notes

- Coordinate System:
The origin of the global coordinate system corresponds to the center
of occupancy grid map.  Standard coordinate orientation is used; i.e.,
positive x is towards the right of the map, positive y towards the top
of the map.

- Number of particles:
The number of particles in the filter can be controlled using the
configuration file parameters @p pf_err and @p pf_z.  Specifically, @p
pf_err is the maximum allowed error between the true distribution and
the estimated distribution, while @p pf_z is the upper standard normal
quantile for (1 - p), where p is the probability that the error on the
estimated distribution will be less than @p pf_err.  If you dont know
what that means, dont worry, I'm not exactly sure either.  See Fox's
paper for a more meaningful explanation.

- Speed:
Many factors affect the speed at which the @p amcl driver
runs, but the following tips might be helpful:
  - Reducing the number of laser range readings being used (@p
    laser_max_beams in the configuration file) will significantly increase
    driver speed, but may also lead to slower convergence and/or less
    accurate localization.
  - Increasing the allowed error @p pf_err and reducing the quantile
    @p pf_z will lead to smaller particle sets and will hence increase
    driver speed.  This may also lead, however, to over-convergence.
As a benchmark, this driver has been successfully deployed on a
Pioneer2DX equipped with a SICK LMS200 and a 266MHz Mobile Pentium
with 32Mb of RAM.

- Memory:
The two key factors affecting memory usage are:
  - The size and resolution of the map.
  - The maximum number of particles.
As currently configured, the @p amcl driver will typically use 10 to
20Mb of memory.  On embedded systems, where memory is at a premium,
users may have to decrease the map resolution or the maximum number of
particles to achieve acceptable preformance.

@par Example: Using the amcl driver with a Pioneer robot

The following configuration file illustrates the use of the @p amcl
driver on a Pioneer robot equipped with a SICK LMS200 scanning laser
range finder:
@verbatim
driver
(
  name "p2os_position"
  provides ["odometry:::position2d:0"]
  port "/dev/ttyS0"
)
driver
(
  name "sicklms200"
  provides ["laser:0"]
  port "/dev/ttyS2"
)
driver
(
  name "mapfile"
  provides ["map:0"]
  resolution 0.05
  filename "mymap.pgm"
)
driver
(
  name "amcl"
  provides ["localize:0"]
  requires ["odometry:::position2d:0" "laser:0" "laser:::map:0"]
)
@endverbatim
Naturally, the @p port, @p filename and @p resolution values should be
changed to match your particular configuration.

@author Andrew Howard

@todo
- Implement / update other sensor models

*/

/** @} */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <assert.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>       // for atoi(3)
#include <sys/types.h>
#include <unistd.h>
#include <sys/time.h>

#define PLAYER_ENABLE_TRACE 1
#define PLAYER_ENABLE_MSG 1

#include <libplayercore/playercore.h>
#include <libplayercore/error.h>

#include "amcl.h"
// Sensors
#include "amcl_odom.h"
#include "amcl_laser.h"
//#include "amcl_fiducial.h"
//#include "amcl_gps.h"
//#include "amcl_imu.h"


////////////////////////////////////////////////////////////////////////////////
// Create an instance of the driver
Driver* AdaptiveMCL_Init( ConfigFile* cf, int section)
{
  return ((Driver*) (new AdaptiveMCL(cf, section)));
}


////////////////////////////////////////////////////////////////////////////////
// Register the driver
void AdaptiveMCL_Register(DriverTable* table)
{
  table->AddDriver("amcl", AdaptiveMCL_Init);
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Useful macros
#define AMCL_DATASIZE MAX(sizeof(player_localize_data_t), sizeof(player_position_data_t))


////////////////////////////////////////////////////////////////////////////////
// Constructor
AdaptiveMCL::AdaptiveMCL( ConfigFile* cf, int section)
    : Driver(cf, section)
{
  int i;
  double u[3];
  AMCLSensor *sensor;

  memset(&this->localize_addr, 0, sizeof(player_devaddr_t));
  memset(&this->position_addr, 0, sizeof(player_devaddr_t));

  // Do we create a localize interface?
  if(cf->ReadDeviceAddr(&(this->localize_addr), section, "provides",
                        PLAYER_LOCALIZE_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->localize_addr))
    {
      this->SetError(-1);
      return;
    }
  }

  // Do we create a position interface?
  if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides",
                        PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_addr))
    {
      this->SetError(-1);
      return;
    }
  }

  this->init_sensor = -1;
  this->action_sensor = -1;
  this->sensor_count = 0;

  player_devaddr_t odom_addr;
  player_devaddr_t laser_addr;
  //player_devaddr_t fiducial_addr;

  // Create odometry sensor
  if(cf->ReadDeviceAddr(&odom_addr, section, "requires",
                        PLAYER_POSITION2D_CODE, -1, "odometry") == 0)
  {
    this->action_sensor = this->sensor_count;
    if (cf->ReadInt(section, "odom_init", 1))
      this->init_sensor = this->sensor_count;
    sensor = new AMCLOdom(*this,odom_addr);
    sensor->is_action = 1;
    this->sensors[this->sensor_count++] = sensor;
  }

  // Create laser sensor
  if(cf->ReadDeviceAddr(&laser_addr, section, "requires",
                        PLAYER_LASER_CODE, -1, NULL) == 0)
  {
    sensor = new AMCLLaser(*this,laser_addr);
    sensor->is_action = 0;
    this->sensors[this->sensor_count++] = sensor;
  }

#if 0
  // Create fiducial sensor
  if(cf->ReadDeviceAddr(&fiducial_addr, section, "requires",
                        PLAYER_FIDUCIAL_CODE, -1, NULL) == 0)
  {
    sensor = new AMCLFiducial(fiducial_addr);
    sensor->is_action = 0;
    this->sensors[this->sensor_count++] = sensor;
  }
#endif

  /*
  // Create GPS sensor
  if (cf->ReadInt(section, "gps_index", -1) >= 0)
  {
    if (cf->ReadInt(section, "gps_init", 1))
      this->init_sensor = this->sensor_count;
    this->sensors[this->sensor_count++] = new AMCLGps();
  }

  // Create IMU sensor
  if (cf->ReadInt(section, "imu_index", -1) >= 0)
    this->sensors[this->sensor_count++] = new AMCLImu();
  */

  // We need an "action" sensor
  if(this->action_sensor < 0)
  {
    PLAYER_ERROR("No action sensor");
    this->SetError(-1);
    return;
  }

  // Load sensor settings
  for (i = 0; i < this->sensor_count; i++)
    this->sensors[i]->Load(cf, section);

  // Create the sensor data queue
  this->q_len = 0;
  this->q_start = 0;
  this->q_size = 20000;
  this->q_data = new AMCLSensorData*[this->q_size];
  memset(this->q_data,0,sizeof(AMCLSensorData*)*this->q_size);

  // Particle filter settings
  this->pf = NULL;
  this->pf_min_samples = cf->ReadInt(section, "pf_min_samples", 100);
  this->pf_max_samples = cf->ReadInt(section, "pf_max_samples", 10000);

  // Adaptive filter parameters
  this->pf_err = cf->ReadFloat(section, "pf_err", 0.01);
  this->pf_z = cf->ReadFloat(section, "pf_z", 3);

  // Initial pose estimate
  this->pf_init_pose_mean = pf_vector_zero();
  this->pf_init_pose_mean.v[0] = cf->ReadTupleLength(section, "init_pose", 0, 0);
  this->pf_init_pose_mean.v[1] = cf->ReadTupleLength(section, "init_pose", 1, 0);
  this->pf_init_pose_mean.v[2] = cf->ReadTupleAngle(section, "init_pose", 2, 0);

  // Initial pose covariance
  u[0] = cf->ReadTupleLength(section, "init_pose_var", 0, 1);
  u[1] = cf->ReadTupleLength(section, "init_pose_var", 1, 1);
  u[2] = cf->ReadTupleAngle(section, "init_pose_var", 2, 2*M_PI);
  this->pf_init_pose_cov = pf_matrix_zero();
  this->pf_init_pose_cov.m[0][0] = u[0] * u[0];