MBED Example: Nice and Smooth Steering for RC Car

.from: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/

Entering the Sparkfun 2011 Autonomous Vehicle Competition, my mbed-controlled 1:10 scale RC truck needed smooth, predictable steering when navigating from one waypoint to the next or whenever it needed to change heading.
I wanted a way to decouple steering calculations from the underlying vehicle geometry. The steering calculation is independent of vehicle's track width and steering geometries. I also didn't want to waste a lot of time with trial and error and felt that mathematically modeling the situation might help. Maybe this approach is overly complex, but it seems to work very nicely and I basically got it right the first time and never thought about it again. It just works.

I modeled the heading change as in the attached picture. The robot turns with some radius, r, along a circle from the current heading (vertical line) to the new heading that has a relative bearing of theta degrees from the current heading.
The circular path of the robot intercepts the desired heading path at a distance, I, the intercept distance.
By selecting a fixed intercept distance, then the robot has to calculate the front tire steering angles in order to achieve a turn of radius r.
Knowing the track width and by measuring steering angles as related to steering servo pulse widths, one can then translate steering angle into servo pulses very easily. (I found that there's a nearly linear relationship at least until about a 20 or 25 degree steering angle).


XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx Steering.cpp XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx


#include "Steering.h"
#include "math.h"


/** create a new steering calculator for a particular vehicle
 *
 */
Steering::Steering(float wheelbase, float track)
    : _wheelbase(wheelbase)
    , _track(track)
    , _intercept(2.0)
{
}


void Steering::setIntercept(float intercept)
{
    _intercept = intercept;
}


/** Calculate a steering angle based on relative bearing
 *
 */
float Steering::calcSA(float theta) {
    float radius;
    float SA;
    bool neg = (theta < 0);


    // I haven't had time to work out why the equation is slightly offset such
    // that negative angle produces slightly less steering angle
    //
    if (neg) theta *= -1.0;   
   
    // The equation peaks out at 90* so clamp theta artifically to 90, so that
    // if theta is actually > 90, we select max steering
    if (theta > 90.0) theta = 90.0;
   
    radius = _intercept/(2*sin(angle_radians(theta)));
    SA = angle_degrees(asin(_wheelbase / (radius - _track/2)));


    if (neg) SA *= -1.0;


    return SA;
}


XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx Steering.h XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx


#define PI 3.141592653589

/** A class for managing steering angle calculations based on current and desired heading
 *  and specified intercept distance along the new path. 
 *
 *  See Notebook entry: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/
 */
class Steering
{
  public:
    /** create a new steering calculator
     *
     * @param wheelbase vehicle wheelbase
     * @param track vehicle track width
     * @param intercept new course intercept distance
     */
    Steering(float wheelbase, float track);

    /** set intercept distance
     * @param intercept distance along new course at which turn arc will intercept
     */
    void setIntercept(float intercept);

    /** convert course change to average steering angle
     * assumes Ackerman steering, with track and wheelbase
     * and course intercept distance specified.
     *
     * See notebook: http://mbed.org/users/shimniok/notebook/smooth-steering-for-rc-car/
     *
     * @param theta relative bearing of the new course
     * @returns steering angle in degrees
     */
    float calcSA(float theta);
   
  private:

    inline static float angle_radians(float deg) {return (PI/180.0)*deg;}

    inline static float angle_degrees(float rad) {return (180/PI)*rad;}

    float _wheelbase;
    float _track;
    float _intercept;
};

XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx EOF XxXxXxXxXxXxXxXxXxXxXxXxXxXxXx