硬核操作，用四种语言对无人机KAKA进行PID仿真（C，C++，Matlab，Python）(源码和结果）
前言：本文只提供源代码和仿真结果，了解原理和推导过程可参考《某科学的PID算法学习笔记》
- 前言：本文只提供源代码和仿真结果，了解原理和推导过程可参考《[某科学的PID算法学习笔记](https://www.cnblogs.com/brosy/p/12838456.html)》
- 一、问题
- 二、解答
- 三、源码和结果
- 1) Matlab
- 2) C
- 3) 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将输出结果导入到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 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