当前位置： 首页 > article >正文

C 语言实现四旋翼飞行器姿态控制：基于 PID 控制器(2)

article 2024/12/23 12:30:22

在这里插入图片描述

一、代码整体框架与功能概述

本 C 语言代码旨在实现四旋翼飞行器的姿态控制，通过 PID 控制器对飞行器的俯仰（Pitch）、滚转（Roll）和偏航（Yaw）进行精确调控，以达到稳定飞行或按照预定姿态飞行的目的。其核心功能包括初始化 PID 控制器、根据目标姿态与当前姿态计算控制输出、将控制量合理分配到四个电机并输出相应的电机 PWM 值。

二、代码详细解析

（一）头文件与宏定义

#include <stdio.h>
#include <math.h>

// 飞行控制参数
#define KP 1.0   // 比例增益
#define KI 0.1   // 积分增益
#define KD 0.05  // 微分增益
#define MAX_PWM 2000
#define MIN_PWM 1000

这里引入了标准输入输出头文件 stdio.h 用于数据的打印输出，以及数学函数头文件 math.h （虽然在当前示例代码中未明确看到其使用，但在更复杂的飞行控制计算中可能会用到三角函数等数学运算）。通过宏定义设定了比例（KP）、积分（KI）、微分（KD）增益系数，这些系数对 PID 控制器的性能有着关键影响。同时定义了电机 PWM 值的最大（MAX_PWM）和最小（MIN_PWM）限制范围，确保电机控制信号在合理区间内。

（二）结构体定义

// 飞行器目标姿态（单位：角度）
typedef struct {
    float pitch; // 俯仰目标
    float roll;  // 滚转目标
    float yaw;   // 偏航目标
} TargetAttitude;

// 飞行器当前姿态（单位：角度）
typedef struct {
    float pitch; // 当前俯仰
    float roll;  // 当前滚转
    float yaw;   // 当前偏航
} CurrentAttitude;

// PID 控制器
typedef struct {
    float kp;    // 比例增益
    float ki;    // 积分增益
    float kd;    // 微分增益
    float prev_error;  // 上一次误差
    float integral;    // 积分项
} PIDController;

定义了三个结构体。TargetAttitude 结构体用于存储飞行器的目标姿态信息，包括俯仰、滚转和偏航角度目标值。CurrentAttitude 结构体则记录飞行器当前的实际姿态信息。PIDController 结构体是 PID 控制器的核心数据结构，包含了比例、积分、微分增益系数，以及用于记录上一次误差的 prev_error 和积分项 integral，这些变量在 PID 控制算法的计算过程中起着重要作用。

（三）PID 控制器相关函数

  1. 初始化函数 initPID
// 初始化 PID 控制器
void initPID(PIDController *pid, float kp, float ki, float kd) {
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->prev_error = 0;
    pid->integral = 0;
}

该函数用于初始化一个 PID 控制器结构体。接受一个指向 PIDController 结构体的指针 pid 以及比例、积分、微分增益系数作为参数。在函数内部，将传入的增益系数赋值给结构体成员，并将上一次误差 prev_error 初始化为 0，积分项 integral 初始化为 0，为后续的 PID 计算做好准备。
2. PID 计算函数 computePID

// PID 控制算法
float computePID(PIDController *pid, float target, float current, float dt) {
    float error = target - current;
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    pid->prev_error = error;

    return (pid->kp * error) + (pid->ki * pid->integral) + (pid->kd * derivative);
}

此函数实现了经典的 PID 控制算法。首先计算当前的误差 error，即目标值 target 与当前值 current 的差值。然后将误差乘以时间间隔 dt 累加到积分项 integral 中。接着计算误差的变化率 derivative，即当前误差与上一次误差的差值除以时间间隔。之后更新上一次误差 prev_error 为当前误差。最后根据 PID 算法公式，将比例项（kp * error）、积分项（ki * integral）和微分项（kd * derivative）相加并返回计算结果，该结果即为基于当前误差状态的控制输出量。

