CN108594834B - 一种面向未知环境下多auv自适应目标搜索和避障方法 - Google Patents

Abstract

本发明公开一种面向未知环境下多AUV自适应目标搜索和避障方法，适用于未知复杂水下环境下的多AUV的目标搜索。本发明主要分为三种模式：有目标模式，根据感知外界的目标信息，动态实时预测实现自适应搜索；无目标模式，利用分区域策略实现全区域覆盖搜索和协同规划任务；避障模式，基于改进的动态窗口法实时避开障碍威胁。本发明根据不同的水下环境信息，通过三种模式间交替切换执行方式，执行多AUV的目标搜索任务，能够应对未知水下的不确定信息，保障目标状态信息的可信区间，具有环境适应性和搜索高效性。

Description

一种面向未知环境下多AUV自适应目标搜索和避障方法

技术领域

本发明涉及的是一种水下无人航行器智能控制方法，具体地说是一种面向未知环境下多AUV的目标搜索和避障方法。

背景技术

在现代海洋活动中，多AUV的协作已经成为必然趋势，相比单AUV来讲，多AUV能够提高作业效率和任务鲁棒性。由于水下环境的复杂性和目标状态不确定性，多AUV在作业规划时是不可能预先完全获得任务环境的目标信息。面对未知复杂的水下环境，多AUV必须具备实时规划和目标搜索的能力。

目前大部分AUV集群的目标搜索方法均是建立在已知环境或部分已知环境的基础上，其搜索航迹为离线预先规划，面对复杂的水下环境，搜索效率较低，且缺乏动态应变能力。传统方法有A*算法、Dijkstra算法等，智能仿生算法如粒子群优化算法、蚁群算法、遗传算法等，该类算法均不适用于完全未知的环境下的多AUV目标搜索与避障，在外界不确定扰动下，不能保证对外界静态目标的有效观测。此外，由于搜索区域为水下随机环境，必须要求多AUV具有较强的环境适应能力，规划须具备时效性。一部分学者针对部分未知环境下进行了多AUV搜索过程中的局部航迹调整，但并不能实现完全未知环境下高效的搜索效果和实时适应环境的能力。随机搜索法等在理论上可确保未知状态下的多AUV目标搜索，但由于全局信息未知，从而使得算法难以独立支撑，且计算量过大。

发明内容

本发明的目的在于提供一种能够安全、高效地完成在未知环境下的目标搜索任务的面向未知环境下多AUV自适应目标搜索和避障方法。

本发明的目的是这样实现的：

步骤一：AUV利用前视声呐感知外界环境，将功能划分为有目标搜索模式、无目标搜索模式和避障模式，实现三种任务模式交替切换；

步骤二：AUV对目标进行位置估计，

AUV对目标进行位置估计表示为在每一时刻对如下联合后验概率密度：

p(x_v,k,θ|z_0:k,u _0:k,x_v,0)

其中，x表示离散时间下AUV的状态、θ表示目标的位置向量、z表示目标的观测值、u表示控制输入向量，

时间和量测更新表示为：

观测目标的时间更新：

p(x_v,k,θ|z_0:k-1,u_0:k,x_v,0)＝∫p(x_v,k|x_v,k-1,u_k)p(x_v,k-1,θ|z_0:k-1,u_0:k-1,x_v,0)dx_v,k-1

观测目标的测量更新：

将所有目标的信息计入数据库中，通过数据对比融合后，保存最准确的观测值，即：

P_min＝min{P_i,j,i＝1...n,j＝1...m}

其中，P表示目标位置的观测误差、i表示目标编号、j表示被观测的次数，

AUV在每个时间节点的位置处，预测出数个新的候选位置，针对任意一个候选位置，都对应一个独立估计的标准优化函数值，在所有标准优化函数计算的估计值中，以最优估计值的

作为AUV下一时刻的新位置，

通过贝叶斯估计对目标观测信息的不断更新与校正，完成区域搜索目标的任务；

(1)、当AUV视域范围内存在目标时，AUV进行目标类型识别，如若为静目标，则记录目标的包括位置的状态信息；若为有害动目标，则根据观测结果实现路径预测并跟踪，AUV接近有害动目标，有害动目标被摧毁后，该AUV继续执行未完成的目标搜索任务；

(2)、当AUV视域范围内无目标信息时，AUV采用分区域搜索策略，每个子区域的划分原则与可视阈值有关，且包含有独立的栅格矩阵计算值，根据每个AUV所在位置，考虑各个区域的搜索代价和路径代价，匹配合适的区域点作为该AUV所要航向的目标任务点；具体如下：

