在这里插入图片描述

前言

无人机(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);
    }

期望位置固定为1,外环和内环的控制均为比例作用,先打通控制回环。

然后编译看下控制效果.

控制效果如下:
z轴 震荡 几次后 , 最终收敛

收敛结果

在这里插入图片描述
在这里插入图片描述

收敛过程

在这里插入图片描述
震荡约4次,存在一定稳态误差
在这里插入图片描述
最大超调量约为0.2m
到达稳态时间约30s
稳态误差0.002m

串级PID控制

通过对上面收敛过程的分析,超调量比较大,收敛时间长,改善控制效果,适宜加入积分、微分环节。

向其中加入速度环加入pid控制器

    // z轴速度控制器
    float PidPositionControllerNode::z_vel_pid_controller(float pv,float sp){
        float Kp = 10 , Ki = 0.1 ,Kd = 0.3 ; 
        float max_output_pid = 5 , min_output_pid = -5 ;
        float max_output_i = 2,min_output_i = -2 ;
        static float error = 0,error_last=0,error_last_last=0;
        static float output_p,output_i,output_d,output_pid; 

        // 偏差计算
        error = sp - pv ;
        output_p += ( Kp * (error - error_last) );
        output_i += ( Ki * (error) );
        output_d += ( Kd * (error - 2*error_last + error_last_last) );
        // 更新偏差量
        error_last_last = error_last ;
        error_last = error ;

        if(output_i>max_output_i)
        {
            output_i = max_output_i;
        }else if(output_i<min_output_i)
        {
            output_i = min_output_i;
        }  

        // 汇总输出
        output_pid = output_p + output_i + output_d;

        if(output_pid>max_output_pid)
        {
            output_pid = max_output_pid;
        }else if(output_pid<min_output_pid)
        {
            output_pid = min_output_pid;
        }
        // 控制器最终输出
        return output_pid; 
    }

再次编译,查看控制结果

收敛结果

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

收敛过程

在这里插入图片描述
震荡约1次,没有稳态误差

在这里插入图片描述
最大超调为 0.03765,为了达到快速收敛的目的,较小的超调是允许的。
到达稳态时间约 8s
稳态误差 0m

结果总结

将两个高度值的变化曲线放在一起,明显串级PID控制器的效果更优
在这里插入图片描述
最终达到的控制效果:
最大超调为 0.03765
到达稳态时间约 8s
稳态误差 0m