（四）电机 PWM 值计算函数

// 计算电机 PWM 值
void computeMotorPWM(TargetAttitude target, CurrentAttitude current, float motorPWM[4], float dt) {
    PIDController pitchPID, rollPID, yawPID;

    initPID(&pitchPID, KP, KI, KD);
    initPID(&rollPID, KP, KI, KD);
    initPID(&yawPID, KP, KI, KD);

    // 计算每个方向的控制输出
    float pitchOutput = computePID(&pitchPID, target.pitch, current.pitch, dt);
    float rollOutput = computePID(&rollPID, target.roll, current.roll, dt);
    float yawOutput = computePID(&yawPID, target.yaw, current.yaw, dt);

    // 分配到四个电机
    motorPWM[0] = 1500 + pitchOutput - rollOutput + yawOutput; // 前左电机
    motorPWM[1] = 1500 + pitchOutput + rollOutput - yawOutput; // 前右电机
    motorPWM[2] = 1500 - pitchOutput - rollOutput - yawOutput; // 后左电机
    motorPWM[3] = 1500 - pitchOutput + rollOutput + yawOutput; // 后右电机

    // 限制 PWM 范围
    for (int i = 0; i < 4; i++) {
        if (motorPWM[i] > MAX_PWM) motorPWM[i] = MAX_PWM;
        if (motorPWM[i] < MIN_PWM) motorPWM[i] = MIN_PWM;
    }
}

这个函数是整个姿态控制到电机控制的核心转换环节。首先创建了三个分别用于俯仰、滚转和偏航控制的 PID 控制器实例 pitchPID、rollPID 和 yawPID，并使用之前定义的 initPID 函数进行初始化，传入全局定义的增益系数 KP、KI 和 KD。然后分别调用 computePID 函数计算俯仰、滚转和偏航方向的控制输出 pitchOutput、rollOutput 和 yawOutput。接着根据四旋翼飞行器的电机布局和控制原理，将这些控制输出组合并加上一个基础的 PWM 值 1500，计算出四个电机的 PWM 值 motorPWM[0] - motorPWM[3]，分别对应前左、前右、后左和后右电机。最后，通过一个循环检查并限制每个电机的 PWM 值在预定义的最大（MAX_PWM）和最小（MIN_PWM）范围内，确保电机控制信号的有效性和安全性。

（五）主函数

int main() {
    TargetAttitude target = {5.0, 0.0, 0.0}; // 目标俯仰 5 度，滚转 0 度，偏航 0 度
    CurrentAttitude current = {2.0, -1.0, 0.5}; // 当前俯仰 2 度，滚转 -1 度，偏航 0.5 度
    float motorPWM[4]; // 存储电机的 PWM 值

    float dt = 0.02; // 时间间隔（假设控制频率为 50Hz）

    // 计算电机 PWM
    computeMotorPWM(target, current, motorPWM, dt);

    // 打印电机 PWM 值
    printf("Motor PWM Values:\n");
    printf("Motor 1: %.2f\n", motorPWM[0]);
    printf("Motor 2: %.2f\n", motorPWM[1]);
    printf("Motor 3: %.2f\n", motorPWM[2]);
    printf("Motor 4: %.2f\n", motorPWM[3]);

    return 0;
}

在主函数中，首先定义了飞行器的目标姿态 target 和当前姿态 current，这里设置了目标俯仰角为 5 度，滚转和偏航目标为 0 度，当前姿态分别为俯仰 2 度、滚转 -1 度和偏航 0.5 度。然后定义了一个数组 motorPWM 用于存储计算得到的四个电机的 PWM 值。接着设定时间间隔 dt 为 0.02 秒，对应控制频率为 50Hz（因为控制频率 = 1 / 时间间隔）。之后调用 computeMotorPWM 函数根据目标姿态和当前姿态计算电机 PWM 值，并将结果存储在 motorPWM 数组中。最后通过 printf 函数将四个电机的 PWM 值打印输出，以便观察和分析控制结果。

三、原理深入解析