根据AUV运动方程与观测方程，定义一个优化评价标准函数：

其中，x与q表示AUV与目标的位置信息、v表示AUV的视域范围、D表示任务区域、K为常数、ζ为干扰噪声，

在每个时刻的时间节点处，通过感知计算这些函数的噪声测量估计，使用函数近似的方法，估计在每个时刻k处的未知目标函数J：

式中

为优化评价函数的估计值、

表示k时刻计算的参数估计矢量、φ表示回归项的非线性矢量，

根据参数估计向量

结合最小二乘法，计算估计参数为：

在k时刻，AUV以当前时间节点的位置为基准，预选k+1时刻中R个候选位置：

式中，α_k为不大于当前最大速度的正数序列、

为零均值且单位方差的随机变量，

由优化标准函数计算各优化标准值，选取最优标准值作为AUV的最新位置

结合动态预测，将跟随时间节点的航迹数据滤波：

首先计算(2n_x+1)个Sigma采样点向量x⁽ⁱ⁾和其对应的权值ω⁽ⁱ⁾：

其中：k≥0、w⁽ⁱ⁾表示第i个Sigma点的权重，

将每个Sigma点通过AUV运动方程计算，得到如下采样的变换点：

y⁽ⁱ⁾＝f(x⁽ⁱ⁾)

变换点y的均值和协方差的近似值计算如下：

另χ_k为Sigma点的矩阵集合，如下所示：

UKF预测方程如下所示：

χ_k/k-1＝f(χ_k-1)

z_k/k-1＝f(χ_k/k-1)

UKF更新方程如下所示：

其中，Q为过程噪声协方差、R为观测噪声协方差、K是卡尔曼增益；

(3)、当AUV的前视声呐视域范围内出现障碍物时，AUV降低速度，利用改进的动态窗口法完成局部避障，避开障碍物的威胁，继续执行目标搜索任务；

动态窗口法是在速度(v,w)空间中预测多组速度，得到多组航迹后，对所有的预测航迹做出评价，选取最优评价值的航迹作为AUV下一时段的运动轨迹，具体包括：

由于AUV不能全向移动，即：

Δx＝vΔtcos(θ_t)

Δy＝vΔtsin(θ_t)

在一段时间内，AUV预测航迹应表示为：

x＝x+vΔtcos(θ_t)

y＝y+vΔtsin(θ_t)

θ_t＝θ_t＝wΔt

根据上述多组速度预测多组航迹，在二维空间中，速度采样受到AUV本身的限制和外界威胁限制，则将采样速度控制在一定可允许范围：

AUV自身条件的速度和转弯率限制为：

V_max＝{v∈[v_min,v_max],w∈[w_min,w_max]}

为了能够避开威胁，在减速条件下，由外界环境对速度的限制为：

其中D(v,w)为速度在虚拟航迹中，对应航迹里障碍物最近的距离，

在采样速度下，采样评价函数为每条航迹做出评价，采样的评价函数如下所示：

L(v,w)＝σ(α·h(v,w)+β·d(v,w)+γ·v(v,w)+ε·s(v,w))

方向角评价函数h(v,w)用来评价AUV在当前设定采样速度下，艏向角与目标点的角度之差，目标点随着AUV处在不同的环境而不同，当AUV发现威胁后，将分区域模式下，以AUV的方向角内且距离最近的目标区域点作为前向目标，即虚拟目标点，

goal(u)＝Task(x)

式中：u表示AUV的编号、x为满足最优条件的区域编号；

距离评价函数d(v,w)表示AUV在当前采样速度的航迹上与最近障碍物威胁的距离；

速度评价函数v(v,w)则用来评价当前采样速度的速率大小，

当AUV遇到复杂障碍物需要转艏时，当AUV艏向与方向角函数之差大于90°时，此时的评价函数s将会优先选择目标点的反向位置来避开威胁，

将得到的所有采样速度做归一化处理，即：

其中，n为所有采样的速度、i为待评价的当前采样速度；

步骤三：AUV通过前视声呐检测环境，判断当前每个AUV处于步骤二的三种模式下的哪一种模式，返回执行步骤二，执行相应的任务模式。

本发明还可以包括：

1、AUV的搜索任务区域环境模型、前视声呐模型以及目标特性为：

