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

最新推荐文章于 2023-09-20 02:02:22 发布
置顶 最新推荐文章于 2023-09-20 02:02:22 发布
阅读量1.5k 收藏 34
点赞数 8
分类专栏： 自动控制原理 文章标签： 算法 python c++ matlab pid
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_45780842/article/details/106041435
版权

前言：本文只提供源代码和仿真结果，了解原理和推导过程可参考《某科学的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

BROSY
关注
专栏目录
STK matlab python仿真
**My Coding Family**
08-15 904
🏆本文收录于《CSDN问答解惑-专业版》专栏，主要记录项目实战过程中的Bug之前因后果及提供真实有效的解决方案，希望能够助你一臂之力，帮你早日登顶实现财富自由🚀；同时，欢迎大家关注&&收藏&&订阅！持续更新中，up！up！up！！
100天精通Python丨基础知识篇 —— 01、C站最全Python标准库总结
热门推荐
独木不成林，单弦不成音。
06-29 51万+
❤粉丝福利：免费下载海量【PPT模板、简历模板、学习资料】https://app.yinxiang.com/fx/13ce6bbd-f36f-4e92-be53-92dd381ed729
小型无人机纵向姿态模糊自适应 PID控制与仿真 (2008年)
06-14
针对某小型实验无人机智能自主飞行的要求,提出了一种无人机纵向姿态的模糊控制方法,设计了模糊自 适应 PID控制器,可有效实现该无人机的纵向姿态控制和纵向航迹跟踪。仿真结果表明,所设计的模糊自适应 PID 控制器较传统的 PID控制器具有更好的控制性能 ,其响应快、超调小、精度高,而且鲁棒性和自适应能力也较强,可 满足自主飞行的要求。
【PID】【1】无人机起降模拟系统，普通PID
痛苦的时候装出幸福相，这不是那么难做到的事
10-25 466
前言 以前看《企业编程与控制理论》的时候，一直不明白为什么PID控制这么差劲，后面才知道是自己学的太少了。这篇是基础，任然是简单的PID，不抗饱和，也什么都不抗。 PID # -*- encoding: utf-8 -*- # @File:__init__.py # @Author:Demllie # @Time:2021/10/25,12:49 import numpy import matplotlib.pyplot from mpl_toolkits.axisartist.axislines i
用python语言编写ros无人机仿真路线代码
weixin_35749545的博客
02-14 759
用 Python 编写 ROS 无人机仿真路线代码，需要满足以下几个步骤： 安装 ROS 和 Python 开发环境，如果您是初学者可以选择安装 ROS melodic。 创建一个 ROS 工作空间，用于存储代码和运行仿真。 创建一个包，用于存储 Python 脚本。 编写代码，这里提供一份简单的代码示例： #!/usr/bin/env python import rospy from...
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
caiqidong321的博客
01-26 2534
本节中我们讲一讲rotor的参数补偿 和pid控制
硬核消消乐.zip源码cocos creator项目源码下载
03-18
硬核消消乐.zip源码cocos creator项目源码下载硬核消消乐.zip源码cocos creator项目源码下载 1.上线产品适合个人学习技术做项目参考 2.开发脚本为javsScropt或者typeScript 3.上线产品适合小公司开发游戏项目参考
matlab提取文件要素代码-python-ml:使用Python进行机器学习。只是探索
05-21
matlab提取文件要素代码使用Python进行机器学习 关于使用Python学习机器学习的注意事项。 目的是学习机器学习范围内与Python相关的库和工具。 可用语言 显然，我们可以用多种语言编写机器学习代码，但是在机器学习的...
无人驾驶（pid算法）Python实现
u012817688的博客
05-22 3794
参考：https://blog.csdn.net/feixiaoxing/article/details/89348400 对于工科的同学来说，最简单的自动控制代码其实就是PID。在现实的生产中，最难控制的往往是输入和输出之间的关系。很多时候，系统的输入和输出是很复杂的关系，根本没有办法用方程来表示，或者说无法用方程来准确表示。这个时候，PID就可以发挥用处。PID非常简单，主要就是比例系数、积分系数和微分系数。我们可以用一个简单的代码来说明这个问题， # !/usr/bin/python impo
小型飞机模拟程序 - C语言实现
最新发布
UkjUnity的博客
09-20 169
这是一个用C语言实现的小型飞机模拟程序。程序中定义了一个飞机结构体，包含飞机的速度和高度属性。这个程序通过使用结构体和函数实现了一个简单的飞机模拟。你可以根据需要扩展程序，添加更多的功能和交互性。的飞机实例，并将其速度和高度初始化为0。函数将飞机的速度增加100 km/h，调用。函数将飞机的高度增加500米，并再次通过。函数中，首先创建了一个名为。函数打印飞机的当前信息。函数打印飞机的当前信息。函数打印飞机的最终状态。函数使飞机起飞，并通过。函数使飞机降落，并通过。
带你玩转PX4无人机仿真(1) —— 运行官方案例（C++）
weixin_55944949的博客
08-29 4328
本次教程是官方提供的 MAVROS Offboard (板外) 控制示例
无人机视觉方向用什么语言和框架
weixin_42593701的博客
02-11 786
无人机视觉方向通常使用多种语言和框架，具体取决于具体应用场景和需求。 常见的语言有C++, Python, Java等，而框架则可以是OpenCV, TensorFlow, Caffe等。 其中，C++是高效的编程语言，适合开发无人机的底层控制代码；Python是简单易用的语言，适合快速开发原型和实验；而OpenCV和TensorFlow是两个功能强大的图像处理和机器学习框架。 ...
小伙利用C/C++打造无人机控制系统，引得全场惊叹！
10-30 2973
我们第一步的目标是让无人机做到真正无人，从而能够为人类进行更多的工作，并且可以让更多程序员和工程师更简单的为无人机编写程序和制作产品。长期目标是将 GAAS 应用在载人 VTOL 上（也许是飞行汽车）。我们现在支持无人和有人的旋翼和直升机驾驶。下一步计划支持 VTOL 和 eVTOL。 项目总览 当前 GAAS 可为无人机提供以下十大功能，其中一些功能仍有待优化： 注意：GAAS ...
无人机c语言程序设计,c语言程序设计课程设计单元设计教案).pdf
weixin_36079903的博客
05-18 1242
《C语言程序设计》课程整体教学设计(2019～2020学年第1 学期)课程名称： C 语言程序设计所属系部： 机电与汽车工程学院制定人： 张三合作人：制定时间： 2019.9许昌职业技术学院1课程整体教学设计一、课程基本信息课程名称：C 语言程序设计课程所在专业 ...
【无人机】基于PID控制的无人机巡航仿真（Matlab代码实现）
weixin_46039719的博客
04-10 929
2]刘新宇,桑晨,李媛媛,游璧铭,刘畅.基于PID算法的巡逻无人机[J].现代信息科技,2022,6(20):141-145+151.DOI:10.19850/j.cnki.2096-4706.2022.20.033.指当无人机从不稳定回到稳定状态一段时间内积累的误差，四轴无人机在民用无人机领域受到了广泛的关注与认可。几乎各项技术的发展都涉及到了人工智能技术领域。无人机作为新时代人工智能技术的产物应运而生，机身对产生姿态偏移回到稳定的时间，交通运输等领域得到了广泛的应用。P指当无人机收到外界的干扰后，
飞控仿真软件
blog.boringhex.top
07-13 1749
飞控仿真是一种在计算机模拟环境中对飞行控制系统进行测试和验证的方法。它通过使用仿真软件和工具来模拟飞行器的物理行为、传感器数据和控制算法的执行，以评估飞行控制系统的性能和稳定性。
模糊控制应用（二）模糊PID在四旋翼上的应用
高能阿博特的博客
08-29 3419
模糊控制应用（二）模糊PID在四旋翼上的应用1. 四旋翼基本方程2. 经典PID控制框图3. 模糊PID控制 1. 四旋翼基本方程 在博客四旋翼无人机基本方程结尾已经给出了描述四旋翼的基本公式，列举如下： [φ¨θ¨ψ¨x¨y¨z¨]=[θ˙ψ˙Jy−JzJx+lJx(F2−F4)φ˙ψ˙Jz−JxJy+lJy(F1−F3)φ˙θ˙Jx−JyJz+1Jz(MD1−MD2+MD3−MD4)1m(cos⁡ψsin⁡θcos⁡φ+sin⁡ψsin⁡φ)∑i=14Fib1m(sin⁡ψsin⁡θcos⁡φ−cos⁡
ABS模糊PID控制matlab仿真图及具体参数（刚做好，感觉效果凑活）
qq_40180481的博客
10-14 4761
直接开始吧 输入、输出隶属度函数：e、ec均为[-3，3]，ki',kd',kp'均为[0,1]，都采用三角形隶属度 模糊规则就百度一个相关的ABS论文，基本都有。 仿真图就是根据这三个公式搭建的。 来了，上图了：：：：：： 这是模糊PID和单纯PID对比图，大体差不多，就是控制器那块不一样。 下边第一个图是模糊PID控制器内部设计，第二个图是第一个图中次模块的具体参数。这...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值