在这里插入图片描述

前言

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

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

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

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

其中在前两篇博客中已经实现了 高度和水平位置控制，本篇博客在其基础上继续实现最后的航向控制。

航向控制

无人机航向角度的控制框图如下：
在这里插入图片描述

由于rotors_simulator 选择的接口是 yawrate 所以仅需要做最外层的 外环角度控制器 输出 期望航向角速度即可。

P控制

    // 航向角度控制
    void PidPositionControllerNode::YawControl(double yaw_des_degree)
    {
        // 期望航向角度 单位度
        double yaw_des_degree = 30;
        // 将期望航向角转为 弧度单位
        double yaw_des_rad = yaw_des_degree * RADIAN ;
        // 获取无人机航向角度
        // 获得无人机的姿态旋转矩阵
        Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
        // 根据旋转矩阵求得yaw角 单位rad
        double yaw_cur_rad = atan2(R(1, 0), R(0, 0));         

        // 求得通过角度偏差 求得期望角速度 简单p控制
         double yaw_rate_des_rad = 2 * (yaw_des_rad - yaw_cur_rad);

        // 限制最大旋转角速度 单位弧度
        double max_yaw_rate = 120 * RADIAN;
        Math_doubleConstrain(yaw_rate_des_rad,-max_yaw_rate,max_yaw_rate);

        des_yawrate_ = yaw_rate_des_rad ; 

    }

期望位置先固定为30度

控制效果如下：
在这里插入图片描述

收敛结果

在这里插入图片描述

收敛过程

在这里插入图片描述

经过4次震荡最终收敛
最大超调量为：0.262 弧度 相当于15度
收敛时间：26s

PD控制

由于稳态误差不大，存在较大超调，和收敛时间，选择加入微分环节

   float PidPositionControllerNode::yaw_pid_controller(float pv,float sp)
    {
        // 控制器相关参数  根据被控对象要调整的量
        // 控制参数
        float Kp = 2 , Ki = 0 ,Kd = 0; 
        // 输出限幅
        float max_output_pid = 120 , min_output_pid = -120 ;
        // 积分输出限幅
        float max_output_i = 10,min_output_i = -10 ;

        // 控制器 偏差量
        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* RADIAN)
        {
            // 积分输出限制输出最大值
            output_i = max_output_i* RADIAN;
        }else if(output_i<min_output_i* RADIAN)
        {
            // 积分输出限制输出最小值
            output_i = min_output_i* RADIAN;
        }        

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

        // std::cout<< "比例输出 : "<<output_p<<"|||" <<"微分输出"<<output_d<<std::endl;

        // 输出限幅
        if(output_pid>max_output_pid* RADIAN)
        {
            // 输出限制输出最大值
            output_pid = max_output_pid* RADIAN;
        }else if(output_pid<min_output_pid* RADIAN)
        {
            // 输出限制输出最小值
            output_pid = min_output_pid* RADIAN;
        }

        // 控制器最终输出
        return output_pid;         

    }

控制参数整定的思想如下：
增大比例系数P将加快系统的响应，它的作用于输出值较快，但不能很好稳定在一个理想的数值，不良的结果是虽较能有效的克服扰动的影响，但有余差出现，过大的比例系数会使系统有比较大的超调，并产生振荡，使稳定性变坏。积分能在比例的基础上消除余差，它能对稳定后有累积误差的系统进行误差修整，减小稳态误差。微分具有超前作用，对于具有容量滞后的控制通道，引入微分参与控制，在微分项设置得当的情况下，对于提高系统的动态性能指标，有着显著效果，它可以使系统超调量减小，稳定性增加，动态误差减小。

    // 航向角度控制
    void PidPositionControllerNode::YawControl(double yaw_des_degree)
    {
        // 期望航向角度 单位度
        //double yaw_des_degree = 30;

        // 将期望航向角转为 弧度单位
        double yaw_des_rad = yaw_des_degree * RADIAN ;

        // 先简单通过 比例 做一个航向保持

        // 获取无人机航向角度
        // 获得无人机的姿态旋转矩阵
        Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
        // 根据旋转矩阵求得yaw角 单位rad
        double yaw_cur_rad = atan2(R(1, 0), R(0, 0));         

        // 求得通过角度偏差 求得期望角速度 简单p控制
        // double yaw_rate_des_rad = 2 * (yaw_des_rad - yaw_cur_rad);

        // 通过角度偏差 求得期望角速度 pid控制
        double yaw_rate_des_rad = yaw_pid_controller(yaw_cur_rad,yaw_des_rad);

        // 限制最大旋转角速度 单位弧度
        double max_yaw_rate = 120 * RADIAN;
        Math_doubleConstrain(yaw_rate_des_rad,-max_yaw_rate,max_yaw_rate);

        des_yawrate_ = yaw_rate_des_rad ; 

    }

收敛结果

在这里插入图片描述

收敛过程

在这里插入图片描述
一次震荡即可收敛
超调量： 0.038弧度 即 2.23度
收敛时间为： 10s

结果总结

将两个航向值的变化曲线放在一起，明显串级PID控制器的效果更优
在这里插入图片描述

一次震荡即可收敛
超调量： 0.038弧度 即 2.23度
收敛时间为： 10s

至此无人机的位置控制即完成了。可以通过最终的控制函数

    void PidPositionControllerNode::POSXYZYawControl(double pos_x_des,double pos_y_des,double pos_z_des,double yaw_des_degree)

完成 水平位置、垂直位置、航向角度的 空间位置控制。