AUV水下機器人運動控制系統設計AUV水下機器人運動控制系統設計 中國海洋大學工程學院機械電子工程研究生課程考核論文題目： AUV水下機器人運動控制系統研究報告課程名稱： 運動控制技術名：學 號：院 系： 工程學院機電工程系專 業： 機械電子工程時 間： 2010-12-26課程成績：任課老師：譚俊哲AUV水下機器人運動控制系統設計(AUV).同時介紹了它的推進器布置、控制系統結構、推力分配等方法。最后展示了它的運行實驗結果。關鍵詞：水下機器人；總體設計方案；運動控制系統；電機仿真引言VideoRayScout、Explorer、Pro等系列遙控式水下機器人，美國SeabotixLBV-ROVAC-CESSAC-ROV系列。（AutonomousUnderwaterVehicles,AU,載體采用模塊化設計思想,可根據需要適當增減作業或傳感器模塊,載體采用魚雷狀流線外形,2m,25cm,GPS、無線電通訊模塊,,支持光纖通訊,載體可外掛聲學設備,通過光纖系統進行遙控操作可實現其半自主作業,也可在預編程指令下實現自主作業。系統基本模塊組成設計如圖1-1所示。它具有開放式、模塊化的（自主/半自主/遙控1-1系統基本模塊組成設計機器人物理模型2.1AUV物理模型AUVAUVAUV的動力學模型。為了便于分析，建立適合于描述AUV運動的兩種參考坐標系，即固定2-152個12個垂向推進器。左右對稱于縱中剖面，上和下、前和后都不對稱[2]。圖2-1AUV水下機器人物理模型1.2微小型水下機器人動力學分析1.5m，采用鋰電池作為能源，尾部為一對水平舵和一對垂直舵，單槳推進，可攜帶慣導設備、探測聲納、水下攝像機、深度計等設備，設計巡航22-2所示，E?ξη?oxyz。建立的坐標系，如圖1所示。圖中：E-ξηζ—慣性坐標系；Oxyz—載體坐標系。因為機器人在航行時速度不高（<4節，可以對機器人模型進行線性化及一些簡化。F=[X,,ZTM=[K,M,N]T速度：V=[u,v,wT角速率：ω=[p,q,r]。2-2慣性和載體坐標系在圖2-2定義的慣性坐標系和運動坐標系中，機器人的空間運動向量表達為：η1=［xyz］T；η2=［φθψ］Tυ1=［uvw］T；υ2=［p q r］T式中：向量η1—機器人在慣性坐標系中的位置；η2—其在慣性坐標系中的姿態；φ—橫滾角；θ—俯仰角；ψ—航向角；υ1—機器人在載體坐標系中的線速度（；2—其在載體坐標系中的轉動角速度（ω）[3]。總體方案設計系統組成及工作原理2-3為水下機器人控制系統框圖：2-3水下機器人控制系統框圖導航系統設計導航系統的傳感器包括慣性測量器件IMU（陀螺儀和加速度計、磁羅盤、深度計。其中IMU通過三陀螺儀、三加速度計捷聯解算后獲得位置、速度、姿態9維信息，通過RS232串口與導航計算機相連以NEMA0183格式傳輸信息到導航計算機。磁羅盤可以獲取當前載體三維姿態信息，通過 RS232 串口與導航計算機相連，以NEMA0183格式傳輸信息到導航計算機。深度計為液壓變送器，通過膜片感應內外側水壓差來確定水深，其輸出為4～20mA模擬電流信號，轉換為0～5V的電壓信號后經過16位ADC轉換模塊，串行傳送到導航計算機。如圖2-4所示。圖2-4組合導航系統總體設計框圖驅動方式的選用幾乎所有的水下機器人都采用螺旋槳式推進器。80%以上采用電機推進器，其余采用油壓電機推進器。水下機器人要實現水下空間的六維（六自由度）運動，即三個平移運動：推進（Surgex軸、升沉（Heavez軸、橫移（Swayy軸）和三個回轉運動：轉首z軸、縱傾y軸、橫搖x軸。推力器的組成推力器是由電機和螺旋槳組成的，水下機器人用的電機需要密封。密封主要有兩種方式，一種是機械密封，另一種采用磁耦合器。機械密封相對而言比較簡單，但因密封1-3倍，這樣的電機用于推力器，會使啟動電壓升高，從而10000轉/分以上的高速電機，2-5所示。圖圖2-5 推力器組成圖2-6 螺旋槳與推進器示意圖壓縮。如圖2-6所示，建立固定于導管上的直角坐標系O-xyz，以螺旋槳槳葉參考線與軸方向由xV0前進，x軸以等角速度Ω。能源供給方式的選用控制系統設計水下機器人控制系統設計22個直流電機通過聯動軸與螺旋槳相連，實現水下機器人前進、后退、左轉、右轉運動；垂直方向安裝有 3個垂推進器，實現機器人上升、下沉運動。前變焦攝像機安裝有垂直方向一維云臺，避免攝像死區在機器人電子艙內安裝有深度計溫度計和數字式電子羅盤傳感器滿足實際作業環境需要，為檢修人員提供了豐富的作業環境信息。根據功能需要，我們選擇了TI公司推出的MSP430系列的MSP430F149作為主16FLASH的單片機，由于其性價比和集成度高，[6]。利用MSP430F149定時器B比較單元產生的6路PWM信號和5路方向信號，分別控制主推進器、垂推進器、機械手、攝像機云臺電機速度和照明燈亮度；兩路串口實現了羅盤數據的采集和上位系統的通訊；外部傳感器反饋的模擬信息通過 ADC模塊實現轉化，使芯片豐富的外設資源得以充分利用。總線型結構的所有節點都共享一個公共的物理通道(即總線),系統采用單片機作為控制單元完成機器人控制系統中的各種控制任務(如傳感器控制、電機驅動器控制和通信模塊控制等)CAN總線上,成為控制網絡中的一個節點，從而構成多主機結構,即每個節點均為一個主機，通過CAN通信協議協同完成控制任CAN總線的分布式控制網絡應用于水下機器人中。總線上各節點完成不同的任3-1CAN總線是一種多主總線,理論上任何一個節點都可以作為主節點。在控制系統PC1為上位節點,其它節點為底層節點。PC機通過串口與1CPU通信,CPUCAN收發器通信,CAN總線上的發送與接112個底層節點根據應用的不同具有不同的功能,CAN總線通信的能力,上傳數據和接收數據。3-1CAN總線通信節點的硬件結構及接口設計框圖CANPhilipsP87C591，除完成節點自身的CANP87C59180C51指令集，并成功地集成了Philips公司的SJAl000CAN控制器，該嵌入式CAN控制器具有以下特點：(1)完全符合CAN2.0規范，控制CAN幀的接收和發送；CAN5CPUCAN控制器連接的特殊功能寄存器；CANCAN信息幀擴展或標準幀CPU啟動發送,CAN內核模塊；(4)當接收一個信息時，CAN內核模塊將串行數據流轉換成并行數據傳輸到驗收濾波器,P87C591確認接收需要的信息。圖3-2P87C591為核CAN總線接口電路。P1.0、P1.1CAN的收(RxDC)和發(TxDC)線。圖3-2CAN總線接口電路CANPhilips8與地之PCA82C250Rs對地連接的電阻對總線進行斜率控制，以控制射頻干擾。使用雙絞線作為傳輸介質。通過高速光耦6N137P87C591CANCAN124Ω的終端匹配電阻，以減少信號反射干擾。水上控制箱控制系統設計設計中，我們采用了常見的小型控制箱作為岸上控制平臺，其外形尺寸為372mm×266mm×134mm，具有體積小，攜帶方便的特點。其內部硬件模塊主要包含主處理器核心模塊和液晶顯示模塊，完成上下位機間的通訊，視頻圖像、傳感器信息的顯示，實現人機交互。其結構框圖如圖3-3所示。圖3-3 水上控制箱控制系統在該控制系統中采用MSP430F135作為主處理器，充分應用了其內部集成的14路12位模數轉換器和串口通訊模塊，操作人員只需通過設定控制面板上各按鍵、搖桿，AD轉換，將各模擬量轉換為數字量，經串口通訊模塊來實現控制箱對下位控制系統各推進器速度、方向的控制、照明燈的亮度調節以及攝像機云臺的運動設定。控制系統軟件設計微處理器首先進入I/O口的初始化、外設模塊的初始化等。初始化結束后，開中斷，處理器進入串口接收數據的判斷循環當中3-5所示。圖3-5 下位系統程序流程圖控制算法4.1轉艏控制PID控制算法等都可以達到潛器穩定運行的目的。具體的方法需根據不同的載體結構、不同的控制對象、不同的任務要求而確定。4-1速度控制所示。構成航行速度閉環后可以較為精確地控制水下機器人在海底的航行速度。PIDPID參數.PID控制算法為：(1)KpKiKd的數值.在上式中，如果微分項采用ek-ek-1)/T直接計算，由于采樣周期較短,因此會,可采用最小二乘法對角速度進行濾波平滑，然后再求出角速度測量值，這樣可獲得較好的控制效果。2深度(高度)和縱傾控制深度(高度)和縱傾控制又稱為垂直面的控制。潛器在航行過程中，其高度或深度A0為給定矩陣，A為反饋矩陣，E為誤差矩陣，H0、P0分別為給定的高PPn的計算方法如下:A0=[H0 P0] A=[H P] E=[e1 e2]=[H0–H P0–P](2)其中f1f2e1e2的函數,f(x)為其它有關變量的函數。根據我們PID控制算法等均能達,垂直控制量是一個力矩，它被AUV的縱傾角。為了節省能源,AUV的前進中實現的，即由縱傾角的改變而引起上升和下潛。電機系統仿真海洋水下機器人電動機的特點海洋水下機器人電動機除具有不同的電氣性能和結構參數外,還必須具備耐海水(鎳氫電池、銀鋅電池和燃料電池等),供電電壓為幾十伏到幾千伏,功率為幾百瓦到一百多千瓦,交流異步電動機用于大功率液力傳動,永磁釹鐵硼直流電動機則用于小功率多臺電機電力傳動,近年來又在研制開發無刷直流電動機,使之更適于充油式平衡壓力結構。無刷直流電機(BrushlessDCBLDC)是隨著電力電子技術及新型永磁材料的發展而迅速成熟起來的一種新型電機。以其體積小、重量輕、效率高、慣量小和控制精度高等優點,同時還保留了普通直流電動機優良的機械特性,廣泛應用于伺服控制、數控機床、機器人等領域，隨著無刷直流電機應用領域的不斷擴大，要求控制系統設計簡易、成本低廉、控制算法合理、開發周期短。建立無刷直流電機控制系統的仿真。永磁無刷直流電機(BLDC)的數學模型無刷直流電機由定子三相繞組、永磁轉子、逆變器、轉子磁極位置檢測器等組成，BLDC氣隙磁5-1BLDC的感應電動勢為梯BLDCBLDC控制系統時，直接采用相變量法，根據轉子位置，采用分段線性表示感應電動勢。圖5-1A相反電動勢和電流波形電壓方程BLDC三相定子電壓的平衡方程可用以下的狀態方程表示:(3)為三相定子電壓為三相定子的反電動勢為三相定子相電流(A)；La,Lb,Lc為三相定子自感(H)；Lab,Lac,Lba,Lbc,Lca,Lcb為三相定(H)；Ra,Rb,Rc為三相定子繞組的相電阻；p為微分算子（d/dt。由360°電角度內，轉子的磁阻不隨轉子位置的變化而變化，并假定。由于,ia+ib+ic=0,以及Mib+Mic=-Mia(3)轉矩方程BLDC的電磁轉矩方程可表示其中，ωBLDC的角速度(rad/s)。BLDC的運動方程可表示為：

(4)(5)(6)等效電路BLDC5-2的每R、電感(L-M)e串聯構成。圖5-2永磁無刷直流電機等效電路圖MatlabBLDC圖5-3BLDC控制系統設計框圖Matlab7.0SimulinkSimPowerSystemToolbox提供的豐富模塊庫，BLDCBLDC控制系統仿真模型的方法，系統5-3BLDCPID調節3所示的控制系5-4BLDC本體模塊、速度控制模塊、參考電流模塊、電流滯環控制模塊、轉矩計算SMatlab/Simulink中搭建圖5-3BLDC控制系統設計框圖圖5-4Matlab/Simulink中BLDC仿真建模整體控制框圖3.1BLDCM本體模塊BLDCM本體模塊是最重要的部分，該模塊根據BLDC電壓方程式求取BLDC三相相電流，結構框圖如圖5-5所示。由電壓方程式可得，要獲得三相電流信號ia、ib、ic，必需首先求得三相反電動勢ea、eb、ecBLDC建模過程中，梯形波反電動勢的求取方法一直是較難解決的問題，反電動勢波形不理想會造成轉矩脈動增大、相電流波形不理想等問題，嚴重時會導致換向失敗，電機失控。圖5-5BLDCM本體模塊結構框圖及其封裝BLDC仿真建模的關鍵問題之一。目前求取反電動勢較常用的三種方法為：(1)有限元法，應用有限元法求得的反電動勢脈動小，精度高，但方法(2)傅立葉變換(FFT)法，FFT法應用簡單，但需要進行大量三角(3)5-60-360°660BLDC66個階段：0～π/3，π/3～2π/3，2π/3～π，π～4π/3，4π/3～5π/3，5π/3～2π。0～π/3為例，AEm，B相反電動勢處負向最大值-Em，CEm沿斜線規律變化到負的最大值-Em。51BLDC本體模塊中梯形波反電動勢的求取問題。表1轉子位置和反電動勢之間的線性關系表1k為反電動勢系數(V/(r/min)Pos為電角度信號(radw為轉速信號(rad/s。S函數編寫。電流滯環控制模塊

5-6三相反電動勢波形5-7PWM逆變器的工作原理。其工作原理是：當給定電流VT1導通，開VT4關斷，電動機接通直流母線的正端，電流開始上升反之，當給定電流值與反VT1VT4導通，電動機接通直流母線的負端，電流開始下降。選擇適當的滯環環寬，即可使實際電流不斷跟蹤參考電流的波形，實現電流閉環控制。模塊結構框圖如圖5-8所示，輸入PWM逆變器控制信號。(a)滯環電流跟蹤型PWM逆變器單項結構示意圖(b)PWM逆變器輸出電流電壓波形5-7PWM逆變器的工作原理速度控制模塊

圖5-8電流滯環控制模塊結構框圖及其封裝PIDKd為微5-9(n_ref)和實際轉速(n)PID控制器中比例的參數，Ki為積分的參數，Kd為微分的參數。Saturation飽和限幅模塊將輸出的三相參考相電流的幅值限定在要求范圍內。（a）離散PID控制器參考電流模塊

(b)速度控制模塊圖5-9速度控制模塊結構框圖及其封裝Is和位置信號給出三相參考電流，輸出的三相參考電流直接輸入電流滯環控制模塊，用于與實際電流比較進行電流滯環控制。2S函數編程實現。轉矩計算模塊BLDC5-10所示的轉矩計算模塊，。轉速計算模塊

圖5-10轉矩計算模塊結構框圖及其封裝根據運動方程式(4)，由電磁轉矩、負載轉矩以及摩擦轉矩，通過加乘、積分環節，5-11所示。圖5-11轉速計算模塊結構框圖及其封裝電壓逆變器模塊BLDC來說，首先是功率變換裝置，也就是電子換向器，每一個橋臂上PWM電流調節器功SimulinkSimPowerSystem工具箱提供的三相全橋IGBTIGBTACSimulinkBLDC5-12PWM信號，順序導通和關斷，產生方波電流輸出。圖5-12電壓逆變器模塊結構框圖及其封裝5.4仿真結果基于Matlab/Simulink建立了BLDC控制系統的仿真模型并對該模型進行了BLDC雙閉環控制系統的仿真。仿真中，BLDC電機參數設置為：定子相繞組電阻定子相繞組自感L＝0.02L，互感M＝-0.061H，轉動慣量J＝0.005kg.m2，阻尼系數B=0.0002N·m·s/rad，額定轉速n＝1000r/min，極對數p＝1，220V直流電源供電。離散PID控制器三個參數Kp=5，Ki=0.01，Kd=0.001，飽和限幅模塊幅值限定在±35內，采樣周期T=0.001s。為了驗證所設計的BLDC控制系統仿真模型的靜、動態性能，系統空載起動，待進入穩態后，在t＝0.3s時突然加負載在t＝0.65s時突然撤去負載。可得到系統轉速、轉矩、三相電流和三相反電動勢仿真曲線如圖 5-13～5-16所示。由仿真波形可以看出，在n＝1000r/min的參考轉速下，系統響應快速且平穩，相電流和反電動勢波形較為理想。仿真波形圖5-14、5-15表明：起動階段系統保持轉矩t＝0.5s時突加負5-14BLDC仿真建模方法的有效性及控制系統的合理性。5-13轉速響應曲線5-14轉矩響應曲線圖5-15電流Ia，Ib，Ic波形 圖5-16反電動勢Ea,Ea,Ea波形結論PID控制算法在超小型水下機參考文獻[1]． [J][2]． CAN總線的水下機器人執行節點設計與實現[J]2005，24（4：15-16[3]． [J]，計算機測量與控制。2009，17（4：672-678[4]． .微小型水下機器人操縱性能與運動仿真研究[J]學報。2009，21（13：4149-4158.[5]． ,朱紹鋒。基于遺傳算法的自治水下機器人水動力參數辨識方法[J]，機械工程學報。2010，46（1：97-100.[6]． DSP的水下機器人組合導航系統設計2010，23(4)：11-13[7]． [J]2010，31（4：429-469.[8]． Matlab協同仿真計算[J]，微電機。2007，40（1）：40-442015年水下玻纖套筒加固橋梁墩柱成功項目案例水下玻纖套筒加固系統(又稱“夾克法”)，主要應用于對各種腐蝕、沖刷、混凝土脫落、鋼筋露筋等病害的結構基礎碼頭樁基和橋墩柱(包括混凝土樁、鋼樁和木樁)等修復和加固防護，以及對新建墩柱的預先防護。20世紀70年代以來，此技術在世界范圍內已修復了成千上其中水下環氧灌漿料是一種可用于水下和潮濕環境的100