學數(shù)學模型； 步驟二、給定水下機器人機體的期望姿態(tài)穩(wěn)定參數(shù)(翻滾角Φ、俯仰角?、偏航角Ψ)和 期望深度值(深度Z); 步驟三、采用傳感器對水下機器人的機體姿態(tài)(翻滾角Φ、俯仰角?、偏航角Ψ)和深度 (Z軸向）狀態(tài)的信息進行采集； 其中，所述姿態(tài)傳感器，采用AHRS航姿參考系統(tǒng)，并融合了電子羅盤、三軸陀螺儀和三 軸加速度計，能夠直接輸出水下機器人當前的翻滾角Φ、俯仰角Θ、偏航角Ψ，以及地面基 坐標系E在X、Y、Z三個軸上旋轉角速度和角加速度;水深傳感器能夠反饋水下機器人的機體 在水下的深度Ζ; 步驟四、將水下機器人機體的期望姿態(tài)穩(wěn)定參數(shù)和深度值與傳感器采集到的機體狀態(tài) 的反饋數(shù)據(jù)進行對比； 步驟五、對水下機器人的機體姿態(tài)和深度狀態(tài)的控制，加入雙閉環(huán)PID算法進行調節(jié)； 步驟六、根據(jù)步驟一中的數(shù)學模型，對內環(huán)PID輸出項進行力學模型處理； 本步驟采用步驟一中的水下機器人的機體姿態(tài)(翻滾角Φ、俯仰角Θ)與深度(Ζ軸向） 及機體平面移動(X軸向、Υ軸向）與機體姿態(tài)(偏航角Ψ)建立分離式控制模型，為實現(xiàn)對水 下機器人的機體姿態(tài)(翻滾角Φ、俯仰角Θ )的穩(wěn)定與深度(Ζ軸向）的調整，則采用垂直方向 的四臺推進器進行控制，推進裝置的布置結構如圖5所示，參考四旋翼飛行器機體控制模型 的原有四個虛擬輸入量GG、％.，如下式所不：
(1) 式中：％為螺旋槳升力系數(shù)、&為螺旋槳對機體質心的扭矩系數(shù)、友為推進器到機體 質心的距離； 對四旋翼飛行器機體控制模型的原有四個虛擬輸入量巧、％、q、級，進行重新配 置，應用于水下機器人機體姿態(tài)(翻滾角Φ、俯仰角Θ)與深度(Z軸向）的控制，如下式所示： 進一步對式(2)進行處理，可得： (2)
進一步，可得垂直方向的四臺推進器轉速％、A、叫、叫，即 (4) L% 丄
進一步，當水下機器人的機體姿態(tài)和深度偏離機體的期望的姿態(tài)穩(wěn)定參數(shù)和期望深度 時，將機體傳感器檢測到的機體姿態(tài)和深度值進行反饋，并與機體的期望姿態(tài)和深度值進 行做差，得到誤差值，經(jīng)PID算法，從而調整水下機器人的機體姿態(tài)和深度，確保水下機器人 的機體能夠維持姿態(tài)穩(wěn)定和指定深度的狀態(tài)； 針對水下機器人機體的機體平面移動(X軸向、Y軸向）與機體姿態(tài)(偏航角Ψ)的數(shù)學模 型，采用水平平面上的四臺推進器進行控制，推進裝置的布置結構如圖6所示，使用三個虛 擬輸入量％、％、％ (如式5所示），應用于水下機器人的機體平面移動(X軸向、Y軸向）與 姿態(tài)(偏航角Ψ )的控制，如下式所示：
進一步，對3 X 4矩陣B進行奇異值(SVD)分解，矩陣B表示為：
可得[故*^,[..1=5^':￡^)，然后利用.；7、.九. :和^"可得矩陣厶的廣義逆矩陣.:.，即 (7) 式Μ
最后，根據(jù)最小二乘法計算，求得： 即可得平面移動的四臺推進器轉速叫、外、A、外； 步驟七、根據(jù)力學模型處理的輸出轉速參數(shù)對水下機器人垂直方向推進器進行油門控 制； 步驟八、判斷是否滿足水下機器人的機體期望姿態(tài)和深度狀態(tài)，判斷結果成立，則執(zhí)行 步驟九，否則執(zhí)行步驟四； 步驟九、達到機器人的機體在水下的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值。
[0014]以上對本發(fā)明的【具體實施方式】進行了描述，但本發(fā)明并不限于以上描述。對于本 領域的技術人員而言，任何對本技術方案的同等修改和替代都是在本發(fā)明的范圍之中。因 此，在不脫離本發(fā)明的精神和范圍下所作的均等變換和修改，都應涵蓋在本發(fā)明的范圍內。
【主權項】
1. 一種ROV水下機器人懸浮姿態(tài)穩(wěn)定控制方法，其特征在于，包括以下步驟： 步驟一、建立水下機器人的姿態(tài)數(shù)學模型和深度數(shù)學模型； 本步驟對水下機器人模型的機體姿態(tài)(翻滾角Φ、俯仰角Θ )與深度(Z軸向）及機體平 面移動(X軸向、Y軸向）與機體姿態(tài)(偏航角Ψ )進行分離控制，并分別建立數(shù)學模型； 步驟二、給定水下機器人機體的期望姿態(tài)穩(wěn)定參數(shù)(翻滾角Φ、俯仰角?、偏航角Ψ)和 期望深度值(深度Z); 步驟三、采用傳感器對水下機器人的機體姿態(tài)(翻滾角Φ、俯仰角?、偏航角Ψ)和深度 (深度Z)狀態(tài)的彳目息進行米集； 步驟四、將水下機器人機體的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值與傳感器采集到的機體 狀態(tài)的反饋數(shù)據(jù)進行對比； 步驟五、對水下機器人的機體姿態(tài)和深度狀態(tài)的控制，加入雙閉環(huán)PID算法進行調節(jié)； 步驟六、根據(jù)步驟一中的水下機器人的姿態(tài)數(shù)學模型和深度數(shù)學模型，對內環(huán)PID輸出 項進行力學模型處理； 步驟七、根據(jù)力學模型處理的輸出轉速參數(shù)對水下機器人垂直方向推進器進行油門控 制； 步驟八、判斷是否滿足水下機器人的機體期望姿態(tài)和深度狀態(tài)，判斷結果成立，則執(zhí)行 步驟九，否則執(zhí)行步驟四； 步驟九、達到機器人機體在水下的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值。2. 根據(jù)權利要求1所述的R0V水下機器人懸浮姿態(tài)穩(wěn)定控制方法，其特征在于:所述的 水下機器人機體傳感器檢測數(shù)據(jù)與水下機器人水下期望姿態(tài)穩(wěn)定參數(shù)和期望深度值的偏 差，通過雙閉環(huán)PID控制算法對偏差值進彳丁調節(jié)。3. 根據(jù)權利要求1所述的R0V水下機器人懸浮姿態(tài)穩(wěn)定控制方法，其特征在于:所述步 驟六采用的水下機器人的姿態(tài)數(shù)學模型和深度數(shù)學模型對內環(huán)PID輸出項進行力學模型處 理如下：其中，所述水下機器人的機體姿態(tài)(翻滾角Φ、俯仰角Θ )與深度(Z軸向）及機體平 面移動(X軸向、Y軸向）與機體姿態(tài)(偏航角Ψ )建立分離式控制模型，而水下機器人的機體 姿態(tài)(翻滾角Φ、俯仰角Θ)的穩(wěn)定與深度(Z軸向）的調整采用垂直方向的四臺推進器進行 控制，參考四旋翼飛行器機體控制模型的原有四個虛擬輸入量珥、^、珥，如下式所 示：式中為螺旋槳升力系數(shù)、％為螺旋槳對機體質心的扭矩系數(shù)、波為推進器到機體質 心的距離； 對四旋翼飛行器機體控制模型的原有四個虛擬輸入量巧、巧、％、，進行重新配置， 應用于水下機器人機體姿態(tài)(翻滾角Φ、俯仰角Θ)與深度(Z軸向）的控制，如下式所示：進一步對式(2)進行處理，可得：進一步，可得垂直方向的四臺推進器轉速％、％、％、％:，即進一步，當水下機器人的機體姿態(tài)和深度偏離機體的期望姿態(tài)穩(wěn)定參數(shù)和期望深度值 時，將機體傳感器檢測到的機體姿態(tài)和深度值反饋，并與機體的期望姿態(tài)穩(wěn)定參數(shù)和期望 深度值進行做差，得到誤差值，經(jīng)PID算法，調整水下機器人的機體姿態(tài)和深度狀態(tài)，確保水 下機器人的機體位置在穩(wěn)定姿態(tài)和深度的狀態(tài)。4.根據(jù)權利要求1所述的ROV水下機器人懸浮姿態(tài)穩(wěn)定控制方法，其特征在于:所述的 水下機器人的機體平面移動(X軸向、Y軸向）與機體姿態(tài)(偏航角Ψ)的數(shù)學模型，采用水平 平面上的四臺推進器進行控制，使用三個虛擬輸入量拓、巧、％:，應用于水下機器人的機 體平面移動(X軸向、Y軸向）與姿態(tài)(偏航角Ψ)的控制，如下式所示：進一步，對3X4矩陣B進行奇異值(SVD)分解，矩陣B表示為：可得[?7人Π = ，然后利用權、A和P可得矩陣A的廣義逆矩陣方，SP最后，根據(jù)最小二乘法計算，求得：即可得平面移動的四臺推進器轉速％、叫、馮、邱。
【專利摘要】本發(fā)明提供了一種ROV水下機器人懸浮姿態(tài)穩(wěn)定控制方法，采用對水下機器人模型的機體姿態(tài)與機體深度及機體平面移動與機體姿態(tài)進行分離控制的方法，并分別在水下機器人框架結構內建立數(shù)學模型，利用傳感器對水下機器人的機體狀態(tài)進行檢測，并反饋機體姿態(tài)的歐拉角與機體位置參數(shù)，與期望的機體姿態(tài)的歐拉角與機體位置參數(shù)進行做差處理，通過雙閉環(huán)控制算法對機體位置和姿態(tài)的誤差進行調節(jié)，繼而對水下機器人的推進器轉速進行控制，最終實現(xiàn)在水下復雜擾動的環(huán)境下水下機器人依然能夠維持姿態(tài)的穩(wěn)定控和運動到期望位置的控制，以確保水下機器人能夠在水下順利的執(zhí)行規(guī)定動作，并完成水下目標任務。
【IPC分類】G05D1/08
【公開號】CN105676867
【申請?zhí)枴緾N201610249564
【發(fā)明人】李宏勝, 汪允鶴, 陳巍
【申請人】南京工程學院
【公開日】2016年6月15日
【申請日】2016年4月21日