CN109901402B - 一种基于航向平滑技术的自主水下机器人路径跟踪方法 - Google Patents

Abstract

一种基于航向平滑技术的自主水下机器人路径跟踪方法，本发明涉及自主水下机器人路径跟踪方法。本发明的目的是为了解决现有AUV路径跟踪控制中艏向穿越‑π/π不连续点的情况时，路径跟踪准确率低的问题。过程为：一、测量初始时刻AUV的状态测量值，设置AUV的期望路径；二、得到AUV的路径跟踪误差；三、得到优化后的路径跟踪误差；四、使优化后的路径跟踪误差收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行二到五，直至AUV走完跟踪路径。本发明用于自主水下机器人路径跟踪领域。

Description

一种基于航向平滑技术的自主水下机器人路径跟踪方法

技术领域

本发明涉及自主水下机器人路径跟踪方法。

背景技术

近几十年来，随着我国的经济与科技实力的飞速发展，海洋在国家战略上的地位也显著提升。自主水下机器人(Autonomous underwater vehicle,AUV)因为其能够显著的减少水下作业的危险性和作业时间的特点，受到了世界各国的关注。目前已经有许多AUV成功的应用于海底石油管道检测、海底水文检测等任务中去，诸如此类的应用促使有关AUV的研究成为系统科学与机器人学中的一个活跃的领域^[1](Cabezas J M,Pastor E,Camacho LM,et al.I-AUV Mechatronics Integration for the TRIDENT FP7Project[J].IEEE/ASME Transactions on Mechatronics,2015,20(5):2583-2592.)。

在AUV实际执行任务时，路径跟踪能力是衡量AUV智能化程度的一个重要指标。由于AUV复杂的系统动力学特性和不可预测的复杂水下环境，难以应用高精度的基于模型的控制方法。且AUV大多采用欠驱动推进方案来提高续航能力，即采取舵桨作为控制输入，因此三自由度的路径控制则更为复杂。因此实际工程中的AUV路径跟踪控制中一般采用视线引导法将原本的路径跟踪问题转化为参考航向跟踪问题。AUV的航向角的定义域一般为(-π,π]，当参考航向与AUV当前航向之间的差值穿过了-π/π的不连续点时，则会导致参考航向与AUV航向动态不一致的现象，尤其是进行路径跟踪任务时，AUV可能会多次圆周航行，则一定会出现穿越-π/π的交界点的时刻，这时候就会出现一些不良的跟踪效果。例如，AUV沿着圆周进行运动，从圆周顶点开始运动，艏向向西，此时AUV的艏向为π，与此同时，随着AUV前进，参考点期望角会从π跳跃到-π，从而导致AUV的突然转向。

综上，导致AUV路径跟踪控制中艏向穿越-π/π(-180°～180°)不连续点的情况时，路径跟踪准确率低。

发明内容

本发明的目的是为了解决现有AUV路径跟踪控制中艏向穿越-π/π不连续点的情况时，路径跟踪准确率低的问题，而提出一种基于航向平滑技术的自主水下机器人路径跟踪方法。

一种基于航向平滑技术的自主水下机器人路径跟踪方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

步骤三、对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)；

步骤四、使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；

步骤五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤五，直至AUV走完跟踪路径。

本发明的有益效果为：

本发明针对AUV路径跟踪控制中艏向穿越-π/π不连续点导致跟踪效果较差甚至不能完成路径跟踪任务的问题，提出一种航向平滑算法，并结合MPC控制理论设计了一种新型的AUV路径跟踪控制器，来实现AUV路径跟踪任务。首先，将AUV路径跟踪问题转化为期望值跟踪问题，得到跟踪的期望点与期望艏向，以适应MPC控制框架。接着，利用辅助变量存储每个时刻航向角的绝对变化量，设计航向平滑算法将原本有限范围定义域内的航向角延拓到无限范围的定义域中，得到连续的期望航向角。最后，设计基于MPC的AUV路径跟踪控制器，利用合适的性能函数，利用滚动时域优化过程得到最优的控制序列，消除位置跟踪误差与航向跟踪误差，解决现有AUV路径跟踪控制中艏向穿越-π/π不连续点的情况时，路径跟踪准确率低的问题。

