无人机双环PID悬停控制全解析

优质文章学习记录

— 6 min read
无人机双环PID悬停控制全解析

目录

1. 无人机悬停控制系统架构

2. 位置 PID 控制器设计

PID 结构体（扩展到位置控制）

3. 位置控制与姿态控制结合

位置数据结构

位置环 PID 初始化

4. 位置控制循环

5. 完整控制流程（位置 + 姿态 + 电机）

6. 传感器数据融合（GPS / 光流）

GPS 数据读取示例

光流数据读取示例

7. 调试与优化建议

我们在之前的 姿态控制 PID 基础上，增加 位置控制 PID 层，这样无人机就可以根据 GPS 或 光流模块 提供的位置信息，实现精准悬停。

我会给你一个 完整的位置 + 姿态双环 PID 控制方案，包括：

  1. 系统架构（位置环 → 姿态环 → 电机混控）
  2. 位置 PID 设计
  3. 传感器数据融合（GPS / 光流 → 位置估计）
  4. 代码实现（STM32 HAL 库）
  5. 调试与优化建议

1. 无人机悬停控制系统架构

悬停控制是一个 双环 PID 结构：

┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 位置环 PID │ │ 姿态环 PID │ │ 角速度环PID │ │ 电机混控 │ │ (X/Y/Z) │ │ (Roll/Pitch/Yaw) │ │ (ωx/ωy/ωz) │ │ (Motor1~4) │ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ └──────┬──────┘ │ │ │ │ ▼ ▼ ▼ ▼ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ │ 期望位置 │ │ 期望姿态角 │ │ 期望角速度 │ │ PWM信号输出 │ └─────────────┘ └─────────────┘ └─────────────┘ └─────────────┘
  • 位置环：输入期望位置（如悬停点）和实际位置（来自 GPS / 光流），输出期望姿态角（Roll/Pitch）和期望高度（油门修正）。
  • 姿态环：输入期望姿态角和实际姿态角（来自 MPU-9250），输出期望角速度。
  • 角速度环：输入期望角速度和实际角速度（来自陀螺仪），输出电机控制量。
  • 电机混控：结合油门和姿态控制量，计算四个电机的 PWM 值。

2. 位置 PID 控制器设计

位置环需要对 X、Y、Z 三个方向分别控制：

  • X/Y 轴：控制无人机在水平面上的位置，输出期望的横滚角（Roll）和俯仰角（Pitch）。
  • Z 轴：控制无人机的高度，输出油门修正量。

PID 结构体（扩展到位置控制）

typedef struct { float Kp, Ki, Kd; float setpoint; float feedback; float error; float integral; float derivative; float prev_error; float output; float integral_limit; float output_limit; } PID_Controller; void PID_Init(PID_Controller *pid, float Kp, float Ki, float Kd, float integral_limit, float output_limit) { pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->integral_limit = integral_limit; pid->output_limit = output_limit; pid->setpoint = 0.0f; pid->feedback = 0.0f; pid->error = 0.0f; pid->integral = 0.0f; pid->derivative = 0.0f; pid->prev_error = 0.0f; pid->output = 0.0f; } void PID_Update(PID_Controller *pid, float dt) { pid->error = pid->setpoint - pid->feedback; float P = pid->Kp * pid->error; pid->integral += pid->error * dt; if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit; if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit; float I = pid->Ki * pid->integral; pid->derivative = (pid->error - pid->prev_error) / dt; float D = pid->Kd * pid->derivative; pid->output = P + I + D; if (pid->output > pid->output_limit) pid->output = pid->output_limit; if (pid->output < -pid->output_limit) pid->output = -pid->output_limit; pid->prev_error = pid->error; }

3. 位置控制与姿态控制结合

位置数据结构

typedef struct { float x; float y; float z; } Position; Position target_pos; // 期望位置 Position current_pos; // 当前位置（来自GPS/光流）

位置环 PID 初始化

