CN114879703A - 水下机器人路径跟踪控制方法 - Google Patents

水下机器人路径跟踪控制方法 Download PDF

Info

Publication number
CN114879703A
CN114879703A CN202210796832.3A CN202210796832A CN114879703A CN 114879703 A CN114879703 A CN 114879703A CN 202210796832 A CN202210796832 A CN 202210796832A CN 114879703 A CN114879703 A CN 114879703A
Authority
CN
China
Prior art keywords
path
area
time
underwater
algorithm
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210796832.3A
Other languages
English (en)
Other versions
CN114879703B (zh
Inventor
王贺
万俊贺
刘凯
李辉
寇磊
袁健
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Oceanographic Instrumentation Shandong Academy of Sciences
Original Assignee
Institute of Oceanographic Instrumentation Shandong Academy of Sciences
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Oceanographic Instrumentation Shandong Academy of Sciences filed Critical Institute of Oceanographic Instrumentation Shandong Academy of Sciences
Priority to CN202210796832.3A priority Critical patent/CN114879703B/zh
Publication of CN114879703A publication Critical patent/CN114879703A/zh
Application granted granted Critical
Publication of CN114879703B publication Critical patent/CN114879703B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种水下机器人路径跟踪控制方法，包括机载控制器接收岸基控制器生成并输出的任务控制指令，探测目标水下区域；若为危险区域，则通过切片算法生成局部路径，并基于局部路径生成期望路径；若为安全区域，则将安全区域划分为若干个子区域，基于子区域的局部路径生成期望路径；机载控制器配置为执行制导算法以得到与期望路径对应的期望航向角，机载控制器配置为以期望角速度和制导算法生成的期望航向角为控制目标，执行运动控制算法以控制潜水器主体运动，使得潜水器主体追踪期望路径，运动控制算法基于CFDL‑MFAC模型生成。本发明能够有效抵御水下风、浪、流的干扰，具备在复杂海况下进行水下航行的条件。

Description

水下机器人路径跟踪控制方法
技术领域
本发明属于海洋工程技术领域，尤其涉及一种水下机器人路径跟踪控制方法。
背景技术
水下机器人，也称无人遥控潜水器（Remote Operated Vehicle，ROV）是一种工作于水下的极限作业机器人。水下环境恶劣危险，人的潜水深度有限，所以水下机器人已成为开发海洋的重要工具。
典型的水下机器人包括水面设备和水下设备，其中水面设备包括但不限于操纵控制台、电缆绞车、吊放设备和供电系统，而水下设备主要包括中继器和潜水器主体。潜水器主体在水下靠推进器运动，潜水器主体上通常搭载有观测设备和作业设备，其中观测设备以摄像机、照相机和照明灯为代表，作业设备则包括但不限于机械手、切割器和清洗器等。
路径跟踪控制功能是实现水下机器人自主控制的关键，水下机器人的水下运动和作业，是指由操作员在上位机上控制和监视，靠电缆向潜水器主体提供动力和交换信息。中继器可减少电缆对潜水器主体运动的干扰。新型水下机器人从简单的遥控式向监控式发展，即由母舰计算机和潜水器主体计算机实行递阶控制，它能对观测信息进行加工，建立环境和内部状态模型。操作人员通过人机交互系统以面向过程的抽象符号或语言下达命令，并接受经计算机加工处理的信息，对潜水器主体的运行和动作过程进行监视并排除故障 。
但是，水下机器人设计与控制十分复杂，一方面是因为水下环境复杂，水下机器人在海底收到自身浮力、重力的作用，以及海洋风、浪、流的影响；另一方面海底环境恶劣，亮度很低；传统的水下机器人利用摄像头探测周围环境，然后通过岸基遥控来控制水下机器人航行，当水下环境的辨识度较低时，水下机器人路径跟踪控制难度较大。
本背景技术所公开的上述信息仅仅用于增加对本申请背景技术的理解，因此，其可能包括不构成本领域普通技术人员已知的现有技术。
发明内容
针对传统的水下机器人利用摄像头探测周围环境，然后通过岸基遥控来控制水下机器人航行，当水下环境的辨识度较低时，水下机器人路径跟踪控制难度较大的问题，设计并提供一种水下机器人路径跟踪控制方法。
为实现上述发明目的，本发明采用下述技术方案予以实现：
水下机器人路径跟踪控制方法，其特征在于包括以下步骤：机载控制器接收岸基控制器生成并输出的任务控制指令，探测目标水下区域；如果目标水下区域为危险区域，则通过切片算法生成与所述危险区域对应的局部路径，并基于局部路径生成危险区域的期望路径；如果目标水下区域为安全区域，将安全区域划分为若干个子区域，并在每个子区域内设置一个基点；基于若干个子区域的局部路径生成安全区域的期望路径；机载控制器配置为执行制导算法以得到与期望路径对应的期望航向角；所述制导算法由下式表示：
Figure 771862DEST_PATH_IMAGE001
其中，
Figure 689003DEST_PATH_IMAGE002
为期望航向角，
Figure 203161DEST_PATH_IMAGE003
为潜水器主体的实时位置在期望路径上的投影点所对应的期望路径切向坐标系相对于固定地坐标系的顺时针旋转角度，
Figure 598370DEST_PATH_IMAGE004
代表潜水器主体实时位置与期望路径的横向误差；Δ为前视距离，β为偏移角；所述前视距离Δ由下式计算：
Figure 412742DEST_PATH_IMAGE005
其中k1，k2为控制参数，k1为常数且满足k1>0，k2=1/U 且满足k2>0，Δmax为常数，表示前视距离的最大值，Δmin为常数，代表前视距离的最小值，U为潜水器主体合速度，e为自然常数；
机载控制器配置为以期望角速度和所述制导算法生成的期望航向角为控制目标，执行运动控制算法以控制潜水器主体运动，使得潜水器主体追踪期望路径，所述运动控制算法基于CFDL-MFAC模型生成。
进一步的，所述运动控制算法由下式表示：
Figure 235205DEST_PATH_IMAGE006
其中，
Figure 95713DEST_PATH_IMAGE007
表示采样时间；
Figure 294613DEST_PATH_IMAGE008
表示k时刻的航向角输出；
Figure 963492DEST_PATH_IMAGE009
表示k时刻纵向线速度输出；
Figure 753594DEST_PATH_IMAGE010
表示k时刻横向线速度输出；
Figure 507923DEST_PATH_IMAGE011
表示k时刻航向角速度输出；
Figure 244935DEST_PATH_IMAGE012
表示k时刻偏航力矩输入，
Figure 768320DEST_PATH_IMAGE013
表示k+1时刻的航向角输出，
Figure 667006DEST_PATH_IMAGE014
表示k+1时刻的航向角速度输出，m11、m22和m33为潜水器主体的惯性质量参数，d33为潜水器主体的水动力阻尼参数。
进一步的，所述运动控制算法的航向角速度增益
Figure 908631DEST_PATH_IMAGE015
满足：
Figure 183755DEST_PATH_IMAGE016
其中，
Figure 827226DEST_PATH_IMAGE017
为一个采样周期内航向角速度变化量，
Figure 21447DEST_PATH_IMAGE018
为单位时间内最大正向航向角速度，
Figure 750368DEST_PATH_IMAGE007
表示采样时间。
进一步的，判定目标水下区域是否为危险区域时执行以下步骤：当机载控制器接收岸基控制器生成并输出的任务控制指令后，采样摄像装置和声呐的检测信号，并判断在设定范围内是否存在直径大于潜水器主体自身最大直径的对象；如果设定范围内存在直径大于潜水器主体自身最大直径的对象，则判定目标水下区域为危险区域；否则判定目标水下区域为安全区域。
进一步的，通过切片算法生成与危险区域对应的局部路径并基于局部路径生成危险区域的期望路径包括以下步骤：机载控制器在判定为危险区域的目标水下区域的设定高度上确定一个与潜水器主体实时航行角平行的水平切面，并在水平切面上确定一条直线作为局部路径；重复上述步骤并将若干个所述局部路径连接起来生成危险区域的期望路径。
与现有技术相比，本发明的优点和积极效果是：本发明将三维路径转化为平面路径，降低计算难度，有利于控制水下机器人，使用改进的视线法作为制导律以及CFDL-MFAC作为控制律进行追踪控制，能够有效抵御水下风、浪、流的干扰，具备在复杂海况下进行水下航行的条件。
结合附图阅读本发明的具体实施方式后，本发明的其他特点和优点将变得更加清楚。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1 为适用于本发明所提供的水下机器人路径跟踪控制方法的水下机器人中潜水器主体的主视图；
图2为适用于本发明所提供的水下机器人路径跟踪控制方法的水下机器人中潜水器主体的后视图；
图3为水下机器人中机载控制器和岸基控制器之间的通信原理示意框图；
图4为本发明所提供的水下机器人路径跟踪控制方法一个具体实施例的流程图；
图5为本发明所提供的水下机器人路径跟踪控制方法的原理示意框图；
图6为本发明所提供的水下机器人路径跟踪控制方法的视法图；
图7为CFDL-MFAC的控制原理示意框图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下将结合附图和实施例，对本发明作进一步详细说明。
需要说明的是，在本发明的描述中，术语“上”、“下”、“左”、“右”、“竖”、“横”、“内”、“外”等指示的方向或位置关系的术语是基于附图所示的方向或位置关系，这仅仅是为了便于描述，而不是指示或暗示所述装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。
图1和图2所示为适用于本发明所提供的水下机器人路径跟踪控制方法的水下机器人的部分结构示意图。如图1和图2所示，水下机器人包括潜水器主体10。潜水器主体10包括水下推进系统、电源控制舱30和耐压舱32。潜水器主体10选用氧化铝合金和浮力材料制成，这两种材料的组合一方面可以有效减轻潜水器主体10的自重，另一方面也可以确保潜水器主体10在水下保持良好的悬浮状态。在本实施方式中，浮力材料优选为玻璃微珠，玻璃微珠可以确保潜水器主体10具备良好的耐水性和抗压性。
潜水器主体10在水平方向配备四个水下推进器（12、14、16和18），在垂直方向也配备四个水下推进器（20、22、24和26），共八个水下推进器组成水下推进系统，从而满足水下六自由度驱动，实现大于2节速度以及抗1节以上海流的能力，并可准确、稳定、快速实现位置运动控制。电源控制舱30和耐压舱32可选地采用不锈钢材质（例如选用316L不锈钢）加工制成，满足耐压及抗海水腐蚀的要求。潜水器主体10中所选用的结构连接附件可以选用由聚四氟乙烯、尼龙、聚甲醛树脂（Polyformaldehyde、POM）等材料制成的隔板进行区隔和组装，一方面可以保证结构强度，另一方面也可以防止电化学腐蚀等情况的发生，更好的保证装置的安全性。
电源控制舱30内设置有电源、第一直流转换器模块（也称DC-DC转换模块，Directcurrent-Direct current converter）、电力载波模块、电子调速器、单片机和第一漏水检测装置等部件。八个水下推进器与侧板（34和36）螺栓连接，电源为水下推进器供电。第一漏水检测装置配置为检测电源控制舱30内部是否存在漏水现象，漏水检测装置可以选用市售的漏水检测仪。
耐压舱32中设置有惯性导航系统（Inertial Navigation System，INS）、机载控制器48、第二漏水检测装置、交换机和第二直流转换器模块等部件。惯性导航系统利用惯性敏感器件（加速度计和陀螺仪）、基准方向以及初始位置信息确定潜水器主体10在惯性空间中的位置、方向和速度，实现自主式导航。在本实施方式中，惯性导航系统可以采用现有技术中成熟的系统和算法（例如积分计算），不是本发明的保护重点，在此不再赘述。惯性导航系统配置为向机载控制器48提供潜水器主体10的实时位置和实时姿态。
潜水器主体10还包括传感器模块，如图2所示，传感器模块包括但不限于搭载于潜水器主体10的摄像装置38（其中集成有灯组）、图像采集装置（例如红外传感器，图中未示出）、声音导航测距装置40（声呐）、声学多普勒速度仪42、深度计44、定位模块（例如GPS模块、北斗定位模块和无线电台）、温度传感器、湿度传感器、电压检测装置和电流检测装置；其中声学多普勒速度仪42配置为提供潜水器主体10自身坐标系下的速度信息，深度计44配置为提供深度信息，定位模块则可以在水面航行时获取精确位置信息。虽然惯性导航系统作为公共参考系统可以提供全部导航数据，但是惯性导航系统的导航精度会随着时间逐渐发散，定位模块可以配置为当潜水器主体10在水面使用时对惯性导航系统的定位累计误差进行修正，声学多普勒速度仪42可以限制惯性导航系统的速度误差累计，从而抑制定位漂移，深度计44则可以修正惯性导航系统在高度通道上的误差。
传感器模块与机载控制器48通信连接，传感器模块采样的数据以及惯性导航系统利用惯性敏感器件采样的数据均输出至机载控制器48中，由机载控制器48进行处理或转发。考虑到水下运行对结构强度、防尘、防潮、防腐蚀的要求，机载控制器48优选由工控机实现。机载控制器48配置为实现潜水器主体10的导航、路径规划和决策以及运动控制，并对潜水器主体10的状态进行监测以进行必要的应急控制。机载控制器48配置有多串口扩展卡和高速控制器局域网络（Controller Area Network， CAN）接口卡。
潜水器主体10中所采用的水密接插件46可以有效解决供电以及各传感器之间的水密连接和通信问题，其中水密接插件46用于为潜水器主体10供电，其余水密接插件46用于各个传感器之间的水密连接通信。
除潜水器主体10之外，水下机器人还包括岸基控制器50。岸基控制器50优选设置在岸基工作站中。岸基控制器50中设置有存储单元、处理器、输入/输出接口、通信接口等元件，其中存储单元可以包括易失性存储器和非易失性存储器，存储单元配置为存储与潜水器主体10至少一个装置或设备相关联的指令或数据，例如存储潜水器主体10的任务规划和控制指令。示例性地，应用程序可以为通过潜水器主体10执行某一区域的打捞、求生、检测等任务。处理器可以是专用处理器、中央处理单元（CPU）等。处理器可以访问存储单元以执行在存储单元中所存储的指令以实现相关功能。输入/输出端口可以是串行通信接口。岸基控制器50可以通过输入/输出端口通信连接显示器，以通过显示器显示潜水器主体10的状态、动力参数、姿态、告警信息；或者通过输入/输出端口通信连接人机交互界面，通过人机交互界面实现控制参数的调节或调试。岸基控制器50还可以配置支持一种或多种无线通信协议的通信接口，以与无线终端实现交互连接，实现远程控制。
如图3所示，岸基控制器50与机载控制器48优选通过光纤实现双向通信连接，例如通过千兆交换机实现双向通信。当机载控制器48和岸基控制器50之间通信异常时，岸基控制器50自行执行信道异常检测并执行信道切换。
本发明所提供的水下机器人路径跟踪控制方法具体包括如图4所示的多个步骤。
步骤S11：机载控制器接收岸基控制器生成并输出的任务控制指令，探测目标水下区域。
步骤S12：判定目标水下区域是否为危险区域。
具体来说，当机载控制器接收岸基控制器生成并输出的任务控制指令后，采样摄像装置和声呐的检测信号，通过图像处理判断在设定范围内是否存在直径大于潜水器主体自身最大直径的对象；如果设定范围内存在直径大于潜水器主体自身最大直径的对象，则判定目标水下区域为危险区域；否则判定目标水下区域为安全区域。设定范围可选的设定为以10米为单位（边长或直径）的三维空间。潜水器主体自身最大直径可以是最大外切圆的直径。
步骤S13：如果目标水下区域为危险区域，则通过切片算法生成与所述危险区域对应的局部路径，并基于局部路径生成危险区域的期望路径。
具体来说，机载控制器在判定为危险区域的目标水下区域的设定高度上确定一个与潜水器主体实时航行角平行的水平切面，并在水平切面上确定一条直线作为局部路径。重复上述过程以遍历整个危险区域，将若干个局部路径连接起来生成危险区域的期望路径。设定高度优选为安全高度，即通过图像处理可以判定在设定高度下，潜水器主体不会与对象发生接触。
步骤S14：如果目标水下区域不属于危险区域，则目标水下区域为安全区域，则将安全区域划分为若干个子区域，并在每个子区域内设置一个基点。在子区域内可确定局部路径，局部路径可选地覆盖所述基点，将若干个局部路径连接起来生成安全区域的期望路径。由于子区域和基点一一对应，通过搜索基点，即可以找到该子区域，并确定期望路径是否遍历到对应的子区域。
通过上述步骤，将三维路径转化为平面路径，降低计算难度，从而更有利于控制水下机器人。
步骤S15：机载控制器配置为执行制导算法以得到与期望路径对应的期望航向角。
以下对制导算法（也称制导律）进行介绍。制导算法的原理是：如果使被控的潜水器主体的航向保持对准视线角（期望航向角），那么经过适当的控制就能使被控的潜水器主体到达期望的位置，达到航迹跟踪的效果。也即制导算法根据期望路径点与潜水器主体的实时位置信息，计算当前期望航向角。
如图6所示，具体来说，潜水器主体的实时位置、速度以及误差可以由一个三维路径点坐标表示。三维路径点坐标可示例性地表示为：
Figure 829183DEST_PATH_IMAGE019
；其中，
Figure 123898DEST_PATH_IMAGE020
代表潜水器主体在固定于地面的惯性坐标系
Figure 895545DEST_PATH_IMAGE021
下沿
Figure 846183DEST_PATH_IMAGE022
的投影（图6中分别用X E 和Y E 表示
Figure 728689DEST_PATH_IMAGE022
）；u代表潜水器主体在固定于潜水器主体自身的坐标系0-xyz下沿x轴的速度，v代表潜水器主体在固定于潜水器主体自身的坐标系0-xyz下沿y轴的速度，
Figure 550014DEST_PATH_IMAGE023
代表潜水器主体实时位置与期望路径的横向误差，
Figure 492562DEST_PATH_IMAGE024
代表潜水器主体实时位置与期望路径的纵向误差。
基于潜水器主体的实时位置与期望路径点的位置，计算前视距离
Figure 930497DEST_PATH_IMAGE025
。
在可选的实施方式中，前视距离
Figure 351114DEST_PATH_IMAGE025
基于自适应视线制导法计算。以下对前视距离计算的推导过程进行介绍。
潜水器主体的合速度可以表示为：
Figure 682738DEST_PATH_IMAGE026
；
潜水器主体三自由度方程为：
Figure 796188DEST_PATH_IMAGE027
其中，
Figure 721418DEST_PATH_IMAGE028
为航向角，r为航向角速度。
假设潜水器主体的位置（x，y）在期望路径上的投影记为
Figure 414568DEST_PATH_IMAGE029
，
Figure 476065DEST_PATH_IMAGE030
是一个正的路径变量。期望路径
Figure 760416DEST_PATH_IMAGE031
由若干个非封闭期望路径点组成。投影点对应的期望路径切向坐标系在投影点相对于固定地坐标系的顺时针旋转角度可以表示为：
Figure 172942DEST_PATH_IMAGE032
，其中：
Figure 997679DEST_PATH_IMAGE033
。
误差向量如式（2）所示：
Figure 179262DEST_PATH_IMAGE034
对
Figure 368935DEST_PATH_IMAGE035
求导数：
Figure 268757DEST_PATH_IMAGE036
将式1代入式3得：
Figure 428343DEST_PATH_IMAGE037
其中
Figure 730012DEST_PATH_IMAGE038
；
Figure 90586DEST_PATH_IMAGE039
；
β为侧滑角，
Figure 212126DEST_PATH_IMAGE040
；
虚拟参考点速度
Figure 581927DEST_PATH_IMAGE041
可以表示为：
Figure 472523DEST_PATH_IMAGE042
；
对
Figure 269577DEST_PATH_IMAGE043
求导数：
Figure 675151DEST_PATH_IMAGE044
将式1代入式6得：
Figure 848643DEST_PATH_IMAGE045
得到误差的导数如下式表示：
Figure 859325DEST_PATH_IMAGE046
由于侧滑角β非零时变，且是非常小的正数（通常不大于5度），可以近似认为
Figure 827281DEST_PATH_IMAGE047
。
假设
Figure 516888DEST_PATH_IMAGE048
，式8可以改写为：
Figure 228492DEST_PATH_IMAGE049
进一步可以将前视距离
Figure 93680DEST_PATH_IMAGE025
设定为与速度、跟踪误差相关的函数，前视距离
Figure 966958DEST_PATH_IMAGE025
可以表示为：
Figure 550386DEST_PATH_IMAGE050
其中k1，k2为控制参数，k1>0且k2>0。在可选的实施方式中，k1=0.8，k2=1/U。Δmax为前视距离最大值，Δmin为前视距离最小值，e为自然常数。在可选的实施方式中，
Figure 800102DEST_PATH_IMAGE051
，
Figure 785375DEST_PATH_IMAGE052
。
制导律的原理是：如果使被控的潜水器主体的航向保持对准视线角，那么经过适当的控制就能使被控的潜水器主体到达期望的位置，达到航迹跟踪的效果；也就是期望航向角（即LOS角度）
Figure 688609DEST_PATH_IMAGE053
会引导被控的潜水器主体向期望轨迹移动直到
Figure 759333DEST_PATH_IMAGE054
。传统的制导律可以由下式表示：
Figure 812740DEST_PATH_IMAGE055
然而，在未知的海况下，潜水器主体主要收到外界扰动引起的漂移力，在路径跟随过程中非零漂移角会产生明显扰动。为了处理洋流和其它环境扰动的影响，设计并通过机载控制器执行一种新的制导算法。
制导算法由下式表示：
Figure 652520DEST_PATH_IMAGE056
由于前视距离
Figure 929918DEST_PATH_IMAGE025
为速度、跟踪误差相关的函数，而在本发明所提供的制导算法中进一步引入偏移角β，可以克服在制导过程中水下漂移角对路径跟踪控制的影响，可以降低潜水器主体偏离期望路径的概率，提高路径跟踪的精确性，避免较大的跟踪误差；上述制导算法可以避免在横向误差等于0的情况下，输出的期望航向角过大，导致跟踪精度下降。偏移角可以由现有的控制方法或检测手段获得。
结合期望速度以及制导算法输出的期望航向角
Figure 222359DEST_PATH_IMAGE053
（以期望角速度为例），可以得到潜水器主体的期望姿态。
步骤S16：机载控制器配置为以期望角速度和所述制导算法生成的期望航向角为控制目标，执行运动控制算法以控制潜水器主体运动，使得潜水器主体追踪期望路径。运动控制算法基于CFDL-MFAC模型生成。在可选的实施方式中，期望角速度的初始值也可以根据所执行的任务确定并由岸基控制器传输至机载控制器。
如图5所示，在制导算法输出期望航向角
Figure 79456DEST_PATH_IMAGE053
后，结合期望速度，机载控制器配置为执行无模型自适应控制算法（Model Free Adaptive Control，MFAC）以对潜水器主体进行控制，也即对推进系统进行控制，控制各个水下推进器的运动状态使得潜水器主体实现期望路径的追踪。
潜水器主体航向非线性控制系统的设计是整个控制系统核心部分。无模型自适应控制算法是于1994年提出的、针对离散时间非线性系统使用一种新的动态线性化方法，提出伪偏导数（Pseudo Partial Derivative，PPD）的新概念。伪偏导数参数可仅仅使用被控对象的输入/输出（Input/Output，I/O）数据来进行估计。无模型自适应控制算法的动态线性化有三种具体形式，分别是紧格式动态线性化（Compact Form Dynamic Linearization，CFDL）、偏格式动态线性化（Partial Form Dynamic Linearization，PFDL）和全格式动态线性化（Full Form Dynamic Linearization，FFDL）。
一般SISO离散时间非线性系统可以表示为：
Figure 773743DEST_PATH_IMAGE057
其中：u(k)表示k时刻非线性系统的输入，y(k)表示k时刻非线性系统的输出，
Figure 894146DEST_PATH_IMAGE058
，R是未知的非线性函数；
Figure 939462DEST_PATH_IMAGE059
是两个未知的正整数。
对式（13）表示的非线性系统有如下假设。
假设1：除有限时刻点外，
Figure 334671DEST_PATH_IMAGE060
关于第
Figure 742519DEST_PATH_IMAGE061
个变量的偏导数是连续的。
假设2：除有限时刻点外，非线性系统（13）满足广义的利普希茨连续条件（Lipschitz continuity），即对任意
Figure 830561DEST_PATH_IMAGE062
和
Figure 97594DEST_PATH_IMAGE063
有：
Figure 296494DEST_PATH_IMAGE064
其中，
Figure 168635DEST_PATH_IMAGE065
，i=1，2；b为大于0的常数。
定理1：对满足假设1和假设2的非线性系统（如式13所示），当
Figure 427578DEST_PATH_IMAGE066
时，一定存在一个被称为伪偏导数(PPD)的时变参数
Figure 181907DEST_PATH_IMAGE067
，使得非线性系统可转化为如下的紧格式动态线性化数据模型：
Figure 918919DEST_PATH_IMAGE068
并且参数
Figure 239042DEST_PATH_IMAGE069
对任意时刻k有界。
通过紧格式动态线性化将一般的离散时间非线性系统转化为含有一个纯量参数
Figure 668887DEST_PATH_IMAGE070
的线性时变动态数据模型，紧格式动态线性化本质是只考虑系统在下一时刻的输出变化量与当前时刻的输入变化量之间的时变动态关系。
基于紧格式动态线性化方法，由定理1可知，当非线性系统（13）满足假设1和假设2，且对所有时刻k有
Figure 644933DEST_PATH_IMAGE071
成立时，紧格式动态线性化数据模型可以表示为：
Figure 44690DEST_PATH_IMAGE072
其中，
Figure 688161DEST_PATH_IMAGE073
为非线性系统（如式13所示）的伪偏导数（Pseudo PartialDerivative, PPD），
Figure 23327DEST_PATH_IMAGE074
。
式（15）是非线性系统的一种等价动态线性化表示，是一个用于控制器设计、具有简单增量形式的含有一个单参数的线性时变数据模型。对于离散时间系统，由最小化一步向前预报误差准则函数得到的控制算法有可能产生过大的控制输入，使控制系统本身遭到破坏，而由最小化加权一步向前预报误差准则函数得到的控制算法又有可能产生稳态的跟踪误差。
为解决上述问题，引入如下控制输入准则函数：
Figure 752249DEST_PATH_IMAGE075
其中，
Figure 299905DEST_PATH_IMAGE076
为权重因子，用于限制控制输入量的变化，
Figure 797882DEST_PATH_IMAGE077
；
Figure 303950DEST_PATH_IMAGE078
为期望的输出信号。
将式（15）代入式（16）中，对
Figure 520168DEST_PATH_IMAGE079
求导，并令
Figure 199411DEST_PATH_IMAGE079
等于0，可以得到：
Figure 551895DEST_PATH_IMAGE080
其中，
Figure 494443DEST_PATH_IMAGE081
为步长因子，用于使控制算法更具一般性，
Figure 525853DEST_PATH_IMAGE082
。
伪偏导数估计准则函数如下所示：
Figure 946470DEST_PATH_IMAGE083
其中，
Figure 419040DEST_PATH_IMAGE084
为权重因子，
Figure 266910DEST_PATH_IMAGE085
。
对式（18）关于
Figure 926561DEST_PATH_IMAGE086
取极值，可得到伪偏导数的估计算法为：
Figure 150869DEST_PATH_IMAGE087
其中
Figure 477946DEST_PATH_IMAGE088
是加入的步长因子，
Figure 762296DEST_PATH_IMAGE089
。
当
Figure 768298DEST_PATH_IMAGE090
，如果
Figure 530718DEST_PATH_IMAGE091
，
Figure 712301DEST_PATH_IMAGE092
，或
Figure 167553DEST_PATH_IMAGE093
有：
Figure 864113DEST_PATH_IMAGE094
其中，
Figure 430224DEST_PATH_IMAGE095
；
Figure 466313DEST_PATH_IMAGE096
是一个充分小的正数；
Figure 561308DEST_PATH_IMAGE097
是
Figure 948427DEST_PATH_IMAGE098
的初始值；
Figure 52649DEST_PATH_IMAGE099
是符号函数。
因为伪偏导数
Figure 208824DEST_PATH_IMAGE069
对时变参数、时变结构、时变相位甚至滞后均并不敏感，因此，CFDL-MFAC具有非常强的鲁棒性和适应性。MFAC尤其适用于为一类离散时间非线性系统构建控制方案，此类系统在满足假设1和假设2的基础上，还满足假设3和假设4。
假设3：对某一给定的有界期望输出信号
Figure 864933DEST_PATH_IMAGE100
，总存在一个有界的
Figure 473769DEST_PATH_IMAGE101
，使得系统在控制输入信号的驱动下，其输出等于
Figure 647262DEST_PATH_IMAGE100
。
假设4：对任意时刻k及
Figure 657943DEST_PATH_IMAGE102
，离散时间非线性系统的符号保持不变，即满足
Figure 422637DEST_PATH_IMAGE103
或
Figure 518769DEST_PATH_IMAGE104
，其中
Figure 964793DEST_PATH_IMAGE105
是一个非常小的正数。
假设3可以认为是系统可设计求解的一个必要条件，即如式（15）所示的系统是输出可控的。假设4的物理意义是指控制输入增加时，其相应的受控系统输出应是不减的。
考虑到潜水器主体的航向输出范围为-180度至180度。但是当输入的舵角或偏航力矩增大时，航向角并不是一定增大，所以这组输入输出参数不能满足CFDL-MFAC中的假设4，而假设4可以认为是受控系统所必须满足的条件，所以将受控系统的输出定义为：
Figure 95560DEST_PATH_IMAGE106
即将航向角
Figure 703259DEST_PATH_IMAGE028
和航向角速度r作为系统的输出信号，
Figure 286687DEST_PATH_IMAGE015
为航向角速度增益，
Figure 536403DEST_PATH_IMAGE107
是正常数，且满足
Figure 380731DEST_PATH_IMAGE108
，即有：
Figure 690490DEST_PATH_IMAGE109
其中期望航向角速度
Figure 761214DEST_PATH_IMAGE110
恒为零，在选择合适的参数
Figure 814621DEST_PATH_IMAGE015
后，当控制输入增大时，
Figure 123242DEST_PATH_IMAGE111
增大，可以抵消航向角
Figure 603902DEST_PATH_IMAGE028
的减小值，保证受控输出
Figure 896343DEST_PATH_IMAGE112
持续增大。可以满足假设4的要求，使得CFDL-MFAC可以用于潜水器主体航向控制。
以下对航向角速度增益
Figure 753441DEST_PATH_IMAGE113
进行分析，已知潜水器主体旋转矩阵
Figure 244465DEST_PATH_IMAGE114
，系统惯性矩阵M，科氏力矩阵
Figure 896026DEST_PATH_IMAGE115
和阻尼矩阵
Figure 941343DEST_PATH_IMAGE116
：
Figure 336552DEST_PATH_IMAGE117
三自由度运动学及动力学模型为：
Figure 478820DEST_PATH_IMAGE118
式（25）中第3项和第6项构成航向控制系统，可以改写为离散形式：
Figure 566862DEST_PATH_IMAGE119
Figure 833895DEST_PATH_IMAGE007
表示采样时间；
Figure 298375DEST_PATH_IMAGE008
表示系统在k时刻航向角输出；
Figure 967253DEST_PATH_IMAGE009
表示系统在k时刻纵向线速度输出；
Figure 819672DEST_PATH_IMAGE010
表示系统在k时刻横向线速度输出；
Figure 308422DEST_PATH_IMAGE011
表示系统在k时刻航向角速度输出；
Figure 311013DEST_PATH_IMAGE012
表示系统在k时刻偏航力矩输入；
Figure 834398DEST_PATH_IMAGE013
表示k+1时刻的航向角输出，
Figure 733084DEST_PATH_IMAGE120
表示k+1时刻的航向角速度输出，m11、m22和m33为潜水器主体的惯性质量参数，d33为潜水器主体的水动力阻尼参数。
式（22）差分形式的输出为：
Figure 974710DEST_PATH_IMAGE121
考虑航向控制系统（如式26所示），对比式（27）和式（14），航向控制子系统伪偏导数等价表示为：
Figure 515412DEST_PATH_IMAGE122
在满足
Figure 486779DEST_PATH_IMAGE123
的条件下，
Figure 87525DEST_PATH_IMAGE124
成立等价于不等式（29）成立。
Figure 550867DEST_PATH_IMAGE125
。
假设航向角
Figure 691999DEST_PATH_IMAGE126
逐渐增大到180度后，下一时刻变为-180度，此时满足
Figure 189976DEST_PATH_IMAGE127
；由式（26）可知，当满足
Figure 696044DEST_PATH_IMAGE128
时，
Figure 381103DEST_PATH_IMAGE129
恒成立。式（29）可以改写为：
Figure 263608DEST_PATH_IMAGE130
即：
Figure 616092DEST_PATH_IMAGE131
潜水器主体航向控制具有相对固定的动力学特性，即在一个采样周期内航向角速度变化量
Figure 293061DEST_PATH_IMAGE017
满足
Figure 590051DEST_PATH_IMAGE132
，
Figure 276247DEST_PATH_IMAGE018
为单位时间内最大正向航向角速度。由于
Figure 483237DEST_PATH_IMAGE133
，所以，不等式（31）满足：
Figure 596687DEST_PATH_IMAGE134
即
Figure 990759DEST_PATH_IMAGE135
存在最小值
Figure 215067DEST_PATH_IMAGE136
，对于任意时刻k，伪偏导数
Figure 542143DEST_PATH_IMAGE137
，满足CFDL-MFAC所适用的离散时间非线性系统的假设。
设定潜水器主体的惯性质量参数m11-m33和水动力阻尼参数d11-d33，输入期望航向角和期望角速度，即可以生成控制信号驱动八个水下推进器转动，通过反馈使得潜水器主体的姿态和位置得到控制，潜水器主体与期望路径的偏差减小，并最终在期望路径的允许偏差内移动。本发明所使用的控制方法能够有效抵御水下风、浪、流的干扰，具备在复杂海况下进行水下航行的目的。控制信号驱动水下推进器转动可以采用现有技术中常见的电机调向、调速算法，不是本发明的重点，在此不再赘述。
以上实施例仅用以说明本发明的技术方案，而非对其进行限制；尽管参照前述实施例对本发明进行了详细的说明，对于本领域的普通技术人员来说，依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明所要求保护的技术方案的精神和范围。

Claims (5)

1.水下机器人路径跟踪控制方法，其特征在于，包括以下步骤：
机载控制器接收岸基控制器生成并输出的任务控制指令，探测目标水下区域；
如果目标水下区域为危险区域，则通过切片算法生成与所述危险区域对应的局部路径，并基于局部路径生成危险区域的期望路径；
如果目标水下区域为安全区域，将安全区域划分为若干个子区域，并在每个子区域内设置一个基点；基于若干个子区域的局部路径生成安全区域的期望路径；
机载控制器配置为执行制导算法以得到与期望路径对应的期望航向角；所述制导算法由下式表示：
Figure 554033DEST_PATH_IMAGE001
其中，
Figure 700980DEST_PATH_IMAGE002
为期望航向角，
Figure 728979DEST_PATH_IMAGE003
为潜水器主体的实时位置在期望路径上的投影点所对应的期望路径切向坐标系相对于固定地坐标系的顺时针旋转角度，
Figure 707300DEST_PATH_IMAGE004
代表潜水器主体实时位置与期望路径的横向误差；Δ为前视距离，β为偏移角；所述前视距离Δ由下式计算：
Figure 162552DEST_PATH_IMAGE005
其中k1，k2为控制参数，k1为常数且满足k1>0；k2=1/U且满足k2>0；Δmax为常数，表示前视距离的最大值；Δmin为常数，代表前视距离的最小值；U为潜水器主体合速度；e为自然常数；
机载控制器配置为以期望角速度和所述制导算法生成的期望航向角为控制目标，执行运动控制算法以控制潜水器主体运动，使得潜水器主体追踪期望路径，所述运动控制算法基于CFDL-MFAC模型生成。
2.根据权利要求1所述的水下机器人路径跟踪控制方法，其特征在于，
所述运动控制算法由下式表示：
Figure 62375DEST_PATH_IMAGE006
其中，
Figure 628485DEST_PATH_IMAGE007
表示采样时间；
Figure 523629DEST_PATH_IMAGE008
表示k时刻的航向角输出；
Figure 149782DEST_PATH_IMAGE009
表示k时刻纵向线速度输出；
Figure 271322DEST_PATH_IMAGE010
表示k时刻横向线速度输出；
Figure 109965DEST_PATH_IMAGE011
表示k时刻航向角速度输出；
Figure 266140DEST_PATH_IMAGE012
表示k时刻偏航力矩输入，
Figure 63195DEST_PATH_IMAGE013
表示k+1时刻的航向角输出，
Figure 672031DEST_PATH_IMAGE014
表示k+1时刻的航向角速度输出，m11、m22和m33为潜水器主体的惯性质量参数，d33为潜水器主体的水动力阻尼参数。
3.根据权利要求2所述的水下机器人路径跟踪控制方法，其特征在于，
所述运动控制算法的航向角速度增益
Figure 376681DEST_PATH_IMAGE015
满足：
Figure 387363DEST_PATH_IMAGE016
其中，
Figure 355319DEST_PATH_IMAGE017
为一个采样周期内航向角速度变化量，
Figure 451451DEST_PATH_IMAGE018
为单位时间内最大正向航向角速度，
Figure 756530DEST_PATH_IMAGE007
表示采样时间。
4.根据权利要求1所述的水下机器人路径跟踪控制方法，其特征在于，
判定目标水下区域是否为危险区域时执行以下步骤：
当机载控制器接收岸基控制器生成并输出的任务控制指令后，采样摄像装置和声呐的检测信号，并判断在设定范围内是否存在直径大于潜水器主体自身最大直径的对象；
如果设定范围内存在直径大于潜水器主体自身最大直径的对象，则判定目标水下区域为危险区域；否则判定目标水下区域为安全区域。
5.根据权利要求4所述的水下机器人路径跟踪控制方法，其特征在于，
通过切片算法生成与危险区域对应的局部路径并基于局部路径生成危险区域的期望路径包括以下步骤：
机载控制器在判定为危险区域的目标水下区域的设定高度上确定一个与潜水器主体实时航行角平行的水平切面，并在水平切面上确定一条直线作为局部路径；重复上述步骤并将若干个所述局部路径连接起来生成危险区域的期望路径。
CN202210796832.3A 2022-07-08 2022-07-08 水下机器人路径跟踪控制方法 Active CN114879703B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210796832.3A CN114879703B (zh) 2022-07-08 2022-07-08 水下机器人路径跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210796832.3A CN114879703B (zh) 2022-07-08 2022-07-08 水下机器人路径跟踪控制方法

Publications (2)

Publication Number Publication Date
CN114879703A true CN114879703A (zh) 2022-08-09
CN114879703B CN114879703B (zh) 2022-09-09

Family

ID=82683618

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210796832.3A Active CN114879703B (zh) 2022-07-08 2022-07-08 水下机器人路径跟踪控制方法

Country Status (1)

Country Link
CN (1) CN114879703B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115562312A (zh) * 2022-11-17 2023-01-03 三亚哈尔滨工程大学南海创新发展基地 一种强跟踪全格式无模型自适应船舶艏向控制方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104898594A (zh) * 2015-04-02 2015-09-09 山东省科学院海洋仪器仪表研究所 一种用于水下机器人群协同控制的通信装置与方法
CN109901402A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 一种基于航向平滑技术的自主水下机器人路径跟踪方法
CN109901598A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 基于随机模型预测控制技术的自主水下机器人路径跟踪方法
CN112147899A (zh) * 2020-09-30 2020-12-29 沈阳工业大学 一种基于模糊滑模算法的水下机器人自主避障控制方法
CN113220000A (zh) * 2021-05-11 2021-08-06 华中科技大学 一种面向水下探测作业的无人艇路径跟踪预设性能控制方法及系统
CN113784042A (zh) * 2021-08-24 2021-12-10 中国电子科技集团公司电子科学研究院 一种障碍物环境下无人机集群的自组织侦察监视方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104898594A (zh) * 2015-04-02 2015-09-09 山东省科学院海洋仪器仪表研究所 一种用于水下机器人群协同控制的通信装置与方法
CN109901402A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 一种基于航向平滑技术的自主水下机器人路径跟踪方法
CN109901598A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 基于随机模型预测控制技术的自主水下机器人路径跟踪方法
CN112147899A (zh) * 2020-09-30 2020-12-29 沈阳工业大学 一种基于模糊滑模算法的水下机器人自主避障控制方法
CN113220000A (zh) * 2021-05-11 2021-08-06 华中科技大学 一种面向水下探测作业的无人艇路径跟踪预设性能控制方法及系统
CN113784042A (zh) * 2021-08-24 2021-12-10 中国电子科技集团公司电子科学研究院 一种障碍物环境下无人机集群的自组织侦察监视方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
吴宝举等: "小型自治水下机器人运动控制系统研究", 《机械设计与制造》 *
赵宁宁等: "基于Serret-Frenet坐标系的多AUV编队路径跟踪控制", 《鱼雷技术》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115562312A (zh) * 2022-11-17 2023-01-03 三亚哈尔滨工程大学南海创新发展基地 一种强跟踪全格式无模型自适应船舶艏向控制方法
CN115562312B (zh) * 2022-11-17 2023-10-13 哈尔滨工程大学三亚南海创新发展基地 一种强跟踪全格式无模型自适应船舶艏向控制方法

Also Published As

Publication number Publication date
CN114879703B (zh) 2022-09-09

Similar Documents

Publication Publication Date Title
CN107957727B (zh) 水下机器人控制系统及动力定位方法
Bibuli et al. Path‐following algorithms and experiments for an unmanned surface vehicle
Dukan et al. Dynamic positioning system for a small size ROV with experimental results
CN107953350A (zh) 一种用于检测与作业的水下机器人控制系统
CN110794855A (zh) 一种水下机器人综合控制系统及其方法
Hegrenaes et al. Comparison of mathematical models for the HUGIN 4500 AUV based on experimental data
CN111026135B (zh) 一种无人艇高性能航行前馈控制系统及其控制方法
Tran et al. Design, control, and implementation of a new AUV platform with a mass shifter mechanism
Kurowski et al. Multi-vehicle guidance, navigation and control towards autonomous ship maneuvering in confined waters
CN114879703B (zh) 水下机器人路径跟踪控制方法
CN111452939A (zh) 一种用于引水隧洞检测的自主巡线水下直升机
JP4706032B2 (ja) 自動船位保持制御方法及び自動船位保持制御装置
CN116679709A (zh) 一种基于改进人工势场的无人艇编队避障控制方法和系统
CN106767796B (zh) 类渡槽环境中无人船测距单元与惯性测量单元的融合算法
Horner et al. AUV experiments in obstacle avoidance
WO2021191729A1 (en) Hydrofoil with autopilot configuration
CN108469731B (zh) 一种波浪滑翔器故障监测与鲁棒控制方法
Hassanein et al. Development of low cost autonomous underwater vehicle platform
Serna et al. Bilateral teleoperation of a commercial small-sized underwater vehicle for academic purposes
Fernandes et al. Path generation for high-performance motion of ROVs based on a reference model
JP3999976B2 (ja) 操船方法及び装置
Gavrilina et al. Attitude Control System of a Highly Maneuverable Hybrid ROV for Ship-Hull Inspection
Rosario et al. Experimental variable structure trajectory tracking control of a surface vessel with a motion capture system
Zhu Design of control system of USV based on double propellers
Bergantin et al. Minimalistic in-flight odometry based on two optic flow sensors along a bouncing trajectory

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant