基于DSP的穩(wěn)定平臺(tái)設(shè)計(jì)

摘要：為了消除海水運(yùn)動(dòng)對(duì)需要保持穩(wěn)定姿態(tài)物體的影響，設(shè)計(jì)了兩軸穩(wěn)定平臺(tái)。該穩(wěn)定平臺(tái)以TMS320F28335DSP為核心微處理器，采用了多傳感器采集、伺服控制技術(shù)等；并架設(shè)了嵌入式操作系統(tǒng)μC／OS-Ⅱ來(lái)管理多任務(wù)穩(wěn)定平臺(tái)系統(tǒng)。實(shí)驗(yàn)結(jié)果表明，該兩軸穩(wěn)定平臺(tái)在穩(wěn)定性及動(dòng)態(tài)性能上均能滿足要求，起到了隔離海水運(yùn)動(dòng)的目的。
關(guān)鍵詞：穩(wěn)定平臺(tái)；DSP；捷聯(lián)慣性系統(tǒng)；μC／OS-Ⅱ

0 引言
    在海面上要求保持物體水平狀態(tài)時(shí)，由于海浪的影響，將導(dǎo)致物體的姿態(tài)隨海浪的波動(dòng)而變化。兩軸穩(wěn)定平臺(tái)的設(shè)計(jì)正是基于隔離海水運(yùn)動(dòng)的目的，在平面內(nèi)保持物體的水平狀態(tài)。隨著傳感器技術(shù)、嵌入式控制技術(shù)等多項(xiàng)技術(shù)的應(yīng)用，穩(wěn)定平臺(tái)也得到了廣泛的發(fā)展。國(guó)外在穩(wěn)定平臺(tái)方面的發(fā)展已經(jīng)趨向小型化、數(shù)字化、集成化。近年來(lái)我國(guó)對(duì)穩(wěn)定跟蹤平臺(tái)的研究也開(kāi)始增多，有多個(gè)科研單位對(duì)穩(wěn)定跟蹤平臺(tái)開(kāi)展研究，已經(jīng)在國(guó)防、科研及民用各領(lǐng)域發(fā)揮了重要作用。該機(jī)載穩(wěn)定平臺(tái)通過(guò)嵌入式DSP系統(tǒng)的運(yùn)算及控制，建立了測(cè)量與控制系統(tǒng)。以μC／OS-Ⅱ操作系統(tǒng)來(lái)管理多任務(wù)平臺(tái)，從而實(shí)現(xiàn)了穩(wěn)定平臺(tái)的智能化、穩(wěn)定性、快速和精確性。

1 系統(tǒng)原理
    穩(wěn)定平臺(tái)的工作原理為通過(guò)姿態(tài)測(cè)量得出當(dāng)前實(shí)時(shí)姿態(tài)信息，通過(guò)驅(qū)動(dòng)伺服電機(jī)的轉(zhuǎn)動(dòng)，調(diào)節(jié)上平臺(tái)面達(dá)到穩(wěn)定姿態(tài)。當(dāng)平臺(tái)受海水運(yùn)動(dòng)所產(chǎn)生的力矩干擾，其姿態(tài)會(huì)發(fā)生變化，偏離穩(wěn)定位置，通過(guò)姿態(tài)測(cè)量可以得出橫滾角及俯仰角信息。姿態(tài)測(cè)量系統(tǒng)布局在穩(wěn)定平臺(tái)的上平臺(tái)面，隨著上平臺(tái)面的運(yùn)動(dòng)而運(yùn)動(dòng)。受上平臺(tái)面尺寸方面的限制，系統(tǒng)布局如圖1所示。

    捷聯(lián)慣性測(cè)量系統(tǒng)采用三陀螺、三加速度計(jì)組合的方式，構(gòu)成了測(cè)姿系統(tǒng)的載體坐標(biāo)系：其中ax，ay，az分別為三加速度計(jì)在載體坐標(biāo)系三正交軸的加速度輸出，ωx，ωy，ωz分別為三陀螺儀在載體坐標(biāo)系三正交軸上的角速度輸出。
    傳感器的布局設(shè)計(jì)構(gòu)成了測(cè)姿系統(tǒng)的載體坐標(biāo)系，而最終的平臺(tái)姿態(tài)信息是相對(duì)于地理坐標(biāo)系來(lái)說(shuō)的，所以必須將測(cè)得的載體三軸向加速度和載體三軸向角速度轉(zhuǎn)換到地理坐標(biāo)系。
    在上平臺(tái)面初始靜態(tài)條件下，三加速度計(jì)輸出值與重力加速度之比的反余弦值即為載體的初始狀態(tài)值，定義為俯仰角θ0和橫滾角γ0。得出初始姿態(tài)之后，便根據(jù)三陀螺儀輸出的三角速度ωx，ωy，ωz進(jìn)行姿態(tài)解算。姿態(tài)矩陣解算采用最典型的四元數(shù)法。四元數(shù)法中，載體坐標(biāo)系相對(duì)地理坐標(biāo)系的轉(zhuǎn)動(dòng)可以看作是剛體定點(diǎn)轉(zhuǎn)動(dòng)，其基本表達(dá)式用來(lái)轉(zhuǎn)動(dòng)四元數(shù)Q來(lái)表示，即：
   
    式中：q0，q1，q2，q3為轉(zhuǎn)換系數(shù)；i，j，k為三軸轉(zhuǎn)換向量。故要求載體坐標(biāo)系到地理坐標(biāo)系的轉(zhuǎn)換矩陣，需要解下列四元數(shù)運(yùn)動(dòng)方程：
   
    式中：Q為四元數(shù)矢量矩陣，用以描述載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的姿態(tài)變化量；W(ω)為載體坐標(biāo)系相對(duì)地理坐標(biāo)系的轉(zhuǎn)動(dòng)角速度在載體坐標(biāo)系上的投影，也就是前面解算出來(lái)的載體三個(gè)軸向上的角速率。
    [!--empirenews.page--]

