杜偉偉 許江寧 何泓洋

（海軍工程大學(xué)電氣工程學(xué)院 武漢 430033）

1 引言

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）是以牛頓經(jīng)典力學(xué)為依據(jù)的自主式導(dǎo)航系統(tǒng)，只需要對慣性測量單元（Inertial Measurement Unit,IMU）輸出結(jié)果進行積分就能得到導(dǎo)航信息。目前慣性導(dǎo)航系統(tǒng)常見做法是采用嵌入式DSP（Digital Signal Processing,DSP）或者FPGA（Field-Programmable Gate Array,FPGA）實現(xiàn)積分以及導(dǎo)航解算。DSP在嵌入式領(lǐng)域具有體積小，功耗低，計算能力強等特點多用于復(fù)雜計算系統(tǒng)中處理數(shù)字信號燈問題。DSP是TI(Texas Instruments,TI)公司于20世紀(jì)80年代設(shè)計制作的專用于數(shù)字信號處理的專用芯片。得益于其對數(shù)字信號處理能力強于其他CPU或微處理器，因此在通訊系統(tǒng)，信號處理系統(tǒng)，自動控制系統(tǒng)，航天、航空、軍事等領(lǐng)域得到廣泛應(yīng)用。本文介紹了基于DSP 28335平臺多種優(yōu)化方法，本文提出程序設(shè)計優(yōu)化技巧以及DSP特有優(yōu)化方式。通過多種優(yōu)化方式提升系統(tǒng)整體性能。

文獻(xiàn)［1～6]重點介紹了DSP環(huán)境下C語言編程優(yōu)化方式，從數(shù)據(jù)類型，數(shù)據(jù)操作以及變量定義方向進行優(yōu)化，在語言優(yōu)化其他方面稍有不足不能完全發(fā)揮DSP性能優(yōu)勢；文獻(xiàn)［7～9]提出基于高性能DSP平臺和FPGA平臺共同完成捷聯(lián)慣性導(dǎo)航系統(tǒng)的設(shè)計，利用FPGA對硬件時序控制優(yōu)勢采集信號，將數(shù)字信號通過特定接口輸出至DSP芯片，由DSP芯片完成算法，實現(xiàn)高頻、高速的慣導(dǎo)解算。由于文獻(xiàn)中系統(tǒng)采用FPGA以及高性能DSP芯片導(dǎo)致系統(tǒng)硬件成本較高，在對成本有一定要求的系統(tǒng)中使用會受到限制。

因此，本文針對DSP 28335平臺針對有限的系統(tǒng)資源進行優(yōu)化問題，開展了程序設(shè)計以及優(yōu)化方面的研究，對捷聯(lián)慣導(dǎo)解算程序進行了優(yōu)化。從而提升了程序運行效率?？梢酝ㄟ^較低的硬件成本，實現(xiàn)慣導(dǎo)高速、高頻解算。

2 捷聯(lián)慣導(dǎo)系統(tǒng)基本理論

2.1 坐標(biāo)系定義

對文中實際提到的坐標(biāo)系統(tǒng)定義如下：

1）地心慣性坐標(biāo)系統(tǒng)（i系）：坐標(biāo)系原點位于地球中心O，xi軸指向地球春分放向，zi軸為地球自轉(zhuǎn)軸，yi、xi、zi軸構(gòu)成右手坐標(biāo)系。

2）地球坐標(biāo)系（e系）：與地球固定。原點位于地心，xe軸穿過地球赤道和本初子午線交點，ye穿過地球東經(jīng)90°子午線與赤道交點，ze軸川渝地球北極點。

3）導(dǎo)航坐標(biāo)系（n系）：坐標(biāo)原點O位于載體質(zhì)心，xn、yn、zn分別指向載體所在地的東、北、天。

2.2 慣性導(dǎo)航力學(xué)編排

捷聯(lián)慣導(dǎo)系統(tǒng)是一種將加速度計和陀螺儀直接固定在載體上的一種慣性導(dǎo)航系統(tǒng)，計算機根據(jù)載體固定的加速度計測量線性運動信息和陀螺儀測量的角運動信息解算出載體的速度、姿態(tài)、運動方向、以及載體位置信息。圖1為捷聯(lián)式慣性導(dǎo)航系統(tǒng)工作原理圖。

