Jun 162010

I got a question about the speed control and motor on my robot. Here are the details:

  1. Motor. The Grasshopper comes with a stock Mabuchi 380 motor instead of the 540 motor that’s typical in most 1/10 RC cars. The 380 motor is plenty fast for the AVC contest and as a bonus, you get much longer run times. This was a big selling point back when these cars ran off of low capacity (by today’s standards) NiCd packs and charging took a while. 
  2. Electronic Speed Control. The new Grasshopper reissue kits come with a Tamiya branded ESC. It’s a relatively low-end unit, but it works fine.
  3. Motor interface. I wanted to leave the car essentially unchanged so my computer talks to the steering servo and ESC by generating PWM signals just like the RC receiver does. I used the Arduino Servo library and the commands to set the motor speed are generated in an open loop fashion. In fact, I set the ESC command once and the car will go until I switch to manual control (or run into something). The actual servo values that I use to set the car speed were determined by some brief trial and error.
The Arduino Pro Mini uses an interrupt routine to read the PWM from the third channel on the RC receiver and based upon this input outputs a high/low signal that controls a multiplexer chip (74LS157) that routes the desired PWM commands (computer/remote) to the ESC and steering servo.
 Posted by at 1:44 PM
Jun 012010

I’ve been pretty busy working, moving, and traveling, so this update on my AVC entry is a little late. 

Here’s the rundown on the hardware and software I used to win the Sparkfun 2010 AVC. I’m not happy with the vehicles speed: I was hoping for a much faster time. I think that if I improve the obstacle avoidance and tweak up the state estimator, I should be able to cut my time in half. We’ll have to wait for next year or a clear slate of work and projects to find out.

Chassis: Tamiya Grasshopper. Just because it’s a classic and it has a rear differential (essential of odometry)

Computer #1: Arduino Mega. Samples all sensors, runs an extended Kalman filter (EKF) at 20 Hz, and runs the navigation control law. The Mega is required because it has additional RAM. All of those floating point matrices take up a lot of space and they don’t fit inside a standard Arduino. The code that runs the robot is about 21K.

Computer #2: Ardunio Nano. This computer monitors the third channel of the R/C receiver and switches a multiplexer between R/C control and computer control from the Mega. The Mega doesn’t know when it’s in control; it just constantly reads the sensors at 20 Hz and continuously updates the state vector. The only control output form the Mega is the front steering wheel servo angle.
Data logging and telemetry: An xBee Pro is used to send a data stream back to a laptop for debugging and a OpenLog is used to get a full log of all the sensor readings and EKF output for the entire run. I found the xBee to be unreliable and I got dropouts in my data stream often enough to make a full recording of all the data from a run impossible. Even with the 60 mW version!
Construction: All of the electronics are built onto an Ardunio Mega Shield using point-to-point construction. The sensors are all socketed with 0.1 inch headers.
Power: The u-Blox GPS and the Sharp IR sensors both draw more current than the Ardunio regulators can supply, so two three-pin regulators are on the board to provide high-current rails at +3.3V and +5V. I use a standard 7.2 V NiMH RC car battery pack to power everything. 


1. Encoders on both rear wheels: I used the Sparkfun analog QRE1113  IR senor breakout board and 7419 hex Schmidt trigger chip to square up the output form the IR sensors and the hysteresis prevents false counts. The ink-jet printed encoders give me count 16 pulses per revolution on each wheel. This information is fed into the EKF to provide odometric location data. I assume the car is always moving forward, so I can’t distinguish forward from reverse with the encoders. Driving the robot along a straight painted line on a tennis court for about fifty feet was the calibration procedure to get counts/meter on each wheel (they are a few precent different). I take the counts on each wheel along with the sampling interval (50 ms) to compute a velocity for each wheel.

2. Compass: A HMC5843 three-axis magnetometer is used to provide heading information. I originally tried a HMC6352, but I found the built-in calibration routine to be flakey. I use a continuous calibration scheme to scale the x- and y-axis magnetometer values and compute a heading. I also correct for local declination. The heading is fed into the EKF.