PID_Controller pid_pos_x, pid_pos_y, pid_pos_z; void Position_PID_Init(void) { // X轴位置PID（输出期望Roll角） PID_Init(&pid_pos_x, 2.0f, 0.1f, 0.05f, 50.0f, 15.0f); // 输出限幅±15° // Y轴位置PID（输出期望Pitch角） PID_Init(&pid_pos_y, 2.0f, 0.1f, 0.05f, 50.0f, 15.0f); // Z轴位置PID（输出油门修正量） PID_Init(&pid_pos_z, 5.0f, 0.2f, 0.1f, 100.0f, 200.0f); // 输出限幅±200us }

4. 位置控制循环

void Position_Control_Loop(float dt) { // 更新位置环PID pid_pos_x.setpoint = target_pos.x; pid_pos_x.feedback = current_pos.x; PID_Update(&pid_pos_x, dt); pid_pos_y.setpoint = target_pos.y; pid_pos_y.feedback = current_pos.y; PID_Update(&pid_pos_y, dt); pid_pos_z.setpoint = target_pos.z; pid_pos_z.feedback = current_pos.z; PID_Update(&pid_pos_z, dt); // 位置环输出作为姿态环输入 pid_roll.setpoint = pid_pos_y.output; // Y轴位置误差→Roll角 pid_pitch.setpoint = pid_pos_x.output; // X轴位置误差→Pitch角 pid_yaw.setpoint = 0.0f; // 悬停时航向角保持0° // 油门修正 throttle = 1500.0f + pid_pos_z.output; // 基础油门1500us + Z轴修正 throttle = (throttle < 1000) ? 1000 : (throttle > 2000) ? 2000 : throttle; }

5. 完整控制流程（位置 + 姿态 + 电机）

void Drone_Hover_Loop(void) { float dt = 0.01f; // 100Hz控制频率 // 1. 读取传感器数据 read_mpu9250_data(&euler, gyro); // 姿态角和角速度 read_position_sensor(&current_pos); // GPS/光流位置 // 2. 位置环控制 Position_Control_Loop(dt); // 3. 姿态环控制 pid_roll.feedback = euler.roll; pid_pitch.feedback = euler.pitch; pid_yaw.feedback = euler.yaw; PID_Update(&pid_roll, dt); PID_Update(&pid_pitch, dt); PID_Update(&pid_yaw, dt); // 4. 角速度环控制 pid_rate_roll.setpoint = pid_roll.output; pid_rate_pitch.setpoint = pid_pitch.output; pid_rate_yaw.setpoint = pid_yaw.output; pid_rate_roll.feedback = gyro[0]; pid_rate_pitch.feedback = gyro[1]; pid_rate_yaw.feedback = gyro[2]; PID_Update(&pid_rate_roll, dt); PID_Update(&pid_rate_pitch, dt); PID_Update(&pid_rate_yaw, dt); // 5. 电机混控 motor1 = throttle + pid_rate_roll.output - pid_rate_pitch.output - pid_rate_yaw.output; motor2 = throttle - pid_rate_roll.output - pid_rate_pitch.output + pid_rate_yaw.output; motor3 = throttle - pid_rate_roll.output + pid_rate_pitch.output - pid_rate_yaw.output; motor4 = throttle + pid_rate_roll.output + pid_rate_pitch.output + pid_rate_yaw.output; // 限幅 motor1 = (motor1 < 1000) ? 1000 : (motor1 > 2000) ? 2000 : motor1; motor2 = (motor2 < 1000) ? 1000 : (motor2 > 2000) ? 2000 : motor2; motor3 = (motor3 < 1000) ? 1000 : (motor3 > 2000) ? 2000 : motor3; motor4 = (motor4 < 1000) ? 1000 : (motor4 > 2000) ? 2000 : motor4; // 6. 输出到电机 __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor1); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, motor2); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, motor3); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, motor4); }

6. 传感器数据融合（GPS / 光流）

GPS 数据读取示例

void read_gps_data(Position *pos) { // 假设GPS模块通过串口输出NMEA数据 // 解析$GPGGA或$GPRMC帧，提取经纬度和高度 // 转换为局部坐标系（如ENU坐标系） pos->x = gps_lon_to_x(gps_lon); pos->y = gps_lat_to_y(gps_lat); pos->z = gps_alt; }

光流数据读取示例

void read_optical_flow_data(Position *pos) { // 读取光流模块的X/Y位移和高度数据 pos->x += optical_flow_x * dt; pos->y += optical_flow_y * dt; pos->z = sonar_alt; // 超声波或气压计高度 }

7. 调试与优化建议

  1. 参数整定顺序：
    • 先调姿态环，再调位置环。
    • 位置环的 Kp 从小开始，逐步增加，避免振荡。
  2. 传感器数据滤波：
    • GPS 数据噪声大，建议使用卡尔曼滤波或滑动平均。
    • 光流数据需结合高度信息进行缩放。
  3. 安全保护：
    • 加入位置超范围保护（如偏离悬停点过远时自动返航）。
    • 低电压保护，避免失控。
  4. 仿真验证：
    • 先用 MATLAB/Simulink 搭建仿真模型，验证位置控制逻辑。
  5. 实际测试：
    • 先在室内用光流 + 超声波测试悬停，再到室外用 GPS 测试。

✅ 我已经给你一个 完整的位置 + 姿态双环 PID 悬停控制方案，可以直接在 STM32 上实现，支持 GPS 或光流模块。

如果你需要的话，我可以帮你 加上卡尔曼滤波（EKF）融合 GPS 和光流数据，这样位置估计会更稳定，尤其在 GPS 信号弱的环境下。

Read more

Flutter 三方库 shelf_web_socket 的鸿蒙化适配指南 - 实现具备高性能全双工长连接与协议协商能力的端侧服务端架构、支持分布式实时信令与多端协同实战

Flutter 三方库 shelf_web_socket 的鸿蒙化适配指南 - 实现具备高性能全双工长连接与协议协商能力的端侧服务端架构、支持分布式实时信令与多端协同实战

欢迎加入开源鸿蒙跨平台社区：https://openharmonycrossplatform.ZEEKLOG.net Flutter 三方库 shelf_web_socket 的鸿蒙化适配指南 - 实现具备高性能全双工长连接与协议协商能力的端侧服务端架构、支持分布式实时信令与多端协同实战 前言 在进行 Flutter for OpenHarmony 开发时，当我们的鸿蒙应用需要充当“控制中心”角色（如控制智能家居、开启本地调试服务或实现 P2P 实时对抗脚本时），如何在端侧直接拉起一个支持 WebSocket 协议的高性能微服务端？shelf_web_socket 是针对 shelf 后端框架封装的一款官方级 WebSocket 处理器。本文将探讨如何在鸿蒙端构建极致、透明的长连接交互引擎。 一、原直观解析 / 概念介绍 1.1 基础原理 该库本质上是一个 shelf 处理函数（Handler）
【Spring Boot开发实战手册】掌握Springboot开发技巧和窍门（十三）前端匹配界面、后端匹配WebSocket

【Spring Boot开发实战手册】掌握Springboot开发技巧和窍门（十三）前端匹配界面、后端匹配WebSocket

前言 在现代 Web 开发中，前端和后端的协作变得越来越重要，特别是在需要实时交互和数据更新的应用场景中。WebSocket 技术作为一种全双工通信协议，使得前端和后端之间的实时数据传输变得更加高效和稳定。本篇博客将会探讨如何设计和实现一个实时匹配系统，其中前端负责展示用户界面并与后端进行交互，而后端则通过 WebSocket 协议来处理数据通信。 前端 onMounted: 当组件被挂载的时候执行的函数 onUnmonted: 当组件被卸载的时候执行的函数 初步调试阶段，我们是将token传进user.id的 store/pk.js: import ModuleUser from'./user'exportdefault{state:{socket:null,//ws链接opponent_username:"",opponent_photo:"",status:"matching",//matching表示匹配界面，playing表示对战界面},getters:
前端引入的JS加载失败页面功能无法使用？JS加载失败的终极解决方案

前端引入的JS加载失败页面功能无法使用？JS加载失败的终极解决方案

🌷 古之立大事者，不惟有超世之才，亦必有坚忍不拔之志 🎐 个人CSND主页——Micro麦可乐的博客 🐥《Docker实操教程》专栏以最新的Centos版本为基础进行Docker实操教程，入门到实战 🌺《RabbitMQ》专栏19年编写主要介绍使用JAVA开发RabbitMQ的系列教程，从基础知识到项目实战 🌸《设计模式》专栏以实际的生活场景为案例进行讲解，让大家对设计模式有一个更清晰的理解 🌛《开源项目》本专栏主要介绍目前热门的开源项目，带大家快速了解并轻松上手使用 🍎 《前端技术》专栏以实战为主介绍日常开发中前端应用的一些功能以及技巧，均附有完整的代码示例 ✨《开发技巧》本专栏包含了各种系统的设计原理以及注意事项，并分享一些日常开发的功能小技巧 💕《Jenkins实战》专栏主要介绍Jenkins+Docker的实战教程，让你快速掌握项目CI/CD，是2024年最新的实战教程 🌞《Spring Boot》专栏主要介绍我们日常工作项目中经常应用到的功能以及技巧，代码样例完整 👍《Spring Security》专栏中我们将逐步深入Spring Security的各个
Web Worker 性能优化实战：将计算密集型逻辑从主线程剥离的正确姿势

Web Worker 性能优化实战：将计算密集型逻辑从主线程剥离的正确姿势

在前端开发中，用户体验的流畅度往往取决于“主线程”的响应速度。然而，随着 Web 应用功能的日益复杂，浏览器在处理图像处理、大型二维码生成或复杂数据转换时，常常会出现页面瞬时卡顿甚至假死。 欢迎访问我的个人网站 https://hixiaohezi.com 为了解决这一痛点，Web Worker 成为了一种不可或缺的性能优化方案。 一、 单线程的困局 JavaScript 在浏览器中是单线程执行的。这意味着渲染、交互和脚本逻辑共享同一个“生命通道”。当某个计算任务运行时间超过 16.7ms（即 60FPS 下的一帧）时，浏览器就无法及时响应用户的输入或更新 UI，从而造成明显的掉帧或假死。 二、 Web Worker 是什么？ Web Worker 是 HTML5 标准引入的一项技术，允许在后台线程中运行 JavaScript 脚本。它具有以下核心特点： 1.