Wheel Odometry
The WheelOdometry node receives the servo status and wheel speeds to estimate the vehicle's linear velocities and yaw rate. This node provides significant information for the state estimator because it adds constraints to the vehicle's pose so that it can continue to run even when the IMU or GPS fluctuate. This node also outputs covariances for the linear velocities and yaw rate so that the state estimator can determine when the wheel odometry information is reliable. While the position and heading are published by this node, these values will drift over time because there is no correcting mechanism for these values.
The node estimates the vehicle's steering angle based on the servo status using the following linear equation that was found using MATLAB, where STEERING_ALPHA is approximately -21 and STEERING_BETA is the offset when the servo value is 0 (STEERING_BETA should be 0 if vehicle's steering is calibrated):
steering_angle_ = STEERING_ALPHA * servo_val_ + STEERING_BETA;The steering angle is important for later calculations. Servo values are negative when steering left. The steering angle will be positive when steering left to simplify calculations.
The following equations are used to calculate the vehicle's pose, X and Y velocities in a local reference frame, and yaw rate. Roll pitch, and Z position are assumed to be zero.
R: turn radius
V: velocity
φ: angular distance
α: steering angle
ℓ: wheelbase
x: x position
y: y position
θ: heading
dt: timestep
dx: x displacement in local reference frame
dy: y displacement in local reference frame
dθ: change in heading
vx: x velocity in local reference frame
vy: y velocity in local reference frame
vθ: yaw rate
R = ℓ / sin(α) (when steering angle is not 0)
φ = V * dt / R
dx = R * sin(φ)
dy = (R - R * cos(φ)) (for left turn)
dy = -(R - R * cos(φ)) (for right turn)
dθ = V / ℓ * sin(α) * dt
x = x + dx * cos(θ) - dy * sin(θ)
y = y + dx * sin(θ) + dy * cos(θ)
θ = θ + dθ
vx = dx / dt
vy = dy / dt
vθ = dθ / dt
Because this node does not account for slip (e.g. the vehicle drifts), it does not accurately model the vehicle's motion when it makes aggressive maneuvers. The essential outputs are the X and Y velocities, and the yaw rate. However, this node does not provide an accurate Y velocity because it is unable to capture lateral motion, so we create error functions for the X velocity and yaw rate to be used in the covariance matrices.
Error in X velocity occurs when the speeds of the front wheels differ from the speeds of the back wheels, so the function used to capture the error in X velocity is
error = |FL - BL| + |FR - BR|
where FL is the speed of the front left wheel, BL is the speed of the back left wheel, FR is the speed of the front right wheel, and BR is the speed of the back right wheel, all in meters per second.
This node determines when the vehicle is slipping by estimating what the velocity of the whole vehicle should be based on the front left wheel and steering angle, and again based on the front right wheel and steering angle. A larger difference between the two estimates signifies slippage of the vehicle. This process is shown below:
We then find the variance of the difference between this node's output and the ground truth across different error values. The state estimator using and IMU and GPS was used as the ground truth. Functions were then fit to the variance plot and used in the covariance matrices.
The WheelOdometry node can be found in the autorally_core package. The node can be launched using:
roslaunch autorally_core wheelOdometry.launchThe WheelOdometry node has several launch parameters.
- vehicle_wheelbase: the distance between the front and rear axles
- vehicle_width: the distance between the center of the front (or rear) two wheels
- using_sim: The simulator vehicle's steering has a slightly different mechanism than the AutoRally platform. It has a max steering angle of 21 degrees in both directions and is centered at 0.
- time_delay: There is a delay between commands to the vehicle and the vehicle's response. Delaying the output of the yaw rate by approximately 0.1 seconds significantly decreases the error between the estimated yaw rate and the ground truth.
- debug: When debug is true, the node subscribes to /pose_estimate. This can allow the node to get an initial pose, or test the odometry output using state estimator values.
The WheelOdometry output can be visualized in RVIZ when you add the /wheel_odometry topic.