一篇文章用最为基本SX来构建Single-Shooting解决MPC问题的方法。这篇最开始会简单讲解一下Mult-Shooting的不同之处,并且用其他的CasADi结构实现同样的问题。

Multi-Shooting

如上节对比图中所示,multi-shooting对状态也进行离散。这里不对方法进行过多展开,只介绍一下代码上的区别之处。具体完整的代码详见MPC/sim_2_mpc_mul_shooting.py代码。

if __name__ == '__main__':
    ... 建模部分 完全一致 ...
    ...
    # 优化目标及约束条件
    obj = 0 # 初始化优化目标
    g = [] # 约束条件(相等约束)
    g.append(X[:, 0]-P[:3]) # 【主要不同之处】在每一离散段内开始处相等
    for i in range(N):
        obj = obj + ca.mtimes([(X[:, i]-P[3:]).T, Q, X[:, i]-P[3:]]) + ca.mtimes(
                      [U[:, i].T, R, U[:, i]]) # 和之前一样的代价函数
        x_next_ = f(X[:, i], U[:, i])*T +X[:, i]
        g.append(X[:, i+1]-x_next_) # 之后每一段开始也是得相等
    # 定义优化问题
    ## 输入变量。和之前不同的是,这里需要同时将系统状态X也作为优化变量输入,
    ## 根据CasADi要求,必须将它们都变形为一维向量
    opt_variables = ca.vertcat( ca.reshape(U, -1, 1), ca.reshape(X, -1, 1)) 
    ## 和前例相同的定义方式和设置
    nlp_prob = {'f': obj, 'x': opt_variables, 'p':P, 'g':ca.vertcat(*g)}
    opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}

    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)

和之前一样,得到问题的定义和其求解方法后,我们可以开始仿真。

if __name__ == '__main__':
    ... 
    ...
    # 仿真部分
    ## 约束条件,因为约束条件不变,在while循环外定义
    ### g(条件约束,均为0,即起始点相等)
    lbg = 0.0
    ubg = 0.0
    ### 优化状态约束
    lbx = []
    ubx = []
    #### 因为之前定义的时候是先U再X所以这里定义的时候也需要先定义U的约束后再添加X约束
    for _ in range(N):
        lbx.append(-v_max)
        lbx.append(-omega_max)
        ubx.append(v_max) 
        ubx.append(omega_max)
    for _ in range(N+1):
        lbx.append(-2.0)
        lbx.append(-2.0)
        lbx.append(-np.inf)
        ubx.append(2.0)
        ubx.append(2.0)
        ubx.append(np.inf)

    ## 其他初始条件,同前
    t0 = 0.0
    x0 = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
    x0_ = x0.copy()
    x_m = np.zeros((n_states, N+1))
    next_states = x_m.copy().T
    xs = np.array([1.5, 1.5, 0.0]).reshape(-1, 1) # final state
    u0 = np.array([1,2]*N).reshape(-1, 2).T # 要注意输入的形状为(n, 2)
    x_c = []
    u_c = []
    t_c = []
    xx = []
    sim_time = 20.0

    ## MPC开始
    mpciter = 0
    start_time = time.time()
    index_t = []
    while(np.linalg.norm(x0-xs)>1e-2 and mpciter-sim_time/T<0.0 ):
        ### 每次循环先初始化U,X和参数。注意初始化顺序和形状
        c_p = np.concatenate((x0, xs))
        init_control = np.concatenate((u0.reshape(-1, 1), next_states.reshape(-1, 1)))
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
        index_t.append(time.time()- t_)
        ### 得到结果
        estimated_opt = res['x'].full() # 先转换为numpy
        u0 = estimated_opt[:200].reshape(N, n_controls) # 已知前200为U的结果,转换成(n,2)
        x_m = estimated_opt[200:].reshape(N+1, n_states) # 剩余部分为MPC计算出之后N步可能的状态
        x_c.append(x_m.T)
        u_c.append(u0[0, :])
        t_c.append(t0)
        t0, x0, u0, next_states = shift_movement(T, t0, x0, u0, x_m, f)
        x0 = ca.reshape(x0, -1, 1)
        x0 = x0.full()
        xx.append(x0)
        mpciter = mpciter + 1