搜索任务区域模型：设搜索任务为某未知大范围矩形海域，该区域有未知X个静态目标与Y个动态目标，执行本次任务的AUV数量为n个，利用栅格法将该海域划分为M*N个栅格，并将单元格的集合作为AUV航迹的代价矩阵，即v＝{(i,j)|i＝1,2...M；j＝1,2...N}，每个栅格表示代价矩阵中的第i行第j列；

前视声呐模型：采用真实的声纳数据作为模型内容，其视域范围R为150m，水平开角α为120°，垂直开角β为15°，水平波束为80条，垂直方向共有三层；声纳模型获取信息的方法，视为将栅格数据计入二维数组内，通过填充每个栅格里的数组元素来表示在视域范围内是否存有目标，如果在声呐的栅格中填充了有效的数据信息时，然后将数据信息记录到二维数组中，通过数学模型计算出声呐探测到物体的位置信息，

建立前视声呐数学模型为：

其中(x_t,y_t,z_t)表示为：

式中，(x,y,z)为目标在艇体坐标系(Ox_zy_zz_z)下的坐标、(x₀,y₀,z₀)为AUV配置声呐的本体坐标，在同一水深下搜索不考虑z轴方向，

前视声呐在数据采集过程中，容易受到水介质或其他外界因素的影响，会导致非线性干扰问题，简单描述如下：

式中，y_x-q表示前视声呐采集的目标信息、N表示目标数据不存在、L为视觉阈值、h表示无噪声条件下的声呐探测函数、d表示目标与声呐的距离、ζ为非线性干扰；

目标特性：

静态目标的位置信息在任意时刻将保持不变，则静态目标的特征模型描述为：

未知环境下，设所有障碍物均对AUV是静态威胁，则障碍物描述为由数据点组成的面结构，其模型描述为如下矩阵：

Obs＝[m]_i,j,m＝0,1

在i行j列矩阵中，元素为1的点表示构成障碍物威胁面，

动态目标随机出现在环境中，设其为匀角速度转弯运动，在直角坐标系下，运动目标数学模型的离散时间方程为：

式中：ω表示转弯角速度、T为采样时间。

2、AUV运动方程与观测方程为：

首先考虑如下的AUV运动方程与观测方程

观测方程具体表示为：

式中，χ表示k时刻声呐距观测到的物体的距离、θ表示物体与AUV艏向的相对角度。

为了实现多AUV在未知环境下自适应的目标搜索和避障目的，安全、高效的完成在未知环境下的目标搜索任务，本发明以分布式结构体系的多AUV完成目标区域搜索任务，考虑声呐噪声的非线性影响和复杂的水下未知环境，提出一种自适应目标搜索和避障方法。

本发明主要包括如下内容：

一：分析未知环境下多AUV协同目标搜索任务的影响因素；

二：建立搜索任务区域环境模型、前视声呐模型以及目标特性；

三：建立AUV运动方程与观测方程；

四：基于步骤三利用AUV前视声呐感知外界环境，将功能划分为有目标搜索模式、无目标搜索模式和避障模式，实现三种任务模式交替切换；

五：当AUV视域范围内存在目标时，AUV要在短时间内识别目标类型，如若为静目标，则将记录其位置等状态信息。若为有害动目标，则根据观测结果实现路径预测并跟踪，AUV接近有害动目标，动态目标被摧毁后，该AUV继续执行未完成的目标搜索任务；

六：当AUV视域范围内无目标信息时，AUV将采用分区域搜索策略，每个子区域的划分原则与可视阈值有关，且包含有独立的栅格矩阵计算值，根据每个AUV所在位置，考虑各个区域的搜索代价和路径代价，匹配合适的区域点作为该AUV所要航向的目标任务点；

七：当AUV的前视声呐视域范围内出现障碍物时，要求AUV能够及时降低速度，利用改进的动态窗口法完成局部避障，避开障碍物的威胁，继续执行目标搜索任务；

八：AUV通过前视声呐检测环境，判断当前每个AUV处于步骤五、六和七三种模式下的哪一种模式，返回执行步骤五～七，执行相应的任务模式。

本发明首先以分布式结构体系的多AUV完成目标区域搜索任务，考虑声呐等传感设备探测过程中的非线性噪声影响，分析了未知环境下的环境特性，提出一种基于自适应的目标搜索和避障方法，能够完成在未知的环境状态下，高效并安全的执行搜索任务。

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

1.针对未知环境下，提出一种基于自适应的目标搜索和避障方法，相比于传统搜索方法和仿生优化算法，不仅兼顾搜索代价，还考虑因前视声纳噪声影响下对目标观测不准确等问题，保障目标的定位精度。

