1.本发明属于水下滑翔机任务规划技术领域，具体涉及一种水下滑翔机动态运动规划方法。


背景技术：

2.自主式水下滑翔机(autonomous underwater glider)是一种以净浮力为驱动力的新型水下航行器，具有滑翔速度低，单周期滑翔时间长，能耗低等特点，被广泛地用于海洋观测、水下监控、海底勘探等科学研究。水下滑翔机单周期的滑翔过程主要包括：(1)准备下潜阶段。水下滑翔机收到下潜任务指令时，开始减小浮力，准备下潜；(2)稳态下潜阶段。控制系统保持水下滑翔机以固定的俯仰角稳态下潜；(3)上浮浮力调节阶段。当水下滑翔机到达目标深度时，开始浮力调节，移动滑块增大油馕体积进而转为上浮。(4)稳态上浮阶段。控制系统保持水下滑翔机以固定俯仰角稳态上浮。(5)浮出水面阶段。水下滑翔机浮出水面，通过卫星发送水下滑翔机当前状态，然后接收操作人员新的指令。
3.深平流指不同深度下水平方向海流的平均值。由于水下滑翔机动力小，滑翔速度低，其对海洋环境信息(海流、气象、障碍、地形等)非常敏感，因此在运动规划的过程中需要充分考虑海洋环境信息。尤其是海流信息，表面的海流往往比较容易获取，但是获取不同深度下的海流信息比较困难。同时，由于滑翔机能耗的限制和载荷量的限制，水下滑翔机往往不携带测流装置，因此，如何根据滑翔机特殊的运动方式来估计和预测深平流对滑翔机运动规划研究有重要的意义。本方法采用的是基于vmd
‑
pso
‑
lssvm的水下滑翔机深平流预测模型。该预测模型能够准确的估计和预测滑翔机未来多周期的深平流信息，为后续动态运动规划提供理论指导。
4.目前，水下滑翔机路径规划方法研究的非常成熟，无论是在时不变的海流环境还是在时变的海流环境中，路径规划都能取得不错的结果。然而，国内外针对水下滑翔机运动规划的研究内容较少。本方法提出了一种基于海流预测的水下滑翔机动态运动规划方法，结合了海洋环境模型和滑翔机能耗模型，规划了滑翔机每一周的运动参数：航向、深度和俯仰。动态运动规划有重要的研究意义，一方面，利用滑翔机每一周期的航向、深度和俯仰可以确定一条精细的三维路径；另一方面，详细的动作规划结果可作为滑翔机轨迹跟踪的控制期望，一定程度上减轻滑翔机控制压力。本方法适用于海流信息未知、障碍物未知的复杂海洋环境，通过异步双精度优化窗口策略，为滑翔机运动规划提供安全保障和降低能耗。


技术实现要素：

