CN105549611A - 自主水下机器人的海洋环境自识别的航迹精确跟踪方法 - Google Patents

Abstract

本发明涉及水下机器人技术领域，尤其涉及自主机器人的航迹精确跟踪方法，实现自主水下机器人在变化海洋环境下的精确航迹跟踪控制。包括以下步骤：海洋环境参数识别：计算海流速度值在水下机器人垂直方向的投影；航迹跟踪：通过计算水下机器人与规划航迹的横向距离、水下机器人航向角与规划航迹角的偏差量，结合水下机器人的对底前向速度和水下机器人的转艏角速度计算水下机器人的水平面转艏力矩。与传统的控制方法比较，本方法具有更好的鲁棒性，更能适应外界环境的改变和AUV自身参数的改变，提高了AUV的控制能力。本方法移植方便，可以适用于各种水下机器人。

Description

自主水下机器人的海洋环境自识别的航迹精确跟踪方法

技术领域

本发明涉及水下机器人技术领域，尤其涉及一种自主水下机器人(简称AUV)的海洋环境自识别的航迹精确跟踪方法。

背景技术

在海洋应用中，水下机器人发挥越来越重要的作用。水下机器人分成两类：一种是遥控式有缆水下机器人(简称ROV)，一种是自主水下机器人(简称AUV)。ROV需要水面母船支持，同时受到电缆长度的限制，其作业距离有限，一般只有几百米；而AUV自身携带能源，可以远离母船，活动距离达到几十公里甚至上百公里。所以AUV的研究越来越受到各国的重视，AUV的发展代表了未来水下机器人的发展方向。

AUV的控制方法比ROV的控制方法复杂，只有设计出好的控制方法才能发挥出AUV强大的作业能力。AUV执行远距离地形勘查、管线跟踪都依赖于AUV的精确航迹跟踪控制，即AUV精确航迹跟踪控制技术的发展影响AUV的远程和深海作业能力。传统AUV精确航迹跟踪控制方法主要采用经典线性控制理论进行控制，这种方法最大的好处是算法简单，但是经典精确航迹跟踪控制方法依赖于AUV参数的稳定。而AUV的参数是强非线性耦合和时变的，理论计算和试航时的参数辨识是特定时间和外界环境条件下的参数。

AUV作业的海洋环境复杂，海浪和海流等干扰因素随着工作海域和深度的不同而发生变化，这些不确定干扰因素是空间和时间的复杂函数，无法被预知和准确建模。环境的改变使AUV参数发生改变，经典的控制方法无法适应外界环境的变化，导致AUV的精确航迹跟踪效果降低。

发明内容

针对传统精确航迹跟踪控制方法在对于系数敏感性较强和抗干扰性较弱的问题，本发明要解决的技术问题是提供一种自主水下机器人的海洋环境自识别的航迹精确跟踪方法，将传统精确跟踪控制方法和在线参数辨识相结合的控制方法，降低了控制系统对参数敏感性和提高系统的控制抗干扰性。

本发明为实现上述目的所采用的技术方案是：一种自主水下机器人的海洋环境自识别的航迹精确跟踪方法，包括以下步骤：

海洋环境参数识别：计算海流速度值在水下机器人垂直方向的投影；

航迹跟踪：通过计算水下机器人与规划航迹的横向距离ε(t)、水下机器人航向角与规划航迹角的偏差量结合水下机器人的对底前向速度u和水下机器人的转艏角速度r计算水下机器人的水平面转艏力矩τ_n。

所述计算海流速度值在水下机器人垂直方向的投影，具体为：

计算侧向来流偏角

