CN116300982B - 水下航行器及其路径跟踪控制方法和装置 - Google Patents

Abstract

本发明提供一种水下航行器及其路径跟踪控制方法和装置，获取用于监测待测物体的期望路径以及水下航行器的运行参数；基于期望路径和运行参数，确定水下航行器的路径跟踪误差；将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰作为水下航行器的运动学不确定项，并预估该运动学不确定项；基于路径跟踪误差和运动学不确定项，对水下航行器进行路径跟踪控制。本发明在路径跟踪控制的过程中，对运动学不确定项进行了补偿，从而提高了水下航行器路径跟踪控制精度。

Description

水下航行器及其路径跟踪控制方法和装置

技术领域

本发明涉及水下机器人控制技术领域，尤其涉及一种水下航行器及其路径跟踪控制方法和装置。

背景技术

水下物体检测是人们探索水下领域、维护水下基础建设、开展水下搜寻救援等活动必不可少的环节。以水下桥墩为例，其长期处于复杂的水文地质中，河水及其裹挟泥沙对水下桥墩的流水冲刷、侵蚀，使得桥墩基础容易产生各种缺陷，这些缺陷会对桥梁的耐久性和承载能力形成较大的危害，严重时甚至可能危及使用安全。不定时地对水下桥墩进行检测即可避免危害的发生。

目前，水下航行器常被用于进行水下物体自主检测作业。水下航行器三维路径跟踪控制对于水下航行器能否精准完成检测任务有着重要意义。然而水下环境复杂多变，对水下航行器航线瞬时影响及累积影响较大，现有技术未考虑水下航行器在水下作业受到的时变未知海流干扰对路径跟踪影响，以致水下航行器路径跟踪控制的精度低，易发生航线偏离甚至撞毁风险，难以满足水下物体自主检测任务要求。因此，亟需创新性的设计一种高精度水下航行器路径跟踪控制技术。

发明内容

本发明提供水下航行器及其路径跟踪控制方法和装置，用以解决现有技术中水下航行器路径跟踪控制精度低，难以满足水下物体自主检测任务要求的缺陷，提升水下航行器的跟踪性能。

第一方面，本发明提供一种水下航行器的路径跟踪控制方法，所述方法包括：

获取用于监测待测物体的期望路径；

基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；

预估运动学不确定项；

基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

根据本发明提供的水下航行器的路径跟踪控制方法，所述期望路径是以所述待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；

其中，所述μ是与虚拟导向控制律和时间有关的参量，所述R为所述期望路径的半径。

根据本发明提供的水下航行器的路径跟踪控制方法，所述期望路径上的任意点p_t的表达式如下所示：

所述μ的表达式如下所示：

其中，p_t为所述水下航行器在航行t时间时的虚拟目标点，x_H、y_H、z_H分别为p_H的横坐标、纵坐标和竖坐标，t为时间，/>为虚拟导向控制律。

根据本发明提供的水下航行器的路径跟踪控制方法，所述运行参数包括但不限于：姿态角和重心位置坐标；

所述基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差，包括：

在所述期望路径上定位所述水下航行器的虚拟目标点；

确定所述虚拟目标点的姿态角以及位置坐标；

根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下位置跟踪误差；

根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下姿态跟踪误差；

将期望路径坐标系下所述位置跟踪误差和所述姿态跟踪误差整体作为路径跟踪误差。

根据本发明提供的水下航行器的路径跟踪控制方法，所述预估所述水下航行器的运动学不确定项，包括：

利用干扰观测器预估所述水下航行器的运动学不确定项；

其中，所述干扰观测器的表达式如下所示：

上式中，为D_χ的估计值，/>为D_υ的估计值，ω₁为所述干扰观测器的第一带宽，ω₂为所述干扰观测器的第二带宽，ξ₁为所述干扰观测器的第一辅助变量，ξ₂为所述干扰观测器的第二辅助变量，χ_e和υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ_Q和υ_Q分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯仰角和偏航角，D_χ为与所述水下航行器的航迹角对应的运动学不确定项，D_υ为与所述水下航行器的潜伏角对应的运动学不确定项，·为求导符号。