2.考虑到AUV视域内并非实时存在目标，则将提出一种分区域策略与动态预测融合，辅助AUV在提高搜索覆盖范围，增加搜索目标个数的可靠性。

3.考虑水下环境会出现障碍物等威胁信息，特别针对凹型障碍物，则改进一种动态窗口法，使其能够完成凹型区域或常规区域的避障，安全可靠的执行目标搜索。

附图说明

图1前视声纳模型示意图。

图2位置预测示意图。

图3速度预测示意图。

图4初始化任务示意图。

图5发现动态目标并摧毁动态目标示意图。

图6任务截止示意图。

图7AUV协同情况示意图。

图8AUV协方差轨迹示意图。

图9静目标的真实位置与估计位置的误差数据统计表1。

具体实施方式

下面举例对本发明做更详细的描述。

步骤一：分析未知环境下多AUV协同目标搜索任务的影响因素；

任务影响主要包括以下几点：

多AUV之间需要合作完成，做到数据资源共享，任务分配明确；

1、AUV前视声呐探测距离有限，且目标距离越远，噪声干扰越大，定位精度越低。

2、在未知环境下，AUV必须能够确保自身安全，躲避障碍物，并根据环境变化，做出相应的状态调整。

步骤二：建立搜索任务区域环境模型、前视声呐模型以及目标特性；

搜索任务区域模型：假设搜索任务为某未知大范围矩形海域，该区域有未知X个静态目标与Y个动态目标，执行本次任务的AUV数量为n个。利用栅格法将该海域划分为M*N个栅格，并将单元格的集合作为AUV航迹的代价矩阵，即v＝{(i,j)|i＝1,2...M；j＝1,2...N}，每个栅格表示代价矩阵中的第i行第j列。

前视声呐模型：采用真实的声纳数据作为模型内容，其视域范围R为150m，水平开角α为120°，垂直开角β为15°，水平波束为80条，垂直方向共有三层。声纳模型获取信息的方法，可视为将栅格数据计入二维数组内，通过填充每个栅格里的数组元素来表示在视域范围内是否存有目标，如果在声呐的栅格中填充了有效的数据信息时，然后将数据信息记录到二维数组中，通过数学模型计算出声呐探测到物体的位置信息。

建立前视声呐数学模型为：

其中(x_t,y_t,z_t)可表示为：

式中，(x,y,z)为目标在艇体坐标系(Ox_zy_zz_z)下的坐标。(x₀,y₀,z₀)为AUV配置声呐的本体坐标，本发明为同一水深下搜索，因此不考虑z轴方向。

前视声呐在数据采集过程中，容易受到水介质或其他外界因素的影响，会导致非线性干扰问题，其可简单描述如下：

式中，y_x-q表示前视声呐采集的目标信息，N表示目标数据不存在，L为视觉阈值，h表示无噪声条件下的声呐探测函数，d表示目标与声呐的距离，ζ为非线性干扰。上述表示当前视声呐与目标的距离超过视域范围，或声呐与目标之间有障碍物时，均不能反馈目标信息。如若视域范围内有目标信息，则其对目标的观测值随着距离的增加，扰动也随着增加。

目标特性：未知水下环境，可存在静态目标、动态目标或障碍物，AUV感知不同的环境而做出不同的决策。

静态目标的位置信息在任意时刻将保持不变，则静态目标的特征模型可以描述为：

未知环境下的威胁环境较为复杂，本发明假设所有障碍物均对AUV来说是静态威胁，则障碍物可描述为由数据点组成的面结构，其模型可描述为如下矩阵：

Obs＝[m]_i,j,m＝0,1 (5)

在i行j列矩阵中，元素为1的点表示构成障碍物威胁面。

动态目标随机出现在环境中，假设其为匀角速度转弯运动，在直角坐标系下，运动目标数学模型的离散时间方程为：

式中：ω表示转弯角速度；T为采样时间

步骤三：建立AUV运动方程与观测方程；

首先考虑如下的AUV运动方程与观测方程；

观测方程具体表示为：

式中，χ表示k时刻声呐距观测到的物体的距离，θ表示物体与AUV艏向的相对角度

步骤四：基于步骤三利用AUV前视声呐感知外界环境，将功能划分为有目标搜索模式、无目标搜索模式和避障模式，实现三种任务模式交替切换；

步骤五：当AUV视域范围内存在目标时，AUV要在短时间内识别目标类型，如若为静目标，则将记录其位置等状态信息。若为有害动目标，则根据观测结果实现路径预测并跟踪，AUV接近有害动目标，动态目标被摧毁后，该AUV继续执行未完成的目标搜索任务。

