[發明專利]一種ROV水下機器人懸浮姿態穩定控制方法

有效

• 申請號: 201610249564.8
• 申請日: 2016-04-21
• 公開（公告）號: CN105676867B
• 公開（公告）日: 2019-02-22
• 發明（設計）人: 李宏勝;汪允鶴;陳巍
• 申請（專利權）人: 南京工程學院
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 江蘇圣典律師事務所 32237
• 代理人: 鄧麗

1.一種ROV水下機器人懸浮姿態穩定控制方法，其特征在于，包括以下步驟：

步驟一、建立水下機器人的姿態數學模型和深度數學模型；

本步驟對水下機器人模型在翻滾角Φ、俯仰角Θ的機體姿態與在Z軸向的深度及在X軸向、Y軸向的機體平面移動與在偏航角Ψ的機體姿態進行分離控制，并分別建立數學模型；

步驟二、給定水下機器人機體的在翻滾角Φ_d、俯仰角Θ_d、偏航角Ψ_d的期望姿態穩定參數和在深度Z_d的期望深度值；

步驟三、采用傳感器對水下機器人在翻滾角Φ、俯仰角Θ、偏航角Ψ的機體姿態和在深度Z的深度狀態的信息進行采集；

步驟四、將水下機器人機體的期望姿態穩定參數和期望深度值與傳感器采集到的機體狀態的反饋數據進行對比；

步驟五、對水下機器人的機體姿態和深度狀態的控制，加入雙閉環PID算法進行調節；

步驟六、根據步驟一中的水下機器人的姿態數學模型和深度數學模型，對內環PID輸出項進行力學模型處理；

步驟七、根據力學模型處理的輸出轉速參數對水下機器人垂直方向推進器進行油門控制；

步驟八、判斷是否滿足水下機器人的機體期望姿態和深度狀態，判斷結果成立，則執行步驟九，否則執行步驟四；

步驟九、達到機器人機體在水下的期望姿態穩定參數和期望深度值。

2.根據權利要求1所述的ROV水下機器人懸浮姿態穩定控制方法，其特征在于：所述的水下機器人機體通過傳感器檢測數據與水下機器人水下期望姿態穩定參數和期望深度值的偏差，通過雙閉環PID控制算法對偏差值進行調節。

3.根據權利要求1所述的ROV水下機器人懸浮姿態穩定控制方法，其特征在于：所述步驟六采用的水下機器人的姿態數學模型和深度數學模型對內環PID輸出項進行力學模型處理如下：其中，所述水下機器人在翻滾角Φ、俯仰角Θ的機體姿態與在Z軸向的深度及在X軸向、Y軸向的機體平面移動與在偏航角Ψ的機體姿態建立分離式控制模型，而在翻滾角Φ、俯仰角Θ的水下機器人的機體姿態的穩定與在Z軸向的深度的調整采用垂直方向的四臺推進器進行控制，參考四旋翼飛行器機體控制模型的原有四個虛擬輸入量、、、，如下式所示：

（1）

式中：為螺旋槳升力系數、為螺旋槳對機體質心的扭矩系數、為推進器到機體質心的距離；對四旋翼飛行器機體控制模型的原有四個虛擬輸入量、、、，進行重新配置，如下式所示，應用于在翻滾角Φ、俯仰角Θ的 水下機器人與在Z軸向的深度的控制；

（2）

進一步對式（2）上式進行處理，可得：

（3）

式中矩陣

進一步，可得垂直方向的四臺推進器轉速、、、，即

（4）

進一步，當水下機器人機體的姿態和深度偏離期望的機體穩定姿態和工作深度時，將機體傳感器檢測的機體姿態和深度值反饋，并與機體期望的姿態和深度值進行做差，得到誤差值，經PID控制算法，調整機體的姿態和深度，確保水下機器人機體位置在穩定姿態和深度的狀態。

4.根據權利要求1所述的ROV水下機器人懸浮姿態穩定控制方法，其特征在于：所述的水下機器人的在X軸向、Y軸向的機體平面移動與在偏航角Ψ的機體姿態的數學模型，采用水平平面上的四臺推進器進行控制，使用三個虛擬輸入量、、，應用于水下機器人在X軸向、Y軸向的機體平面移動與在偏航角Ψ的姿態的控制，如下式所示：

(5)

進一步對式（5）進行處理，可得：

（6）

式中矩陣，；

進一步，對3×4矩陣B進行奇異值分解，矩陣B表示為：

可得，然后利用、和可得矩陣B的廣義逆矩陣，即

式中，

最后，根據最小二乘法計算，求得：

（7）

即可得平面移動的四臺推進器轉速、、、。