CN108318039B - 异介质无人驾驶航行器协同轨迹规划方法 - Google Patents

Abstract

异介质无人驾驶航行器协同轨迹规划方法，属于海洋应用中协同轨迹规划技术领域。本发明是为了解决水下无人航行器执行任务时，由于水下信号传播距离受限而无法与工作人员通信的问题。它包括：使水下航行器保持匀速等深运动，航向不变，到达指定位置；使无人机保持等高匀速运动，与水下航行器同时到达指定位置；无人机在到达指定位置前航向不变，到达指定位置后以水下航行器为中心进行圆周运动。本发明用于无人机和水下航行器的协同轨迹规划。

Description

异介质无人驾驶航行器协同轨迹规划方法

技术领域

本发明涉及异介质无人驾驶航行器协同轨迹规划方法，属于海洋应用中协同轨迹规划技术领域。

背景技术

目前在海洋应用领域中，诸如打捞、搜索、资源探测等任务大量依赖人工参与，任务执行的自动化、智能化不足，极大的影响了工作效率并提高了工作成本。采用水下无人航行器执行任务可在很大程度上解决这一问题，但对于远海任务，由于距离较远，水下信号传播距离受限，水下无人航行器和工作人员间的通信问题需要解决。

发明内容

本发明目的是为了解决水下无人航行器执行任务时，由于水下信号传播距离受限而无法与工作人员通信的问题，提供了一种异介质无人驾驶航行器协同轨迹规划方法。

本发明所述异介质无人驾驶航行器协同轨迹规划方法，它包括：

使水下航行器保持匀速等深运动，航向不变，到达指定位置；

使无人机保持等高匀速运动，与水下航行器同时到达指定位置；无人机在到达指定位置前航向不变，到达指定位置后以水下航行器为中心进行圆周运动；

所述无人机与水下航行器同时到达指定位置，并在到达指定位置后进行圆周运动的运动方法包括：建立运动数学模型；基于运动数学模型以约束条件的形式描述水下航行器和无人机的通信协同需求，建立优化模型；再采用优化算法求解获得无人机和水下航行器的出发时间差ΔT、初始航向角差Δψ、无人机飞行高度h_k及圆周运动半径R_w，从而实现对无人机和水下航行器的协同轨迹规划。

本发明的优点：本发明通过采用空中航行器(无人机)和水下航行器协同工作的模式，能使无人机作为通信媒介，间接实现水下航行器和基地工作人员间实时状态交互、指令及监视等目的。由于无人机和水下航行器运行速度大不相同，本发明通过优化算法完成协同航迹规划，能够保证指定海域内的无人机-航行器通信及无人机-人员通信，同时使指定任务指标达到最优，有利于实现海洋任务的自主化，降低人工成本和任务风险。

附图说明

图1是本发明所述异介质无人驾驶航行器协同轨迹规划方法的任务场景描述示意图；

图2是无人机与水下航行器协同轨迹规划的运动示意图；

图3是无人机与水下航行器到达指定位置前的纬度变化曲线；

图4是无人机与水下航行器到达指定位置前的经度变化曲线；图3和图4中A表示水下航行器，B表示无人机；横坐标为时间/秒；

图5是无人机到达指定位置后的圆周运动轨迹；

图6是无人机的全程轨迹；图5和图6中H/m表示深度/米。

具体实施方式

下面结合图1至图6对本发明的实施方式进行具体说明：

首先进行任务场景的描述：

从基地先后释放水下航行器和无人机，使二者在速度不同的情况下同时到达指定海域。此后无人机在水下航行器上空盘旋，始终保持与水下航行器的通信。为保持异介质航行器间的通信，无人机和航行器间的距离不得超过指定范围；同时，考虑地球曲率、海洋干扰等因素，无人机需高于海平面一定高度以保持和基地上工作人员的通信。

本实施方式所述异介质无人驾驶航行器协同轨迹规划方法，它包括：

使水下航行器保持匀速等深运动，航向不变，到达指定位置；

使无人机保持等高匀速运动，与水下航行器同时到达指定位置；无人机在到达指定位置前航向不变，到达指定位置后以水下航行器为中心进行圆周运动；

所述无人机与水下航行器同时到达指定位置，并在到达指定位置后进行圆周运动的运动方法包括：建立运动数学模型；基于运动数学模型以约束条件的形式描述水下航行器和无人机的通信协同需求，建立优化模型；再采用优化算法求解获得无人机和水下航行器的出发时间差ΔT、初始航向角差Δψ、无人机飞行高度h_k及圆周运动半径R_w，从而实现对无人机和水下航行器的协同轨迹规划。

本实施方式中首先可以将任务场景转化为数学模型。首先建立航行器运动模型，包括运动学模型和动力学模型；然后建立优化模型，包括设计变量、约束条件和性能指标等。其中，通信协同需求以约束条件的形式描述。

优化算法设计：根据建立的数学模型，设计优化算法实现协同优化问题的求解，同时考虑算法的实时性、鲁棒性、收敛性等。

运动模型：设水下航行器到达指定位置后保持静止；由于水下航行器运动规律相对固定，因此协同航迹规划主要针对空中无人机进行。