圖1 捷聯(lián)式慣性導(dǎo)航系統(tǒng)工作原理圖

導(dǎo)航計算原理根據(jù)固定在載體的陀螺和加速度計所輸出的角運動信息和線運動信息進行計算，角運動信息計算得出姿態(tài)轉(zhuǎn)移矩陣，線運動信息輸出的比力減去誤差速度得到載體運動速度，經(jīng)過姿態(tài)轉(zhuǎn)移矩陣，將速度轉(zhuǎn)為導(dǎo)航坐標(biāo)系下，進行導(dǎo)航解算就能得到速度、姿態(tài)以及位置坐標(biāo)信息。

慣性導(dǎo)航系統(tǒng)基本解算方程有速度更新、位置更新、姿態(tài)更新。姿態(tài)更新解算方法目前主流的有四元數(shù)姿態(tài)更新法、歐拉角法、等效旋轉(zhuǎn)矢量法［10]。由于四元數(shù)姿態(tài)更新法相比其他算法計算量小，避免萬向節(jié)死鎖和奇異點的問題出現(xiàn)［11]。

慣導(dǎo)解算包括位置解算、速度解算與姿態(tài)解算。如下所示。

位置解算方程：

其中L、λ、h分別為載體所在位置的地理坐標(biāo)系的緯度、經(jīng)度、高度，vn為載體所在導(dǎo)航坐標(biāo)系速度，為向東速度為向北速度為向天速度。

速度解算方程：

2.3 四元數(shù)姿態(tài)更新方法

四元數(shù)旋轉(zhuǎn)可以避免歐拉角中萬向節(jié)鎖現(xiàn)象，而且只需要一個四維的四元數(shù)即可實現(xiàn)任意繞過原點的向量旋轉(zhuǎn)，在某些情況下比旋轉(zhuǎn)矩陣效率更高。四元數(shù)相比歐拉旋轉(zhuǎn)可以實現(xiàn)球面平滑插值［12]。

2.3.1 四元數(shù)定義

四元數(shù)表示姿態(tài)，就是用四個元構(gòu)成的數(shù)，可以表示為

其中q0、q1、q2、q3為實數(shù)，q0為四元數(shù)的實部，qv=q1i+q2j+q3k為四元數(shù)的虛部。i、j、k是具有互相正交關(guān)系的單位向量同時又是虛單位四元數(shù)可以看做是復(fù)數(shù)的擴充，有的地方稱為超復(fù)數(shù)。四元數(shù)之間運算規(guī)則滿足加法交換律和加法結(jié)合律，但不滿足乘法交換律也不滿足乘法結(jié)合律。

2.3.2 歐拉角與四元數(shù)

分別定義俯仰角、偏航角、橫滾角為繞X軸、Y軸、Z軸的夾角分別為α、β、γ，旋轉(zhuǎn)軸的方向可以表示完成一個單位矢量

則有固定點剛體通過繞該點的某個軸旋轉(zhuǎn)角度θ到達(dá)新姿態(tài)，則描述該旋轉(zhuǎn)角度的四元數(shù)Q表示為

由式（6）得出歐拉角到四元數(shù)的轉(zhuǎn)換公式：

2.3.3 四元數(shù)姿態(tài)更新方程

慣導(dǎo)數(shù)據(jù)是定時采樣，固定時間間隔輸出。為了減少噪聲在積分中的影響。系統(tǒng)直接將角增量來確定四元數(shù)［10]。

四元數(shù)姿態(tài)更新方程

為了降低計算復(fù)雜度對式（5）進行展開得到三階近似算法：

其中I為單位矩陣。

3 基于DSP 28335的慣導(dǎo)解算設(shè)計與優(yōu)化

3.1 基于DSP 28335的程序設(shè)計

DSP28335芯片有兩個帶數(shù)據(jù)緩沖功能的高速輸入輸出串口，和一個不帶數(shù)據(jù)緩沖功能的普通串口。本文程序設(shè)計只使用其中兩個帶數(shù)據(jù)緩沖功能的高速串口：第一個串口用于接收陀螺和加速度計數(shù)據(jù)；第二個串口用于接收上位機命令數(shù)據(jù)并且發(fā)送慣導(dǎo)解算數(shù)據(jù)。串口中斷程序負(fù)責(zé)接收串口數(shù)據(jù)，主程序負(fù)責(zé)解析串口協(xié)議以及執(zhí)行相應(yīng)串口命令和解算慣導(dǎo)數(shù)據(jù)，并控制串口發(fā)送數(shù)據(jù)。