根据本发明提供的水下航行器的路径跟踪控制方法，所述基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制，包括：

确定载体坐标系下所述水下航行器的合速度；

将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；

根据所述输出值对所述水下航行器进行路径跟踪控制；

其中，所述运动学控制器，是以补偿所述运动学不确定项为目的设计的。

根据本发明提供的水下航行器的路径跟踪控制方法，所述姿态角包括航迹角和潜伏角；所述运动学不确定项包括：与所述水下航行器的航迹角对应的运动学不确定项D_χ，以及与所述水下航行器的潜伏角对应的运动学不确定项D_υ；

所述运动学控制器的表达式如下所示：

其中，q_d为载体坐标系下所述水下航行器绕纵轴的俯仰角速度的控制量，r_d为载体坐标系下所述水下航行器绕竖轴的偏航角速度的控制量，为虚拟导向控制律，x_e、y_e、z_e、χ_e，υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的横向跟踪误差、纵向跟踪误差、竖向跟踪误差、航迹角跟踪误差和潜伏角跟踪误差，/>为D_χ的估计值，/>为D_υ的估计值，U_B为载体坐标系下所述水下航行器的合速度，θ所述水下航行器的俯仰角，k₁为所述运动学控制器的第一控制增益，k₂为所述运动学控制器的第二控制增益，k₃为所述运动学控制器的第三控制增益，k₄为所述运动学控制器的第四控制增益。

第二方面，本发明提供一种水下航行器的路径跟踪控制装置，所述装置包括：

获取模块，用于获取用于监测待测物体的期望路径；

路径跟踪误差计算模块，用于基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；

预估模块，用于预估运动学不确定项；

路径跟踪控制模块，用于基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

第三方面，本发明提供一种水下航行器，包括如第二方面所述的水下航行器的路径控制装置。

第四方面，本发明提供一种非暂态计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如第一方面所述水下航行器的路径跟踪控制方法。

本发明提供一种水下航行器及其路径跟踪控制方法和装置，将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰视为水下航行器的运动学不确定项，获取用于监测待测物体的期望路径和水下航行器的运行参数；基于期望路径和运行参数，确定水下航行器的路径跟踪误差；预估水下航行器的运动学不确定项；基于路径跟踪误差和运动学不确定项，对水下航行器进行路径跟踪控制。本发明在对水下航行器进行路径跟踪控制时，对水下航行器的运动学不确定项进行了补偿，从而提高了水下航行器路径跟踪控制精度。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的水下航行器的路径跟踪控制方法的流程示意图；

图2是本发明提供的大地坐标系、载体坐标系、合速度坐标系、期望路径坐标系和期望路径的示意图；

图3是本发明提供的水下航行器三维空间路径跟踪控制框图；

图4是本发明提供的期望路径、运动学控制器下的水下航行器三维空间路径跟踪曲线以及传统视线制导控制器下的水下航行器路径跟踪曲线的对比图；

图5是本发明提供的期望路径、运动学控制器下的水下航行器三维空间路径跟踪曲线以及传统视线制导控制器下的水下航行器路径跟踪曲线在水平面和垂直面的投影对比图；

图6(a)是本发明提供的运动学控制器和传统视线制导控制器的水下航行器横向跟踪误差曲线分量的对比图；

图6(b)是本发明提供的运动学控制器和传统视线制导控制器的水下航行器纵向跟踪误差曲线对比图；

图6(c)是本发明提供的运动学控制器和传统视线制导控制器的水下航行器竖向跟踪误差曲线的对比图；

图7是本发明提供的水下航行器的路径跟踪控制装置的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合图1-图7描述本发明提供的水下航行器的路径跟踪控制方法和装置。

第一方面，本发明提供一种水下航行器的路径跟踪控制方法，用于控制水下航行器进行自主作业，如图1所示，所述方法包括：

S11、获取用于监测待测物体的期望路径；

期望路径是工程师根据现场工况以及工程经验设计的；