$\left\{\begin{array}{c}\widehat{\mathrm{\β}}=\arctan \frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\η}}_i-N\stackrel{\‾}{\mathrm{\ξ\η}}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i-N\stackrel{\‾}{\mathrm{\ξ}}}\\ \stackrel{\‾}{\mathrm{\ξ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i\\ \stackrel{\‾}{\mathrm{\η}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_i\\ \stackrel{\‾}{\mathrm{\ψ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ψ}}_i\end{array}\right.---\left(1\right)$

其中，N是测量时刻总次数；i是第i次测量时刻；ξ_i和η_i是第i次测量时的北向位置和东向位置，是已知参数；和是N次测量的北向位置和东向位置的期望；ψ_i是第i次测量时的水下机器人航向角，是已知参数；是N次测量的水下机器人航向角的期望；和它们是方程(1)的待定系数，通过求解方程(1)，计算出和的值；

在计算出值后，海流速度值在水下机器人垂直方向的投影v_c为：

$v_c=u|\sin \widehat{\mathrm{\β}}|---\left(2\right)$

其中，u是水下机器人的对底前向速度，是已知参数。

所述水下机器人与规划航迹的横向距离ε(t)、水下机器人航向角与规划航迹角的偏差量通过下式计算：

$\left\{\begin{array}{c}\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}=\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}+\widehat{\mathrm{\β}}\\ {\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}=\arctan 2({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1},{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})\\ \mathrm{\ξ}\left(t\right)=\frac{[{\mathrm{\ξ}}_i-\mathrm{\ξ},{\mathrm{\η}}_i-\mathrm{\η}]\·[{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1},{\mathrm{\η}}_i-{\mathrm{\η}}_{i-1}]}{\sqrt{{({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})}^2+{({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1})}^2}}\sin (\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)})\end{array}\right.---\left(3\right)$

其中，ψ(t)是水下机器人当前的航向角；(ξ_i-1,η_i-1)是规划航迹起点的北向位置坐标和东向位置坐标；(ξ_i,η_i)是规划航迹终点的北向位置坐标和东向位置坐标；ψ_trk(i)是规划航迹角，是中间变量；ψ(t)是航向角；是侧向来流偏角；ξ是当前水下机器人的东向位置坐标；η是当前水下机器人的北向位置坐标。

所述水下机器人的水平面转艏力矩τ_n的计算方法为：

$\left\{\begin{array}{c}{\mathrm{\τ}}_n=\frac{1}{u\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)}\×[-{\mathrm{\λ}}_{2}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)-\mathrm{\η}\tanh (\mathrm{\σ}/\mathrm{\φ})\\ \mathrm{\σ}=\mathrm{ur}\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_{1}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_2\mathrm{\ϵ}\left(t\right)\end{array}\right.---\left(4\right)$

其中，是水下机器人航向角与规划航迹角的偏差量，ε(t)是水下机器人与规划航迹的横向距离，σ是中间变量，η、λ₁、λ₂和φ是水下机器人航迹控制参数，由实验测定的常量，1≤η≤3，1≤λ₁≤3，1≤λ₂≤3，0≤φ≤1。

所述水下机器人的对底前向速度u和水下机器人的转艏角速度r通过水下机器人搭载的传感器测量得到。

本发明采用滑模控制理论与在线参数辨识理论相结合的方法，对于水动力参数变化和海洋环境参数变化的不敏感性和控制抗干扰性优于传统的精确航迹跟踪控制方法，更加适合AUV这种在复杂海洋环境下的非线性时变模型系统。具体的说，本发明具有以下优点及有益效果：

1.对于水动力系数不敏感。相比传统的控制方法，本方法对于水动力系数不敏感，能够在水动力模型不准确的情况下使用。

2.能够更好地适应外界环境的改变。当外界环境发生改变时，例如外部海流环境发生变化时，传统的控制方法会偏离最佳工作状态，降低控制系统性能；本方法具有更好的鲁棒性，更好地适应外界环境的改变。

3.应用范围广。本发明不但可以应用于AUV，还可以用于各种水下机器人的精确航迹控制。

附图说明

图1为本发明应用环境示意图；

图2为本发明中相关参数关系图。

具体实施方式

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

本发明的硬件要求是一台AUV，艉部配置2个水平舵，在艉部水平舵上装有2个推进电机；在艉部配置一个垂直舵，如图1所示。在AUV设计完成后，首先进行水池水动力试验，获得AUV的水动力系数。然后按照本发明方法设计AUV的精确航迹跟踪控制器。最后，将使用本发明方法的AUV进行航行试验，验证控制方法的正确性。

本发明的AUV精确航迹跟踪控制方法包括海洋环境参数识别和精确航迹跟踪控制方法，以下详细描述这两方面内容：

本发明中的海洋环境参数识别是指计算海流速度值在AUV垂直方向的投影。为了方便计算侧向来流的数值，定义表示侧向来流偏角，它的计算方法如下：

