一種rov水下機(jī)器人懸浮姿態(tài)穩(wěn)定控制方法
【技術(shù)領(lǐng)域】
[0002] 本發(fā)明涉及一種R0V水下機(jī)器人懸浮姿態(tài)穩(wěn)定控制方法，屬于水下機(jī)器人控制技 術(shù)領(lǐng)域。
【背景技術(shù)】
[0003] 在我國，水下作業(yè)主要是依賴于人和簡單的潛水器，而復(fù)雜多變的海洋和水下環(huán) 境對人的生命安全造成很大的威脅，這就依賴于水下機(jī)器人代替人去執(zhí)行水下作業(yè)任務(wù)， 例如水下搜救打撈、考古科研、海洋生物觀測、調(diào)查取證、海洋油氣管道調(diào)查、能源勘探等工 作，不僅可保障工作人員安全，還可以節(jié)約作業(yè)成本。同時由于水下環(huán)境的復(fù)雜性，要求水 下機(jī)器人具備較高的機(jī)動性、穩(wěn)定性和操作性，在水下機(jī)器人執(zhí)行水下作業(yè)任務(wù)的過程中， 水下機(jī)器人在水下環(huán)境多方面的擾動下仍然能夠做出指定動作來完成規(guī)定任務(wù)，這就要求 水下機(jī)器人能夠?qū)崟r調(diào)整自身姿態(tài)與機(jī)體深度，使機(jī)體能夠保持機(jī)體姿態(tài)穩(wěn)定和維持在指 定深度，從而保證機(jī)器人能夠做出規(guī)定的動作來完成目標(biāo)任務(wù)。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是提供一種R0V水下機(jī)器人懸浮姿態(tài)穩(wěn)定控制方法，解決現(xiàn)有技術(shù) 中存在的不足和缺陷，能夠為實現(xiàn)機(jī)器人水下懸停和執(zhí)行任務(wù)時保持機(jī)體姿態(tài)的穩(wěn)定，提 升水下機(jī)器人在水下執(zhí)行任務(wù)時的穩(wěn)定性、快速性和準(zhǔn)確性。
[0005] 為解決上述技術(shù)問題，本發(fā)明提供了一種R0V水下機(jī)器人懸浮姿態(tài)穩(wěn)定控制方法， 包括以下步驟： 步驟一、建立水下機(jī)器人的姿態(tài)數(shù)學(xué)模型和深度數(shù)學(xué)模型； 本步驟對水下機(jī)器人模型的機(jī)體姿態(tài)(翻滾角Φ、俯仰角Θ )與深度(Z軸向）及機(jī)體平 面移動(X軸向、Y軸向）與機(jī)體姿態(tài)(偏航角Ψ )進(jìn)行分離控制，并分別建立數(shù)學(xué)模型； 步驟二、給定水下機(jī)器人機(jī)體的期望姿態(tài)穩(wěn)定參數(shù)(翻滾角Φ、俯仰角?、偏航角Ψ)和 期望深度值(深度Z); 步驟三、采用傳感器對水下機(jī)器人的機(jī)體姿態(tài)(翻滾角Φ、俯仰角?、偏航角Ψ)和深度 (深度Z)狀態(tài)的彳目息進(jìn)行米集； 步驟四、將水下機(jī)器人機(jī)體的期望姿態(tài)穩(wěn)定參數(shù)和深度期望值與傳感器采集到的機(jī)體 狀態(tài)的反饋數(shù)據(jù)進(jìn)行對比； 步驟五、對水下機(jī)器人的機(jī)體姿態(tài)和深度狀態(tài)的控制，加入雙閉環(huán)PID算法進(jìn)行調(diào)節(jié)； 步驟六、根據(jù)步驟一中的數(shù)學(xué)模型，對內(nèi)環(huán)PID輸出項進(jìn)行力學(xué)模型處理； 本步驟采用步驟一中水下機(jī)器人的機(jī)體姿態(tài)(翻滾角Φ、俯仰角Θ)與深度(Z軸向）及 機(jī)體平面移動(X軸向、Y軸向）與機(jī)體姿態(tài)(偏航角Ψ )建立分離式控制的數(shù)學(xué)模型，而水下 機(jī)器人的機(jī)體姿態(tài)(翻滾角Φ、俯仰角Θ )的穩(wěn)定與深度(Z軸向）的調(diào)整采用垂直方向的四 臺推進(jìn)器進(jìn)行控制，參考四旋翼飛行器機(jī)體控制模型的原有四個虛擬輸入量Μ、％、％:、 每，如下式所示：
式中：％為螺旋槳升力系數(shù)、&為螺旋槳對機(jī)體質(zhì)心的扭矩系數(shù)、友為推進(jìn)器到機(jī)體 質(zhì)心的距離； 對四旋翼飛行器機(jī)體控制模型的原有四個虛擬輸入量巧、％、蘇、試，進(jìn)行重新配 置，應(yīng)用于水下機(jī)器人機(jī)體姿態(tài)(翻滾角Φ、俯仰角Θ)與深度(Ζ軸向）的控制，如下式所示：
進(jìn)一步對上式進(jìn)行處理，可得：
進(jìn)一步，可得垂直方向的四臺推進(jìn)器轉(zhuǎn)速％、％、叫、叫，即
進(jìn)一步，當(dāng)水下機(jī)器人的機(jī)體姿態(tài)和深度偏離機(jī)體的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值 時，將機(jī)體傳感器檢測的機(jī)體姿態(tài)和深度值反饋，并與機(jī)體的期望姿態(tài)穩(wěn)定參數(shù)和期望深 度值進(jìn)行做差，得到誤差值，經(jīng)PID控制算法，調(diào)整水下機(jī)器人的機(jī)體姿態(tài)和深度狀態(tài)，確保 水下機(jī)器人的機(jī)體位置在穩(wěn)定姿態(tài)和深度的狀態(tài)。
[0006]步驟七、根據(jù)力學(xué)模型處理的輸出轉(zhuǎn)速參數(shù)對水下機(jī)器人垂直方向推進(jìn)器進(jìn)行油 門控制； 步驟八、判斷是否滿足水下機(jī)器人的機(jī)體期望姿態(tài)和深度狀態(tài)，判斷結(jié)果成立，則執(zhí)行 步驟九，否則執(zhí)行步驟四； 步驟九、達(dá)到機(jī)器人機(jī)體在水下的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值。
[0007] 在本發(fā)明中：所述的水下機(jī)器人的機(jī)體平面移動(X軸向、Y軸向）與機(jī)體姿態(tài)(偏航 角Ψ )的數(shù)學(xué)模型，采用水平平面上的四臺推進(jìn)器進(jìn)行控制，使用三個虛擬輸入量&、％、 Εζ,，應(yīng)用于水下機(jī)器人的機(jī)體平面移動(X軸向、Y軸向）與姿態(tài)(偏航角Ψ)的控制，如下式 所示：
進(jìn)一步，對3 X 4矩陣Β進(jìn)行奇異值(SVD)分解，矩陣Β表示為：
可得[?7，Λ,Η = ^￡>04)，然后利用y、Λ和?？傻镁仃嚘〉膹V義逆矩陣#，SP
式4
最后，根據(jù)最小二乘法計算，求得：
即可得平面移動的四臺推進(jìn)器轉(zhuǎn)速A、碼、瑪、呷。
[0008] 采用上述技術(shù)方案后，本發(fā)明的有益效果為:解決現(xiàn)有技術(shù)中存在的不足和缺陷， 能夠為實現(xiàn)機(jī)器人水下懸停和執(zhí)行任務(wù)時保持機(jī)體姿態(tài)的穩(wěn)定，提升水下機(jī)器人在水下執(zhí) 行任務(wù)時的穩(wěn)定性、快速性和準(zhǔn)確性。
【附圖說明】
[0009] 圖1為本發(fā)明的R0V水下機(jī)器人機(jī)體姿態(tài)穩(wěn)定和深度控制方法的流程示意圖； 圖2為本發(fā)明的ROV水下機(jī)器人的機(jī)體姿態(tài)(翻滾角Φ、俯仰角θ )與深度(Z軸向）的控 制方法流程示意圖； 圖3為本發(fā)明的R0V水下機(jī)器人的機(jī)體平面移動(X軸向、Y軸向）與機(jī)體姿態(tài)(偏航角Ψ ) 的控制方法流程示意圖； 圖4為本發(fā)明的R0V水下機(jī)器人推進(jìn)裝置的布置結(jié)構(gòu)示意圖； 圖5為本發(fā)明的R0V水下機(jī)器人垂直方向推進(jìn)裝置的布置結(jié)構(gòu)及坐標(biāo)系結(jié)構(gòu)示意圖； 圖6為本發(fā)明的R0V水下機(jī)器人水平方向推進(jìn)裝置的布置結(jié)構(gòu)及坐標(biāo)系結(jié)構(gòu)示意圖。
【具體實施方式】
[0010]本發(fā)明所采用的R0V水下機(jī)器人的機(jī)體采用對稱平衡的結(jié)構(gòu)設(shè)計，即機(jī)體框架以 及機(jī)體搭載的裝置在幾何形狀上對稱，機(jī)體配置的對稱平衡設(shè)計能夠保證機(jī)體重心向下、 機(jī)體自身穩(wěn)定有一定的自適應(yīng)性，R0V水下機(jī)器人推進(jìn)裝置的布置結(jié)構(gòu)如圖4所示，圖中序 號1-8為水下機(jī)器人推進(jìn)器的序號，R0V水下機(jī)器人垂直方向推進(jìn)裝置的布置結(jié)構(gòu)如圖5所 示，圖中序號1-4為水下機(jī)器人推進(jìn)器的序號，R0V水下機(jī)器人水平方向推進(jìn)裝置的布置結(jié) 構(gòu)如圖6所示，圖中序號5-8為水下機(jī)器人推進(jìn)器的序號。
[0011] 為了能夠更好的了解本發(fā)明的技術(shù)特征、技術(shù)內(nèi)容及其達(dá)到的技術(shù)效果，現(xiàn)將本 發(fā)明的附圖結(jié)合實施例進(jìn)行更加詳細(xì)的說明。
[0012] 接下來的內(nèi)容，將結(jié)合附圖和實施例對本發(fā)明專利的內(nèi)容做進(jìn)一步的說明。
[0013] 如圖1所示，本發(fā)明提供了一種R0V水下機(jī)器人懸浮姿態(tài)穩(wěn)定控制方法，包括以下 步驟： 步驟一、建立水下機(jī)器人的姿態(tài)數(shù)學(xué)模型和深度數(shù)學(xué)模型； 本發(fā)明所采用的R0V水下機(jī)器人推進(jìn)裝置的布置結(jié)構(gòu)如圖4所示，機(jī)體框架以及機(jī)體搭 載的裝置在幾何形狀上對稱； 本步驟為方便確切的描述水下機(jī)器人機(jī)體的運動狀態(tài)，更好的建立水下機(jī)器人的數(shù)學(xué) 模型，需對機(jī)體建立兩個坐標(biāo)系，分別為:地面基坐標(biāo)系E和機(jī)體姿態(tài)坐標(biāo)系B，機(jī)體坐標(biāo)系 的配置如圖5所示； 為更好的描述兩個坐標(biāo)系間的轉(zhuǎn)換關(guān)系，定義三個歐拉角： 翻滾角Φ :機(jī)體繞X軸旋轉(zhuǎn)與Y軸形成的角度； 俯仰角Θ :機(jī)體繞Y軸旋轉(zhuǎn)與X軸形成的角度； 偏航角Ψ :機(jī)體繞Z軸旋轉(zhuǎn)形成的角度； 兩個坐標(biāo)系間，由地面基坐標(biāo)系E到機(jī)體姿態(tài)坐標(biāo)系B的轉(zhuǎn)換關(guān)系如下：
本步驟對水下機(jī)器人模型的機(jī)體姿態(tài)(翻滾角Φ、俯仰角Θ )與深度(Z軸向）及機(jī)體平 面移動(X軸向、Y軸向）與機(jī)體姿態(tài)(偏航角Ψ )進(jìn)行分離控制，并分別建立數(shù)學(xué)模型;機(jī)體姿 態(tài)(翻滾角Φ、俯仰角Θ )與深度Z控制方法流程如圖2所示，機(jī)體平面移動(X軸向、Y軸向）與 機(jī)體姿態(tài)(偏航角Ψ )控制方法流程如圖3所示； 為方便建立水下機(jī)器人的動力學(xué)數(shù)學(xué)模型且便于控制算法的研究，不失一般性地作如 下假設(shè)： (1 )、水下機(jī)器人是一個均勻?qū)ΨQ的剛體，忽略自身的彈性振動和變形； (2) 、地面基坐標(biāo)系為慣性坐標(biāo)系，且起始時刻地面基坐標(biāo)系的原點和水下機(jī)器人的質(zhì) 心即幾何中心位于同一位置； (3) 、水下機(jī)器人在水下所受阻力和重力不受下沉深度等因素影響，總保持不變； (4) 、水下機(jī)器人各個方向的拉力與推進(jìn)器轉(zhuǎn)速的平方成正比； 進(jìn)一步，根據(jù)以上假設(shè)條件對水下機(jī)器人建立動力