硬核操作，用四种语言对无人机KAKA进行PID仿真（C，C++，Matlab，Python）(源码和结果）

Penelope ·

更新时间:2024-11-13

· 981 次阅读

前言：本文只提供源代码和仿真结果，了解原理和推导过程可参考《某科学的PID算法学习笔记》 前言：本文只提供源代码和仿真结果，了解原理和推导过程可参考《[某科学的PID算法学习笔记](https://www.cnblogs.com/brosy/p/12838456.html)》一、问题二、解答三、源码和结果1) Matlab2) C3) C++4) Python懒得复制？直接下载源码 一、问题

在《机甲大师》动漫中，主角“单单”拥有一架语音遥控的双旋翼无人机，名叫“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
#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  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 Loop(400);// 计算400次
        delete pid[i]; // 释放内存
    }
}

PID.cpp

#include "PID.h"
#include 
/**
@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  0)
        {
            Ek_1 = Ek; // 获取k-1的偏差值
        }
        Ek = GetCurrE(); // 获取当前偏差值
        SumEk += Ek; // 历史偏差之和
        std::cout << Ek < 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()

在这里插入图片描述