图3-5给出了AUV对“8”字形路径的跟踪效果和误差图，其中图3中虚线为参考路径，实线为实际跟踪轨迹，图4代表路径跟踪误差，图5是航向跟踪误差，其中虚线为不连续的期望航向角，点线为利用航向平滑算法优化后的连续期望航向角，实线为实际航向跟踪效果。从图中可以看出，从初始点5秒左右，AUV就能快速收敛到期望路径中，跟踪误差始终不超过0.3m，能够满足路径跟踪精度要求。图5中可以看出，航向平滑算法将不连续的期望航向角转化为连续的航向角，避免了不连续点导致控制器给出错误响应的情况。图6、图7表示AUV的推力与力矩输入，在整个跟踪控制过程中，控制输入平缓且能够保持在控制输入约束范围中。

附图说明

图1为本发明大地坐标系与随体坐标系示意图；

图2为本发明单位圆示意图；

图3为本发明“8”字型路径跟踪效果图；

图4为本发明距离跟踪误差图；

图5为本发明艏向跟踪效果图；

图6为本发明控制输入推力示意图；

图7为本发明控制输入力矩示意图。

具体实施方式

具体实施方式一：本实施方式一种基于航向平滑技术的自主水下机器人路径跟踪方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

步骤三、对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)；

步骤四、使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；

步骤五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤五，直至AUV走完跟踪路径。

具体实施方式二：本实施方式与具体实施方式一不同的是，所述步骤二中测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；具体过程为：

设置AUV水平面运动数学模型：

其中η＝[x′ y ψ]^T为大地坐标系下的坐标和姿态角，x′、y为大地坐标系下AUV的位置，ψ为大地坐标系下AUV艏向，上角标T表示求转置，

为大地坐标系与随体坐标系之间的关系，

R(ψ)为水平面坐标变换矩阵；M＝M_RB+M_A为惯性矩阵；M_RB为刚体惯性阵，M_A为附加质量阵，v＝[u′ v′ r]^T为随体坐标系下的速度向量，包括运动的速率和角速度；u′为随体坐标系下AUV的纵向，v′为随体坐标系下AUV的横向，r为随体坐标系下AUV的转艏速率，

为随体坐标系下的加速度，C(v)＝C_RB(v)+C_A(v)为科氏力和向心力的矩阵；C_RB(v)为科氏力的矩阵；C_A(v)为向心力的矩阵；D(v)为流体阻尼力和力矩对AUV产生的作用；τ_thr为随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入；τ_env为环境对AUV作用的干扰力，例如风、浪、流等；

为了便于AUV控制器设计，将上述水平面AUV运动数学模型简化为如下形式：

其中x为AUV的状态量(AUV的大地坐标系下的坐标和姿态角η＝[x′ y ψ]^T和随体坐标系下的速度向量v＝[u′ v′ r]^T)，u为输入量(控制AUV的力和力矩)，f为足够连续可微的非线性函数；

设置一条规则曲线Ρ

其中p(σ)为期望路径，σ为路径参数；

为路径参数的上界；

曲线Ρ定义在映射

上；

根据公式(3)，定义路径跟踪误差为：

e_p(t)＝x(t)-p(σ(t)) (4)

其中e_p为路径跟踪误差，x(t)为AUV的状态量，p(σ(t))为期望路径；

路径跟踪的控制目标就是保证路径跟踪误差收敛，即lime_p(t)＝0；

由于跟踪问题在每个时刻都需要一个期望值作为跟踪目标，因此在已知参考路径的条件下，需要生成参考点作为控制目标。

考虑当前AUV的位姿状态[x,y,ψ],以及路径p:x＝α₁(σ),y＝α₂(σ)，寻找曲线上离AUV当前状态最近的点为路径参考点，即可以归纳为如下形式：

其中α₁(σ)、α₂(σ)为参数方程；

通过求解上述优化问题式(5)，得到当前时刻的期望点的参数值σ(t)，即当前时刻的期望点的位置(x(σ(t)),y(σ(t)))；

