CN109814581A - Rov水下机器人自动定深运动控制方法 - Google Patents

Abstract

本发明涉及一种ROV水下机器人自动定深运动控制方法，其解决了现有三推进器ROV自动定深控制算法复杂，不易实现，ROV自动定深运动过程稳定性差，抵抗水下浪涌干扰能力差的技术问题，其采用双闭环模糊PID控制器，内环为倾角环，控制垂直推进器工作使ROV的俯角达到要求，外环为深度环，控制水平推进器工作。本发明可广泛应用于水下机器人技术领域。

Description

ROV水下机器人自动定深运动控制方法

技术领域

本发明涉及水下机器人技术领域，具体而言，涉及一种ROV水下机器人自动定深运动控制方法。

背景技术

在陆地资源被大量开发以至短缺的今天，深入开发海洋资源刻不容缓。地球是唯一已知存在固体、液体和气体三种状态水的行星，它也是目前唯一已知地表有液态水存在的行星。全球97％的供水来源于海洋。水对于维持生命至关重要，水与人类生存的方方面面都息息相关。海洋覆盖地球表面的70.8％，远远超过地球的陆地面积。在海洋覆盖范围内，大西洋占16.2％，太平洋占2.4％，印度洋占14.4％，边缘和邻近地区占7.8％。值得注意的是，仅太平洋的覆盖面积就比所有的陆地面积多3.2％。

我国陆地国土有大部分处于西部大高原区域，生态环境脆弱，有效国土面积较小，开发成本高，需要以海补陆。因此，提高海洋资源开发能力，对我国的发展至关重要。为了开发海洋资源，我们应该不断地了解、勘测海洋，探索开发海洋的方法，研制可用的设备。而水下机器人因其灵活机动、环境适应能力强、安全等突出特点，逐渐成为海洋开发相关行业的宠儿。在科学研究、渔业和水产养殖、军事和国土及公共安全方面都能看到水下机器人的身影。

目前，水下机器人分为两大类：载人水下机器人和无人水下机器人(UUV，unmannedunderwater vehicles)。无人水下机器人又分为有缆遥控水下机器人(ROV，RemoteOperated Vehicle)和无缆自治水下机器人(AUV，Autonomous Underwater Vehicle)。便携式ROV以其体积小、重量轻、成本低、投放使用简单等特点在水文监测、水产养殖等领域具有广阔的应用前景。与AUV相比，ROV的推进器数量及布局形式与其自由度数目、运动性能以及控制方式有着密切联系，常见的推进器数量有三个、四个、六个、八个，加上安装位置的不同，ROV演变出多种布局形式和结构外形。而对于应用于近海、湖泊、水库等浅水水域的观察级ROV，出于成本、体积重量的考虑，主要采用三推进器、四推进器的布局形式。

自动定深控制是ROV的关键技术之一，现有的控制算法复杂，不易实现，三推进器ROV自动定深运动过程稳定性差，抵抗水下浪涌干扰能力差。

发明内容

本发明就是为了解决现有三推进器ROV自动定深控制算法复杂，不易实现，ROV自动定深运动过程稳定性差，抵抗水下浪涌干扰能力差的技术问题，提供了一种能够实现稳定地、可靠地进行斜航定深运动的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)]}。

优选地，a＝0.1，b＝0.05，c＝0.1。

本发明的有益效果是：

本发明的控制方法方便实用，能够使三推进器布局的欠驱动便携式ROV(RemotelyOperated Vehicle)稳定地、可靠地进行斜航定深运动，保证水下机器人自动到达并维持在指定深度，抵抗水下浪涌的干扰，实现水下稳定悬停观测及作业。ROV定深运动的斜航角度不超过±30°(30°为最佳角度)，最大斜航速度为1m/s，确保ROV在斜航定深过程中的机动性和稳定性，同时降低能量损耗和调控频率。

本发明进一步的特征和方面，将在以下参考附图的具体实施方式的描述中，得以清楚地记载。

附图说明

图1是高机动性监测水下机器人的立体图；

图2是图1所示结构拆除亚克力半球罩后的立体图；

图3是图1所示结构拆除亚克力半球罩后的主视图；

图4是亚克力半球罩、圆柱形中间段壳体、过渡段壳体和尾部壳体构成本体的外形曲线示意图；

图5是控制系统的一种具体实现形式；

图6是双闭环模糊PID控制系统原理图；

图7是自动定深运动控制过程的流程图；

图8是ROV水下机器人斜航定深运动轨迹示意图；

图9是隶属度函数的示意图；

图10是图9所示属度函数中间的三角形隶属函数。

图中符号说明：

