Trajectory Tracking Control Problem

In this example, we demonstrate the modeling and solving process for a vehicle trajectory tracking control problem.

Additionally, for this type of problem that requires continuous solving, we demonstrate how to accelerate solving:

• How to use warm start to reduce the number of iterations
• The effect of different barrier parameters on solving accuracy and number of iterations

Problem Description

We constructed a closed-loop simulation with the following settings:

• The reference trajectory is a straight line parallel to the x-axis with a speed of 1 m/s
• When $t\in[0,5]$, $y^{\text{ref}} = 0$; when $t>5$, $y^{\text{ref}}=1.0$
• The vehicle's initial state is $(x(0),y(0),\phi(0),v(0))=(0,1,0,1)$
• The closed-loop control frequency is 20 Hz, simulation time is 12 s

The objective function of the trajectory tracking controller is to minimize the vehicle's tracking error (discretized into 20 time points):

$$\begin{split} \min \sum_{i=1}^{20} w_x (x_i - x_i^{\text{ref}})^2 + w_y (y_i - y^{\text{ref}})^2 + w_{v} (v_i - v^{\text{ref}})^2 \end{split}$$

Additionally, we need to consider the following constraints:

• Vehicle kinematic constraints (time step of 0.2 s):

$$\begin{split} \dot{x} &= v \cos \phi \\ \dot{y} &= v \sin \phi \\ \dot{\phi} &= \frac{v \tan \delta}{l} \\ \dot{v} &= a \end{split}$$

• bound constraints:

$$\begin{split} -1 \leq &a \leq 1 \\ -0.2 \leq & \delta \leq 0.2 \end{split}$$

• boundary conditions:

$$\begin{split} x(0) &= x_0, \\ y(0) &= y_0, \\ \phi(0) &= \phi_0, \\ v(0) &= v_0 \end{split}$$

Results

Closed-loop control results are as follows:

Warm Start

info

Warm start can help the solver converge and reduce the number of iterations. OPTIMake's warm start includes the following operations:

• Setting the initial guess of variables in the workspace
• Setting the try_warm_start_barrier parameter in options

For this type of problem that requires continuous solving, we can use warm start to accelerate solving. Here, we discuss the effect of the try_warm_start_barrier parameter on the number of iterations.

Below is a reference for setting this parameter (see Solver Interface for details):

option.try_warm_start_barrier = 1;

Below is the iteration count comparison with and without try_warm_start_barrier (thin blue line is enabled, thick black line is disabled):

As can be seen, after enabling this option:

• When warm start succeeds, the number of iterations can be significantly reduced
• When warm start fails, the number of iterations is only 1 more than without this option

Barrier Parameter

When the solver parameter in code generation is set to 'pdipm', OPTIMake uses the primal-dual interior point method.

Simply put, the constraint $g(v)\geq 0$ is approximated by a barrier function:

$$g(v)\geq 0 \rightarrow -\rho\log(g(v))$$

The target value of the barrier function parameter $\rho$ is set via the barrier_target parameter. During solving, $\rho$ gradually changes from the barrier_init parameter to the barrier_target parameter. The smaller the barrier_target value, the more accurate the constraint approximation, but the number of solving iterations also increases. Here, we show the closed-loop simulation results and iteration count comparison under different barrier_target values.

Closed-loop simulation results comparison under different barrier_target values:

barrier_target=1e-2

barrier_target=1e-4

barrier_target=1e-6

Iteration count comparison under different barrier_target values (thin blue line is with try_warm_start_barrier enabled, thick black line is with try_warm_start_barrier disabled):

barrier_target=1e-2

barrier_target=1e-4

barrier_target=1e-6

As can be seen, the effect of barrier_target is as follows:

• When barrier_target=1e-2, the number of iterations is the smallest, the warm start success rate is the highest, and the computation time (proportional to iteration count) is the lowest. However, the control variable $\delta$ is far from the bound constraints, resulting in lower accuracy
• When barrier_target=1e-6, the number of iterations is the largest, the warm start success rate is the lowest, and the computation time (proportional to iteration count) is the highest. However, the control variable $\delta$ is close to the bound constraints, resulting in the highest accuracy

In practice, the barrier_target value should be chosen based on specific requirements.

Appendix

The modeling code for the trajectory tracking controller is as follows:

prob = multi_stage_problem('traj_tracking', 20)

xweight, yweight, vweight = prob.parameters(['xweight', 'yweight', 'vweight'], stage_dependent=False)
x0, y0, phi0, v0 = prob.parameters(['x0', 'y0', 'phi0', 'v0'], stage_dependent=False)
vref, yref = prob.parameters(['vref', 'yref'], stage_dependent=False)
xref = prob.parameter('xref', stage_dependent=True)

