速度规划问题 - 基础模型

在这个例子中, 我们演示了如何使用OPTIMake建模与求解一个multi-stage问题, 例如轨迹规划问题或MPC控制问题.

问题描述

考虑一个10 s的速度规划问题, 我们需要使得速度达到给定的参考速度$v_{ref}$. 同时, 我们需要考虑以下约束:

• 加速度受限于[-3, 3] $m/s^2$
• 初始位置为0 $m$, 初始速度为0 $m/s$
• 在[7, 8] s的时间窗口内的位置需要大于60 $m$

基于以上的问题设定, 我们设定如下的目标函数:

$$\min \sum_{i=1}^{N} (v_i - v_{ref})^2$$

同时, 我们需要考虑以下的约束条件:

$$\begin{split} &\dot{s} = v \\ &\dot{v} = a \\ &s(0) = 0 \\ &v(0) = 0 \\ &s(t) \geq 60, t \in [7, 8] \\ &a \in [-3, 3] \end{split}$$

建模

上述的优化问题在OPTIMake中的建模如下.

我们将10 s的时间窗口划分为100个时间点, 也就是$N=100$ (对应时间步长为0.1 s).

prob = multi_stage_problem('speed_planning', 100)

接下来, 我们需要定义问题的参数: 速度参考值$v_{ref}$ 和 位置下限 $s_{min}$. 因为$s_{min}$是一个时间相关的参数, 所以我们需要将其定义为一个stage-dependent的参数. 也就是在每个stage (时间点)都可以设置不同的值.

vref = prob.parameter('vref',  stage_dependent=False)
smin = prob.parameter('smin',  stage_dependent=True)

然后, 我们需要定义问题的变量$s$, $v$ 和$a$. 注意到我们在定义变量时, 可以同时设置该变量的上下界 (硬边界与软边界). 例如, 我们可以设置$s$的下界为$s_{min}$, $a$的上下界为[-3, 3].

s = prob.variable('s', hard_lowerbound=smin)
v = prob.variable('v')
a = prob.variable('a', hard_lowerbound=-3, hard_upperbound=3)

接下来, 我们需要定义问题的目标函数. 这里我们使用了general_objective函数来定义目标函数.

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

接下来, 我们需要定义问题的约束条件. 这里我们使用了differential_equation函数来定义状态方程. 注意到我们在定义状态方程时, 需要设置时间步长 (也就是离散化的步长). 这里我们使用了4阶Runge-Kutta方法来离散化状态方程.

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

或者, 当状态方程是离散形式时, 我们可以使用discrete_equation函数来定义状态方程. 例如, 我们可以将状态方程离散化为如下的形式 ($h$为时间步长):

$$\begin{split} &s_{i+1} = s_i + h v_i + \frac{1}{2} h^2 a_i \\ &v_{i+1} = v_i + h a_i \\ \end{split}$$

使用discrete_equation函数来定义状态方程约束的代码如下:

h = 0.1
dis_eq = discrete_equation(
    expr_next_stage=[s, v],
    expr_this_stage=[s + h * v + 0.5 * h**2 * a, v + h * a]
)
prob.equality(dis_eq)

接下来, 我们需要定义初始条件. 这里我们使用了start_equality函数来定义初始条件.

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

完成了上述的建模后, 我们就可以生成代码了:

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

全部的代码如下:

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

s = prob.variable('s', hard_lowerbound=smin)
v = prob.variable('v')
a = prob.variable('a', hard_lowerbound=-3, hard_upperbound=3)

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

# use differential equation to define the state equation
ode = differential_equation(
    state=[s, v],
    state_dot=[v, a], 
    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],
#     expr_this_stage=[s + h * v + 0.5 * h**2 * a, v + h * a]
# )
# prob.equality(dis_eq)

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

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

求解

下面是使用C/C++调用求解器的代码.

#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 */
        if ((i >= 70) && (i <= 80)) {
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 60.0;
        } else {
            prob.param_stage[i][SPEED_PLANNING_PARAM_STAGE_SMIN] = 0.0;
        }
    }
    
    /* option */
    int solve_status = speed_planning_solve(&prob, &option, &ws, &output);
    
    printf("solve_status = %d\n", solve_status);
    return 0;
}

结果