无人机双环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

从Dugoff到魔术公式：轮胎模型在车辆动力学中的演进与融合

从Dugoff到魔术公式：轮胎模型在车辆动力学中的演进与融合 轮胎模型是车辆动力学仿真的基石，其精度和效率直接影响整车控制的可靠性与实时性。无论是学术研究还是工程开发，选择合适的轮胎模型都至关重要。Dugoff模型以其简洁的数学形式和良好的实时性受到广泛关注，而魔术公式（Magic Formula）则凭借高精度和广泛适用性成为工业标准之一。两者各有优劣，也分别适用于不同的场景。随着自动驾驶和电动化技术的快速发展，对轮胎模型提出了更高要求——既需要在常规工况下保持高效稳定，又要在极限条件下具备可靠的预测能力。因此，深入理解不同轮胎模型的特点，探索其融合与改进的可能性，已成为当前研究的热点。 1. 轮胎模型的理论基础与发展脉络 轮胎模型的发展经历了从经验模型到物理模型，再到混合模型的演进过程。早期的轮胎模型多基于简单的线性假设，如小侧偏角下的侧向力与侧偏角成线性关系。然而，实际驾驶中轮胎受力涉及复杂的非线性行为，尤其是在大滑移率或大侧偏角工况下，线性模型的误差显著。这促使研究者开发出更为复杂的模型，其中Dugoff模型和魔术公式是两类具有代表性的方法。 Dugoff模型由H. D
Vivado 使用教程

Vivado 使用教程

目录 一、创建工程 二、创建文件 三、编写代码 四、仿真验证 五、配置管脚 六、生成Bitstream文件并烧录 一、创建工程 1.左边创建（或打开）工程，右侧可以快速打开最近打开过的工程。 2.来到这一步，命名工程并设置工程的存放路径（这里以D触发器为例） 3.选择RTL点击next。会来到添加文件环节（可以在这里添加.v等文件，不过后面再添加是一样的）直接点击next。 4.选择芯片型号（根据开发板选，这里随便选的），完成后点next会弹出信息概要，finish完成。         二、创建文件 完成上述步骤会进入当前界面： 1.工程管理器add sourse添加（创建）设计文件，创建文件后选择Verilog语言并命名。 2.定义端口（可选），若在这定义后，
用OpenClaw做飞书ai办公机器人（含本地ollama模型接入+自动安装skills+数据可视化）

用OpenClaw做飞书ai办公机器人（含本地ollama模型接入+自动安装skills+数据可视化）

执行git clone https://github.com/openclaw/openclaw克隆项目，执行cd openclaw进入项目 执行node --version看看node的版本是否大于等于22（没有node.js需自行安装），再执行npm install -g pnpm安装作为包管理器，并执行pnpm install安装依赖 首次执行pnpm ui:build构建 Web UI（会先安装 ui/ 目录的依赖） 执行pnpm build构建主程序 执行pnpm openclaw onboard --install-daemon运行配置向导（安装守护进程），完成初始化 按键盘右箭头选择Yes，同样Yes 任选一个模型提供商都行，没有对应的提供商的密钥可以跳过，如果是本地模型选vLLM（需用vLLM框架启动模型，有性能优势，但原生vLLM仅完全支持Linux的cuda）、Custom Provider（可以连接任何 OpenAI 或 Anthropic 兼容的端点，
【征文计划】AR健身教练：形随心动 - 基于Rokid CXR-M SDK的实践落地

【征文计划】AR健身教练：形随心动 - 基于Rokid CXR-M SDK的实践落地

一、项目背景与创意起源 在当今快节奏的都市生活中，健身已成为许多人保持健康的重要方式。然而，居家健身面临一个普遍痛点：缺乏专业指导，容易因动作不规范导致运动损伤，同时低头看手机或平板的体验也大大降低了健身的沉浸感和效率。 根据《2024年中国健身行业白皮书》显示，超过65%的居家健身用户表示"缺乏专业指导"是他们放弃健身的主要原因。而Rokid Glasses作为一款轻量级AR眼镜，其独特的"抬头即见"交互方式，为解决这一问题提供了绝佳的硬件基础。 "形随心动"创意的诞生源于一个简单但关键的观察：如果能将专业教练"投射"到用户视野中，实时指导动作，同时提供直观的数据反馈，那么居家健身体验将发生质的飞跃。通过Rokid CXR-M SDK的AI场景、自定义页面和提词器功能，我们能够实现这一愿景。 二、Rokid CXR-M SDK 相关 1. Rokid