而路径跟踪问题不仅仅需要期望点的位置(x(σ(t)),y(σ(t)))，还需要期望的航向角ψ(σ(t))来保证AUV沿期望路径的运动方向始终满足前向移动条件；

期望航向角通过下式计算得到：

其中atan2是求方位角的函数，atan2(x,y)返回原点到点(x,y)的方位角，定义域范围为ψ∈(-π,π]；

为参数方程；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤三中对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；具体过程为：

因为当前的航向与期望的航向角ψ(σ(t))的差值存在(-π,π]不连续点，导致AUV打转，所以对其进行优化；

航向平滑算法：

虽然式(6)给出了参考点期望角度的计算公式，但atan2函数的定义域为(-π,π]会导致在-π/π的交界点上不连续的情况，这对于路径跟踪问题是非常不利的，因为会引起参考航向与AUV航向动态不一致。当AUV在进行路径跟踪时，一定会出现穿越-π/π的交界点的时刻，这时候就会出现一些不良的跟踪效果。

因此在将跟踪误差提供给控制器之前，需要引入一个参考模型的概念，参考模型过滤输出来使AUV航向角在经过-π/π不连续点时避免突然的转向。

首先将原有的期望航向角度的定义域(-π,π]重新映射到(-∞,∞]中，可以使输入控制器的角度变为连续值，并通过参考模型的转换再映射回原定义域(-π,π]中。

一般来说，很难建立一个有限范围(-π,π]到无限范围(-∞,∞]的数学可解析的一一映射。因此要在两个区间之间创建信息含量较低的区间到信息含量较高的区间之间的映射，必须利用状态和记忆变量。此外，还必须从无穷多个可能的映射方案中选择一种调节规则。

考虑如图2所示的单位圆，表示AUV的方位角的定义，将其分为四个象限。上文已经提过，需要引入一些辅助变量来生成映射关系，首先用一个变量来跟踪单位圆上atan2函数在前一个时刻的位置，即一个表示前一个时刻所在象限的状态机，记为S1，其值为当前象限；其次，需要一个变量来记录atan2函数前一个时刻的角度值，记为ψ_p；同样的，当前时刻的状态与角度记为S2和ψ_n；

此外，显然需要一个来变量来表示所有之前的时刻累积的角度信息或者称为每个时刻航向角的绝对变化量，记为ψ_R。而需要寻找的映射关系就是上个时刻角度ψ_p到当前时刻ψ_n的变化量，记为Δψ_R，因为存在着-π/π的不连续点，所以Δψ_R≠ψ_n-ψ_p，寻找这个映射的关键就是找到Δψ_R的计算公式，从而得到连续的角度量ψ_R＝ψ_R+Δψ_R。

找到这个映射需要遵循一个原则，即每次更新期望值时，角度的变化尽可能的小，通过这样一个原则，可以将所有可能的更新ψ_R的情况分成16种情况，每个不同的S1和S2均会导致不同的结果，参照图2所示的单位圆，可以得到如下的更新算法：

将单位圆分为四个象限，第一象限角度为(0，π/2)，第二象限角度为(0，-π/2)，第三象限角度为(-π/2，-π)，第四象限角度为(π/2，π)；1表示第一象限，2表示第二象限，3表示第三象限，4表示第四象限；

1)

若S1＝1，即对应前一个时刻期望角度在第一象限；

11)当S2＝1,2,4时，Δψ_R＝ψ_n-ψ_p(不通过不连续点)；

所述，S1为单位圆上atan2函数在前一个时刻的位置，即一个表示前一个时刻所在象限的状态机；S2为单位圆上atan2函数在当前时刻的状态，ψ_n为单位圆上atan2函数在当前时刻的角度值；ψ_p为单位圆上atan2函数在前一个时刻的角度值；Δψ_R为航向角每个时刻绝对变化量；ψ_R为所有之前的时刻累积的角度信息，即参考模型中的航向角；

11)当S2＝3时，利用上文所说的原则，可以推出如下算法：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

其中abs()为绝对值函数；

2)

