【STM32硬核实战】基于光流+陀螺仪的“无磁”惯性导航轨迹记录仪

摘要：在室内无 GPS 且存在强磁干扰（无法使用磁力计）的环境下，如何精准记录物体的移动轨迹？本文提出了一种基于 STM32F103 的低成本航位推算（Dead Reckoning）方案。通过融合 PMW3901 光流传感器（测量位移）与 MPU6050 陀螺仪（测量航向），并配合 VL6180 ToF 测距（检测抬起状态），实现了在任意纹理平面上的二维轨迹重现。

关键词：STM32, 光流定位, PMW3901, MPU6050 DMP, 传感器融合, 航位推算

---

1. 项目背景与痛点

在机器人室内定位、空中鼠标或手持测量设备中，我们常面临以下痛点：

1. GPS 失效：室内无法接收卫星信号。

2. 磁干扰严重：在电机、扬声器或铁磁性物质附近，电子罗盘（磁力计）的数据不可用。

3. 累积误差：单纯使用加速度计进行二次积分求位移，几秒钟后漂移就会达到数米，完全不可用。

解决方案：采用类似“光电鼠标”的原理，利用光流传感器获取相对位移，但光流传感器无法感知自身的旋转。因此，我们需要引入高精度的陀螺仪来修正坐标系，从而在全局坐标系下还原出真实的运动轨迹。

---

2. 硬件架构与选型

系统采用模块化设计，主控为 STM32F103C8T6，挂载三个核心传感器。

模块名称	核心芯片	作用	通讯接口	选型理由
主控	STM32F103C8T6	传感器数据读取与算法融合	-	资源丰富，支持浮点运算（软解）
位移传感器	PMW3901	测量 X/Y 轴的像素级位移	SPI	相比传统鼠标传感器，不需要特制鼠标垫，适应性强
姿态传感器	MPU6050	测量 Yaw (航向角)	I2C	利用内置 DMP (数字运动处理器) 输出稳定的欧拉角
状态传感器	VL6180	激光测距 (0-10cm)	I2C	用于检测设备是否“离地”，实现智能暂停
显示交互	0.96寸 OLED	实时显示坐标与状态	I2C	方便离线调试

硬件连接示意 (Pin Map)

• SPI 总线 (高速)：PA4 (CS), PA5 (SCK), PA6 (MISO), PA7 (MOSI) -> PMW3901

• I2C 总线 (模拟)：PB10 (SCL), PB11 (SDA) -> MPU6050, VL6180, OLED

  • 注意：由于挂载多个 I2C 设备，SCL 和 SDA 线必须接 4.7kΩ 上拉电阻，否则会导致通讯不稳定。

---

3. 核心传感器解析：PMW3901 光流模块

3.1 什么是光流（Optical Flow）？

在计算机视觉中，光流是指图像亮度模式在观察者视角下的视在运动。简单来说，PMW3901 就是一个每秒高速拍摄几百张照片的微型相机。

它内部集成了 DSP（数字信号处理）芯片，通过对比前后两帧图像纹理的移动，直接输出 X 轴和 Y 轴的像素位移量（Delta X, Delta Y）。这与光电鼠标的原理完全一致，但 PMW3901 专为无人机悬停设计，因此对不同表面的适应性更强（地砖、地毯、皮肤、混凝土均可）。

3.2 为什么选择 PMW3901？

• 体积小：适合手持设备。

• 接口简单：标准 4 线 SPI 接口。

• 无需信标：不需要像 UWB 那样布置基站，也不像 GPS 那样依赖卫星，完全自主定位。

• 数据特性：输出的是相对位移增量，这正是航位推算（Dead Reckoning）所需要的原材料。

---

3.3 驱动层代码实现

PMW3901 的驱动核心在于 SPI 时序的模拟 以及 寄存器的配置。

注意：PMW3901 对上电时序有严格要求，且内部有约 80 个“魔法寄存器”需要初始化（官方提供的 Performance Optimization 序列），否则性能极差。

A. 底层 SPI 通讯 (MySPI.c)

由于 PMW3901 对 SPI 时序要求较宽容，我们使用软件模拟 SPI 即可，方便移植。

