MPC多旋翼控制 - 线性MPC代码解读（mav_control_rw项目）

MPC无人机控制 - mav_control_rw项目 - 线性MPC代码解读

大型工程的代码还是得记录一下，不然每次都得回头看。紧接上篇趁热打铁一下
MPC多旋翼控制 - mav_control_rw项目

一. 代码构成

├── mav_control_rw
│   ├── LICENSE
│   ├── mav_control_interface
│   ├── mav_disturbance_observer
│   ├── mav_linear_mpc
│   ├── mav_lowlevel_attitude_controller
│   ├── mav_nonlinear_mpc
│   └── README.md

git上克隆下来后就是这些文件夹。

1. mav_control_interface
主要用于将mpc生成的控制指令传给飞控，接收mpc生成的输入序列，还有rc方面的一些回调函数，这里先不展开。

2. mav_disturbance_observer
实现扰动观测器，以考虑扰动。

3. mav_linear_mpc/mav_nonlinear_mpc
分别是线性mpc控制器和非线性mpc控制器，是主要的算法模块，本篇主要解读线性mpc控制器代码，非线性结构与线性结构类似，只是无人机模型是非线性的。

4. mav_lowlevel_attitude_controller
由high level control command到low level control command的转换，有点类似于飞控做的工作，不过由于不是论文的工作范围，所以就不看了。

二. mav_linear_mpc模块

src中有三个文件：linear_mpc.cpp，linear_mpc_node.cpp，steady_state_calculation.cpp。ROS启动的节点只有linear_mpc_node。那首先看看linear_mpc_node.cpp。

看一下main函数

int main(int argc, char** argv)
{
  //初始化ros
  ros::init(argc, argv, "LinearModelPredictiveControllerNode");
  //初始化节点
  ros::NodeHandle nh, private_nh("~");
  //构造LinearModelPredictiveControllerNode类
  std::shared_ptr<mav_control::LinearModelPredictiveControllerNode> mpc(
      new mav_control::LinearModelPredictiveControllerNode(nh, private_nh));
  //构造RcInterfaceAci类
  std::shared_ptr<mav_control_interface::RcInterfaceAci> rc(
      new mav_control_interface::RcInterfaceAci(nh));
  //构造MavControlInterface类
  mav_control_interface::MavControlInterface control_interface(nh, private_nh, mpc, rc);
  //等待消息
  ros::spin();
  return 0;
}

可以看到没有什么操作，都是初始化，所以需要去类的构造函数里看。先看看LinearModelPredictiveControllerNode类。

LinearModelPredictiveControllerNode::LinearModelPredictiveControllerNode(
    const ros::NodeHandle& nh, const ros::NodeHandle& private_nh)
    : linear_mpc_(nh, private_nh),
      dyn_config_server_(private_nh)
{
  dynamic_reconfigure::Server<mav_linear_mpc::LinearMPCConfig>::CallbackType f;
  f = boost::bind(&LinearModelPredictiveControllerNode::DynConfigCallback, this, _1, _2);
  dyn_config_server_.setCallback(f);
}

构造函数首先初始化了linear_mpc_和dyn_config_server_。linear_mpc_是一个LinearModelPredictiveController类，此处就跟linear_mpc.cpp连接起来了，dyn_config_server_是一个dynamic_reconfigure::Server类，这个类没怎么看，不过看名称主要是参数设置相关的，现阶段不是太需要关注，函数主体也是定义了一个类f，用于参数动态设置。前面提到了linear_mpc_，这是个LinearModelPredictiveController类，也就是src文件夹中另一个文件写的类。函数主体部分就是动态调参相关。下面直接去看LinearModelPredictiveController类。

LinearModelPredictiveController::LinearModelPredictiveController(const ros::NodeHandle& nh,
                                                                 const ros::NodeHandle& private_nh)
    : nh_(nh),
      private_nh_(private_nh),
      initialized_parameters_(false),
      position_error_integration_(0, 0, 0),
      command_roll_pitch_yaw_thrust_(0, 0, 0, 0),
      linearized_command_roll_pitch_thrust_(0, 0, 0),
      mpc_queue_(nh, private_nh, kPredictionHorizonSteps),
      disturbance_observer_(nh, private_nh),
      verbose_(false),
      solve_time_average_(0),
      steady_state_calculation_(nh, private_nh),
      received_first_odometry_(false)
{
  reset_integrator_service_server_ = nh_.advertiseService(
        "reset_integrator", &LinearModelPredictiveController::resetIntegratorServiceCallback, this);

  initializeParameters();

  mpc_queue_.initializeQueue(sampling_time_, prediction_sampling_time_);
}

