前言

无人机（Unmanned Aerial Vehicle），指的是一种由动力驱动的、无线遥控或自主飞行、机上无人驾驶并可重复使用的飞行器，飞机通过机载的计算机系统自动对飞行的平衡进行有效的控制，并通过预先设定或飞机自动生成的复杂航线进行飞行，并在飞行过程中自动执行相关任务和异常处理。

在前面的博客中，分析了 rotors_simulator 一个开源的无人机gazebo的仿真系统的一个控制接口(roll、pitch、yawrate、thrust)，并通过键盘发布控制指令，使飞机飞了起来，但是真正实验过的人则知道，起控制会飞常难，需要一直调整键盘，稍微一不注意，无人机就飞走了。
其原因就是这个接口在无人机内部并没有位置控制的闭环。

在这篇文章中，分析了自动控制原理；并在这篇文章中分析了无人机各种模式的控制框图。

本篇博客主要就是基于无人机的控制原理与控制框图，基于PID控制器，利用rotors_simulator 的控制接口，实现无人机的位置控制。

构建软件框架

编译 cpp

在cmakelists.txt中加入如下

add_executable(pid_position_controller_node
  src/nodes/pid_position_controller_node.cpp)
add_dependencies(pid_position_controller_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(pid_position_controller_node
  roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})

生成 pid_position_controller_node 可执行文件

构建代码

main函数构建

int main(int argc, char** argv) {
    // 初始化节点
    ros::init(argc, argv, "pid_position_controller_node");
    // 终端输出开始信息
    std::cout<< "pid position controll start" <<std::endl;
    // 实例化 类
    rotors_control::PidPositionControllerNode pid_position_controller_node;
    ros::spin();
    return 0;
}

main函数构建

初始化节点
终端输出开始信息
实例化类

构建类的定义

新建pid_position_controller_node.h文件并声明类

class PidPositionControllerNode{

 public:
    PidPositionControllerNode();
    ~PidPositionControllerNode();

 private:
    // 无人机控制用的里程计
    EigenOdometry odometry_ ;

    // 订阅里程计
    ros::Subscriber odometry_sub_;
    
    // 发布控制指令
    ros::Publisher Control_RollPitchYawrateThrust_pub_;

    // 里程计回调函数
    void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg);

};

先完成一些必要的变量和函数定义，后续需要的内容再向里添加。

之后便可以做功能的设计开发了

订阅无人机里程计信息

       // 订阅里程计 信息
        odometry_sub_ = nh.subscribe("firefly/odometry_sensor1/odometry", 1,
                               &PidPositionControllerNode::OdometryCallback, this);

    // 里程计信息 回调函数
    void PidPositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {

        // 转成eigen 的里程计信息格式
        EigenOdometry odometry;
        eigenOdometryFromMsg(odometry_msg, &odometry);

        double pos_x =  odometry.position.x();
        double pos_y =  odometry.position.y();
        double pos_z =  odometry.position.z();

        std::cout<< "pos x : "<<pos_x <<std::endl;
        std::cout<< "pos y : "<<pos_y <<std::endl;
        std::cout<< "pos z : "<<pos_z <<std::endl;

    }

无人机位置如下：
在这里插入图片描述
大约在 gazebo 坐标系下 (0,0,1)位置

终端打印信息：
在这里插入图片描述

其中 EigenOdometry 里程计信息包括：

struct EigenOdometry {
  EigenOdometry()
      : position(0.0, 0.0, 0.0),
        orientation(Eigen::Quaterniond::Identity()),
        velocity(0.0, 0.0, 0.0),
        angular_velocity(0.0, 0.0, 0.0) {};

  EigenOdometry(const Eigen::Vector3d& _position,
                const Eigen::Quaterniond& _orientation,
                const Eigen::Vector3d& _velocity,
                const Eigen::Vector3d& _angular_velocity) {
    position = _position;
    orientation = _orientation;
    velocity = _velocity;
    angular_velocity = _angular_velocity;
  };

  Eigen::Vector3d position;
  Eigen::Quaterniond orientation;
  Eigen::Vector3d velocity; // Velocity is expressed in the Body frame!
  Eigen::Vector3d angular_velocity;
};

无人机位置 —gazebo 坐标系
无人机姿态四元数— 机体坐标系
无人机速度— 机体坐标系
无人机角速度— 机体坐标系

这部分相当于控制框图中的航姿参考系统，为控制器，提供无人机的实际位姿及各种信息。
在这里插入图片描述

垂直位置控制

无人机的垂直位置控制的控制框图如下：
在这里插入图片描述
其框图的逻辑和原因，以在前文描述，这里不再赘述。

在框图中的遥控器输入期望速度，这可以放到后面再做，首先实现整体的控制回环。

串级P控制

首先实现一个简单的串级P控制
代码如下：

   // 垂直方向位置控制
    void PidPositionControllerNode::PosZControl(){

        // 无人机期望 高度 先固定为1m
        double pos_z_des = 1;

        // 无人机当前高度
        double pos_z_cur = odometry_.position.z();

        // 高度差 期望减当前值
        double pos_z_err = pos_z_des - pos_z_cur;

        // 转为期望速度
        // 垂直位置增益
        float PID_POS_Z_GAIN = 1;

        // 无人机期望速度
        double vel_z_des = pos_z_err*PID_POS_Z_GAIN;

        double vel_z_cur = odometry_.velocity.z();

        // 速度偏差
        double vel_z_err = vel_z_des - vel_z_cur;

        // 悬停时增益
        double loiter_thrust = 1.56779*9.81;

        // 转为电机油门
        // 垂直速度增益
        float PID_VEL_Z_GAIN = 1;
        double thrust_z = vel_z_err*PID_VEL_Z_GAIN+loiter_thrust;

        std::cout<< "thrust_z : "<<thrust_z <<std::endl;
        // 控制量
        mav_msgs::RollPitchYawrateThrust roll_pitch_yawrate_thrust;
        roll_pitch_yawrate_thrust.thrust.z= thrust_z;

        Control_RollPitchYawrateThrust_pub_.publish(roll_pitch_yawrate_thrust);
    }