AUV对目标的位置估计，表示为在每一时刻对如下联合后验概率密度：

p(x_v,k,θ|z_0:k,u_0:k,x_v,0) (10)

式中，x表示离散时间下AUV的状态，θ表示目标的位置向量，z表示目标的观测值，u表示控制输入向量。

通过时间和量测的不断更新如下所示：

观测目标的时间更新：

p(x_v,k,θ|z_0:k-1,u_0:k,x_v,0)＝∫p(x_v,k|x_v,k-1,u_k)p(x_v,k-1,θ|z_0:k-1,u_0:k-1,x_v,0)dx_v,k-1 (11)

观测目标的测量更新：

将所有目标的信息计入数据库中，通过数据对比融合后，保存最准确的观测值，即：

P_min＝min{P_i,j,i＝1...n,j＝1...m} (13)

式中，P表示目标位置的观测误差，i表示目标编号，j表示被观测的次数。

AUV在每个时间节点的位置处，预测出数个新的候选位置，针对任意一个候选位置，都对应一个独立估计的标准优化函数值，在所有标准优化函数计算的估计值中，以最优估计值的

作为AUV下一时刻的新位置，如图2所示。

通过贝叶斯估计对目标观测信息的不断更新与校正，可以提高目标定位的置信区间，完成区域高效搜索目标的任务。

步骤六：当AUV视域范围内无目标信息时，AUV将采用分区域搜索策略，每个子区域的划分原则与可视阈值有关，且包含有独立的栅格矩阵计算值，根据每个AUV所在位置，考虑各个区域的搜索代价和路径代价，匹配合适的区域点作为该AUV所要航向的目标任务点。具体如下：

根据步骤三建立的AUV运动方程与观测方程，随后定义一个优化评价标准函数：

式中，x与q表示AUV与目标的位置信息，v表示AUV的视域范围，D表示任务区域，K为常数，ζ为干扰噪声。

目标函数的显式形式是无法预知的，但是在每个时刻的时间节点处，可通过感知计算这些函数的噪声测量估计，使用函数近似得方法，估计在每个时刻k处的未知目标函数J：

式中

为优化评价函数的估计值；

表示k时刻计算的参数估计矢量；φ表示回归项的非线性矢量。

根据参数估计向量

结合最小二乘法，计算估计参数为：

在k时刻，AUV以当前时间节点的位置为基准，预选k+1时刻中R个候选位置：

式中，α_k为不大于当前最大速度的正数序列；

为零均值且单位方差的随机变量。

由优化标准函数计算各优化标准值，选取最优标准值作为AUV的最新位置：

利用无迹卡尔曼滤波，降低干扰数据的影响，防止累积误差出现造成算法的失效性，结合动态预测，将跟随时间节点的航迹数据采用滤波去除干扰。

首先计算(2n_x+1)个Sigma采样点向量x⁽ⁱ⁾和其对应的权值ω⁽ⁱ⁾：

其中：k≥0，w⁽ⁱ⁾表示第i个Sigma点的权重。

将每个Sigma点通过AUV运动方程计算，得到如下采样的变换点：

y⁽ⁱ⁾＝f(x⁽ⁱ⁾) (21)

变换点y的均值和协方差的近似值计算如下：

另χ_k为Sigma点的矩阵集合，如下所示：

UKF预测方程如下所示：

χ_k/k-1＝f(χ_k-1) (25)

z_k/k-1＝f(χ_k/k-1) (28)

UKF更新方程如下所示：

式中，Q为过程噪声协方差，R为观测噪声协方差，K是卡尔曼增益。

AUV预测位置的选择，可以保证

对优化标准函数J是可靠而准确的估计。将AUV预测后的新位置利用UKF滤波，可降低因非线性干扰的影响，保障AUV能够按照预设轨迹运行。

步骤七：当AUV的前视声呐视域范围内出现障碍物时，要求AUV能够及时降低速度，利用改进的动态窗口法完成局部避障，避开障碍物的威胁，继续执行目标搜索任务。动态窗口法主要是在速度(v,w)空间中预测多组速度，得到多组航迹后，对所有的预测航迹做出评价，选取最优评价值的航迹作为AUV下一时段的运动轨迹。

由于AUV不能全向移动，即：

Δx＝vΔtcos(θ_t)

Δy＝vΔt sin(θ_t) (35)

在一段时间内，其预测航迹应表示为：

x＝x+vΔtcos(θ_t)

y＝y+vΔtsin(θ_t)

