从零搭建四旋翼无人机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 NoeticUbuntu 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 ~/.bashrc1.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_TYPERelease2.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 -y2.3 编译工作空间cd ~/catkin_ws catkin build source devel/setup.bash3. MPC控制器实现3.1 创建控制器ROS包cd ~/catkin_ws/src catkin_create_pkg nmpc_controller roscpp std_msgs geometry_msgs mav_msgs3.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.launchlaunch arg namemodel defaulthummingbird/ !-- 启动Gazebo仿真 -- include file$(find rotors_gazebo)/launch/mav_hovering_example.launch arg namemav_name value$(arg model)/ arg nameenable_logging valuefalse/ /include !-- 启动MPC控制器 -- node namenmpc_controller pkgnmpc_controller typenmpc_controller_node outputscreen rosparam commandload file$(find nmpc_controller)/config/controller_params.yaml/ /node !-- 启动轨迹生成器 -- node namewaypoint_publisher pkgnmpc_controller typewaypoint_publisher.py outputscreen/ /launch4.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: 104.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} -15. 常见问题排查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 ~/.bashrc5.2 RotorS编译问题错误Could not find a package configuration file provided by mav_msgs解决方案确保mav_comm仓库已正确克隆并编译5.3 MPC控制器运行问题错误控制器输出不稳定或发散可能原因权重矩阵设置不合理预测时域太短动力学模型不准确调试建议先尝试增大位置误差权重逐步增加预测时域长度检查动力学方程实现是否正确错误求解器无法收敛可能原因初始猜测不合理约束条件过于严格解决方案提供更好的初始猜测适当放宽约束限制6. 性能优化建议代码生成优化在ACADO配置中启用REAL_TIME_MODE使用更高效的积分方法如INT_RK78求解器选择尝试不同的QP求解器如qpOASES或HPMPC调整求解器精度参数并行计算利用ACADO的并行计算功能在多核CPU上分配计算任务代码热路径优化使用perf工具分析性能瓶颈对关键循环进行手动优化// 示例使用Eigen进行矩阵运算优化 Eigen::MapEigen::MatrixXd Q_map(acadoVariables.Q, NX, NX); Eigen::MapEigen::MatrixXd R_map(acadoVariables.R, NU, NU); Q_map Q_matrix; // 批量赋值比逐元素赋值更快 R_map R_matrix;在实际项目中我发现最耗时的部分往往是状态估计和传感器数据处理。将这部分与控制器解耦使用单独的线程运行可以显著提高系统响应速度。