DSP 28335主程序如圖2所示，系統(tǒng)啟動后首先初始化芯片資源設(shè)置看門狗，設(shè)置芯片主頻，初始化全局變量，初始化GPIO口，初始化串口，初始化系統(tǒng)中斷，進入程序循環(huán)，判斷串口緩存區(qū)是否存儲串口數(shù)據(jù)，如果串口緩存區(qū)存在數(shù)據(jù)就進行串口協(xié)議解析，當(dāng)串口數(shù)據(jù)滿足協(xié)議時進行相應(yīng)分析，如果是上位機設(shè)置命令則進行相應(yīng)參數(shù)設(shè)置，參數(shù)設(shè)置完畢后發(fā)送參數(shù)設(shè)置應(yīng)答；如果是來自慣導(dǎo)數(shù)據(jù)則進行相應(yīng)串口解算，將解算結(jié)果通過串口發(fā)送上位機。

圖2 DSP 28335慣導(dǎo)解算系統(tǒng)主程序流程圖

當(dāng)有串口收到數(shù)據(jù)后DSP芯片會產(chǎn)生中斷信號，調(diào)用相應(yīng)中斷函數(shù)。中斷函數(shù)流程如圖3所示，首先讀串口數(shù)據(jù)，然后將串口數(shù)據(jù)添加緩存區(qū)中，最后清除中斷標(biāo)志，表示串口中斷處理完成。

圖3 DSP 28335慣導(dǎo)解算系統(tǒng)串口中斷函數(shù)流程圖

3.2 算法優(yōu)化

通過算法計算原理進行優(yōu)化，這一步可以極大減少計算量也是最重要的優(yōu)化手段。先分析計算原理和過程，有目的性減少計算量。例如慣導(dǎo)解算公式（10）

通常做法：

第一步計算Δθ2=Δθ×Δθ；

第二步計算 -Δθ2=-1×Δθ2；

第三步創(chuàng)建單位矩陣I；

第四步將單位矩陣每一個元素和-Δθ2相乘。

優(yōu)化后：

第一步計算Δθ2=Δθ×Δθ；

第二步計算 -Δθ2=0-Δθ2；

第三步程序初始化定義值為0的矩陣I0緯度和矩陣I一致；

第四步將-Δθ2直接賦值于I0對角線元素。

通常做法計算原理過于呆板導(dǎo)致計算量過大，本文利用DSP系統(tǒng)加減法速度優(yōu)于乘除法的優(yōu)勢，用 -Δθ2=0-Δθ2代替 -Δθ2=-1×Δθ2降低系統(tǒng)運算時間，并且利用單位矩陣只有對角線元素為1，其余為0的特性以及1乘任何數(shù)等于其本身，0乘任何數(shù)等于0，直接將-Δθ2賦值于I0矩陣對角線元素免除乘計算。再次降低系統(tǒng)計算量，提升系統(tǒng)性能。

3.3 循環(huán)優(yōu)化

C語言優(yōu)化過程中最主要的手段是從循環(huán)語句著手重點優(yōu)化，采用高效循環(huán)方式，或者減少循環(huán)次數(shù)，亦可不用循環(huán)語句。例如C語言中for循環(huán)：

for(表達(dá)式1;表達(dá)式2;表達(dá)式3)

{

語句;

}

進入循環(huán)前先執(zhí)行表達(dá)式1，在判斷表達(dá)式2，如果表達(dá)式2位真執(zhí)行for循環(huán)中的語句，如果為假則調(diào)出循環(huán)。當(dāng)循環(huán)語句執(zhí)行完后則執(zhí)行表達(dá)式3。

可以看出每執(zhí)行一次循環(huán)語句都要判斷一次表達(dá)式2和執(zhí)行一次表達(dá)式3。則優(yōu)化方法就是減小循環(huán)語句，如果循環(huán)次數(shù)較少則不使用循環(huán)語句。

例如C語言程序中Bk矩陣為4×4,該矩陣很小可以不使用for循環(huán)語句，而是直接執(zhí)行表達(dá)式：