水下航行器，主要指具有自主航行控制能力的水下航行器，例如自主式水下航行器和具有自控能力的无人水下潜航器；还可以是能够进行人工控制作业和独立作业灵活切换的半自主水下航行器。

S12、基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；

所述运行参数包括但不限于：姿态角和重心位置坐标；

通常姿态角包括横滚角、俯仰角和偏航角，但是由于水下航行器的横滚角通常是非常小的，因此可以忽略横滚角。

S13、预估运动学不确定项；

S14、基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

本发明提供一种水下航行器的路径跟踪控制方法，将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰视为水下航行器的运动学不确定项，获取用于监测待测物体的期望路径和水下航行器的运行参数；基于期望路径和运行参数，确定水下航行器的路径跟踪误差；预估水下航行器的运动学不确定项；基于路径跟踪误差和运动学不确定项，对水下航行器进行路径跟踪控制。本发明在对水下航行器进行路径跟踪控制时，及时对运动学不确定项进行了补偿，使水下航行器的路径跟踪控制精度得到了显着的提升，降低水下环境对水下航行器航线的影响，节省运行时间及成本，提高运行效率、成功率和安全性。

具体的，所述S11之前，还包括：获取预先构建的运动学控制器；该动力学控制器按照如下过程构建：

(一)构建坐标系

水下航行器路径跟踪是指控制水下航行器跟踪一条满足航向要求和性能约束的期望路径，期望路径是在大地坐标系{I}上针对待测物体检测任务设计的目标路径。为此本发明先定义大地坐标系{I}、载体坐标系{B}、合速度坐标系{W}及期望路径坐标系{F}；

其中，大地坐标系{I}，原点是待测物体轴线和水平面相切的点；横轴x_I指向正北方向，纵轴y_I指向正东方向、竖轴z_I通过右手螺旋定则确定；

所述载体坐标系{B}，原点是载体重心，横轴x_B指向载体艏向方向，纵轴y_B指向载体右舷方向，竖轴z_B通过右手螺旋定则确定；这里所说的载体指水下航行器；

合速度坐标系{W}，原点是载体重心，横轴x_W指向载体航迹的切线方向，纵轴y_W与大地坐标系的x_Ioy_I平面平行，竖轴z_W通过右手螺旋定则确定；这里所说的载体指水下航行器；

期望路径坐标系{F}，原点是期望路径上任意一点，横轴方向是期望路径的切线方向，纵轴方向是与大地坐标系的x_Ioy_I平面平行的方向，竖轴方向通过右手螺旋定则确定。

为方面全文描述，未特别强调参量的坐标系时，该参量为大地坐标系{I}下的参量。例如：获取水下航行器重心的位置，即为获取水下航行器重心在大地坐标系下的位置。

图2示例出了大地坐标系、载体坐标系、合速度坐标系、期望路径坐标系以及期望路径。对于合速度坐标系，因只关注载体航迹的切线方向(即载体合速度方向)，故而仅画出横轴x_W。

(二)：在上述坐标系下对多不确定性下的水下航行器三维空间路径跟踪控制进行建模，构建运动学控制器，过程如下：

首先，定义水下航行器运动学模型。

根据变量的定义和空间几何旋转关系，可得如下六自由度的欠驱动水下航行器运动学模型：

上式中，(x,y,z)为大地坐标系{I}下水下航行器重心的位置坐标，记为p；(φ,θ,ψ)为大地坐标系{I}下水下航行器的姿态角，(u,v,w)为载体坐标系{B}下水下航行器的速度，(p,q,r)为载体坐标系{B}下水下航行器的角速度，φ、θ和ψ分别为大地坐标系{I}下水下航行器的横滚角、俯仰角和偏航角，u、v和w分别是载体坐标系{B}下水下航行器沿x_B、y_B、z_B方向的速度.p为载体坐标系{B}下水下航行器绕x_B的滚转角速度，q为载体坐标系{B}下水下航行器绕y_B的俯仰角速度，r为载体坐标系{B}下水下航行器绕z_B的偏航角速度，为速度旋转矩阵，/>为角速度旋转矩阵，文中字符加点表示求导操作。

其中，的表达式为：

的表达式为：

