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

最新推荐文章于 2024-05-10 09:36:12 发布
置顶 最新推荐文章于 2024-05-10 09:36:12 发布
阅读量1.4k 收藏 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
关注
  • 8
    点赞
  • 踩
  • 34
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
硬核消消乐.zip源码cocos creator项目源码下载
03-18
硬核消消乐.zip源码cocos creator项目源码下载硬核消消乐.zip源码cocos creator项目源码下载 1.上线产品适合个人学习技术做项目参考 2.开发脚本为javsScropt或者typeScript 3.上线产品适合小公司开发游戏项目参考
2023硬核Python科技技巧必学
09-20
2023硬核Python科技技巧必学
matlab提取文件要素代码-python-ml:使用Python进行机器学习。只是探索
05-21
matlab提取文件要素代码使用Python进行机器学习 关于使用Python学习机器学习的注意事项。 目的是学习机器学习范围内与Python相关的库和工具。 可用语言 显然，我们可以用多种语言编写机器学习代码，但是在机器学习的范围内，有四种语言处于领先地位。 （Python）[] 由于其简单性和柔和的学习曲线，它现在是比赛的领先者。 对于编程和机器学习的初学者来说，这是特别好的和成功的。 图书馆的生态系统和社区的支持是巨大的。 （R）[] 它是为统计分析和可视化而设计的，经常用于解锁大数据块中的模式。 借助RStudio，开发人员可以轻松构建算法和统计可视化。 它是Matlab等较昂贵的软件的免费替代品。 （Matlab）[] 对于复杂的数学，它是快速，稳定和安全的。 它被认为是数学家和科学家的硬核语言。 （Julia）[] 专为处理数值分析需求和计算科学而设计。 基本的Julia库已与C和Fortram开源库集成在一起。 Jupyter和Julia社区之间的协作，为Julia提供了强大的UI。 选择语言时应考虑的一些重要点是： 速度。 学习曲线。 成本。 社区支持。 生产率。 在这
基于OpenGL的飞行环境虚拟仿真 (2009年)
05-11
采用VisualC++6.0可视化开发环境和OpenGL编程技术，根据飞行动力学和运动学数学模型，对飞行器飞行规律进行仿真。用来模拟飞行器在三维空间的飞行运动规律，用户可通过键盘实现对三维飞行器的控制，主要参数亦可动态随之显示变化。软件界面友好，易于操作。
RM2019机甲大师赛官方赠与步兵代码开源--RM Robot Open Source Code.rar
06-01
RM2019机甲大师赛官方赠与步兵代码开源
飞控仿真软件
blog.boringhex.top
07-13 1236
飞控仿真是一种在计算机模拟环境中对飞行控制系统进行测试和验证的方法。它通过使用仿真软件和工具来模拟飞行器的物理行为、传感器数据和控制算法的执行，以评估飞行控制系统的性能和稳定性。
小伙利用C/C++打造无人机控制系统，引得全场惊叹！
10-30 2876
我们第一步的目标是让无人机做到真正无人，从而能够为人类进行更多的工作，并且可以让更多程序员和工程师更简单的为无人机编写程序和制作产品。长期目标是将 GAAS 应用在载人 VTOL 上（也许是飞行汽车）。我们现在支持无人和有人的旋翼和直升机驾驶。下一步计划支持 VTOL 和 eVTOL。 项目总览 当前 GAAS 可为无人机提供以下十大功能，其中一些功能仍有待优化： 注意：GAAS ...
带你玩转PX4无人机仿真(1) —— 运行官方案例（C++）
weixin_55944949的博客
08-29 1830
本次教程是官方提供的 MAVROS Offboard (板外) 控制示例
无人驾驶（pid算法）Python实现
u012817688的博客
05-22 3617
参考：https://blog.csdn.net/feixiaoxing/article/details/89348400 对于工科的同学来说，最简单的自动控制代码其实就是PID。在现实的生产中，最难控制的往往是输入和输出之间的关系。很多时候，系统的输入和输出是很复杂的关系，根本没有办法用方程来表示，或者说无法用方程来准确表示。这个时候，PID就可以发挥用处。PID非常简单，主要就是比例系数、积分系数和微分系数。我们可以用一个简单的代码来说明这个问题， # !/usr/bin/python impo
用python语言编写ros无人机仿真路线代码
weixin_35749545的博客
02-14 662
用 Python 编写 ROS 无人机仿真路线代码，需要满足以下几个步骤： 安装 ROS 和 Python 开发环境，如果您是初学者可以选择安装 ROS melodic。 创建一个 ROS 工作空间，用于存储代码和运行仿真。 创建一个包，用于存储 Python 脚本。 编写代码，这里提供一份简单的代码示例： #!/usr/bin/env python import rospy from...
轻型公务机迎角限制系统设计与飞行品质评估
10-17
设计了基于C*响应的某轻型公务机的纵向控制增稳系统，为了飞行安全，采用闭环控制方法设计了迎角限制器，并对迎角限制系统与控制增稳系统模态兼容问题进行了研究，实现了模态平滑转换，最后对飞行品质进行了评估。系统数字仿真结果验证了迎角限制的有效性。
2023深圳杯数学建模C题论文及代码（硬核）：基于几何构形和人工势场的无人机协调避障问题研究
08-21
2023深圳杯C题无人机协同避障航迹规划，专业团队花费几天时间打造，原创首发，全程硬核，基于几何构形和人工势场，较为完整地解决了问题，格式规范，正文+代码近50页，附python代码文件。团队自身不参加深圳杯比赛，...
matlab做信号实验需要安装那些模块_无人机基于Matlab/Simulink的模型开发(连载一）...
weixin_39606575的博客
10-20 210
“一切可以被控制的对象，都需要被数学量化”这是笔者从事多年研发工作得出的道理，无论是车辆控制，机器人控制，飞机控制，还是无人机控制，所有和机械运动相关的控制，如果不能被很好的数学量化，那么将不会被很好的控制。因为工作需要，笔者曾拜访过很多无人机研发公司，高校和研究所。发现大多数无人机研发公司的研发手段，相较于国外，还很初级。基本都是嵌入式开发居多，侧重于驱动的修改，飞行逻辑的修改。我认为这算不上是...
在PX4中如何使用offboard模式以及对c_uart_interface_example程序的分析
weixin_39472686的博客
10-21 1712
这个程序（mavlink-examles/c_uart_interface_example)演示了如何用c语言调用mavlink API做offboard控制。 这个程序仅针对串口和udp口通讯，目前pixhawk飞控计算机与外部计算机的通讯只支持串口，而udp口是针对PX4放在仿真环境里时的通讯接口，本文只针对uart进行分析。 c_uart_interface_example在打开串口（port.start())后，就启动了2个线程，一个是读线程，一个是写线程，顾名思义，写线..........
Matlab仿真——基于NMPC实现飞行机器人运动规划
weixin_55188463的博客
08-17 977
Matlab仿真——基于NMPC实现飞行机器人运动规划
m基于PID控制算法的四旋翼无人机飞行控制simulink仿真
fpga/matlab/simulink算法仿真工程
01-13 1万+
该模型，主要通过输入初始的Roll，Pitch，Yaw，初始的p，q，r，以及XYZ轴的转动惯量。参数ki​​​​​​​表示的是积分调整模块的系数，其作用将消除系统中存在的稳态误差，当存在稳态误差的时候，通过积分调节模块进行调节，直到完全消除稳态误差为止，之后积分调节功能停止工作，积分调节模块输出一个固定值。表示的是微分调整模块的系数，其主要是对系统反馈误差的变化率进行调整，其具有超前调整功能，可以预测系统反馈误差的变化率，因此可以在反馈误差产生之间将误差消除，因此通过设置微分调整参数，可以降低。
四旋翼无人机PID控制
热门推荐
哈哈哈哈的博客
12-15 2万+
一、建立四旋翼无人机动力学方程 二、确定PID控制律 1、姿态控制回路       2、位置控制回路 3、仿真图
Java 7大排序
最新发布
m0_74270127的博客
05-10 812
插入排序、希尔排序、选择排序、堆排序、冒泡排序、快速排序、归并排序

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值