sitl_gazebo，px4模型matlab接口分析，gazebo与matlab联合仿真

最近在使用matlab建立四旋翼控制程序，奈何没有好的可视化仿真软件，便想起px4的gazebo仿真环境，使用matlab连接px4底层通信模型，实现matlab在软件层面的可视化仿真。

Gazebo插件分析

参考gazebo插件开发手册，可以了解其开发的流程，之后也只关注gazebo插件入口函数

    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
      // 这里大抵是Gazebo插件的入口函数，插件的修改也是在这个部分修改
      std::cerr << "\nThe velodyne plugin is attach to model[" <<
        _model->GetName() << "]\n";
    }

其他部分暂时也无需去更改，使用px4默认的方式就行。

Gazebo 中Ros的使用

在Gazebo插件中，首先需要订阅Gazebo自身发布的话题，而要实现仿真器内部件与外部的对接，还需要插件Ros话题，在px4中使用Mavlink直接与simulator模块通信，可以直接发送pwm信号至电机电调模型，驱动其转动，而在传感器方面，虽然可以使用mavlink协议读取，但考虑到matlab相关模块局限性，这里选择了使用Ros读取。
Gazebo话题的订阅发布于Ros话题订阅发布类似，也可参考px4相关源码，以下是ros话题的订阅发布方式：
ros节点初始化，也可以省略，一般默认使用已初始化的节点

if (!ros::isInitialized())
{
  int argc = 0;
  char **argv = NULL;
  ros::init(argc, argv, "gazebo_client",
      ros::init_options::NoSigintHandler);
}
// Create our ROS node. This acts in a similar manner to
// the Gazebo node
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

创建ros话题并发布

其次是创建并发布话题

// Create a named topic, publish to it.
  mag_pub = this->rosNode->advertise<geometry_msgs::Vector3>("/" +model_->GetName() + "/mag",20);
  gps_ros_pub = this->rosNode->advertise<geometry_msgs::Twist>("/" +model_->GetName() + "/gps",1);
  bar_ros_pub=this->rosNode->advertise<geometry_msgs::Vector3>("/" +model_->GetName() + "/bar",20);

以上代码发布格式可以根据自身情况确定，这里为了方便如此定义
其次添加发布模块

    mag_.x =   mag_msg->magnetic_field().x();
    mag_.y =   mag_msg->magnetic_field().y();
    mag_.z =   mag_msg->magnetic_field().z();
   mag_pub.publish(mag_);

此处作用为将gazebo话题的消息转播为ros话题消息，方便matlab的读取。
对于四旋翼的控制，其控制主要依靠磁力计，gps，气压计，剩余两种传感器数据发送方式类似，不再叙述。

mavlink控制转播

虽然可以使用mavlink直接控制模型的电机电调，但由于matlab在mavlink封装方面的局限性，这里选择使用python转播信号。
关于电机控制mavlink消息定义如下，命令代号93，按需求填写其内容即可

HIL_ACTUATOR_CONTROLS ( #93 )
[Message] Sent from autopilot to simulation. Hardware in the loop control outputs (replacement for HIL_CONTROLS)
Field Name Type Units Values Description
time_usec uint64_t us Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.
controls float[16] Control outputs -1 … 1. Channel assignment depends on the simulated hardware.
mode uint8_t MAV_MODE_FLAG System mode. Includes arming state.
flags uint64_t Flags as bitfield, 1: indicate simulation using lockstep.

from pymavlink import mavutil
connection=mavutil.mavlink_connection(('tcp:0.0.0.0:4560'))
connection.mav.hil_actuator_controls_send(int(time.time()*1e6),channel_values,255,1)

其中channel_values为16维float32数组，关于ros转播话题发布不再赘述。
完成以上转播后即可开始matlab的配置。

matlab配置

组内搭建的四旋翼控制系统如图所示。
接下来接入ros传感器


pwm信号输出

完成以上配置后，即可开始matlab与gazebo的联合仿真了。

联合仿真

通过model_state还原无人机位置状态

通过matlab运行仿真程序，并分析数据