$\left\{\begin{array}{c}\widehat{\mathrm{\β}}=\arctan \frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\η}}_i-N\stackrel{\‾}{\mathrm{\ξ}}\stackrel{\‾}{\mathrm{\η}}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i-N\stackrel{\‾}{\mathrm{\ξ}}}-\stackrel{\‾}{\mathrm{\ψ}}\\ \stackrel{\‾}{\mathrm{\ξ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i\\ \stackrel{\‾}{\mathrm{\η}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_i\\ \stackrel{\‾}{\mathrm{\ψ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ψ}}_i\end{array}\right.---\left(1\right)$

其中，N是测量时刻总次数，i是第i次测量时刻，ξ_i和η_i是第i次测量时的北向位置和东向位置，它们是已知参数；和是N次测量的北向位置和东向位置的期望；ψ_i是第i次测量时的AUV航向角，它是已知参数；是N次测量的航向角的期望；和它们是方程(1)的待定系数，通过求解方程(1)，计算出和的值。

在计算出值后，海流速度值在AUV垂直方向的投影vc计算方法如下：

$v_c=u|\sin \widehat{\mathrm{\β}}|---\left(2\right)$

其中，u是AUV的对底前向速度，是已知参数；是来流偏角，它的值在上一步已经被计算。

精确航迹跟踪控制方法包含两个步骤：第一步，计算被控制量；第二步，设计控制器。精确航迹跟踪控制方法将AUV与规划航迹的横向距离ε(t)和AUV航向角与规划航迹角的偏差量作为被控制量，它们的计算方法如下：

$\left\{\begin{array}{c}\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}=\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}+\widehat{\mathrm{\β}}\\ {\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}=\arctan 2({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1},{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})\\ \mathrm{\ξ}\left(t\right)=\frac{[{\mathrm{\ξ}}_i-\mathrm{\ξ},{\mathrm{\η}}_i-\mathrm{\η}]\·[{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1},{\mathrm{\η}}_i-{\mathrm{\η}}_{i-1}]}{\sqrt{{({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})}^2+{({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1})}^2}}\sin (\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)})\end{array}\right.---\left(3\right)$

其中，ψ(t)是AUV当前的航向角，(ξ_i-1,η_i-1)是规划航迹起点的北向位置坐标和东向位置坐标，(ξ_i,η_i)是规划航迹终点的北向位置坐标和东向位置坐标，它们是已知量。ψ_trk(i)是规划航迹角，中间变量。当等于0时，是AUV航向角与规划航迹角的偏差量，是该方程组的未知变量；当不等于0时，没有明确的物理含义。ξ(t)是AUV与规划航迹的横向距离，它是该方程组的未知变量。和ξ(t)AUV是精确航迹跟踪控制方法的控制输入量。航向角ψ(t)、规划轨迹角ψ_trk(i)、横向距离ξ(t)定义如图2所示。

精确航迹跟踪控制的作用是令AUV精确跟踪规划轨迹，当AUV偏离规划轨迹时，控制器自动引导AUV返回到规划轨迹上。精确轨迹跟踪控制算法的控制输入量是AUV与规划航迹的横向距离ξ(t)、AUV航向角与规划航迹角的偏差量AUV的对底前向速度u和AUV的转艏角速度r；控制输出量是水平面转艏力矩τ_n。

精确航迹跟踪控制的控制方法如下：

$\left\{\begin{array}{c}{\mathrm{\τ}}_n=\frac{1}{u\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)}\×[-{\mathrm{\λ}}_{2}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)-\mathrm{\η}\tanh (\mathrm{\σ}/\mathrm{\φ})\\ \mathrm{\σ}=\mathrm{ur}\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_{1}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_2\mathrm{\ϵ}\left(t\right)\end{array}\right.---\left(4\right)$

其中，τ_n是水平面转艏力矩，是控制输出量。u是AUV的对底前向速度，r是AUV的转艏角速度，它们通过AUV上搭载的传感器测量得到，是控制输入量。是AUV航向角与规划航迹角的偏差量，是AUV航向角与规划航迹角的偏差量，ε(t)是AUV与规划航迹的横向距离，它们是上面方程组计算出的结果，也是控制输入量；σ是中间变量；η、λ₁、λ₂和φ是已知的AUV精确航迹控制参数。

Claims (5)

1.一种自主水下机器人的海洋环境自识别的航迹精确跟踪方法，其特征在于，包括以下步骤：

海洋环境参数识别：计算海流速度值在水下机器人垂直方向的投影；

航迹跟踪：通过计算水下机器人与规划航迹的横向距离ε(t)、水下机器人航向角与规划航迹角的偏差量结合水下机器人的对底前向速度u和水下机器人的转艏角速度r计算水下机器人的水平面转艏力矩τ_n。

