一种基于近端策略优化算法的AUV动态避障方法与流程

一种基于近端策略优化算法的auv动态避障方法
技术领域
1.本发明涉及智能决策技术领域，具体为一种基于近端策略优化算法的auv动态避障方法。


背景技术：

2.自主水下航行器(autonomous undersea vehicle,auv)是一种轻型的水下探测工具，具有体积小、操控灵活及智能化程度高等特点，近年来在海洋资源探索、国防海军装备等领域大放异彩。海洋环境变化无常，为了保证auv的正常航行与探测必须研究出足够安全的避障技术，这也是其导航控制技术的重要组成部分。随着各国对海洋探索程度的日益加深，如何进一步提升auv在复杂海洋环境中的动态避障能力，已成为制约auv发挥功效的关键。
3.在auv的具体应用环境中，不确定场景的密集复杂动态障碍对auv的航行安全提出了巨大的挑战，传统的避障方法，如a*算法、人工势场法、voronoi图、rrt算法、群体智能算法等方法均是在已知环境信息的情况下进行避障。但由于环境状态的不确定性，auv无法提前获取动态障碍物的运动信息，因此很难将传统的方法应用到不确定环境中进行实时避障。此外，不确定环境的复杂多变性对auv的避障算法的时间提出了更高的要求。传统方法过于依赖环境动态模型和auv模型，这些模型的准确性会很大程度上影响传统方法的性能。简单的模型不能表征环境的复杂性，复杂的模型意味着计算量过于巨大，不但大量浪费计算资源，而且计算时间长，不能满足auv应用于不确定环境的需求。因此，就需要设计一种能够实现auv在不确定环境中的动态避障的方法。
4.随着人工智能的发展，越来越多的高级智能算法被应用于各个领域来解决传统算法无法解决的问题。其中，深度强化学习方法凭借其强大的高维度信息感知、理解以及非线性处理能力在智能决策算法中脱颖而出。近端策略优化算法基于强化学习技术，先前研究在其各自的环境中都取得了不错的效果，但是仍然存在一些不足，主要表现为以下两点：一是大多算法仅在静态环境中进行避障或路径规划，缺乏处理动态障碍的能力，很难应用于不确定环境中；二是因为其避障环境设定和对模型复杂度、计算量的考虑，仅能将深度强化学习算法应用于二维平面避障领域，与三维环境仍有不小差距，对指导现实应用有一定的局限性。


技术实现要素：

