【RotorS仿真】四旋翼无人机MPC控制器ROS实现与Gazebo仿真

1 前言

模型预测控制（MPC, Model Predictive Control）不是一个新的话题，但是将其与视觉（感知约束）和规划等结合，能达到意想不到的效果。
近几年比较简单的论文有：

• 2018年IROS， 瑞士UZH RPG Davide组 PAMPC: Perception-Aware Model Predictive Control for Quadrotors
• 2020年ICRA ，荷兰 Antonio Franchi组 Perception-constrained and Motor-level Nonlinear MPC for both Underactuated and Tilted-propeller UAVS
• 2016年IFAC，瑞士 ETHz ASL组 Linear vs Nonlinear MPC for Trajectory Tracking Applied to Rotary Wing Micro Aerial Vehicles
  Author links open overlay panel

因为笔者近期在学习MPC的设计及工程实现，因此做了一个项目，旨在以练促学。
参考的开源项目有：
uzh-rpg/rpg_mpc
ethz-asl/mav_control_rw

本文将使用基于Gazebo的四旋翼无人机仿真器RotorS，剔除上述项目中次要以及冗余的部分，实现最基本的模型预测控制器，代码使用C++编写。

代码仓库：nmpc_controller

2 理论

2.1 MPC原理简介

2.2 Problem formulation

设无人机机体状态为 $x_b$，
$${\mathbf{x}}_{\mathbf{b}}=[{\mathbf{p}}^{\mathsf{T}}{\mathbf{v}}^{\mathsf{T}}{\eta }^{\mathsf{T}}{\omega }^{\mathsf{T}}]\in {\mathbb{R}}^{12},$$
$p$ 是机体位置在世界坐标系的值，$v$ 是机体速度在世界坐标系的值，$\eta =[\varphi \theta \psi {]}^{\mathsf{T}}$ 是 $Z-Y-X$ 欧拉角用于表示机体姿态，$\omega$ 是机体相对世界的角速度在机体坐标系下的表达。

系统输入 $\mathbf{u}=[T{\tau }_x{\tau }_y{\tau }_z{]}^{\mathsf{T}}$, 机体状态 $\mathbf{x}={\mathbf{x}}_{\mathbf{b}}$。那么这个MPC问题可以写为：
$$\begin{array}{c}\begin{array}{rl}\underset{{\mathbf{u}}_0\dots {\mathbf{x}}_N}{\min }& \sum _{k=0}^N\Vert {\mathbf{x}}_k-{\mathbf{x}}_{r,k}{\Vert }_{\mathbf{Q}}^2+\sum _{k=0}^{N-1}\Vert {\mathbf{u}}_k-{\mathbf{u}}_{r,k}{\Vert }_{\mathbf{R}}^2\\ \text{s.t.}& {\mathbf{x}}_0=\mathbf{x}(t)\\ & {\mathbf{x}}_{k+1}=\mathbf{f}({\mathbf{x}}_k,{\mathbf{u}}_k),k=0,1,\dots ,N-1\\ & \underline{\mathbf{u}}\le {\mathbf{u}}_k\le \overline{\mathbf{u}},k=0,1,\dots ,N-1\end{array}\end{array}$$
上面的约束条件中，第一个约束条件为初值约束，第二个约束条件为无人机动力学约束，第三个约束为系统输入约束（拉力和力矩有最大值和最小值）。

3 实现

3.1 仿真环境介绍

笔记本硬件配置为Intel Core i9-12900H 2.50 GHz处理器，16GB 4800 MHz DDR5内存。软件环境为Ubuntu 20.04 及 Robot Operation System(ROS)。所设计的控制器会在Gazebo中测试与验证，同时使用RotorS提供的工具。MPC求解器为ACADO，这是由ETHz针对最优化问题开发的开源的软件框架。代码将以ROS packages的形式给出，用C++编写。

3.2 代码结构

代码功能可以分为以下模块：

First, in the model folder, define the MPC model in a C++ source file, e.g., quadrotor_nmpc.cpp, and then follow the instructions in README.m file to generate the C++ code for the computation of the MPC problem. The external solver qpOASES should also be included as in the externals folder. Then, the mpc_wrapper.cpp is used to encapsulate the interface code generated by ACADO. In nmpc_controller.cpp, the current state and the reference state is set for the controller to run, and results of each control loop is updated and stored here. The nmpc_controller_node is mainly responsible for communication purpose, it will interact with the trajectory generation module and the Gazebo simulator. It will set the controller parameters like the weights and the boundaries from ROS parameter server. It subscribes to odometry for estimated value and trajectory command. It publishes the controller output as the motor speeds to Gazebo simulator. It runs the controller at a given rate using a timer.

控制器相关参数如下：

# weights for states
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}

# weights for inputs
r_thrust: 0.1
r_tau: {x: 1, y: 1, z: 1}

# limits
max_thrust: 18 # [N]
min_thrust: 0.1 # [N]
max_tau: {x: 0.05, y: 0.05, z: 0.05} # [Nm]

# time
controller_frequency: 100 # [Hz]
queue_dt: 0.01 # [s]
prediction_sampling_time: 1 # [s] 
number_of_samples: 10 # N in the equation

3.3 Usage

3.3.1 Acado安装

请参考笔者往期的博客：
Configure Acado on Linux
Generate MPC C++ code using Acado 这个非常重要，看明白了可以举一反三！

3.3.2 终端命令

创建工作空间

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

下载 rotors_simulator packages

cd ~/catkin_ws/src
git clone git@github.com:ethz-asl/rotors_simulator.git
git clone git@github.com:ethz-asl/mav_comm.git

下载 nmpc_controller

cd ~/catkin_ws/src

编译

cd ~/catkin_ws
catkin build

开启一个终端，运行如下launch命令以启动Gazebo和控制器

source ./devel/setup.bash
roslaunch nmpc_controller nonlinear_mpc_sim.launch

再开启一个终端，运行如下命令以发布轨迹点，执行悬停动作，悬停在 1m 处。

source ./devel/setup.bash
rosrun nmpc_controller waypoint_publisher_5rd_polynomial \
__ns:=/ardrone 0 0 0 0 1 0.08 0 0 10 # to hover

Note:

1. arguments: <x_d> <x_sen> <y_d> <y_sen> <z_d> <z_sen> <yaw_d> <yaw_sen> （d表示desire，期望值；sen表示sense，实际值）
2. __ns:=/ardrone 是为了设置节点的命名空间，不要忽略该参数

另外开启一个终端，以观察轨迹随时间的变化，这里笔者使用了一个可视化面板工具，foxglove，详情可见其官网 website

source ./devel/setup.bash
rosrun foxglove_bridge foxglove_bridge

3.4 效果

这里的轨迹是通过给定初始位置和期望位置，在其间生成五阶多项式曲线。
效果见视频：

NMPC Controller Gazebo Demo