（一）PID 控制器原理

  1. 比例控制（P）
    • 比例控制在本代码中的体现是在 computePID 函数中的 pid->kp * error 部分。它根据当前的误差大小直接进行修正。例如，如果目标俯仰角为 5 度，当前俯仰角为 2 度，误差为 3 度，比例增益 KP 为 1.0，那么比例控制输出就是 3 * 1.0 = 3。比例增益越高，对误差的修正力度就越大，飞行器对姿态偏差的响应就越快。但如果比例增益过高，可能会导致飞行器姿态调整过于剧烈，甚至出现不稳定的振荡现象。
  2. 积分控制（I）
    • 由 computePID 函数中的 pid->ki * pid->integral 实现。它对误差进行累计积分，旨在消除长期存在的稳态误差。比如，在一段时间内，飞行器的俯仰角始终略低于目标值，虽然每次的误差可能较小，但积分项会不断累积这些误差，随着时间推移，积分控制输出会逐渐增大，推动飞行器调整姿态，直到消除稳态误差。然而，如果积分增益 KI 过大，可能会使积分项增长过快，导致控制输出过度，引起飞行器姿态的超调或振荡。
  3. 微分控制（D）
    • 对应 computePID 函数中的 pid->kd * derivative。它依据误差的变化率进行修正。当飞行器姿态快速接近目标姿态时，误差变化率较大，微分控制会产生一个与变化方向相反的控制量，减缓姿态调整速度，从而减少超调和振荡，提高系统的响应速度和稳定性。例如，当飞行器的俯仰角快速从 2 度向 5 度变化时，微分控制会根据俯仰角变化的速率输出一个修正量，防止飞行器因惯性而冲过目标姿态。但微分增益 KD 过大时，可能会对噪声或系统的微小快速变化过于敏感，导致不必要的控制动作。

（二）姿态控制

  1. 飞行器的姿态由俯仰（Pitch）、滚转（Roll）、偏航（Yaw）三个方向的角度确定。在本代码中，通过 TargetAttitude 和 CurrentAttitude 结构体分别记录目标姿态和当前姿态信息。
  2. PID 控制器针对每个姿态方向独立运行。对于俯仰方向，computePID 函数根据目标俯仰角和当前俯仰角计算控制输出 pitchOutput；同理，对于滚转和偏航方向也分别计算相应的控制输出 rollOutput 和 yawOutput。然后将这些控制输出按照特定的组合方式分配到四个电机上，以实现对飞行器姿态的精确控制。

（三）电机 PWM 分配

  1. 四旋翼飞行器的电机布局为电机 1 - 4 分别代表前左、前右、后左、后右。在 computeMotorPWM 函数中，根据计算得到的俯仰、滚转和偏航控制输出，按照 motorPWM[0] = 1500 + pitchOutput - rollOutput + yawOutput 等公式分配到四个电机。这种分配方式基于四旋翼飞行器的力学原理，通过改变不同电机的转速来产生不同的力矩，从而实现对飞行器姿态的调整。例如，增加前左电机的 PWM 值（即提高其转速），同时降低后左电机的 PWM 值，会使飞行器产生向前的俯仰力矩，使飞行器前倾。
  2. 对电机 PWM 值进行了范围限制，确保电机不会接收到超出其正常工作范围的控制信号。这是为了保护电机以及保证飞行器的安全稳定运行。如果电机 PWM 值超出最大或最小限制，可能会导致电机损坏、飞行器失控等问题。