5.本发明的目的是针对现有技术的缺陷，提供一种基于近端策略优化算法的auv动态避障方法，以解决上述背景技术提出的问题。
6.为实现上述目的，本发明提供如下技术方案：一种基于近端策略优化算法的auv动态避障方法，具体步骤如下：
7.s1、根据三维前视声呐数据格式进行数据解析，每t秒生成一帧三维前视声呐图像，并对图像进行上伪色、降噪等处理；
8.s2、根据声强信息对声呐图像进行障碍物检测，计算障碍物相对于auv的位置信息，构建障碍物、auv和目标位置的绝对坐标三维地图；
9.s3、比较地图中当前帧和上一帧绝对坐标发生改变的障碍物，计算此动态障碍物在t秒内的位移、平均速度和方向信息，构建碰撞预估模型；
10.s4、使用训练好的近端策略优化算法，根据碰撞预估模型，控制三维地图中的虚拟auv在t秒内向着目标位置不断前行和试错，直至探索出一条安全避开障碍物的路径；
11.s5、auv使用自身的推进器，在t秒内按照三维地图中虚拟auv探索的路径前行，完成航行和避障动作；重复以上步骤，直至auv到达目标位置。
12.作为本发明的一种优选技术方案，所述s1中的数据解析，使用opencv处理声纳数据，使用applycolormap创建colormap_turbo伪彩色图像；降噪处理采用双边滤波去除噪声，结合图像的空间近端度和像素值相似度折中处理，同时考虑空域信息和灰度相似性，达到保边去噪的目的。
13.作为本发明的一种优选技术方案，所述s2中三维地图的构建，以主动航行功能开启时auv的位置为原点(0,0,0)，航向为y轴正方向，水平垂直于航向方向向右为x轴正方向，竖直于航向方向指向水面为z轴正方向构建三维坐标系。将根据声强信息检测到的障碍物识别框映射到地图中，并实时更新障碍物和auv的坐标信息。
14.作为本发明的一种优选技术方案，所述s3中假设障碍物在t秒内速度v
obs
、俯仰角θ
obs
和偏航角ψ
obs
是固定的，声呐测得障碍物上一帧坐标系位置为(x1,y1,z1)，障碍物当前帧位置为(x
obs
,y
obs
,z
obs
)，则障碍物航行的速度为：
[0015][0016]
偏航角为：
[0017]
ψ
obs
＝arctan((y1-y
obs
)/(x1-x
obs
))
[0018]
俯仰角为：
[0019][0020]
据此可以判断出障碍物的动态信息；存储以上信息，形成障碍物、目标位置和auv本身的绝对坐标三维地图。
[0021]
构建碰撞预估模型，首先计算碰撞距离；
[0022]
假设auv当前帧的位置为(x
auv
,y
auv
,z
auv
)，在完成一个step航行动作后坐标的移动量为(δx
auv
,δy
auv
,δz
auv
)，即完成一个step航行动作后auv的位置为(x
auv
+δx
auv
,y
auv
+δy
auv
,z
auv
+δz
auv
)，auv完成一个step航行动作所需时间为δt秒(δt为毫秒量级)；
[0023]
障碍物经过δt秒后在x轴移动量为：δx
obs
＝v
obs
δtcosθ
obs
cosψ
obs
[0024]
在y轴移动量为：δy
obs
＝v
obs
δtcosθ
obs
sinψ
obs
[0025]
在z轴移动量为：δz
obs
＝v
obs
δtsinθ
obs
[0026]
即δt秒后障碍物的坐标为(x
obs
+δx
obs
,y
obs
+δy
obs
,z
obs
+δz
obs
)；
[0027]
则经过δt秒后，auv与障碍物之间的距离为：
[0028][0029]
根据dist进行障碍物距离评分，得到障碍物距离奖励r
δt
。
[0030]
auv动态避障是一个连续的过程，当前step采取的航行动作会在很大程度上影响下一个动作，所以只关注于当前动作产生的效果往往不能顾全大局，影响整体的避障效果；
[0031]
同时考虑到物体运动的惯性，auv和动态障碍物都不太可能在很短的几十个δt秒内改变自身原有的速度和航向。所以不妨假设auv在接下来的几十个step都采取当前的航行动作，预估惯性运动带来的影响，从而计算一段整体的auv障碍物距离奖励g
mδt
：
[0032][0033]
式中，g
mδt
为m个step获得的总和障碍物距离奖励；r
nδt
为第n个step(即n个δt秒后)的障碍物距离奖励；γ为衰减因子，介于(0,1)之间，因为越近的r
δt
对算法影响更为重要，而越远的r
δt
因为其预估性使得准确度逐渐降低，γ的加入可以使碰撞预估模型的目光既不短浅也不会过于长远；
[0034]
本发明考虑到auv的计算性能，经过仿真实验模拟，最终取m＝30，γ＝0.95，即：
[0035][0036]
本发明的碰撞预估模型分为4个等级，分别是a(安全)、b(较小碰撞风险)、c(较大碰撞风险)、d(极度危险)，将g
30δt
代入以下公式得到auv对此障碍物的碰撞预估评级sq：
[0037][0038]
假设在同一帧声呐图像上识别到了q个障碍物，对这q个障碍物重复以上步骤，得到碰撞预估集s：
[0039]
s＝{s1，s2，s3，
…
，sq}。
[0040]
作为本发明的一种优选技术方案，所述s4中为了训练基于近端策略优化算法的深度强化学习网络模型，采用基于python开发的物理引擎pybullet进行仿真环境搭建，并依靠碰撞预估集s进行动态避障训练。
[0041]
作为本发明的一种优选技术方案，所述s5中auv使用自身的推进器沿着规划的路径前行，完成航行和避障动作。
[0042]
本发明的有益效果是：本方法使用三维前视声呐检测障碍物，通过构建碰撞预估模型，在执行一步动作后对未来的位置状态进行预估评级，增强auv对动态障碍物的敏感度，提升避障可靠性。同时将近端策略优化算法引入三维空间动态避障领域，克服了先前研究仅能将算法应用于二维平面静态避障的局限性，拓宽了算法的使用领域，同时取得了比
其他强化学习算法更好的避障效果。
附图说明
[0043]
图1为本发明的流程图；
[0044]
图2为本发明使用近端策略优化算法训练auv过程图；
[0045]
图3为本发明仿真训练环境图；
[0046]
图4为本发明每十轮的平均奖励图；
[0047]
图5为本发明到达目标位置所用步数图；
[0048]
图6为本发明算法训练模型规划路径图。
[0049]
图7为本发明基于不同奖励函数的训练情况对比图
[0050]
图8为本发明多动态障碍物场景平均奖励对比图。
[0051]
图9为本发明算法在多动态障碍物场景避障过程图。
具体实施方式
[0052]
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易被本领域人员理解，从而对本发明的保护范围做出更为清楚明确的界定。
[0053]
实施例：请参阅图1，本发明提供一种技术方案：一种基于近端策略优化算法的auv动态避障方法，包括以下步骤：
[0054]
s1、根据三维前视声呐数据格式进行数据解析，每t秒生成一帧三维前视声呐图像，并对图像进行上伪色、降噪等处理；
[0055]
s2、根据声强信息对声呐图像进行障碍物检测，计算障碍物相对于auv的位置信息，构建障碍物、auv和目标位置的绝对坐标三维地图；
[0056]
s3、比较地图中当前帧和上一帧绝对坐标发生改变的障碍物，计算此动态障碍物在t秒内的位移、平均速度和方向信息，构建碰撞预估模型；
[0057]
s4、使用训练好的近端策略优化算法，根据碰撞预估模型，控制三维地图中的虚拟auv在t秒内向着目标位置不断前行和试错，直至探索出一条安全避开障碍物的路径；
[0058]
s5、auv使用自身的推进器，在t秒内按照三维地图中虚拟auv探索的路径前行，完成航行和避障动作；重复以上步骤，直至auv到达目标位置。
[0059]
上述s1中，本发明使用opencv处理声纳数据。原始图像为灰度图像，为便于分析与观测，一般按照一定的色表进行像素映射，从而得到较为直观的彩色图像。本发明使用opencv的applycolormap创建colormap_turbo伪彩色图像。
[0060]
由于各类干扰源的存在，三维前视声纳的原始图像存在一定的条纹干扰与像素空洞，一般为采样数据丢失或回波数据丢失造成的。本发明采用双边滤波去除噪声，结合图像的空间近端度和像素值相似度折中处理，同时考虑空域信息和灰度相似性，达到保边去噪的目的。
[0061]
上述s2中，以主动航行功能开启时auv的位置为原点(0,0,0)，航向为y轴正方向，水平垂直于航向方向向右为x轴正方向，竖直于航向方向指向水面为z轴正方向构建三维坐标系。将根据声强信息检测到的障碍物识别框映射到地图中，并实时更新障碍物和auv的坐标信息。
[0062]
上述s3中，假设障碍物在t秒内速度v
obs
、俯仰角θ
obs
和偏航角ψ
obs
是固定的，声呐测得障碍物上一帧坐标系位置为(x1,y1,z1)，障碍物当前帧位置为(x
obs
,y
obs
,z
obs
)，则障碍物航行的速度为：
[0063][0064]
偏航角为：
[0065]
ψ
obs
＝arctan((y1-y
obs
)/(x1-x
obs
))
[0066]
俯仰角为：
[0067][0068]
据此可以判断出障碍物的动态信息；
[0069]
存储以上信息，形成障碍物、auv和目标位置的绝对坐标三维地图。构建碰撞预估模型，首先计算碰撞距离。
[0070]
假设auv当前帧的位置为(x
auv
,y
auv
,z
auv
)，在完成一个step航行动作后坐标的移动量为(δx
auv
,δy
auv
,δz
auv
)，即完成一个step航行动作后auv的位置为(x
auv
+δx
auv
,y
auv
+δy
auv
,z
auv
+δz
auv
)，auv完成一个step航行动作所需时间为δt秒(δt为毫秒量级)；
[0071]
障碍物经过δt秒后在x轴移动量为：δx
obs
＝v
obs
δtcosθ
obs
cosψ
obs
[0072]
在y轴移动量为：δy
obs
＝v
obs
δtcosθ
obs
sinψ
obs
[0073]
在z轴移动量为：δz
obs
＝v
obs
δtsinθ
obs
[0074]
即δt秒后障碍物的坐标为(x
obs
+δx
obs
,y
obs
+δy
obs
,z
obs
+δz
obs
)；
[0075]
则经过δt秒后，auv与障碍物之间的距离为：
[0076][0077]
根据dist进行障碍物距离评分，得到障碍物距离奖励r
δt
。本发明设定安全距离为5米、一般距离为3.5米、危险距离为2米，则auv障碍物距离奖励r
δt
为：
[0078][0079]
auv动态避障是一个连续的过程，当前step采取的航行动作会在很大程度上影响下一个动作，所以只关注于当前动作产生的效果往往不能顾全大局，影响整体的避障效果；
[0080]
同时考虑到物体运动的惯性，auv和动态障碍物都不太可能在很短的几十个δt秒内改变自身原有的速度和航向。所以不妨假设auv在接下来的几十个step都采取当前的航行动作，预估惯性运动带来的影响，从而计算一段整体的auv障碍物距离奖励g
mδt
：
[0081]
[0082][0083]
式中，g
mδt
为m个step获得的总和障碍物距离奖励；r
nδt
为第n个step(即n个δt秒后)的障碍物距离奖励；γ为衰减因子，介于(0,1)之间，因为越近的r
δt
对算法影响更为重要，而越远的r
δt
因为其预估性使得准确度逐渐降低，γ的加入可以使碰撞预估模型的目光既不短浅也不会过于长远；
[0084]
本发明考虑到auv的计算性能，经过仿真实验模拟，最终取m＝30，γ＝0.95，即：
[0085][0086]
本发明的碰撞预估模型分为4个等级，分别是a(安全)、b(较小碰撞风险)、c(较大碰撞风险)、d(极度危险)，将g
30δt
代入以下公式得到auv对此障碍物的碰撞预估评级sq：
[0087][0088]
假设在同一帧声呐图像上识别到了q个障碍物，对这q个障碍物重复以上步骤，得到碰撞预估集s：
[0089]
s＝{s1，s2，s3，
…
，sq}
[0090]
上述s4中，为了训练基于近端策略优化算法的深度强化学习网络模型，本发明采用基于python开发的物理引擎pybullet进行仿真环境搭建。
[0091]
基本参数设置如下：
[0092]
状态空间定义为：s
t
＝(x
auv
,y
auv
,z
auv
,dist
end
,step,s)，其中(x
auv
,y
auv
,z
auv
)为auv当前帧的位置，dist
end
为auv和目标位置间的距离，step为已采取航行动作的步数，s为碰撞预估集。
[0093]
为了加快网络模型收敛的速度，动作空间由11个离散的动作组成：a
t
＝(a0,a1,a2,
…
,a
10
)。其中a0为静止不动，a1、a2、a3、a4分别为向+x轴、+y轴、-x轴、-y轴方向前进0.1m，a5、a6、a7、a8分别为向+x轴偏+y轴45
°
、+y轴偏-x轴45
°
、-x轴偏-y轴45
°
、-y轴偏+x轴45
°
方向前进0.07m，a9、a
10
分别为向+z轴和-z轴前进0.07m；+和-表示正反向和反方向。
[0094]
深度强化学习算法中，所有目标都可以通过预期累积奖励的最大化来描述。auv可以通过与环境互动得到的反馈信号来学习正确的策略。
[0095]
奖惩函数是决定深度强化学习网络模型能否成功收敛的关键。本发明的奖惩函数r主要由三部分组成，分别为dist
end
变化奖惩r1、碰撞预估奖惩r2和到达、越界、发生碰撞奖惩r3。
[0096]
r1表示auv执行一个step动作之后，如果距离目标位置比执行动作之前更近，则给予适当奖励，反之则给予惩罚；r2表示根据s里的每一个评级sq给予碰撞预估奖惩；r3表示
auv到达目标位置则给予完成奖励，如果坐标超过划定边界或发生碰撞则给予失败惩罚。
[0097]
奖惩函数设计如下：
[0098]
r＝r1+r2+r3[0099][0100][0101][0102]
其中predist
end
表示auv执行动作之前和目标位置间的距离。
[0103]
通过碰撞预估集给予适当的安全奖励和严厉的危险动作惩罚可以使算法偏向于做出安全的避障动作。
[0104]
为防止auv一直无法到达目标位置，本发明设置了一个根据地图的大小发生变化的地图最大限制步数σ：
[0105]
σ＝λ*(l*w*h)
[0106]
其中，l、w、h为地图的长、宽、高，λ为一个与地图复杂程度有关的参数，复杂的地图下应设置更大的λ。
[0107]
当r≥30000或者r≤-10000或者step数量≥σ会直接结束本轮episode。
[0108]
下面简述近端策略优化算法的原理：
[0109]
近端策略优化算法的原理是将策略参数化，即π
θ
(a|s)，利用线性函数的参数化或者神经网络来表示策略。本发明使用近端策略优化算法训练auv过程如图2所示。
[0110]
近端策略优化算法策略梯度通过计算估计量结合随机梯度上升算法来实现，更新公式为：
[0111][0112]
其中，θb为更新前的策略参数，θr为更新后的策略参数，α为学习率，为重要性权重。j为优化目标，即在状态s下的未来奖励的期望值。
[0113]
策略梯度算法最大优势是可以在连续空间中选择动作，缺点是它对步长敏感却很难选择合适的步长。近端策略优化算法通过以下公式将新旧策略网络的动作输出概率的变化范围r(θ)限制在一定区域内：
[0114][0115]
近端策略优化算法的目标函数表达式为：
[0116][0117]
[0118]
其中，ε为截断常数用来辅助设定策略更新的范围，通常设置为0.1或0.2；为优势函数，q(sr,ar)为在状态sr下采取动作ar累积奖励值，v(sr,ar)为状态估计值。当时，说明此动作比平均动作要好，所以增大选择该动作的概率；当时，说明此动作比平均动作要差，所以减少选择此动作的概率，但是网络得到的动作的概率分布不能差太远，因此分别在1+ε和1-ε处截断，限制策略更新的幅度。
[0119]
下面简述仿真训练环境的搭建：
[0120]
如图3所示，训练环境长、宽、高分别为55m、18m、14m。红线为边界线，绿线为目标位置，橙线为auv每40个step生成的一段航行轨迹。
[0121]
auv首先要穿过三个立柱，然后穿过五个横向的静态障碍物。接着需要穿过两个左右运动和一个上下运动的动态障碍物，障碍物做匀速往返直线运动。
[0122]
分别记录训练过程中每十轮获得的平均奖励和auv每一次到达目标位置所用的步数，如图4和图5所示。随着迭代轮数的增加，在算法迭代到4000轮左右时，平均奖励已经从负值提升至0，说明近端策略优化算法已经学习到部分避障经验；在算法迭代到第5000轮时，每十轮的平均奖励基本在20000左右波动。平均奖励没能收敛在30000以上是因为算法成功率并不是百分之百，失败的尝试会拉低每十轮的平均奖励。从图5可以得出在auv初次到达目标位置之后，所用步数逐渐下降，在到达目标位置600次之后，路径长度基本趋于稳定并维持在720步上下波动，说明近端策略优化算法基本趋于收敛状态。
[0123]
图6为利用近端策略优化算法训练模型规划的路径，可以看出模型已学习到趋向目标位置和动态避障功能，路径平滑度较高。
[0124]
在相同实验环境中，本发明将奖励函数分为两种情况进行对比：第一、完整的奖惩机制，即r＝r1+r2+r3；第二、不使用碰撞预估模型，即r＝r1+r3。
[0125]
图7中蓝色线表示第一种训练情况，橙色线表示第二种情况。图中可直观地看到蓝色线条在更少的迭代次数达到较优的累计奖励值，在训练4500次时平均奖励就已达到20000，而在不使用碰撞预估模型的情况下，auv在训练5000次时平均奖励达到10000。实验结果表明增加碰撞预估模型对auv训练更有效率，加快auv对环境的探索。
[0126]
针对更加复杂的多动态障碍物场景，分别基于dqn算法、a2c算法、trpo算法和本发明算法模型实现auv动态避障任务，对照在相同场景下获得的平均奖励和到达目标位置所用步数。
[0127]
多动态障碍物场景由7个做往返直线运动的正方体构成，它们的航向、速度都不同。图8为在多动态障碍物环境下dqn算法、trpo算法和本发明算法模型所获得的每十轮平均奖励，a2c算法经过多次训练尝试均陷入局部最优值而无法收敛。可以看出本发明算法相对于dqn算法和trpo算法在前期训练过程中波动较小，三种算法均在6000轮左右开始收敛。本发明算法能够在碰撞预估模型的帮助下累积更多奖励，在训练后期每十轮平均奖励收敛在22000上下波动，而dqn算法和trpo算法分别收敛在10000和15000，说明本发明算法模型性能高、稳定性强且具备更好的泛化能力。
[0128]
图9给出了本发明算法在多动态障碍物场景避障过程图。从图中可明显看出auv为了躲避正方体障碍物做出机动动作，始终和障碍物保持合理安全距离，在驶向目标位置的过程中完成避障任务，路径较平滑且无大幅度转向，没有过多冗余路段。
[0129]
上述s5中，auv使用自身的推进器沿着规划的路径前行，完成航行和避障动作。
[0130]
本发明使用近端策略优化算法控制地图中虚拟的auv探索避障路径，而不是直接控制真实的auv，这样做可以使避障方法和auv的推进系统解耦合。只要auv的推进系统能够受控制按照地图中的路径前行，就可以使用本发明的避障方法，而不论其有几个推进器、怎样的推进方式，极大提升了算法的泛化能力。
[0131]
以上实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。