轨迹跟踪控制问题

在本示例中, 我们将展示一个车辆轨迹跟踪控制问题的建模与求解过程.

同时, 对于这类连续求解的问题, 我们将展示如何加速求解:

• 如何使用warm start减少迭代次数
• 不同的barrier参数对求解精度和迭代次数的影响

问题描述

我们构建了一个闭环仿真, 其设定如下:

• 参考轨迹为平行于x轴的直线, 速度为1 m/s
• 当$t\in[0,5]$时, $y^{\text{ref}} = 0$; 当$t>5$ 时, $y^{\text{ref}}=1.0$
• 车辆的初始状态为$(x(0),y(0),\phi(0),v(0))=(0,1,0,1)$
• 闭环控制频率为20 Hz, 仿真时间为12 s

轨迹跟踪控制器的目标函数为最小化车辆的跟踪误差 (离散化为20个时间点):

$$\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}$$

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

• 车辆运动学约束 (时间步长为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}$$

结果

闭环控制结果如下:

Warm Start

信息

Warm start可以帮助求解器收敛, 减少迭代次数. OPTIMake的warm start包括以下操作:

• 通过设置workspace中变量的initial guess
• 通过设置option中的try_warm_start_barrier参数

对于这类连续求解的问题, 我们可以使用warm start来加速求解. 在这里, 我们讨论try_warm_start_barrier参数对迭代次数的影响.

下面为设置该参数的参考 (详细请参考求解接口):

option.try_warm_start_barrier = 1;

下面为是否使用try_warm_start_barrier的迭代次数对比 (蓝色细线为开启, 黑色粗线为关闭):

可以看到, 开启该option后:

• 当warm start成功时, 迭代次数可以被显著减少
• 当warm start失败时, 迭代次数比不开启该option时多1次

Barrier参数

当代码生成中的solver参数设置为'pdipm'时, OPTIMake会使用primal-dual interior point method.

简单来说, 约束$g(v)\geq 0$会被近似为barrier函数:

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

barrier函数的参数$\rho$的目标值通过barrier_target参数设置. 在求解过程中, $\rho$会从barrier_init参数逐渐变化到barrier_target参数. barrier_target值越小, 约束的近似越精确, 但求解的迭代次数也会增加. 在这里, 我们展示了不同barrier_target值下闭环仿真效果和迭代次数对比.

不同barrier_target值下的闭环仿真效果对比:

barrier_target=1e-2

barrier_target=1e-4

barrier_target=1e-6

不同barrier_target值下的迭代次数对比 (蓝色细线为开启try_warm_start_barrier, 黑色粗线为关闭try_warm_start_barrier):

barrier_target=1e-2

barrier_target=1e-4

barrier_target=1e-6

可以看到, 当barrier_target的影响如下:

• 当barrier_target=1e-2时, 迭代次数最少, warm start成功率最高, 计算时耗 (正比于迭代次数) 最小. 但控制量$\delta$离边界约束较远, 精度较差
• 当barrier_target=1e-6时, 迭代次数最多, warm start成功率最低, 计算时耗 (正比于迭代次数) 最高. 但控制量$\delta$离边界约束较近, 最精确

在实际应用中, 我们需要根据具体的需求来选择barrier_target的值.

附录

轨迹跟踪控制器的建模代码如下:

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)

轨迹跟踪闭环仿真的代码如下:

#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;
}