忽略地球自转和扁率，所述无人机在位置坐标系下的运动数学模型如下：

式中V为速度，γ为路径角，Y为法向控制力，m为无人机质量，σ为倾侧角，g为重力加速度，r为无人机到地心的距离，ψ为航向角，φ为纬度，θ为经度；

由于无人机保持等高匀速运动，水下航行器保持匀速等深运动，所以对于无人机和水下航向器而言，无人机到达指定位置前航向不变，

由无人机运动模型可知，决定无人机运动的实际上是法向控制力和倾侧角。为使无人机保持等高运动，需始终保持故：

无人机进行圆周运动时，设圆周半径为R_w，则：

由于无人机运动速度较低，故进行圆周运动时,无人机控制方程可写为：

将公式(4)变形为：

优化模型：

所述优化模型包括：

1)约束条件：

首先应保证水下航行器到达目标点时，与无人机的距离满足通信约束。

使无人机和水下航行器之间的距离L₁₂满足：

L₁₂＜L_max，(6)

L_max为无人机和水下航行器之间能够保持通讯所允许的最大直线距离；

无人机作圆周运动时，无人机和水下航行器之间的相对距离L₁′₂为：

使相对距离L₁′₂在L₁₂的取值范围内；h_s为水下航行器深度，h_k为无人机飞行高度；

其次考虑无人机控制系统能力，应对倾侧角和控制力进行约束。

根据无人机控制系统能力，对倾侧角σ和控制力N_Y进行约束如下：

式中N_Ymax为无人机控制力的最大值；

另外，为满足无人机和基地间的通讯，无人机飞行高度不得过低，以免由于地球曲率和海面干扰使通讯中断。设地球半径为R_e，基地和指定位置间的地心角为μ：

μ＝arccos(sinφ_fTsinφ_0M+cosφ_fTcosφ_0Mcos(θ_fT-θ_0M))，(9)

式中：θ_fT为指定位置经度，φ_fT为指定位置纬度，θ_0M为基地经度，φ_0M为基地纬度；

为满足地球曲率约束，使：

2)优化变量：

空中/水下单元分离后，各介质中的单元将分别在水下和空中运行。由于不同介质中单元的运动速度不同，因此首先选择无人机和水下航行器出发时间差(记为ΔT)为优化参量。

为实现后续的圆周运动，无人机初始航向应不同于水下航行器。设二者初始航向角差为Δψ，设定指定位置点距离无人机出发点距离为R₀，则：

选取另两个优化变量为无人机飞行高度h_k和圆周运动半径R_w。

3)优化指标：

为减小无人机的控制压力，由式(5)可知，圆周运动半径应尽量大；另外由于优化问题通常求解极小值，故选取优化指标J为：

J＝min N_Y。 (12)

优化算法：

所述优化算法为粒子群优化算法。

本发明的优化问题实际上是对ΔT和h_k的搜索，因此采用改进粒子群优化算法进行求解。

1)基本PSO算法

基本的PSO算法是一种基于迭代模式的优化算法，它的每一个解都是搜索空间中的一个“粒子”，每个粒子都有一个由优化函数所决定的适应度，以评价该粒子当前位置的优劣。

算法描述如下：假设种群的规模为N，搜索空间是D维的。粒子群中第i个粒子的位置表示为x_i＝(x_i1,x_i2,…,x_iD)(x_i中的粒子可以分别为出发时间差ΔT、初始航向角差Δψ、无人机飞行高度h_k及圆周运动半径R_w)，第i个粒子的速度表示为v_i＝(v_i1,v_i2,…,v_iD)，第i个粒子迄今为止搜索到的最好位置记为p_i＝(p_i1,p_i2,…,p_iD)，整个粒子迄今为止搜索到的最好位置记为p_g＝(p_g1,p_g2,…,p_gD)。

对于每一个粒子，其第j维(1<j<D)的位置和速度根据如下公式变化：

式中：c₁、c₂为学习因子，用来调节粒子向个体最优方向和全局最优方向飞行的步长，本文取c₁＝c₂＝2.0；r₁、r₂为[0,1]之间的随机数；w称为惯性权重，它决定了粒子先前速度对当前速度的影响程度，从而起到平衡算法全局搜索和局部搜索能力的作用；t为当前迭代次数。

大量实验表明，较大的惯性权重可以加强算法的全局搜索能力，反之可以加强算法的局部搜索能力。为了更好的提高算法的搜索性能，本文采用指数递减的惯性权重：

式中：w₀为起始权重，w_f为终端权重，d₁、d₂为控制因子，t_max为最大的迭代次数。

2)约束处理方法

利用随机性优化方法求解约束优化问题时，处理好约束条件是取得好的优化效果的关键。在此定义约束违反度函数：

显然，Φ(x)是所有违反约束的和，Φ(x)≥0并且Φ(x)＝0当且仅当x∈F。如果(ε为精度)，则解为不可行解。以为初始点，利用外点法进行求解，得到满足精度的解来代替继续进行优化。

