[发明专利]ROV水下机器人自动定深运动控制方法

权利要求书

1.一种ROV水下机器人自动定深运动控制方法，其特征在于，采用双闭环模糊PID控制器，包括以下步骤：

第一步，生成模糊查询表，具体过程包括：

步骤1，输入变量的模糊化：

按照以下公式将从深度传感器获取的数值做差、求导：

Δd＝d₀-d_r

同理，按照以下公式对从姿态传感器获取的数值做差、求导：

Δθ＝θ₀-θ_r

输入量为深度或倾角的偏差e和偏差变化率ec，

倾角偏差Δθ的基本论域为[-1rad，+1rad]，

倾角偏差变化率的基本论域为[-0.05rad/s，+0.05rad/s]，

垂直推进器的控制电压基本论域为[-5V，+5V]，

深度偏差Δd的基本论域为[-0.5m，+0.5m]，

垂向速度的基本论域为[-0.2m/s，+0.2m/s]，

水平推进器的控制电压基本论域为[-5V，+5V]，

偏差e和偏差变化率ec的离散论域为{-3,-2-1,0,1,2,3}，

K_P，K_I，K_D的修正量ΔK_P，ΔK_I，ΔK_D的离散论域为{-3,-2-1,0,1,2,3}，对应着模糊子集{NB,NM,NS,ZE,PS,PM,PB}；

为输入输出参数选取隶属度函数，隶属度函数的中间为三角形隶属函数μ_F(x)，

左侧为Z型函数f(x,a,c)，

右侧为S型函数f(x)，

步骤2，设定修正量ΔK_P的自整定参数表，如表1-1所示；设定修正量ΔK_I的自整定参数表，如表1-2所示；设定修正量ΔK_D的自整定参数表，如表1-3所示；

表1-1

表1-2

表1-3

第二步，在线查表，从表1-1、表1-2、表1-3的自整定参数表中得出数值ΔK_P,ΔK_I,ΔK_D，根据深度传感器、姿态传感器反馈的数据与期望值对比，选取不同的调整值，进而按以下公式(5)、(6)、(7)可以计算出PID控制器的各个参数：

K_p＝K_p0+a·ΔK_p (5)

K_I＝K_I0+b·ΔK_I (6)

K_D＝K_D0+c·ΔK_D (7)

公式(5)、(6)、(7)中，K_P0,K_I0,K_D0为初始值，a、b、c为参数调整因子；

第三步，PID控制过程，根据采样数据与期望值进行比较获得偏差e(n)，对偏差进行PID运算从而调节推进器的电机两端电压，进而控制电机转速；

式中，u(n)表示第n个采样周期PID控制器输出量，

e(n)表示第n个采样周期控制量偏差，

Ti表示积分时间常数，

Td表示微分时间常数，

T表示采样周期，

其相应的增量表达式为

Δu(n)＝u(n)-u(n-1)

＝K_P{e(n)-e(n-1)+K_Ie(n)+K_D[e(n)-2e(n-1)+e(n+1)]}。

2.根据权利要求1所述的ROV水下机器人自动定深运动控制方法，其特征在于，所述第二步中，a＝0.1，b＝0.05，c＝0.1。