2.根据权利要求1所述的自主水下机器人的海洋环境自识别的航迹精确跟踪方法，其特征在于，所述计算海流速度值在水下机器人垂直方向的投影，具体为：

计算侧向来流偏角

$\left\{\begin{array}{c}\widehat{\mathrm{\β}}=\arctan \frac{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i{\mathrm{\η}}_i-N\stackrel{\‾}{\mathrm{\ξ\η}}}{\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i-N\stackrel{\‾}{\mathrm{\ξ}}}-\stackrel{\‾}{\mathrm{\ψ}}\\ \stackrel{\‾}{\mathrm{\ξ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i\\ \stackrel{\‾}{\mathrm{\η}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\η}}_i\\ \stackrel{\‾}{\mathrm{\ψ}}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ψ}}_i\end{array}\right.---\left(1\right)$

其中，N是测量时刻总次数；i是第i次测量时刻；ξ_i和η_i是第i次测量时的北向位置和东向位置，是已知参数；和是N次测量的北向位置和东向位置的期望；ψ_i是第i次测量时的水下机器人航向角，是已知参数；是N次测量的水下机器人航向角的期望；和它们是方程(1)的待定系数，通过求解方程(1)，计算出和的值；

在计算出值后，海流速度值在水下机器人垂直方向的投影v_c为：

$v_c=u\left|\sin \widehat{\mathrm{\β}}\right|---\left(2\right)$

其中，u是水下机器人的对底前向速度，是已知参数。

3.根据权利要求1所述的自主水下机器人的海洋环境自识别的航迹精确跟踪方法，其特征在于，所述水下机器人与规划航迹的横向距离ε(t)、水下机器人航向角与规划航迹角的偏差量通过下式计算：

$\left\{\begin{array}{c}\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}=\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}+\widehat{\mathrm{\β}}\\ {\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)}=\arctan 2({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1},{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})\\ \mathrm{\ξ}\left(t\right)=\frac{[{\mathrm{\ξ}}_i-\mathrm{\ξ},{\mathrm{\η}}_i-\mathrm{\η}]\·[{\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1},{\mathrm{\η}}_i-{\mathrm{\η}}_{i-1}]}{\sqrt{{({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_{i-1})}^2+{({\mathrm{\η}}_i-{\mathrm{\η}}_{i-1})}^2}}\sin (\mathrm{\ψ}\left(t\right)-{\mathrm{\ψ}}_{\mathrm{trk}\left(i\right)})\end{array}\right.---\left(3\right)$

其中，ψ(t)是水下机器人当前的航向角；(ξ_i-1,η_i-1)是规划航迹起点的北向位置坐标和东向位置坐标；(ξ_i,η_i)是规划航迹终点的北向位置坐标和东向位置坐标；ψ_trk(i)是规划航迹角，是中间变量；ψ(t)是航向角；是侧向来流偏角；ξ是当前水下机器人的东向位置坐标；η是当前水下机器人的北向位置坐标。

4.根据权利要求1所述的自主水下机器人的海洋环境自识别的航迹精确跟踪方法，其特征在于，所述水下机器人的水平面转艏力矩τ_n的计算方法为：

$\left\{\begin{array}{c}{\mathrm{\τ}}_n=\frac{1}{u\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)}\×[{-\mathrm{\λ}}_{2}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)-\mathrm{\η}\tanh (\mathrm{\σ}/\mathrm{\φ})]\\ \mathrm{\σ}=\mathrm{ur}\cos \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_{1}u\sin \left(\widetilde{\mathrm{\ψ}}{\left(t\right)}_{\mathrm{CTE}\left(i\right)}\right)+{\mathrm{\λ}}_2\mathrm{\ϵ}\left(t\right)\end{array}\right.---\left(4\right)$

其中，是水下机器人航向角与规划航迹角的偏差量，ε(t)是水下机器人与规划航迹的横向距离，σ是中间变量，η、λ₁、λ₂和φ是水下机器人航迹控制参数，由实验测定的常量，1≤η≤3，1≤λ₁≤3，1≤λ₂≤3，0≤φ≤1。

5.根据权利要求1所述的自主水下机器人的海洋环境自识别的航迹精确跟踪方法，其特征在于，所述水下机器人的对底前向速度u和水下机器人的转艏角速度r通过水下机器人搭载的传感器测量得到。