基于STM32 的VI-SLAM 数据存储系统设计

邓 斌，刘 明

（云南民族大学电气信息工程学院，云南昆明 650500）

随着对海洋资源开发程度的逐渐深入，水下探测技术也随之不断更新。当前，在水下作业方面，尤其是在水下环境探测场景中，水下航行器的SLAM技术有着广阔的应用前景。水下航行器SLAM 技术的前提是它所搭载的各种传感器能正常传回相关数据，借助这些传感器所采集的数据，航行器便可以了解自身周围环境的情况，并确定自身的位置信息，进而构建出水下环境的地图，最终实现航行器的自主导航。传统的视觉SLAM 在航行器进行快速运动时位姿解算误差较大，且会出现构建的地图与实际地图尺度不匹配的问题。在视觉SLAM 中加入惯导数据可较好地解决上述两个问题。视觉惯导SLAM（VI-SLAM）技术融合了图像信息与惯导信息这两种互补的数据，惯导为快速运动提供了较好的解决方案，而相机又能解决慢速运动下惯导的漂移问题[1,2]，是SLAM 未来发展的趋势之一。而在研究VI-SLAM算法之前，需要实现图像与惯导数据的采集与存储。

在水下航行器行驶过程中，会产生大量需要存储的传感器数据，又因为后续数据解算处理以及多传感器数据融合的SLAM 算法实验均需要在机能强大的PC 上位机上进行，这就需要数据可以方便地导入PC 机中。考虑到小型航行器体积有限，采集系统体积不宜过大，且受单片机硬件的制约，想要存储大量的数据，还需要便捷地将存储的数据传送给PC机，综合考虑最终选用SD 卡来存储数据[3]。SD 卡自身容量可根据航行时间进行灵活选择，且SD 卡相关电路的可移植性强，安装与编程较为简单，支持SPI与SDIO 两种传输方式。搭配移植到STM32 上的FatFS 文件系统，能较好地满足频繁传输数据到PC机的需求，且基于FatFS 文件系统保存的数据可以直接在PC 机上打开[4-6]，具有一定的实用价值。

1 系统组成与硬件介绍

数据采集存储系统主要搭载在小型水下航行器上，航行器在水中运动的同时，系统采集并存储传感器数据。该设计包含STM32 核心板、惯导模块（IMU）、相机模块与SD 卡，系统框架如图1 所示。

图1 系统框架

1.1 STM32

设计采用了STM32F103 系列单片机，该系列芯片是意法半导体（ST）公司旗下一款常用的增强型系列微控制器。其内核是32 位的Cortex-M3；内部集成了定时器Timer、SPI、I2C、USB 等多种外设接口。STM32 电路如 图2 所示。

图2 STM32电路

1.2 相机模块

OV7725 是一款集成了1/4 英寸单芯片VGA 相机和图像处理器的高性能传感器。具有标准的SCCB 配置接口；支持VGA、QVGA，以及40×30 到CIF（352×288）分辨率的图像尺寸；可搭配AL422B 高速缓存器一起使用。相机电路如图3 所示。

图3 相机电路

图4 IMU电路

1.3 JY-901惯导模块

JY-901 模块是一款集成了高精度陀螺仪、加速度计和地磁场传感器的小型惯导模块。它可由STM32 核心板上的供电引脚直接供电；可用串口和I2C 两种接口分别连接PC 与单片机进行数据传输；惯导数据回传频率为0.2～200 Hz；角度静态误差为0.05°；动态误差为0.1°。IMU 电路如4 所示。

1.4 SD卡

SD 卡选用了闪迪32 GB 大容量高速内存卡。SD 卡槽与STM32 板连接电路如图5 所示。

图5 SD卡槽与STM32板连接电路

1.5 系统性能指标

