多旋翼无人机仿真 rotors_simulator：基于PID控制器的位置控制---水平位置控制

前言

无人机（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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

期望位置固定为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; 
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

详情请参考古月居