θ_t＝θ_t＝wΔt (36)

根据上述多组速度可以预测多组航迹，在二维空间中，速度采样受到AUV本身的限制和外界威胁限制，则需将采样速度控制在一定可允许范围：

AUV自身条件的速度和转弯率限制为：

V_max＝{v∈[v_min,v_max],w∈[w_min,w_max]} (37)

为了能够避开威胁，在减速条件下，由外界环境对速度的限制为：

其中D(v,w)为速度在虚拟航迹中，对应航迹里障碍物最近的距离。

动态窗口采样的速度航迹可简单描述如图3所示。

在采样速度下，采样评价函数为每条航迹做出评价，采样的评价函数如下所示：

L(v,w)＝σ(α·h(v,w)+β·d(v,w)+γ·v(v,w)+ε·s(v,w)) (39)

方向角评价函数h(v,w)用来评价AUV在当前设定采样速度下，艏向角与目标点的角度之差，本发明中，目标点随着AUV处在不同的环境而不同，当AUV发现威胁后，将分区域模式下，以AUV的方向角内且距离最近的目标区域点作为前向目标，即虚拟目标点。

goal(u)＝Task(x) (40)

式中：u表示AUV的编号，x为满足最优条件的区域编号。

距离评价函数d(v,w)表示AUV在当前采样速度的航迹上，与最近障碍物威胁的距离。

速度评价函数v(v,w)则用来评价当前采样速度的速率大小。

当AUV遇到复杂障碍物需要转艏时，当AUV艏向与方向角函数之差大于90°时，此时的评价函数s将会优先选择目标点的反向位置来避开威胁。

将得到的所有采样速度做归一化处理，即：

其中，n为所有采样的速度，i为待评价的当前采样速度。

通过设立虚拟目标点和反向优化函数，可以将动态窗口法融入自适应搜索过程中，在面向静态威胁时，可以通过该局部路径规划避开威胁，保障AUV搜索任务的实施。

步骤八：AUV通过前视声呐检测环境，判断当前每个AUV处于步骤五、六和七三种模式下的哪一种模式，返回执行步骤五～七，执行相应的任务模式。

为了验证本发明方法有的有效性，设计了仿真案例，设计仿真环境为900x900m未知水下区域，AUV数量为3艘，分别设置x轴方向(150,0)，(450,0)，(750,0)为初始位置。随机布置若干个静态目标，1个动态目标，3个静态威胁，为了保障AUV在执行任务中因干扰而造成与威胁相撞的危险因素，则将判定用的障碍物距离增至1.5m，避障容错距离设为5m，定位误差小于5m，为模拟环境的不确定性，手动输入所有不确定静态目标。仿真结果如图4-图8与表1所示；图4表示初始化任务，设置了三个AUV的起始坐标，障碍物信息和未知目标信息；图5表示AUV接近有害动目标，动态目标被摧毁后，该AUV继续执行未完成的目标搜索任务；图6表示当所有目标满足定位精度要求或所有子区域均有遍历时，则达到结束条件，最后结束目标搜索任务；图7表示各个AUV执行子区域搜索的任务分布情况，以及各个子区域的搜索路径代价数据；图8说明了AUV运动没有累计误差的出现，且服从马尔科夫过程。本次搜索过程中，1，3号AUV避开简单区域威胁。2号AUV遇到复杂障碍威胁，首先通过搜索模式观测到静态目标，随后切换避障模式躲避威胁。由1号AUV发现动态目标，并收集动态目标运动信息，持续跟踪动目标，当1号AUV接近动目标至某一程度时，假设其达到摧毁要求并摧毁目标后，继续执行未完成的目标搜索任务。自适应搜索并不要求一次性生成搜索航迹，而是AUV每次通过感知周围外界信息，在一定范围内预测更新自身位姿。当所有目标满足定位精度要求或所有子区域均有遍历时，则达到结束条件。

本发明最终针对未知复杂环境的搜索问题，提出基于自适应多AUV目标搜索和避障方法，在面向不同的外界环境能够及时调整运动策略，更能胜任随机的外界环境，特别是处于任务范围内目标较多时，具有更好的搜索效率和环境适应性，能够灵活应对不同外界环境，保障搜索任务的高效进行，通过实验仿真，验证了该方法在未知复杂环境下搜索的可行性。

Claims (4)

1.一种面向未知环境下多AUV自适应目标搜索和避障方法，其特征是：

步骤一：AUV利用前视声呐感知外界环境，将功能划分为有目标搜索模式、无目标搜索模式和避障模式三种任务模式；

