鄧 斌，劉 明

（云南民族大學(xué)電氣信息工程學(xué)院，云南昆明 650500）

隨著對(duì)海洋資源開(kāi)發(fā)程度的逐漸深入，水下探測(cè)技術(shù)也隨之不斷更新。當(dāng)前，在水下作業(yè)方面，尤其是在水下環(huán)境探測(cè)場(chǎng)景中，水下航行器的SLAM技術(shù)有著廣闊的應(yīng)用前景。水下航行器SLAM 技術(shù)的前提是它所搭載的各種傳感器能正常傳回相關(guān)數(shù)據(jù)，借助這些傳感器所采集的數(shù)據(jù)，航行器便可以了解自身周?chē)h(huán)境的情況，并確定自身的位置信息，進(jìn)而構(gòu)建出水下環(huán)境的地圖，最終實(shí)現(xiàn)航行器的自主導(dǎo)航。傳統(tǒng)的視覺(jué)SLAM 在航行器進(jìn)行快速運(yùn)動(dòng)時(shí)位姿解算誤差較大，且會(huì)出現(xiàn)構(gòu)建的地圖與實(shí)際地圖尺度不匹配的問(wèn)題。在視覺(jué)SLAM 中加入慣導(dǎo)數(shù)據(jù)可較好地解決上述兩個(gè)問(wèn)題。視覺(jué)慣導(dǎo)SLAM（VI-SLAM）技術(shù)融合了圖像信息與慣導(dǎo)信息這兩種互補(bǔ)的數(shù)據(jù)，慣導(dǎo)為快速運(yùn)動(dòng)提供了較好的解決方案，而相機(jī)又能解決慢速運(yùn)動(dòng)下慣導(dǎo)的漂移問(wèn)題[1,2]，是SLAM 未來(lái)發(fā)展的趨勢(shì)之一。而在研究VI-SLAM算法之前，需要實(shí)現(xiàn)圖像與慣導(dǎo)數(shù)據(jù)的采集與存儲(chǔ)。

在水下航行器行駛過(guò)程中，會(huì)產(chǎn)生大量需要存儲(chǔ)的傳感器數(shù)據(jù)，又因?yàn)楹罄m(xù)數(shù)據(jù)解算處理以及多傳感器數(shù)據(jù)融合的SLAM 算法實(shí)驗(yàn)均需要在機(jī)能強(qiáng)大的PC 上位機(jī)上進(jìn)行，這就需要數(shù)據(jù)可以方便地導(dǎo)入PC 機(jī)中?？紤]到小型航行器體積有限，采集系統(tǒng)體積不宜過(guò)大，且受單片機(jī)硬件的制約，想要存儲(chǔ)大量的數(shù)據(jù)，還需要便捷地將存儲(chǔ)的數(shù)據(jù)傳送給PC機(jī)，綜合考慮最終選用SD 卡來(lái)存儲(chǔ)數(shù)據(jù)[3]。SD 卡自身容量可根據(jù)航行時(shí)間進(jìn)行靈活選擇，且SD 卡相關(guān)電路的可移植性強(qiáng)，安裝與編程較為簡(jiǎn)單，支持SPI與SDIO 兩種傳輸方式。搭配移植到STM32 上的FatFS 文件系統(tǒng)，能較好地滿(mǎn)足頻繁傳輸數(shù)據(jù)到PC機(jī)的需求，且基于FatFS 文件系統(tǒng)保存的數(shù)據(jù)可以直接在PC 機(jī)上打開(kāi)[4-6]，具有一定的實(shí)用價(jià)值。

1 系統(tǒng)組成與硬件介紹

數(shù)據(jù)采集存儲(chǔ)系統(tǒng)主要搭載在小型水下航行器上，航行器在水中運(yùn)動(dòng)的同時(shí)，系統(tǒng)采集并存儲(chǔ)傳感器數(shù)據(jù)。該設(shè)計(jì)包含STM32 核心板、慣導(dǎo)模塊（IMU）、相機(jī)模塊與SD 卡，系統(tǒng)框架如圖1 所示。

圖1 系統(tǒng)框架

1.1 STM32

設(shè)計(jì)采用了STM32F103 系列單片機(jī)，該系列芯片是意法半導(dǎo)體（ST）公司旗下一款常用的增強(qiáng)型系列微控制器。其內(nèi)核是32 位的Cortex-M3；內(nèi)部集成了定時(shí)器Timer、SPI、I2C、USB 等多種外設(shè)接口。STM32 電路如 圖2 所示。

圖2 STM32電路

1.2 相機(jī)模塊

OV7725 是一款集成了1/4 英寸單芯片VGA 相機(jī)和圖像處理器的高性能傳感器。具有標(biāo)準(zhǔn)的SCCB 配置接口；支持VGA、QVGA，以及40×30 到CIF（352×288）分辨率的圖像尺寸；可搭配AL422B 高速緩存器一起使用。相機(jī)電路如圖3 所示。

