Skip to content

Latest commit

 

History

History
261 lines (147 loc) · 13.1 KB

README.md

File metadata and controls

261 lines (147 loc) · 13.1 KB
Raw

scriba_localization

The Scriba robot localization ROS packages.

  • scriba_ekf provides a position EKF for the robot
  • scriba_vision provides a position estimation from the robot camera

Next steps:

  • Use the calibration rig to get sensor covariance
  • Transform the camera localization fix in the camera node instead of the EKF node
  • Implement dynamic covariances dependent on the measurement rather than the sensor
    • Based on good matches/outlier ratio?
    • Based on homography difference to perfect rectangle?

scriba_ekf

Overview

Provides an EKF to fuse position estimation from cameras and odometry.

Estimation architecture

The estimation diagram of the robot is given below. The steer angle of the front wheel is estimated from the steer angle stepper motor command and the steer angle potentiometer value using a 1-D Kalman filter. Localization from one or more cameras can be fused to obtain the robot position estimation.

Robot model

The robot is modeled using a bicycle model with the front wheel steer angle (φ) and front wheel traveled distance (d_fw) being measured. The robot model class is implemented in /scriba_ekf/src/scriba_ekf/robot_model.py.

The robot odometry vector is u_{t} = \begin{bmatrix} \varphi_{t} \\ d_{fw_{t}} \end{bmatrix} and robot state vector is x_{t} = \begin{bmatrix} x_{t} \\ y_{t} \\ \theta_{t} \end{bmatrix}

The robot position prediction is \widehat{x_{t}} = f(x_{t-1}, u_t) = \begin{bmatrix}x_{t-1} \\y_{t-1} \\ \theta_{t-1} \end{bmatrix} + \begin{bmatrix}cos( \theta_{t} )\times d_{x_{t}} - sin(\theta_{t})\times d_{y_{t}} \\sin(\theta_{t})\times d_{x_{t}} + cos( \theta_{t} )\times d_{y_{t}}\\ \omega_{t} \end{bmatrix}

with:

d_{x_{t}} = \frac{L}{tan(\varphi_{t})} \times sin(\omega_{t})

d_{y_{t}} = \frac{L}{tan(\varphi_{t})} \times (1-cos(\omega_{t}))

\omega_{t} = \big( \frac {d_{fw_{t}}\times sin(\varphi_{t})}{L} \big)

As needed for the EKF, the Jacobians of the position prediction to respectively the state vector and the odometry vector are:

F_{x} = \begin{bmatrix}1 & 0 & -sin(\theta)\times d_{x} - cos(\theta)\times d_{y} \\0 & 1 & cos(\theta)\times d_{x} - sin(\theta)\times d_{y} \\ 0 & 0 & 1\end{bmatrix}

and

Fu = \begin{bmatrix}cos(\theta + \omega)*cos(\varphi) & (L*cos(\omega)*sin(\theta) - L*sin(\theta) + L*sin(\omega)*cos(\theta) - d_{fw}*cos(\omega)*cos(\varphi)^2*cos(\theta)*sin(\varphi) + d_{fw}*sin(\omega)*cos(\varphi)^2*sin(\varphi)*sin(\theta))/(cos(\varphi)^2 - 1)\\sin(\theta + \omega)*cos(\varphi) &-(L*cos(\omega)*cos(\theta) - L*cos(\theta) - L*sin(\omega)*sin(\theta) + d_{fw}*cos(\omega)*cos(\varphi)^2*sin(\varphi)*sin(\theta) + d_{fw}*sin(\omega)*cos(\varphi)^2*cos(\theta)*sin(\varphi))/(cos(\varphi)^2 - 1)\\\fraq{sin(\varphi)}{L}&\fraq{(d_{fw}*cos(\varphi))}{L} \end{bmatrix}

Usage

Config files

  • scriba_ekf_params.yaml Default EKF configuration for fusing odometry and localization measurement from the front camera.

Launch files

  • scriba_ekf.launch: Run the EKF node with the default configuration for fusing odometery and localization measurement from the front camra.
    • init_pose_from_param If true, get initial pose and covariance from parameters. Default: true.

Nodes

ekf_scriba_node.py

Position EKF node for the Scriba robot. Publishes a position estimation at a fixed frequency using inputs from the odometry and external localization sources (camera, ...). The node inputs are modular an allow for a range of sensor to be fused as long as they provide their localization measurement on the same format.

Subscribed Topics

  • update source topic (scriba_msgs/localization_fix)

    A topic for every update source for the filter. Update is on the form of a "localization_fix", a 2D pose with covariance (x, y, yaw angle). Topic name is set by the parameter update_sources/<update_source>/topic. The topic triggers the update step of the EKF.

  • prediction source topic (scriba_msgs/motion_odom)

    A topic for every prediction source for the filter. Prediction data is on the form of a "motion_odom" message, a motion data vector with covariance (front wheel steer angle phi, front wheel traveled distance dfw). Topic name is set by the parameter prediction_sources/<prediction_source>/topic. The topic triggers the prediction step of the EKF.

  • initialpose (geometry_msgs/PoseWithCovarianceStamped)

    Initial pose and covariance of the filter. Only taken in account if parameter ~init_pose_from_param is set to false.