1.亚克力半球罩，2.水平推进器，3.圆柱形中间段壳体，4.水密封头，5.过渡段壳体，6.尾部壳体，7.垂直推进器，8.摄像头，9.摄像头云台，10.重力块，11.防水开关，12.深度传感器，13.穿线螺丝；14.路由器，15.水面电力载波模块，16.智能手机，17.水下电力载波模块，18.电源模块，19.树莓派模块，20.Arduino控制板，21.姿态传感器，22.尾翼，100.ROV水下机器人。

具体实施方式

以下参照附图，以具体实施例对本发明作进一步详细说明。

如图1-3所示，本发明涉及到的ROV水下机器人包括机器人本体、动力装置和控制系统。机器人本体包括亚克力半球罩1、圆柱形中间段壳体3、水密封头4、过渡段壳体5、尾部壳体6、重力块10、穿线螺丝13，亚克力半球罩1与圆柱形中间段壳体3的前端连接，过渡段壳体5与圆柱形中间段壳体3的后端连接，尾部壳体6与过渡段壳体5连接，重力块10与圆柱形中间段壳体3的前端连接，穿线螺丝13与尾部壳体6连接，水密封头4与圆柱形中间段壳体3连接。亚克力半球罩1与圆柱形中间段壳体3之间通过○型密封圈进行密封，过渡段壳体5与圆柱形中间段壳体3之间通过○型密封圈进行密封，尾部壳体6与过渡段壳体5之间通过○型密封圈进行密封。尾翼22与尾部壳体6连接。

尾部壳体6可以采用3D打印制作，过渡段壳体5和圆柱形中间段壳体3可以采用6061铝合金制作。亚克力半球罩1的材质是亚克力，透光性能好，其为半球形壳，作为封头。

尾翼22与尾部壳体6可以通过3D打印技术进行一体化制作。

圆柱形中间段壳体3、过渡段壳体5和尾部壳体6可以采用采用密度较低、强度高、导热性好、耐腐蚀，并且易加工的铝合金材料制作。

水密封头4是脐带缆与壳体内部通信模块的防水连接件。

如图4所示，过渡段壳体5和尾部壳体6构成流线型回转体，尾部壳体6外形曲线的切线与轴线之间的夹角为：36°≤α≤39°，进一步优选为37.18°。亚克力半球罩1、圆柱形中间段壳体3、过渡段壳体5和尾部壳体6的具体尺寸优选的方案是(尺寸单位是mm)：亚克力半球罩1的半径为：78≤R≤83；圆柱形中间段壳体3的长度为:150≤L1≤155；过渡段壳体5的长度为：103≤L2≤107；过渡段壳体5和尾部壳体6合一起的长度是：197≤L1≤201。

动力装置包括一个垂直推进器7和两个水平推进器2，垂直推进器7与尾部壳体6连接，两个水平推进器2分别连接于圆柱形中间段壳体3的两侧。垂直推进器7位于机器人本体的尾部，两个水平推进器2位于机器人本体中部靠近亚克力半球罩1处。推进器具体可以采用螺旋桨式，推进器采用高转速大扭矩无刷直流电机进行驱动，推进器的电源线和信号线通过穿线螺丝13接入控制舱。

如图5所示，控制系统包括水面显控单元和水下控制单元，水面显控单元具有手动控制和自动控制两种模式。水下控制单元安装在机器人本体内部的控制舱中。

水面显控单元包括路由器14、水面电力载波模块15、智能手机16，智能手机16通过WIFI无线网络与路由器14连接，路由器14通过以太网线与水面电力载波模块15连接。

水下控制单元包括摄像头8、摄像头云台9、防水开关11、水下电力载波模块17、树莓派模块19、Arduino控制板20、深度传感器12、姿态传感器21、电源模块18，摄像头云台9安装在圆柱形中间段壳体3的前端，摄像头8安装在摄像头云台9上，防水开关11安装在圆柱形中间段壳体3上，深度传感器12安装在圆柱形中间段壳体3上，姿态传感器21安装在机器人壳体内，位于机器人本体的重心处。水下电力载波模块17、树莓派模块19、Arduino控制板20和电源模块18安装在圆柱形中间段壳体3或过渡段壳体5或尾部壳体6内。Arduino控制板20是一款拥有丰富的接口、简便的编程环境并且提供了极高的自由度和可拓展性的控制板，该Arduino控制板的硬件原理图、电路图、IDE软件及核心库文件都是开源的，该Arduino控制板是现有技术中的产品，可以从市场上买到，Arduino控制板的官方网站是：https://www.arduino.cc/。

电源模块18用于向水下电力载波模块17、树莓派模块19、Arduino控制板20、摄像头8、摄像头云台9、深度传感器12、姿态传感器21以及推进器供电。电源模块18可以采用锂电池和多路电压转换模块的结构，锂电池作为水下机器人的全部能源，提高了便携性，通过多路电压转换模块分别给舱体内部的电子器件供电，避免了互相干扰。深度传感器12和姿态传感器21的信号输出端与Arduino控制板20连接，摄像头云台9与Arduino控制板20连接，树莓派模块19通过USB接口与Arduino控制板20连接，水下电力载波模块17通过以太网线与树莓派模块19连接，摄像头8的信号输出端通过USB接口与树莓派模块19连接，防水开关11与Arduino控制板20连接。Arduino控制板20的控制信号输出端分别与垂直推进器7、两个水平推进器2连接。