步骤二：AUV对目标进行位置估计，实现三种任务模式交替切换；

(1)、当AUV视域范围内存在目标时，AUV进行目标类型识别，如若为静目标，则记录目标的包括位置的状态信息；若为有害动目标，则根据观测结果实现路径预测并跟踪，AUV接近有害动目标，有害动目标被摧毁后，该AUV继续执行未完成的目标搜索任务；

(2)、当AUV视域范围内无目标信息时，AUV采用分区域搜索策略，每个子区域的划分原则与可视阈值有关，且包含有独立的栅格矩阵计算值，根据每个AUV所在位置，考虑各个区域的搜索代价和路径代价，匹配合适的区域点作为该AUV所要航向的目标任务点；

(3)、当AUV的前视声呐视域范围内出现障碍物时，AUV降低速度，利用改进的动态窗口法完成局部避障，避开障碍物的威胁，继续执行目标搜索任务；所述动态窗口法是在速度(v,w)空间中预测多组速度，得到多组航迹后，对所有的预测航迹做出评价，选取最优评价值的航迹作为AUV下一时段的运动轨迹；

所述AUV对目标进行位置估计具体包括：

AUV对目标进行位置估计表示为在每一时刻对如下联合后验概率密度，

p(x_v,k,θ|z_0:k,u_0:k,x_v,0)

其中，x表示离散时间下AUV的状态、θ表示目标的位置向量、z表示目标的观测值、u表示控制输入向量，

时间和量测更新表示为：

观测目标的时间更新：

p(x_v,k,θ|z_0:k-1,u_0:k,x_v,0)＝∫p(x_v,k|x_v,k-1,u_k)p(x_v,k-1,θ|z_0:k-1,u_0:k-1,x_v,0)dx_v,k-1

观测目标的测量更新：

将所有目标的信息计入数据库中，通过数据对比融合后，保存最准确的观测值，即：

P_min＝min{P_i,j,i＝1...n,j＝1...m}

其中，P表示目标位置的观测误差、i表示目标编号、j表示被观测的次数，

AUV在每个时间节点的位置处，预测出数个新的候选位置，针对任意一个候选位置，都对应一个独立估计的标准优化函数值，在所有标准优化函数计算的估计值中，以最优估计值的

作为AUV下一时刻的新位置，

通过贝叶斯估计对目标观测信息的不断更新与校正，完成区域搜索目标的任务；

所述AUV的分区域搜索策略具体包括：

根据AUV运动方程与观测方程，定义一个优化评价标准函数：

其中，x与q表示AUV与目标的位置信息、v表示AUV的视域范围、D表示任务区域、K为常数、ζ为干扰噪声，

在每个时刻的时间节点处，通过感知计算这些函数的噪声测量估计，使用函数近似的方法，估计在每个时刻k处的未知目标函数J：

式中

为优化评价函数的估计值、

表示k时刻计算的参数估计矢量、φ表示回归项的非线性矢量，

根据参数估计向量

结合最小二乘法，计算估计参数为：

在k时刻，AUV以当前时间节点的位置为基准，预选k+1时刻中R个候选位置：

式中，α_k为不大于当前最大速度的正数序列、

为零均值且单位方差的随机变量，

由优化标准函数计算各优化标准值，选取最优标准值作为AUV的最新位置

结合动态预测，将跟随时间节点的航迹数据滤波：

首先计算(2n_x+1)个Sigma采样点向量x⁽ⁱ⁾和其对应的权值ω⁽ⁱ⁾：

其中：k≥0、w⁽ⁱ⁾表示第i个Sigma点的权重，

将每个Sigma点通过AUV运动方程计算，得到如下采样的变换点：

y⁽ⁱ⁾＝f(x⁽ⁱ⁾)

变换点y的均值和协方差的近似值计算如下：

另χ_k为Sigma点的矩阵集合，如下所示：

UKF预测方程如下所示：

χ_k/k-1＝f(χ_k-1)

z_k/k-1＝f(χ_k/k-1)

UKF更新方程如下所示：

其中，Q为过程噪声协方差、R为观测噪声协方差、K是卡尔曼增益；

步骤三：AUV通过前视声呐检测环境，判断当前每个AUV处于步骤二的三种模式下的哪一种模式，返回执行步骤二，执行相应的任务模式。

2.根据权利要求1所述的面向未知环境下多AUV自适应目标搜索和避障方法，其特征是AUV的搜索任务区域环境模型、前视声呐模型以及目标特性为：