// 软件模拟 SPI 交换字节
// Mode 0: CPOL=0, CPHA=0 (空闲低电平，第一个边沿采样)
uint8_t MySPI_SwapByte(uint8_t ByteSend)
{
    uint8_t i, ByteReceive = 0x00;
    
    for (i = 0; i < 8; i++)
    {
        // 1. 主机准备数据 (MOSI)
        if (ByteSend & 0x80) GPIO_SetBits(GPIOA, GPIO_Pin_7); // MOSI
        else                 GPIO_ResetBits(GPIOA, GPIO_Pin_7);
        ByteSend <<= 1;
        
        // 2. 拉高时钟 (SCK)，从机采样
        GPIO_SetBits(GPIOA, GPIO_Pin_5); // SCK High
        
        // 3. 主机读取数据 (MISO)
        if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6)) // MISO
            ByteReceive |= (0x80 >> i);
            
        // 4. 拉低时钟，准备下一位
        GPIO_ResetBits(GPIOA, GPIO_Pin_5); // SCK Low
    }
    return ByteReceive;
}

B. 寄存器读写封装 (PMW3901.c)

PMW3901 规定：

• 写操作：地址最高位（MSB）为 1。

• 读操作：地址最高位（MSB）为 0。

// 写寄存器
void PMW3901_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    MySPI_Start(); // 拉低 CS
    MySPI_SwapByte(RegAddress | 0x80); // 写操作标志位
    MySPI_SwapByte(Data);
    MySPI_Stop();  // 拉高 CS
    Delay_us(20);  // 必要的延时，防止指令粘连
}

// 读寄存器
uint8_t PMW3901_ReadReg(uint8_t RegAddress)
{
    uint8_t Data;
    MySPI_Start();
    MySPI_SwapByte(RegAddress & 0x7F); // 读操作标志位
    Data = MySPI_SwapByte(0xFF);       // 发送空字节置换回数据
    MySPI_Stop();
    Delay_us(20);
    return Data;
}

C. 获取运动数据 (核心函数)

这是主循环中调用的关键函数。我们需要读取 0x02 (Motion), 0x03 (Delta_XL) 到 0x06 (Delta_YH) 寄存器。

/**
 * @brief  读取光流传感器的位移增量
 * @param  delta_x: 返回 X 轴位移 (指针)
 * @param  delta_y: 返回 Y 轴位移 (指针)
 */
void PMW3901_ReadMotion(int16_t *delta_x, int16_t *delta_y)
{
    // 1. 读取 Motion 寄存器 (0x02) 判断是否有运动
    // Bit 7: Motion Occurred (1=有运动, 0=无)
    uint8_t motion = PMW3901_ReadReg(0x02);
    
    if(motion & 0x80) 
    {
        // 2. 连续读取低八位和高八位
        // 注意：必须先读低位 (L)，再读高位 (H)，否则数据可能锁死
        uint8_t xl = PMW3901_ReadReg(0x03);
        uint8_t xh = PMW3901_ReadReg(0x04);
        uint8_t yl = PMW3901_ReadReg(0x05);
        uint8_t yh = PMW3901_ReadReg(0x06);
        
        // 3. 拼接成 16位 有符号整型
        *delta_x = (int16_t)((xh << 8) | xl);
        *delta_y = (int16_t)((yh << 8) | yl);
    }
    else
    {
        *delta_x = 0;
        *delta_y = 0;
    }
}

---

3.4 光流数据应用的 3 个“坑”

在实际调试中，单纯跑通代码往往是不够的，你可能会遇到以下物理特性带来的问题：

1. 焦距与离地高度 (The Sweet Spot)

PMW3901 通常自带一个透镜。

• 现象：如果传感器紧贴着物体表面（距离 < 2mm），或者离得太远（> 10cm），画面会模糊，导致读数均为 0 或乱跳。

• 解决：确保传感器镜头距离被测表面 10mm ~ 40mm 之间。如果是手持设备，需要在结构上设计一个限位支架。

2. 纹理依赖 (Texture Dependency)

光流依赖“看”到的纹理移动。

• 现象：在纯白纸、镜面或玻璃上移动，数据不动。

