CN109933077A - 一种水下机器人自动靠泊的路径规划和控制方法 - Google Patents

Abstract

本发明涉及一种水下机器人自动靠泊的路径规划和控制方法，实现水下机器人自主规划出由当前位置航行至回收位置的路径。本发明包括人机交互、路径规划和跟踪控制；所述人机交互指母船操控人员向UUV发送母船的位置和方向或多种遥控命令，同时接收UUV的位置和方向，以实时监控UUV轨迹；所述路径规划是UUV根据母船和自身的位置和方向，自主规划出从当前位置至目标停泊点的参考路径；所述跟踪控制为根据参考路径实时控制UUV的航向和航速使得实际航迹收敛于参考路径。本方法只需操控人员监控和应急操作，不但能减轻操控人员的工作负担，且能提高UUV的控制能力，加快UUV的回收时间。本方法适用于水下机器人、水面无人艇等各种无人系统。

Description

一种水下机器人自动靠泊的路径规划和控制方法

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种水下机器人(UUV)自动靠泊的路径规划和控制方法，实现UUV自主规划水面靠泊路径和路径跟踪控制。

背景技术

随着水下机器人的日益发展和外场应用频繁，水下机器人回收过程自动化和降低人员下小艇回收的劳动强度及危险性的需求越来越高。以往UUV水面遥控回航方法主要采用操控人员手操控制，这种方法优势是简单直接，但对操控人员的经验和专业程度要求高，且人员操控往往凭借主观判断进行遥操作，大多缺少全局规划和优化控制层面的精力和技术。运用人机交互、路径规划和跟踪控制等技术，水下机器人自主规划出合理有效的回航靠泊路径，并能实时跟踪参考路径直至航行至目标停泊点，这将大大减轻操控人员的劳动强度和减少主观判断偏差，使操控人员只需进行判断、决策和监督等智能任务，更能保证水下机器人回收的高效性和安全性。本发明采用经典控制理论与变结构控制相结合的方法，也适用于其他能在水面进行人机交互和遥操控的无人系统。

发明内容

为了克服水下机器人回收过程需要人员下小艇回收的劳动强度及危险性或人员操控往往凭借主观判断进行遥操作、缺少全局规划和优化控制层面的精力和技术等问题，本发明要解决的技术问题是提供一种水下机器人自动靠泊的路径规划和控制方法，减轻操控人员的劳动强度和减少主观判断偏差，使操控人员只需进行判断、决策和监督等任务，增加水下机器人回收的高效性和安全性。

本发明为实现上述目的所采用的技术方案是：一种水下机器人自动靠泊的路径规划和控制方法，用于实现UUV自主规划水面靠泊路径和路径跟踪控制，包括人机交互、路径规划和跟踪控制；

所述人机交互：UUV上浮水面等待回收过程中，母船上操控人员通过水面遥控装置向UUV发送母船的位置和方向信息，同时母船接收UUV的位置和方向信息，以实时监控UUV的实际回收轨迹；

所述路径规划：UUV根据母船和自身的位置和方向信息，自主规划出从当前位置航行至目标停泊点的目标路径，并将路径规划结果发送至母船后，自主进入跟踪控制阶段；

所述跟踪控制：根据目标路径实时控制UUV的航向使得实际航迹收敛于目标路径，最终实现UUV按照目标路径达到目标停泊点。

所述人机交互包括如下：

母船上操控人员通过水面遥控装置向UUV发送母船的位置和方向信息，同时母船接收UUV周期性发来的位置和方向信息，以实时监控UUV的实际航行轨迹。

所述路径规划方法如下：

水下机器人到母船的自主回航路径规划分为两段式停泊轨迹，分别是直航段和圆弧段

水下机器人到母船的距离，即P₀至E₀的距离为：

水下机器人距离目标停泊点所在最小回转圆的圆心的距离，即P₀至O₁的距离为：

其中，L₁表示水下机器人目标停泊点至母船的距离；L₂表示水下机器人当前位置距离目标停泊点的纵向距离；L₃表示水下机器人当前位置距离至母船的横向距离；R表示水下机器人的最小回转半径；

其中，α是圆弧对应的圆心角；

再根据P₁得到轨迹。

所述路径跟踪控制方法如下：

路径规划的轨迹发送给目标解算，目标解算根据水下机器人的目标位置和当前位置得出目标航速和目标航向角，然后将目标航速和目标航向角分别发送给转向控制环和速度控制环，航向角和速度都采用闭环控制，经过推力解算后将各个执行机构的目标值发送给相应的执行机构，作用于水下机器人后，通过传感器反馈回位置、速度和航向角；具体如下：

其中，T_p为目标解算中位置环的控制输出，T_v和T_h分别是速度环和航向环的控制输出；K_{*_p}，K_{*_i}和K_{*_d}分别是位置环、速度环和航向环的比例、积分、微分项系数，_*为p、v或h；p_r、v_r和h_r分别是目标位置、目标航速和目标航向；p、v和h分别是实际位置、实际航速和实际航向。

本发明具有以下有益效果及优点：

