ACADOS学习（1）

前言

继之前介绍了CasADi的简单使用后，我收到了很多朋友的私信，其中有不少人是来咨询CasADi的C版本的使用。一部分原因是因为担心Python的运行速度。说实话，我个人没有用过CasADi的C版本，官网似乎也没有很多这方面的介绍。在CasADi的Python版本中，虽然可以通过生成C语言文件或是直接生成静态链接库来提升运行效率（个人经验，比直接的Python版本只需要50%的计算时间），但是其速度仍然是比较慢的，特别是于ACADO这样C++的库相比。

前段时间，在网上查看论文的时候发现了一个比较有意思的库ACADOS(GitHub)。它的名字正好和ACADO只差一个字母，同时提供了Python/C/MATLAB的API。对于同时使用C和Python的我，决定研究看看。不过需要注意的是，其实每个库都有自己的特点，互相间不是替代关系。在此篇中，我将使用之前CasADi教程中同样的例子来展示ACADOS代码的流程，基于建立MPC的方式实现一个全向移动机器人的轨迹规划与控制。所展示的例子是比较简单的，具体的API请详见官网介绍。本文所介绍的代码，可以在本人GitHub中下载，欢迎大家测试。

ACADOS

Fast and embedded solvers for nonlinear optimal control.

如开发人员自己的介绍，该库就是为了解决非线性（也包括线性）优化问题。其他更为宽泛的应用还得使用CasADi。由于使用了 BLASFEO，可以在不同平台进行部署和配置，官网有关于dSPACE部署的介绍，理论上也可以在树莓派等ARM架构的设备上部署，不过本人尚未测试。欢迎有兴趣的朋友一起互相讨论。

至于该库与CasADi和ACADO的区别，可以详见官网FAQ。

配置问题

Github代码是基于macOS进行配置（CMakeList）。如果是Linux系统，应该可以无缝运行。Windows尚未测试过，欢迎大家debug。在当前版本，安装ACADOS时，macOS需要手动设置BLASFEO库，禁用其自动识别，并将Target architecture设置为GENERIC设备，因为如果是自动识别的Intel的CPU，则会有bug。目前正与该库开发者协作进行debug，如果后续问题解决的话，会在这里更新。Linux目前没有大的问题。

Python实现

本例将使用和之前介绍的CasADi相同的例子来展示ACADOS代码的实现，具体的例子可以详见之前的例子CasADi (2)，相同内容这里不再赘述。

小车模型

由于模型在之前的博客已经介绍过了，这里直接上代码。ACADOS其实用CasADi作为其前端，所以代码理论上一样的。在本例中，将小车的模型单独作为一个类来定义，大家可以根据自己需要自行选择合适的文件结构。代码解释详见代码注释：

class MobileRobotModel(object):
    def __init__(self,):
        # ACADOS 模型类
        model = AcadosModel() 
        # 另一种基于CasADi的自由类型定义方式，类似于dict
        constraint = ca.types.SimpleNamespace()
        # 控制输入
        v = ca.SX.sym('v')
        omega = ca.SX.sym('omega')
        # 控制输入集合
        controls = ca.vertcat(v, omega)
        # 系统状态 3x1
        x = ca.SX.sym('x')
        y = ca.SX.sym('y')
        theta = ca.SX.sym('theta')
        # 系统状态集合
        states = ca.vertcat(x, y, theta)
		# 系统微分方程
        rhs = [v*ca.cos(theta), v*ca.sin(theta), omega]
        # 定义状态和控制变量与系统方程之间的关系
        f = ca.Function('f', [states, controls], [ca.vcat(rhs)], ['state', 'control_input'], ['rhs'])
        # f_expl = ca.vcat(rhs) # 当然也可以这样直接定义
        x_dot = ca.SX.sym('x_dot', len(rhs))
        # 隐式定义
        f_impl = x_dot - f(states, controls)
        # 模型各个部分定义，从CasADi的表达式映射到ACADOS的模型中
        model.f_expl_expr = f(states, controls)
        model.f_impl_expr = f_impl
        model.x = states
        model.xdot = x_dot
        model.u = controls
        model.p = []
        model.name = 'mobile_robot'

        # 约束条件，注意这里不是ACADOS必须的要求，只是为了代码结构清晰
        constraint.v_max = 0.6
        constraint.v_min = -0.6
        constraint.omega_max = np.pi/4.0
        constraint.omega_min = -np.pi/4.0
        constraint.x_min = -2.
        constraint.x_max = 2.
        constraint.y_min = -2.
        constraint.y_max = 2.
        constraint.expr = ca.vcat([v, omega])
		# 最后得到ACADOS模型和约束的集合
        self.model = model
        self.constraint = constraint

完成如上模型后。就可以利用ACADOS来构建优化问题并求解了。官网提供了一个非常详尽的优化问题构建方法介绍文件，详请参看介绍文件。 与CasADi教程类似的，我们希望给定约束和模型后，小车能从初始位置规划并运动到最终位置。在新的文件中，我们将首先定义好系统模型和其约束，代码详见下面解释：