• 解决：这在手持设备上通常不是问题（人的皮肤、衣服、木纹桌面的纹理都足够丰富）。但在测试时，请不要在光滑的白色桌面上测试，垫一张报纸效果最好。

3. 坐标系方向 (Orientation)

芯片安装的方向可能与你的定义不同。

• 现象：向前推设备，Y 轴数据却是负的，或者变成了 X 轴数据。

• 解决：

  1. 物理上：将芯片上的 Dot (原点标记) 对准设备车头。

  2. 软件上：通过简单的交换或取反来修正。

  // 简单的坐标系修正示例
  int16_t raw_x, raw_y;
  PMW3901_ReadMotion(&raw_x, &raw_y);
  
  // 假设安装旋转了90度
  *final_dx = -raw_y; 
  *final_dy = raw_x;
  

（VL6180和MPU6050DMP的相关代码我之前都发过）

4. 核心算法原理：带旋转修正的航位推算

光流传感器输出的是机身坐标系下的位移 Delta x, Delta y。当设备发生旋转时，机身的“前方”不再是地图的“北方”。

我们需要利用陀螺仪测得的航向角  (Yaw)，利用旋转矩阵将位移映射到全局坐标系。

4.1 数学模型

即：

4.2 智能启停逻辑 (Lift Detection)

利用 VL6180 测距传感器实现“提笔暂停”功能：

• 有效工作区：当距离 $< 30mm$ 时，认为设备贴合表面，正常积分坐标。

• 悬空区：当距离 $\ge 30mm$ 时，认为设备被拿起，锁定全局坐标，防止光流传感器在空中的噪声导致轨迹乱飞。

---

5. 关键代码实现

5.1 坐标融合算法 (Algorithm.c)

这是整个系统的核心数学逻辑。

#include <math.h>
#include "Algorithm.h"

// 全局位置变量
float Global_X = 0.0f;
float Global_Y = 0.0f;

// 比例尺系数：将光流的脉冲数转换为物理距离(mm)
// 该系数需要根据传感器安装高度进行实测标定
#define SCALE_FACTOR 0.15f 
#define DEG_TO_RAD   0.01745329f // π/180

/**
 * @brief  传感器融合更新位置
 * @param  flow_dx: 光流传感器 X 轴增量
 * @param  flow_dy: 光流传感器 Y 轴增量
 * @param  yaw_deg: 当前相对航向角 (度)
 */
void Fusion_Update_Position(int16_t flow_dx, int16_t flow_dy, float yaw_deg)
{
    // 1. 角度转弧度
    float rad = yaw_deg * DEG_TO_RAD;
    float sin_val = sin(rad);
    float cos_val = cos(rad);

    // 2. 旋转矩阵变换 (机身系 -> 全局系)
    // 注意：根据传感器安装方向，flow_dy 可能需要取反
    float d_world_x = (float)flow_dx * cos_val - (float)flow_dy * sin_val;
    float d_world_y = (float)flow_dx * sin_val + (float)flow_dy * cos_val;

    // 3. 累加全局坐标
    Global_X += d_world_x * SCALE_FACTOR;
    Global_Y += d_world_y * SCALE_FACTOR;
}

5.2 主程序逻辑 (main.c)

主循环采用分时调度策略：优先保证传感器数据读取，降低屏幕刷新频率以减轻 I2C 总线压力。