这个构造函数首先初始化了一些类成员，函数主体中发布了reset_integrator的服务，用于重置积分误差。下面是一个初始化参数的函数，里面对模型矩阵进行初始化并返回true。
模型矩阵定义：

来源：Kamel M , Stastny T , Alexis K , et al. Model Predictive Control for Trajectory Tracking of Unmanned Aerial Vehicles Using Robot Operating System[J]. Springer International Publishing, 2017.

最后就是对mpc_queue_进行初始化，mpc_queue_就是一个装mpc解算出来的结果的容器，初始化直接将odometry的数据装入。

而LinearModelPredictiveControllerNode中的主函数中还有一个初始化，就是mav_control_interface::RcInterfaceAci。

这个类主要定义了遥控的一些函数，包括按键映射，设置Deadzone等，在构造函数里定义了一个rc_sub_用于订阅rc话题并调用回调函数，上面说的那些操作都在回调函数中完成。

到现在为止都还是一些初始化，定义话题之类的，真正的mpc解算过程还没出现。答案就在最后一句

mav_control_interface::MavControlInterface control_interface(nh, private_nh, mpc, rc);

可以看到这句也是构造了一个类，接下来就看看MavControlInterface这个类。

MavControlInterface::MavControlInterface(ros::NodeHandle& nh, ros::NodeHandle& private_nh,
                                         std::shared_ptr<PositionControllerInterface> controller,
                                         std::shared_ptr<RcInterfaceBase> rc_interface)
{
  mav_control_interface_impl_.reset(new MavControlInterfaceImpl(nh, private_nh, controller, rc_interface));
}

类成员函数只有一个构造一个析构函数，又嵌套着另一个类：mav_control_interface_impl_，到了这个类才开始看到与发布轨迹相关的部分。下面是该类的成员函数：

public:
  MavControlInterfaceImpl(ros::NodeHandle& nh, ros::NodeHandle& private_nh,
                          std::shared_ptr<PositionControllerInterface> controller,
                          std::shared_ptr<RcInterfaceBase> rc_interface);

  virtual ~MavControlInterfaceImpl();

 private:
  static constexpr double kOdometryWatchdogTimeout = 1.0;  // seconds

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  ros::Subscriber odometry_subscriber_;
  ros::Subscriber command_trajectory_subscriber_;
  ros::Subscriber command_trajectory_array_subscriber_;
  ros::Timer odometry_watchdog_;

  ros::ServiceServer takeoff_server_;
  ros::ServiceServer back_to_position_hold_server_;

  std::shared_ptr<RcInterfaceBase> rc_interface_;

  std::unique_ptr<state_machine::StateMachine> state_machine_;

  void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
  void CommandTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& msg);
  void OdometryCallback(const nav_msgs::OdometryConstPtr& msg);
  void OdometryWatchdogCallback(const ros::TimerEvent& e);
  void RcUpdatedCallback(const RcInterfaceBase&);
  bool TakeoffCallback(std_srvs::EmptyRequest& request, std_srvs::EmptyResponse& response);
  bool BackToPositionHoldCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

  void publishAttitudeCommand(const mav_msgs::RollPitchYawrateThrust& command);

此处可以看到主要有3个订阅以及两个服务。由字面意思可以看出command_trajectory_subscriber_是订阅轨迹点，command_trajectory_array_subscriber_是订阅轨迹序列的，而odometry_subscriber_则是订阅Odometry相关数据的。
除此之外，两个服务分别是起飞以及恢复定点模式。其余函数都是定义回调函数。而构造函数中首先创建了订阅以及服务对象，接着是获取并应用相关的参数，比如rc_max_roll_pitch_command_，rc_max_yaw_rate_command_等。其中state_machine_又是另一个类，不得不说真是一环套一环阿。
这篇先到这吧，下一篇争取把剩余的部分总结完成。