16-735 Final Project Report

16-735 Robot Motion Planning
Fall 2007
State Estimation & Path Planning for Scarab Rover

Michael Dille and Umashankar Nagarajan


Figure 1: Scarab rover	Figure 2: GPS antenna on Scarab

Overview

State Estimation
- We implemented a Linear Kalman Filter (LKF) that fuses GPS and IMU data to estimate the rover's state
- We also implemented a "half" Kalman Filter to track odometry using an optical sensor and IMU data without GPS
Path Planning
- We implemented a reactively-replanning A* path planner for a simulated obstacle-laden environment

State Estimation

Linear Kalman Filter using GPS and IMU data

We implemented a linear kalman filter in the typical manner:


"Predict"		"Correct"

where:

x	=	18x1 state vector: [x,y,z,θ_roll,θ_pitch,θ_yaw,dx,dy,dz,dθ_roll,dθ_pitch,dθ_yaw,d²x,d²y,d²z,d²θ_roll,d²θ_pitch,d²θ_yaw]^T
u	=	Control input
		(assumed to be zero here, so any motion is a perturbation)
A,B	=	Process model
		(to simplify unknown complex model, assumed to be axis-independent constant velocity and constant acceleration kinematics for point-object)
P	=	Estimate error covariance
Q	=	Process noise covariance
		(estimated empirically from data and hand-optimized)
z	=	9x1 measurement vector: [x,y,z,dθ_roll,dθ_pitch,dθ_yaw,d²x,d²y,d²z]^T
R	=	Measurement noise covariance
		(estimated empirically from data and hand-optimized)
H	=	Sensor model
		(a matrix with ones in the appropriate locations)
K	=	Kalman gain

"Half" Kalman Filter using optic flow and IMU data

Because Scarab is designed to be a prototype for a lunar mission into deep craters, for which absolute fixes using as GPS or star-tracking will not be available, we are interested in computing odometry during motion using only locally-available information. The method currently under evaluation uses a quad-lens optic flow sensor shown in figures 3 and 4 to estimate ground speed. This was chosen over wheel-based odometry due to the frequent occurrence of slip in the targeted environments.


Figure 3: Optic flow sensor		Figure 4: Optical sensor on Scarab

The state estimation scheme employed here is really just odometric velocity integration, but it can be formally stated as an instance of what might informally be described as a "half" kalman filter because it iterates only prediction steps without any correction (none being available). Specifically, the process model may be described in two-dimensions (the 3-D version being the obvious-enough extension thereof) as:

so that for error-estimation purposes:

and

in which the "control input" u_k = [ΔD_k,Δθ_k]^T is comprised of the linear distance traveled per timestep (ground velocity from the optic flow sensor multiplied by the timestep width) and the IMU-provided angular velocities.

Results

We collected data from a particularly challenging traverse in which Scarab followed a circular path leading through a crater. At one point during this run, it reached a pitch angle of upward of 30 degrees.


Figure 5: Entering crater		Figure 6: Exiting crater

LKF with GPS and IMU data

The results of our LKF are compared to the filter built into the commercial INS/GPS system used on Scarab. The built-in filter path is taken as ground truth as it has access to more data and runs at a higher frequency than our filter, though we acknowledge that it too is imperfect. Click on the following figure for a visualization.

Figure 7: LKF crater results

As can be seen from this plot, our LKF implementation follows ground truth quite closely.

Optical Odometry

The optical sensor was attached during this traverse, and integrating the velocity it provided the following results. Click on the following figure for a visualization.

Figure 8: Optical sensor crater results

The error here is higher than that of the LKF as expected, owing to the lack of GPS data. However, it is remarkably accurate considering that by the end of this traverse, the absolute position error is less than 1.5m.

Additional exciting results have become available, from a 10+ hour 1km traverse that took place after the class presentation. The path traced around the Robot City LTV Steel site test area is here overlaid on an overhead view provided by Google Earth.

Figure 9: Optical sensor 1km results

As this plot shows, an error of less than 35m was accumulated after nearly 1km of travel, for an error of less than 4%. This, we are told, is quite remarkable for a dead reckoning system and is only possible because the optical sensor detects actual body motion without being subject to wheel slip or physical imperfections such as a slight error in the estimate of wheel radius.

Path Planning

Our original intentions were to write a path planner for an Ackermann-steered vehicle and feed its output to the drive-by-wire system to the Kodi vehicle, a robot housed in the FRC. This would be integrated with our state estimation component by using a real-time Kalman filter to estimate the robot's position during execution. Unfortunately, contention for the vehicle and adverse weather conditions by the time we were ready to test prevented us from carrying this out. Instead, we here show results from a simulated environment.

The planner is an A* planner operating in the space of commands to an Ackermann-steered vehicle. Specifically, at each state/timestep, the planner must select between any of several steering angles that are nearby offsets from the current one (for realism, we impose a maximum slew rate, that is, the maximum rate at which the steering wheel can turn). This is then integrated forward for one timestep in simulation to determine the state that would follow if this command were given. The objective is to determine a path through the 2-D world that does not intersect any obstacles.

To make this more interesting, we have included the notion of a limited worldview requiring replanning. Initially, the robot does not know of any obstacles, and the path it finds to the goal is a straight line (possibly intersecting many obstacles). A virtual sensor range is specified so that as the robot follows this path, it may discover any obstacles lying within this radius. If during execution it discovers that a collision is soon imminent, it stops, replans using the updated map, and recommences execution. Click on the following figure for a visualization of this process.

Figure 10: Simulation of A* planner for Ackermann vehicle

This particular example demonstrates this replanning on several occasions, after which the robot is finally able to reach the goal.

Conclusions

In this project, we explored both probabilistic state estimation and path planning. We demonstrated a functioning LKF implemented on a real robot and achieved significant results in odometric pose estimation on the same robot. We also demonstrated a dynamically-replanning path planner for a mildly-complex vehicle, though not on a real robot as we had wished.