代码收藏家 MPC控制四旋翼无人机：利用ACADO及Python实践指南
目录
一、acados安装（ubantu）
1.Clone acados
2.CMake
二、python接口使用
1.Create a Python virtual environment using
2.安装acados_template这个python库
4.Run a Python example to check that everything works.
三、使用acados控制mpc
1.polynomial_trajectory_generator.py
2.quadrotor_model.py
3.mpc_solver.py
4.mpc_controller.py
5. 运行mpc_controller.py
一、acados安装（ubantu）
参考链接：Installation — acados documentation
1.Clone acados
git clone https://github.com/acados/acados.git
cd acados
git submodule update --recursive --init2.CMake
mkdir -p build
cd build
cmake -DACADOS_WITH_QPOASES=ON ..
make install -j4上述两步完成后就把acados安装到自己的电脑上了。
二、python接口使用
参考连接：Python Interface — acados documentation
1.Create a Python virtual environment using
# virtualenv 创建虚拟环境的指令
# env 虚拟环境所在路径， env是一个文件夹
# 虚拟环境所用python解释器的路径/usr/bin/python3
virtualenv env --python=/usr/bin/python3
# Source the environment
source env/bin/activate
# 退出虚拟环境的指令是deactivate
激活虚拟环境成功后如下图，在最左边多了一个env，在这个虚拟环境下运行python文件，就是使用的这个虚拟环境下的python解释器和对应的库。
2.安装acados_template这个python库
#acados_root 表示你刚才克隆下来的acadas的路径
pip install -e <acados_root>/interfaces/acados_template这个库被装在虚拟环境下了，你在非虚拟环境下直接用透视找不到这个库。这个指令需要再虚拟环境下执行。
3. Add the path to the compiled shared libraries
最后直接把这个环境变量放到自己的./.bashrc里面
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"<acados_root>/lib"
export ACADOS_SOURCE_DIR="<acados_root>"4.Run a Python example to check that everything works.
python <acados_root>/examples/acados_python/getting_started/minimal_example_ocp.py运行成功后的结果如下图。
三、使用acados控制mpc
包括四个文件，quadrotor_model.py， mpc_controller.py，mpc_solver.py，polynomial_trajectory_generator.py。
1.polynomial_trajectory_generator.py
这个文件的作用是生成一个多项式，四旋翼无人机跟随这个多项式轨迹
import numpy as np
def generate_polynomial_trajectory_at_t(t):
"""
根据给定时间 t 生成多项式轨迹的状态
:param t: 当前时间
:return: 多项式轨迹在 t 时刻的状态，形状为 (10,)，包含速度、位置和姿态信息
"""
# 示例多项式：x = 0.1 * t^2, y = 0.2 * sin(0.5 * t), z = 0.05 * t
x = 0.1 * t ** 2
y = 0.2 * np.sin(0.5 * t)
z = 0.05 * t
# 通过求导计算平滑的速度
vx = 0.2 * t
vy = 0.1 * np.cos(0.5 * t)
vz = 0.05
# 假设姿态为单位四元数
q0 = 1
q1 = 0
q2 = 0
q3 = 0
state = np.array([vx, vy, vz, x, y, z, q0, q1, q2, q3])
return state2.quadrotor_model.py
这个文件是描述了四旋翼无人机的动力学模型
import casadi as cs
import numpy as np
# 定义状态维度，包含三轴速度、三轴位置和四元数
STATE_DIM = 10
# 定义输入维度，包含合推力和三轴角速度
INPUT_DIM = 4
def quadrotor_dynamics(x, u):
"""
四旋翼无人机的动力学模型，使用 CasADi 符号变量
:param x: 状态向量 [vx, vy, vz, x, y, z, q0, q1, q2, q3] 的 CasADi 符号变量
:param u: 输入向量 [thrust, wx, wy, wz] 的 CasADi 符号变量
:return: 状态导数的 CasADi 符号表达式
"""
# 提取状态和输入
vx = x[0]
vy = x[1]
vz = x[2]
px = x[3]
py = x[4]
pz = x[5]
q0 = x[6]
q1 = x[7]
q2 = x[8]
q3 = x[9]
thrust = u[0]
wx = u[1]
wy = u[2]
wz = u[3]
# 这里给出简化的动力学模型示例，实际应用中需要更精确的模型
# 假设质量 m = 1，重力加速度 g = 9.81
m = 1
g = 9.81
# 速度导数
dvx_dt = thrust * (2 * (q1 * q2 + q0 * q3)) / m
dvy_dt = thrust * (2 * (q2 * q3 + q0 * q1)) / m
dvz_dt = thrust * (q0 ** 2 + q3 ** 2 - q1 ** 2 - q2 ** 2) / m - g
# 位置导数
dx_dt = vx
dy_dt = vy
dz_dt = vz
# 四元数导数
dq0_dt = 0.5 * (-wx * q1 - wy * q2 - wz * q3)
dq1_dt = 0.5 * (wx * q0 + wz * q2 - wy * q3)
dq2_dt = 0.5 * (wy * q0 - wz * q1 + wx * q3)
dq3_dt = 0.5 * (wz * q0 + wy * q1 - wx * q2)
# 状态导数向量
dxdt = cs.vertcat(dvx_dt, dvy_dt, dvz_dt, dx_dt, dy_dt, dz_dt, dq0_dt, dq1_dt, dq2_dt, dq3_dt)
return dxdt
def quadrotor_dynamics_np(x, u):
"""
四旋翼无人机的动力学模型，输入和输出都是 numpy 数组
:param x: 状态向量 [vx, vy, vz, x, y, z, q0, q1, q2, q3] 的 numpy 数组
:param u: 输入向量 [thrust, wx, wy, wz] 的 numpy 数组
:return: 状态导数的 numpy 数组
"""
# 提取状态和输入
vx, vy, vz, px, py, pz, q0, q1, q2, q3 = x
thrust, wx, wy, wz = u
# 假设质量 m = 1，重力加速度 g = 9.81
m = 1
g = 9.81
# 速度导数
dvx_dt = thrust * (2 * (q1 * q2 + q0 * q3)) / m
dvy_dt = thrust * (2 * (q2 * q3 + q0 * q1)) / m
dvz_dt = thrust * (q0**2 + q3**2 - q1**2 - q2**2) / m - g
# 位置导数
dx_dt = vx
dy_dt = vy
dz_dt = vz
# 四元数导数
dq0_dt = 0.5 * (-wx * q1 - wy * q2 - wz * q3)
dq1_dt = 0.5 * (wx * q0 + wz * q2 - wy * q3)
dq2_dt = 0.5 * (wy * q0 - wz * q1 + wx * q3)
dq3_dt = 0.5 * (wz * q0 + wy * q1 - wx * q2)
# 状态导数向量
dxdt = np.array([dvx_dt, dvy_dt, dvz_dt, dx_dt, dy_dt, dz_dt, dq0_dt, dq1_dt, dq2_dt, dq3_dt])
return dxdt3.mpc_solver.py
这个文件是配置mpc求解器，比较关键。特别是下面这条代码很重要
x_ref = np.zeros(STATE_DIM)
self.ocp.constraints.x0 = x_ref如果不加上上面的代码，那么在运行下面的代码时，就会提示‘lbx‘的维度为0。
self.ocp_solver.set(0, 'lbx', x0）
self.ocp_solver.set(0, 'ubx', x0)全部代码
import numpy as np
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
import casadi as cs
from quadrotor_model import STATE_DIM, INPUT_DIM, quadrotor_dynamics
from quadrotor_model import quadrotor_dynamics_np
from polynomial_trajectory_generator import generate_polynomial_trajectory_at_t
import time
class MPCSolver:
def __init__(self):
# 创建 acados OCP 问题实例
self.ocp = AcadosOcp()
# 设置模型名称
self.ocp.model.name = 'quadrotor_model'
# 定义状态变量
v = cs.MX.sym('v', 3) # 速度
p = cs.MX.sym('p', 3) # 位置
q = cs.MX.sym('a', 4) # 四元数
x = cs.vertcat(v, p, q)
# 定义控制输入变量
u1 = cs.MX.sym('T')
u2 = cs.MX.sym('W_x')
u3 = cs.MX.sym('W_y')
u4 = cs.MX.sym('W_z')
u = cs.vertcat(u1, u2, u3, u4)
# 定义状态和控制输入
self.ocp.model.x = x
self.ocp.model.u = u
rhs = quadrotor_dynamics(self.ocp.model.x, self.ocp.model.u)
# 定义状态和控制变量与系统方程之间的关系
f = cs.Function('f', [x, u], [rhs], ['state', 'control_input'], ['rhs'])
# f_expl = ca.vcat(rhs) # 当然也可以这样直接定义
x_dot = cs.MX.sym('x_dot', 10)
# 模型各个部分定义，从CasADi的表达式映射到ACADOS的模型中
self.ocp.model.f_expl_expr = rhs#
self.ocp.model.x = x
self.ocp.model.xdot = x_dot
self.ocp.model.u = u
# 设置预测时域
self.N = 10
self.ocp.solver_options.N_horizon = self.N
# 设置时间步长 tf
# 假设每个时间步长为 0.01 秒，总时间为 N * 0.1
tf = self.N * 0.01
self.ocp.solver_options.tf = tf
# 设置状态误差权重矩阵
Q = np.diag([1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
# 设置控制输入权重矩阵
R = np.diag([1, 1, 1, 1])
# 设置成本函数类型
self.ocp.cost.cost_type = 'LINEAR_LS'
self.ocp.cost.cost_type_e = 'LINEAR_LS'
# 构建成本函数的权重矩阵
self.ocp.cost.W = np.block([[Q, np.zeros((STATE_DIM, INPUT_DIM))],
[np.zeros((INPUT_DIM, STATE_DIM)), R]])
# 终端状态误差权重矩阵
self.ocp.cost.W_e = Q
ny = STATE_DIM + INPUT_DIM
self.ocp.cost.Vx = np.zeros((ny, STATE_DIM))
self.ocp.cost.Vx[:STATE_DIM, :STATE_DIM] = np.eye(STATE_DIM)
self.ocp.cost.Vu = np.zeros((ny, INPUT_DIM))
self.ocp.cost.Vu[-INPUT_DIM:, -INPUT_DIM:] = np.eye(INPUT_DIM)
self.ocp.cost.Vx_e = np.eye(STATE_DIM)
# 设置控制输入约束
self.ocp.constraints.lbu = np.array([5, -2, -2, -2])
self.ocp.constraints.ubu = np.array([15, 2, 2, 2])
self.ocp.constraints.idxbu = np.array([0, 1, 2, 3])
# 定义参考状态和输入向量
x_ref = np.zeros(STATE_DIM)
u_ref = np.zeros(INPUT_DIM)
self.ocp.constraints.x0 = x_ref
self.ocp.cost.yref = np.concatenate((x_ref, u_ref))
### N
self.ocp.cost.yref_e = x_ref
self.ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
self.ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
self.ocp.solver_options.integrator_type = 'ERK'
self.ocp.solver_options.print_level = 0
self.ocp.solver_options.nlp_solver_type = 'SQP_RTI'
# 设置状态约束
# 示例：设置速度的上下限
# lbx = np.array([-5, -5, -5, -np.inf, -np.inf, -np.inf, -1, -1, -1, -1]) # 状态下限
# ubx = np.array([5, 5, 5, np.inf, np.inf, np.inf, 1, 1, 1, 1]) # 状态上限
# self.ocp.constraints.lbx = lbx
# self.ocp.constraints.ubx = ubx
# self.ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]) # 所有状态变量都受约束
# Solver options
# 创建求解器
self.ocp_solver = AcadosOcpSolver(self.ocp, json_file='acados_ocp.json')
def solve_mpc(self, x0, ref_traj):
"""
求解 MPC 问题
:param x0: 初始状态
:param ref_traj: 参考轨迹
:return: 最优控制输入
"""
# 重置求解器状态，确保每次求解独立，当求解出现故障的时候再使用
# self.ocp_solver.reset()
# 设置初始状态约束
self.ocp_solver.set(0, 'lbx', x0)
self.ocp_solver.set(0, 'ubx', x0)
# 设置每个时刻的参考状态和输入
m = 1 # 质量，与 quadrotor_dynamics 中的质量保持一致
g = 9.81 # 重力加速度
for i in range(self.N):
# 构造参考输入向量，推力与重力加速度抵消，三轴角速度为 0
ref_input = np.array([m * g, 0, 0, 0])
yref = np.concatenate([ref_traj[i], ref_input])
self.ocp_solver.set(i, 'yref', yref)
# 设置终端参考状态
self.ocp_solver.set(self.N, 'yref', ref_traj[self.N])
# 求解 OCP 问题
status = self.ocp_solver.solve()
if status != 0:
print(f'acados 求解失败，状态码: {status}')
# 获取最优控制输入
u_opt = self.ocp_solver.get(0, 'u')
return u_opt
4.mpc_controller.py
这个文件是数值分析仿真实验的内容。
MPC求解平均耗时：0.385 ms
5. 运行mpc_controller.py
运行结果如下。
MPC求解平均耗时：0.412 ms
MPC求解平均耗时：0.390 ms
MPC求解平均耗时：0.385 ms
作者：淮北494
