【MPC】无人机模型预测控制复现Data-Driven MPC for Quadrotors项目（Part 1）

环境：

Ubuntu ：20.04 LTS

Python: 3.8

参考链接

Github仓库地址

论文Data-Driven MPC for Quadrotors

Youtube视频

背景和问题

高速度和高加速度下的精确轨迹跟踪对于四旋翼飞行器仍然是一个挑战。由于空气动力学效应的复杂性，这些效应在高速度时会对飞行器的轨迹跟踪产生显著干扰，并引入较大的位置误差。这些效应难以建模，而现有的方法通常忽略了这些空气动力学效应，导致在执行快速和灵活的飞行任务时精度不足。

方法与贡献

本文提出了一种利用**高斯过程（Gaussian Processes, GP）**对四旋翼飞行器的动力学模型进行数据驱动增强的方法。具体而言，GP用于学习残差动力学，即在简化的四旋翼模型基础上，预测并修正空气动力学引起的误差，然后将这一增强的动力学模型集成到模型预测控制（Model Predictive Control, MPC）中，来实现高效且精确的实时反馈控制。

• 结合了学习到的GP修正项与四旋翼飞行器的标称动力学，形成了一种高效的MPC控制管线。

• 通过仿真和现实世界的实验，验证了这种方法在高速度和高加速度下的轨迹跟踪精度，能够显著优于传统的线性空气动力学模型。

实验结果

通过仿真和实际飞行实验，本文方法在多种轨迹（如随机、圈、双纽线轨迹等）中展示了其优越性。与未增强的MPC相比，本文提出的方法在速度达到14 m/s、加速度超过4g的条件下，能够将轨迹跟踪误差减少高达70 %。实验还表明，该方法在不同的轨迹之间具有良好的泛化能力，并在实验中持续表现优于基于线性阻力模型的MPC。

安装ROS

安装 ROS ，推荐用鱼香 ROS 的一键安装。

鱼香ROS网站上线|一行代码安装ROS/ROS2/解决rosdep问题|小鱼脚本

wget http://fishros.com/install -O fishros && bash fishros

创建工作空间

创建工作空间。

mkdir -p ~/mpc_ws/src
cd ~/mpc_ws/src
catkin_init_workspace  # initialize your catkin workspace
wstool init

下载RotorS仿真器源码和依赖

下载 RotorS 仿真器源码和依赖。

cd ~/mpc_ws/src
git clone https://github.com/catkin/catkin_simple.git
git clone https://github.com/ethz-asl/rotors_simulator.git
git clone https://github.com/ethz-asl/mav_comm.git
git clone https://github.com/ethz-asl/eigen_catkin.git
sudo apt-get install libgoogle-glog-dev
sudo apt-get install liblapacke-dev

编译工作空间。

cd ~/mpc_ws
catkin build

创建Python虚拟环境

Python virtualenv 是一个用于创建和管理虚拟环境的工具。它可以帮助开发者在不同的项目中使用不同的 Python 版本和包，而不会相互干扰。使用 virtualenv，可以轻松地创建一个独立的 Python 环境，在其中安装所需的包和版本，而不会影响系统中已经安装的其他 Python 环境和包。

我这里使用的是 Ubuntu 20.04 、 Python 3.8 的环境配置。

sudo pip3 install virtualenv
cd ~
virtualenv mpc_venv --python=/usr/bin/python3.8
source ~/mpc_venv/bin/activate

下载data_driven_mpc仓库代码

下载 data_driven_mpc 仓库代码。

cd ~/mpc_ws/src
git clone https://github.com/uzh-rpg/data_driven_mpc.git

该代码已在 Ubuntu 18.04 、 Python 3.6 和 ROS Melodic 上进行了测试运行。

切换到分支python3.8_support，可以在 Ubuntu 20.04 、 Python 3.8 和 ROS Noetic 上运行。

切换分支，激活 Python 环境并安装其余必需的 Python 库。

cd data_driven_mpc
git checkout python3.8_support
source ~/mpc_venv/bin/activate
python3 setup.py install