在和Sim1完全相同条件下,sim2的方法只需要0.026秒完成一个循环。可见不同方法对于求解效果的影响十分显著。

不同版本对比

接下来对其他不同的实现方法进行说明和对比。

MX

MX是CasADi提供的另一种符合实现建模的形式。具体的定义和说明,官网已经给出很好的解释。

The MX class is used to build up trees made up from MXNodes. It is a more general graph representation than the scalar expression, SX, and much less efficient for small objects. On the other hand, the class allows much more general operations than does SX, in particular matrix valued operations and calls to arbitrary differentiable functions.

理解是一种介于SX和直接数据的格式。由于在构建问题上只是SX->MX这样切换。这里就不过多介绍。

Structure

对于更为复杂的问题,CasADi还提供了一种较为综合的方式来定义状态,不至因为众多定义而导致混乱,可以理解为CasADi提供的一个便捷工具(有SX和MX版本,我这里只以SX为例)。使用这个工具需要添加如下引用

import casadi.tools as ca_tools

接下来我们进行具体代码讲解。同样地,和之前一致的内容不重复介绍。

if __name__ == '__main__':
    T = 0.2 
    N = 100
    rob_diam = 0.3
    v_max = 0.6
    omega_max = np.pi/4.0
    # 新的定义状态方式,非常直观。
    states = ca_tools.struct_symSX([
        (
            ca_tools.entry('x'),
            ca_tools.entry('y'),
            ca_tools.entry('theta')
        )
    ])
    # 为了方便操作,可以将里面的元素单独取出。如果只有一个状态,取出的变量后一定要加
    # 逗号,例如: a, = some_structure[...]。否则后续会出现维度错误的问题。
    x, y, theta = states[...]
    # 可以直接获得状态尺寸
    n_states = states.size
    # 对于控制变量同理可得
    controls  = ca_tools.struct_symSX([
        (
            ca_tools.entry('v'),
            ca_tools.entry('omega')
        )
    ])
    v, omega = controls[...]
    n_controls = controls.size

    # 定义函数的时候就可以借用之前定义的状态,因为x_dot = f(x)。
    # 注意这里是struct_SX
    rhs = ca_tools.struct_SX(states)
    rhs['x'] = v*np.cos(theta)
    rhs['y'] = v*np.sin(theta)
    rhs['theta'] = omega

    # 这一步和之前一样,定义好函数
    f = ca.Function('f', [states, controls], [rhs], ['input_state', 'control_input'], ['rhs'])