x = prob.variable('x')
y = prob.variable('y')
phi = prob.variable('phi')
delta = prob.variable('delta', hard_lowerbound=-0.2, hard_upperbound=0.2)
v = prob.variable('v')
a = prob.variable('a', hard_lowerbound=-1.0, hard_upperbound=1.0)

obj = general_objective(xweight * (x - xref)**2 + yweight * (y - yref)**2 + vweight * (v - vref)**2)
prob.objective(obj)

""" ode """
length = 1.0
ode = differential_equation(
    state=[x, y, phi, v], 
    state_dot=[v * cos(phi), v * sin(phi), v * tan(delta) / length, a], 
    stepsize=0.2, 
    discretization_method='erk4')
prob.equality(ode)

seq = general_equality([x - x0, y - y0, phi - phi0, v - v0])
prob.start_equality(seq)

option = codegen_option()
codegen = code_generator()
codegen.codegen(prob, option)

The trajectory tracking closed-loop simulation code is as follows:

#include "traj_tracking_prob.h"
#include "traj_tracking_solver.h"
#include <stdio.h>

void simulate(const double u[2], const double x[4], const double dt, double x_next[4])
{
    /* simulate the system */
    const double a = u[0];
    const double delta = u[1];

    const double phi = x[2];
    const double v = x[3];

    x_next[0] = x[0] + v * cos(phi) * dt;
    x_next[1] = x[1] + v * sin(phi) * dt;
    x_next[2] = phi + v * tan(delta) / 1.0 * dt;
    x_next[3] = v + a * dt;
}


int main(void)
{
    Traj_tracking_Problem prob;
    Traj_tracking_Option option;
    Traj_tracking_WorkSpace ws;
    Traj_tracking_Output output;
    traj_tracking_init(&prob, &option, &ws);

    const double vref = 1.0;
    /* params */
    prob.param[TRAJ_TRACKING_PARAM_XWEIGHT] = 1.0;
    prob.param[TRAJ_TRACKING_PARAM_YWEIGHT] = 1.0;
    prob.param[TRAJ_TRACKING_PARAM_VWEIGHT] = 1.0;

    // initial state
    prob.param[TRAJ_TRACKING_PARAM_X0] = 0.0;
    prob.param[TRAJ_TRACKING_PARAM_Y0] = 1.0;
    prob.param[TRAJ_TRACKING_PARAM_PHI0] = 0.0;
    prob.param[TRAJ_TRACKING_PARAM_V0] = vref;

    // reference trajectory
    prob.param[TRAJ_TRACKING_PARAM_VREF] = vref;
    
    /* option */
    option.print_level = 0;
    option.try_warm_start_barrier = 1;
    option.barrier_target = 1e-6;

    const double ts_sim = 0.05;
    const double sim_length = 12.0;
    const double ts_discretization = 0.2;
    for (size_t sim_step = 0; sim_step < (size_t)(sim_length / ts_sim); sim_step++) {
        // update reference trajectory
        const double time = (double)sim_step * ts_sim;
        if (time > 5.0) {
            prob.param[TRAJ_TRACKING_PARAM_YREF] = 1.0;
        } else {
            prob.param[TRAJ_TRACKING_PARAM_YREF] = 0.0;
        }
        for (size_t i = 0; i < TRAJ_TRACKING_DIM_N; i++) {
            prob.param_stage[i][TRAJ_TRACKING_PARAM_STAGE_XREF] = vref * time + (double)i * ts_discretization;
        }

        // solve
        traj_tracking_solve(&prob, &option, &ws, &output);

        // simulate
        double u[2];
        double x[4];
        double x_next[4];
        u[0] = output.primal.var[0][TRAJ_TRACKING_VAR_A];
        u[1] = output.primal.var[0][TRAJ_TRACKING_VAR_DELTA];
        x[0] = prob.param[TRAJ_TRACKING_PARAM_X0];
        x[1] = prob.param[TRAJ_TRACKING_PARAM_Y0];
        x[2] = prob.param[TRAJ_TRACKING_PARAM_PHI0];
        x[3] = prob.param[TRAJ_TRACKING_PARAM_V0];
        simulate(u, x, ts_sim, x_next);

        // update initial state
        prob.param[TRAJ_TRACKING_PARAM_X0] = x_next[0];
        prob.param[TRAJ_TRACKING_PARAM_Y0] = x_next[1];
        prob.param[TRAJ_TRACKING_PARAM_PHI0] = x_next[2];
        prob.param[TRAJ_TRACKING_PARAM_V0] = x_next[3];

        printf("time %f: %f, %f, %f, %f, %f, %f;\n", time, x[0], x[1], x[2], x[3], u[0], u[1]);
    }
    
    return 0;
}