2 控制系統(tǒng)硬件設(shè)計(jì)
    考慮到穩(wěn)定平臺(tái)系統(tǒng)對(duì)數(shù)據(jù)處理方面的嚴(yán)格要求，核心微處理器選擇TI公司的TMS320F28335數(shù)字信號(hào)處理器。TMS320F28335是TI公司到目前為止用于數(shù)字控制領(lǐng)域最好的DSP芯片，它具有浮點(diǎn)運(yùn)算、集成度高、片上資源豐富、運(yùn)算速度快等特點(diǎn)。平臺(tái)系統(tǒng)硬件架構(gòu)如圖2所示。

    姿態(tài)測(cè)量系統(tǒng)采用三陀螺、三加速度計(jì)組合的方式，通過(guò)SPI接口與高速DSP處理器TMS320F28335相連接，用于在兩者之間傳輸數(shù)字信息。其中共有4線相連，分別為：串行時(shí)鐘線SCLK、主機(jī)輸入／從機(jī)輸出數(shù)據(jù)線MISO、主機(jī)輸出／從機(jī)輸人數(shù)據(jù)線MOSI和低電平有效的從機(jī)選擇線SS。SPI為全雙工通信，具有傳輸速率快，簡(jiǎn)單高效等優(yōu)點(diǎn)。三陀螺、三加速度計(jì)均采用3．3 V電壓供電，由系統(tǒng)電源提供。系統(tǒng)時(shí)鐘采用30 MHz的外部晶體給CPU提供時(shí)鐘，并通過(guò)使能片上PLL電路及控制寄存器的修改得到所需的時(shí)鐘頻率。
    伺服控制系統(tǒng)采用TMS320F28335作為核心控制器。F28335有12個(gè)增強(qiáng)型脈寬調(diào)制模塊，通過(guò)增強(qiáng)型脈寬調(diào)制模塊ePWM的使用，從而將姿態(tài)測(cè)量系統(tǒng)測(cè)得的具體姿態(tài)角信息轉(zhuǎn)換為不同占空比的方波。由于同時(shí)存在橫滾角及俯仰角姿態(tài)信息，必須建立橫滾伺服控制系統(tǒng)、俯仰伺服控制系統(tǒng)。橫滾及俯仰方向上的方波經(jīng)PWM功率放大器放大后，分別轉(zhuǎn)換為橫搖驅(qū)動(dòng)驅(qū)動(dòng)及縱搖驅(qū)動(dòng)。兩路驅(qū)動(dòng)可采用不同頻率的方波，控制伺服電機(jī)產(chǎn)生不同的轉(zhuǎn)速，在一定時(shí)間內(nèi)，將姿態(tài)角信息轉(zhuǎn)換為滾珠絲杠相應(yīng)的轉(zhuǎn)動(dòng)，從而達(dá)到平臺(tái)穩(wěn)定的目的。

3 系統(tǒng)軟件設(shè)計(jì)
    由于穩(wěn)定平臺(tái)采用并聯(lián)方式驅(qū)動(dòng)，因此結(jié)構(gòu)在橫搖、縱搖兩個(gè)系統(tǒng)上采用并發(fā)執(zhí)行的方式。對(duì)于簡(jiǎn)單多任務(wù)系統(tǒng)的穩(wěn)定控制，可采用嵌入式操作系統(tǒng)μC／OS-Ⅱ?qū)ο到y(tǒng)多任務(wù)進(jìn)行管理和調(diào)度，以滿足并發(fā)控制的要求。
