一种基于能耗最优的多水下滑翔机路径规划系统

1.本发明涉及水下滑翔机路径规划领域，尤其涉及一种基于能耗最优的多水下滑翔机路径规划系统。


背景技术：

2.当今时代，海洋资源越来越得到各国的重视。水下滑翔机因其具有小型化、轻量化、智能化等特点，是人们探索、开发和利用海洋的一种重要工具。水下滑翔机的路径规划、协调与控制在许多方面是一个新型的研究课题。在水下滑翔机自主航行过程中，由于海上和水下实时通信技术难以突破，现阶段路径规划对水下滑翔机航行安全和节省能耗来说十分重要。因此，设计一种基于能耗最优的水下滑翔机集群路径规划方法是十分必要的。
3.在水下滑翔机集群路径规划方面，国内外学者已经取得了一些进展。为实现水下滑翔机集群的路径规划，现有路径规划方法任然存在以下不足：
4.第一，现有研究结果仅适用于单水下滑翔机的路径规划，未考虑面向水下滑翔机集群的路径规划方法。
5.第二，现有研究结果研究了水下滑翔机集群的最短航行路径问题，但未针对水下滑翔机集群低航速、弱导航的特点综合考虑海流影响和避障避碰等特殊问题，无法满足水下滑翔机集群组网协同观测的实际需求。
6.针对水下滑翔机对路径曲率约束的要求，结合水下滑翔机运动学和动力学特性，探索对规划的三维路径进行平滑处理和参数化处理的方法，也是圆碟形水下滑翔机集群路径规划需要考虑的问题。


技术实现要素：