若S1＝2，即对应前一个时刻期望角度在第二象限；

21)当S2＝1,2,3时，

Δψ_R＝ψ_n-ψ_p；

22)当S2＝4时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

3)

若S1＝3，即对应前一个时刻期望角度在第三象限；

31)当S2＝1时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

32)当S2＝2,3时，

Δψ_R＝ψ_n-ψ_p

33)当S2＝4时，

Δψ_R＝-(π+ψ_p)-(π-ψ_n)＝ψ_n-ψ_p-2π

4)

若S1＝4，即对应前一个时刻期望角度在第四象限；

41)当S2＝1，4时，

Δψ_R＝ψ_n-ψ_p

42)当S2＝2时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

43)当S2＝3时，

Δψ_R＝(π-ψ_p)+(π+ψ_n)＝ψ_n-ψ_p+2π

通过平滑算法，再利用式(6)得到AUV的参考模型中的航向角ψ_R；

将航向角ψ_R反映射回(-π,π]区间中，获得输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤四中使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；具体过程为：

MPC控制器设计：

将路径跟踪问题归纳为约束优化问题：

满足约束

其中J为性能指标函数，e_p(k+i|k)为路径跟踪误差，u(k+i|k)为AUV的控制输入，σ(k+i|k)为路径参数；x(k+i+1|k)为AUV的状态量，x(k+i|k)为AUV的状态量，X为定义域(AUV状态量的范围)，U为定义域(控制AUV的推力和力矩，推力为0-100N，力矩为-200-200N)，p(σ(·))为期望路径，σ(x(k+i|k))为路径参数；k为离散时间，i为离散时间。

求解约束优化问题(7)得到控制AUV的力矩和力。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是，所述性能指标函数J表达式为

其中N为MPC预测时域，Q、R为对应的加权矩阵，u(k+i+1|k)为AUV的控制输入。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是，所述求解约束优化问题(7)得到控制AUV的力矩和力；具体过程为：

求解约束优化问题(7)，得到预测时域内的最优控制序列：

其中

为第一个时刻的最优控制输入，

为二个时刻的最优控制输入，

为第N个时刻的最优控制输入；

将最优控制序列的第一个控制量

作为当前时刻控制AUV的力矩和力。

其它步骤及参数与具体实施方式一至五之一相同。

AUV运动数学模型：AUV的运动数学模型是分析AUV在外力作用下的运动的有力工具，在进行AUV的运动控制研究时，对运动的建模是不可或缺的。AUV的运动建模是指用数学方法描述其运动过程的动态特征的方法。AUV运动模型包括运动学方程和动力学方程两个部分，是仿真和研究AUV的基础。AUV动力学方程是指AUV在流体运动中的受力方程。AUV的运动学是将AUV的运动在两种不同的参考系之间进行转换，它纯粹与运动的几何方面有关，通常使用两种坐标系描述AUV在空间中的运动：大地坐标系O_E-X_EY_EZ_E和随体坐标系O_b-X_bY_bZ_b，如图1所示。

MPC：MPC的控制机理可以描述为在每一个采用时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解。