需要安装以下库。

install_requires=['numpy==1.19.0','scipy==1.5.0','tqdm==4.46.1','matplotlib==3.2.2','scikit-learn==0.23.2','casadi==3.5.1','pyquaternion==0.9.5','joblib==0.15.1','pandas==1.0.5','PyYAML==5.3.1','pycryptodomex==3.9.8','gnupg==2.3.1','rospkg==1.2.8','tikzplotlib==0.9.4'],

可以使用以下命令查看 Python 下所有包和包的版本。

pip list -o

多尝试几次，这个可能受网络环境影响，直到显示Finished processing dependencies ...为止。

再次编译工作空间。

cd ~/mpc_ws
catkin build

下载并配置ACADO求解器

参考链接

首先，安装必要的软件包。

sudo apt-get install gcc g++ cmake git gnuplot doxygen graphviz

下载 ACADO 求解器源码。

git clone https://github.com/acado/acado.git
cd acados
git submodule update --recursive --init

在 acados 目录下建立 build 文件夹，用于存放编译文件。

cd ~/acados
mkdir -p build
cd build
cmake -DACADOS_WITH_QPOASES=ON ..

进行编译。

make install -j4

执行一个例程以检查配置是否成功。

cd ..
cd examples/getting_started
./simple_ocp

运行结果如下。

如果运行结果如下图所示，出现显示异常。

打开 Ubuntu 系统配置，将缩放这一栏调回100%即可。

如果你想在debug模式下编译 acado ，可以运行如下命令：

cmake -DCMAKE_BUILD_TYPE=Debug ..

下载并配置ACADO求解器的Python接口

参考链接

在 acados 目录下下载接口文件。

cd ~/acados
pip install -e ~/acados/interfaces/acados_template

在 ~/.bashrc 文件中添加路径，这里需要写绝对路径， hccwb 是我的计算机名。

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:"/home/hccwb/acados/lib"
export ACADOS_SOURCE_DIR="/home/hccwb/acados"

在激活的虚拟环境中，测试 Python 案例。

source ~/mpc_venv/bin/activate
cd ~/acados/examples/acados_python/getting_started/
python minimal_example_ocp.py

如果运行成功，结果如下。

如果提示安装 Tera 模板渲染器，这里需要手动下载一下。

t_renderer-v0.0.34-linux下载地址

将下载的文件移动到 acados 的 bin 目录，重命名为 t_renderer，并赋予可执行权限。

mv ~/Downloads/t_renderer-v0.0.34-linux ~/acados/bin/t_renderer
chmod +x ~/acados/bin/t_renderer

下载并配置rpg_quadrotor_control源码

安装 ROS 依赖。

sudo apt-get install libgoogle-glog-dev protobuf-compiler ros-$ROS_DISTRO-octomap-msgs ros-$ROS_DISTRO-octomap-ros ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-rqt*

安装 vcstool 包。

sudo apt-get install python3-vcstool 

如果提示找不到包，可以使用以下方法。

方法一：已经安装了 ROS 。

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 install curl # if you haven't already installed curl
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo apt-get update
sudo apt-get install python3-vcstool

方法二：没有安装 ROS 。

curl -s https://packagecloud.io/install/repositories/dirk-thomas/vcstool/script.deb.sh | sudo bash
sudo apt-get update
sudo apt-get install python3-vcstool

下载 rpg_quadrotor_control 源码

cd ~/mpc_ws/src
git clone https://github.com/uzh-rpg/rpg_quadrotor_control.git

下载相关依赖。

vcs-import < rpg_quadrotor_control/dependencies.yaml

如果下载失败，可以使用 HTTP 来克隆 GitHub 上的仓库。修改rpg_quadrotor_control/dependencies.yaml文件，将其中的所有 SSH 链接替换为 HTTP 链接。