其次，将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰视为水下航行器的运动学不确定项，推导考虑运动学不确定项的路径跟踪误差动力学模型；这里，路径跟踪误差动力学模型包括：路径跟踪位置误差动力学模型和路径跟踪姿态误差动力学模型；

其中，路径跟踪位置误差动力学模型构建过程为：

根据期望路径，在期望路径坐标系{F}下水下航行器的路径跟踪位置误差的表达式为：

上式中，P_e为期望路径坐标系{F}下水下航行器跟踪其虚拟目标点时的位置跟踪误差，x_e、y_e和z_e分别为期望路径坐标系{F}下水下航行器跟踪其虚拟目标点时的横向跟踪误差、纵向跟踪误差和竖向跟踪误差，为大地坐标系{I}到期望路径坐标系{F}的转换矩阵，p_Q为大地坐标系{I}下虚拟目标点的位置坐标，用(x_Q，y_Q，z_Q)表示。虚拟目标点Q位于期望路径上，是期望水下航行器重心达到的位置。

其中，的表达式为：

上式中，υ_Q为大地坐标系下虚拟目标点的潜伏角，χ_Q为大地坐标系下虚拟目标点的航迹角，υ_Q和χ_Q由期望路径决定。

将上述P_e的表达式展开并求导，可得路径跟踪位置误差动力学模型，表达式如下所示

上式中，χ_e和υ_e分别为期望路径坐标系{F}下水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，为虚拟导向控制律，即期望路径坐标系{F}下所述虚拟目标点的速度，c₁(s)为期望路径的曲率，c₂(s)为期望路径的挠率，U_B为载体坐标系下水下航行器的合速度，等于/>υ_Q为大地坐标系下所述虚拟目标点的潜伏角，·为求导符号；

路径跟踪姿态误差动力学模型的构建过程为：

定义χ和υ分别为大地坐标系下水下航行器的航迹角和潜伏角，其表达式如下：

定义χ_Q和υ_Q分别为水下航行器的虚拟目标点的航迹角和潜伏角，其表达式如下：

考虑如下等价坐标旋转得到路径跟踪姿态误差为：

其中，

尽管通过对χ_e和υ_e求导可以得到路径跟踪姿态误差动力学模型，然而形式过于复杂。为此，设计如下的路径跟踪姿态误差动力学模型,表达式为：

上式中，D_χ为由水下航行器的非零时变横滚角、攻角和侧滑角所引起的航迹角跟踪误差不确定项，D_υ为由水下航行器的非零时变横滚角、攻角和侧滑角所引起的潜伏角跟踪误差不确定项。

接着，设计用于估计所述运动学不确定项的干扰观测器。

干扰观测器的定义如下：

上式中，ω₁为所述干扰观测器的第一带宽，ω₂为所述干扰观测器的第二带宽，ξ₁为所述干扰观测器的第一辅助变量，ξ₂为所述干扰观测器的第二辅助变量，为D_v的估计值，/>为D_v的估计值。

最后，进行运动学控制器设计。

该运动学控制器是以水下航行器的位置跟踪误差和姿态跟踪误差均趋于0为目标，结合所述路径跟踪误差动力学模型和所述干扰观测器的表达式设计得到的，定义如下：

其中，q_d为载体坐标系下水下航行器绕纵轴的俯仰角速度的控制量，r_d为载体坐标系下水下航行器绕竖轴的偏航角速度的控制量，k₁为运动学控制器的第一控制增益，k₂为运动学控制器的第二控制增益，k₃为运动学控制器的第三控制增益，k₄为运动学控制器的第四控制增益。

图3是水下航行器三维空间路径跟踪控制框图，如图3所示，位置跟踪位置误差[x_e，y_e，z_e]^T由水下航行器重心位置与虚拟目标点位置之间的差值经坐标转换后得到，姿态跟踪误差[χ_e，υ_e]^T由水下航行器位姿与虚拟目标点位姿之间的差值经坐标转换后得到，曲率c₁(s)和挠率c₂(s)由期望路径模型给出；不确定项估计值由和/>由干扰观测器测得。另外，路径跟踪误差动力学模型所需参数由期望路径与水下航行器运动学模型给出，干扰观测器基于路径跟踪误差动力学模型设计得到。