3．1 μC／OS-Ⅱ
    在嵌入式操作系統(tǒng)領(lǐng)域，μC／OS-Ⅱ以其源代碼開(kāi)放、研究免費(fèi)、強(qiáng)實(shí)時(shí)性等特點(diǎn)被廣泛應(yīng)用。它是一種基于優(yōu)先級(jí)的硬實(shí)時(shí)可剝奪型多任務(wù)內(nèi)核，在多任務(wù)管理上表現(xiàn)卓越，而且在可移植性、裁剪等方面也具有優(yōu)越的性能。已有成千上萬(wàn)的開(kāi)發(fā)者把它成功地應(yīng)用于各種系統(tǒng)，安全性和穩(wěn)定性也已經(jīng)得到認(rèn)證，現(xiàn)已經(jīng)通過(guò)美國(guó)FAA認(rèn)證。因此將μC／OS-Ⅱ應(yīng)用于多任務(wù)管理的穩(wěn)定平臺(tái)系統(tǒng)是非常合適的。
3．2 多任務(wù)管理
    穩(wěn)定平臺(tái)系統(tǒng)主要執(zhí)行以下任務(wù)：DSP硬件初始化、姿態(tài)測(cè)量、姿態(tài)數(shù)據(jù)處理、自適應(yīng)PID解算、ePWM模塊、橫搖驅(qū)動(dòng)和縱搖驅(qū)動(dòng)。在μC／OS-Ⅱ操作系統(tǒng)管理下，任務(wù)管理分為：DSP硬件初始化、μC／OS-Ⅱ系統(tǒng)初始化、定時(shí)中斷、橫搖調(diào)整和縱搖調(diào)整。

    操作系統(tǒng)第一步執(zhí)行DSP硬件的初始化工作，包括設(shè)置系統(tǒng)的中斷向量、初始化數(shù)字I／O、串行通信接口SPI、定時(shí)器模塊等。完成后將開(kāi)始操作系統(tǒng)的初始化，通過(guò)調(diào)用OSIint()完成操作系統(tǒng)的配置及數(shù)據(jù)的初始化。接著通過(guò)調(diào)用任務(wù)創(chuàng)建函數(shù)OSTaskCreat()函數(shù)，依次創(chuàng)建定時(shí)中斷、橫搖調(diào)整和縱搖調(diào)整3個(gè)任務(wù)。調(diào)用OSStart()最終啟動(dòng)多任務(wù)運(yùn)行。
     各任務(wù)優(yōu)先級(jí)設(shè)定從高到低依次為：橫搖調(diào)整、縱搖調(diào)整、定時(shí)中斷，各任務(wù)間通過(guò)消息郵箱機(jī)制來(lái)實(shí)現(xiàn)各個(gè)任務(wù)間的同步。開(kāi)始運(yùn)行時(shí)，通過(guò)DSP定時(shí)器每50 ms一個(gè)周期的定時(shí)中斷，使得定時(shí)中斷任務(wù)就緒運(yùn)行，該任務(wù)通過(guò)消息郵箱機(jī)制使得橫搖調(diào)整及縱搖調(diào)整任務(wù)處于就緒態(tài)，并同時(shí)掛起等待下一個(gè)定時(shí)中斷。橫搖調(diào)整及縱搖調(diào)整主要完成的工作有姿態(tài)測(cè)量、姿態(tài)數(shù)據(jù)處理、自適應(yīng)PID解算、ePWM模塊、橫搖驅(qū)動(dòng)和縱搖驅(qū)動(dòng)。橫搖調(diào)整同縱搖調(diào)整相似，最后控制的相應(yīng)驅(qū)動(dòng)分別為橫搖驅(qū)動(dòng)、縱搖驅(qū)動(dòng)。其中縱搖調(diào)整任務(wù)流程如圖3所示：

[!--empirenews.page--]
    橫搖調(diào)整及縱搖調(diào)整在一個(gè)周期內(nèi)完成相應(yīng)的平臺(tái)姿態(tài)調(diào)整之后，因等待定時(shí)中斷發(fā)送的消息郵箱而掛起，待下一個(gè)消息到來(lái)之后再一次對(duì)平臺(tái)姿態(tài)進(jìn)行周期性的調(diào)整，保持平臺(tái)在周期范圍內(nèi)的穩(wěn)定。系統(tǒng)中數(shù)據(jù)采集與處理的部分關(guān)鍵代碼如下所示。
   
    自適應(yīng)PID控制程序?qū)崿F(xiàn)流程圖如圖4所示。

4 結(jié)語(yǔ)
    本文通過(guò)嵌入式控制系統(tǒng)的搭建，實(shí)現(xiàn)了穩(wěn)定平臺(tái)隔離海水運(yùn)動(dòng)的目的。該穩(wěn)定平臺(tái)以DSP TMS320F28335為核心微處理器，使用多傳感器測(cè)量技術(shù)，對(duì)兩自由度上實(shí)行模糊自適應(yīng)PID控制，并通過(guò)架設(shè)嵌入式操作系統(tǒng)μC／OS-Ⅱ來(lái)管理并行橫搖及縱搖任務(wù)，通過(guò)搖擺臺(tái)的模擬實(shí)驗(yàn)，該穩(wěn)定平臺(tái)在穩(wěn)定性上達(dá)到了預(yù)期的要求。