圖3 相機(jī)電路

圖4 IMU電路

1.3 JY-901慣導(dǎo)模塊

JY-901 模塊是一款集成了高精度陀螺儀、加速度計(jì)和地磁場(chǎng)傳感器的小型慣導(dǎo)模塊。它可由STM32 核心板上的供電引腳直接供電；可用串口和I2C 兩種接口分別連接PC 與單片機(jī)進(jìn)行數(shù)據(jù)傳輸；慣導(dǎo)數(shù)據(jù)回傳頻率為0.2～200 Hz；角度靜態(tài)誤差為0.05°；動(dòng)態(tài)誤差為0.1°。IMU 電路如4 所示。

1.4 SD卡

SD 卡選用了閃迪32 GB 大容量高速內(nèi)存卡。SD 卡槽與STM32 板連接電路如圖5 所示。

圖5 SD卡槽與STM32板連接電路

1.5 系統(tǒng)性能指標(biāo)

相機(jī)模塊的圖像數(shù)據(jù)以RGB565 的格式進(jìn)行輸出,采用QVGA（320×240）像素進(jìn)行圖像拍攝，每張圖像的大小為150 kB。高速緩存模塊寫(xiě)入時(shí)鐘最小周期為20 ns，換算可得讀寫(xiě)操作速率最高為12.5 MB/s，能夠以較高的速度對(duì)圖像數(shù)據(jù)進(jìn)行緩存。IMU 模塊每個(gè)寄存器中的數(shù)據(jù)為16 bit，實(shí)際每次采集的數(shù)據(jù)一般只涉及到6 個(gè)寄存器，也就是每次采集12 B 數(shù)據(jù)。進(jìn)行慣導(dǎo)解算時(shí)一般所用的IMU數(shù)據(jù)為100 Hz，每秒采集的數(shù)據(jù)大小為1.2 kB。所采用的STM32F103芯片板理論串口速率可達(dá)到4.5 MB/s，既可滿(mǎn)足從FIFO 中讀取緩存一幀圖像數(shù)據(jù)的速度要求，也可以滿(mǎn)足IMU 數(shù)據(jù)讀取的速度要求。采用的閃迪A1 高速SD 卡經(jīng)實(shí)測(cè)寫(xiě)入速度最高可達(dá)到50 MB/s[7]。移植到STM32 中的FatFS 系統(tǒng)，在寫(xiě)入SD 卡時(shí)的速度會(huì)隨寫(xiě)入塊大小的改變而改變，在向空白的SD 卡中寫(xiě)入512 kB 或是1 MB 的數(shù)據(jù)塊時(shí)，其速度可達(dá)4.6 MB/s，基本可以滿(mǎn)足IMU 與相機(jī)的數(shù)據(jù)寫(xiě)入需求[8]。要注意的是當(dāng)SD 卡的存儲(chǔ)空間不足兩成時(shí)，寫(xiě)入速度會(huì)明顯降低。

2 數(shù)據(jù)采集實(shí)現(xiàn)流程

2.1 圖像數(shù)據(jù)采集流程

以QVGA 像素（240×320）模式為例，分析圖像數(shù)據(jù)的傳輸過(guò)程，其數(shù)據(jù)傳輸時(shí)序圖如圖6 所示。

圖6 QVGA時(shí)序圖

圖6 中VSYNC 為幀同步信號(hào)，相鄰兩個(gè)信號(hào)之間傳輸?shù)臄?shù)據(jù)便是一幀圖像。當(dāng)STM32 檢測(cè)到該信號(hào)時(shí)，便控制AL422B 開(kāi)始緩存一幀的圖像數(shù)據(jù)，可以選擇使用下降沿或上升沿來(lái)觸發(fā)中斷[9]。HREF和HSYNC 均為行同步信號(hào)，相鄰兩個(gè)信號(hào)之間為一行的圖像數(shù)據(jù)。兩者均為一個(gè)引腳產(chǎn)生的信號(hào)，在不同的場(chǎng)合下，可使用不同的信號(hào)來(lái)控制。在使用HREF 信號(hào)時(shí)，只有在HREF 高電平期間的數(shù)據(jù)才為有效數(shù)據(jù)。STM32 通過(guò)檢測(cè)幀同步信號(hào)與行同步信號(hào)即可獲得完整的圖像數(shù)據(jù)[10]。在VGA 模式下，每行輸出320 個(gè)周期，每一個(gè)周期中根據(jù)圖像格式的不同會(huì)有一個(gè)或兩個(gè)字節(jié)，共輸出240 行數(shù)據(jù)完成一幀圖像。一幀原始數(shù)據(jù)的大小約為150 kB[11-12]。

