硬核操作，用四種語言對無人機KAKA進行PID仿真（C，C++，Matlab，Python）(源碼和結果）

前言：本文只提供源代碼和仿真結果，瞭解原理和推導過程可參考《某科學的PID算法學習筆記》

一、問題

在《機甲大師》動漫中，主角“單單”擁有一架語音遙控的雙旋翼無人機，名叫“KAKA"。如圖1，動漫第一集5:35左右，KAKA在追蹤飛盤時，突然受海風影響，飛行姿態偏離水平位置。性能超高的KAKA通過內部傳感器測得偏角後，迅速調整姿態，恢復水平。請對這一情形進行建模分析。

二、解答

略，答案較長，請參考《某科學的PID算法學習筆記》

三、源碼和結果

1) Matlab

編輯器：在線版matlab

clear,clc,close all % 清屏
 
syms x
SV = 0; % 設定值，角（弧）度 0 (rad)
T = 0.01; % 計算週期/採樣週期
Kp = 50; % 比例係數
Ti = 5;  % 積分時間常數
Td = 0.05; % 微分時間常數
 
E = []; % 歷史偏差
Fout = []; % 輸出值，升力
E(1) = pi ./3; % 初始角度 π/3 (rad)
 
for i = 1:1:3 % 繪製3種比較曲線
    if i == 2;
        Kp = 50;  % 比例係數
        Ti = 0.05;   % 積分時間常數
        Td = 0.05; % 微分時間常數
        E = []; % 歷史偏差
        Fout = []; % 輸出值，升力
        E(1) = pi ./3; % 初始角度 π/3 (rad)
    end % if i ==2
     
    if i ==3;
        Kp = 50;  % 比例係數
        Ti = 5;   % 積分時間常數
        Td = 0.005; % 微分時間常數
        E = []; % 歷史偏差
        Fout = []; % 輸出值，升力
        E(1) = pi ./3; % 初始角度 π/3 (rad)
    end % if i ==3
     
    for t = 0:0.01:3; % 計算300次
        k = round(t*100 + 2); % 當前指數
         
        E(k) = E(1) - 25*(T^2)*sum(Fout); % 獲取當前值
         
        %#### 核心，PID計算輸出值 ####%
        if k>2;
            if E(k) != 0;
                Fout(k) = Kp*(E(k) + (T./Ti)*sum(E) + (Td./T)*(E(k)-E(k-1)));
            end % end if E(k) !=0
        end % end if k>2
        %#############################%
         
        k++; % 當前指數+1
         
    end % end for 計算400次
 
    hold on
    plot(E) % 顯示數據圖
end # for 繪製3種比較曲線
 
legend('PID(50, 5, 0.05)','PID(50,  0.05, 0.05)','PID(20,  5,   0.005)')
 
hold on
plot([0,300],[0,0],'--'); % 顯示參考線，斜率0，截距0

2) C

編輯器：Visual Studio 2019 社區版

/**@file     main.c
* @brief     位置式PID  C語言算法仿真
* @author    BROSY
* @copyright CSU | BROSY
********************************************************************************/


/*************************************************************************************
注：以便查閱，我將所有函數和聲明都放在main.c中，進行項目實踐時，再設計文件架構
*************************************************************************************/


#include<stdio.h>
#define PI (3.1416)

typedef struct {
    const int SV = 0; // 設定值（弧度rad)

    double InitVal;        //初始偏差值
    double T;            // 採樣週期
    double Kp;        // 比例係數
    double Ti;        // 積分時間常數
    double Td;        // 微分時間常數
    double Ek;        //當前偏差
    double SumEk;    //歷史偏差之和
    double Ek_1;        //上次偏差
    double SumFout;    // 輸出值之和
}PID_Structure;