上述运动学控制器，是以代码形式封装了水下航行器控制算法的器件。

在已知运动学控制器表达式的下，即可在对水下航行器进行路径跟踪控制。

本发明通过设计运动学控制器，补偿水下航行器的运动学不确定项，提升水下航行器的抗干扰能力。该动力学控制器相较于现有控制器，鲁棒性更强。

具体的，所述S11中，期望路径是以待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；其中，μ是与虚拟导向控制律和时间有关的参量，R为所述期望路径的半径；

以待测物体为边长为7米的水下桥墩为例，针对水下桥墩设计的期望路径螺旋式地围绕水下桥墩，期望路径的半径为水下桥墩边长一半加1米安全工作距离，即3.5m+1m＝4.5m。因此水下航行器跟踪水下桥墩的期望路径，即可实现对待测物体的系统全面检测。期望路径的半径可以根据路径位置不同而不同，如圆锥螺旋线、边境圆弧螺旋线等。安全工作距离至少考虑水下航行器的机身尺寸、水下工作距离、待测物体周边水文情况等因素，期望路径根据待测物体的不同而不同，待测物体不局限于水下桥墩。

再进一步地，所述期望路径上的任意点p_t的表达式如下所示：

所述μ的表达式如下所示：

其中，p_t为所述水下航行器在航行t时间时的虚拟目标点，x_H、y_H、z_H分别为p_H的横坐标、纵坐标和竖坐标，t为时间，/>为虚拟导向控制律。

需要说明的是，期望路径本身是一条与时间无关曲线，本发明将虚拟导向控制律和时间引入了期望路径上的坐标点的表示中，在水下航行器和期望路径之间建立联系，为后续定位任意航行时间水下航行器的虚拟目标点在期望路径上的位置奠定基础。

具体的，所述S12包括：

S12.1：在所述期望路径上定位所述水下航行器的虚拟目标点；

运行中的水下航行器在期望路径上有唯一的虚拟目标点。如上述所言，期望路径上的坐标点表示中引入了时间，即可根据水下航行器当前航行时间，在期望路径上定位对应的虚拟点；

但是，本发明也适用期望路径上的坐标点表示中未引入时间的情况，例如通过标注方式实现水下航行器和其虚拟目标点的动态对应。

S12.2确定所述虚拟目标点的姿态角以及位置坐标；

这里，姿态角包括：俯仰角和偏航角。

S12.3根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下的位置跟踪误差；

可以理解的是，所述S12.3的公式表达式为：

S12.4根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下的姿态跟踪误差；

可以理解的是，所述S12.4通过计算姿态跟踪误差χ_e和υ_e；

S12.5将期望路径坐标系下所述位置跟踪误差所述和姿态跟踪误差整体作为路径跟踪误差。

最终，路径跟踪误差用[x_e，y_e，z_e，χ_e，υ_e]^T来表示。

本发明给出了应用时水下航行器路径跟踪误差的计算方式，为水下航行器路径跟踪控制提供了基础。

具体的，所述S13中，利用前述干扰观测器预估所述运动学不确定项。

本发明以预估水下航行器的运动学不确定项为目的构建了干扰观测器，为水下航行器的运动学不确定项的补偿奠定了基础。

具体的，所述S14包括：

确定载体坐标系下所述水下航行器的合速度；

将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；

根据所述输出值对所述水下航行器进行路径跟踪控制；

其中，所述运动学控制器，是以补偿所述运动学不确定项为目的(也即以水下航行器的位置跟踪误差和姿态跟踪误差均趋于0为目标)设计的。

水下环境复杂多变，对水下航行器航线瞬时影响及累积影响较大，航线偏离容易造成探测失败或撞毁风险，将运动学控制器用于水下航行器的路径跟踪控制中，及时对水下航行器的运动学不确定项的补偿，使水下航行器的路径跟踪控制精度得到了显着的提升，降低水下环境对水下航行器航线的影响，节省运行时间及成本，提高运行效率、成功率和安全性。

