#### 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:

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

**b. With two obstacles:**

Parametric plot of trajectory:

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

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

## 0 comments on “Optimal Control of Kinematic Car”Add yours →