外点法是求解约束优化问题最有代表性的算法之一，具有很好的收敛性质。外点法简单，易于实现，对目标函数和约束函数要求不高，适用范围较广。计算步骤如下：

2.1)选取M₁＞0，精度ε＞0，c≥0，初始点x⁽⁰⁾，令k＝1；

2.2)以x(^k-1)为初始点，求解无约束优化问题：

设其最优解为x^(k)＝x(M_k)；

2.3)令：

2.4)若T＜ε，则迭代结束，取x^*＝x^(k)；否则令M_k+1＝cM_k，k＝k+1，转步骤2.2)。

在某些情况下可行域的边界往往接近于搜索空间的边界，因此在迭代的过程中会出现一些粒子在可行域的边界违背约束。为了解决此问题，对位于搜索空间边界的粒子重新赋值：

式中：r∈U[0,1]。

3)寻优能力提升

寻优能力提升的关键在于避免早熟。无论是早熟收敛还是全局收敛，粒子都会出现早熟现象，要么所有粒子聚集在某一特定位置，要么聚集在某几个特定位置，这主要取决于问题本身的特性以及适应度函数的选择。为了定量描述粒子群的状态，下面引入群体适应度方差的定义。设粒子群的粒子数目为n，f_i为第i个粒子的适应度，f_avg为粒子群目前的平均适应度，δ²为粒子群的群体适应度方差，δ²可以定义为：

式中：f为归一化定标因子，其作用是限制δ²的大小。

f＝max{1,max(|f_i-f_avg|)},i∈[1,n]，(21)

群体适应度方差δ²反映的是粒子群中所有粒子的“聚集”程度，δ²越小，则粒子群的“聚集”程度越大，“聚集”将使群体失去多样性，从而陷入早熟收敛，故当δ²＜C(C为一给定常数)时，对当前最优个体粒子p_i进行随机扰动，扰动按如下方式进行：

式中：η是服从标准正态分布的随机变量，即η～N(0,1)。

3.仿真结果

设水下航行器速度为10m/s，深度100m；无人机速度为80m/s，质量200kg；出发点(基地)位于(E120°，N40°)，目标点位于(E120.2°，N40.2°)；σ_max＝60°，N_Ymax＝5。

在这种场景下，基地到目标点的距离为28km，对应最低的飞行高度为62m。设无人机和水下航行器的通信最大距离为500m。粒子数为20，最大迭代次数为20。

优化可得ΔT＝2483.2s，h_k＝65.7m，R_w＝465.4m，Δψ＝0.952°。其它结果如图3至图6所示。

Claims (3)

1.一种异介质无人驾驶航行器协同轨迹规划方法，其特征在于，它包括：

使水下航行器保持匀速等深运动，航向不变，到达指定位置；

使无人机保持等高匀速运动，与水下航行器同时到达指定位置；无人机在到达指定位置前航向不变，到达指定位置后以水下航行器为中心进行圆周运动；

所述无人机与水下航行器同时到达指定位置，并在到达指定位置后进行圆周运动的运动方法包括：建立运动数学模型；基于运动数学模型以约束条件的形式描述水下航行器和无人机的通信协同需求，建立优化模型；再采用优化算法求解获得无人机和水下航行器的出发时间差ΔT、初始航向角差Δψ、无人机飞行高度h_k及圆周运动半径R_w，从而实现对无人机和水下航行器的协同轨迹规划；

所述无人机的运动数学模型如下：

式中V为速度，γ为路径角，Y为法向控制力，m为无人机质量，σ为倾侧角，g为重力加速度，r为无人机到地心的距离，ψ为航向角，φ为纬度，θ为经度；

由于无人机保持等高匀速运动，无人机到达指定位置前航向不变，则有：

无人机进行圆周运动时，设圆周运动半径为R_w，则：

此时无人机控制方程为：

将公式(4)变形为：

2.根据权利要求1所述的异介质无人驾驶航行器协同轨迹规划方法，其特征在于，

所述优化模型包括：

使无人机和水下航行器之间的距离L₁₂满足：

L₁₂＜L_max，(6)

L_max为无人机和水下航行器之间能够保持通讯所允许的最大直线距离；

无人机作圆周运动时，无人机和水下航行器之间的相对距离L₁′₂为：

使相对距离L₁′₂在L₁₂的取值范围内；h_s为水下航行器深度，h_k为无人机飞行高度；根据无人机控制系统能力，对倾侧角σ和控制力N_Y进行约束如下：

式中N_Ymax为无人机控制力的最大值；

设地球半径为R_e，基地和指定位置间的地心角为μ：

μ＝arccos(sinφ_fTsinφ_0M+cosφ_fTcosφ_0Mcos(θ_fT-θ_0M))，(9)

式中：θ_fT为指定位置经度，φ_fT为指定位置纬度，θ_0M为基地经度，φ_0M为基地纬度；为满足地球曲率约束，使：

设定指定位置点距离无人机出发点距离为R₀，则：

选取优化指标J为：

J＝min N_Y。 (12)

3.根据权利要求2所述的异介质无人驾驶航行器协同轨迹规划方法，其特征在于，

所述优化算法为粒子群优化算法。