if __name__ == '__main__':
    ...
    # 定义MPC问题
    ## 优化的目标,同样包含U和X。在这里需要注意形式和结果的差别,
    ## 之前SX的方法实际上是定义了矩阵,而struc_symSX加上repeat的方式
    ## 定义的是list,每个list是一个struct
    optimizing_target = ca_tools.struct_symSX([
        (
            ca_tools.entry('U', repeat=N, struct=controls),
            ca_tools.entry('X', repeat=N+1, struct=states)
        )
    ])
    ## 取出,简化公式书写
    U, X, = optimizing_target[...]
    ## 对于参数,只有shape,因为就一维
    current_parameters = ca_tools.struct_symSX([
        (
            ca_tools.entry('P', shape=n_states+n_states),
        )
    ])
    P, = current_parameters[...] # 注意逗号

    Q = np.array([[1.0, 0.0, 0.0],[0.0, 1.0, 0.0],[0.0, 0.0, .1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    ## 代价函数和约束
    obj = 0 
    g = [] 
    ### 正如之前所说的,repeat定义出来的结果是list,所以这里index的表达方式跟之前的有所区别
    g.append(X[0]-P[:3])
    for i in range(N):
        obj = obj + ca.mtimes([(X[i]-P[3:]).T, Q, X[i]-P[3:]]) + ca.mtimes([U[i].T, R, U[i]])
        x_next_ = f(X[i], U[i])*T + X[i]
        g.append(X[i+1] - x_next_)
    # 这里optimizing_target不需要压缩成一维矩阵就可以作为输入
    nlp_prob = {'f': obj, 'x': optimizing_target, 'p':current_parameters, 'g':ca.vertcat(*g)}
    opts_setting = {'ipopt.max_iter':200, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}

    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)

求解器定义完后,就可以开始仿真了。不过这里需要特别说明的是这个约束的定义。这也是我花了很多时间才搞清楚的关系。

if __name__ == '__main__':
    ...
    ...
    # 约束条件
    ## g的约束跟之前一致
    lbg = 0.0
    ubg = 0.0
    lbx = []
    ubx = []
    ## 因为U和X放在一个结构里,所以在前N循环是根据以下顺序定义的
    for _ in range(N):
        lbx = lbx + [-v_max, -omega_max, -2.0, -2.0, -np.inf]
        ubx = ubx + [v_max, omega_max, 2.0, 2.0, np.inf]
    ## 因为X有N+1个元素,单独需要再定义第N+1个元素
    lbx.append(-2.0)
    lbx.append(-2.0)
    lbx.append(-np.inf)
    ubx.append(2.0)
    ubx.append(2.0)
    ubx.append(np.inf)

后来,才发现其实CasADi还提供了更为优美的定义方法

if __name__ == '__main__':
    ...
    ...
    # 约束条件
    ## g的约束跟之前一致
    lbg = 0.0
    ubg = 0.0
    lbx = optimizing_target(-ca.inf)
    ubx = optimizing_target(ca.inf)
    lbx['U', :, 'v'] = -v_max
    lbx['U', :, 'omega'] = -omega_max
    lbx['X', :, 'x'] = -2.0
    lbx['X', :, 'y'] = -2.0
    ubx['U', :, 'v'] = v_max
    ubx['U', :, 'omega'] = omega_max
    ubx['X', :, 'x'] = 2.0
    ubx['X', :, 'y'] = 2.0

瞬间美观很多。。。接下来就是循环了。

if __name__ == '__main__':
    # Simulation
    t0 = 0.0
    x0 = np.array([0.0, 0.0, 0.0]).reshape(-1, 1)
    x0_ = x0.copy()
    xs = np.array([1.5, 1.5, np.pi/2.0]).reshape(-1, 1)
    u0 = np.array([1,2]*N).reshape(-1, 2).T
    ff_value = np.array([0.0, 0.0, 0.0]*(N+1)).reshape(-1, 3).T
    x_c = []
    u_c = []
    t_c = []
    xx = []
    sim_time = 20.0

    ## 开始MPC
    mpciter = 0
    ### 初始化变量
    c_p = current_parameters(0)
    init_control = optimizing_target(0)
    start_time = time.time()
    index_t = []
    while(np.linalg.norm(x0-xs)>1e-2 and mpciter-sim_time/T<0.0 ):
        ### 设置每次循环的初始值
        c_p['P'] = np.concatenate((x0, xs))
        init_control['X', lambda x:ca.horzcat(*x)] = ff_value
        init_control['U', lambda x:ca.horzcat(*x)] = u0[:, 0:N]
        t_ = time.time()
        res = solver(x0=init_control, p=c_p, lbg=lbg, lbx=lbx, ubg=ubg, ubx=ubx)
        index_t.append(time.time()- t_)
        ### 主要的不同点是取出结果的时候。暂时不知道有没有更好的解决办法。
        ### 原则上还是需要知道其内部U和X是按照什么顺序定义的。
        estimated_opt = res['x'].full()
        ### 最后三个结果是N+1的X
        ff_last_ = estimated_opt[-3:] 
        ### 之前一共有N个U和X
        temp_estimated = estimated_opt[:-3].reshape(-1, 5)
        u0 = temp_estimated[:, :2].T
        ff_value = temp_estimated[:, 2:].T
        ### 将最后一个N+1接上前面的N个结果
        ff_value = np.concatenate((ff_value, estimated_opt[-3:].reshape(3, 1)), axis=1) 
        x_c.append(ff_value)
        u_c.append(u0[:, 0])
        t_c.append(t0)
        t0, x0, u0 = shift_movement(T, t0, x0, u0, f)
        x0 = ca.reshape(x0, -1, 1)
        xx.append(x0.full())
        mpciter = mpciter + 1

这样通过结构的方式就能定义好相同问题。方便的地方是对于多个维度变量的处理,相对较为直观,缺点是得了解内部排列的顺序,否则得到的结果会乱套。速度上其和SX不分伯仲。

Opti

本节最后介绍的就是Opti方法。这个方法最像在写数学表达式。

if __name__ == '__main__':
    T = 0.2
    N = 100
    v_max = 0.6
    omega_max = np.pi/4.0
    # 创建一个opti对象
    opti = ca.Opti()
    # 创建并添加一个变量,这个变量就是优化的变量。
    # 注意这里我使用Nx2维度的表达,比较直观
    opt_controls = opti.variable(N, 2)
    # 获得单独速度和角速度
    v = opt_controls[:, 0]
    omega = opt_controls[:, 1]
    # 同理对于小车状态
    opt_states = opti.variable(N+1, 3)
    x = opt_states[:, 0]
    y = opt_states[:, 1]
    theta = opt_states[:, 2]

    # 添加两个额外参数,每个opti参数在求解前必须初始化值
    opt_x0 = opti.parameter(3)
    opt_xs = opti.parameter(3)
    # 用Python方式建立数学模型
    f = lambda x_, u_: ca.vertcat(*[u_[0]*np.cos(x_[2]), u_[0]*np.sin(x_[2]), u_[1]])
    # 这里是为了方便用numpy直接调用的版本
    f_np = lambda x_, u_: np.array([u_[0]*np.cos(x_[2]), u_[0]*np.sin(x_[2]), u_[1]])

    # 初始条件,..受约束于..跟公式一致
    opti.subject_to(opt_states[0, :] == opt_x0.T)
    for i in range(N):
        x_next = opt_states[i, :] + f(opt_states[i, :], opt_controls[i, :]).T*T
        opti.subject_to(opt_states[i+1, :]==x_next)

    Q = np.array([[1.0, 0.0, 0.0],[0.0, 5.0, 0.0],[0.0, 0.0, .1]])
    R = np.array([[0.5, 0.0], [0.0, 0.05]])
    # 代价函数,注意index记法
    obj = 0
    for i in range(N):
        obj = obj + ca.mtimes([(opt_states[i, :]-opt_xs.T), Q, (opt_states[i, :]-opt_xs.T).T]) + ca.mtimes([opt_controls[i, :], R, opt_controls[i, :].T])
    # 纯数学标识
    opti.minimize(obj)

    # 添加边界条件
    opti.subject_to(opti.bounded(-2.0, x, 2.0))
    opti.subject_to(opti.bounded(-2.0, y, 2.0))
    opti.subject_to(opti.bounded(-v_max, v, v_max))
    opti.subject_to(opti.bounded(-omega_max, omega, omega_max))
    # 同样的设置
    opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}

    opti.solver('ipopt', opts_setting)

    # 以下开始仿真
    final_state = np.array([1.5, 1.5, 0.0])
    ## 设定opti参数的值
    opti.set_value(opt_xs, final_state)
    t0 = 0
    init_state = np.array([0.0, 0.0, 0.0])
    u0 = np.zeros((N, 2))
    current_state = init_state.copy()
    next_states = np.zeros((N+1, 3))
    x_c = [] 
    u_c = []
    t_c = []
    xx = []
    sim_time = 20.0

    ## start MPC
    mpciter = 0
    start_time = time.time()
    index_t = []
    while(np.linalg.norm(current_state-final_state)>1e-2 and mpciter-sim_time/T<0.0  ):
        ### 设置x0也就是每一步小车当前的位置作为参数
        opti.set_value(opt_x0, current_state)
        ### 初始化优化对象的值,注意维度
        opti.set_initial(opt_controls, u0)# (N, 2)
        opti.set_initial(opt_states, next_states) # (N+1, 3)
        t_ = time.time()
        sol = opti.solve()
        index_t.append(time.time()- t_)
        ### 获得结果
        u_res = sol.value(opt_controls)
        u_c.append(u_res[0, :])
        t_c.append(t0)
        next_states_pred = sol.value(opt_states)# 
        x_c.append(next_states_pred)
        # 为下一个循环做准备
        t0, current_state, u0, next_states = shift_movement(T, t0, current_state, u_res, next_states_pred, f_np)
        mpciter = mpciter + 1
        xx.append(current_state)

是不是感觉非常地直观。缺点嘛就是速度和SX相比会慢一些,可能是调用较多其他API的缘故,但是不失为初始模型测试的入手格式,或者对计算时间不敏感的,也可以使用。

预留链接

CasADi学习(1

CasADi学习(2