基于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 算法的使用需求，有一定的實際應用價值。