5.为提高水下滑翔机在复杂海洋环境中动态运动规划能力，本发明提供一种基于海流预测的水下滑翔机动态运动规划方法。首先，建立水下滑翔机的稳态滑翔模型和能耗模型，分别得出滑翔速度与净浮力和俯仰角之间的关系和滑翔机滑翔一周期的总能耗；然后利用深平流预测方法预测未来滑翔周期的海流信息；最后利用异步双精度优化窗口策略动态规划滑翔机每一周期的航向、深度和俯仰。本发明能够在海流信息未知、障碍物未知的复
杂海洋环境下，实现动态避障。
6.本发明为解决其技术问题所采用的技术方案包括以下步骤：
7.步骤1：建立水下滑翔机稳态滑翔模型；
8.通过对水下滑翔机稳态滑翔时的受力分析，可得出稳态滑翔速度是净浮力和滑翔角的函数：
[0009][0010]
式中，b表示水下滑翔机的净浮力，ξ表示滑翔角，k
l
和k
l0
表示水下滑翔机的升力系数，k
d
和k
d0
为水下滑翔机的阻力系数，升力系数和阻力系数由水下滑翔机的结构决定，为常值。
[0011]
水下滑翔机稳态下潜时的攻角为：
[0012][0013]
滑翔角与俯仰角和攻角的关系为：
[0014]
ξ＝θ
‑
α
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0015]
由于水下滑翔机的攻角很小，可以忽略不计，因此滑翔角近似等于俯仰角。水下滑翔机的滑翔速度可以表示为：
[0016][0017]
式中，v
h
表示稳态下潜的水平速度，v
z
表示稳态下潜的垂直速度，计算方法分别如下：
[0018][0019]
v
z
＝vsin(ξ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0020]
步骤2：建立水下滑翔机能耗模型；
[0021]
水下滑翔机能耗模型主要分为与时间相关的能耗和与时间无关的能耗。
[0022]
(1)与时间相关的能耗
[0023]
与时间相关的能耗主要包括控制与导航系统的能耗，测量传感器的能耗和定位与通讯系统的能耗。水下滑翔机滑翔一个周期的时间为：
[0024][0025]
式中，h表示水下滑翔机下潜深度。取这些能耗的平均功率为p
t
，则水下滑翔机滑翔一周期与时间相关的能耗为：
[0026]
e
t
＝p
t
·
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0027]
(2)与时间无关的能耗
[0028]
与时间无关的能耗主要包括浮力调节系统的能耗和姿态调节系统的能耗。水下滑翔机滑翔一周期浮力调节系统的能耗为：
[0029]
[0030]
式中，ρ为海水密度，g为重力加速度，p
c
为液压泵的流量，q
p
为液压泵的功率。
[0031]
姿态调节系统的能耗又可分为俯仰调节和横滚调节。由于滑翔机受海流影响比较大，很难稳定地控制横滚角，导致很难建立横滚调节系统能耗模型，因此本方法仅考虑俯仰调节系统的能耗。假设滑块移动的速度为v
m
，移动的电功率为p
m
，则水下滑翔机滑翔一个周期推动滑块的能耗为：
[0032][0033]
式中，r滑块偏离平衡点的距离。
[0034]
由上述分析可得，水下滑翔机滑翔一周期的总能耗为：
[0035]
e＝e
t
+e
h
+e
m
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0036]
步骤3：建立海流未知且时变的复杂海洋环境模型；
[0037]
为模拟时变海流环境，本方法利用多个涡流场函数建立时变海流场，设定涡流中心s0随着时间变化，虽然建立的海流环境和真实情况还有一定的差距，但主要目的是为了检验所建立的水下滑翔机模型和动态运动规划算法的有效性和正确性。涡流场函数为：
[0038][0039][0040]
式中，u
c
(s)和v
c
(s)分别为海流在x轴和y轴方向的分量，s0表示涡流中心，η和ζ分别表示涡流半径和涡流强度，s表示二维平面上的任意位置。
[0041]
步骤4：利用基于vmd
‑
pso
‑
lssvm的水下滑翔机深平流预测方法对滑翔机未来k个周期的深平流进行预测；
[0042]
水下滑翔机将历史滑翔周期计算得到的深平流数据看作时间序列，然后利用结合变分模态分解(vmd)、粒子群优化算法(pso)和最小二乘支持向量机(lssvm)的方法实现对海流的预测。深平流预测方法的详细过程见具体实施例。
[0043]
步骤5：建立海流影响下的水下滑翔机速度合成模型；
[0044]
根据步骤1建立的稳态滑翔模型可求出水下滑翔机在不同净浮力和俯仰角下的水平滑翔速度v
h
。假设海流的大小为v
cur
，方向为φ
cur
，v
h
的方向为φ
aug
。则海流影响下的水下滑翔机水平速度为：
[0045][0046]
步骤6：利用异步双精度优化窗口策略动态规划水下滑翔机每一周期的航向、深度和俯仰。
[0047]
异步双精度优化窗口策略由基于全局已知信息(地形、静态障碍等)的全局窗口和基于探测信息(台风、障碍等)的局部窗口组成。全局窗口和局部窗口采用不同的步长和滚动机制，更新频率和时刻均不同。全局窗口利用a*算法可为局部窗口提供航路点信息，局部窗口利用滚动优化的方式动态规划滑翔机每一周期的航向、深度和俯仰。步骤6的详细过程见具体实施例。
[0048]
由于采用了本发明提出的一种基于海流预测的水下滑翔机动态运动规划方法，带
来了如下有益效果：
[0049]
1、建立了准确的水下滑翔机模型分析方法：稳态滑翔模型和能耗模型；
[0050]
2、深平流预测和异步双精度优化窗口策略的结合可使在海流信息未知的复杂海洋环境中动态运动规划成为可能，既能为滑翔机提供安全保障又可以降低能耗。
[0051]
3、滑翔机动态运动规划结果可为滑翔机轨迹跟踪提供控制期望，在一定程度上减轻滑翔机控制压力。
附图说明
[0052]
图1为本发明方法稳态滑翔受力分析图。
[0053]
图2为本发明方法基于vmd
‑
pso
‑
lssvm的水下滑翔机深平流预测模型图。
[0054]
图3为本发明方法a*算法流程图。
[0055]
图4为本发明方法基于异步双精度优化窗口策略的动态运动规划流程图。
[0056]
图5为本发明方法水下滑翔机动态运动规划结果图。
[0057]
图6为本发明方法水下滑翔机在二维平面运动轨迹图。
[0058]
图7为本发明方法水下滑翔机在三维空间运动轨迹图。
[0059]
图8为异步双精度优化窗口策略。
具体实施方式
[0060]
下面结合附图和实施例对本发明进一步说明。
[0061]
为更好地实现水下滑翔机在复杂海洋环境中躲避障碍，安全地到达目标点，本发明提出了一种基于海流预测的水下滑翔机动态运动规划方法。在整个算法的实现过程中，需要建立水下滑翔机稳态滑翔模型和能耗模型。在动态运动规划过程中充分考虑了海洋环境信息对水下滑翔机的影响，利用异步双精度优化窗口策略为滑翔机规划了每一周期的航向、深度和俯仰。
[0062]
本发明提供一种基于海流预测的水下滑翔机动态运动规划方法包括以下步骤：
[0063]
步骤1：建立水下滑翔机稳态滑翔模型；
[0064]
通过对水下滑翔机稳态滑翔时的受力分析，可得出稳态滑翔速度是净浮力和滑翔角的函数：
[0065][0066]
式中，b表示水下滑翔机的净浮力，ξ表示滑翔角，k
l
和k
l0
表示水下滑翔机的升力系数，k
d
和k
d0
为水下滑翔机的阻力系数，升力系数和阻力系数由水下滑翔机的结构决定，为常值。
[0067]
水下滑翔机稳态下潜时的攻角为：
[0068][0069]
滑翔角与俯仰角和攻角的关系为：
[0070]
ξ＝θ
‑
α
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0071]
由于水下滑翔机的攻角很小，可以忽略不计，因此滑翔角近似等于俯仰角。水下滑翔机的滑翔速度可以表示为：
[0072][0073]
式中，v
h
表示稳态下潜的水平速度，v
z
表示稳态下潜的垂直速度，计算方法分别如下：
[0074][0075]
v
z
＝vsin(ξ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0076]
步骤2：建立水下滑翔机能耗模型；
[0077]
水下滑翔机能耗模型主要分为与时间相关的能耗和与时间无关的能耗。
[0078]
(1)与时间相关的能耗
[0079]
与时间相关的能耗主要包括控制与导航系统的能耗，测量传感器的能耗和定位与通讯系统的能耗。水下滑翔机滑翔一个周期的时间为：
[0080][0081]
式中，h表示水下滑翔机下潜深度。取这些能耗的平均功率为p
t
，则水下滑翔机滑翔一周期与时间相关的能耗为：
[0082]
e
t
＝p
t
·
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0083]
(2)与时间无关的能耗
[0084]
与时间无关的能耗主要包括浮力调节系统的能耗和姿态调节系统的能耗。水下滑翔机滑翔一周期浮力调节系统的能耗为：
[0085][0086]
式中，ρ为海水密度，g为重力加速度，p
c
为液压泵的流量，q
p
为液压泵的功率。
[0087]
姿态调节系统的能耗又可分为俯仰调节和横滚调节。由于滑翔机受海流影响比较大，很难稳定地控制横滚角，导致很难建立横滚调节系统能耗模型，因此本方法仅考虑俯仰调节系统的能耗。假设滑块移动的速度为v
m
，移动的电功率为p
m
，则水下滑翔机滑翔一个周期推动滑块的能耗为：
[0088][0089]
式中，r滑块偏离平衡点的距离。
[0090]
由上述分析可得，水下滑翔机滑翔一周期的总能耗为：
[0091]
e＝e
t
+e
h
+e
m
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0092]
步骤3：建立海流未知且时变的复杂海洋环境模型；
[0093]
为模拟时变海流环境，本方法利用多个涡流场函数建立时变海流场，设定涡流中心s0随着时间变化，虽然建立的海流环境和真实情况还有一定的差距，但主要目的是为了检验所建立的水下滑翔机模型和动态运动规划算法的有效性和正确性。涡流场函数为：
[0094]
[0095][0096]
式中，u
c
(s)和v
c
(s)分别为海流在x轴和y轴方向的分量，s0表示涡流中心，η和ζ分别表示涡流半径和涡流强度，s表示二维平面上的任意位置。
[0097]
步骤4：利用基于vmd
‑
pso
‑
lssvm的水下滑翔机深平流预测方法对滑翔机未来k个周期的深平流进行预测；
[0098]
水下滑翔机将历史滑翔周期计算得到的深平流数据看作时间序列，然后利用结合变分模态分解(vmd)、粒子群优化算法(pso)和最小二乘支持向量机(lssvm)的方法实现对海流的预测。深平流预测方法的详细过程见具体实施例。
[0099]
步骤5：建立海流影响下的水下滑翔机速度合成模型；
[0100]
根据步骤1建立的稳态滑翔模型可求出水下滑翔机在不同净浮力和俯仰角下的水平滑翔速度v
h
。假设海流的大小为v
cur
，方向为φ
cur
，v
h
的方向为φ
aug
。则海流影响下的水下滑翔机水平速度为：
[0101][0102]
步骤6：利用异步双精度优化窗口策略动态规划水下滑翔机每一周期的航向、深度和俯仰。
[0103]
异步双精度优化窗口策略由基于全局已知信息(地形、静态障碍等)的全局窗口和基于探测信息(台风、障碍等)的局部窗口组成。全局窗口和局部窗口采用不同的步长和滚动机制，更新频率和时刻均不同。全局窗口利用a*算法可为局部窗口提供航路点信息，局部窗口利用滚动优化的方式动态规划滑翔机每一周期的航向、深度和俯仰。步骤6的详细过程见具体实施例。
[0104]
具体实施例：
[0105]
本发明实施例以水下滑翔机在未知且时变海流环境中动态运动规划方法为例，对动态运动规划方法进行了验证：
[0106]
1、水下滑翔机稳态滑翔受力分析。
[0107]
水下滑翔机稳态下潜时的受力分析如图1所示，由力矩平衡可得：
[0108]
dcos(ξ)+lsin(ξ)＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0109]
‑
dsin(ξ)+lcos(ξ)＝b
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)式中，d为水下滑翔机受到的阻力，l为水下滑翔机受到的升力，b为水下滑翔机的净浮力，ξ为水下滑翔机的滑翔角。由(1)和(2)可得：
[0110]
l＝bcos(ξ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0111]
d＝
‑
bsin(ξ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0112]
水下滑翔机的水动力还可近似表示为：
[0113]
l＝(k
l0
+k
l
α)v2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0114]
d＝(k
d0
+k
d
α2)v2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0115]
根据公式(3)、(4)、(5)和(6)，可得稳态滑翔速度和稳态下潜的攻角分别为：
[0116][0117][0118]
2、建立基于vmd
‑
pso
‑
lssvm的水下滑翔机深平流预测模型。
[0119]
深平流预测模型如图2所示，主要包括以下步骤：
[0120]
(1)将深平流时间序列归一化，利用变分模态分解法(vmd)将其分解为m个子时间序列{x1,x2,
…
,x
m
}；
[0121]
(2)对每个子时间序列，先利用粒子群优化算法(pso)优化最小二乘支持向量机(lssvm)的正则化因子和核参数，然后使用优化后的预测模型pso
‑
lssvm对每个子时间序列x
i
进行预测，得到预测结果y
i
；
[0122]
(3)将各子时间序列的预测结果{y1,y2,
…
,y
m
}叠加并反归一化。最终得到深平流预测结果。
[0123]
3、建立基于异步双精度优化窗口策略的动态运动规划模型。
[0124]
异步双精度优化窗口策略的动态运动规划流程如图4所示，主要包括全局窗口和局部窗口。
[0125]
(1)全局窗口
[0126]
全局窗口基于已知的全局范围信息(地形、静态障碍)，利用a*算法，以能耗最低作为优化目标，进行全局路径规划，并为局部窗口运动规划提供参考。a*算法的原理可以描述为：
[0127]
f
n
＝g
n
+h
n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0128]
式中，f
n
表示从起点到终点的代价估计；g
n
表示从起点到当前点n的实际代价；h
n
表示从当前点n到目标状态的估计代价，用于启发搜索。a*算法的详细流程如图3所示。
[0129]
(2)局部窗口
[0130]
局部窗口利用基于探测的局部信息，进行滚动优化，利用a*规划的粗略轨迹来指导精细的运动参数规划：航向、深度和俯仰。运动参数的范围为：航向yaw∈[0
°
,360
°
]，深度depth∈[100m,500m]，俯仰pitch∈[15
°
,30
°
]。
[0131]
假设滑翔机的预测窗口为k个执行窗口，则水下滑翔机从当前位置到达预测窗口位置的坐标为：
[0132][0133][0134]
式中，x(n+k)表示滑翔机第n+k滑翔周期的x坐标，y(n+k)表示滑翔机第n+k滑翔周期的y坐标。表示海流影响下的第i周期水平滑翔速度，表示第i周期垂直滑翔速度。水下滑翔机滑翔一个周期的时间为为：
[0135]
[0136]
根据技术方案中的能耗模型分析，可计算每一个滑翔周期的能耗e
i
，滑翔机从当前位置到达预测窗口的位置的总能耗为：
[0137][0138]
假设从预测窗口位置到目标点的估计代价为：
[0139][0140]
式中：distance表示预测窗口位置到目标点的距离，e
n+k
表示第n+k周期的能耗。水下滑翔机从当前位置到目标点的能耗代价为：
[0141]
e＝l1·
e
p
+l2·
e
e
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0142]
式中，l1和l2表示权重系数。若在规划的过程中碰到障碍或碰到海底，能耗代价e为无穷大。在滚动优化过程中利用差分进化算法，优化出滑翔机从当前位置到达目标点位置能耗代价最低的运动参数组合：[yaw,depth,pitch]。然后水下滑翔机根据规划的结果[yaw,depth,pitch]滑翔一个周期，到达执行窗口后，继续进行局部窗口滚动优化，直到到达目标点。
[0143]
图5、图6和图7表示在海流未知且时变的复杂海洋环境下运动规划结果，规划结果验证了该方法的有效性。