TinyMPC 使用教程（二）

系列文章目录

前言

一、如何使用 TinyMPC

1.1 加载程序库

import tinympc
import numpy as np

tinympc_python_dir = "/path/to/tinympc-python"
tinympc_dir = tinympc_python_dir + "/tinympc/TinyMPC"

prob = tinympc.TinyMPC()
prob.compile_lib(tinympc_dir)

os_ext = ".so" # 
lib_dir = tinympc_dir + "/build/src/tinympc/libtinympcShared" + os_ext  # Path to the compiled library
prob.load_lib(lib_dir)

1.2 设置问题

查看模型，学习如何获得 A 和 B 矩阵。

对于车杆，我们使用离散车杆动力学的线性化模型来稳定直立位置。

n = 4  # states: x, xdot, theta, thetadot
m = 1  # controls: force on cart
N = 10  # horizon

A = [ # row-major order 
    1, 0, 0, 0, 
    0.01, 1, 0, 0, 
    2.2330083403-5, 0.0044662105, 1.0002605176, 0.0521057900,
    7.4430379746-8, 2.2330083403-5, 0.0100008683, 1.0002605176
]
B = [ # row-major order
    7.4683685627-5,
    0.0149367653,
    3.7976332318-5,
    0.0075955962
]

Q = [10.0, 1, 10, 1]  # diagonal elements in row-major order
R = [1.0]  # diagonal elements in row-major order
rho = 0.1  # ADMM penalty parameter

x_min = [-5.0] * n * N  # state constraints
x_max = [5.] * n * N  # state constraints
u_min = [-5.] * m * (N - 1)  # force constraints
u_max = [5.] * m * (N - 1)  # force constraints

abs_pri_tol = 1.0e-3  # absolute primal tolerance
abs_dual_tol = 1.0e-3  # absolute dual tolerance
max_iter = 100  # maximum number of iterations
check_termination = 1  # how often termination conditions are checked

# Setup problem data
prob.setup(n, m, N, A, B, Q, R, x_min, x_max, u_min, u_max, rho,
    abs_pri_tol, abs_dual_tol, max_iter, check_termination)

1.3 生成求解器代码

我们生成了可在嵌入式硬件上部署的低级 C++ 代码。

output_dir = tinympc_python_dir + "/generated_code"  # Path to the generated code
prob.tiny_codegen(tinympc_dir, output_dir)
prob.compile_lib(output_dir)

prob.load_lib(output_dir + "/build/tinympc/libtinympcShared" + os_ext)  # Load the library

1.4 与求解器代码交互

在部署之前，我们还可以通过高级语言交互来验证程序。这自然为软件在环 (SIL) 测试提供了便利。以下是在仿真中运行 MPC 的示例，您可以使用任何仿真器。

x = [0.0] * n * N  # Initial state
u = [0.0] * m * (N - 1)  # List of control inputs in horizon
x_all = []  # List of all stored states
x_noise = x * 1
# Matrices for simulation
Anp = np.array(A).reshape((n, n)).transpose()
Bnp = np.array(B).reshape((n, m))

print("=== START INTERACTIVE MPC ===")

NSIM = 300
for i in range(NSIM):
    # 1. Set initial state from measurement    
    prob.set_x0(x_noise, 0)  # Set initial state to C code

    # 2. Set the reference state if needed    

    # 3. Solve the problem
    prob.solve(0)  # Call the solve in C code

    # 4. Get the control input
    prob.get_u(u, 0)  # Get the control input from C code

    # 5. Simulate the dynamics    
    x = Anp@np.array(x).reshape((n, 1))+ Bnp*np.array(u[0]) 

    noise = np.random.normal(0, 0.01, (n, 1))
    x_noise = x + noise
    # print(f"X = {x}")
    x = x.reshape(n).tolist() 
    x_noise = x_noise.reshape(n).tolist() 
    # print(f"X = {x}")
    x_all.append(x)

print((x_all))

二、获取模型

2.1 线性化

TinyMPC 原型只能处理线性动力学，这意味着在求解器使用之前，系统必须围绕一个平衡点进行线性化。TinyMPC 的扩展功能允许用户通过存储多个线性化来近似系统的非线性动力学，但我们将只从一个开始。

离散线性化系统的形式为 $x_{k+1}=Ax_k+Bu_k$ ，其中 $x_k$ 和 $u_k$ 分别为当前时间步的状态和控制， $A$ 为状态转换矩阵， $B$ 为控制或输入矩阵， $x_{k+1}$ 为下一时间步的状态。在上一页给出的每个示例中，状态转换矩阵 $A$ 和输入矩阵 $B$ 都是根据系统的连续非线性动力学计算得出的。

2.2 车杆示例

