[发明专利]一种ROV水下机器人悬浮姿态稳定控制方法

权利要求书

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）

即可得平面移动的四台推进器转速、、、。