基于MCU的室外移動(dòng)機(jī)器人組合導(dǎo)航定位系統(tǒng)

對(duì)于在室外環(huán)境工作的移動(dòng)機(jī)器人通常使用慣導(dǎo)/衛(wèi)星組合導(dǎo)航方式。慣性導(dǎo)航系統(tǒng)[1]具有完全自主、抗干擾強(qiáng)、隱蔽能力好和輸出參數(shù)全面等優(yōu)點(diǎn)，但它的魯棒性極低，誤差會(huì)不斷隨時(shí)間累積發(fā)散。衛(wèi)星導(dǎo)航系統(tǒng)具有精度高、定位范圍廣和誤差不隨時(shí)間累積等優(yōu)點(diǎn)，但其自主性差、易受外界遮擋和干擾、接收機(jī)數(shù)據(jù)更新頻率低等缺點(diǎn)。因此工程上常常將兩者互補(bǔ)結(jié)合使用，組成衛(wèi)星/慣性組合導(dǎo)航系統(tǒng)。

本文以低功耗MSP430F149為核心，設(shè)計(jì)了能夠同時(shí)實(shí)現(xiàn)衛(wèi)星導(dǎo)航（GNSS）接收機(jī)、慣性測(cè)量單元（IMU）、氣壓高度等導(dǎo)航信息的高速采集與高速合路傳輸，并進(jìn)行初步導(dǎo)航定位信息融合的導(dǎo)航系統(tǒng)，即可為室外移動(dòng)機(jī)器人提供直接的導(dǎo)航服務(wù)，也可作為高精度組合導(dǎo)航系統(tǒng)的原始測(cè)量信息高速采集系統(tǒng)。

系統(tǒng)設(shè)計(jì)的關(guān)鍵是利用單片機(jī)有限的接口資源實(shí)現(xiàn)了多傳感器信息并行采集，設(shè)計(jì)了有效的數(shù)據(jù)同步方法，解決了氣壓傳感器數(shù)據(jù)手冊(cè)疏漏導(dǎo)致的無(wú)法接入問(wèn)題，給出了機(jī)器人組合定位的基本方法。系統(tǒng)充分利用了MSP430F149單片機(jī)的能力，具有結(jié)構(gòu)簡(jiǎn)單、低功耗、對(duì)傳感器具有普適性等優(yōu)點(diǎn)。

1總體設(shè)計(jì)

本系統(tǒng)由電源、氣壓計(jì)接口、IMU接口、GNSS接收機(jī)接口、SPI轉(zhuǎn)UART模塊及MSP430F149構(gòu)成。系統(tǒng)組成如圖1所示。

組合導(dǎo)航系統(tǒng)的功能實(shí)現(xiàn)分為IMU數(shù)據(jù)接收與解析、GNSS數(shù)據(jù)接收與解析、氣壓計(jì)數(shù)據(jù)接收與解析、組合導(dǎo)航解算以及數(shù)據(jù)輸出五個(gè)部分。IMU數(shù)據(jù)接收與解析功能用來(lái)獲取導(dǎo)航解算中需要的加速度和角速度信息；GNSS數(shù)據(jù)接收與解析功能用來(lái)獲取導(dǎo)航解算中需要的位置和速度信息（松耦合組合）或者GNSS偽距和偽距率（緊耦合組合）；氣壓計(jì)數(shù)據(jù)接收與解析功能用來(lái)獲取高度信息；組合導(dǎo)航解算功能為系統(tǒng)核心，用來(lái)進(jìn)行組合導(dǎo)航解算；數(shù)據(jù)的輸出包括原始數(shù)據(jù)包的整合輸出和解算結(jié)果的輸出。

圖1系統(tǒng)組成結(jié)構(gòu)圖

本文所使用的慣性器件和GNSS接收機(jī)都是RS-232電平的UART接口，具有通用性，用戶(hù)可根據(jù)成本考慮不同精度的設(shè)備。氣壓計(jì)選用美國(guó)MEAS公司生產(chǎn)的MS5803-02BA，已經(jīng)固化在電路中。

