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

摘要

本发明涉及一种ROV水下机器人自动定深运动控制方法，其解决了现有三推进器ROV自动定深控制算法复杂，不易实现，ROV自动定深运动过程稳定性差，抵抗水下浪涌干扰能力差的技术问题，其采用双闭环模糊PID控制器，内环为倾角环，控制垂直推进器工作使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)]}。