面向水下航行器在水下桥墩自主检测方面的应用，本发明的技术能够及时掌握水下桥墩的状况，及时发现“病害”并采取措施整治，从而显著降低桥梁养护成本与潜在风险。

为了验证运动学控制器的有效性，在MATLAB/Simulink环境下建立水下航行器三维路径跟踪控制系统的仿真模型，并对航行器进行桥墩监测情况下的路径跟踪控制性能进行验证。

仿真参数如表1所示：

表1

图4是本发明提供的期望路径、运动学控制器下的水下航行器三维空间路径跟踪曲线以及传统视线制导控制器下的水下航行器路径跟踪曲线的对比图。可以看出本发明所设计的运动学控制器和传统视线制导控制器都能使欠驱动水下航行器收敛至期望路径，但运动学控制器更平滑。

图5是本发明提供的期望路径、运动学控制器下的水下航行器三维空间路径跟踪曲线以及传统视线制导控制器下的水下航行器路径跟踪曲线在水平面和垂直面的投影对比图。可以看出，传统视线制导控制器的收敛速度要快于运动学控制器，这是由于本发明所设计的运动学控制器收敛速度受跟踪误差修正变量T的影响，初始状态跟踪误差大，收敛速度慢。

图6(a)～图6(c)分别是本发明提供的运动学控制器和传统视线制导控制器对应的水下航行器横向跟踪误差曲线对比图、水下航行器纵向跟踪误差曲线对比图和水下航行器竖向跟踪误差曲线对比图，水下环境复杂多变，对水下航行器航线瞬时影响及累积影响较大，航线偏离容易造成探测失败或撞毁风险，相比于传统视线制导控制器，本发明运动学控制器能及时对水下航行器的运动学不确定项的补偿，减小水下航行器的稳态跟踪误差，降低水下环境对水下航行器航线的影响，节省运行时间及成本，提高运行效率、成功率和安全性。

其中，本发明附图中为期望路径，-------为本发明运动学控制器，—·—·—·为传统视线制导控制器。

通过上述结果和分析，证明了本发明所设计的运动学控制器的有效性、可行性、鲁棒性、对模型不确定性和滚动运动效应的抗干扰能力，均比已有方法优越。

下面对本发明提供的面向桥墩检测的水下航行器精准跟踪控制装置进行描述，下文描述的面向桥墩检测的水下航行器精准跟踪控制装置与上文描述的面向桥墩检测的水下航行器精准跟踪控制方法可相互对应参照。图7为面向桥墩检测的水下航行器精准跟踪控制装置的结构示意图，如图7所示，所述装置包括：

获取模块21，用于获取用于监测待测物体的期望路径；

路径跟踪误差计算模块22，用于基于所述期望路径和所述水下航行器的运行参数，确定所路径跟踪误差；

预估模块23，用于预估运动学不确定项；

路径跟踪控制模块24，用于基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

本发明提供一种水下航行器的路径跟踪控制装置，将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰视为水下航行器的运动学不确定项，获取用于监测待测物体的期望路径和水下航行器的运行参数；基于期望路径和运行参数，确定水下航行器的路径跟踪误差；预估水下航行器的运动学不确定项；基于路径跟踪误差和运动学不确定项，对水下航行器进行路径跟踪控制。水下环境复杂多变，对水下航行器航线瞬时影响及累积影响较大，航线偏离容易造成探测失败或撞毁风险，本发明在对水下航行器进行路径跟踪控制时，及时对水下航行器的运动学不确定项进行了补偿，使水下航行器的路径跟踪控制精度得到了显着的提升，降低水下环境对水下航行器航线的影响，节省运行时间及成本，提高运行效率、成功率和安全性。

在上述各实施例的基础上，作为一种可选的实施例，所述期望路径是以所述待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；

其中，所述μ是与虚拟导向控制律和时间有关的参量。

在上述各实施例的基础上，作为一种可选的实施例，所述期望路径上的任意点p_t的表达式如下所示：

所述μ的表达式如下所示：