AUV运动学模型：通常使用两种坐标系描述AUV在空间中的运动：大地坐标系O_E-X_EY_EZ_E和随体坐标系O_b-X_bY_bZ_b ^[2；3]([2]Fossen T I.Marine Control Systems:Guidance,Navigation,and Control of Ships,Rigs and Underwater Vehicles[M].2002.[3]SkjetneR,Teel A R,Kokotovic P V.Nonlinear maneuvering with gradientoptimization[C]//IEEE Conference on Decision&Control.IEEE,2002.)。

大地坐标系O_E-X_EY_EZ_E：如图1所示，大地坐标系的坐标系原点固定在地球上某一点处，其中，规定O_EX_E向北为正；O_EY_E向东为正；O_EZ_E向下为正，遵守右手定则。因为坐标轴随着地球旋转，严格来说，这不是一个惯性框架，但是对大多数海洋航行器来说，其可以看做一个惯性框架，即牛顿运动定律适用于此框架中。

随体坐标系O_b-X_bY_bZ_b：随体坐标系是固定在目标AUV质心上的参考坐标系，因此它不是一个惯性坐标系。值得注意的是，AUV当前所在位置与方向在这个坐标系中没有意义，因为它随着AUV一起移动。其中，规定O_bX_b沿AUV纵向，艏向为正，O_bY_b沿AUV横向，向右为正，O_bY_b向下为正，遵循右手定则。

在考虑AUV的路径跟踪问题时，通常要将AUV放在大地坐标系下来观测，而AUV的本身运动状态与受力分析在随体坐标系下又较容易分析，因此两个坐标系之间的转换是必不可少的，两种坐标系间的变换是根据坐标系间的夹角定义。

因为本专利考虑的是AUV的水平面的控制问题，假设AUV的俯仰角和横倾角很小，则大地坐标系与随体坐标系之间的关系可以用下式表述

其中

是从非惯性的随体坐标系O_b-X_bY_bZ_b向大地坐标系O_E-X_EY_EZ_E转换的矩阵，R(ψ)代表沿大地坐标系的z轴旋转一个角度ψ。此外η＝[x y ψ]^T代表大地坐标系下的AUV位置和艏向，而v＝[u v r]^T则代表随体坐标系下AUV的纵向、横向以及转艏速率。

AUV的六自由度动力学方程采用Fossen的AUV六自由度动力学模型^[2](Fossen TI.Marine Control Systems:Guidance,Navigation,and Control of Ships,Rigs andUnderwater Vehicles[M].2002.)：

其中M＝M_RB+M_A是惯性矩阵，包括附加质量阵M_A和刚体惯性阵M_RB,该矩阵的存在解释了整个流体-机器人系统的动能大于机器人刚体动能的原因。C(v)＝C_RB(v)+C_A(v)是描述刚体运动C_RB(v)以及流体作用C_A(v)产生的科氏力和向心力的矩阵。D(v)代表流体阻尼力和力矩对AUV产生的作用。这种力和力矩有许多种来源，例如潜在阻尼，表面摩擦，波浪漂移阻尼等等，它们可以使系统能量耗散，使得系统状态对于有界输入是有界的。g(η)代表的是AUV的重力和浮力带来的恢复力和恢复力矩。τ_thr是随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入，它可以有许多形式，可以由推进器和舵的联合作用构成，也可以用多个推进器构成。τ_env描述了环境对AUV作用的干扰力，例如风、浪、流等。

联立AUV的动力学和运动学方程，可得AUV三自由度运动数学模型：

采用以下实施例验证本发明的有益效果：

实施例一：

本实施例具体是按照以下步骤制备的：

为了能够平滑的完成AUV路径跟踪任务，除了本发明算法外还有Serrent-Frenet(S-F)坐标系路径跟踪方法、全驱动路径跟踪方案等，以下简要介绍这两种方案，并与本发明算法作比较

基于S-F坐标系的路径跟踪方案：

文献^[4](LapierreL,Soetanto D,Pascoal A.Nonlinear path following withapplications to the control of autonomous underwater vehicles[C]//IEEEConference on Decision&Control.IEEE,2003.)引入S-F坐标系，基于虚拟参考目标点坐标系建立误差动力学方程，基于Lyapunov理论和Backstepping技术导出路径跟踪控制器。在此坐标系下建立的误差动力学方程比较简单，控制器的推导过程也得到简化，容易得到非奇异的路径跟踪控制律。文献^[5](齐雪,张利军,赵杰梅.Serret-Frenet坐标系下AUV自适应路径跟踪控制[J].系统科学与数学,2016(11).)利用S-F坐标系将原跟踪问题转化为镇定问题，并基于虚拟参考目标点坐标系建立误差动力学方程.选择虚拟参考目标点为路径上任意一点，从而放宽了基于S-F坐标系的路径跟踪方法中较强的初始约束条件。与本发明专利的方法对比，基于S-F坐标系的路径跟踪方案理论上跟踪精度较高，但实现较为复杂，需要对原AUV运动方程进行微分几何变换，因此对建模的精确性和外部干扰的鲁棒性较差，尤其是AUV这种存在强外界干扰与建模不精确的载体，实用性有待商榷。

全驱动AUV控制方案：

全驱动AUV为路径跟踪控制提供了更简便的方法，即可以利用侧推与主推产生三自由度的输入，从而完成三自由度运动控制。文献^[6](杨雪.自主式智能体的跟踪控制问题研究[D].中国海洋大学.)研究海流影响及参数不确定条件下AUV的地形跟踪控制。基于Serret-Frenet坐标系，建立海流影响下的路径跟踪误差方程，并引入额外的系统自由度来达到控制目的。利用Lyapunov方法设计AUV的运动学跟踪控制器，然后采用Backstepping法得到动力学跟踪控制器，最后针对AUV参数未知的情况，给出其自适应控制方案。

与本发明算法相比，全驱动AUV在运动灵活性方面更佳，但是增加推进器带来的是续航方面的减少，而以舵桨为操纵系统的AUV在续航方面远比全驱动AUV优秀，且控制效果也能满足工程需求。

仿真准备

为了验证航向平滑算法应用于欠驱动AUV路径跟踪控制的效果，利用AUV数学模型，在MATLAB R2016b仿真环境下进行仿真实验，式(13)中所需的水动力系数值如表1所示

表1仿真参数表

AUV的执行器由一个主推和垂直舵组成，这里设定执行器的最大纵向推力为100N，最大扭转力矩为200N·m，不考虑倒车情况，控制节拍为0.2秒。

预设跟踪路径为“8”字型路径，该路径的特点为曲率不断变化，对跟踪响应速度要求较高，且在跟踪过程中AUV的航向将多次穿越-π/π不连续点。该路径的参数方程为：

仿真分析

在仿真实验中，控制器的控制输入为推力与力矩的形式。图3和图4表示路径跟踪控制效果，图5表示航向跟踪情况，其中点虚线与实线为重映射到(-∞,∞)的参考模型中的跟踪效果。图6、图7表示推力和力矩输入。

从图4和图5中可以看出，跟踪误差可以快速收敛至可以接受的范围，此外还可以从图6、图7中看出，航向平滑算法的引入可以使艏向的参考值不会出现突变的情况，即不会被图中25秒和43秒左右的不连续点所影响，因此整个跟踪过程较为平滑。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (4)

1.一种基于航向平滑技术的自主水下机器人路径跟踪方法，其特征在于：所述方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

步骤三、对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)；

步骤四、使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；

步骤五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤五，直至AUV走完跟踪路径；

所述步骤二中测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；具体过程为：

设置AUV水平面运动数学模型：

其中η＝[x′ y ψ]^T为大地坐标系下的坐标和姿态角，x′、y为大地坐标系下AUV的位置，ψ为大地坐标系下AUV艏向，上角标T表示求转置，

为大地坐标系与随体坐标系之间的关系，

R(ψ)为水平面坐标变换矩阵；M＝M_RB+M_A为惯性矩阵；M_RB为刚体惯性阵，M_A为附加质量阵，v＝[u′ v′ r]^T为随体坐标系下的速度向量，包括运动的速率和角速度；u′为随体坐标系下AUV的纵向速率，v′为随体坐标系下AUV的横向速率，r为随体坐标系下AUV的转艏速率，

为随体坐标系下的加速度，C(v)＝C_RB(v)+C_A(v)为科氏力和向心力的矩阵；C_RB(v)为科氏力的矩阵；C_A(v)为向心力的矩阵；D(v)为流体阻尼力和力矩对AUV产生的作用；τ_thr为随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入；τ_env为环境对AUV作用的干扰力；

将上述AUV水平面运动数学模型简化为如下形式：

其中x为AUV的状态量，u为AUV的控制输入，f为足够连续可微的非线性函数；

设置一条规则曲线Ρ

其中p(σ)为期望路径，σ为路径参数；

为路径参数的上界；

曲线Ρ定义在映射p:

上；

根据公式(3)，定义路径跟踪误差为：

e_p(t)＝x(t)-p(σ(t)) (4)

其中e_p为路径跟踪误差，x(t)为AUV的状态量，p(σ(t))为期望路径；

考虑当前AUV的位姿状态[x,y,ψ],以及路径p:x＝α₁(σ),y＝α₂(σ)，寻找曲线上离AUV当前状态最近的点为路径参考点，即归纳为如下形式：

其中α₁(σ)、α₂(σ)为参数方程；

通过求解上述优化问题式(5)，得到当前时刻的期望点的参数值σ(t)，即当前时刻的期望点的位置(x(σ(t)),y(σ(t)))；

期望航向角通过下式计算得到：

其中atan2是求方位角的函数，定义域范围为ψ∈(-π,π]；

为参数方程；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

所述步骤三中对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；具体过程为：

将单位圆分为四个象限，第一象限角度为(0，π/2)，第二象限角度为(0，-π/2)，第三象限角度为(-π/2，-π)，第四象限角度为(π/2，π)；1表示第一象限，2表示第二象限，3表示第三象限，4表示第四象限；

1)