2硬件電路設(shè)計(jì)

2.1微控制器接口

整個(gè)組合導(dǎo)航定位系統(tǒng)需要三個(gè)UART接口和兩個(gè)SPI接口。其中兩個(gè)UART接口由430單片機(jī)自帶的UART資源提供，另外一個(gè)UART接口由GPIO模擬SPI通過(guò)MAX3111E芯片轉(zhuǎn)化得到；兩個(gè)SPI接口由GPIO模擬得到。另外需要一個(gè)外部中斷引腳捕獲秒脈沖信號(hào)（PPS）、一個(gè)外部中斷引腳捕獲MAX3111E中斷信號(hào)。MSP430F149管腳資源分配如表1所示。

2.2電源電路

本系統(tǒng)供電需求為3.3V供電，因此采用AMS1117穩(wěn)壓芯片，接入5V電源即可輸出3.3V穩(wěn)定電壓，可提供1A電流，滿足系統(tǒng)供電需求。電路設(shè)計(jì)如圖2所示。

圖2電源電路

2.3 IMU器件及GNSS接收機(jī)接口電路

IMU器件及GNSS接收機(jī)都采用UART接口方式接入，采用RS232協(xié)議。因此可使用430單片機(jī)上自帶的兩個(gè)UART接口，但是需要進(jìn)行TTL電平與RS232電平轉(zhuǎn)換。這里采用常見(jiàn)的MAX3232芯片，電路設(shè)計(jì)如圖3所示。

圖3 IMU及GNSS接口電路

2.4氣壓計(jì)MS5803-02BA接口電路

MS5803-02BA[3]是由MEAS公司生產(chǎn)的數(shù)字壓力傳感器，分辨率達(dá)10cm.芯片內(nèi)部包含一個(gè)高線性的壓力傳感器和一個(gè)內(nèi)部工廠標(biāo)定系數(shù)的超低功耗24位ΔΣ型ADC.該款芯片有SPI和I2C兩種接口方式，通過(guò)芯片的PS引腳配置了選擇不同的接口方式（PS置低時(shí)，采用SPI工作模式；PS置高時(shí)，采用I2C工作模式）。本文所闡述的定位系統(tǒng)將氣壓計(jì)配置為SPI工作模式。MS5803-02BA與微控制器間的接口電路設(shè)計(jì)如圖4所示。

圖4 MS5803-02BA接口電路

MS5803-02BA的控制命令包括復(fù)位命令、溫度ADC命令、氣壓ADC命令、ADC讀取命令、PROM讀取命令。控制命令如表2所示。

控制命令通過(guò)SDI口移位輸入，響應(yīng)結(jié)果從SDO移位輸出。輸入的電平判定在時(shí)鐘信號(hào)的上升沿，輸出的電平判定在時(shí)鐘信號(hào)的下降沿。輸出的氣壓值可以進(jìn)行溫度補(bǔ)償，需要利用芯片內(nèi)部PROM中的系數(shù)來(lái)補(bǔ)償。ADC讀取命令輸入之后，輸出24位ADC結(jié)果；PROM讀取命令輸入之后，輸出16位補(bǔ)償系數(shù)。

下面是讀取ADC的C語(yǔ)言代碼：

CSN_OFF_MS（）；//CS置低

SPI_WRITE_8BIT（CMD）；//SDI移入8位CMD

delay_ms（10）； //延時(shí)10ms

CSN_ON_MS（）；//CS置高

CSN_OFF_MS（）；//CS置低

SPI_WRITE_8BIT（0x00）；//SDI移入8位0x00

result = SPI_READ_24BIT（）；//SDO移出24位

CSN_ON_MS（）；//CS置高

下面是讀取PROM的C語(yǔ)言代碼：

CSN_OFF_MS（）；//CS置低