1.增强了遥控回收的控制精度和时效性。相比于以往的人工遥控回收，本发明可由水下机器人自主进行路径规划和跟踪控制，操控人员只需进行判断、决策和监督等智能任务，大大减轻操控人员的劳动强度和减少主观判断偏差；本方法具有更好的实用性和自主性，增加了水下机器人回收的时效性和安全性。

2.应用范围广泛。本发明不但适用于水下机器人、水面无人艇等，还可用于各种可在水面航行和接受远程遥控的无人系统。

附图说明

图1是本发明的路径规划示意图。

图2是本发明的跟踪控制示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明的路径规划如图1所示，跟踪控制如图2所示。

本发明的UUV自动靠泊方法包括以下几个内容：

第一步，人机交互。母船上操控人员通过水面遥控装置向UUV发送母船的位置和方向信息，也能发送给UUV多种遥控命令；同时母船接收UUV周期性发来的位置和方向信息，以实时监控UUV的实际航行轨迹。

第二步，路径规划。假设一般水下机器人的最小回转半径为几十米级，水下机器人距离母船的距离至少是百米级，所以水下机器人距离母船的距离大于2R。水下机器人到母船的自主回航路径规划分为两段式停泊轨迹，如图1所示。

水下机器人到母船的距离，即P₀至E₀的距离为：

水下机器人距离目标停泊点所在最小回转圆的圆心的距离，即P₀至O₁的距离为：

其中，L₁表示水下机器人目标停泊点至母船的距离；L₂表示水下机器人当前位置距离目标停泊点的纵向距离；L₃表示水下机器人当前位置距离至母船的横向距离；R表示水下机器人的最小回转半径。

其中，α是圆弧对应的圆心角。

第三步，跟踪控制。如图2所示，路径规划的轨迹发送给目标解算，目标解算根据水下机器人的目标位置和当前位置计算出目标航速和目标航向角，然后将目标航速和目标航向角发送给转向控制环和速度控制环，航向角和速度都采用闭环控制，经过推力解算后将各个执行机构的目标值发送给相应的执行机构，作用于水下机器人后，通过各类传感器反馈回位置、速度和航向角。具体算法如下：

其中，T_p为目标解算中位置环的控制输出，T_v和T_h分别是速度环和航向环的控制输出；K_{*_p}，K_{*_i}和K_{*_d}分别是位置环、速度环和航向环的比例、积分、微分项系数，_*为p、v或h；p_r、v_r和h_r分别是目标位置、目标航速和目标航向；p、v和h分别是实际位置、实际航速和实际航向。

Claims (4)

1.一种水下机器人自动靠泊的路径规划和控制方法，其特征在于，用于实现UUV自主规划水面靠泊路径和路径跟踪控制，包括人机交互、路径规划和跟踪控制；

所述人机交互：UUV上浮水面等待回收过程中，母船上操控人员通过水面遥控装置向UUV发送母船的位置和方向信息，同时母船接收UUV的位置和方向信息，以实时监控UUV的实际回收轨迹；

所述路径规划：UUV根据母船和自身的位置和方向信息，自主规划出从当前位置航行至目标停泊点的目标路径，并将路径规划结果发送至母船后，自主进入跟踪控制阶段；

所述跟踪控制：根据目标路径实时控制UUV的航向使得实际航迹收敛于目标路径，最终实现UUV按照目标路径达到目标停泊点。

2.根据权利要求1所述的一种水下机器人自动靠泊的路径规划和控制方法，其特征在于，所述人机交互包括如下：

母船上操控人员通过水面遥控装置向UUV发送母船的位置和方向信息，同时母船接收UUV周期性发来的位置和方向信息，以实时监控UUV的实际航行轨迹。

3.根据权利要求1所述的一种水下机器人自动靠泊的路径规划和跟踪控制方法，其特征在于，所述路径规划方法如下：

水下机器人到母船的自主回航路径规划分为两段式停泊轨迹，分别是直航段和圆弧段

水下机器人到母船的距离，即P₀至E₀的距离为：

水下机器人距离目标停泊点所在最小回转圆的圆心的距离，即P₀至O₁的距离为：

其中，L₁表示水下机器人目标停泊点至母船的距离；L₂表示水下机器人当前位置距离目标停泊点的纵向距离；L₃表示水下机器人当前位置距离至母船的横向距离；R表示水下机器人的最小回转半径；

其中，α是圆弧对应的圆心角；

再根据P₁得到轨迹。

4.根据权利要求1所述的一种水下机器人自动靠泊的路径规划和控制方法，其特征在于，所述路径跟踪控制方法如下：

路径规划的轨迹发送给目标解算，目标解算根据水下机器人的目标位置和当前位置得出目标航速和目标航向角，然后将目标航速和目标航向角分别发送给转向控制环和速度控制环，航向角和速度都采用闭环控制，经过推力解算后将各个执行机构的目标值发送给相应的执行机构，作用于水下机器人后，通过传感器反馈回位置、速度和航向角；具体如下：

其中，T_p为目标解算中位置环的控制输出，T_v和T_h分别是速度环和航向环的控制输出；K_{*_p}，K_{*_i}和K_{*_d}分别是位置环、速度环和航向环的比例、积分、微分项系数，*为p、v或h；p_r、v_r和h_r分别是目标位置、目标航速和目标航向；p、v和h分别是实际位置、实际航速和实际航向。