若S1＝1，即对应前一个时刻期望角度在第一象限；

11)当S2＝1,2,4时，Δψ_R＝ψ_n-ψ_p；

其中，S1为单位圆上atan2函数在前一个时刻的位置，即一个表示前一个时刻所在象限的状态机；S2为单位圆上atan2函数在当前时刻的状态，ψ_n为单位圆上atan2函数在当前时刻的角度值；ψ_p为单位圆上atan2函数在前一个时刻的角度值；Δψ_R为航向角每个时刻绝对变化量；ψ_R为所有之前的时刻累积的角度信息，即参考模型中的航向角；

12)当S2＝3时，

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

其中abs()为绝对值函数；

2)

若S1＝2，即对应前一个时刻期望角度在第二象限；

21)当S2＝1,2,3时，

Δψ_R＝ψ_n-ψ_p；

22)当S2＝4时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

3)

若S1＝3，即对应前一个时刻期望角度在第三象限；

31)当S2＝1时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

32)当S2＝2,3时，

Δψ_R＝ψ_n-ψ_p

33)当S2＝4时，

Δψ_R＝-(π+ψ_p)-(π-ψ_n)＝ψ_n-ψ_p-2π

4)

若S1＝4，即对应前一个时刻期望角度在第四象限；

41)当S2＝1，4时，