其中，p_t为所述水下航行器在航行t时间时的虚拟目标点，x_H、y_H、z_H分别为p_H的横坐标、纵坐标和竖坐标，t为时间，/>为虚拟导向控制律。

在上述各实施例的基础上，作为一种可选的实施例，所述运行参数包括但不限于：姿态角和重心位置坐标；所述姿态角包括：俯仰角和偏航角；

所述路径跟踪误差计算模块，包括：

定位单元，用于在所述期望路径上定位所述水下航行器的虚拟目标点；

第一确定单元，用于确定所述虚拟目标点的姿态角以及位置坐标；

位置跟踪误差确定单元，用于根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下的位置跟踪误差；

姿态跟踪误差确定单元，用于根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下的姿态跟踪误差；

设定单元，用于将期望路径坐标系下的所述位置跟踪误差和所述姿态跟踪误差整体作为所述路径跟踪误差。

在上述各实施例的基础上，作为一种可选的实施例，所述预估模块，用于利用干扰观测器预估所述水下航行器的运动学不确定项；

其中，所述干扰观测器的表达式如下所示：

上式中，为D_χ的估计值，/>为D_υ的估计值，ω₁为所述干扰观测器的第一带宽，ω₂为所述干扰观测器的第二带宽，ξ₁为所述干扰观测器的第一辅助变量，ξ₂为所述干扰观测器的第二辅助变量，χ_e和υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ_Q和υ_Q分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯仰角和偏航角，D_χ为与所述水下航行器的航迹角对应的运动学不确定项，D_υ为与所述水下航行器的潜伏角对应的运动学不确定项，·为求导符号。

在上述各实施例的基础上，作为一种可选的实施例，所述路径跟踪控制模块，包括：

第二确定单元，用于确定载体坐标系下所述水下航行器的合速度；

代入单元，用于将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；

控制单元，用于根据所述输出值对所述水下航行器进行路径跟踪控制；

其中，所述运动学控制器，是以补偿所述运动学不确定项为目的设计的。

在上述各实施例的基础上，作为一种可选的实施例，所述姿态角包括航迹角和潜伏角；所述运动学不确定项包括：与所述水下航行器的航迹角对应的运动学不确定项D_χ，以及与所述水下航行器的潜伏角对应的运动学不确定项D_υ；

所述运动学控制器的表达式如下所示：

其中，q_d为载体坐标系下所述水下航行器绕纵轴的俯仰角速度的控制量，r_d为载体坐标系下所述水下航行器绕竖轴的偏航角速度的控制量，为虚拟导向控制律，x_e、y_e、z_e、χ_e，υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的横向跟踪误差、纵向跟踪误差、竖向跟踪误差、航迹角跟踪误差和潜伏角跟踪误差，/>为D_χ的估计值，/>为D_υ的估计值，U_B为载体坐标系下所述水下航行器的合速度，θ所述水下航行器的俯仰角，k₁为所述运动学控制器的第一控制增益，k₂为所述运动学控制器的第二控制增益，k₃为所述运动学控制器的第三控制增益，k₄为所述运动学控制器的第四控制增益。

第三方面，本发明提供一种水下航行器，包括如第二方面所述的水下航行器的路径控制装置。

第四方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的水下航行器的路径跟踪控制方法，该方法包括：获取用于监测待测物体的期望路径；基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；预估运动学不确定项；基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

第五方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的水下航行器的路径跟踪控制方法，该方法包括：获取用于监测待测物体的期望路径；基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；预估运动学不确定项；基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种水下航行器的路径跟踪控制方法，其特征在于，所述方法包括：

获取用于监测待测物体的期望路径；

基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；

预估运动学不确定项；

基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰；

所述运行参数包括但不限于：姿态角和重心位置坐标；

所述预估运动学不确定项，包括：

利用干扰观测器预估所述运动学不确定项；

其中，所述干扰观测器的表达式如下所示：

