提高IMU的精度和速度的硬件設計和程序優(yōu)化

【摘要】在導航系統(tǒng)中，慣性測量組件(IMU)的精度和速度嚴重影響到導航計算機的性能。為了提高IMU系統(tǒng)的精度和速度，提出了在硬件方面采用高速AD轉(zhuǎn)換器和DSP，同時在程序中采用了基于LD分解的Kalman濾波技術，并采用線性匯編語言對濾波程序進行了優(yōu)化。系統(tǒng)的可靠性和速度得到很大的提高。

【關鍵詞】IMU DSP Kalman濾波 線性匯編語言

【中圖分類號】TP314【文獻標識碼】A【文章編號】1009-8585(2011)01-00-03

1 引言

捷聯(lián)慣性測量組件(IMU)在捷聯(lián)導航系統(tǒng)中所起的作用是相當重要的，其誤差參數(shù)標定的準確性以及速度直接影響著導航精度。載體的導航信息包括角速度和加速度，當這種導航信息發(fā)生變化時，陀螺儀和加速度計就會檢測到這種變化量，并進行相關的轉(zhuǎn)化，最后以脈沖數(shù)大小的形式來表示這種變化量的大小。數(shù)據(jù)采集卡就是用于在采樣時間內(nèi)統(tǒng)計脈沖的數(shù)目，并利用預先測定的標度因子進行解算處理，進而得出直觀的導航信息。因而，這里對采集卡速度和精度就有較高的要求。然而，隨著對IMU體積的要求，僅僅依靠硬件電路的設計來提高系統(tǒng)的速度和精度已經(jīng)難以達到要求，這就需要改進算法程序。

2 系統(tǒng)硬件電路

本系統(tǒng)主要采用了三個陀螺和三個加速度計、高速AD器件和核心處理器DSP。由于需要采集六路模擬量，而系統(tǒng)空間有限以及節(jié)省硬件開銷，故需要對輸入的信號進行依次選通，最后當所有的數(shù)據(jù)都采集完畢以后，再對數(shù)據(jù)進行補償處理，其基本框圖如圖1所示:

為了提高系統(tǒng)的速度，本系統(tǒng)采用了高速AD器件ADS5521和高性能核心處理器件TMS320C6713。ADS5521是一種高性能12位模數(shù)轉(zhuǎn)換器，它的采樣速度可以達到105MSPS，僅采用3.3V單電源供電，并且是內(nèi)置參考電壓。TMS320C6713芯片的關鍵性能特點是:最高可達225MHz的時鐘頻率;264K的在上數(shù)據(jù)存儲器，存儲資源足夠大;憑借超常指令字結構和二級高速緩存，DSP的性能可達2400MIPS，最大1800MFLOPS，速度非常快;

該系統(tǒng)采用這兩種芯片，在很大程度上可以提高系統(tǒng)的速度，但是系統(tǒng)的運行程序也會影響整個IMU的速度和精度，因此必須充分考慮程序的執(zhí)行時間和效率。

3 系統(tǒng)軟件設計以及優(yōu)化

DSP程序是系統(tǒng)進行數(shù)據(jù)處理的核心模塊。根據(jù)AD輸入的陀螺和加速度計信號進行誤差補償，然后轉(zhuǎn)化為RS422形式并發(fā)送給外部設備。整個系統(tǒng)根據(jù)功能要求主要分為兩個模塊:引導程序bootloader模塊和用戶程序模塊。

軟件結構如圖2所示。

引導程序Boot模塊:DSP的系統(tǒng)引導裝載(Boot)是指在系統(tǒng)加電時，由DSP將一段存儲在外部非易失存儲器中的代碼移植到高速存儲器單元中執(zhí)行。引導程序負責將中斷向量表和用戶代碼段從片外FLASH移植到其它高速存儲器中。

用戶程序模塊:首先數(shù)據(jù)采集程序采集陀螺和加速度計的數(shù)據(jù)，然后對陀螺和加速度計的數(shù)據(jù)進行誤差補償處理，最后將補償后的數(shù)據(jù)通過RS422發(fā)送程序發(fā)送到系統(tǒng)的外部。參見圖3。

程序中最消耗時間的是數(shù)據(jù)處理程序中的誤差補償部分，在補償濾波程序中，為了防止發(fā)散，我們采用了LD分解的Kalman濾波技術，該技術的應用同時也提高的系統(tǒng)的精度。但是，該方法不僅增加了代碼的長度，而且消耗了大量的時間，因此要對次部分程序做大量的優(yōu)化工作。

3.1 利用編輯器選項優(yōu)化

C6000的C優(yōu)化器是TI 公司的一個創(chuàng)新，它對程序循環(huán)的流水處理使得C代碼的效率得到很大的提高，，只要在編譯時加入優(yōu)化項即可。

使用-mt編譯選項，使用此選項是函數(shù)在參數(shù)傳遞過程中，C編譯器會假設存儲器關聯(lián)消除，從而能夠更好的優(yōu)化。該系統(tǒng)中采用最高級別的優(yōu)化選項進行編譯: cl6x-o3-pm file.c-z。