7.本发明提供一种基于能耗最优的多水下滑翔机路径规划系统，以克服上述技术问题。
8.一种基于能耗最优的多水下滑翔机路径规划系统，其特征在于，包括水下滑翔机能耗设计模块、水下滑翔机全局路径规划模块、水下滑翔机局部路径规划模块，
9.所述水下滑翔机能耗设计模块用于计算水下滑翔机在单个滑翔周期的能耗，包括浮力调节所需的能耗、姿态调节所需的能耗和通信定位所需的能耗和水下滑翔机的传感器在单个滑翔周期内连续工作所需的能耗；
10.所述水下滑翔机全局路径规划模块用于通过粒子群算法获取水下滑翔机在整个航程中所需经过的路径点集合，通过水下滑翔机能耗设计模块计算航程所需的总能耗，将总能耗作为粒子的适应度值，将路径点是否出现在碍航物区域作为惩罚函数，根据总能耗最低时的路径点集合获取每艘水下滑翔机的路径；
11.所述水下滑翔机局部路径规划模块用于根据每艘水下滑翔机的路径，判断不同水下滑翔机之间是否出现碰撞，若出现碰撞，则根据人工势场法重新计算任一水下滑翔机的路径点，水下滑翔机根据重新计算的路径点航行，当重新计算的路径点与路径中的路径点
重合，表示碰撞解除，将重新计算后的路径点集合更新在路径中。
12.优选地，所述计算水下滑翔机在单个滑翔周期的能耗是通过公式(1)计算，
[0013][0014]
其中，e
t
是单个滑翔周期产生与滑翔时间有关的能耗，e
fd
为下潜抽油阶段能耗，上浮排油阶段能耗e
fu
，ec是单个滑翔周期产生的通信定位能耗，ez为上浮或下潜姿态调节系统产生的能耗，∵为航迹角，bd为下潜净浮力，pv和pa分别为电磁阀功率和气泵功率，qv和qa分别为液压阀流量和气泵流量，ρ为海水密度，g为重力加速度，q
p
为液压泵排量，η
p
为液压泵效率，h为下潜深度，ue、i0和ηm分别为液压泵拖动电机的额定电压、额定电流和效率，bu为上浮净浮，pm为滑块拖动电机功率，v
p1
为滑块移动速度，v
p1
为滑块移动速度，pc和tc分别为通信功率和通信时间，p
t
为连续工作制元器件的平均功率，vg为水下滑翔机的实时航速，dd表示海面附近的浮力、姿态的调节次数和通信定位次数，du表示最大工作水深附近的浮力和姿态调节次数，假设水下滑翔机在水平滑翔距离s下的运动位于同一剖面，且各剖面内最大工作水深h相同，则水下滑翔机航行的周期数c＝s tan∵/(2h)，m为粒子群的维度。
[0015]
优选地，所述将路径点是否出现在碍航物区域作为惩罚函数是指通过公式(2)建立，
[0016][0017]
其中，表示路径起始点至路径终点之间在海面附近的浮力、姿态的调节次数和通信定位次数；表示路径起始点至路径终点之间在最大工作水深附近的浮力和姿态调节次数；为水下滑翔机航行周期数；为路径起始点至路径终点之间的水平距离；sf表示碍航物所在区域，为路径点集合，t为迭代次数，i表示粒子，m为粒子的维度，e
t
是单个滑翔周期产生与滑翔时间有关的能耗，e
fd
为下潜抽油阶段能耗，上浮排油阶段能耗e
fu
，ec是单个滑翔周期产生的通信定位能耗，ez为上浮或下潜姿态调节系统产生的能耗。
[0018]
优选地，所述判断不同水下滑翔机之间是否出现碰撞是指分别计算每艘水下滑翔机经过不同路径点的时间，以水下滑翔机经过路径点的最早时间作为航行时间的开始时间，以水下滑翔机经过路径点的最晚时间作为航行时间的结束时间，将航行时间划分为时间间隔，计算在时间间隔内所有水下滑翔机之间的距离，若水下滑翔机之间的距离小于阈值，表示水下滑翔机之间会出现碰撞。
[0019]
优选地，所述根据人工势场法重新计算任一水下滑翔机的路径点是指选取任一水下滑翔机作为调整滑翔机，根据碰撞发生前水下滑翔机的路径点，计算调整滑翔机与其它滑翔机之间的斥力，以调整滑翔机发生碰撞的路径点作为参考位置，计算参考位置对调整滑翔机产生的引力，根据斥力和合力计算调整滑翔机的路径点。
[0020]
本发明提供一种基于能耗最优的多水下滑翔机路径规划系统，与现有海洋航行器路径规划研究结果相比，路径规划系统的优点如下：
[0021]
第一，相比于现有的路径规划方法仅适用于单水下滑翔机，本发明提出了多水下滑翔机路径规划系统，并且采用matlab内置的pchip插值法对规划路径进行平滑，符合水下滑翔机运动学特性。
[0022]
第二，相比于现有的路径规划方法仅能在二维水平面进行路径规划，本发明能够满足水下滑翔机集群开展三维海洋观测的路径规划需求。
[0023]
第三，相比于现有的路径规划方法未考虑路径规划的安全问题，本发明采用惩罚函数实现避障功能，并采用人工势场法解决局部区域多水下滑翔机的避碰问题。
附图说明
[0024]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0025]
图1是本发明系统结构示意图；
[0026]
图2是本发明能耗最优的多水下滑翔机路径规划模块结构图；
[0027]
图3是本发明能耗最优的水下滑翔机全局路径规划模块结构图；
[0028]
图4是本发明能耗最优的水下滑翔机局部路径规划模块结构图；
[0029]
图5是本发明能耗最优的多水下滑翔机路径规划二维效果图；
[0030]
图6是本发明能耗最优的多水下滑翔机路径规划三维效果图。
具体实施方式
[0031]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0032]
图1为本发明系统结构示意图，如图1所示，本实施例的方法可以包括：
[0033]
一种基于能耗最优的多水下滑翔机(autonomous underwater glider，下称aug)路径规划系统，包括水下滑翔机能耗设计模块、水下滑翔机全局路径规划模块、水下滑翔机局部路径规划模块，
[0034]
所述水下滑翔机全局路径规划模块的输出端与水下滑翔机局部路径规划模块相连；所述水下滑翔机局部路径规划模块的输入端与水下滑翔机全局路径规划模块相连，其输出端输出水下滑翔机最优路径。
[0035]
所述水下滑翔机能耗设计模块用于计算水下滑翔机在单个滑翔周期的能耗，包括
浮力调节所需的能耗、姿态调节所需的能耗和通信定位所需的能耗和水下滑翔机的传感器在单个滑翔周期内连续工作所需的能耗。
[0036]
水下滑翔机的运动轨迹类似锯齿形，整个航程可分为若干个周期，θ、α和∵分别为俯仰角、攻角和航迹角。根据水下滑翔机的能量消耗方式，在单个定常运动周期内的总能耗可分为两部分：
[0037]
一部分与滑翔周期有关，与滑翔周期有关的能耗主要包括浮力调节系统产生的能耗、姿态调节系统产生的能耗和通信定位系统产生的能耗三部分；
[0038]
另一部分与滑翔时间有关，如测量传感器和嵌入式控制系统产生的能耗。
[0039]
下潜抽油阶段能耗e
fd
主要与电磁阀和气泵有关，将气体看做不可压缩的理想气体，则e
fd
可表示为：
[0040][0041]
其中bd为下潜净浮力，pv和pa分别为电磁阀功率和气泵功率，qv和qa分别为液压阀流量和气泵流量，ρ为海水密度，g为重力加速度。
[0042]
上浮时，浮力调节系统收到指令后，起动电机驱动液压泵将液压油从油箱排至外油囊。
[0043]
液压泵的输入功率p
p
为：
[0044][0045]
其中q
p
为液压泵排量，η
p
为液压泵效率，h为下潜深度。由此可得液压泵拖动系统消耗的功率pm为：
[0046][0047]
其中p
m0
、ue、i0和ηm分别为液压泵拖动电机的空载功率、额定电压、额定电流和效率，η
p
为液压泵的效率。
[0048]
由上述工作过程可知上浮排油阶段能耗e
fu
可表示为：
[0049][0050]
其中bu为上浮净浮力。
[0051]
由此可得，单个滑翔周期内浮力调节系统产生的能耗为：
[0052][0053]
水下滑翔机在上浮与下潜两种状态之间转换时，需要通过电机移动滑块来改变机身重心，由此产生能量消耗。上浮或下潜姿态调节系统产生的能耗为：
[0054][0055]
其中x
p1
为滑块偏离平衡位置的位移，m为水下滑翔机质量，h0为水下滑翔机的稳心
高度，m
p1
为滑块质量，pm为滑块拖动电机功率，v
p1
为滑块移动速度。
[0056]
单个滑翔周期产生的通信定位能耗为：
[0057]
ec＝pctcꢀꢀꢀ
(7)
[0058]
其中pc和tc分别为通信功率和通信时间。
[0059]
单个滑翔周期产生与滑翔时间有关的能耗为：
[0060][0061]
其中连续工作制元器件的平均功率为p
t
，vg为水下滑翔机实时航速。
[0062]
综合上述分析可知，水下滑翔机在整个航程的能量消耗模型为：
[0063][0064]
其中dd表示海面附近的浮力、姿态的调节次数和通信定位次数，du表示最大工作水深附近的浮力和姿态调节次数。假设水下滑翔机在水平滑翔距离s下的运动位于同一剖面，且各剖面内最大工作水深h相同，则水下滑翔机航行的周期数c＝s tan∵/(2h)。
[0065]
图2是本发明能耗最优的多水下滑翔机路径规划模块结构图，如图2所示，系统进行路径规划时，先进行全局规划，后进行局部规划。
[0066]
所述水下滑翔机全局路径规划模块用于通过粒子群算法获取水下滑翔机在整个航程中所需经过的路径点集合，通过水下滑翔机能耗设计模块计算航程所需的总能耗，将总能耗作为粒子的适应度值，将路径点是否出现在碍航物区域作为惩罚函数，根据总能耗最低时的路径点集合获取每艘水下滑翔机的路径。
[0067]
图3是本发明能耗最优的水下滑翔机全局路径规划模块结构图，如图3所示，粒子群算法属于更新随机解的迭代方法，本发明采用自适应权重量子粒子群优化算法，适应度函数值是此类方法中评价生成路径质量的标准，这里将水下滑翔机在整个航程的能量消耗模型作为适应度函数对生成路径的质量进行判断。以求解维度为m的粒子i在第t次迭代产生的路径点集合为例，粒子的适应度值为路径起始点至路径终点的能量消耗，考虑到路径点不能出现在碍航物区域，加入了惩罚函数，则有：
[0068][0069]
其中表示路径起始点至路径终点之间在海面附近的浮力、姿态的调节次数和
通信定位次数；表示路径起始点至路径终点之间在最大工作水深附近的浮力和姿态调节次数；为水下滑翔机航行周期数；为路径起始点至路径终点之间的水平面路径距离；sf表示碍航物所在区域。
[0070]
所述水下滑翔机局部路径规划模块用于根据每艘水下滑翔机的路径，判断不同水下滑翔机之间是否出现碰撞，若出现碰撞，则根据人工势场法重新计算任一水下滑翔机的路径点，水下滑翔机根据重新计算的路径点航行，当重新计算的路径点与路径中的路径点重合，表示碰撞解除，将重新计算后的路径点集合更新在路径中。
[0071]
图4是本发明能耗最优的水下滑翔机局部路径规划模块结构图，如图4所示，经过全局路径规划可以获得每艘水下滑翔机从起始点到目标点的优化路径，根据相应路径可以计算出水下滑翔机经过各路径点的时刻，并以此来预测出水下滑翔机间可能存在碰撞的路径点。
[0072]
判断不同水下滑翔机之间是否出现碰撞是指分别计算每艘水下滑翔机经过不同路径点的时间，以水下滑翔机经过路径点的最早时间作为航行时间的开始时间，以水下滑翔机经过路径点的最晚时间作为航行时间的结束时间，将航行时间划分为时间间隔，计算在时间间隔内所有水下滑翔机之间的距离，若水下滑翔机之间的距离小于阈值，表示水下滑翔机之间会出现碰撞。
[0073]
根据人工势场法重新计算任一水下滑翔机的路径点是指选取任一水下滑翔机作为调整滑翔机，根据碰撞发生前水下滑翔机的路径点，计算调整滑翔机与其它滑翔机之间的斥力，以调整滑翔机发生碰撞的路径点作为参考位置，计算参考位置对调整滑翔机产生的引力，根据斥力和合力计算调整滑翔机的路径点。
[0074]
局部调整的目的是当判断碰撞可能发生时调用人工势场法进行局部路径规划，让优先级低的水下滑翔机提前调整航向来避让优先级高的水下滑翔机，所述优先级按照水下滑翔机的编号进行排序，以此来防止碰撞的发生。
[0075]
斥力势场可表示为：
[0076][0077]
其中ri和rj分别表示第i艘水下滑翔机和第j艘水下滑翔机的位置，第i艘的优先级低于第j艘，ρ(ri，rj)为两艘水下滑翔机之间的相对距离，ρ0为斥力场的影响范围，k
rep1
和k
rep2
的斥力场强度系数，则第i艘水下滑翔机所受的合斥力为：
[0078][0079]
引力部分可驱使水下滑翔机恢复于期望航行状态，将全局路径上匀速运动的参考位置p0设定为目标点，并对水下滑翔机产生引力作用，引力的表达式如下：
[0080][0081]
其中r0为路径上的参考位置的坐标，k
att
为引力系数。
[0082]
斥力和引力的合力为：
[0083]ftol
＝f
rep
+f
att
ꢀꢀꢀ
(14)
[0084]
通过斥力与引力的合成力可计算出下一位置点，直到合力将水下滑翔机拉回全局路径。
[0085]
水下滑翔机的运动轨迹类似锯齿形，水下滑翔机以下潜上浮为周期向前运动，一个下潜上浮周期的水平面路径距离为2h/tan∵，以第一个下潜上浮周期为例，对滑翔机三维路径进行解析。首先水下滑翔机在路径起点做下潜运动，经过h/tan∵水平面路径距离下潜至深度h，水下滑翔机此时由下潜状态调整为上浮状态，经过h/tan∵水平面路径距离上浮至水面，周期性运动至路径终点。
[0086]
本实例中采用式(1)-(14)所描述的路径规划模块，具体的包含参数如下：
[0087]
p
t
＝3w，pv＝1.728w，qv＝0.0114l/s，pa＝10w，qa＝0.2l/s，bd＝25.1n，ue＝12v，i0＝0.2a，ηm＝0.8，q
p
＝0.0021l/s，η
p
＝0.8，pm＝2w，m＝144.3kg，m
p1
＝4kg，v
p1
＝0.0005m/s，h0＝0.005m，pc＝2.6w，tc＝300s，∵＝35
°
，h＝80m，k
rep1
＝10，k
rep2
＝10，k
att
＝10
[0088]
仿真结果如图5-6所示。
[0089]
图5是本发明能耗最优的多水下滑翔机路径规划二维效果图，即水下滑翔机二维运动轨迹，图中的实线为第一艘水下滑翔机路径，虚线为第二艘水下滑翔机路径，虚线为第三艘水下滑翔机路径。从图中可以看出水下滑翔机在一定距离上避开了障碍物，并在一定程度上顺应海流到达终点。
[0090]
图6是本发明能耗最优的多水下滑翔机路径规划三维效果图，即水下滑翔机三维运动轨迹，图中的实线为第一艘水下滑翔机路径，虚线为第二艘水下滑翔机路径，虚线为第三艘水下滑翔机路径。图中可以看出水下滑翔机三维运动以上浮下潜为周期向前运动，在一定距离上避开了海底山，并在一定程度上顺应海流到达终点。
[0091]
表1是能耗最优的多水下滑翔机路径规划结果，第一艘水下滑翔机能耗为99362焦耳，第二艘水下滑翔机能耗为99994焦耳，第三艘水下滑翔机能耗为116940焦耳。
[0092]
表1能耗最优的多水下滑翔机路径规划结果
[0093][0094]
整体有的有益效果：
[0095]
本发明提供一种基于能耗最优的多水下滑翔机路径规划系统，与现有海洋航行器路径规划研究结果相比，路径规划系统的优点如下：
[0096]
第一，相比于现有的路径规划方法仅适用于单水下滑翔机，本发明提出了多水下
滑翔机路径规划系统，并且采用matlab内置的pchip插值法对规划路径进行平滑，符合水下滑翔机运动学特性。
[0097]
第二，相比于现有的路径规划方法仅能在二维水平面进行路径规划，本发明能够满足水下滑翔机集群开展三维海洋观测的路径规划需求。
[0098]
第三，相比于现有的路径规划方法未考虑路径规划的安全问题，本发明采用惩罚函数实现避障功能，并采用人工势场法解决局部区域多水下滑翔机g的避碰问题。
[0099]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。