/**
@brief    位置式PID輸出函數
@param    [in] PID結構體
@return    算法輸出值（額外升力）
*/
double PID_OUT(PID_Structure* PID)
{
    double Fout;
    Fout = PID->Kp * (PID->Ek
        + (PID->T / PID->Ti) * PID->SumEk
        + (PID->Td / PID->T) * (PID->Ek - PID->Ek_1));

    return Fout;  // 輸出值（額外升力）
}


/**
@brief    獲取當前偏差值
@param    [in] PID結構體, 歷史輸出值（數組）
@return    kaka當前狀態偏差值
*/
double GetCurrE(PID_Structure PID)
{
    double Ek;
    Ek = PID.InitVal - 25 * (PID.T * PID.T) * PID.SumFout;
    return Ek;
}

int main()
{
    PID_Structure PID; // 創建PID

    PID.InitVal = PI / 3;
    PID.T = 0.01;
    PID.Kp = 50;
    PID.Ti = 5;
    PID.Td = 0.005;
    PID.Ek = 0;
    PID.Ek_1 = 0;
    PID.SumFout = 0;
    PID.SumEk = 0;

    // 計算400次
    for (int i = 0; i < 400; i++)
    {
        if (i > 0)
        {
            PID.Ek_1 = PID.Ek; // 獲取k-1的偏差值
        }
        PID.Ek = GetCurrE(PID); // 獲取當前偏差值
        PID.SumEk += PID.Ek; // 歷史偏差之和

        printf("%f\n", PID.Ek);
        if (PID.Ek != 0 && i > 0) // 誤差
        {
            PID.SumFout += PID_OUT(&PID); // 獲取輸出值之和

        }
        else
        {
            PID.SumFout += 0; // 儲存輸出值
        }
    }

}

將輸出結果導入到excel中並繪製曲線：

3) C++

注意C++分爲三個文件main.cpp, PID.cpp, PID.h，編輯器爲Visual Studio 2019 社區版。
main.cpp

/**@file     main.cpp
* @brief     位置式PID  C語言算法仿真
* @author    BROSY
* @copyright CSU | BROSY
********************************************************************************/

#include "PID.h"

int main()
{
    PID* pid[3]; // 創建PID


    pid[0] = new PID(50, 5, 0.05); // 初始化PID1
    pid[1] = new PID(50, 0.05, 0.05); // 初始化PID2
    pid[2] = new PID(50, 5, 0.005); // 初始化PID3

    for (int i = 0; i < 3; i++)
    {
        pid[i]->Loop(400);// 計算400次
        delete pid[i]; // 釋放內存
    }
}

PID.cpp

#include "PID.h"
#include <iostream>
/**
@brief    初始化PID參數
@param    [in] P I D係數
只設置P I D的係數，其餘默認
*/
PID::PID(double P, double I, double D)
{
    Kp = P;
    Ti = I;
    Td = D;

    InitVal = (3.1426)/3; // 初始偏差值π/3
    T  = 0.01;            // 採樣週期
    Ek = 0;            //當前偏差
    SumEk = 0;        //歷史偏差之和
    Ek_1 = 0;            //上次偏差
    SumFout = 0;        // 輸出值之和
}

/**
@brief    位置式PID輸出函數
@return    算法輸出值（額外升力）
*/
double PID::PID_OUT()
{
    double Fout;

    Fout = Kp * (Ek
        + (T / Ti) * SumEk
        + (Td / T) * (Ek - Ek_1));

    return Fout;  // 輸出值（額外升力）
}


/**
@brief    獲取當前偏差值
@return    kaka當前狀態偏差值
*/
double PID::GetCurrE()
{
    double Ek;
    Ek = InitVal - 25 * (T * T) * SumFout;
    return Ek;
}