四、实际应用场景

  1. 稳定悬停：当目标姿态设置为 {0, 0, 0} 时，即俯仰、滚转和偏航目标角度均为 0 度，飞行器将尝试保持水平稳定悬停状态。PID 控制器会根据当前姿态与目标姿态的偏差，计算并调整电机的 PWM 值，使飞行器克服外界干扰（如微风等），保持在固定位置和姿态。
  2. 飞行前进：通过增加俯仰目标角度，例如设置为 {5, 0, 0}，飞行器会根据 PID 控制器计算出的控制量，增加前电机的转速，降低后电机的转速，使飞行器前倾并产生向前的推力，从而实现向前飞行。在飞行过程中，PID 控制器会持续监测飞行器的姿态变化，根据实际姿态与目标姿态的偏差不断调整电机转速，确保飞行姿态的稳定和飞行方向的准确。
  3. 转向控制：调整偏航目标角度，如设置为 {0, 0, 10}，会使飞行器绕垂直轴旋转。此时，PID 控制器会根据偏航误差计算控制输出，改变左右电机的转速差，产生使飞行器旋转的力矩，实现转向操作。在转向过程中，同样依靠 PID 控制器来维持其他姿态方向（俯仰和滚转）的稳定，避免因转向动作而导致飞行器姿态失控。

五、输出结果分析

代码运行后，根据输入的目标姿态和当前姿态，输出四个电机的 PWM 值。如示例输出：

Motor PWM Values:
Motor 1: 1510.00
Motor 2: 1495.00
Motor 3: 1485.00
Motor 4: 1500.00

从这些值可以看出，前左电机（Motor 1）的 PWM 值为 1510.00，相对基础值 1500 有所增加；后左电机（Motor 3）的 PWM 值为 1485.00，相对基础值有所降低。根据电机 PWM 分配原理，这种电机转速的差异会使飞行器产生向前的俯仰力矩，飞行器会向前倾斜（增加俯仰角），以接近目标姿态。通过分析输出的电机 PWM 值，可以直观地了解飞行器的控制动作和姿态调整趋势，有助于调试和优化飞行控制算法。

六、总结

此代码展示了飞行控制算法的核心逻辑流程：首先通过传感器获取飞行器的当前姿态信息，然后计算目标姿态与当前姿态的误差，接着利用 PID 控制器根据误差计算控制输出，最后将控制量合理分配到电机并转换为电机的 PWM 值来调整电机转速，从而实现对飞行器姿态的控制。通过调整 PID 参数（KP、KI、KD）和目标姿态，可以实现多种复杂的飞行控制行为，如稳定悬停、精确转向和直线飞行等。这是飞行器控制系统的基础框架，在无人机、机器人等领域广泛应用，为实现自主飞行和智能控制奠定了坚实的基础。在实际应用中，需要根据飞行器的具体特性（如重量、尺寸、气动性能等）、飞行环境（如风速、海拔等）以及任务需求，对 PID 参数进行精细调整和优化，以达到最佳的飞行控制效果。

七、完整代码

#include <stdio.h>
#include <math.h>

// 飞行控制参数
#define KP 1.0   // 比例增益
#define KI 0.1   // 积分增益
#define KD 0.05  // 微分增益
#define MAX_PWM 2000
#define MIN_PWM 1000

// 飞行器目标姿态（单位：角度）
typedef struct {
    float pitch; // 俯仰目标
    float roll;  // 滚转目标
    float yaw;   // 偏航目标
} TargetAttitude;

// 飞行器当前姿态（单位：角度）
typedef struct {
    float pitch; // 当前俯仰
    float roll;  // 当前滚转
    float yaw;   // 当前偏航
} CurrentAttitude;

// PID 控制器
typedef struct {
    float kp;    // 比例增益
    float ki;    // 积分增益
    float kd;    // 微分增益
    float prev_error;  // 上一次误差
    float integral;    // 积分项
} PIDController;

// 初始化 PID 控制器
void initPID(PIDController *pid, float kp, float ki, float kd) {
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->prev_error = 0;
    pid->integral = 0;
}

// PID 控制算法
float computePID(PIDController *pid, float target, float current, float dt) {
    float error = target - current;
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    pid->prev_error = error;

    return (pid->kp * error) + (pid->ki * pid->integral) + (pid->kd * derivative);
}

