当前位置：首页 > news >正文

保姆级教程：在Ubuntu 20.04上用RotorS和ACADO搞定四旋翼MPC控制器（附完整代码）

news 2026/5/24 8:34:25

从零搭建四旋翼无人机MPC控制器：Ubuntu 20.04实战指南

四旋翼无人机的控制算法一直是机器人领域的热门话题。模型预测控制（MPC）因其优秀的处理约束能力和动态响应特性，成为许多研究者和工程师的首选方案。本文将带你从零开始，在Ubuntu 20.04系统上使用RotorS仿真器和ACADO工具包，一步步构建一个完整的四旋翼MPC控制器。

1. 环境准备与工具安装

在开始之前，我们需要确保系统环境配置正确。以下是完整的准备工作清单：

1.1 系统基础配置

首先更新系统并安装必要工具：

sudo apt update && sudo apt upgrade -y sudo apt install -y build-essential cmake git wget

接下来安装ROS Noetic（Ubuntu 20.04对应的官方版本）：

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt update sudo apt install -y ros-noetic-desktop-full echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc source ~/.bashrc

1.2 安装ACADO工具包

ACADO是MPC实现的关键工具，安装步骤如下：

下载ACADO源码：

wget https://github.com/acado/acado/archive/refs/tags/v1.2.2beta.tar.gz -O acado.tar.gz tar -xzf acado.tar.gz cd acado-1.2.2beta

编译安装：

mkdir build cd build cmake .. make -j$(nproc) sudo make install

验证安装：

cd ../examples/getting_started mkdir build cd build cmake .. make ./simple_mpc

如果能看到MPC求解过程输出，说明安装成功。

2. RotorS仿真环境搭建

2.1 创建工作空间

mkdir -p ~/catkin_ws/src cd ~/catkin_ws catkin init catkin config --extend /opt/ros/noetic catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

2.2 安装RotorS及相关依赖

cd ~/catkin_ws/src git clone https://github.com/ethz-asl/rotors_simulator.git git clone https://github.com/ethz-asl/mav_comm.git rosdep install --from-paths rotors_simulator mav_comm --ignore-src -y

2.3 编译工作空间

cd ~/catkin_ws catkin build source devel/setup.bash

3. MPC控制器实现

3.1 创建控制器ROS包

cd ~/catkin_ws/src catkin_create_pkg nmpc_controller roscpp std_msgs geometry_msgs mav_msgs

3.2 MPC问题定义

在quadrotor_nmpc.cpp中定义MPC问题：

#include <acado_toolkit.hpp> int main() { USING_NAMESPACE_ACADO // 定义系统状态变量 DifferentialState p_x, p_y, p_z; // 位置 DifferentialState v_x, v_y, v_z; // 速度 DifferentialState phi, theta, psi; // 欧拉角 DifferentialState w_x, w_y, w_z; // 角速度 // 定义控制输入 Control T, tau_x, tau_y, tau_z; // 推力和力矩 // 定义微分方程 DifferentialEquation f; // 定义系统动力学 f << dot(p_x) == v_x; f << dot(p_y) == v_y; f << dot(p_z) == v_z; // ... 完整动力学方程 // 定义MPC控制器 OCP ocp(0.0, 1.0, 10); // 预测时域1秒，10个采样点 // 定义代价函数 Function h; h << p_x << p_y << p_z // 位置误差 << v_x << v_y << v_z // 速度误差 << phi << theta << psi // 姿态误差 << w_x << w_y << w_z; // 角速度误差 // 权重矩阵 DMatrix Q(12,12); Q.setIdentity(); Q(0,0)=20; Q(1,1)=20; Q(2,2)=50; // 位置权重 // ... 其他权重设置 ocp.minimizeLSQ(Q, h); // 添加约束 ocp.subjectTo( f ); ocp.subjectTo( 0.1 <= T <= 18.0 ); // 推力约束 // ... 其他约束 // 导出代码 OCPexport mpc(ocp); mpc.set(INTEGRATOR_TYPE, INT_RK45); mpc.set(NUM_INTEGRATOR_STEPS, 5); if (mpc.exportCode("nmpc_generated") != SUCCESSFUL_RETURN) exit(EXIT_FAILURE); return 0; }

3.3 控制器封装与ROS集成

创建mpc_wrapper.h封装ACADO生成的代码：