SPI_WRITE_8BIT（CMD）； //SDI移入8位CMD

result=SPI_READ_16BIT（）；//SDO移出24位

CSN_ON_MS（）；//CS置高

結(jié)合器件的使用手冊(cè)及手冊(cè)疏漏的地方，使用MS5803-02BA時(shí)需要注意：

1、溫度和氣壓ADC命令發(fā)送之后，芯片內(nèi)部需要一定的時(shí)間進(jìn)行采樣轉(zhuǎn)換，具體時(shí)間與過(guò)采樣率（OSR）有關(guān)，最大需求時(shí)間為10ms，因此本文采用的延時(shí)時(shí)間為10ms；

2、片選信號(hào)CS的下降沿到時(shí)鐘SCLK信號(hào)的第一個(gè)上升沿至少要有21ns的時(shí)間延遲，否則命令無(wú)法正確寫(xiě)入芯片；

3、8位的ADC讀取命令之后，必須保持CS片選信號(hào)持續(xù)為低，再產(chǎn)生24位時(shí)鐘信號(hào)輸入，將24位的ADC結(jié)果讀取出來(lái)（即一個(gè)命令字為8位，但實(shí)際需要32個(gè)連續(xù)的時(shí)鐘周期才能完整讀取ADC結(jié)果）；

4、對(duì)于PROM讀取命令同ADC讀取命令，一共需要24個(gè)連續(xù)的時(shí)鐘周期完成，其中8位命令字輸入，16位數(shù)據(jù)讀出。

5、對(duì)于所有從SDO移位輸出數(shù)據(jù)的同時(shí)，都需要SDI輸入端保持低電平。

2.5 SPI轉(zhuǎn)UART電路

由于MSP430F149的片上UART資源都被IMU器件和GNSS接收機(jī)占用，因此需要擴(kuò)充一個(gè)UART接口才能滿足定位系統(tǒng)與PC機(jī)間的通信。為此將MSP430F149上的GPIO模擬SPI，再通過(guò)MAX3111E芯片轉(zhuǎn)成UART接口。

MAX3111E是MAXIM公司推出的全功能收發(fā)器，內(nèi)部包含UART和RS232電平轉(zhuǎn)換兩個(gè)獨(dú)立的部分。UART部分包括兼容于SPI的串行接口、可編程波特率發(fā)生器、發(fā)送移位寄存器、接收移位寄存器、8字長(zhǎng)的FIFO以及4種可屏蔽中斷發(fā)生器；RS232部分包括電泵電容、硬件關(guān)斷（SHDN管腳），具有±15kV靜電保護(hù)作用。它可以選擇1.8432MHz或3.6864MHz兩種晶振作為外部晶振，芯片能夠工作在300bps～230kbps波特率，本文使用的是3.6864MHz晶振。MAX3111E與MSP430F149連接電路如圖5.

圖5 SPI轉(zhuǎn)UART電路

MAX3111E的控制命令分為寫(xiě)配置命令、讀配置命令、寫(xiě)數(shù)據(jù)命令以及讀數(shù)據(jù)命令四種。本文寫(xiě)入芯片的配置命令是0xE809，8bit數(shù)據(jù)長(zhǎng)度、使能FIFO、發(fā)送中斷使能、波特率38400bps.數(shù)據(jù)的寫(xiě)入需要在每個(gè)字節(jié)前面加上0x81，例如若要發(fā)送字節(jié)0x55，則需要從DIN端移位輸入0x8155.

下面是連續(xù)發(fā)送n個(gè)字節(jié)的C語(yǔ)言代碼：

SPI_WRITE_16BIT（0xE809）；//寫(xiě)配置命令

delay_ms（10）；//延時(shí)10ms

SPI_WRITE_16BIT（0x8100+DATA1）；//發(fā)送第1個(gè)數(shù)據(jù)DATA1

delay_ns（250）；//延時(shí)250ns