以上語句雖然代碼量比雖然較長，但是在進行優(yōu)化前計算次數(shù)為278次，改為優(yōu)化后的代碼計算次數(shù)為16次，大大減少計算時間。

3.4 變量優(yōu)化

C語言編程時在函數(shù)中往往會定義比較多的局部變量，每定義一個局部變量CPU調(diào)用該函數(shù)時就會執(zhí)行壓棧出棧操作。算法中的變量比較多，而在普通PC機的CPU性能十分強大可忽略壓棧出棧執(zhí)行語句的耗時。但DSP芯片主頻有限就需要盡可能優(yōu)化減少不必要的指令。因此將函數(shù)中的中間變量全部定義為全局變量，這樣又能提升系統(tǒng)性能。

3.5 DSP系統(tǒng)優(yōu)化

DSP系統(tǒng)對Flash儲存區(qū)訪問速度遠(yuǎn)遠(yuǎn)慢于對系統(tǒng)內(nèi)部RAM（Random Access Memory，RAM）的訪問，因此要大大提升DSP運行速度就要在系統(tǒng)啟動時將程序從flash區(qū)搬移至RAM區(qū)運行，這樣可以大大減少解算時間，對系統(tǒng)性能提升具有非常重要意義。DSP系統(tǒng)還可以通過編譯器編譯選項優(yōu)化，TI公司自帶DSP開發(fā)環(huán)境CCS（Code Composer Studio，CCS）編譯器提供編譯選項-O3最高等級編譯優(yōu)化，此時編譯效率已經(jīng)接近純匯編模式，因此沒有必要對代碼進行匯編語言的優(yōu)化。

4 試驗驗證

通過示波器波形分析對比優(yōu)化前后的程序執(zhí)行時間，圖4未優(yōu)化Flash運行解算時間，圖5優(yōu)化后Flash運行解算時間，圖6優(yōu)化后RAM運行解算時間。

圖4 源碼未優(yōu)化Flash運行解算時間

圖5 源碼優(yōu)化后Flash運行解算時間

圖6 源碼優(yōu)化后RAM允許解算時間

源碼未經(jīng)過優(yōu)化程序在Flash上運行時慣導(dǎo)解算時間為4.13ms，源碼經(jīng)過優(yōu)化程序在Flash上運行時慣導(dǎo)解算時間為3.940ms，對比程序優(yōu)化解算時間減少0.19ms。源碼優(yōu)化后讓程序在DSP系統(tǒng)內(nèi)部RAM上運行解算時間為0.928ms，對比解算時間減少3.012ms。通過整體優(yōu)化共節(jié)約時間3.202ms。

5 結(jié)語

本文通過慣導(dǎo)解算程序進行設(shè)計優(yōu)化，針對DSP 28335平臺的優(yōu)化進行詳細(xì)分析介紹，并通過試驗及實測數(shù)據(jù)驗證了優(yōu)化的有效性。通過程序優(yōu)化可以使系統(tǒng)運行效率有小幅度提升。試驗驗證慣導(dǎo)解算時間節(jié)約3.940ms，性能提升7.8%。通過對DSP 28335系統(tǒng)優(yōu)化，將程序搬移至DSP內(nèi)部RAM區(qū)運行慣性導(dǎo)航導(dǎo)解算時間節(jié)約2.952ms，性能提升76%，使系統(tǒng)性能滿足高頻、高速的慣導(dǎo)解算要求，并得出以下結(jié)論：通過程序優(yōu)化對DSP系統(tǒng)性能提升有限；通過對DSP系統(tǒng)優(yōu)化能夠獲得較大幅度性能提升。因此DSP系統(tǒng)優(yōu)化是慣導(dǎo)解算中一種重要的優(yōu)化方式，程序優(yōu)化雖然對系統(tǒng)性能提升有限，依然是DSP系統(tǒng)優(yōu)化中重要方式之一。遇到對系統(tǒng)要求更加嚴(yán)苛的項目，還可以通過其他優(yōu)化方式提升系統(tǒng)性能。

TI公司DSP芯片種類繁多，但無論系統(tǒng)設(shè)計采用哪種芯片想要最大化獲得性能輸出，就必須對系統(tǒng)進行整體優(yōu)化，最大可能性地利用芯片提供資源。