class MobileRobotOptimizer(object):
    def __init__(self, m_model, m_constraint, t_horizon, n_nodes):
        # 获得定义模型
        model = m_model
        # 时长
        self.T = t_horizon
        # 时间间隔数，也就是dT = T/N
        self.N = n_nodes

        # 设置环境变量
        os.chdir(os.path.dirname(os.path.realpath(__file__)))
        ## 获得系统中ACADOS的安装路径，请参照安装要求设置好
        acados_source_path = os.environ['ACADOS_SOURCE_DIR']
        sys.path.insert(0, acados_source_path)

        # x状态数
        nx = model.x.size()[0]
        self.nx = nx
        # u状态数
        nu = model.u.size()[0]
        self.nu = nu
        # ny数为x与u之和，原因详见系统CasADi例子以及ACADOS构建优化问题PDF介绍
        ny = nx + nu
        # 额外参数，本例中没有
        n_params = len(model.p)

        # 构建OCP
        ocp = AcadosOcp()
        ## 设置ACADOS系统引用以及库的路径（因为ACADOS最后将以C的形式运行，所以必须设置正确）
        ocp.acados_include_path = acados_source_path + '/include'
        ocp.acados_lib_path = acados_source_path + '/lib'
        ## 设置模型
        ocp.model = model
        ocp.dims.N = self.N
        ocp.solver_options.tf = self.T
        ocp.dims.np = n_params
        ocp.parameter_values = np.zeros(n_params)

        # 代价函数
        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]])
        ## cost类型为线性
        ocp.cost.cost_type = 'LINEAR_LS'
        ocp.cost.cost_type_e = 'LINEAR_LS'
        ocp.cost.W = scipy.linalg.block_diag(Q, R)
        ocp.cost.W_e = Q
        ## 这里V类矩阵的定义需要参考ACADOS构建里面的解释，实际上就是定义一些映射关系
        ocp.cost.Vx = np.zeros((ny, nx))
        ocp.cost.Vx[:nx, :nx] = np.eye(nx)
        ocp.cost.Vu = np.zeros((ny, nu))
        ocp.cost.Vu[-nu:, -nu:] = np.eye(nu)
        ocp.cost.Vx_e = np.eye(nx)

        # 约束条件设置
        ocp.constraints.lbu = np.array([m_constraint.v_min, m_constraint.omega_min])
        ocp.constraints.ubu = np.array([m_constraint.v_max, m_constraint.omega_max])
        ## 这里是为了定义之前约束条件影响的index，它不需要像CasADi那样定义np.inf这种没有实际意义的约束。
        ocp.constraints.idxbu = np.array([0, 1])
        ocp.constraints.lbx = np.array([m_constraint.x_min, m_constraint.y_min])
        ocp.constraints.ubx = np.array([m_constraint.x_max, m_constraint.y_max])
        ocp.constraints.idxbx = np.array([0, 1])

        # 一些状态的值，在实际仿真中可以重新给定，所里这里就定义一些空值
        x_ref = np.zeros(nx)
        u_ref = np.zeros(nu)
        ## 将给定值设定，注意到这里不需要像之前那样给所有N-1设定ref值，ACADOS会默认进行设置
        ocp.constraints.x0 = x_ref
        ### 0--N-1
        ocp.cost.yref = np.concatenate((x_ref, u_ref))
        ### N
        ocp.cost.yref_e = x_ref

        # 设置优化器参数
        ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
        ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
        ocp.solver_options.integrator_type = 'ERK'
        ocp.solver_options.print_level = 0
        ocp.solver_options.nlp_solver_type = 'SQP_RTI'

        # 最后一步就是生成配置文件以及相应的C代码，ACADOS可以将仿真器也自动生成
        # 也就是我们不需要自己模拟系统的变化状态
        ## 配置文件
        json_file = os.path.join('./'+model.name+'_acados_ocp.json')
        ## 求解器
        self.solver = AcadosOcpSolver(ocp, json_file=json_file)
        ## 仿真模拟器（仅仿真使用）
        self.integrator = AcadosSimSolver(ocp, json_file=json_file)
	...
    

在上述类的初始化后，我们得到了基于C语言的编译后的求解器和仿真器，文件会自动保持在一个c_generated_code文件夹中。接下来我们可以尝试进行仿真。在这里我们需要提供小车的初始和末端值：

class MobileRobotOptimizer(object):
    # ...
    # 开始仿真
    def simulation(self, x0, xs):
        # 存储仿真结果
        simX = np.zeros((self.N+1, self.nx))
        simU = np.zeros((self.N, self.nu))
        # 初始位置
        x_current = x0
        simX[0, :] = x0.reshape(1, -1)
        # 中间期望状态
        xs_between = np.concatenate((xs, np.zeros(2)))
        # 只是为了计算运行时间
        time_record = np.zeros(self.N)

        # 设置N时刻的期望状态
        self.solver.set(self.N, 'yref', xs)
        # 设置0至N-1时刻的期望状态
        for i in range(self.N):
            self.solver.set(i, 'yref', xs_between)
            
		# 闭环仿真
        for i in range(self.N):
            # 计时开始
            start = timeit.default_timer()
            # 设置当前循环x0 (stage 0)
            self.solver.set(0, 'lbx', x_current)
            self.solver.set(0, 'ubx', x_current)
            # 求解
            status = self.solver.solve()
			# 检查求解结果
            if status != 0 :
                raise Exception('acados acados_ocp_solver returned status {}. Exiting.'.format(status))
			# 得到下个时刻最优控制
            simU[i, :] = self.solver.get(0, 'u')
            # 计时结束
            time_record[i] =  timeit.default_timer() - start
            
            # 以下纯粹为了仿真
            # 仿真器获得当前位置和控制指令
            self.integrator.set('x', x_current)
            self.integrator.set('u', simU[i, :])
			# 仿真器计算结果
            status_s = self.integrator.solve()
            if status_s != 0:
                raise Exception('acados integrator returned status {}. Exiting.'.format(status))

            # 将仿真器计算的小车位置作为下一个时刻的初始值
            x_current = self.integrator.get('x')
            simX[i+1, :] = x_current

最终结果

时间上求解的结果应该与CasADi的结果是一致的，大家可以自行测试。但是从计算时间上看，ACADOS平均耗时应该是2-3ms，而CasADi未优化的结果是0.02秒以上。速度提高10倍左右。下期将介绍C++语言开发的版本。敬请期待。