// 计算电机 PWM 值
void computeMotorPWM(TargetAttitude target, CurrentAttitude current, float motorPWM[4], float dt) {
    PIDController pitchPID, rollPID, yawPID;

    initPID(&pitchPID, KP, KI, KD);
    initPID(&rollPID, KP, KI, KD);
    initPID(&yawPID, KP, KI, KD);

    // 计算每个方向的控制输出
    float pitchOutput = computePID(&pitchPID, target.pitch, current.pitch, dt);
    float rollOutput = computePID(&rollPID, target.roll, current.roll, dt);
    float yawOutput = computePID(&yawPID, target.yaw, current.yaw, dt);

    // 分配到四个电机
    motorPWM[0] = 1500 + pitchOutput - rollOutput + yawOutput; // 前左电机
    motorPWM[1] = 1500 + pitchOutput + rollOutput - yawOutput; // 前右电机
    motorPWM[2] = 1500 - pitchOutput - rollOutput - yawOutput; // 后左电机
    motorPWM[3] = 1500 - pitchOutput + rollOutput + yawOutput; // 后右电机

    // 限制 PWM 范围
    for (int i = 0; i < 4; i++) {
        if (motorPWM[i] > MAX_PWM) motorPWM[i] = MAX_PWM;
        if (motorPWM[i] < MIN_PWM) motorPWM[i] = MIN_PWM;
    }
}

int main() {
    TargetAttitude target = {5.0, 0.0, 0.0}; // 目标俯仰 5 度，滚转 0 度，偏航 0 度
    CurrentAttitude current = {2.0, -1.0, 0.5}; // 当前俯仰 2 度，滚转 -1 度，偏航 0.5 度
    float motorPWM[4]; // 存储电机的 PWM 值

    float dt = 0.02; // 时间间隔（假设控制频率为 50Hz）

    // 计算电机 PWM
    computeMotorPWM(target, current, motorPWM, dt);

    // 打印电机 PWM 值
    printf("Motor PWM Values:\n");
    printf("Motor 1: %.2f\n", motorPWM[0]);
    printf("Motor 2: %.2f\n", motorPWM[1]);
    printf("Motor 3: %.2f\n", motorPWM[2]);
    printf("Motor 4: %.2f\n", motorPWM[3]);

    return 0;
}

http://www.kler.cn/a/447893.html

相关文章：

  • clickhouse优化记录
  • python如何保存.npy
  • 通过阿里云 Milvus 与 PAI 搭建高效的检索增强对话系统
  • 搜索召回：倒排召回
  • 小红书关键词搜索采集 | AI改写 | 无水印下载 | 多维表格 | 采集同步飞书
  • flask before_request 请求拦截器返回无值则放行，有值则拦截
  • 【前端js】 indexedDB Nosql的使用方法
  • Sourcegraph 概述
  • Redis篇--常见问题篇8--缓存一致性3（注解式缓存Spring Cache）
  • opencv项目--文档扫描
  • 3.metagpt中的软件公司智能体 (Architect 角色)
  • 纯血鸿蒙APP实战开发——文字展开收起案例
  • C# cad启动自动加载启动插件、类库编译 多个dll合并为一个
  • 图解HTTP-HTTP协议
  • 反归一化 from sklearn.preprocessing import MinMaxScaler
  • 2024年最新多目标优化算法：多目标麋鹿群优化算法（MOEHO）求解DTLZ1-DTLZ7及工程应用---盘式制动器设计，提供完整MATLAB代码
  • iframe和浏览器页签切换
  • 解决uniapp中使用axios在真机和模拟器下请求报错问题
  • 亚马逊API接口深度解析：如何高效获取商品详情与评论数据
  • 洛谷 P1644 跳马问题 C语言
  • （耗时4天制作）详细介绍macOS系统 本博文含有全英版 （全文翻译稿）
  • 【NLP 16、实践 ③ 找出特定字符在字符串中的位置】
  • 2024.12 迈向可解释和可解释的多模态大型语言模型：一项综合调查
  • JDK13主要特性
  • Mysql复习（一）
  • 【唐叔学算法】第18天：解密选择排序的双重魅力-直接选择排序与堆排序的Java实现及性能剖析