前言

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

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

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

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

其中在前一篇博客中已经实现了高度控制，本篇博客在其基础上继续实现水平位置控制。

水平位置控制

无人机的水平位置控制的控制框图如下：
在这里插入图片描述
经过前面的分析，我们需要利用的 rotors_simulator 的控制接口有 roll pitch 。

即姿态环的控制不需我们自己实现，只实现水平位置控制器和水平速度控制器即可。

与高度控制类似，我们先实现串级P控制

串级P控制

    void PidPositionControllerNode::PosXYControl()
    {
        float PID_POS_X_GAIN = 1;
        float PID_POS_Y_GAIN = 1;
        float PID_VEL_X_GAIN = 0.1;
        float PID_VEL_Y_GAIN = 0.1;   
        double pos_x_des = 1;
        double pos_y_des = 1;
        double pos_x_cur = odometry_.position.x();        
        double pos_y_cur = odometry_.position.y();
        double vel_x_des = (pos_x_des-pos_x_cur)*PID_POS_X_GAIN;
        double vel_y_des = (pos_y_des-pos_y_cur)*PID_POS_Y_GAIN;
        Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
        double yaw = atan2(R(1, 0), R(0, 0)); 
        double b_vel_x_des = vel_x_des*cos(yaw) + vel_y_des*sin(yaw);
        double b_vel_y_des = -vel_x_des*sin(yaw) + vel_y_des*cos(yaw);
        double b_vel_x_cur = odometry_.velocity.x();
        double b_vel_y_cur = odometry_.velocity.y();
        double b_acc_x_des = (b_vel_x_des-b_vel_x_cur)*PID_VEL_X_GAIN;
        double b_acc_y_des = (b_vel_y_des-b_vel_y_cur)*PID_VEL_Y_GAIN;
        des_pitch_ = b_acc_x_des*RADIAN;
        des_roll_ = - b_acc_y_des*RADIAN; //gazebo里是反的

        std::cout<< "无人机当前位置x : "<<std::setprecision(4)<<pos_x_cur<<std::endl;
        std::cout<< "无人机当前位置y : "<<std::setprecision(4)<<pos_y_cur<<std::endl; 
    }

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

控制效果如下：（以x轴为例，y轴对称关系）
x轴缓慢贴近期望值，极小值震荡，最终收敛

收敛结果

在这里插入图片描述

时间足够长，可无稳态误差

收敛过程

在这里插入图片描述
超调量不大

约0.03
但是收敛时间过长
约40s

串级PID控制

通过对上面收敛过程的分析，收敛时间长，改善控制效果，适宜加入积分、微分环节。

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

    float PidPositionControllerNode::x_vel_pid_controller(float pv,float sp)
    {
        float Kp = 6 , Ki = 0.01 ,Kd = 3; 
        float max_output_pid = 30 , min_output_pid = -30 ;
        float max_output_i = 5,min_output_i = -5 ;
        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; 
    }

任何闭环控制系统的首要任务是要稳（稳定）、快（快速）、准（准确）的响应命令。

想要达到良好的控制效果，需要对代码中的三个参数进行调整。其中三个参数分别是：

比例环节
成比例地反映控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用，以减小偏差。当仅有比例控制时系统输出存在稳态误差
积分环节
控制器的输出与输入误差信号的积分成正比关系。主要用于消除静差，提高系统的无差度。
微分环节
反映偏差信号的变化趋势，并能在偏差信号变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减少调节时间。在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。

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

void PidPositionControllerNode::PosXYControl(double pos_x_des,double pos_y_des)
    {
        // 位置控制器增益
         float PID_POS_X_GAIN = 0.5;
         float PID_POS_Y_GAIN = 0.5;
        double pos_x_cur = odometry_.position.x();        
        double pos_y_cur = odometry_.position.y();
        double vel_x_des = (pos_x_des-pos_x_cur)*PID_POS_X_GAIN;
        double vel_y_des = (pos_y_des-pos_y_cur)*PID_POS_Y_GAIN;
        vel_x_des = Math_doubleConstrain(vel_x_des,-5,5);
        vel_y_des = Math_doubleConstrain(vel_y_des,-5,5);

        // 获取无人机航向角度
        Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
        double yaw = atan2(R(1, 0), R(0, 0)); 
        double b_vel_x_des = vel_x_des*cos(yaw) + vel_y_des*sin(yaw);
        double b_vel_y_des = -vel_x_des*sin(yaw) + vel_y_des*cos(yaw);
        double b_vel_x_cur = odometry_.velocity.x();
        double b_vel_y_cur = odometry_.velocity.y();
        double b_acc_x_des = x_vel_pid_controller(b_vel_x_cur,b_vel_x_des);
        double b_acc_y_des = y_vel_pid_controller(b_vel_y_cur,b_vel_y_des);

        des_pitch_ = b_acc_x_des*RADIAN;
        des_roll_ = - b_acc_y_des*RADIAN; //gazebo里是反的

        std::cout<< "无人机当前位置x : "<<std::setprecision(4)<<pos_x_cur<<std::endl;
        std::cout<< "无人机当前位置y : "<<std::setprecision(4)<<pos_y_cur<<std::endl; 
    }