基于DSP的穩(wěn)定平臺設(shè)計
摘要:為了消除海水運動對需要保持穩(wěn)定姿態(tài)物體的影響,設(shè)計了兩軸穩(wěn)定平臺。該穩(wěn)定平臺以TMS320F28335DSP為核心微處理器,采用了多傳感器采集、伺服控制技術(shù)等;并架設(shè)了嵌入式操作系統(tǒng)μC/OS-Ⅱ來管理多任務(wù)穩(wěn)定平臺系統(tǒng)。實驗結(jié)果表明,該兩軸穩(wěn)定平臺在穩(wěn)定性及動態(tài)性能上均能滿足要求,起到了隔離海水運動的目的。
關(guān)鍵詞:穩(wěn)定平臺;DSP;捷聯(lián)慣性系統(tǒng);μC/OS-Ⅱ
0 引言
在海面上要求保持物體水平狀態(tài)時,由于海浪的影響,將導(dǎo)致物體的姿態(tài)隨海浪的波動而變化。兩軸穩(wěn)定平臺的設(shè)計正是基于隔離海水運動的目的,在平面內(nèi)保持物體的水平狀態(tài)。隨著傳感器技術(shù)、嵌入式控制技術(shù)等多項技術(shù)的應(yīng)用,穩(wěn)定平臺也得到了廣泛的發(fā)展。國外在穩(wěn)定平臺方面的發(fā)展已經(jīng)趨向小型化、數(shù)字化、集成化。近年來我國對穩(wěn)定跟蹤平臺的研究也開始增多,有多個科研單位對穩(wěn)定跟蹤平臺開展研究,已經(jīng)在國防、科研及民用各領(lǐng)域發(fā)揮了重要作用。該機載穩(wěn)定平臺通過嵌入式DSP系統(tǒng)的運算及控制,建立了測量與控制系統(tǒng)。以μC/OS-Ⅱ操作系統(tǒng)來管理多任務(wù)平臺,從而實現(xiàn)了穩(wěn)定平臺的智能化、穩(wěn)定性、快速和精確性。
1 系統(tǒng)原理
穩(wěn)定平臺的工作原理為通過姿態(tài)測量得出當(dāng)前實時姿態(tài)信息,通過驅(qū)動伺服電機的轉(zhuǎn)動,調(diào)節(jié)上平臺面達到穩(wěn)定姿態(tài)。當(dāng)平臺受海水運動所產(chǎn)生的力矩干擾,其姿態(tài)會發(fā)生變化,偏離穩(wěn)定位置,通過姿態(tài)測量可以得出橫滾角及俯仰角信息。姿態(tài)測量系統(tǒng)布局在穩(wěn)定平臺的上平臺面,隨著上平臺面的運動而運動。受上平臺面尺寸方面的限制,系統(tǒng)布局如圖1所示。
捷聯(lián)慣性測量系統(tǒng)采用三陀螺、三加速度計組合的方式,構(gòu)成了測姿系統(tǒng)的載體坐標(biāo)系:其中ax,ay,az分別為三加速度計在載體坐標(biāo)系三正交軸的加速度輸出,ωx,ωy,ωz分別為三陀螺儀在載體坐標(biāo)系三正交軸上的角速度輸出。
傳感器的布局設(shè)計構(gòu)成了測姿系統(tǒng)的載體坐標(biāo)系,而最終的平臺姿態(tài)信息是相對于地理坐標(biāo)系來說的,所以必須將測得的載體三軸向加速度和載體三軸向角速度轉(zhuǎn)換到地理坐標(biāo)系。
在上平臺面初始靜態(tài)條件下,三加速度計輸出值與重力加速度之比的反余弦值即為載體的初始狀態(tài)值,定義為俯仰角θ0和橫滾角γ0。得出初始姿態(tài)之后,便根據(jù)三陀螺儀輸出的三角速度ωx,ωy,ωz進行姿態(tài)解算。姿態(tài)矩陣解算采用最典型的四元數(shù)法。四元數(shù)法中,載體坐標(biāo)系相對地理坐標(biāo)系的轉(zhuǎn)動可以看作是剛體定點轉(zhuǎn)動,其基本表達式用來轉(zhuǎn)動四元數(shù)Q來表示,即:
式中:q0,q1,q2,q3為轉(zhuǎn)換系數(shù);i,j,k為三軸轉(zhuǎn)換向量。故要求載體坐標(biāo)系到地理坐標(biāo)系的轉(zhuǎn)換矩陣,需要解下列四元數(shù)運動方程:
式中:Q為四元數(shù)矢量矩陣,用以描述載體坐標(biāo)系相對于地理坐標(biāo)系的姿態(tài)變化量;W(ω)為載體坐標(biāo)系相對地理坐標(biāo)系的轉(zhuǎn)動角速度在載體坐標(biāo)系上的投影,也就是前面解算出來的載體三個軸向上的角速率。
[!--empirenews.page--]
2 控制系統(tǒng)硬件設(shè)計
考慮到穩(wěn)定平臺系統(tǒng)對數(shù)據(jù)處理方面的嚴(yán)格要求,核心微處理器選擇TI公司的TMS320F28335數(shù)字信號處理器。TMS320F28335是TI公司到目前為止用于數(shù)字控制領(lǐng)域最好的DSP芯片,它具有浮點運算、集成度高、片上資源豐富、運算速度快等特點。平臺系統(tǒng)硬件架構(gòu)如圖2所示。
姿態(tài)測量系統(tǒng)采用三陀螺、三加速度計組合的方式,通過SPI接口與高速DSP處理器TMS320F28335相連接,用于在兩者之間傳輸數(shù)字信息。其中共有4線相連,分別為:串行時鐘線SCLK、主機輸入/從機輸出數(shù)據(jù)線MISO、主機輸出/從機輸人數(shù)據(jù)線MOSI和低電平有效的從機選擇線SS。SPI為全雙工通信,具有傳輸速率快,簡單高效等優(yōu)點。三陀螺、三加速度計均采用3.3 V電壓供電,由系統(tǒng)電源提供。系統(tǒng)時鐘采用30 MHz的外部晶體給CPU提供時鐘,并通過使能片上PLL電路及控制寄存器的修改得到所需的時鐘頻率。
伺服控制系統(tǒng)采用TMS320F28335作為核心控制器。F28335有12個增強型脈寬調(diào)制模塊,通過增強型脈寬調(diào)制模塊ePWM的使用,從而將姿態(tài)測量系統(tǒng)測得的具體姿態(tài)角信息轉(zhuǎn)換為不同占空比的方波。由于同時存在橫滾角及俯仰角姿態(tài)信息,必須建立橫滾伺服控制系統(tǒng)、俯仰伺服控制系統(tǒng)。橫滾及俯仰方向上的方波經(jīng)PWM功率放大器放大后,分別轉(zhuǎn)換為橫搖驅(qū)動驅(qū)動及縱搖驅(qū)動。兩路驅(qū)動可采用不同頻率的方波,控制伺服電機產(chǎn)生不同的轉(zhuǎn)速,在一定時間內(nèi),將姿態(tài)角信息轉(zhuǎn)換為滾珠絲杠相應(yīng)的轉(zhuǎn)動,從而達到平臺穩(wěn)定的目的。
3 系統(tǒng)軟件設(shè)計
由于穩(wěn)定平臺采用并聯(lián)方式驅(qū)動,因此結(jié)構(gòu)在橫搖、縱搖兩個系統(tǒng)上采用并發(fā)執(zhí)行的方式。對于簡單多任務(wù)系統(tǒng)的穩(wěn)定控制,可采用嵌入式操作系統(tǒng)μC/OS-Ⅱ?qū)ο到y(tǒng)多任務(wù)進行管理和調(diào)度,以滿足并發(fā)控制的要求。
3.1 μC/OS-Ⅱ
在嵌入式操作系統(tǒng)領(lǐng)域,μC/OS-Ⅱ以其源代碼開放、研究免費、強實時性等特點被廣泛應(yīng)用。它是一種基于優(yōu)先級的硬實時可剝奪型多任務(wù)內(nèi)核,在多任務(wù)管理上表現(xiàn)卓越,而且在可移植性、裁剪等方面也具有優(yōu)越的性能。已有成千上萬的開發(fā)者把它成功地應(yīng)用于各種系統(tǒng),安全性和穩(wěn)定性也已經(jīng)得到認(rèn)證,現(xiàn)已經(jīng)通過美國FAA認(rèn)證。因此將μC/OS-Ⅱ應(yīng)用于多任務(wù)管理的穩(wěn)定平臺系統(tǒng)是非常合適的。
3.2 多任務(wù)管理
穩(wěn)定平臺系統(tǒng)主要執(zhí)行以下任務(wù):DSP硬件初始化、姿態(tài)測量、姿態(tài)數(shù)據(jù)處理、自適應(yīng)PID解算、ePWM模塊、橫搖驅(qū)動和縱搖驅(qū)動。在μC/OS-Ⅱ操作系統(tǒng)管理下,任務(wù)管理分為:DSP硬件初始化、μC/OS-Ⅱ系統(tǒng)初始化、定時中斷、橫搖調(diào)整和縱搖調(diào)整。
操作系統(tǒng)第一步執(zhí)行DSP硬件的初始化工作,包括設(shè)置系統(tǒng)的中斷向量、初始化數(shù)字I/O、串行通信接口SPI、定時器模塊等。完成后將開始操作系統(tǒng)的初始化,通過調(diào)用OSIint()完成操作系統(tǒng)的配置及數(shù)據(jù)的初始化。接著通過調(diào)用任務(wù)創(chuàng)建函數(shù)OSTaskCreat()函數(shù),依次創(chuàng)建定時中斷、橫搖調(diào)整和縱搖調(diào)整3個任務(wù)。調(diào)用OSStart()最終啟動多任務(wù)運行。
各任務(wù)優(yōu)先級設(shè)定從高到低依次為:橫搖調(diào)整、縱搖調(diào)整、定時中斷,各任務(wù)間通過消息郵箱機制來實現(xiàn)各個任務(wù)間的同步。開始運行時,通過DSP定時器每50 ms一個周期的定時中斷,使得定時中斷任務(wù)就緒運行,該任務(wù)通過消息郵箱機制使得橫搖調(diào)整及縱搖調(diào)整任務(wù)處于就緒態(tài),并同時掛起等待下一個定時中斷。橫搖調(diào)整及縱搖調(diào)整主要完成的工作有姿態(tài)測量、姿態(tài)數(shù)據(jù)處理、自適應(yīng)PID解算、ePWM模塊、橫搖驅(qū)動和縱搖驅(qū)動。橫搖調(diào)整同縱搖調(diào)整相似,最后控制的相應(yīng)驅(qū)動分別為橫搖驅(qū)動、縱搖驅(qū)動。其中縱搖調(diào)整任務(wù)流程如圖3所示:
[!--empirenews.page--]
橫搖調(diào)整及縱搖調(diào)整在一個周期內(nèi)完成相應(yīng)的平臺姿態(tài)調(diào)整之后,因等待定時中斷發(fā)送的消息郵箱而掛起,待下一個消息到來之后再一次對平臺姿態(tài)進行周期性的調(diào)整,保持平臺在周期范圍內(nèi)的穩(wěn)定。系統(tǒng)中數(shù)據(jù)采集與處理的部分關(guān)鍵代碼如下所示。
自適應(yīng)PID控制程序?qū)崿F(xiàn)流程圖如圖4所示。
4 結(jié)語
本文通過嵌入式控制系統(tǒng)的搭建,實現(xiàn)了穩(wěn)定平臺隔離海水運動的目的。該穩(wěn)定平臺以DSP TMS320F28335為核心微處理器,使用多傳感器測量技術(shù),對兩自由度上實行模糊自適應(yīng)PID控制,并通過架設(shè)嵌入式操作系統(tǒng)μC/OS-Ⅱ來管理并行橫搖及縱搖任務(wù),通過搖擺臺的模擬實驗,該穩(wěn)定平臺在穩(wěn)定性上達到了預(yù)期的要求。