repositories:catkin_simple:type: giturl: https://github.com/catkin/catkin_simple.gitversion: mastereigen_catkin:type: giturl: https://github.com/ethz-asl/eigen_catkin.gitversion: mastermav_comm:type: giturl: https://github.com/ethz-asl/mav_comm.gitversion: masterrotors_simulator:type: giturl: https://github.com/ethz-asl/rotors_simulator.gitversion: masterrpg_quadrotor_common:type: giturl: https://github.com/uzh-rpg/rpg_quadrotor_common.gitversion: masterrpg_single_board_io:type: giturl: https://github.com/uzh-rpg/rpg_single_board_io.gitversion: master

确保 rqt 缓存中的 GUI 已更新，如果报错找不到文件，忽略这个错误。

rm ~/.config/ros.org/rqt_gui.ini

编译工作空间。

cd ~/mpc_ws
catkin build

将工作空间地址添加到~/.bashrc文件。

echo "source ~/mpc_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

测试rpg_quadrotor_control源码

参考链接

测试运行集成开发环境。

cd ~/mpc_ws
catkin run_tests --no-deps rpg_quadrotor_integration_test

如果集成测试开发环境运行正常，应该看到这样的 Gazebo 模拟环境。

启动基础仿真环境使用以下命令。

source ~/mpc_ws/devel/setup.bash
roslaunch rpg_rotors_interface quadrotor_empty_world.launch

如果运行正常会看到如下的界面。

要使四旋翼飞行器飞行，首先点击Connect按钮，然后按下Arm Bridge按钮向无人机发送命令，然后按下Start按钮启动。

测试data_driven_mpc仓库代码

官方提供了如何在两个不同的模拟器中使用此软件包的说明。Simplified Simulation 和 Gazebo Simulation 。Simplified Simulation 是一个轻量级的 Python 模拟器，Gazebo Simulation 是建立在 RotorS 上的。

首先，确保将 data_driven_mpc 包的主目录添加到 Python 路径中。

export PYTHONPATH=$PYTHONPATH:~/mpc_ws/src/data_driven_mpc/ros_gp_mpc

在Simplified Simulation中测试

测试飞行

要验证软件包的正确安装，请首先在简化模拟上执行试飞。

source ~/mpc_ws/devel/setup.bash
source ~/mpc_venv/bin/activate
roscd ros_gp_mpc
python src/experiments/trajectory_test.py

模拟完成后，正确的安装应产生与以下结果非常相似的结果（平均优化时间可能会有所不同）。

:::::::::::::: SIMULATION SETUP ::::::::::::::Simulation: Applied disturbances: 
{"noisy": true, "drag": true, "payload": false, "motor_noise": true}Model: No regression model loadedReference: Executed trajectory `loop` with a peak axial velocity of 8 m/s, and a maximum speed of 8.273 m/s::::::::::::: SIMULATION RESULTS :::::::::::::Mean optimization time: 1.488 ms
Tracking RMSE: 0.2410 m

您可以在config/configuration_parameters.py文件中编辑 Simplified Simulation 的配置变量以获得更好的可视化效果。在SimpleSimConfig类中：

# Set to True to show a real-time Matplotlib animation of the experiments for the Simplified Simulator. Execution 
# will be slower if the GUI is turned on. Note: setting to True may require some further library installation work.
custom_sim_gui = True# Set to True to display a plot describing the trajectory tracking results after the execution.
result_plots = True# Set to True to show the trajectory that will be executed before the execution timepre_run_debug_plots = True# Choice of disturbances modeled in our Simplified Simulator. For more details about the parameters used refer to# the script: src/quad_mpc/quad_3d.py.simulation_disturbances = {"noisy": True,                       # Thrust and torque gaussian noises"drag": True,                        # 2nd order polynomial aerodynamic drag effect"payload": False,                    # Payload force in the Z axis"motor_noise": True                  # Asymmetric voltage noise in the motors}

下面是运行结果。

您还可以改变参考轨迹的峰值速度和加速度，或者使用双纽线轨迹而不是圆轨迹。所有这些选项都可以在脚本参数中指定。键入以下内容可以显示更多信息：

python src/experiments/trajectory_test.py --help

参考资料：

Github仓库地址

论文Data-Driven MPC for Quadrotors

安装 python3-vcstool