Optimal Control of Kinematic Car

  • To generate and implement optimal control trajectories on a simple kinematic car robot (iRobot Roomba) while avoiding obstacles
  • Algorithms and simulations were implemented in Mathematica
  • A team project with Tanay Choudhary
  • Problem Setup:

    The system state (q) is comprised of: x position, y position, heading angle, x velocity, and y velocity. The control inputs (u) are: left and right wheel angular velocities. Desired state trajectory, control trajectory, and obstacle and goal locations in x-y coordinates are set arbitrarily. The dynamics of the kinematic car motion are governed by the equations:

    • x’[t] = 0.001 * 0.5 * Cos(θ[t])(v1[t] + v2[t])
    • y’[t] = 0.001 * 0.5 * Sin(θ[t])(v1[t] + v2[t])
    • θ’[t] = 0.001 * (v1[t] – v2[t])/(0.235)
    • 15 (ul – v1[t])
    • v2’[t] = 15 (ur – v2[t])

    The objective (cost) function comprises of the following components:

    • Error from desired state trajectory (low weight)
    • Error from desired input trajectory (very low weight)
    • Error from terminal desired state (highest weight)
    • Barrier functions for obstacles (high weight)

    Optimization Approach and Results

    An iterative LQR algorithm with Armijo Line Search is performed to find the optimal control trajectories required to minimize the cost function – thereby achieving the desired behavior from the kinematic car. Since Roomba’s state feedback is not used, actual motion resulted in slight drift compared to simulated motion.

    Two kinds of barrier functions are used – inverse functions and logarithmic functions. Logarithmic barrier functions behaved better – converging faster and more robustly w.r.t. changes in the cost weights and initial conditions.

    3 cases are tested:

    • A straight line start to goal motion without obstacles
    • One obstacle in the straight line path
    • Two obstacles in the straight line path

    As expected, the best (and fastest) performance was achieved in the simplest case without obstacles. But the errors from target position were small with one and two obstacles.

    Plots of trajectories and input control velocities generated in Mathematica are shown below:

    a. Without obstacles:

    Parametric plot of trajectory:

    Screen Shot 2016-08-15 at 12.32.47 PM

    Control input (u) vs. time (t) [Blue: left-wheel and yellow: right-wheel]

    Screen Shot 2016-08-15 at 12.32.58 PM

    b. With two obstacles:

    Parametric plot of trajectory:

    Screen Shot 2016-08-15 at 12.33.05 PM

    Control input (u) vs. time (t) [Blue: left-wheel and yellow: right-wheel]

    Screen Shot 2016-08-15 at 12.33.26 PM

    The trajectories generated above were finally tested on Roomba robot as demonstrated in the video.

    0 comments on “Optimal Control of Kinematic CarAdd yours →

    Leave a Reply

    Your email address will not be published. Required fields are marked *