车杆的连续时间动力学已被多次推导。在本例中，我们将使用该推导中的惯例，即杆在 $\theta=0$ 处直立。如果我们忽略该模型中的摩擦力，那么在该推导中我们唯一关心的方程就是 (23) 和 (24)。
让我们把它们写成一个动力学函数

mc = 0.2 # mass of the cart (kg)
mp = 0.1 # mass of the pole (kg)
ℓ = 0.5 # distance to the center of mass (meters)
g = 9.81
# 

def cartpole_dynamics(x, u):
    r       = x[0] # cart position
    theta   = x[1] # pole angle
    rd      = x[2] # change in cart position
    theta_d = x[3] # change in pole angle
    F       = u[0] # force applied to cart

    theta_dd = (g*np.sin(theta) + np.cos(theta) * ((-F - mp*l*(theta_d**2) * \
                    np.sin(theta))/(mc + mp))) / (l*(4/3 - (mp*(np.cos(theta)**2))/(mc + mp)))
    rdd = (F + mp*l*((theta_d**2)*np.sin(theta) - theta_dd*np.cos(theta))) / (mc + mp)

    return np.array([rd, theta_d, rdd, theta_dd])

该函数描述了系统的连续（非线性）动力学，即 $\dot{x}=\text{cartpole}\_\text{dynamics}(x,u)$ 。
在线性化之前，我们先用积分器将连续动力学离散化。RK4（Runge-Kutta 四阶）是一种常见的显式积分器，但也可以随意写入。

def cartpole_rk4(x, u, dt):
    f1 = dt*cartpole_dynamics(x, u)
    f2 = dt*cartpole_dynamics(x + f1/2, u)
    f3 = dt*cartpole_dynamics(x + f2/2, u)
    f4 = dt*cartpole_dynamics(x + f3, u)
    return x + (1/6)*(f1 + 2*f2 + 2*f3 + f4)

我们的积分器接收当前时间步的状态和控制，并将状态向前积分 $\Delta t$ 秒（RK4 函数中的 dt 参数）。现在我们有了由 $x_{k+1}=\text{cartpole\_rk}4(x_k,u_k,dt)$ 定义的系统离散（非线性）动力学。现在，我们围绕平衡位置进行线性化，以获得前面描述的状态转换矩阵 $A$ 和输入矩阵 $B$ 。手工微分 $\text{cartpole\_rk4}$ 会很麻烦，但幸运的是，我们可以使用自动微分工具来完成这项工作。

import autograd as AG

xgoal = np.array([0.0, np.pi, 0.0, 0.0])
ugoal = np.array([0.0])

dt = 0.01

A = AG.jacobian(lambda x_: cartpole_rk4(x_, ugoal))(xgoal)
B = AG.jacobian(lambda u_: cartpole_rk4(xgoal, u_))(ugoal)

现在要做的就是保存 $A$ 和 $B$ ，并将它们传递给 TinyMPC，如示例页面的问题设置部分所示。

三、求解器

TinyMPC 的核心是利用 MPC 问题的结构来加速和压缩 ADMM 算法。

3.1 问题表述

TinyMPC 可求解形式如下的凸二次模型预测控制程序

$\begin{aligned} &\text{minimize}&& \sum_{k=0}^{N-1}\frac12(x_k-\bar{x}_k)^TQ(x_k-\bar{x}_k)+\frac12(u_k-\bar{u}_k)^TR(u_k-\bar{u}_k) \\ &\text{subject to}&& x_0=x_\text{init}, \\ &&&x_{k+1}=Ax_k+Bu_k, \\ &&&u_k^l\leq u_k\leq u_k^u, \\ &&&x_k^l\leq x_k\leq x_k^u, \\ &&&u_k\in\mathcal{K}_u, \\ &&&x_k\in\mathcal{K}_x \end{aligned}$

其中， $x_k\in\mathbb{R}^n,u_k\in\mathbb{R}^m$ 为时间步长 $k$ 时的状态和控制输入， $N$ 为时间步长数（也称为视平线）， $A\in\mathbb{R}^{n\times n}$ 和 $B\in\mathbb{R}^{n\times m}$ 定义了系统动力学， $Q\succeq0,R\succ0$ 和 $Q_f\succeq0$ 为对称成本权重矩阵， $\bar{x}_{k}$ 和 $\bar{u}_k$ 为状态和输入参考轨迹。约束条件包括状态和输入的下限和上限以及二阶锥 $\mathcal{K}$ 。

3.2 算法

将很快提供！同时，请查阅我们的论文或背景资料，了解更多详情。

3.3 实现

TinyMPC 库提供了上述算法的 C++ 实现，以及与几种高级语言的接口。通过这种集成，这些语言可以使用 TinyMPC 无缝地解决最优控制问题。

此外，还有几种社区开发的算法实现： Rust

在该资源库中可以找到与其他微控制器求解器的数值基准比较。

该资源库提供了使用 TinyMPC 的 Crazyflie 固件。

四、背景知识

