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

前言

无人机（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 ; 

    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

期望位置先固定为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<< "比例输出 : "<

        // 输出限幅
        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;         

    }
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
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

详情参考古月居