Published Topics

Parameters

  • ~frequency (int, default: 50)

    Node publish frequency. Careful: not the filter update/prediction step frequency (message triggered).

  • ~init_pose_from_param (bool, default: false)

    If true, will use the initial pose and covariance parameters to initialize the filter. If false, will get initial pose and covariance from topic.

  • ~publish_tf (bool, default: true) . If true, published estimated pose as transform between map and base_link

  • ~initial_pose_x (double, default: 0.0 meters)

    Initial pose mean (x), used to initialize filter with Gaussian distribution.

  • ~initial_pose_ỳ (double, default: 0.0 meters)

    Initial pose mean (y), used to initialize filter with Gaussian distribution.

  • ~initial_pose_theta (double, default: 0.0 radians)

    Initial pose mean (yaw), used to initialize filter with Gaussian distribution.

  • ~initial_cov_xx (double, default: 0.05*0.05 meters)

    Initial pose covariance (x*x), used to initialize filter with Gaussian distribution.

  • ~initial_cov_yy (double, default: 0.05*0.05 meters)

    Initial pose covariance (y*y), used to initialize filter with Gaussian distribution.

  • ~initial_cov_aa (double, default: 0.07 radians²)

    Initial pose covariance (theta*theta), used to initialize filter with Gaussian distribution.

  • ~ekf_params/L (double)

    Length of the robot from rear wheel axle to front wheel axis.

  • ~ekf_params/update_sources (dictionary)

    Dictionary of all update sources (absolute measurement sources). Each source is a dictionary of its own of the format:

    • <update_source>
      • topic (string): topic name for the update source
      • R (double[9]): 3x3 covariance matrix for the sensor noise. will be used if the sensor doesn't provide covariance for its measurement.
      • T (double[16]): Transform matrix (4x4) between localization fix frame and robot body frame.
      • max_distance (double): Max Mahalanobis distance for the validation gate.

  • ~ekf_params/prediction_sources (dictionary)

    Dictionary of all prediction sources (relative measurement sources). Each source is a dictionary of its own of the format:

    • <prediction_source>
      • topic (string): topic name for the prediction source.
      • Q (double[4]): 2x2 covariance matrix for the predicion step noise. will be used if the sensor doesn't provide covariance for its measurement.

scriba_vision

Provide a localization fix using the robot camera. The localization is based on SIFT feature identification and matching using FLANN algorithm.

The camera image localized on the map

The robot camera pointed at the test map

Overview

Image processing

The robot localization is based on binarized image (black and white) as the robot is only able to print black ink on white paper (whitout half-toning).

The image from the camera is flatten using an homography to correct for the camera position. The homography matrix is obtained during a calibration phase. The flatten imaged is then blurred and binarized using an adaptive threshold.

Localization

Features are identified using SIFT on both the camera picture and the map. Features are then matched using FLANN and sorted. A maximum number of match is kept, with a feature allowing to weight matches relative to their distance to an area. At last, homography search using RANSAC algorithm allows to get rid of outliers.

The matches after outliers have been removed with the homography between the camera picture and the map in red

The homography matrix from the camera picture to the map is decomposed to get its translation, rotation and shear components. The height/width ratio and the shear value are compared to tolerance values to accept or discard the localization reading.

Config files

  • front_camera_params.yaml Front camera parameters including homography matrix from the camera image to the floor plane.

  • front_camera.yaml Front camera base parameters.

  • localization_params.yaml Default localization parameters.

  • map Folder of the map images

Launch files

  • camera_localization.launch: Run the localization node with the cv_camera node.

    • device_id Camera ID number for the cv_camera node. Default: 2.

    • camera_name Camera name for the namespace. Default: camera.

  • front_camera_localization.launch: Run the localization node with the cv_camera node with the the front camera parameters.

Nodes

scriba_camera_node.py

Localize an image in a image map using SIFT feature identification and FLANN matching algorithm.

Subscribed Topics

Published Topics

Parameters

  • map_file/package (string, default: "scriba_vision")

    ROS package containing the map.

  • map_file/path (string, default: "config/map/textmap.png")

    Map file path relative to the root of the package

  • min_matches (int, default: 10)

    Minimum number of matches needed for a reading to be valid.

  • scale_diff_tolerance (double, default: 0.15)

    Tolerance on the x-scale/y-scale ratio for a reading to be valid.

  • shear_tolerance (double, default: 0.1)

    Tolerance on the shear value for a reading to be valid.

  • use_lookup_area (bool, default: false)

    If true, the matches will be sorted to favour those within or close to the lookup area estimated from the pose on /robot_pose_estimate.

  • camera_img_scale (double, default: 10000.0)

    meters-to-pixels scale ratio.

  • min_threshold (int, default: 2)

    Adaptive threshold mean offset for image binarization.

  • ksize_height (int, default: 10)

    Gaussian kernel height for the gaussian blur.

  • ksize_width (int, default: 10)

    Gaussian kernel width for the gaussian blur.

  • homography_matrix (int, default: 10)

    Homography matrix to flatten the camera image to the floor plane.