4.1 概述

TinyMPC 的基础算法是交替方向乘法。TinyMPC 将基元更新步骤 —— 通常耗时最长的部分- —— 重新表述为一个 LQR 问题。这些问题已经研究了几十年，我们知道如何以封闭形式写出 LQR 问题：具体来说，就是使用里卡提递归法。我们重组了部分递归函数，以提取只需计算一次的大矩阵。在原型实现中，这限制了 TinyMPC 只能求解线性轨迹跟踪问题（只要能在线快速重线性化，各种约束条件均可求解）。不过，正如我们在演示视频中看到的那样，一次线性化就能解决很多问题。

4.2 交替方向乘法（ADMM）

交替方向乘法算法开发于 20 世纪 70 年代，2011 年被斯坦福大学的研究人员用于更好地解决分布式凸优化问题。其中一些研究人员后来帮助开发了OSQP，即运算器分裂二次规划求解器。TinyMPC 的灵感主要来自这两个方面。

我们希望解决成本函数 $\text{f}$ 和有效状态集 $\mathcal{C}$ 都是凸的优化问题：

$\begin{aligned}\min_x\quad&f(x)\\\text{subject to}\quad&x\in\mathcal{C}.\end{aligned}$

我们为集合 $\mathcal{C}$ 定义一个指标函数：

$I_\mathcal{C}(x)=\begin{cases}0&x\in\mathcal{C}\\\infty&\text{otherwise.}\end{cases}$

指标函数简单地说，当 $x$ 违反约束条件时（状态 $x$ 在有效状态集合 $\mathcal{C}$ 之外），额外成本为无限；而当 $x$ 遵守约束条件时（ $x$ 在有效状态集合 $\mathcal{C}$ 之内），额外成本为零。因此，我们需要确定一个状态是否在集合 $\mathcal{C}$ 中，才能知道问题的所有约束条件是否都得到了满足。为了提高计算速度，通常采用 $Hx\geq h$ （或 $Hx\leq h$ ，或 $Hx\geq h$ 和 $Gx\leq g$ 的组合（每种组合都可以重写为等价于其他组合））的形式。这种形式可以描述 $x$ 中的任何一种线性约束。例如，为了避开障碍物，通常会将 $H$ 和 $h$ 安排为半空间约束，在三维空间中，整个空间被一个平面分割，只有一半在 $\mathcal{C}$ 集内。对于任意维度，我们说空间被超平面分割。

我们修改一般优化问题，将指标函数加入成本中。我们引入一个新的状态变量 $z$ ，称为松弛变量，用来描述原始状态变量 $x$ 的受限版本，我们现在称其为原始变量。

$\begin{aligned}\min_x&\quad f(x)+I_\mathcal{C}(z)\\\mathrm{subject~to}&\quad x=z.\end{aligned}$

在最小成本时，主变量 $x$ 必须等于松弛变量 $z$ ，但在每次求解过程中，它们不一定相等。这是因为松弛变量 $z$ 在算法中表现为原始变量 $x$ 被投影到可行集 $\mathcal{C}$ 上的版本，因此每当原始变量 $x$ 违反任何约束条件时，该迭代的松弛变量就会被投影回 $\mathcal{C}$ 上，从而与 $x$ 不同。为了将原始变量 $x$ 推回可行集 $\mathcal{C}$ ，我们引入了第三个变量 $\lambda$ ，称为对偶变量。这种方法被称为增强拉格朗日法（原名乘数法），在引入对偶变量 $\lambda$ 的同时引入一个标量惩罚参数 $\rho$ （也称为拉格朗日乘数）。惩罚参数 $\rho$ 是对上述约束优化问题的拉格朗日的增强。 $x$ 与 $z$ 的差值越大，增强拉格朗日的成本就越高，从而迫使 $x$ 更接近 $z$ 。

$\mathcal{L}_A(x,z,\lambda)=f(x)+I_\mathcal{C}(z)+\lambda^\intercal(x-z)+\frac\rho2\|x-z\|_2^2.$

现在，我们的优化问题被分为两个变量：原始 $x$ 和松弛 $z$ ，我们可以在保持所有其他变量不变的情况下，对每个变量单独进行优化。要使用 ADMM 算法，我们只需交替求解 $x$ 和使我们的增强拉格朗日最小化的 $z$ 。每次求解后，我们都会根据 $x$ 与 $z$ 的差异更新我们的对偶变量 $\lambda$ 。

$\begin{aligned}\text{primal update: }x^+&=\underset{x}{\operatorname*{\arg\min}}\mathcal{L}_A(x,z,\lambda),\\\text{slack update: }z^+&=\underset{z}{\operatorname*{\arg\min}}\mathcal{L}_A(x^+,z,\lambda),\\\text{dual update: }\lambda^+&=\lambda+\rho(x^+-z^+),\end{aligned}$