AUV水下機(jī)器人運(yùn)動控制系統(tǒng)設(shè)計(李思樂)

歡迎閱讀本文檔，希望本文檔能對您有所幫助!歡迎閱讀本文檔，希望本文檔能對您有所幫助!歡迎閱讀本文檔，希望本文檔能對您有所幫助!歡迎閱讀本文檔，希望本文檔能對您有所幫助!歡迎閱讀本文檔，希望本文檔能對您有所幫助!歡迎閱讀本文檔，希望本文檔能對您有所幫助!中國海洋大學(xué)工程學(xué)院機(jī)械電子工程研究生課程考核論文題目：AUV水下機(jī)器人運(yùn)動控制系統(tǒng)研究報告課程名稱：運(yùn)動控制技術(shù)姓名：李思樂學(xué)號：21100933077院系：工程學(xué)院機(jī)電工程系專業(yè)：機(jī)械電子工程時間：2010-12-26課程成績：任課老師：譚俊哲AUV水下機(jī)器人運(yùn)動控制系統(tǒng)設(shè)計摘要：以主推加舵控制的小型自治水下機(jī)器人為研究對象，建立了水下機(jī)器人的數(shù)學(xué)模型并進(jìn)行了分析。根據(jù)機(jī)器人結(jié)構(gòu)的特點(diǎn)，對模型進(jìn)行了必要的簡化。設(shè)計了機(jī)器人的運(yùn)動控制系統(tǒng)。以成功研制的無纜自治水下機(jī)器人(AUV)為基礎(chǔ)，對其航行控制和定位控制方法進(jìn)行了較詳細(xì)的分析.同時介紹了它的推進(jìn)器布置、控制系統(tǒng)結(jié)構(gòu)、推力分配等方法。最后展示了它的運(yùn)行實驗結(jié)果。關(guān)鍵詞：水下機(jī)器人；總體設(shè)計方案；運(yùn)動控制系統(tǒng)；電機(jī)仿真1引言近年來國外水下機(jī)器人技術(shù)發(fā)展迅速，技術(shù)水平較高。其中，具有代表性的產(chǎn)品有：美國VideoRay公司開發(fā)出的Scout、Explorer、Pro等系列遙控式水下機(jī)器人，美國Seabotix公司研發(fā)的LBV-ROV系列，英國AC-CESS公司的AC-ROV系列。隨著海洋開發(fā)、探測的需求越來越強(qiáng)，水下機(jī)器人成為全世界研究的熱門課題。小型自治水下機(jī)器人具有低成本、小型化、操作靈活等特點(diǎn)成為近年來國內(nèi)外研究的熱點(diǎn)。自治水下機(jī)器人（AutonomousUnderwaterVehicles,AUV）,載體采用模塊化設(shè)計思想,可根據(jù)需要適當(dāng)增減作業(yè)或傳感器模塊,載體采用魚雷狀流線外形,總長約2m,外徑25cm,基本模塊包括推進(jìn)器模塊、能源模塊、電子艙模塊、傳感器模塊以及GPS、無線電通訊模塊,基本傳感器有姿態(tài)傳感器、高度計、深度計和視覺傳感器,支持光纖通訊,載體可外掛聲學(xué)設(shè)備,通過光纖系統(tǒng)進(jìn)行遙控操作可實現(xiàn)其半自主作業(yè),也可在預(yù)編程指令下實現(xiàn)自主作業(yè)。系統(tǒng)基本模塊組成設(shè)計如圖1-1所示[1]。它具有開放式、模塊化的體系結(jié)構(gòu)和多種控制方式（自主/半自主/遙控），自帶能源。這種小型水下機(jī)器人可在大范圍、大深度和復(fù)雜海洋環(huán)境下進(jìn)行海洋科學(xué)研究和深海資源調(diào)查，具有更廣泛的應(yīng)用前景。在控制系統(tǒng)的設(shè)計過程中充分考慮了系統(tǒng)的穩(wěn)定性和操縱性。控制器具有足夠的魯棒性來克服建模誤差，以及水動力參數(shù)變化。圖1-1系統(tǒng)基本模塊組成設(shè)計2機(jī)器人物理模型2.1AUV物理模型為了研究AUV的運(yùn)動規(guī)律，確定運(yùn)行過程中AUV的位置和姿態(tài)，需要建立AUV的動力學(xué)模型。為了便于分析，建立適合于描述AUV運(yùn)動的兩種參考坐標(biāo)系，即固定坐標(biāo)系Eξηζ和運(yùn)動坐標(biāo)系Oxyz，如圖2-1所示：包含5個推進(jìn)器，分別是艉部的2個主推進(jìn)器、艉部的1個垂向推進(jìn)器和艏部的2個垂向推進(jìn)器。左右對稱于縱中剖面，上和下、前和后都不對稱[2]。圖2-1AUV水下機(jī)器人物理模型1.2微小型水下機(jī)器人動力學(xué)分析微小型水下機(jī)器人總長1.5m，采用鋰電池作為能源，尾部為一對水平舵和一對垂直舵，單槳推進(jìn)，可攜帶慣導(dǎo)設(shè)備、探測聲納、水下攝像機(jī)、深度計等設(shè)備，設(shè)計巡航速度約2節(jié)。首先建立適合描述水下機(jī)器人空間運(yùn)動的坐標(biāo)系，其定義如圖2-2所示，慣性坐標(biāo)系為E?ξη?，運(yùn)動坐標(biāo)系為o?xyz。建立的坐標(biāo)系，如圖1所示。圖中：E-ξηζ—慣性坐標(biāo)系；Oxyz—載體坐標(biāo)系。因為機(jī)器人在航行時速度不高（<4節(jié)），可以對機(jī)器人模型進(jìn)行線性化及一些簡化。載體坐標(biāo)系原點(diǎn)取于載體浮心處，在此坐標(biāo)系下，載體在三個方向上的受力及運(yùn)動量表達(dá)為：力：F=T力矩：M=[K,M,N]T速度：V=[u,v,w]T角速率：ω=[p,q,r]T。圖2-2慣性和載體坐標(biāo)系在圖2-2定義的慣性坐標(biāo)系和運(yùn)動坐標(biāo)系中，機(jī)器人的空間運(yùn)動向量表達(dá)為：η1=［xyz］T；η2=［φθψ］Tυ1=［uvw］T；υ2=［pqr］T式中：向量η1—機(jī)器人在慣性坐標(biāo)系中的位置；η2—其在慣性坐標(biāo)系中的姿態(tài)；φ—橫滾角；θ—俯仰角；ψ—航向角；υ1—機(jī)器人在載體坐標(biāo)系中的線速度（V）；2—其在載體坐標(biāo)系中的轉(zhuǎn)動角速度（ω）[3]。2總體方案設(shè)計2.1系統(tǒng)組成及工作原理小型水下觀測機(jī)器人主要由人機(jī)交互平臺、上位系統(tǒng)、下位系統(tǒng)、攝像機(jī)四部分組成，操作人員通過有線遙控，結(jié)合人機(jī)交互界面上的水下視頻圖像，只需扳動上位系統(tǒng)控制面板上相應(yīng)的運(yùn)動控制按鈕即可實現(xiàn)對水下機(jī)器人的運(yùn)動控制，操作簡單、實用。圖2-3為水下機(jī)器人控制系統(tǒng)框圖：圖2-3水下機(jī)器人控制系統(tǒng)框圖2.2導(dǎo)航系統(tǒng)設(shè)計捷聯(lián)慣性導(dǎo)航是最常見和應(yīng)用最廣泛的導(dǎo)航系統(tǒng)，捷聯(lián)式慣性導(dǎo)航系統(tǒng)在工作時不依賴外界信息，也不向外界輻射能量，不易受到干擾破壞，是一種自主式導(dǎo)航系統(tǒng)。但單獨(dú)使用很難滿足水下航行所需的導(dǎo)航精度與定位要求，僅靠提高慣性傳感器的性能來提高的導(dǎo)航、定位精度是非常有限的。組合導(dǎo)航系統(tǒng)融合不同類型的導(dǎo)航傳感器的信息，使它們優(yōu)勢互補(bǔ)，經(jīng)過卡爾曼濾波，得出系統(tǒng)導(dǎo)航參數(shù)的最優(yōu)估計，以獲得比使用單一導(dǎo)航系統(tǒng)更高的性能和導(dǎo)航精度。采用磁羅盤和深度計分別與捷聯(lián)慣導(dǎo)系統(tǒng)構(gòu)成的組合量測值作為卡爾曼濾波的量測值，既可以用精度高的子系統(tǒng)的信息修正慣導(dǎo)誤差，又可以用慣導(dǎo)對動態(tài)響應(yīng)慢的子系統(tǒng)作補(bǔ)償和校正，從而綜合發(fā)揮各自優(yōu)點(diǎn)。導(dǎo)航計算機(jī)在保證導(dǎo)航運(yùn)算速度和精度的同時，還要具有豐富的外設(shè)接口，方便與外部多傳感器進(jìn)行數(shù)據(jù)通信[4]。導(dǎo)航系統(tǒng)的傳感器包括慣性測量器件IMU（陀螺儀和加速度計）、磁羅盤、深度計。其中IMU通過三陀螺儀、三加速度計捷聯(lián)解算后獲得位置、速度、姿態(tài)共9維信息，通過RS232串口與導(dǎo)航計算機(jī)相連。以NEMA0183格式傳輸信息到導(dǎo)航計算機(jī)。磁羅盤可以獲取當(dāng)前載體三維姿態(tài)信息，通過RS232串口與導(dǎo)航計算機(jī)相連，以NEMA0183格式傳輸信息到導(dǎo)航計算機(jī)。深度計為液壓變送器，通過膜片感應(yīng)內(nèi)外側(cè)水壓差來確定水深，其輸出為4～20mA模擬電流信號，轉(zhuǎn)換為0～5V的電壓信號后經(jīng)過16位ADC轉(zhuǎn)換模塊，串行傳送到導(dǎo)航計算機(jī)。如圖2-4所示。圖2-4組合導(dǎo)航系統(tǒng)總體設(shè)計框圖2.3驅(qū)動方式的選用幾乎所有的水下機(jī)器人都采用螺旋槳式推進(jìn)器。80%以上采用電機(jī)推進(jìn)器，其余采用油壓電機(jī)推進(jìn)器。水下機(jī)器人要實現(xiàn)水下空間的六維（六自由度）運(yùn)動，即三個平移運(yùn)動：推進(jìn)（Surge，沿x軸）、升沉（Heave，沿z軸）、橫移（Sway，沿y軸）和三個回轉(zhuǎn)運(yùn)動：轉(zhuǎn)首（Yaw，繞z軸）、縱傾（Pitch，繞y軸）、橫搖（Roll，繞x軸）。為使水下機(jī)器人在所有六維上的運(yùn)動都是可控的，須適當(dāng)選用推進(jìn)器的數(shù)量和給予不同的布置。根據(jù)本水下機(jī)器人的使用目的，不需要使用六維運(yùn)動，只要三個自由度即可，即推進(jìn)、升沉和轉(zhuǎn)首。我們選用了五個直流電機(jī)推進(jìn)器，分別布置在機(jī)器人本體的水平左右兩側(cè)和后部部垂直處，左右推進(jìn)器完成推進(jìn)和轉(zhuǎn)首兩個動作，垂推進(jìn)器完成升沉動作。2.4推力器的組成推力器是由電機(jī)和螺旋槳組成的，水下機(jī)器人用的電機(jī)需要密封。密封主要有兩種方式，一種是機(jī)械密封，另一種采用磁耦合器。機(jī)械密封相對而言比較簡單，但因密封處要承受海水的壓力，其特性因摩擦力的增加而變壞。對電機(jī)來說，則表現(xiàn)為電機(jī)的空載電流增大（有時會增大1-3倍），這樣的電機(jī)用于推力器，會使啟動電壓升高，從而加重推力器非線性。為了改善這種情況可以采用充油電機(jī)，由于電機(jī)內(nèi)部充油，因而耐水壓的性能得到極大的改善，而且電機(jī)因密封而引起的摩擦力要小得多，其空載電流的增加也很小，故可以忽略不計。采用磁耦合器就是利用電磁力傳遞扭矩，這樣減速器和螺旋槳之間沒有直接的機(jī)械聯(lián)系。依據(jù)磁場傳遞扭矩，密封問題很容易解決，只要用非導(dǎo)磁材料將電機(jī)、減速器包圍起來就解決了動密封難題。采用磁耦合器，推力器的效率略有下降，但性能基本上不受影響。電機(jī)的轉(zhuǎn)速與螺旋槳的轉(zhuǎn)速不一定完全匹配，為了得到較高的效率，需要采用減速器，有時為了減小尺寸，采用高速電機(jī)（例如采用10000轉(zhuǎn)/分以上的高速電機(jī)），這時也需要減速器。這樣組成的推力器如圖2-5所示。圖2-5推力器組成圖2-6螺旋槳與推進(jìn)器示意圖考慮帶定子的導(dǎo)管槳在無限寬廣的靜止流體域中工作的情況。設(shè)流體為理想且不可壓縮。如圖2-6所示，建立固定于導(dǎo)管上的直角坐標(biāo)系O-xyz，以螺旋槳槳葉參考線與槳軸交點(diǎn)為原點(diǎn)，x軸與槳軸中心線重合，指向槳的下游，y軸垂直向上，z軸方向由右手法則確定。推進(jìn)器工作時，導(dǎo)管、定子與槳共同沿x軸負(fù)方向以勻速V0前進(jìn)，同時槳葉繞x軸以等角速度Ω旋轉(zhuǎn)[5]。2.5能源供給方式的選用其能源供給方式有兩種選擇：有纜方式或無纜方式，對于無纜水下機(jī)器人能源供給一般在機(jī)器人艙體安裝蓄電池或是帶燃油發(fā)電機(jī)組，這就造成水下機(jī)器人本體體積龐大、超重，此外蓄電池所儲存的能力有限，且受電池質(zhì)量、充電工藝等因素的影響。根據(jù)實際應(yīng)用環(huán)境，此機(jī)器人工作所要求的行走距離不是很大，故設(shè)計時采用了有纜遠(yuǎn)程遙控方式，這樣既可減小本體尺寸、重量，又保證了控制操作的有效性和可靠性，當(dāng)設(shè)備出現(xiàn)不可預(yù)料的故障時可通過纜線撤回安全區(qū)域，不至于丟失。3控制系統(tǒng)設(shè)計3.1水下機(jī)器人控制系統(tǒng)設(shè)計主要包含主處理器核心模塊、電機(jī)驅(qū)動模塊、傳感器模塊和視頻切換模塊等，實現(xiàn)對機(jī)器人推進(jìn)器的動力驅(qū)動、上下位機(jī)的通訊以及視頻圖像的切換等。水下機(jī)器人本體的左右兩邊各安裝2個主推進(jìn)器，分別由2個直流電機(jī)通過聯(lián)動軸與螺旋槳相連，實現(xiàn)水下機(jī)器人前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)運(yùn)動；垂直方向安裝有3個垂推進(jìn)器，實現(xiàn)機(jī)器人上升、下沉運(yùn)動。前變焦攝像機(jī)安裝有垂直方向一維云臺，避免攝像死區(qū)。在機(jī)器人電子艙內(nèi)安裝有深度計、溫度計和數(shù)字式電子羅盤傳感器，滿足實際作業(yè)環(huán)境需要，為檢修人員提供了豐富的作業(yè)環(huán)境信息。根據(jù)功能需要，我們選擇了TI公司推出的MSP430系列的MSP430F149作為主處理器，這是一類具有16位總線的帶FLASH的單片機(jī)，由于其性價比和集成度高，受到了廣大技術(shù)開發(fā)人員的青睞。該控制器可以在超低功耗模式下工作，對環(huán)境和人體的輻射小，可靠性能好，加強(qiáng)電干擾運(yùn)行不受影響，適應(yīng)工業(yè)級的運(yùn)行環(huán)境[6]。利用MSP430F149定時器B比較單元產(chǎn)生的6路PWM信號和5路方向信號，分別控制主推進(jìn)器、垂推進(jìn)器、機(jī)械手、攝像機(jī)云臺電機(jī)速度和照明燈亮度；兩路串口實現(xiàn)了羅盤數(shù)據(jù)的采集和上位系統(tǒng)的通訊；外部傳感器反饋的模擬信息通過ADC模塊實現(xiàn)轉(zhuǎn)化，使芯片豐富的外設(shè)資源得以充分利用?？偩€型結(jié)構(gòu)的所有節(jié)點(diǎn)都共享一個公共的物理通道(即總線)。具有延遲小、速度快、易擴(kuò)展、單個節(jié)點(diǎn)故障影響小的優(yōu)點(diǎn)。本系統(tǒng)即采用總線型拓?fù)浣Y(jié)構(gòu),系統(tǒng)采用單片機(jī)作為控制單元完成機(jī)器人控制系統(tǒng)中的各種控制任務(wù)(如傳感器控制、電機(jī)驅(qū)動器控制和通信模塊控制等)。各功能單元直接掛接在CAN總線上,成為控制網(wǎng)絡(luò)中的一個節(jié)點(diǎn)，從而構(gòu)成多主機(jī)結(jié)構(gòu),即每個節(jié)點(diǎn)均為一個主機(jī)，通過CAN通信協(xié)議協(xié)同完成控制任務(wù)。將CAN總線的分布式控制網(wǎng)絡(luò)應(yīng)用于水下機(jī)器人中?？偩€上各節(jié)點(diǎn)完成不同的任務(wù)和功能，它們大部分時間并行工作，必要時通過總線與其他節(jié)點(diǎn)進(jìn)行通信。如圖3-1所示。CAN總線是一種多主總線,理論上任何一個節(jié)點(diǎn)都可以作為主節(jié)點(diǎn)。在控制系統(tǒng)中設(shè)置與上位PC機(jī)相連的節(jié)點(diǎn)1為上位節(jié)點(diǎn),其它節(jié)點(diǎn)為底層節(jié)點(diǎn)。PC機(jī)通過串口與節(jié)點(diǎn)1上的CPU通信,CPU再與CAN收發(fā)器通信,實現(xiàn)信息在CAN總線上的發(fā)送與接收。節(jié)點(diǎn)1與12個底層節(jié)點(diǎn)根據(jù)應(yīng)用的不同具有不同的功能,但它們都具有與CAN總線通信的能力,上傳數(shù)據(jù)和接收數(shù)據(jù)。圖3-1CAN總線通信節(jié)點(diǎn)的硬件結(jié)構(gòu)及接口設(shè)計框圖CAN總線上各節(jié)點(diǎn)可采用Philips公司生產(chǎn)的單片機(jī)P87C591，除完成節(jié)點(diǎn)自身的控制功能外，還實現(xiàn)了CAN通信接口。P87C591兼容80C51指令集，并成功地集成了Philips公司的SJAl000CAN控制器，該嵌入式CAN控制器具有以下特點(diǎn)：(1)完全符合CAN2.0規(guī)范，控制CAN幀的接收和發(fā)送；(2)CAN接口包含5個實現(xiàn)CPU與CAN控制器連接的特殊功能寄存器；(3)CAN控制器的發(fā)送緩沖區(qū)能夠保存一個完整的CAN信息幀(擴(kuò)展或標(biāo)準(zhǔn)幀)，通過CPU啟動發(fā)送,信息就從發(fā)送緩沖區(qū)傳輸?shù)紺AN內(nèi)核模塊；(4)當(dāng)接收一個信息時，CAN內(nèi)核模塊將串行數(shù)據(jù)流轉(zhuǎn)換成并行數(shù)據(jù)傳輸?shù)津炇諡V波器,通過該可編程濾波器，P87C591確認(rèn)接收需要的信息。圖3-2為以P87C591為核心的CAN總線接口電路。P1.0、P1.1分別對應(yīng)CAN的收(RxDC)和發(fā)(TxDC)線。圖3-2CAN總線接口電路CAN總線收發(fā)器選用Philips公司的PCA82C250，以差分方式發(fā)送。引腳8與地之間的電阻為斜率電阻，其取值決定了系統(tǒng)處于高速方式、斜率控制方式或待機(jī)方式。本系統(tǒng)中PCA82C250工作于斜率方式，通過Rs對地連接的電阻對總線進(jìn)行斜率控制，以控制射頻干擾。使用雙絞線作為傳輸介質(zhì)。通過高速光耦6N137將P87C591與CAN總線收發(fā)器進(jìn)行光電隔離，以增強(qiáng)CAN總線節(jié)點(diǎn)的抗干擾能力?？偩€末端加124Ω的終端匹配電阻，以減少信號反射干擾[6]。3.2水上控制箱控制系統(tǒng)設(shè)計設(shè)計中，我們采用了常見的小型控制箱作為岸上控制平臺，其外形尺寸為372mm×266mm×134mm，具有體積小，攜帶方便的特點(diǎn)。其內(nèi)部硬件模塊主要包含主處理器核心模塊和液晶顯示模塊，完成上下位機(jī)間的通訊，視頻圖像、傳感器信息的顯示，實現(xiàn)人機(jī)交互。其結(jié)構(gòu)框圖如圖3-3所示。圖3-3水上控制箱控制系統(tǒng)在該控制系統(tǒng)中采用MSP430F135作為主處理器，充分應(yīng)用了其內(nèi)部集成的14路12位模數(shù)轉(zhuǎn)換器和串口通訊模塊，操作人員只需通過設(shè)定控制面板上各按鍵、搖桿，經(jīng)AD轉(zhuǎn)換，將各模擬量轉(zhuǎn)換為數(shù)字量，經(jīng)串口通訊模塊來實現(xiàn)控制箱對下位控制系統(tǒng)各推進(jìn)器速度、方向的控制、照明燈的亮度調(diào)節(jié)以及攝像機(jī)云臺的運(yùn)動設(shè)定。3.3控制系統(tǒng)軟件設(shè)計上位系統(tǒng)程序主要完成系統(tǒng)上電初始化，控制面板各按鍵、搖桿信息的采集，液晶模塊顯示，與下位系統(tǒng)的通訊等工作。它采用順序式結(jié)構(gòu)往復(fù)運(yùn)行。運(yùn)行過程中可以被中斷子程序中斷，執(zhí)行完中斷子程序之后返回斷點(diǎn)處繼續(xù)執(zhí)行主程序。下位系統(tǒng)程序主要完成電機(jī)速度、方向設(shè)定，各傳感器信息的采集，與上位系統(tǒng)的通訊等工作。同上位系統(tǒng)一樣，也采用了順序結(jié)構(gòu)往復(fù)運(yùn)行。系統(tǒng)上電后，MSP430F149微處理器首先進(jìn)入系統(tǒng)初始化程序，包括時鐘配置、I/O口的初始化、外設(shè)模塊的初始化等。初始化結(jié)束后，開中斷，處理器進(jìn)入串口接收數(shù)據(jù)的判斷循環(huán)當(dāng)中[7]。程序流程圖如圖3-5所示。圖3-5下位系統(tǒng)程序流程圖4控制算法4.1轉(zhuǎn)艏控制選用正確的控制方法是保證控制系統(tǒng)穩(wěn)定控制的重要環(huán)節(jié)。采用神經(jīng)網(wǎng)絡(luò)控制、自適應(yīng)控制、PID控制算法等都可以達(dá)到潛器穩(wěn)定運(yùn)行的目的。具體的方法需根據(jù)不同的載體結(jié)構(gòu)、不同的控制對象、不同的任務(wù)要求而確定。圖4-1速度控制經(jīng)用多普勒聲學(xué)測試原理可以獲得水下機(jī)器人相對于海底或流層的速度，對速度積分后就可以得到行程，這就是多普勒計程儀的基本原理。速度閉環(huán)控制回路的結(jié)構(gòu)如圖4-1所示。構(gòu)成航行速度閉環(huán)后可以較為精確地控制水下機(jī)器人在海底的航行速度。我們采用分段線性PID控制算法，即根據(jù)潛器運(yùn)行的速度不同采用不同的PID參數(shù).離散后的PID控制算法為：(1)潛器在不同的速度下可采用不同的控制參數(shù)值，經(jīng)過試驗即可確定Kp、Ki、Kd的數(shù)值.在上式中，如果微分項采用(ek-ek-1)/T直接計算，由于采樣周期較短,因此會產(chǎn)生較大的噪聲信號，進(jìn)而影響控制效果。為了避免這種情況的發(fā)生,可采用最小二乘法對角速度進(jìn)行濾波平滑，然后再求出角速度測量值，這樣可獲得較好的控制效果。4.2深度(高度)和縱傾控制深度(高度)和縱傾控制又稱為垂直面的控制。潛器在航行過程中，其高度或深度的變化通常是在運(yùn)動中實現(xiàn)的。一但要求的高度或深度有變化，則必然會引起縱傾角的變化;而縱傾角的變化也常常引起高度或深度的變化。因此潛器在垂直面的控制實際上是一個多輸入多輸出系統(tǒng)。這里A0為給定矩陣，A為反饋矩陣，E為誤差矩陣，H0、P0分別為給定的高度和縱傾角，H、P分別為采集的高度和縱傾角。將控制算法的矩陣解耦后，其輸出Pn的計算方法如下:A0=[H0P0]A=[HP]E=[e1e2]=[H0–HP0–P](2)其中,f1和f2分別為e1和e2的函數(shù),f(x)為其它有關(guān)變量的函數(shù)。根據(jù)我們以往的工作經(jīng)驗，采用自適應(yīng)控制算法、神經(jīng)網(wǎng)絡(luò)控制算法、PID控制算法等均能達(dá)到相應(yīng)的控制要求，定深控制方法與定高控制方法相似,垂直控制量是一個力矩，它被用來改變AUV的縱傾角。為了節(jié)省能源,垂直控制通常是在AUV的前進(jìn)中實現(xiàn)的，即由縱傾角的改變而引起上升和下潛。5電機(jī)系統(tǒng)仿真5.1海洋水下機(jī)器人電動機(jī)的特點(diǎn)海洋水下機(jī)器人電動機(jī)除具有不同的電氣性能和結(jié)構(gòu)參數(shù)外,還必須具備耐海水腐蝕性、耐水壓機(jī)械結(jié)構(gòu)以及可靠的密封結(jié)構(gòu)。海洋水下機(jī)器人的驅(qū)動方式分為電力傳動和液力傳動,供電方式分為電纜供電和蓄電池(鎳氫電池、銀鋅電池和燃料電池等),供電電壓為幾十伏到幾千伏,功率為幾百瓦到一百多千瓦,交流異步電動機(jī)用于大功率液力傳動,永磁釹鐵硼直流電動機(jī)則用于小功率多臺電機(jī)電力傳動,近年來又在研制開發(fā)無刷直流電動機(jī),使之更適于充油式平衡壓力結(jié)構(gòu)。中小型水下機(jī)器人大多用電動機(jī)直接連接螺旋槳。可以用直流電機(jī)、交流電機(jī)。直流電機(jī)成本低，調(diào)速、控制系統(tǒng)簡單，而交流電機(jī)需要逆變器把直流變成交流，成本高，系統(tǒng)復(fù)雜。尤其以電池組做動力源的水下機(jī)器人，都采用直流電機(jī)。無刷直流電機(jī)是近幾年來隨著電子技術(shù)的迅速發(fā)展而發(fā)展起來的一種新型直流電機(jī)，其最大特點(diǎn)是沒有換向器（整流子）和電刷組成的機(jī)械接觸機(jī)構(gòu)。通常采用永磁體為轉(zhuǎn)子，沒有激磁損耗，沒有換向火花，沒有無線電干擾，運(yùn)行可靠，維護(hù)簡便。無刷直流電機(jī)(BrushlessDCMotor，以下簡稱BLDC)是隨著電力電子技術(shù)及新型永磁材料的發(fā)展而迅速成熟起來的一種新型電機(jī)。以其體積小、重量輕、效率高、慣量小和控制精度高等優(yōu)點(diǎn),同時還保留了普通直流電動機(jī)優(yōu)良的機(jī)械特性,廣泛應(yīng)用于伺服控制、數(shù)控機(jī)床、機(jī)器人等領(lǐng)域，隨著無刷直流電機(jī)應(yīng)用領(lǐng)域的不斷擴(kuò)大，要求控制系統(tǒng)設(shè)計簡易、成本低廉、控制算法合理、開發(fā)周期短。建立無刷直流電機(jī)控制系統(tǒng)的仿真模型，可以有效的節(jié)省控制系統(tǒng)設(shè)計時間，及時驗證施加于系統(tǒng)的控制算法，觀察系統(tǒng)的控制輸出；同時可以充分利用計算機(jī)仿真的優(yōu)越性，人為地改變系統(tǒng)的結(jié)構(gòu)、加入不同的擾動和參數(shù)變化，以便考察系統(tǒng)在不同結(jié)構(gòu)和不同工況下的動、靜態(tài)特性[8]。5.2永磁無刷直流電機(jī)(BLDC)的數(shù)學(xué)模型無刷直流電機(jī)由定子三相繞組、永磁轉(zhuǎn)子、逆變器、轉(zhuǎn)子磁極位置檢測器等組成，其轉(zhuǎn)子采用瓦形磁鋼，定子采用整距集中繞組，由逆變器供給方波電流。BLDC氣隙磁場感應(yīng)的反電動勢和相電流之間的關(guān)系，如圖5-1所示。由于BLDC的感應(yīng)電動勢為梯形波，包含有較多的高次諧波，并且BLDC的電感為非線性，在分析和仿真BLDC控制系統(tǒng)時，直接采用相變量法，根據(jù)轉(zhuǎn)子位置，采用分段線性表示感應(yīng)電動勢。圖5-1A5.2.1BLDC三相定子電壓的平衡方程可用以下的狀態(tài)方程表示:(3)式中，ua,ub,uc為三相定子電壓(V)；ea,eb,ec為三相定子的反電動勢(V)；ia,ib,ic為三相定子相電流(A)；La,Lb,Lc為三相定子自感(H)；Lab,Lac,Lba,Lbc,Lca,Lcb為三相定子繞組之間的互感(H)；Ra,Rb,Rc為三相定子繞組的相電阻；p為微分算子（d/dt）。由電機(jī)的結(jié)構(gòu)決定，在360°電角度內(nèi)，轉(zhuǎn)子的磁阻不隨轉(zhuǎn)子位置的變化而變化，并假定三相繞組對稱，則有：La=Lb=Lc=L,Lab=La=Lba=Lbc=Lca=Lcb=M,Ra=Rb=Rc=R。由于三相對稱的電機(jī)中,ia+ib+ic=0,以及Mib+Mic=-Mia，則式(3)可改寫為：(4)5.2.2BLDC的電磁轉(zhuǎn)矩方程可表示(5)其中，ω為BLDC的角速度(rad/s)。BLDC的運(yùn)動方程可表示為：(6)5.2.3由BLDC的電壓方程，可以將其等效地表示為圖5-2所示的等效電路，BLDC的每相由定子繞組電阻R、電感(L-M)及一個反電動勢e串聯(lián)構(gòu)成。圖5-2永磁無刷直流電機(jī)等效電路圖5.3基于Matlab的BLDC系統(tǒng)模型的建立在Matlab7.0的Simulink環(huán)境下，利用SimPowerSystemToolbox提供的豐富模塊庫，在分析BLDC數(shù)學(xué)模型的基礎(chǔ)上，提出了建立BLDC控制系統(tǒng)仿真模型的方法，系統(tǒng)設(shè)計框圖如圖5-3所示。BLDC建模仿真系統(tǒng)采用雙閉環(huán)控制方案：轉(zhuǎn)速環(huán)由PID調(diào)節(jié)器構(gòu)成，電流環(huán)由電流滯環(huán)調(diào)節(jié)器構(gòu)成。根據(jù)模塊化建模的思想，將圖3所示的控制系統(tǒng)分割為各個功能獨(dú)立的子模塊。圖5-4即為BLDC建模的整體控制框圖，其中主要包括：BLDCM本體模塊、速度控制模塊、參考電流模塊、電流滯環(huán)控制模塊、轉(zhuǎn)矩計算模塊和電壓逆變器模塊。把這些功能模塊和S函數(shù)相結(jié)合，在Matlab/Simulink中搭建出BLDC控制系統(tǒng)的仿真模型，并實現(xiàn)雙閉環(huán)的控制算法，圖中各功能模塊的作用與結(jié)構(gòu)簡述如下。圖5-3BLDC控制系統(tǒng)設(shè)計框圖圖5-4Matlab/Simulink中BLDC仿真建模整體控制框圖5.在整個控制系統(tǒng)的仿真模型中，BLDCM本體模塊是最重要的部分，該模塊根據(jù)BLDC電壓方程式求取BLDC三相相電流，結(jié)構(gòu)框圖如圖5-5所示。由電壓方程式可得，要獲得三相電流信號ia、ib、ic，必需首先求得三相反電動勢信號ea、eb、ec。而BLDC建模過程中，梯形波反電動勢的求取方法一直是較難解決的問題，反電動勢波形不理想會造成轉(zhuǎn)矩脈動增大、相電流波形不理想等問題，嚴(yán)重時會導(dǎo)致?lián)Q向失敗，電機(jī)失控。圖5-5BLDCM本體模塊結(jié)構(gòu)框圖及其封裝因此，獲得理想的反電動勢波形是BLDC仿真建模的關(guān)鍵問題之一。目前求取反電動勢較常用的三種方法為：(1)有限元法，應(yīng)用有限元法求得的反電動勢脈動小，精度高，但方法復(fù)雜、專業(yè)性強(qiáng)、不易推廣。(2)傅立葉變換(FFT)法，F(xiàn)FT法應(yīng)用簡單，但需要進(jìn)行大量三角函數(shù)值的計算，對仿真速度影響較大。(3)分段線性法，如圖5-6所示，將一個運(yùn)行周期0-360°分為6個階段，每60°為一個換向階段，每一相的每一個運(yùn)行階段都可用一段直線進(jìn)行表示，根據(jù)某一時刻的轉(zhuǎn)子位置和轉(zhuǎn)速信號，確定該時刻各相所處的運(yùn)行狀態(tài)，通過直線方程即可求得反電動勢波形。分段線性法簡單易行，且精度較高，能夠較好的滿足建模仿真的設(shè)計要求。因而，本文采用分段線性法建立梯形波反電動勢波形。理想情況下，二相導(dǎo)通星形三相六狀態(tài)的BLDC定子三相反電動勢的波形如圖6所示。根據(jù)轉(zhuǎn)子位置將運(yùn)行周期分為6個階段：0～π/3，π/3～2π/3，2π/3～π，π～4π/3，4π/3～5π/3，5π/3～2π。以第一階段0～π/3為例，A相反電動勢處于正向最大值Em，B相反電動勢處負(fù)向最大值-Em，C相反電動勢處于換向階段，由正的最大值Em沿斜線規(guī)律變化到負(fù)的最大值-Em。根據(jù)轉(zhuǎn)子位置和轉(zhuǎn)速信號就可以求出各相反電動勢變化軌跡的直線方程；其它5個階段，也是如此。據(jù)此規(guī)律，可以推得轉(zhuǎn)子位置和反電動勢之間的線性關(guān)系，如表1所示，從而采用分段線性法，解決了在BLDC本體模塊中梯形波反電動勢的求取問題。表1轉(zhuǎn)子位置和反電動勢之間的線性關(guān)系表表1中：k為反電動勢系數(shù)(V/(r/min))，Pos為電角度信號(rad)，w為轉(zhuǎn)速信號(rad/s)。根據(jù)電機(jī)轉(zhuǎn)過的電角度來求反電動勢，用S函數(shù)編寫。圖5-6三相反電動勢波形5.3.2在這個仿真模塊中采用滯環(huán)控制原理來實現(xiàn)電流的調(diào)節(jié)，使得實際電流隨給定電流的變化。圖5-7表示的是滯環(huán)型PWM逆變器的工作原理。其工作原理是：當(dāng)給定電流值與反饋電流值的瞬時值之差達(dá)到滯環(huán)寬度正邊緣時，逆變器的開關(guān)管VT1導(dǎo)通，開關(guān)管VT4關(guān)斷，電動機(jī)接通直流母線的正端，電流開始上升反之，當(dāng)給定電流值與反饋電流值的瞬時值之差達(dá)到滯環(huán)寬度負(fù)邊緣時，逆變器的開關(guān)管VT1關(guān)斷，開關(guān)管VT4導(dǎo)通，電動機(jī)接通直流母線的負(fù)端，電流開始下降。選擇適當(dāng)?shù)臏h(huán)環(huán)寬，即可使實際電流不斷跟蹤參考電流的波形，實現(xiàn)電流閉環(huán)控制。模塊結(jié)構(gòu)框圖如圖5-8所示，輸入為三相參考電流和三相實際電流，輸出為PWM逆變器控制信號。(a)滯環(huán)電流跟蹤型PWM逆變器單項結(jié)構(gòu)示意圖(b)滯環(huán)電流跟蹤型PWM逆變器輸出電流電壓波形圖5-7滯環(huán)電流跟蹤型PWM逆變器的工作原理圖5-8電流滯環(huán)控制模塊結(jié)構(gòu)框圖及其封裝5.3.3速度調(diào)節(jié)采用離散PID算法，以獲得最佳的動態(tài)效果。速度為積分的參數(shù)Kd為微分的參數(shù)。控制模塊的結(jié)構(gòu)較為簡單，如圖5-9所示，單輸入：參考轉(zhuǎn)速(n_ref)和實際轉(zhuǎn)速(n)的差值，單輸出：三相參考相電流的幅值Is。其中，Kp為PID控制器中比例的參數(shù)，Ki為積分的參數(shù)，Kd為微分的參數(shù)。Saturation飽和限幅模塊將輸出的三相參考相電流的幅值限定在要求范圍內(nèi)。（a）離散PID控制器(b)速度控制模塊圖5-9速度控制模塊結(jié)構(gòu)框圖及其封裝5.3.4參考電流模塊的作用是根據(jù)電流幅值信號Is和位置信號給出三相參考電流，輸出的三相參考電流直接輸入電流滯環(huán)控制模塊，用于與實際電流比較進(jìn)行電流滯環(huán)控制。轉(zhuǎn)子位置和三相參考電流之間的對應(yīng)關(guān)系如表2所示，參考電流模塊的這一功能可通過S函數(shù)編程實現(xiàn)。5.3.5根據(jù)BLDC數(shù)學(xué)模型中的電磁轉(zhuǎn)矩方程式，可以建立圖5-10所示的轉(zhuǎn)矩計算模塊，模塊輸入為三相相電流與三相反電動勢，通過加、乘模塊即可求得電磁轉(zhuǎn)矩信號Te。圖5-10轉(zhuǎn)矩計算模塊結(jié)構(gòu)框圖及其封裝5.3.6根據(jù)運(yùn)動方程式(4)，由電磁轉(zhuǎn)矩、負(fù)載轉(zhuǎn)矩以及摩擦轉(zhuǎn)矩，通過加乘、積分環(huán)節(jié)，即可得到轉(zhuǎn)速信號，求得的轉(zhuǎn)速信號經(jīng)過積分就可得到電機(jī)位置信號，如圖5-11所示。圖5-11轉(zhuǎn)速計算模塊結(jié)構(gòu)框圖及其封裝5.3.7逆變器對BLDC來說，首先是功率變換裝置，也就是電子換向器，每一個橋臂上的一個功率器件相當(dāng)于直流電動機(jī)的一個機(jī)械換向器，還同時兼有PWM電流調(diào)節(jié)器功能。對逆變器的建模，本文采用Simulink的SimPowerSystem工具箱提供的三相全橋IGBT模塊。給IGBT的A、B、C三相加三個電壓表，輸出的Simulink信號可以與BLDC直接連接，如圖5-12所示。逆變器根據(jù)電流控制模塊所控制PWM信號，順序?qū)ê完P(guān)斷，產(chǎn)生方波電流輸出。圖5-12電壓逆變器模塊結(jié)構(gòu)框圖及其封裝5.4仿真結(jié)果基于Matlab/Simulink建立了BLDC控制系統(tǒng)的仿真模型，并對該模型進(jìn)行了BLDC雙閉環(huán)控制系統(tǒng)的仿真。仿真中，BLDC電機(jī)參數(shù)設(shè)置為：定子相繞組電阻R＝1Ω，定子相繞組自感L＝0.02L，互感M＝-0.061H，轉(zhuǎn)動慣量J＝0.005kg.m2，阻尼系數(shù)B=0.0002N·m·s/rad，額定轉(zhuǎn)速n＝1000r/min，極對數(shù)p＝1，220V直流電源供電。離散PID控制器三個