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

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

【技术实现步骤摘要】
水下航行器及其路径跟踪控制方法和装置


[0001]本专利技术涉及水下机器人控制
，尤其涉及一种水下航行器及其路径跟踪控制方法和装置。

技术介绍

[0002]水下物体检测是人们探索水下领域、维护水下基础建设、开展水下搜寻救援等活动必不可少的环节。以水下桥墩为例，其长期处于复杂的水文地质中，河水及其裹挟泥沙对水下桥墩的流水冲刷、侵蚀，使得桥墩基础容易产生各种缺陷，这些缺陷会对桥梁的耐久性和承载能力形成较大的危害，严重时甚至可能危及使用安全。不定时地对水下桥墩进行检测即可避免危害的发生。
[0003]目前，水下航行器常被用于进行水下物体自主检测作业。水下航行器三维路径跟踪控制对于水下航行器能否精准完成检测任务有着重要意义。然而水下环境复杂多变，对水下航行器航线瞬时影响及累积影响较大，现有技术未考虑水下航行器在水下作业受到的时变未知海流干扰对路径跟踪影响，以致水下航行器路径跟踪控制的精度低，易发生航线偏离甚至撞毁风险，难以满足水下物体自主检测任务要求。因此，亟需创新性的设计一种高精度水下航行器路径跟踪控制技术。

技术实现思路