水下电力载波模块17通过脐带缆与水面电力载波模块15连接。采用电力载波通信，可将以太网信号通过载波方式借助脐带缆进行远距离通信，抗干扰能力强。

摄像头8对水下情况进行检测，将视频信号发送给树莓派模块19，深度传感器12和姿态传感器21将检测到的深度、姿态信号发送给Arduino控制板20。Arduino控制板20将采集的深度、姿态信号发送给树莓派模块19，树莓派模块19将视频信号、深度、姿态信号依次通过水下电力载波模块17、脐带缆和水面电力载波模块15传输至路由器14，然后路由器14通过WIFI将数据发送给智能手机16，用户用过智能手机观测水下情况以及机器人运行状态信息。用户操作智能手机6上的APP软件，智能手机16发送的控制指令依次通过路由器14、水面电力载波模块15、脐带缆、水下电力载波模块17、树莓派模块19发送给Arduino控制板20，Arduino控制板20再控制摄像头云台9、垂直推进器7、两个水平推进器2工作，实现对水下机器人的运动控制和环境监测。通过智能手机6可进行按键式操作的手动控制，也可以按照预设的深度、航行轨迹进行自动控制；自动定深时采用模糊PID控制。

水平推进器2可实现机器人进退和转向，垂直推进器7可实现机器人的俯仰，三个推进器可单独或同时工作，组合工作可实现机器人浮潜。

摄像头8的视角可通过调节摄像头云台9改变。

防水开关11为总的电源开关，打开后电源模块18工作输出电压。

需要说明的是，Arduino控制板20可以用其他公知的嵌入式微控制器代替，水面显控单元用公知的PC计算机来控制。总之，控制系统的硬件采用公知技术，不在赘述。

本发明的ROV水下机器人的推进器布置形式为中部两个水平推进器和尾部单垂直推进器，这种布局使它的定深运动需要通过改变俯角或仰角的方式来实现。如图8所示，下潜时，ROV水下机器人的俯角为30°，同理，上升时，ROV水下机器人的仰角为30°，因此，参考图7的流程，使用如图6所示的双闭环模糊PID控制系统来实现自动定深运动，双闭环模糊PID控制系统的内环为倾角环(姿态传感器21为反馈装置)，外环为深度环(深度传感器12为反馈装置)，两个闭环分别由两个独立的且算法相同的模糊控制器和PID控制器来控制。两个独立的模糊控制器为二输入三输出控制器，输入量为深度的偏差e、偏差变化率ec或者倾角的偏差e、偏差变化率ec，输出量为PID参数K_P，K_I，K_D。因此，控制ROV水下机器人进行定深运动的具体过程如下：

第一步：通过MATLAB软件离线生成模糊查询表，具体过程包括：

步骤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}。

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

左侧为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

步骤3，模糊推理及解模糊化过程，设倾角环输入变量e＝α，ec＝β，从隶属度函数中可以看出α、β分别对应两个语言变量，如果α、β分别对应的语言变量为A1、A2和B1、B2，则隶属度值表示为：

μ_A1(α)，μ_A2(α)，μ_B1(β)，μ_B2(α)

由四个隶属度值根据以下公式(1)、(2)、(3)、(4)计算每条控制规则的强度:

ω₁＝μ_A1(α)Λμ_B1(β) (1)

ω₂＝μ_A1(α)Λμ_B2(β) (2)

ω₃＝μ_A2(α)Λμ_B1(β) (3)

ω₄＝μ_A4(α)Λμ_B2(β) (4)

从输入量的精确值α、β和相关的模糊规则中，可以得到每条规则对应的推理结果x₁，x₂，x₃，x₄，然后用重心法计算控制量的精确值：

第二步，在线查表，从表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为参数调整因子，a＝0.1，b＝0.05，c＝0.1。

在线查表的语言举例:

If e＝NM ec＝PS,thenΔKp＝PS,ΔKi＝NS,ΔKd＝NM

If e＝NS ec＝NM,thenΔKp＝PM,ΔKi＝NB,ΔKd＝NS

If e＝PM ec＝NM,thenΔKp＝ZE,ΔKi＝ZE,ΔKd＝PS

第三步，PID控制过程，根据采样数据与期望值进行比较获得偏差e(n)，对偏差进行PID运算从而改变PWM调速电路的脉冲的占空比实现推进器的电机两端电压的调节，进而控制电机转速。

式中，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)]}

以上所述仅对本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。

Claims (2)

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。