當(dāng)完成一幀數(shù)據(jù)的緩存之后，STM32 控制緩存模塊停止圖像數(shù)據(jù)的讀取，并開(kāi)始讀取其中的數(shù)據(jù)；待一幀圖像讀取完畢后，將數(shù)據(jù)指針復(fù)位，控制模塊繼續(xù)讀取圖像數(shù)據(jù)；等待下一個(gè)幀同步信號(hào)到來(lái)時(shí)，緩存模塊再緩存下一幀的圖像[13-14]。循環(huán)該過(guò)程，完成圖像數(shù)據(jù)的采集。

2.2 慣導(dǎo)數(shù)據(jù)采集流程

將JY-901 慣導(dǎo)模塊通過(guò)I2C 總線與STM32 板相連接。STM32 采用選擇寄存器地址的方式對(duì)寄存器中的數(shù)據(jù)進(jìn)行訪問(wèn)[15]。每個(gè)地址內(nèi)的數(shù)據(jù)均為16 位數(shù)據(jù)，占兩個(gè)字節(jié)。通過(guò)對(duì)其內(nèi)部特性寄存器數(shù)據(jù)的修改，來(lái)設(shè)置波特率、選擇輸出數(shù)據(jù)的類(lèi)型、從站的地址等信息。實(shí)際所需要采集的數(shù)據(jù)以及數(shù)據(jù)所對(duì)應(yīng)的寄存器地址如表1 所示。

表1 寄存器數(shù)據(jù)表

讀取IMU 模塊數(shù)據(jù)需要STM32 先通過(guò)I2C 總線向慣導(dǎo)元件從機(jī)發(fā)送一個(gè)開(kāi)始符；再寫(xiě)入模塊的I2C 從站地址；之后查看手冊(cè)找到所需數(shù)據(jù)對(duì)應(yīng)的寄存器地址，將地址輸入進(jìn)去；IMU 模塊收到該信息后返回一個(gè)收到符；然后IMU 會(huì)按照寄存器的字節(jié)前后順序依次將選中的數(shù)據(jù)輸出；STM32 需在收到每一個(gè)字節(jié)后，向IMU 回復(fù)一個(gè)收到符；當(dāng)接收到的數(shù)據(jù)總量到達(dá)16 bit 時(shí)，STM32 便不再回復(fù)，IMU 也不再輸出，并回復(fù)一個(gè)結(jié)束符表明傳輸過(guò)程已經(jīng)結(jié)束[16]。

3 數(shù)據(jù)SD卡存儲(chǔ)流程

數(shù)據(jù)存儲(chǔ)設(shè)計(jì)思路：首先為STM32 板接通電源進(jìn)行復(fù)位操作，緊接著配置時(shí)鐘、GPIO、SD 卡接口、使能SPI 時(shí)鐘；然后對(duì)IMU、相機(jī)、SD 卡進(jìn)行硬件配置，并將設(shè)置文件保存在程序中。配置的內(nèi)容主要是IMU 輸出數(shù)據(jù)寄存器的選擇、相機(jī)分辨率與圖像格式的選擇、SD 卡傳輸模式的選擇；接著配置FaTFS底層存儲(chǔ)介質(zhì)的接口函數(shù)，例如創(chuàng)建工作區(qū)、數(shù)據(jù)讀寫(xiě)之類(lèi)的函數(shù)；然后借助提前留好的API 接口函數(shù)來(lái)創(chuàng)建工作區(qū)、新建文本文件并向SD 卡寫(xiě)入數(shù)據(jù)；完成數(shù)據(jù)的存儲(chǔ)之后，需要把保存好數(shù)據(jù)的SD 卡從STM32 中拔出并插入PC 機(jī)，檢查存儲(chǔ)的數(shù)據(jù)。在數(shù)據(jù)格式無(wú)誤的情況下，導(dǎo)出數(shù)據(jù)給后續(xù)的算法使用。系統(tǒng)流程圖如圖7 所示。

圖7 系統(tǒng)流程圖

3.1 SD卡的初始化

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

1）在將SPI 模塊初始化好之后，將SPI 速度設(shè)為低速模式（100～400 kHz），并做好接收本地指令的準(zhǔn)備；

2）上電延時(shí)250 ms，相當(dāng)于等待74 個(gè)時(shí)鐘，然后發(fā)送CMD0 命令，得到返回值0x01；

3）進(jìn)入卡類(lèi)型檢驗(yàn)。首先發(fā)送命令CMD8，然后檢測(cè)返回值。返回值為0x01 時(shí)，說(shuō)明SD 卡為V2.0卡，之后發(fā)送循環(huán)命令CMD55 與ACMD41，直到返回0x00，確定SD2.0 卡初始化成功。

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

3.2 SD卡操作命令