3.2 采用C程序和匯編程序混合編程

混合編程的一般方法有三種:一是對C程序編譯后形成的匯編程序進行手工的修改與優(yōu)化。二是直接在C語言程序中嵌入?yún)R編語句。三是分別編寫C程序和匯編程序，再獨立編譯成目標代碼模塊，再進行鏈接。第三種方式是混合編程最常用的方式之一，也是我們系統(tǒng)采用的方式。在這種方式下，C程序與匯編程序均可使用另一方定義的函數(shù)與變量。

變量定義的規(guī)則與數(shù)組、指針的使用變量定義是混合編程的基本問題。C程序與匯編程序定義的變量相互之間可以進行訪問。在匯編程序中定義時，需要在變量前加下劃線“_”，然后再用.global定義為全局變量，在C程序中則需要說明為extern變量。

以下程序是為了完成LD分解的Kalman濾波的線性匯編程序而編寫的部分程序:

程序1:矩陣乘法

線性匯編程序

.global _mpyMatix

_ mpyMatrix:

.cproc a，b，c，n

.reg l_out，l_mid，l_in，i，j，sum，k，

v1，v2，v11，v21，v12，a_i，b_i，c_i

MV n，l_out

ZEROi

outloop: ZERO j

MV n，l_mid

midloop: ZEROsum

ZEROk

MV n，l_in

inloop:MPY i，n，v1

MPY k，n，v2

ADDv1，k，v11

ADDv2，j，v21

ADDv1，j，v12

LDW*+a[v11]，a_i

LDW*+b[v21]，b_i

MPYa_i，b_i，c_i

ADDsum，c_i，sum

ADDk，1，k

[l_in] SUBl_in，1，l_in

[l_in] B inloop

STWsum，*+c[v12]

ADDj，1，j

[l_mid]SUBl_mid，1，l_mid

[l_mid]B midloop

ADDi，1，i

[l_out]SUBl_out，1，l_out

[l_out]B outloop

.endproc

C程序中的聲明和調(diào)用

double mpyMatix(double *a， double *b， double *c，int n);

mpyMatix (a[0][0]，b[0][0]，c[0][0]，n);

程序2:LD分解

.global _LDdis

_LDdis:

.eproc n，pmatrix，lmatrix，dmatrix

.reg i，j，k，m_loop1，m_loop2，m_loop3，

m_loop4， sum1，constant1，constant2， v1，v2，v3，

v， x，x1，y

ZERO i

MVn，m_loop1

Loop1:MPYn，i，x

ADDx，i，x

MVK1，constant1

STWconstant1，*+lmatrix[x]

ADDi，1，i

[m_loop1] SUBm_loop1，1，m_loop1

[m_loop1] B loop1

LDW*+pmatrix[0]，x

STWx，*+dmatrix[0]

MVn，m_loop2

SUBm_loop2，1，m_loop2

MVconstant1，i

RCPDP x，constant2

Loop2:MPYn，i，x

LDW*+pmatrix[x]，v1

MPYv1，constant2，v2

STWv2，*+lmatrix[x]

ADDi，1，i

[m_loop2] SUBm_loop2，1，m_loop2

[m_loop2] B loop2

MVn，m_loop3

SUBm_loop3，1，m_loop3

MVK 1，j

Loop3: MPYj，n，x1

MPYk，n，y

ZERO sum1

ZERO k

MVj，m_loop4

Loop4: ADDx1，k，x

ADDy，k，y

LDW*+lmatrix[x]，v1

LDW*+dmatrix[y]，v2

MPYv1，v2，v

MPYv，v1，v

ADDsum1，v，sum1

ADDk，1，k

[m_loop4]SUBm_loop4，1，m_loop4

[m_loop4]B m_loop4

ADDx1，j，x1

LDW*+pmatrix[x1]，v3

SUBv3，sum1，v3

STWv3，*+pmatix[x1]

ADDj，1，j

[m_loop3]SUBm_loop3，1，m_loop3

[m_loop3]B loop3

…

…

由于空間有限，只列寫了一部分該程序。

4 結論

本系統(tǒng)在硬件電路上采用了高速AD器件和DSP芯片，同時在程序設計上也采取了適當?shù)膬?yōu)化措施，對于消耗時間比較長的誤差處理補償程序，采用了線性匯編語言編寫，雖然增加了代碼的長度，但是卻大大提高了系統(tǒng)的時間。

參考文獻

[1] 裴聽國，李漢舟.全數(shù)字IMU方案[J].中國慣性技術學報.2003.12:69－83.

[2] 李璞，蔡同英.捷聯(lián)測量裝置的數(shù)據(jù)采集技術[J].應用科技.2004.4:48-50.

[3] 陽明曄， 張志勇. 基于TMS320C6000系列DSP的C代碼優(yōu)化方法研究[J].微處理機.2004.4:59-64.

[4] 張淑艷，劉偉寧.TIC6200系列DSP代碼優(yōu)化技術研究[J].計算機工程.2006.1:82-86.

[5] 李方慧，王飛等.TMS320C600系列DSPs原理與應用[M].北京:電子工業(yè)出版社.2003.197-314.