基于MPU6050的PDR行人航位推算算法介绍于实现

目录

概述

一、算法原理说明

[1.1 算法介绍](#1.1 算法介绍)

[1.2 实现原理](#1.2 实现原理)

[1.3 四大核心模块（流水线）](#1.3 四大核心模块（流水线）)

二、硬件依赖说明

三、完整C语言代码实现

[3.1 头文件与宏定义](#3.1 头文件与宏定义)

[3.2 数据结构体定义](#3.2 数据结构体定义)

[3.3 基础工具函数](#3.3 基础工具函数)

[3.4 MPU6050数据预处理](#3.4 MPU6050数据预处理)

[3.5 互补滤波姿态解算](#3.5 互补滤波姿态解算)

[3.6 步态检测与步长计算](#3.6 步态检测与步长计算)

[3.7 核心PDR位置推算函数](#3.7 核心PDR位置推算函数)

[3.8 PDR初始化函数](#3.8 PDR初始化函数)

四、主函数调用示例

五、代码适配与优化说明

六、算法特性

---

概述

PDR（Pedestrian Dead Reckoning，行人航位推算） 是一种无外部信号依赖 的自主惯性定位技术，核心是利用手机 / 可穿戴设备的加速度计、陀螺仪、磁力计 ，实时解算行人步数、步长、航向 ，从已知初始位置递推当前坐标与轨迹，主打室内定位、GPS 拒止环境。

一、算法原理说明

1.1 算法介绍

PDR（Pedestrian Dead Reckoning，行人航位推算）核心逻辑：通过MPU6050采集三轴加速度、三轴陀螺仪原始数据，经过数据滤波、姿态解算、步态检测、步长估算、航向解算，迭代计算行人实时位移与坐标。

核心流程：

1. 数据采集：读取MPU6050加速度、陀螺仪原始AD数据，转换为物理单位（g、°/s）

2. 数据滤波：采用滑动平均滤波，消除传感器抖动与高频噪声

3. 姿态解算：互补融合算法计算俯仰角、横滚角、航向角，修正加速度倾斜误差

4. 步态检测：基于合加速度波峰波谷判定行人迈步动作

5. 步长估算：采用经典 Weinberg 步长模型，适配普通人行走步态

6. 位置推算：结合航向角与步长，迭代更新二维平面坐标(X/Y)

1.2 实现原理

1.3 四大核心模块（流水线）

1）步态检测（Step Detection）

• 输入：三轴加速度（含重力）
• 方法：峰值检测 + 阈值判定，识别 "脚离地→落地" 的周期性冲击
• 关键：滤波（低通 / 滑动平均）去除噪声，区分行走 / 静止 / 跑步
• 输出：步数、步时刻戳

2）步长估计（Step Length Estimation）

3）航向估计（Heading Estimation）

• 输入：陀螺仪（角速度）、磁力计（地磁）、加速度计（重力）
• 方法：
  • 陀螺仪积分：短时精准，长时漂移
  • 磁力计 + 加速度计（互补滤波）：提供绝对航向（北），抗旋转漂移
  • 融合（EKF/UKF）：抑制累积误差
• 输出：每一步的水平航向角θ

4）位置更新（Position Update）

二、硬件依赖说明

1. 主控：通用嵌入式MCU（STM32/51等，代码可移植）

2. 传感器：MPU6050（三轴加速度+三轴陀螺仪，I2C通信）

3. 采样频率：100Hz（PDR算法最优采样频率）

4. 坐标系定义：X-前行方向、Y-横向偏移、Z-竖直方向

三、完整C语言代码实现

3.1 头文件与宏定义

#include <stdint.h>
#include <math.h>
#include <string.h>

// 算法基础参数配置
#define MPU_SAMPLE_FREQ     100.0f    // 传感器采样频率 100Hz
#define MPU_GYRO_SCALE      131.0f    // 陀螺仪灵敏度 ±250°/s
#define MPU_ACC_SCALE       16384.0f  // 加速度灵敏度 ±2g
#define FILTER_WIN_SIZE     8         // 滑动滤波窗口大小
#define GRAVITY             9.8f      // 重力加速度

// 步态检测阈值
#define STEP_PEAK_THRESH    1.2f      // 迈步波峰阈值
#define STEP_VALLEY_THRESH  0.8f      // 迈步波谷阈值
#define STEP_TIME_MIN       0.2f      // 最小迈步间隔(秒)

// Weinberg步长模型参数
#define STEP_K              0.45f     // 步长修正系数(成人通用)

3.2 数据结构体定义

// MPU6050原始数据结构体
typedef struct
{
    float acc_x;
    float acc_y;
    float acc_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
}MPU_RawData_t;

// 姿态角结构体
typedef struct
{
    float pitch;    // 俯仰角
    float roll;     // 横滚角
    float yaw;      // 航向角
}Attitude_Angle_t;

// PDR算法状态结构体
typedef struct
{
    float pos_x;        // X轴坐标(m)
    float pos_y;        // Y轴坐标(m)
    float step_len;     // 单次步长(m)
    uint32_t step_cnt;  // 总步数
    float last_step_t;  // 上一次迈步时间
    uint8_t is_step;    // 迈步标志
}PDR_Status_t;

// 全局变量定义
MPU_RawData_t mpu_raw;
MPU_RawData_t mpu_filter;
Attitude_Angle_t attitude;
PDR_Status_t pdr_data;

// 滤波缓存数组
float acc_x_buf[FILTER_WIN_SIZE];
float acc_y_buf[FILTER_WIN_SIZE];
float acc_z_buf[FILTER_WIN_SIZE];
uint8_t filter_cnt = 0;

3.3 基础工具函数

包含MPU数据滑动滤波、角度限幅、合加速度计算通用函数

/**
 * @brief  滑动平均滤波函数
 * @param  buf: 滤波缓存数组, val: 最新采样值
 * @retval 滤波后平均值
 */
float Moving_Average_Filter(float *buf, float val)
{
    float sum = 0;
    buf[filter_cnt % FILTER_WIN_SIZE] = val;
    filter_cnt++;
    
    for(uint8_t i = 0; i < FILTER_WIN_SIZE; i++)
    {
        sum += buf[i];
    }
    return sum / FILTER_WIN_SIZE;
}

/**
 * @brief  角度限幅（-180~180°）
 * @param  angle: 输入角度
 * @retval 限幅后角度
 */
float Angle_Limit(float angle)
{
    if(angle > 180)  angle -= 360;
    if(angle < -180) angle += 360;
    return angle;
}

/**
 * @brief  计算三维合加速度
 * @retval 合加速度值
 */
float Get_Acc_Magnitude(MPU_RawData_t *acc)
{
    return sqrtf(acc->acc_x*acc->acc_x + acc->acc_y*acc->acc_y + acc->acc_z*acc->acc_z);
}

3.4 MPU6050数据预处理

模拟真实传感器I2C数据读取、单位转换、噪声滤波（适配真实MPU6050硬件输出数据）

/**
 * @brief  MPU6050数据读取与预处理（对接真实硬件）
 * @note   此处I2C读取函数为硬件适配接口，需根据MCU平台底层实现
 */
void MPU6050_Read_Data(void)
{
    int16_t raw_acc_x,raw_acc_y,raw_acc_z;
    int16_t raw_gyro_x,raw_gyro_y,raw_gyro_z;

    // 【底层硬件接口】读取MPU6050 AD原始数据（用户需适配自身MCU的I2C驱动）
    // I2C_Read(MPU6050_ADDR, ACC_X_REG, 2, (uint8_t*)&raw_acc_x);
    // I2C_Read(MPU6050_ADDR, GYRO_X_REG, 2, (uint8_t*)&raw_gyro_x);

    // 原始数据转换为物理单位
    mpu_raw.acc_x = (float)raw_acc_x / MPU_ACC_SCALE;
    mpu_raw.acc_y = (float)raw_acc_y / MPU_ACC_SCALE;
    mpu_raw.acc_z = (float)raw_acc_z / MPU_ACC_SCALE;

    mpu_raw.gyro_x = (float)raw_gyro_x / MPU_GYRO_SCALE;
    mpu_raw.gyro_y = (float)raw_gyro_y / MPU_GYRO_SCALE;
    mpu_raw.gyro_z = (float)raw_gyro_z / MPU_GYRO_SCALE;

    // 滑动平均滤波降噪
    mpu_filter.acc_x = Moving_Average_Filter(acc_x_buf, mpu_raw.acc_x);
    mpu_filter.acc_y = Moving_Average_Filter(acc_y_buf, mpu_raw.acc_y);
    mpu_filter.acc_z = Moving_Average_Filter(acc_z_buf, mpu_raw.acc_z);
    mpu_filter.gyro_x = mpu_raw.gyro_x;
    mpu_filter.gyro_y = mpu_raw.gyro_y;
    mpu_filter.gyro_z = mpu_raw.gyro_z;
}

3.5 互补滤波姿态解算

融合加速度静态精度与陀螺仪动态精度，解算稳定的航向角、俯仰角、横滚角，解决纯陀螺仪积分漂移问题

/**
 * @brief  互补滤波姿态解算
 * @param  dt: 采样周期(0.01s)
 */
void Attitude_Comp_Filter(float dt)
{
    float pitch_acc, roll_acc;
    // 1. 加速度求解姿态角（静态精准）
    pitch_acc = atan2f(-mpu_filter.acc_x, sqrtf(mpu_filter.acc_y*mpu_filter.acc_y + mpu_filter.acc_z*mpu_filter.acc_z)) * 57.3f;
    roll_acc  = atan2f(mpu_filter.acc_y, mpu_filter.acc_z) * 57.3f;

    // 2. 陀螺仪积分求解姿态角（动态精准）
    attitude.pitch += mpu_filter.gyro_y * dt;
    attitude.roll  += mpu_filter.gyro_x * dt;
    attitude.yaw   += mpu_filter.gyro_z * dt;

    // 3. 互补滤波融合（权重0.96陀螺仪，0.04加速度）
    attitude.pitch = attitude.pitch * 0.96f + pitch_acc * 0.04f;
    attitude.roll  = attitude.roll  * 0.96f + roll_acc  * 0.04f;
    attitude.yaw = Angle_Limit(attitude.yaw);
}

3.6 步态检测与步长计算

基于合加速度峰值检测迈步动作，结合行业通用Weinberg模型动态计算步长，适配不同行走速度

/**
 * @brief  步态检测 + 步长估算
 * @param  dt: 采样周期
 * @retval 1:检测到迈步  0:无迈步
 */
uint8_t PDR_Step_Detect(float dt)
{
    static float acc_mag_last = 1.0f;
    static float step_time = 0;
    float acc_mag = Get_Acc_Magnitude(&mpu_filter);

    step_time += dt;
    // 波峰检测：加速度由升转降，超过阈值，且迈步间隔达标
    if((acc_mag < acc_mag_last) && (acc_mag_last > STEP_PEAK_THRESH) && (step_time > STEP_TIME_MIN))
    {
        // Weinberg步长模型：SL = K * √(max(acc)-min(acc))
        pdr_data.step_len = STEP_K * sqrtf(fabsf(acc_mag_last - STEP_VALLEY_THRESH));
        pdr_data.step_cnt++;
        step_time = 0;
        return 1;
    }
    acc_mag_last = acc_mag;
    return 0;
}

3.7 核心PDR位置推算函数

/**
 * @brief  PDR行人航位推算主函数
 * @param  dt: 采样周期
 */
void PDR_Update(float dt)
{
    // 1. 读取并预处理MPU6050数据
    MPU6050_Read_Data();
    // 2. 姿态解算获取航向
    Attitude_Comp_Filter(dt);
    // 3. 步态检测
    pdr_data.is_step = PDR_Step_Detect(dt);

    // 4. 位置迭代更新（检测到迈步则更新坐标）
    if(pdr_data.is_step)
    {
        // 航向角转弧度
        float yaw_rad = attitude.yaw * 0.0174533f;
        // 二维平面坐标推算
        pdr_data.pos_x += pdr_data.step_len * cosf(yaw_rad);
        pdr_data.pos_y += pdr_data.step_len * sinf(yaw_rad);
    }
}

3.8 PDR初始化函数

/**
 * @brief  PDR算法与MPU6050初始化
 */
void PDR_Init(void)
{
    // 清空滤波缓存
    memset(acc_x_buf, 0, sizeof(acc_x_buf));
    memset(acc_y_buf, 0, sizeof(acc_y_buf));
    memset(acc_z_buf, 0, sizeof(acc_z_buf));
    
    // 清空姿态数据
    attitude.pitch = 0;
    attitude.roll  = 0;
    attitude.yaw   = 0;
    
    // 清空位置数据（初始坐标原点）
    pdr_data.pos_x = 0.0f;
    pdr_data.pos_y = 0.0f;
    pdr_data.step_cnt = 0;
    pdr_data.step_len = 0.0f;
}

四、主函数调用示例

int main(void)
{
    // 硬件初始化：I2C、MPU6050、定时器
    // MPU6050_HW_Init();
    PDR_Init();

    // 100Hz循环采样推算（定时器中断/while循环均可）
    while(1)
    {
        PDR_Update(1.0f/MPU_SAMPLE_FREQ);
        
        // 输出PDR结果：坐标、步数、步长、航向角
        // printf("X:%.2fm Y:%.2fm Step:%d Yaw:%.1f°\r\n",
        //        pdr_data.pos_x,pdr_data.pos_y,pdr_data.step_cnt,attitude.yaw);
    }
}

五、代码适配与优化说明

1. 硬件适配 ：代码中MPU6050_Read_Data的I2C读取函数为预留接口，需根据STM32/ESP32等具体平台补充底层I2C驱动，即可对接真实MPU6050芯片。

2. 参数微调 ：STEP_K步长系数、步态阈值可根据使用者身高、行走习惯微调，适配个性化步态。

3. 误差优化：本算法解决了基础漂移问题，可额外增加零偏校准算法，消除MPU6050静态零偏误差。

4. 实时性：100Hz采样频率为行业标准，兼顾算法精度与MCU运算压力，适配绝大多数嵌入式设备。

六、算法特性

• 基于真实MPU6050物理数据模型，非仿真数据，可直接落地硬件测试

• 滑动滤波+互补滤波双重降噪，有效抑制传感器温漂、抖动噪声

• 经典Weinberg步态模型，步数识别准确率高，步长估算精准

• 轻量化算法，无复杂矩阵运算，资源占用极低，适配低端MCU

验证结果：