基于DSP28335的慣導(dǎo)解算設(shè)計(jì)與優(yōu)化*

杜偉偉 許江寧 何泓洋

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

1 引言

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

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

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

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

2.1 坐標(biāo)系定義

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

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

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

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

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

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

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

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

慣性導(dǎo)航系統(tǒng)基本解算方程有速度更新、位置更新、姿態(tài)更新。姿態(tài)更新解算方法目前主流的有四元數(shù)姿態(tài)更新法、歐拉角法、等效旋轉(zhuǎn)矢量法［10]。由于四元數(shù)姿態(tài)更新法相比其他算法計(jì)算量小，避免萬(wàn)向節(jié)死鎖和奇異點(diǎn)的問(wèn)題出現(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)可以避免歐拉角中萬(wàn)向節(jié)鎖現(xiàn)象，而且只需要一個(gè)四維的四元數(shù)即可實(shí)現(xiàn)任意繞過(guò)原點(diǎn)的向量旋轉(zhuǎn)，在某些情況下比旋轉(zhuǎn)矩陣效率更高。四元數(shù)相比歐拉旋轉(zhuǎn)可以實(shí)現(xiàn)球面平滑插值［12]。

2.3.1 四元數(shù)定義

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

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

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

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

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

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

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

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

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

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

其中I為單位矩陣。

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

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

DSP28335芯片有兩個(gè)帶數(shù)據(jù)緩沖功能的高速輸入輸出串口，和一個(gè)不帶數(shù)據(jù)緩沖功能的普通串口。本文程序設(shè)計(jì)只使用其中兩個(gè)帶數(shù)據(jù)緩沖功能的高速串口：第一個(gè)串口用于接收陀螺和加速度計(jì)數(shù)據(jù)；第二個(gè)串口用于接收上位機(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)啟動(dòng)后首先初始化芯片資源設(shè)置看門(mén)狗，設(shè)置芯片主頻，初始化全局變量，初始化GPIO口，初始化串口，初始化系統(tǒng)中斷，進(jìn)入程序循環(huán)，判斷串口緩存區(qū)是否存儲(chǔ)串口數(shù)據(jù)，如果串口緩存區(qū)存在數(shù)據(jù)就進(jìn)行串口協(xié)議解析，當(dāng)串口數(shù)據(jù)滿足協(xié)議時(shí)進(jìn)行相應(yīng)分析，如果是上位機(jī)設(shè)置命令則進(jìn)行相應(yīng)參數(shù)設(shè)置，參數(shù)設(shè)置完畢后發(fā)送參數(shù)設(shè)置應(yīng)答；如果是來(lái)自慣導(dǎo)數(shù)據(jù)則進(jìn)行相應(yīng)串口解算，將解算結(jié)果通過(guò)串口發(fā)送上位機(jī)。

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

當(dāng)有串口收到數(shù)據(jù)后DSP芯片會(huì)產(chǎn)生中斷信號(hào)，調(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)化

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

通常做法：

第一步計(jì)算Δθ2=Δθ×Δθ；

第二步計(jì)算 -Δθ2=-1×Δθ2；

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

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

優(yōu)化后：

第一步計(jì)算Δθ2=Δθ×Δθ；

第二步計(jì)算 -Δθ2=0-Δθ2；

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

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

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

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

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

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

{

語(yǔ)句;

}

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

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

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

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

3.4 變量?jī)?yōu)化

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

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

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

4 試驗(yàn)驗(yàn)證

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

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

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

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

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

5 結(jié)語(yǔ)

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

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