相机模块的图像数据以RGB565 的格式进行输出,采用QVGA（320×240）像素进行图像拍摄，每张图像的大小为150 kB。高速缓存模块写入时钟最小周期为20 ns，换算可得读写操作速率最高为12.5 MB/s，能够以较高的速度对图像数据进行缓存。IMU 模块每个寄存器中的数据为16 bit，实际每次采集的数据一般只涉及到6 个寄存器，也就是每次采集12 B 数据。进行惯导解算时一般所用的IMU数据为100 Hz，每秒采集的数据大小为1.2 kB。所采用的STM32F103芯片板理论串口速率可达到4.5 MB/s，既可满足从FIFO 中读取缓存一帧图像数据的速度要求，也可以满足IMU 数据读取的速度要求。采用的闪迪A1 高速SD 卡经实测写入速度最高可达到50 MB/s[7]。移植到STM32 中的FatFS 系统，在写入SD 卡时的速度会随写入块大小的改变而改变，在向空白的SD 卡中写入512 kB 或是1 MB 的数据块时，其速度可达4.6 MB/s，基本可以满足IMU 与相机的数据写入需求[8]。要注意的是当SD 卡的存储空间不足两成时，写入速度会明显降低。

2 数据采集实现流程

2.1 图像数据采集流程

以QVGA 像素（240×320）模式为例，分析图像数据的传输过程，其数据传输时序图如图6 所示。

图6 QVGA时序图

图6 中VSYNC 为帧同步信号，相邻两个信号之间传输的数据便是一帧图像。当STM32 检测到该信号时，便控制AL422B 开始缓存一帧的图像数据，可以选择使用下降沿或上升沿来触发中断[9]。HREF和HSYNC 均为行同步信号，相邻两个信号之间为一行的图像数据。两者均为一个引脚产生的信号，在不同的场合下，可使用不同的信号来控制。在使用HREF 信号时，只有在HREF 高电平期间的数据才为有效数据。STM32 通过检测帧同步信号与行同步信号即可获得完整的图像数据[10]。在VGA 模式下，每行输出320 个周期，每一个周期中根据图像格式的不同会有一个或两个字节，共输出240 行数据完成一帧图像。一帧原始数据的大小约为150 kB[11-12]。

当完成一帧数据的缓存之后，STM32 控制缓存模块停止图像数据的读取，并开始读取其中的数据；待一帧图像读取完毕后，将数据指针复位，控制模块继续读取图像数据；等待下一个帧同步信号到来时，缓存模块再缓存下一帧的图像[13-14]。循环该过程，完成图像数据的采集。

2.2 惯导数据采集流程

将JY-901 惯导模块通过I2C 总线与STM32 板相连接。STM32 采用选择寄存器地址的方式对寄存器中的数据进行访问[15]。每个地址内的数据均为16 位数据，占两个字节。通过对其内部特性寄存器数据的修改，来设置波特率、选择输出数据的类型、从站的地址等信息。实际所需要采集的数据以及数据所对应的寄存器地址如表1 所示。

表1 寄存器数据表

读取IMU 模块数据需要STM32 先通过I2C 总线向惯导元件从机发送一个开始符；再写入模块的I2C 从站地址；之后查看手册找到所需数据对应的寄存器地址，将地址输入进去；IMU 模块收到该信息后返回一个收到符；然后IMU 会按照寄存器的字节前后顺序依次将选中的数据输出；STM32 需在收到每一个字节后，向IMU 回复一个收到符；当接收到的数据总量到达16 bit 时，STM32 便不再回复，IMU 也不再输出，并回复一个结束符表明传输过程已经结束[16]。

3 数据SD卡存储流程