SPI_WRITE_16BIT（0x8100+DATA2）；//發(fā)送第2個(gè)數(shù)據(jù)DATA2

……

delay_ns（250）；//延時(shí)250ns

SPI_WRITE_16BIT（0x8100+DATAn）；//發(fā)送第n個(gè)數(shù)據(jù)DATAn

使用MAX3111E時(shí)需要注意：

1、片選信號(hào)CS的下降沿到時(shí)鐘SCLK信號(hào)的第一個(gè)上升沿至少要有100ns的時(shí)間延遲；

2、芯片判定片選信號(hào)CS失效時(shí)，需要CS管腳保持高電平的時(shí)間至少為200ns.因此當(dāng)需要連續(xù)發(fā)送數(shù)據(jù)時(shí)，兩個(gè)寫(xiě)數(shù)據(jù)命令之間至少需要200ns，建議間隔250ns；

3、寫(xiě)配置命令需要晶振工作穩(wěn)定，因此可以在寫(xiě)配置命令后再讀配置，直到寫(xiě)入和讀出的配置數(shù)據(jù)相應(yīng)位相同時(shí)才保證芯片按需求配置完畢。

3軟件設(shè)計(jì)

3.1 GPIO模擬SPI實(shí)現(xiàn)方法

使用GPIO模擬SPI，對(duì)選定的IO口進(jìn)行時(shí)序上的電平操作，模擬出SPI時(shí)序。本文將SPI的電平操作采用宏定義的方式，可參考表3中的源代碼。

程序按照SPI邏輯關(guān)系使用上述的宏定義，可以在選定的IO端口產(chǎn)生SPI信號(hào)，并且可以靈活的控制每個(gè)CS有效期間的時(shí)鐘數(shù)。使用GPIO模擬的SPI具有操作直觀、靈活可變等優(yōu)點(diǎn)。相比較模擬得到的SPI，控制器上自帶的SPI接口基本不占用控制器資源，發(fā)送和接收的移位、時(shí)鐘信號(hào)的產(chǎn)生都交由內(nèi)部SPI模塊處理。而GPIO模擬的SPI需要控制器不斷對(duì)IO進(jìn)行操作，因此會(huì)占用控制器處理時(shí)間。時(shí)鐘信號(hào)也因?qū)﹄娖讲僮飨臋C(jī)器周期，導(dǎo)致時(shí)鐘信號(hào)速度有限。

3.2數(shù)據(jù)的采集和處理方法

IMU和GNSS接收機(jī)的數(shù)據(jù)會(huì)主動(dòng)發(fā)送到430單片機(jī)的UART接口，本文采用中斷接收的方式接收數(shù)據(jù)，將每次到來(lái)的一個(gè)字節(jié)循環(huán)存儲(chǔ)在指定的存儲(chǔ)區(qū)；氣壓計(jì)需要430單片機(jī)查詢(xún)式獲取高度信息。因此當(dāng)需求IMU或者GNSS信息的時(shí)候，需要在存儲(chǔ)區(qū)內(nèi)掃描數(shù)據(jù)，掃描依據(jù)是設(shè)備發(fā)送一幀數(shù)據(jù)的協(xié)議（幀頭、幀尾、CRC等），再根據(jù)協(xié)議獲取相應(yīng)物理量的數(shù)值；當(dāng)需要高度信息的時(shí)候，需要430單片機(jī)對(duì)氣壓計(jì)發(fā)送控制字獲取信息。在對(duì)IMU和GNSS信息掃描時(shí)需要一個(gè)變量ptr保存有效數(shù)據(jù)的首地址在存儲(chǔ)區(qū)內(nèi)的偏移地址，為了不重復(fù)使用已用數(shù)據(jù)，需要在利用完信息后，破壞這一幀有效數(shù)據(jù)的幀頭幀尾。