Δψ_R＝ψ_n-ψ_p

42)当S2＝2时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

43)当S2＝3时，

Δψ_R＝(π-ψ_p)+(π+ψ_n)＝ψ_n-ψ_p+2π

再利用式(6)得到AUV的参考模型中的航向角ψ_R；

将航向角ψ_R反映射回(-π,π]区间中，获得输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)。

2.根据权利要求1所述一种基于航向平滑技术的自主水下机器人路径跟踪方法，其特征在于：所述步骤四中使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；具体过程为：

将路径跟踪问题归纳为约束优化问题：

满足约束

其中J为性能指标函数，e_p(k+i|k)为路径跟踪误差，u(k+i|k)为AUV的控制输入，σ(k+i|k)为路径参数；x(k+i+1|k)为AUV的状态量，x(k+i|k)为AUV的状态量，X为定义域，U为定义域，p(σ(·))为期望路径，σ(x(k+i|k))为路径参数；k为离散时间，i为离散时间；

求解约束优化问题(7)得到控制AUV的力矩和力。

3.根据权利要求2所述一种基于航向平滑技术的自主水下机器人路径跟踪方法，其特征在于：所述性能指标函数J表达式为

其中N为MPC预测时域，Q、R为对应的加权矩阵，u(k+i+1|k)为AUV的控制输入。

4.根据权利要求3所述一种基于航向平滑技术的自主水下机器人路径跟踪方法，其特征在于：所述求解约束优化问题(7)得到控制AUV的力矩和力；具体过程为：

求解约束优化问题(7)，得到预测时域内的最优控制序列：

其中

为第一个时刻的最优控制输入，

为二个时刻的最优控制输入，

为第N个时刻的最优控制输入；

将最优控制序列的第一个控制量

作为当前时刻控制AUV的力矩和力。

Images