3. Rate Gyro: A ADXRS610 rate gyro is used to provide yaw rate information to the EKF. In addition, the vehicle state vector is augmented with a gyro bias term so the gyro bias is taken care of essentially for free. It’s almost magical. You give the EKF a gyro measurement corrupted by drift and you get an output with an estimate of the true angular rate and the drift!

4. GPS: A GS407 GPS module using the u-Blox chipset is used to provide location data. I found that the heading and speed data were unreliable at the low speeds the vehicle ran at so it’s not used at all. The position data is fed to the EKF at 4 Hz, while the rest of the sensors update the EKF at 20 Hz. This is done by swapping observer matrices depending on the data that’s available. The Latitude/Longitude data from the GPS is converted to UTM using a linear transformation that accounts for the slight cross coupling due to tapering of latitude lines towards the north pole. In addition, a constant term is subtracted from the UTM values in order to increase the effective precision of the location data. If I didn’t do this I’d only have one effective digit of precision since most of the UTM coordinate digits don’t change when you go around the Sparkfun HQ. UTM really rocks and it’s much nicer than latitude and longitude for navigation. Unfortunately, the u-Blox chipset won’t do UTM on it own. I also use the reported GPS accuracy as an input to the Kalman filter covariance matrix.
Extended Kalman Filter:
In total the observed data is a six-element column vector: {Right wheel velocity, Left wheel velocity, theta, omega, UTM northing, UTM easting}.
These values are pushed through a textbook implementation of an EKF. The EKF is required because odometry is nonlinear. I’ve implemented a matrix math library for the Ardunio and it’s plenty fast to run the filters with no optimizations. For example, most of the matrices are symmetric, but I don’t use this fact to save space or to reduce the op-count required to do matrix inversion or matrix multiplies.
The result out of the EKF is an estimated state vector for the robot that gets updated at 20 Hz. The state vector is: {north (m), east (m), velocity (m/s), heading (radians), heading rate (rad/s), and gyro bias (rad/s)}
The control law simply uses atan2 and the difference between the vehicle location and waypoint location to compute a desired heading. The difference between the desired heading and the estimated heading is used to form an error signal that steers the front wheels via a simple proportional control loop. Vehicle speed is controlled in an open loop fashion and set to a constant value throughout the run.
The distance between the vehicle and the next waypoint is updated continuously and once it drops below a threshold value, the waypoint counter is incremented to the next waypoint. Once the car gets to the last waypoint, the counter is reset to zero so that car will just drive the course over and over until the batteries die, or the car crashes.
Collision avoidance: Sharp GP2Y0A700K IR sensors (They are huge and have a 5.5 meter range) are used to detect obstacles and the distance readings from front-left, ahead, and front-right modify the error signal controlling the steering angle. When the distance measured by the range sensors exceeds a threshold value, the obstacle avoidance system does not modify the steering angle at all. Since the collision avoidance system only modifies the steering command, it’s completely decoupled form the EKF. The state estimator keeps on chugging along no mater what you do to the steering control.
I drove the car open loop on some straight lines to get the counts/meter for both encoders. Driving the car around some square box courses of about 50 to 100 feet on each side gave me some odometry data that should have resulted in a nice closed course. I used an optimization scheme coded in Matlab to do a best fit of some of the other odometry calibration terms (like the effective distance between the rear wheels). Basically, I played with the calibration until I got the ground track from odometry alone that was a best match to the known course that I actually drove. 
I used the actual uncertainly reported for the GPS location data for the covariance values for the GPS data and I played with some Matlab simulations of the cars EKF and varied the other covariance terms by hand until I got what I thought were semi-optimized results. My covariance matrix was diagonal and I didn’t try adding any off-diagonal terms. 
The Matlab code is a functional duplicate of the EKF running on the Arduino. I can take raw sensor data from a run and plot the estimated robot position and heading as a function of EKF parameters. This allowed me to do a lot of simulation; in fact, I didn’t do a lot of closed-loop test runs with the robot. I spent most of my time running simulations of the car position estimator using real data taken form open-loop test drives. My first successful closed-loop run around the Sparkfun HQ was the morning of the contest.
I think there is a lot of room for improvement here.  Using something other than paddle tires (although they are super-rad) would probably improve the odometry a lot. 

That’s the basics. I still need to download and plot the data from the contest runs. Hopefully soon!

 Posted by at 5:35 PM