另外需要注意，使用循環(huán)存儲(chǔ)數(shù)據(jù)和掃描取用這種方式，需要數(shù)據(jù)的處理速度比數(shù)據(jù)的接收速度快，否則舊數(shù)據(jù)會(huì)被新數(shù)據(jù)覆蓋。

3.3高度計(jì)算方法

高度信息的獲取需要通過(guò)MS5803-02BA獲取的氣壓信息轉(zhuǎn)換得到。對(duì)這款氣壓計(jì)的操作，需要先在芯片內(nèi)部的PROM獲取C1~C6六個(gè)參數(shù)，再獲取溫度ADC結(jié)果D1和氣壓ADC結(jié)果D2，最后利用官方提供的計(jì)算公式計(jì)算得到帶補(bǔ)償?shù)臍鈮褐?。氣壓值獲取的軟件流程圖如圖6.

圖6高度計(jì)算流程圖

按照氣壓計(jì)的用戶(hù)手冊(cè)中提供的計(jì)算公式，最后可以計(jì)算得到一個(gè)經(jīng)過(guò)溫度補(bǔ)償?shù)臉?biāo)準(zhǔn)氣壓值，單位mbar.高度信息的獲取還需要將氣壓值經(jīng)過(guò)函數(shù)關(guān)系轉(zhuǎn)換。高度h和標(biāo)準(zhǔn)大氣P之間有如下函數(shù)關(guān)系：

Th=288.15K，是g0對(duì)應(yīng)高度下的溫度下限值；β=-6.5K/km，是溫度的垂直變化率；H=0m，是g0對(duì)應(yīng)的高度；Ph=101325Pa，是g0對(duì)應(yīng)高度下的氣壓下限值；R=287.05287m2/（K●s2），是氣體常數(shù)；g0=9.80665m/s2，是海平面重力加速度。

在氣壓與高度的關(guān)系轉(zhuǎn)換中，本文采用分段線性化的方法擬合它們的非線性關(guān)系。在不同氣壓值區(qū)間內(nèi)，線性化公式得到h=a●P+b，取得不同的線性參數(shù)a、b，參考表4.

3.4組合導(dǎo)航的時(shí)間對(duì)準(zhǔn)

進(jìn)行組合導(dǎo)航濾波解算時(shí)，從IMU和GNSS接收機(jī)接收到的數(shù)據(jù)在時(shí)間上應(yīng)該是同步的，因?yàn)槿绻谝粋€(gè)數(shù)據(jù)融合點(diǎn)上，進(jìn)入Kalman濾波器的來(lái)自?xún)蓚€(gè)子系統(tǒng)中的數(shù)據(jù)來(lái)自不同的時(shí)間點(diǎn)，會(huì)給濾波計(jì)算帶來(lái)誤差，同時(shí)也會(huì)給校正計(jì)算帶來(lái)影響。GNSS接收機(jī)輸出的導(dǎo)航數(shù)據(jù)都帶有精確的時(shí)間標(biāo)簽，而從IMU輸出的數(shù)據(jù)只有一個(gè)相對(duì)時(shí)間標(biāo)簽。以GNSS接收機(jī)的時(shí)間標(biāo)簽作為時(shí)間基準(zhǔn)，分析時(shí)間差的組成。系統(tǒng)時(shí)間示意圖如圖7所示。

圖7系統(tǒng)時(shí)鐘示意圖

3.4.1時(shí)間差分析