上式中， 为D_χ的估计值，/>为D_υ的估计值，ω₁为所述干扰观测器的第一带宽，ω₂为所述干扰观测器的第二带宽，ξ₁为所述干扰观测器的第一辅助变量，ξ₂为所述干扰观测器的第二辅助变量，χ_e和υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ_Q和υ_Q分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯仰角和偏航角，D_χ为与所述水下航行器的航迹角对应的运动学不确定项，D_υ为与所述水下航行器的潜伏角对应的运动学不确定项，·为求导符号；

所述基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制，包括：

确定载体坐标系下所述水下航行器的合速度；

将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；

根据所述输出值对所述水下航行器进行路径跟踪控制；

其中，所述运动学控制器，是以补偿所述运动学不确定项为目的设计的。

2.根据权利要求1所述的水下航行器的路径跟踪控制方法，其特征在于，所述期望路径是以所述待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；

其中，所述μ是与虚拟导向控制律和时间有关的参量，所述R为所述期望路径的半径。

3.根据权利要求2所述的水下航行器的路径跟踪控制方法，其特征在于，所述期望路径上的任意点p_t的表达式如下所示：

所述μ的表达式如下所示：

其中，p_t为所述水下航行器在航行t时间时的虚拟目标点，x_t、y_t、z_t分别为p_t的横坐标、纵坐标和竖坐标，t为时间，/>为虚拟导向控制律。

4.根据权利要求1所述的水下航行器的路径跟踪控制方法，其特征在于，

所述基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差，包括：

在所述期望路径上定位所述水下航行器的虚拟目标点；

确定所述虚拟目标点的姿态角以及位置坐标；

根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下的位置跟踪误差；

根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下的姿态跟踪误差；

将期望路径坐标系下的所述位置跟踪误差和所述姿态跟踪误差整体作为所述路径跟踪误差。

5.根据权利要求3所述的水下航行器的路径跟踪控制方法，其特征在于，所述姿态角包括航迹角和潜伏角；所述运动学不确定项包括：与所述水下航行器的航迹角对应的运动学不确定项D_χ，以及与所述水下航行器的潜伏角对应的运动学不确定项D_υ；

所述运动学控制器的表达式如下所示：

其中，q_d为载体坐标系下所述水下航行器绕纵轴的俯仰角速度的控制量，r_d为载体坐标系下所述水下航行器绕竖轴的偏航角速度的控制量，为虚拟导向控制律，x_e、y_e、z_e、χ_e，υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的横向跟踪误差、纵向跟踪误差、竖向跟踪误差、航迹角跟踪误差和潜伏角跟踪误差，U_B为载体坐标系下所述水下航行器的合速度，/>k₁为所述运动学控制器的第一控制增益，k₂为所述运动学控制器的第二控制增益，k₃为所述运动学控制器的第三控制增益，k₄为所述运动学控制器的第四控制增益。

6.一种水下航行器的路径跟踪控制装置，其特征在于，所述装置包括：

获取模块，用于获取用于监测待测物体的期望路径；

路径跟踪误差计算模块，用于基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；

预估模块，用于预估运动学不确定项；

路径跟踪控制模块，用于基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；

其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰；

所述运行参数包括但不限于：姿态角和重心位置坐标；

所述预估运动学不确定项，包括：

利用干扰观测器预估所述运动学不确定项；

其中，所述干扰观测器的表达式如下所示：

上式中， 为D_χ的估计值，/>为D_υ的估计值，ω₁为所述干扰观测器的第一带宽，ω₂为所述干扰观测器的第二带宽，ξ₁为所述干扰观测器的第一辅助变量，ξ₂为所述干扰观测器的第二辅助变量，χ_e和υ_e分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ_Q和υ_Q分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯仰角和偏航角，D_χ为与所述水下航行器的航迹角对应的运动学不确定项，D_υ为与所述水下航行器的潜伏角对应的运动学不确定项，·为求导符号；

所述基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制，包括：

确定载体坐标系下所述水下航行器的合速度；

将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；

根据所述输出值对所述水下航行器进行路径跟踪控制；

其中，所述运动学控制器，是以补偿所述运动学不确定项为目的设计的。

7.一种水下航行器，其特征在于，包括如权利要求6所述的水下航行器的路径控制装置。

8.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述水下航行器的路径跟踪控制方法。