搜索任务区域模型：设搜索任务为某未知大范围矩形海域，该区域有未知X个静态目标与Y个动态目标，执行本次任务的AUV数量为n个，利用栅格法将该海域划分为M*N个栅格，并将单元格的集合作为AUV航迹的代价矩阵，即v＝{(i,j)|i＝1,2...M；j＝1,2...N}，每个栅格表示代价矩阵中的第i行第j列；

前视声呐模型：采用真实的声纳数据作为模型内容，其视域范围R为150m，水平开角α为120°，垂直开角β为15°，水平波束为80条，垂直方向共有三层；声纳模型获取信息的方法，视为将栅格数据计入二维数组内，通过填充每个栅格里的数组元素来表示在视域范围内是否存有目标，如果在声呐的栅格中填充了有效的数据信息时，然后将数据信息记录到二维数组中，通过数学模型计算出声呐探测到物体的位置信息，

建立前视声呐数学模型为：

其中(x_t,y_t,z_t)表示为：

式中，(x,y,z)为目标在艇体坐标系(Ox_zy_zz_z)下的坐标、(x₀,y₀,z₀)为AUV配置声呐的本体坐标，在同一水深下搜索不考虑z轴方向，

前视声呐在数据采集过程中，容易受到水介质或其他外界因素的影响，会导致非线性干扰问题，简单描述如下：

式中，y_x-q表示前视声呐采集的目标信息、N表示目标数据不存在、L为视觉阈值、h表示无噪声条件下的声呐探测函数、d表示目标与声呐的距离、ζ为非线性干扰；

目标特性：

静态目标的位置信息在任意时刻将保持不变，则静态目标的特征模型描述为：

未知环境下，设所有障碍物均对AUV是静态威胁，则障碍物描述为由数据点组成的面结构，其模型描述为如下矩阵：

Obs＝[m]_i,j,m＝0,1

在i行j列矩阵中，元素为1的点表示构成障碍物威胁面，

动态目标随机出现在环境中，设其为匀角速度转弯运动，在直角坐标系下，运动目标数学模型的离散时间方程为：

式中：ω表示转弯角速度、T为采样时间。

3.根据权利要求2所述的面向未知环境下多AUV自适应目标搜索和避障方法，其特征是AUV运动方程与观测方程为：

首先考虑如下的AUV运动方程与观测方程

观测方程具体表示为：

式中，χ表示k时刻声呐距观测到的物体的距离、θ表示物体与AUV艏向的相对角度。

4.根据权利要求3所述的面向未知环境下多AUV自适应目标搜索和避障方法，其特征是利用改进的动态窗口法完成局部避障具体包括：

由于AUV不能全向移动，即：

Δx＝vΔtcos(θ_t)

Δy＝vΔtsin(θ_t)

在一段时间内，AUV预测航迹应表示为：

x＝x+vΔtcos(θ_t)

y＝y+vΔtsin(θ_t)

θ_t＝θ_t＝wΔt

根据上述多组速度预测多组航迹，在二维空间中，速度采样受到AUV本身的限制和外界威胁限制，则将采样速度控制在一定可允许范围：

AUV自身条件的速度和转弯率限制为：

V_max＝{v∈[v_min,v_max],w∈[w_min,w_max]}

为了能够避开威胁，在减速条件下，由外界环境对速度的限制为：

其中D(v,w)为速度在虚拟航迹中，对应航迹离障碍物最近的距离，

在采样速度下，采样评价函数为每条航迹做出评价，采样的评价函数如下所示：

L(v,w)＝σ(α·h(v,w)+β·d(v,w)+γ·v(v,w)+ε·s(v,w))

方向角评价函数h(v,w)用来评价AUV在当前设定采样速度下，艏向角与目标点的角度之差，目标点随着AUV处在不同的环境而不同，当AUV发现威胁后，将分区域模式下，以AUV的方向角内且距离最近的目标区域点作为前向目标，即虚拟目标点，

goal(u)＝Task(x)

式中：u表示AUV的编号、x为满足最优条件的区域编号；

距离评价函数d(v,w)表示AUV在当前采样速度的航迹上与最近障碍物威胁的距离；

速度评价函数v(v,w)则用来评价当前采样速度的速率大小，

当AUV遇到复杂障碍物需要转艏时，当AUV艏向与方向角函数之差大于90°时，此时的评价函数s将会优先选择目标点的反向位置来避开威胁，

将得到的所有采样速度做归一化处理，即：

其中，n为所有采样的速度、i为待评价的当前采样速度。

Images