當(dāng)一個(gè)GNSS數(shù)據(jù)（1Hz）到來(lái)時(shí)，接收機(jī)產(chǎn)生一個(gè)秒脈沖信號(hào)（PPS），用于時(shí)間對(duì)準(zhǔn)。GNSS接收機(jī)本身存在數(shù)據(jù)時(shí)延：接收機(jī)整個(gè)計(jì)算過(guò)程會(huì)產(chǎn)生一個(gè)解算時(shí)間延遲△t1、從衛(wèi)星導(dǎo)航接收機(jī)和慣導(dǎo)系統(tǒng)輸出的數(shù)據(jù)分別經(jīng)過(guò)McBSP和RS-232數(shù)據(jù)接口輸入導(dǎo)航解算處理器會(huì)產(chǎn)生一定的傳輸延遲△t2.因此在PPS信號(hào)前的△t1+△t2時(shí)刻才是當(dāng)前接收到的GNSS數(shù)據(jù)幀的真實(shí)時(shí)刻。IMU數(shù)據(jù)處理時(shí)間相對(duì)于遞推時(shí)間來(lái)說(shuō)很小，可以忽略。另外PPS脈沖信號(hào)和慣性測(cè)量信號(hào)之間的時(shí)間差△τ，它描述GNSS絕對(duì)時(shí)間和IMU相對(duì)時(shí)間之間的關(guān)系，使得GNSS和IMU統(tǒng)一在同一個(gè)時(shí)間標(biāo)準(zhǔn)下。

3.4.2處理思想

根據(jù)總時(shí)間差△T =△t1+△t2+△τ，找到IMU數(shù)據(jù)對(duì)應(yīng)的插值時(shí)刻。通過(guò)時(shí)間△T就可以獲得對(duì)準(zhǔn)時(shí)間點(diǎn)與最近的兩個(gè)慣性采集時(shí)刻，其后對(duì)時(shí)間同步點(diǎn)兩側(cè)時(shí)間點(diǎn)上，即圖中t（k-1）和t（k）時(shí)間點(diǎn)的慣性數(shù)據(jù)進(jìn)行線性插值運(yùn)算，就可以獲得了慣導(dǎo)數(shù)據(jù)和GNSS接收機(jī)數(shù)據(jù)在同一時(shí)間點(diǎn)上的同步化測(cè)量數(shù)據(jù)。

3.4.3實(shí)施方法

GNSS接收機(jī)解算時(shí)間△t1由接收機(jī)提供。

傳輸時(shí)間△t2通常為一個(gè)固定時(shí)間，使用示波器分別測(cè)量GNSS接收機(jī)發(fā)送時(shí)間和導(dǎo)航解算電路的接收時(shí)間，再將兩個(gè)時(shí)間作差即可獲得。

△τ的獲取需要開(kāi)啟MSP430F149的計(jì)時(shí)器和外部中斷，通過(guò)中斷計(jì)時(shí)的方式獲得。具體方法是：通過(guò)將PPS信號(hào)接入導(dǎo)航處理器的外部中斷接口，采用邊沿觸發(fā)方式觸發(fā)中斷事件的發(fā)生。中斷事件啟動(dòng)計(jì)時(shí)器工作。當(dāng)通過(guò)串口中斷方式接收到IMU數(shù)據(jù)時(shí)，讀取計(jì)時(shí)器的值，即可得到IMU數(shù)據(jù)相對(duì)于PPS的延遲時(shí)間△τ。

結(jié)論

本文基于MSP430F149單片機(jī)設(shè)計(jì)的室外移動(dòng)機(jī)器人組合導(dǎo)航定位系統(tǒng)，通過(guò)接口的擴(kuò)展使得該款定位系統(tǒng)能夠接入IMU、GNSS接收機(jī)、氣壓計(jì)三路信息，完成初步導(dǎo)航定位服務(wù)功能，同時(shí)可作為多路數(shù)據(jù)采集設(shè)備，將多路數(shù)據(jù)整合到一路高速輸出接口，用于進(jìn)一步的高精度導(dǎo)航解算。該系統(tǒng)根據(jù)使用者的需求不同，可接入不同成本和精度的設(shè)備，只要滿足RS-232協(xié)議即可。筆者將其實(shí)際運(yùn)用，整個(gè)系統(tǒng)充分利用該款單片機(jī)的資源，結(jié)構(gòu)簡(jiǎn)單、功耗低、適用范圍廣，不僅可作為初步導(dǎo)航定位服務(wù)的設(shè)備，還可作為多路數(shù)據(jù)采集設(shè)備。