/**
@brief    循環計算並輸出值
@param    [in] 計算次數
*/
void PID::Loop(int times)
{
    std::cout << "計算次數：" << times << std::endl;
    std::cout << "P  = " << Kp << std::endl;
    std::cout << "I  = " << Ti << std::endl;
    std::cout << "D  = " << Td << std::endl<<std::endl;

    for (int i = 0; i < times; i++)
    {
        if (i > 0)
        {
            Ek_1 = Ek; // 獲取k-1的偏差值
        }
        Ek = GetCurrE(); // 獲取當前偏差值
        SumEk += Ek; // 歷史偏差之和

        std::cout << Ek << std::endl;
        if (Ek != 0 && i > 0) // 誤差
        {
            SumFout += PID_OUT(); // 獲取輸出值之和

        }
        else
        {
            SumFout += 0; // 儲存輸出值
        }
    }
}

PID.h

#pragma once
class PID
{
private:
    const int SV = 0; // 設定值（弧度rad)

    double InitVal;        //初始偏差值
    double T;            // 採樣週期
    double Kp;        // 比例係數
    double Ti;        // 積分時間常數
    double Td;        // 微分時間常數
    double Ek;        //當前偏差
    double SumEk;    //歷史偏差之和
    double Ek_1;        //上次偏差
    double SumFout;    // 輸出值之和

public:
    PID(double P, double I, double D); // PID初始化，只輸入PID係數，其餘默認

    double PID_OUT(); // PID算法核心，計算輸出值
    double GetCurrE(); // 獲取當前值

    void Loop(int times); // 循環計算輸入計算次數
};

4) Python

版本：Python3.6，編輯器：PyCharm 2019.3.1
注意：需要安裝numpy和matplotlib庫
可輸入下面指令進行安裝

pip install numpy
pip install matplotlib

import matplotlib.pyplot as plt  # 導入繪圖庫
import numpy as np

'''
@brief    位置式PID輸出函數
@param    [in] PID結構體
@return    算法輸出值（額外升力）
'''


def pid_out():
    f_out = Kp * (Ek
                  + (T / Ti) * sum_Ek
                  + (Td / T) * (Ek - Ek_1))
    return f_out


'''
@brief    獲取當前偏差值
@param    [in] PID結構體, 歷史輸出值（數組）
@return    kaka當前狀態偏差值
'''


def get_curr_e():
    ek = init_val - 25 * (T ** 2) * sum_f_out
    return ek


sv = 0.0  # 設定值
init_val = (3.1416) / 3  # 初始值
T = 0.01  # 採樣週期
times = 300  # 計算次數
e = np.zeros(times)
for t in range(3):
    Ek = 0.0  # 當前偏差
    sum_Ek = 0.0  # 歷史偏差之和
    Ek_1 = 0.0  # 上一次偏差
    sum_f_out = 0.0  # 輸出值之和（升力）

    if t == 0:
        Kp = 50  # 比例係數
        Ti = 5  # 積分時間常數
        Td = 0.05  # 微分時間常數
    if t == 1:
        Kp = 50  # 比例係數
        Ti = 0.05  # 積分時間常數
        Td = 0.05  # 微分時間常數
    if t == 2:
        Kp = 50  # 比例係數
        Ti = 5  # 積分時間常數
        Td = 0.005  # 微分時間常數

    '''
    @brief    循環計算並輸出值
    @param    [in] 計算次數
    '''
    for i in range(times):
        if i > 0:
            Ek_1 = Ek

        Ek = get_curr_e()  # 獲取當前值
        sum_Ek = sum_Ek + Ek  # 獲取歷史值之和

        e[i] = Ek  # 儲存當前值，方便後面繪圖

        if Ek != 0 and i > 0:
            sum_f_out = sum_f_out + pid_out()  # 獲取輸出值之和

    plt.plot(e, label='PID({0}, {1}, {2})'.format(Kp, Ti, Td))  # 畫曲線圖，顯示PID圖例

plt.plot(np.zeros(times + 25), label='SV', linestyle='--')  # 設定值
plt.legend()  # 顯示圖例

plt.show()

懶得複製？直接下載源碼

鏈接：PID仿真源碼
密碼：hhh