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)
{//初始化rosros::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;  // secondsros::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_又是另一个类，不得不说真是一环套一环阿。
这篇先到这吧，下一篇争取把剩余的部分总结完成。

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

MPC多旋翼控制 - mav_control_rw项目
MPC多旋翼控制 - mav_control_rw项目目录 MPC多旋翼控制 - mav_control_rw项目 mav_control_rw项目介绍项目安装成果 mav_control_rw ...
CMeKG代码解读(以项目为导向从零开始学习知识图谱)（一）
作者从零开始学习和知识图谱有关技术和内容,而本文的核心内容是对CMeKG的python代码进行学习和解读,供大家讨论参考共同进步. CMeKG(Chinese Medical Knowledge Gr ...
【Carsim Simulink自动驾驶仿真】基于MPC的动力学控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论项目介绍: 教 ...
Simulink/Carsim联合仿真，基于pid，mpc自适应巡航控制，定速巡航，紧急避撞等
Simulink/Carsim联合仿真,基于pid,mpc自适应巡航控制,定速巡航,紧急避撞等. 包括以下: (1)Carsim参数设置cpar文件 (2)matlab代码s函数 (3)Simulin ...
m厌氧间歇发酵动态控制系统matlab仿真,对比PID控制和MPC控制
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述生物发酵过程,其中稀释速率和供料基质浓度可作为操作变量.出口的细胞浓度或生物浓度.基质浓度以及产品浓 ...
［代码解读］独立按键控制LED位移c51单片机
先看效果代码解读: 关于独立按键这里注意: (这两个独立按键的针脚刚好是反的) 最后附上代码: #include <REGX52.H> void Delay(unsigned int x ...
先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part1）
关于INDI的前言: 增量型非线性控制方法是一种2010年以后才流行起来的非线性控制方法,目前主要用于设计先进飞行控制律.这种方法的典型代表是增量型动态逆(incremental nonlinear ...
装逼一步到位！GauGAN代码解读来了
↑↑↑关注后"星标"Datawhale 每日干货 & 每月组队学习,不错过 Datawhale干货作者:游璐颖,福州大学,Datawhale成员 AI神笔马良如何装逼一 ...
Resnet的pytorch官方实现代码解读
Resnet的pytorch官方实现代码解读目录 Resnet的pytorch官方实现代码解读前言概述 34层网络结构的"平原"网络与"残差"网络的结构图 ...

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

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

目录

一. 代码构成

二. mav_linear_mpc模块

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

最新文章

热门文章