数据存储设计思路：首先为STM32 板接通电源进行复位操作，紧接着配置时钟、GPIO、SD 卡接口、使能SPI 时钟；然后对IMU、相机、SD 卡进行硬件配置，并将设置文件保存在程序中。配置的内容主要是IMU 输出数据寄存器的选择、相机分辨率与图像格式的选择、SD 卡传输模式的选择；接着配置FaTFS底层存储介质的接口函数，例如创建工作区、数据读写之类的函数；然后借助提前留好的API 接口函数来创建工作区、新建文本文件并向SD 卡写入数据；完成数据的存储之后，需要把保存好数据的SD 卡从STM32 中拔出并插入PC 机，检查存储的数据。在数据格式无误的情况下，导出数据给后续的算法使用。系统流程图如图7 所示。

图7 系统流程图

3.1 SD卡的初始化

SD 卡在SPI 模式初始化的流程：

1）在将SPI 模块初始化好之后，将SPI 速度设为低速模式（100～400 kHz），并做好接收本地指令的准备；

2）上电延时250 ms，相当于等待74 个时钟，然后发送CMD0 命令，得到返回值0x01；

3）进入卡类型检验。首先发送命令CMD8，然后检测返回值。返回值为0x01 时，说明SD 卡为V2.0卡，之后发送循环命令CMD55 与ACMD41，直到返回0x00，确定SD2.0 卡初始化成功。

4）初始化成功后，提高SPI 的速率。

3.2 SD卡操作命令

向SD 卡写入一个CMD 的过程：首先使CS 为低电平，SD 卡使能；其次在SD 卡的Din 写入指令；写入指令后还要附加8 个填充时钟，完成SD 卡内部操作；之后在SD 卡的Dout 上接受回应；接受完毕后使CS为低电平，并添加8 个填充时钟。

常见的命令及其意义可查看SD 卡2.0 协议文件，不再赘述。

3.3 SD卡数据的读写

SD 卡的写入数据流程：STM32 发送CMD24（单块）或者CMD25（多块）的写命令；接收卡响应R1（0x00）；发送写数据起始令牌0xFE 或0xFC；发送512字节的正式数据；发送2 B 的CRC。

SD 卡的读取数据流程：STM32 发送CMD17（单块）或者CMD18（多块）的读指令；接收卡响应R1（0x00）；接收数据起始令牌0xFE 或者0xFC；接收512字节的正式数据；接收2 B 的CRC。

3.4 FaTFS文件管理系统

FaTFS 是一个开源的、为小型嵌入式系统设计的通用FAT 文件管理系统。它支持多种存储媒介，可对多个文件进行读写，且不依赖硬件架构，可以被移植到各种低性能的系统中，例如该文所用的STM32F103。移植过程简单，使用前只需要先通过底层储存介质接口参数函数对存储设备进行配置，再调用所提供的API 接口，借助其中的f_mount 函数在SD 卡中创建工作区，借助其中的f_open 函数在SD卡中打开对应的文本文件，如果未检测到该文件，便会创建一个文本文件，最后借助其中的f_write 函数，将所选寄存器中的传感器数据写入SD 卡。

4 实验测试与结果

所用相机、惯导模块实物如图8 所示。

图8 硬件实物图

将模块连接到STM32 上，通过板载引脚为其供电，惯导模块水平固定静止，相机竖立。在静止状态下系统采集存储的惯导数据如图9 所示。

图9 SD卡存储惯导数据

数据格式每一列依次为三轴的加速度（单位为g）、三轴的角速度（单位为deg/s）。

系统采集存储的图像集中一张图像数据（240×320）如图10 所示。

图10 SD卡存储图像

固定时间间隔连续采集图像数据，并将其按顺序排列组合成图像数据集便可供算法使用。

5 结束语

该文为满足小型水下航行器VI-SLAM 导航的大容量数据存储需求，设计了一款小体型低成本的数据采集存储系统。系统采用基于STM32 与SD 卡的采集存储方式，并借助移植的FatFS 文件系统实现了将采集到的数据写入SD 卡的功能，数据的存储格式也方便了后续算法的直接调用。经测试，该系统可稳定可靠的采集存储图像与惯导数据，所采集的数据基本满足后续VI-SLAM 算法的使用需求，有一定的实际应用价值。