#pragma once #include <acado_common.h> #include <acado_auxiliary_functions.h> class MPCWrapper { public: MPCWrapper(); ~MPCWrapper(); bool init(); bool update(const Eigen::VectorXd& state, const Eigen::VectorXd& ref, Eigen::VectorXd& control); private: ACADOvariables acadoVariables; ACADOworkspace acadoWorkspace; };

4. 系统集成与测试

4.1 创建launch文件

在nmpc_controller/launch目录下创建nonlinear_mpc_sim.launch：

<launch> <arg name="model" default="hummingbird"/> <!-- 启动Gazebo仿真 --> <include file="$(find rotors_gazebo)/launch/mav_hovering_example.launch"> <arg name="mav_name" value="$(arg model)"/> <arg name="enable_logging" value="false"/> </include> <!-- 启动MPC控制器 --> <node name="nmpc_controller" pkg="nmpc_controller" type="nmpc_controller_node" output="screen"> <rosparam command="load" file="$(find nmpc_controller)/config/controller_params.yaml"/> </node> <!-- 启动轨迹生成器 --> <node name="waypoint_publisher" pkg="nmpc_controller" type="waypoint_publisher.py" output="screen"/> </launch>

4.2 控制器参数配置

创建controller_params.yaml配置文件：

# 状态权重 q_position: {x: 20, y: 20, z: 50} q_velocity: {x: 10, y: 10, z: 10} q_attitude: {x: 50, y: 50, z: 50} q_angular_rate: {x: 10, y: 10, z: 10} # 控制输入权重 r_thrust: 0.1 r_tau: {x: 1, y: 1, z: 1} # 约束限制 max_thrust: 18.0 # [N] min_thrust: 0.1 # [N] max_tau: {x: 0.05, y: 0.05, z: 0.05} # [Nm] # 时间参数 controller_frequency: 100 # [Hz] prediction_horizon: 1.0 # [s] num_prediction_steps: 10

4.3 运行与测试

启动仿真环境：

roslaunch nmpc_controller nonlinear_mpc_sim.launch

在另一个终端发送测试指令：

rostopic pub /nmpc_controller/command \ trajectory_msgs/MultiDOFJointTrajectoryPoint \ "transforms: - translation: {x: 1.0, y: 0.0, z: 1.0} rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} velocities: - linear: {x: 0.0, y: 0.0, z: 0.0} angular: {x: 0.0, y: 0.0, z: 0.0} accelerations: - linear: {x: 0.0, y: 0.0, z: 0.0} angular: {x: 0.0, y: 0.0, z: 0.0}" -1

5. 常见问题排查

5.1 ACADO安装问题

错误：acado_toolkit.hpp: No such file or directory
- 解决方案：确保正确设置了ACADO环境变量
```
export ACADO_DIR=/usr/local/share/acado echo "export ACADO_DIR=/usr/local/share/acado" >> ~/.bashrc
```

5.2 RotorS编译问题

错误：Could not find a package configuration file provided by "mav_msgs"
- 解决方案：确保mav_comm仓库已正确克隆并编译

5.3 MPC控制器运行问题

错误：控制器输出不稳定或发散
- 可能原因：
  1. 权重矩阵设置不合理
  2. 预测时域太短
  3. 动力学模型不准确
- 调试建议：
  - 先尝试增大位置误差权重
  - 逐步增加预测时域长度
  - 检查动力学方程实现是否正确
错误：求解器无法收敛
- 可能原因：
  1. 初始猜测不合理
  2. 约束条件过于严格
- 解决方案：
  - 提供更好的初始猜测
  - 适当放宽约束限制

6. 性能优化建议

代码生成优化：
- 在ACADO配置中启用REAL_TIME_MODE
- 使用更高效的积分方法（如INT_RK78）
求解器选择：
- 尝试不同的QP求解器（如qpOASES或HPMPC）
- 调整求解器精度参数
并行计算：
- 利用ACADO的并行计算功能
- 在多核CPU上分配计算任务
代码热路径优化：
- 使用perf工具分析性能瓶颈
- 对关键循环进行手动优化

// 示例：使用Eigen进行矩阵运算优化 Eigen::Map<Eigen::MatrixXd> Q_map(acadoVariables.Q, NX, NX); Eigen::Map<Eigen::MatrixXd> R_map(acadoVariables.R, NU, NU); Q_map = Q_matrix; // 批量赋值比逐元素赋值更快 R_map = R_matrix;

在实际项目中，我发现最耗时的部分往往是状态估计和传感器数据处理。将这部分与控制器解耦，使用单独的线程运行，可以显著提高系统响应速度。

查看全文

http://www.jsqmd.com/news/876153/