向SD 卡寫(xiě)入一個(gè)CMD 的過(guò)程：首先使CS 為低電平，SD 卡使能；其次在SD 卡的Din 寫(xiě)入指令；寫(xiě)入指令后還要附加8 個(gè)填充時(shí)鐘，完成SD 卡內(nèi)部操作；之后在SD 卡的Dout 上接受回應(yīng)；接受完畢后使CS為低電平，并添加8 個(gè)填充時(shí)鐘。

常見(jiàn)的命令及其意義可查看SD 卡2.0 協(xié)議文件，不再贅述。

3.3 SD卡數(shù)據(jù)的讀寫(xiě)

SD 卡的寫(xiě)入數(shù)據(jù)流程：STM32 發(fā)送CMD24（單塊）或者CMD25（多塊）的寫(xiě)命令；接收卡響應(yīng)R1（0x00）；發(fā)送寫(xiě)數(shù)據(jù)起始令牌0xFE 或0xFC；發(fā)送512字節(jié)的正式數(shù)據(jù)；發(fā)送2 B 的CRC。

SD 卡的讀取數(shù)據(jù)流程：STM32 發(fā)送CMD17（單塊）或者CMD18（多塊）的讀指令；接收卡響應(yīng)R1（0x00）；接收數(shù)據(jù)起始令牌0xFE 或者0xFC；接收512字節(jié)的正式數(shù)據(jù)；接收2 B 的CRC。

3.4 FaTFS文件管理系統(tǒng)

FaTFS 是一個(gè)開(kāi)源的、為小型嵌入式系統(tǒng)設(shè)計(jì)的通用FAT 文件管理系統(tǒng)。它支持多種存儲(chǔ)媒介，可對(duì)多個(gè)文件進(jìn)行讀寫(xiě)，且不依賴(lài)硬件架構(gòu)，可以被移植到各種低性能的系統(tǒng)中，例如該文所用的STM32F103。移植過(guò)程簡(jiǎn)單，使用前只需要先通過(guò)底層儲(chǔ)存介質(zhì)接口參數(shù)函數(shù)對(duì)存儲(chǔ)設(shè)備進(jìn)行配置，再調(diào)用所提供的API 接口，借助其中的f_mount 函數(shù)在SD 卡中創(chuàng)建工作區(qū)，借助其中的f_open 函數(shù)在SD卡中打開(kāi)對(duì)應(yīng)的文本文件，如果未檢測(cè)到該文件，便會(huì)創(chuàng)建一個(gè)文本文件，最后借助其中的f_write 函數(shù)，將所選寄存器中的傳感器數(shù)據(jù)寫(xiě)入SD 卡。

4 實(shí)驗(yàn)測(cè)試與結(jié)果

所用相機(jī)、慣導(dǎo)模塊實(shí)物如圖8 所示。

圖8 硬件實(shí)物圖

將模塊連接到STM32 上，通過(guò)板載引腳為其供電，慣導(dǎo)模塊水平固定靜止，相機(jī)豎立。在靜止?fàn)顟B(tài)下系統(tǒng)采集存儲(chǔ)的慣導(dǎo)數(shù)據(jù)如圖9 所示。

圖9 SD卡存儲(chǔ)慣導(dǎo)數(shù)據(jù)

數(shù)據(jù)格式每一列依次為三軸的加速度（單位為g）、三軸的角速度（單位為deg/s）。

系統(tǒng)采集存儲(chǔ)的圖像集中一張圖像數(shù)據(jù)（240×320）如圖10 所示。

圖10 SD卡存儲(chǔ)圖像

固定時(shí)間間隔連續(xù)采集圖像數(shù)據(jù)，并將其按順序排列組合成圖像數(shù)據(jù)集便可供算法使用。

5 結(jié)束語(yǔ)

該文為滿(mǎn)足小型水下航行器VI-SLAM 導(dǎo)航的大容量數(shù)據(jù)存儲(chǔ)需求，設(shè)計(jì)了一款小體型低成本的數(shù)據(jù)采集存儲(chǔ)系統(tǒng)。系統(tǒng)采用基于STM32 與SD 卡的采集存儲(chǔ)方式，并借助移植的FatFS 文件系統(tǒng)實(shí)現(xiàn)了將采集到的數(shù)據(jù)寫(xiě)入SD 卡的功能，數(shù)據(jù)的存儲(chǔ)格式也方便了后續(xù)算法的直接調(diào)用。經(jīng)測(cè)試，該系統(tǒng)可穩(wěn)定可靠的采集存儲(chǔ)圖像與慣導(dǎo)數(shù)據(jù)，所采集的數(shù)據(jù)基本滿(mǎn)足后續(xù)VI-SLAM 算法的使用需求，有一定的實(shí)際應(yīng)用價(jià)值。