目录
1. 融合原理
GPS 与光流的特性对比
融合目标
2. EKF 设计（二维位置融合）
状态向量
状态方程（预测）
观测方程（更新）
GPS 观测矩阵：
光流观测矩阵：
3. 代码实现（STM32 HAL 库）
4. 集成到无人机悬停控制
数据读取与融合流程
5. 参数调优与测试建议
参数初始值
调优步骤
我们在之前的位置 + 姿态双环 PID基础上，增加GPS + 光流融合的扩展卡尔曼滤波（EKF），用于高精度位置估计，从而提升无人机悬停精度到±0.5 米以内。
我会给你一个完整的融合方案，包括：
- 融合原理（GPS 与光流的互补性）
- EKF 状态方程与观测方程设计
- 代码实现（STM32 HAL 库）
- 参数调优与测试建议
1. 融合原理
GPS 与光流的特性对比
| 传感器 | 优势 | 劣势 |
|---|---|---|
| GPS | 全局绝对位置，无累计误差 | 噪声大（±2~5 米），刷新率低（1~10Hz） |
| 光流 | 高刷新率（50~200Hz），相对位移精度高 | 有累计误差，依赖高度信息，无全局定位 |
融合目标
- 短期：用光流的高刷新率数据保持位置跟踪稳定性。
- 长期：用 GPS 的绝对位置修正光流的累计误差。
- 结果：输出一个低噪声、高刷新率、无累计误差的位置估计。
2. EKF 设计（二维位置融合）
状态向量
假设我们只关心水平位置（X, Y），状态向量为：X=xyvxvy其中：
- x,y：位置（米）
- vx,vy：速度（米 / 秒）
状态方程（预测）
基于匀速模型：Xk∣k−1=F⋅Xk−1∣k−1+wk状态转移矩阵：F=10000100dt0100dt01过程噪声协方差矩阵 Q：Q=σx20000σy20000σvx20000σvy2
观测方程（更新）
有两个观测源：
- GPS：直接观测位置 (xgps,ygps)
- 光流：观测位移 (Δxflow,Δyflow)，需转换为速度或位置增量
GPS 观测矩阵：
Hgps=[10010000]观测噪声协方差矩阵 Rgps：Rgps=[σgps,x200σgps,y2]
光流观测矩阵：
光流输出位移 Δxflow,Δyflow，假设采样时间为 dt，则速度观测为：vflow,x=dtΔxflow,vflow,y=dtΔyflow观测矩阵：Hflow=[00001001]观测噪声协方差矩阵 Rflow：Rflow=[σflow,x200σflow,y2]
3. 代码实现（STM32 HAL 库）
#include <math.h> // EKF 状态向量 typedef struct { float x, y; // 位置 (m) float vx, vy; // 速度 (m/s) } EKF_State; // EKF 协方差矩阵 typedef struct { float P[4][4]; } EKF_Covariance; // 传感器数据 typedef struct { float x, y; // GPS 位置 (m) } GPS_Data; typedef struct { float dx, dy; // 光流位移 (m) } Flow_Data; // EKF 参数 typedef struct { EKF_State state; EKF_Covariance cov; float dt; // 采样时间 (s) // 过程噪声 float Q_pos; // 位置过程噪声方差 float Q_vel; // 速度过程噪声方差 // 观测噪声 float R_gps_pos; // GPS 位置噪声方差 float R_flow_vel; // 光流速度噪声方差 } EKF; // 初始化 EKF void EKF_Init(EKF *ekf, float dt) { ekf->dt = dt; ekf->Q_pos = 0.01f; // 位置过程噪声 ekf->Q_vel = 0.1f; // 速度过程噪声 ekf->R_gps_pos = 4.0f; // GPS 位置噪声方差 (2m^2) ekf->R_flow_vel = 0.01f; // 光流速度噪声方差 (0.1m/s)^2 // 初始状态 ekf->state.x = 0.0f; ekf->state.y = 0.0f; ekf->state.vx = 0.0f; ekf->state.vy = 0.0f; // 初始协方差 for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ekf->cov.P[i][j] = 0.0f; } } ekf->cov.P[0][0] = 1.0f; // 初始位置不确定度 ekf->cov.P[1][1] = 1.0f; ekf->cov.P[2][2] = 1.0f; // 初始速度不确定度 ekf->cov.P[3][3] = 1.0f; } // 预测步骤 void EKF_Predict(EKF *ekf) { // 状态预测 ekf->state.x += ekf->state.vx * ekf->dt; ekf->state.y += ekf->state.vy * ekf->dt; // 协方差预测 float F[4][4] = { {1, 0, ekf->dt, 0}, {0, 1, 0, ekf->dt}, {0, 0, 1, 0}, {0, 0, 0, 1} }; float Q[4][4] = { {ekf->Q_pos, 0, 0, 0}, {0, ekf->Q_pos, 0, 0}, {0, 0, ekf->Q_vel, 0}, {0, 0, 0, ekf->Q_vel} }; // P = F*P*F^T + Q float P_temp[4][4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 4; k++) { P_temp[i][j] += F[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ekf->cov.P[i][j] = 0.0f; for (int k = 0; k < 4; k++) { ekf->cov.P[i][j] += P_temp[i][k] * F[j][k]; } ekf->cov.P[i][j] += Q[i][j]; } } } // 更新步骤（GPS） void EKF_Update_GPS(EKF *ekf, GPS_Data *gps) { // 观测矩阵 H = [1,0,0,0; 0,1,0,0] float H[2][4] = { {1, 0, 0, 0}, {0, 1, 0, 0} }; // 观测噪声 R float R[2][2] = { {ekf->R_gps_pos, 0}, {0, ekf->R_gps_pos} }; // 残差 z - H*x float z[2] = {gps->x, gps->y}; float z_pred[2] = {ekf->state.x, ekf->state.y}; float y[2]; y[0] = z[0] - z_pred[0]; y[1] = z[1] - z_pred[1]; // S = H*P*H^T + R float S[2][2]; float H_P[2][4]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 4; j++) { H_P[i][j] = 0.0f; for (int k = 0; k < 4; k++) { H_P[i][j] += H[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { S[i][j] = 0.0f; for (int k = 0; k < 4; k++) { S[i][j] += H_P[i][k] * H[j][k]; } S[i][j] += R[i][j]; } } // 卡尔曼增益 K = P*H^T*S^{-1} float S_inv[2][2]; float det = S[0][0] * S[1][1] - S[0][1] * S[1][0]; S_inv[0][0] = S[1][1] / det; S_inv[0][1] = -S[0][1] / det; S_inv[1][0] = -S[1][0] / det; S_inv[1][1] = S[0][0] / det; float K[4][2]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 2; j++) { K[i][j] = 0.0f; for (int k = 0; k < 4; k++) { K[i][j] += ekf->cov.P[i][k] * H[j][k]; } } } for (int i = 0; i < 4; i++) { for (int j = 0; j < 2; j++) { float temp = 0.0f; for (int k = 0; k < 2; k++) { temp += K[i][k] * S_inv[k][j]; } K[i][j] = temp; } } // 更新状态 for (int i = 0; i < 4; i++) { float delta = 0.0f; for (int j = 0; j < 2; j++) { delta += K[i][j] * y[j]; } if (i == 0) ekf->state.x += delta; else if (i == 1) ekf->state.y += delta; else if (i == 2) ekf->state.vx += delta; else if (i == 3) ekf->state.vy += delta; } // 更新协方差 P = (I - K*H)*P float I_KH[4][4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { I_KH[i][j] = (i == j) ? 1.0f : 0.0f; for (int k = 0; k < 2; k++) { I_KH[i][j] -= K[i][k] * H[k][j]; } } } float P_temp[4][4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 4; k++) { P_temp[i][j] += I_KH[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ekf->cov.P[i][j] = P_temp[i][j]; } } } // 更新步骤（光流） void EKF_Update_Flow(EKF *ekf, Flow_Data *flow) { // 光流位移转速度 float vx_flow = flow->dx / ekf->dt; float vy_flow = flow->dy / ekf->dt; // 观测矩阵 H = [0,0,1,0; 0,0,0,1] float H[2][4] = { {0, 0, 1, 0}, {0, 0, 0, 1} }; // 观测噪声 R float R[2][2] = { {ekf->R_flow_vel, 0}, {0, ekf->R_flow_vel} }; // 残差 z - H*x float z[2] = {vx_flow, vy_flow}; float z_pred[2] = {ekf->state.vx, ekf->state.vy}; float y[2]; y[0] = z[0] - z_pred[0]; y[1] = z[1] - z_pred[1]; // S = H*P*H^T + R float S[2][2]; float H_P[2][4]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 4; j++) { H_P[i][j] = 0.0f; for (int k = 0; k < 4; k++) { H_P[i][j] += H[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { S[i][j] = 0.0f; for (int k = 0; k < 4; k++) { S[i][j] += H_P[i][k] * H[j][k]; } S[i][j] += R[i][j]; } } // 卡尔曼增益 K = P*H^T*S^{-1} float S_inv[2][2]; float det = S[0][0] * S[1][1] - S[0][1] * S[1][0]; S_inv[0][0] = S[1][1] / det; S_inv[0][1] = -S[0][1] / det; S_inv[1][0] = -S[1][0] / det; S_inv[1][1] = S[0][0] / det; float K[4][2]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 2; j++) { K[i][j] = 0.0f; for (int k = 0; k < 4; k++) { K[i][j] += ekf->cov.P[i][k] * H[j][k]; } } } for (int i = 0; i < 4; i++) { for (int j = 0; j < 2; j++) { float temp = 0.0f; for (int k = 0; k < 2; k++) { temp += K[i][k] * S_inv[k][j]; } K[i][j] = temp; } } // 更新状态 for (int i = 0; i < 4; i++) { float delta = 0.0f; for (int j = 0; j < 2; j++) { delta += K[i][j] * y[j]; } if (i == 0) ekf->state.x += delta; else if (i == 1) ekf->state.y += delta; else if (i == 2) ekf->state.vx += delta; else if (i == 3) ekf->state.vy += delta; } // 更新协方差 P = (I - K*H)*P float I_KH[4][4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { I_KH[i][j] = (i == j) ? 1.0f : 0.0f; for (int k = 0; k < 2; k++) { I_KH[i][j] -= K[i][k] * H[k][j]; } } } float P_temp[4][4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 4; k++) { P_temp[i][j] += I_KH[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { ekf->cov.P[i][j] = P_temp[i][j]; } } }4. 集成到无人机悬停控制
数据读取与融合流程
EKF ekf; GPS_Data gps; Flow_Data flow; void Hover_Control_Loop(void) { float dt = 0.01f; // 100Hz // 1. 读取传感器数据 read_gps(&gps); // GPS 位置 (m) read_optical_flow(&flow); // 光流位移 (m) // 2. EKF 预测 EKF_Predict(&ekf); // 3. EKF 更新（GPS 每秒一次，光流每次循环） static uint32_t gps_update_count = 0; if (gps_update_count++ >= 100) { // 100Hz * 0.01s = 1s EKF_Update_GPS(&ekf, &gps); gps_update_count = 0; } EKF_Update_Flow(&ekf, &flow); // 4. 位置环 PID（使用 EKF 输出） pid_pos_x.setpoint = target_pos.x; pid_pos_x.feedback = ekf.state.x; PID_Update(&pid_pos_x, dt); pid_pos_y.setpoint = target_pos.y; pid_pos_y.feedback = ekf.state.y; PID_Update(&pid_pos_y, dt); // 5. 姿态环与电机控制（同上） ... }5. 参数调优与测试建议
参数初始值
- 过程噪声：
Q_pos = 0.01：位置过程噪声较小，假设无人机运动平稳。Q_vel = 0.1：速度过程噪声较大，允许速度变化。
- 观测噪声：
R_gps_pos = 4.0：GPS 噪声方差（2m）。R_flow_vel = 0.01：光流速度噪声方差（0.1m/s）。
调优步骤
- 室内测试：关闭 GPS 更新，仅用光流，调整
Q和R_flow使位置跟踪稳定。 - 室外测试：开启 GPS 更新，调整
R_gps使 GPS 修正不过于频繁或剧烈。 - 最终验证：悬停时位置误差应控制在±0.5 米以内。