[0004]本专利技术提供水下航行器及其路径跟踪控制方法和装置，用以解决现有技术中水下航行器路径跟踪控制精度低，难以满足水下物体自主检测任务要求的缺陷，提升水下航行器的跟踪性能。
[0005]第一方面，本专利技术提供一种水下航行器的路径跟踪控制方法，所述方法包括：
[0006]获取用于监测待测物体的期望路径；
[0007]基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；
[0008]预估运动学不确定项；
[0009]基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；
[0010]其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。
[0011]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述期望路径是以所述待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；
[0012]其中，所述μ是与虚拟导向控制律和时间有关的参量，所述R为所述期望路径的半径。
[0013]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述期望路径上的任意点p
t
的表达式如下所示：
[0014][0015]所述μ的表达式如下所示：
[0016][0017]其中，p
t
为所述水下航行器在航行t时间时的虚拟目标点，x
H
、y
H
、z
H
分别为p
H
的横坐标、纵坐标和竖坐标，t为时间，为虚拟导向控制律。
[0018]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述运行参数包括但不限于：姿态角和重心位置坐标；
[0019]所述基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差，包括：
[0020]在所述期望路径上定位所述水下航行器的虚拟目标点；
[0021]确定所述虚拟目标点的姿态角以及位置坐标；
[0022]根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下位置跟踪误差；
[0023]根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下姿态跟踪误差；
[0024]将期望路径坐标系下所述位置跟踪误差和所述姿态跟踪误差整体作为路径跟踪误差。
[0025]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述预估所述水下航行器的运动学不确定项，包括：
[0026]利用干扰观测器预估所述水下航行器的运动学不确定项；
[0027]其中，所述干扰观测器的表达式如下所示：
[0028][0029][0030]上式中，为D
χ
的估计值，为D
υ
的估计值，ω1为所述干扰观测器的第一带宽，ω2为所述干扰观测器的第二带宽，ξ1为所述干扰观测器的第一辅助变量，ξ2为所述干扰观测器的第二辅助变量，χ
e
和υ
e
分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ
Q
和υ
Q
分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯仰角和偏航
角，D
χ
为与所述水下航行器的航迹角对应的运动学不确定项，D
υ
为与所述水下航行器的潜伏角对应的运动学不确定项，
·
为求导符号。
[0031]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制，包括：
[0032]确定载体坐标系下所述水下航行器的合速度；
[0033]将所述路径跟踪误差、所述运动学不确定项和所述合速度代入运动学控制器中，得到输出值；
[0034]根据所述输出值对所述水下航行器进行路径跟踪控制；
[0035]其中，所述运动学控制器，是以补偿所述运动学不确定项为目的设计的。
[0036]根据本专利技术提供的水下航行器的路径跟踪控制方法，所述姿态角包括航迹角和潜伏角；所述运动学不确定项包括：与所述水下航行器的航迹角对应的运动学不确定项D
χ
，以及与所述水下航行器的潜伏角对应的运动学不确定项D
υ
；
[0037]所述运动学控制器的表达式如下所示：
[0038][0039]其中，q
d
为载体坐标系下所述水下航行器绕纵轴的俯仰角速度的控制量，r
d
为载体坐标系下所述水下航行器绕竖轴的偏航角速度的控制量，为虚拟导向控制律，x
e
、y
e
、z
e
、χ
e
，υ
e
分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的横向跟踪误差、纵向跟踪误差、竖向跟踪误差、航迹角跟踪误差和潜伏角跟踪误差，为D
χ
的估计值，为D
υ
的估计值，U
B
为载体坐标系下所述水下航行器的合速度，θ所述水下航行器的俯仰角，k1为所述运动学控制器的第一控制增益，k2为所述运动学控制器的第二控制增益，k3为所述运动学控制器的第三控制增益，k4为所述运动学控制器的第四控制增益。
[0040]第二方面，本专利技术提供一种水下航行器的路径跟踪控制装置，所述装置包括：
[0041]获取模块，用于获取用于监测待测物体的期望路径；
[0042]路径跟踪误差计算模块，用于基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；
[0043]预估模块，用于预估运动学不确定项；
[0044]路径跟踪控制模块，用于基于所述路径跟踪误差和所本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下航行器的路径跟踪控制方法，其特征在于，所述方法包括：获取用于监测待测物体的期望路径；基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差；预估运动学不确定项；基于所述路径跟踪误差和所述运动学不确定项，对所述水下航行器进行路径跟踪控制；其中，所述运动学不确定项是所述水下航行器运行状态下由非零时变横滚角、攻角和侧滑角所引起的非线性干扰。2.根据权利要求1所述的水下航行器的路径跟踪控制方法，其特征在于，所述期望路径是以所述待测物体的轴线为铅垂线、以1/R为曲率、以L为螺距且以μ为曲线参数的螺旋线；其中，所述μ是与虚拟导向控制律和时间有关的参量，所述R为所述期望路径的半径。3.根据权利要求2所述的水下航行器的路径跟踪控制方法，其特征在于，所述期望路径上的任意点p
t
的表达式如下所示：所述μ的表达式如下所示：其中，p
t
为所述水下航行器在航行t时间时的虚拟目标点，x
H
、y
H
、z
H
分别为p
H
的横坐标、纵坐标和竖坐标，t为时间，为虚拟导向控制律。4.根据权利要求1所述的水下航行器的路径跟踪控制方法，其特征在于，所述运行参数包括但不限于：姿态角和重心位置坐标；所述基于所述期望路径和所述水下航行器的运行参数，确定路径跟踪误差，包括：在所述期望路径上定位所述水下航行器的虚拟目标点；确定所述虚拟目标点的姿态角以及位置坐标；根据所述水下航行器的重心位置坐标和所述虚拟目标点的位置坐标，确定期望路径坐标系下的位置跟踪误差；根据所述水下航行器的姿态角和所述虚拟目标点的姿态角，确定期望路径坐标系下的姿态跟踪误差；将期望路径坐标系下的所述位置跟踪误差和所述姿态跟踪误差整体作为所述路径跟踪误差。5.根据权利要求1所述的水下航行器的路径跟踪控制方法，其特征在于，所述预估运动学不确定项，包括：
利用干扰观测器预估所述运动学不确定项；其中，所述干扰观测器的表达式如下所示：其中，所述干扰观测器的表达式如下所示：上式中，上式中，为D
χ
的估计值，为D
υ
的估计值，ω1为所述干扰观测器的第一带宽，ω2为所述干扰观测器的第二带宽，ξ1为所述干扰观测器的第一辅助变量，ξ2为所述干扰观测器的第二辅助变量，χ
e
和υ
e
分别为期望路径坐标系下所述水下航行器跟踪其虚拟目标点时的航迹角跟踪误差和潜伏角跟踪误差，χ
Q
和υ
Q
分别为所述水下航行器的虚拟目标点的航迹角和潜伏角，θ和ψ分别为所述水下航行器的俯...

【专利技术属性】
技术研发人员：赵宏亮，张木，张育玮，薛任宇欣，孔凡忠，王锐，高宏宇，黄磊，刘鹏，柯南极，
申请(专利权)人：新兴际华北京智能装备技术研究院有限公司，
类型：发明
国别省市：