int main(void)
{
    // 硬件初始化
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    MyI2C_Init();  // 初始化模拟 I2C
    OLED_Init();
    PMW3901_Init();
    
    // MPU6050 DMP 初始化 (注意：需增大 Stack Size)
    OLED_ShowString(0, 0, "DMP Init...", OLED_8X16);
    OLED_Update();
    MPU6050_DMPInit(); 

    // --- 零偏校准 (Zero Calibration) ---
    // 上电后需保持静止，记录初始 Yaw 作为 0 度参考
    OLED_ShowString(0, 0, "Calibrating...", OLED_8X16);
    OLED_Update();
    Delay_ms(3000); // 等待传感器稳定
    
    if (MPU6050_ReadDMP(&Pitch, &Roll, &Yaw) == 0) {
        Yaw_Offset = Yaw; // 记录初始偏移量
    }
    
    // 清空光流传感器积存的历史数据
    int16_t flow_dx, flow_dy;
    PMW3901_ReadMotion(&flow_dx, &flow_dy); 

    while(1)
    {
        // 1. 读取姿态 (DMP中断或轮询)
        if (MPU6050_DataReady) {
            MPU6050_ReadDMP(&Pitch, &Roll, &Yaw);
            relative_yaw = Yaw - Yaw_Offset; // 获取相对角度
            
            // 角度归一化 (-180 ~ +180)
            if(relative_yaw > 180.0f) relative_yaw -= 360.0f;
            if(relative_yaw < -180.0f) relative_yaw += 360.0f;
            
            MPU6050_DataReady = 0;
        }

        // 2. 读取测距与光流 (采样周期约 20ms)
        Delay_ms(20); 
        uint8_t distance = VL6180_Read_Range();
        VL6180_Start_Range(); // 触发下一次测量

        // 3. 轨迹记录判断
        if (distance < 30) // 贴合表面
        {
            PMW3901_ReadMotion(&flow_dx, &flow_dy);
            
            // 简单死区过滤，去除静止噪声
            if (abs(flow_dx) > 1 || abs(flow_dy) > 1) {
                Fusion_Update_Position(flow_dx, flow_dy, relative_yaw);
            }
        }
        else {
            // 悬空状态：只读不记，防止数据积压
            PMW3901_ReadMotion(&flow_dx, &flow_dy); 
        }

        // 4. 数据可视化 (降频刷新)
        static uint8_t show_cnt = 0;
        if(++show_cnt >= 10) { // 200ms 刷新一次
            show_cnt = 0;
            OLED_Printf(0, 0, "A:%4.0f D:%3d", relative_yaw, distance);
            OLED_Printf(0, 16, "X:%6.0f", Global_X);
            OLED_Printf(0, 32, "Y:%6.0f", Global_Y);
            OLED_Update();
        }
    }
}

---

6. 开发避坑指南 (Debug Notes)

在复现该项目时，以下几个问题最为常见：

5.1 程序卡死在 DMPInit

• 现象：OLED 停留在初始化界面，无法进入主循环。

• 原因：STM32 标准库启动文件默认分配的栈空间 (Stack_Size) 只有 0x400 (1KB)，而 MPU6050 的 DMP 库初始化时需要较大的内存缓冲区，导致堆栈溢出 (Stack Overflow)。

• 解决：打开工程中的 startup_stm32f10x_md.s 文件，将 Stack_Size 修改为 0x1000 (4KB)，并在 Keil 设置中勾选 "Use MicroLIB"。

5.2 轨迹漂移问题

• 现象：设备静止时，坐标数值缓慢增加；或上电瞬间坐标跳变。

• 解决：

  1. 启动校准：光流传感器上电瞬间会有初始化噪声，务必在 while(1) 之前读取并丢弃一次数据。

  2. 增加死区：在代码中添加 if (abs(dx) < threshold) dx = 0;。

  3. 零偏处理：严格执行上电前 3 秒静止校准，获取准确的 Yaw_Offset。

5.3 I2C 读出 0xFF

• 原因：软件模拟 I2C 时序对上拉电阻依赖性强。如果模块未板载上拉电阻，SDA/SCL 信号无法拉高。

• 解决：务必在 PB10 和 PB11 上分别接 4.7kΩ 电阻到 3.3V 电源。

---

7. 总结与扩展

本项目实现了一个基于**“相对定位”原理的低成本轨迹记录仪。它避开了 GPS 的环境限制和磁力计的干扰问题，非常适合用于室内移动机器人、手持扫描设备或交互式输入设备**。

未来优化方向：

1. 上位机显示：通过串口将 Global_X, Global_Y 发送至电脑，使用 Python (Matplotlib/Turtle) 实时绘制轨迹图。

2. 平滑滤波：引入卡尔曼滤波 (Kalman Filter) 进一步融合加速度计数据，提高快速移动时的平滑度。

---

源码已上传至附件，欢迎下载交流！如果觉得本文有用，请点赞收藏。