Speed Planning Problem - Extended Model

In the above example, we considered modeling and solving a simple speed planning problem. Below we extend the model to a more complex one:

• Consider rate-of-change constraints/penalties on control variables
• Consider coupled variable constraints
• L1 norm objective function

Rate-of-Change Constraints/Penalties

Assume the rate of change of the control variable $a$ is constrained to [-5, 5] $m/s^3$. Additionally, we need to add a penalty term for the control variable rate of change in the objective function.

We can introduce a higher-order derivative variable $jerk$ to represent the rate of change of the control variable. Then for the $jerk$ variable, we set its bounds to [-5, 5] $m/s^3$ and add a penalty on it.

New variable:

jerk = prob.variable('jerk', hard_lowerbound=-5, hard_upperbound=5)

Objective function changed to:

obj = general_objective((v - vref)**2 + 0.1 * jerk**2)
prob.objective(obj)

Constraints changed to:

ode = differential_equation(
    state=[s, v, a],
    state_dot=[v, a, jerk], 
    stepsize=0.1, 
    discretization_method='erk4')
prob.equality(ode)

Initial conditions changed to (added initial value constraint for acceleration):

seq = general_equality([s - 0, v - 0, a - 0])
prob.start_equality(seq)

效果如下:

Coupled Constraints

Assume that within the [7, 8] s time window, we want the distance constraint to retain a speed-dependent margin:

$$\begin{split} &s(t) \geq 60 + \textcolor{red}{0.2 v(t)}, t \in [7, 8] \\ \end{split}$$

Here we use the general_inequality function to define the coupled constraint.

tsafe = prob.parameter('tsafe',  stage_dependent=True)
ineq = general_inequality([s - tsafe * v - smin], sign=['>='], bound=[0.0])
prob.inequality(ineq)

Similarly, when solving, we need to set the tsafe value to 0.2 for the [7, 8] s interval.

for (size_t i = 0; i < SPEED_PLANNING_DIM_N; i++) {
    /* smin, tsafe */
    if ((i >= 70) && (i <= 80)) {
        prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 60.0;
        prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_TSAFE] = 0.2;
    } else {
        prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 0.0;
        prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_TSAFE] = 0.0;
    }
}

Results are as follows:(./speed_sv_ineq.png)

L1 Norm Objective Function

Assume we need to change the objective function to L1 norm form:

$$\min \sum_{i=1}^{N} |v_i - v_{ref}| + 0.1 |jerk_i|$$

Note that the above objective function is not a smooth objective function, so we need to perform an equivalent transformation. By introducing slack variables $u_v$ and $u_{jerk}$, the above objective function can be transformed to:

$$\min \sum_{i=1}^{N} u_v + 0.1 u_{jerk}$$

Additionally, we need to add the following constraints:

$$\begin{split} &u_v \geq v - v_{ref} \\ &u_v \geq - (v - v_{ref}) \\ &u_{jerk} \geq jerk \\ &u_{jerk} \geq - jerk \\ \end{split}$$

For the above optimization problem, we can use OPTIMake's built-in soft constraint interface for equivalent implementation. This way, we don't need to explicitly define slack variables, reducing memory usage and computation time.

For the $jerk$ variable, the above is equivalent to an L1 soft constraint with both upper and lower bounds of 0:

jerk = prob.variable('jerk', hard_lowerbound=-5, hard_upperbound=5, 
                     soft_lowerbound=0, soft_upperbound=0, 
                     weight_soft_lowerbound=0.1, weight_soft_upperbound=0.1,
                     penalty_type_soft_lowerbound='l1',
                     penalty_type_soft_upperbound='l1')

For the $v$ variable, the above can also be equivalently expressed as an L1 soft constraint with both upper and lower bounds of 0:

ineq = general_inequality([s - tsafe * v - smin, v - vref, v - vref], 
                           sign=['>=', '>=', '<='], 
                           bound=0.0)
prob.inequality(ineq, penalty_type=['none', 'l1', 'l1'], weight_soft=[inf, 1.0, 1.0])

Results are as follows. It can be seen that compared to the quadratic objective function, the L1 norm objective function makes acceleration and deceleration more linear.

Appendix

The complete modeling code is as follows:

prob = multi_stage_problem('speed_planning', 100)
vref = prob.parameter('vref',  stage_dependent=False)
smin = prob.parameter('smin',  stage_dependent=True)
tsafe = prob.parameter('tsafe',  stage_dependent=True)

s = prob.variable('s', hard_lowerbound=smin)
v = prob.variable('v')
a = prob.variable('a', hard_lowerbound=-3, hard_upperbound=3)
jerk = prob.variable('jerk', hard_lowerbound=-5, hard_upperbound=5, 
                     soft_lowerbound=0, soft_upperbound=0, 
                     weight_soft_lowerbound=0.1, weight_soft_upperbound=0.1,
                     penalty_type_soft_lowerbound='l1',
                     penalty_type_soft_upperbound='l1')


# use differential equation to define the state equation
ode = differential_equation(
    state=[s, v, a],
    state_dot=[v, a, jerk], 
    stepsize=0.1, 
    discretization_method='erk4')
prob.equality(ode)

# alternative way to define the state equation
# h = 0.1
# dis_eq = discrete_equation(
#     expr_next_stage=[s, v, a],
#     expr_this_stage=[s + h * v + 0.5 * h**2 * a + 1/6 * h**3 * jerk,
#                      v + h * a + 0.5 * h**2 * jerk,
#                      a + h * jerk],
# )
# prob.equality(dis_eq)

ineq = general_inequality([s - tsafe * v - smin,
                           v - vref, v - vref], 
                           sign=['>=', '>=', '<='], bound=0.0)
prob.inequality(ineq, penalty_type=['none', 'l1', 'l1'], weight_soft=[inf, 1.0, 1.0])

seq = general_equality([s - 0, v - 0, a - 0])
prob.start_equality(seq)

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

The complete solving code is as follows:

#include "speed_planning_prob.h"
#include "speed_planning_solver.h"
#include <stdio.h>

int main(void)
{
    Speed_planning_Problem prob;
    Speed_planning_Option option;
    Speed_planning_WorkSpace ws;
    Speed_planning_Output output;
    speed_planning_init(&prob, &option, &ws);

    /* params */
    prob.param[SPEED_PLANNING_PARAM_VREF] = 10.0; /* vref */
    for (size_t i = 0; i < SPEED_PLANNING_DIM_N; i++) {
        /* smin, tsafe */
        if ((i >= 70) && (i <= 80)) {
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 60.0;
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_TSAFE] = 0.2;
        } else {
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 0.0;
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_TSAFE] = 0.0;
        }
    }
    
    /* option */
    int solve_status = speed_planning_solve(&prob, &option, &ws, &output);
    
    printf("solve_status = %d\n", solve_status);
    return 0;
}