CN112241176B - 一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 - Google Patents
一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 Download PDFInfo
- Publication number
- CN112241176B CN112241176B CN202011109095.2A CN202011109095A CN112241176B CN 112241176 B CN112241176 B CN 112241176B CN 202011109095 A CN202011109095 A CN 202011109095A CN 112241176 B CN112241176 B CN 112241176B
- Authority
- CN
- China
- Prior art keywords
- underwater
- autonomous vehicle
- obstacle avoidance
- underwater autonomous
- obstacle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000012549 training Methods 0.000 claims abstract description 62
- 230000009471 action Effects 0.000 claims abstract description 40
- 238000004088 simulation Methods 0.000 claims abstract description 29
- 230000006870 function Effects 0.000 claims abstract description 23
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000013528 artificial neural network Methods 0.000 claims abstract description 5
- 230000004888 barrier function Effects 0.000 claims description 24
- 238000005070 sampling Methods 0.000 claims description 21
- 230000002787 reinforcement Effects 0.000 claims description 19
- 238000001514 detection method Methods 0.000 claims description 18
- 238000011156 evaluation Methods 0.000 claims description 15
- 239000000126 substance Substances 0.000 claims description 7
- 238000013500 data storage Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 5
- 239000011435 rock Substances 0.000 claims description 5
- 230000003068 static effect Effects 0.000 claims description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 5
- 239000003795 chemical substances by application Substances 0.000 claims description 4
- 230000007613 environmental effect Effects 0.000 claims description 4
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 claims description 3
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000011478 gradient descent method Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000001960 triggered effect Effects 0.000 claims description 3
- YZCKVEUIGOORGS-NJFSPNSNSA-N Tritium Chemical compound [3H] YZCKVEUIGOORGS-NJFSPNSNSA-N 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 19
- 238000013461 design Methods 0.000 description 8
- 241000282414 Homo sapiens Species 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000000605 extraction Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000010391 action planning Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000004083 survival effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明是一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法。本发明涉及水下机器人路径避障规划技术领域，本发明建立存在大尺度连续性障碍物模拟训练环境，以避障传感器信息为输入，航行速度和偏航角速度为输出搭建深度强化学习神经网络的状态与动作，针对运动规划避障控制过程的多目标结构，对奖赏函数进行了模块化设计，为了避免稀疏奖励引起系统不稳定，结合人工势场法设置连续性奖励。本发明利用基于改进的深度确定性策略梯度算法，对水下自主航行器进行避障训练，将训练所得到的避障策略写入机器人下位机控制系统；水下自主航行器在水下峡谷中行驶时，利用训练学习到的避障策略进行避障，安全的到达目标区域。
Description
技术领域
本发明涉及水下机器人路径避障规划技术领域，是一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法。
背景技术
海洋是人类生命的摇篮，其中蕴藏着丰富的能源、生物资源和矿产资源，走向海洋逐渐成为人们的共识，海洋不仅关系到一个国家的兴衰发展，更与人类的生存紧密相连。伴随着海洋开发、海洋军事应用的需要，水下作业技术、作业装备的研究与研发需求日益紧迫。随着人类科技的进步，对海底世界的探索、海洋环境探测和资源调查监测逐渐成为目前海洋工程及海洋科学领域的研究热点课题。目前各国都相近提出由浅海迈向深海发展的战略方针，各个国家的海洋研究机构都把大量人力财力投向，水下自主航行器(水下自主航行器)的研究，因为水下自主航行器(水下自主航行器)是深海世界探索的最重要的手段，它能在水下代替人类完成某一特定任务，它可以搭载不同类型的设备进行水下甚至是海底进行高效率工作。
海洋深处是一种复杂多变的环境，分布着各种山脉，当水下自主航行器到达深海航行时，水下自主航行器面临着大大小小的水下峡谷，坚硬的谷壁等严重威胁着水下自主航行器安全。此外流体的密度和粘性影响着水下自主航行器在水下的运动；海流对水下自主航行器的运动也存在不确定性的干扰。水下自主航行器必须具备很高的自主避障控制能力才能完成深海勘测任务。因此水下自主航行器在水下峡谷中水下自主航行器自主避障研究是水下自主航行器领域重要技术之一。
深度确定性策略梯度算法具有良好的在线自适应性和对非线性系统的学习能力，在人工智能、机器学习和自动控制领域中得到了广泛的研究；将该算法应用于水下自主航行器的控制系统中实现水下自主航行器自主避障功能以提高其环境的自适应性，另外深度确定性策略梯度算法还可以改善其它规划方法的维数灾、规划时间长、精度低等问题，对水下自主航行器的水下峡谷安全航行具有重要的实际意义。
发明内容
本发明为实现水下自主航行器在水下峡谷环境中避开大型连续性静态障碍物岩壁，安全到达目标区域，本发明提供了一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，本发明提供了以下技术方案：
一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，包括以下步骤：
步骤1：建立水下自主航行器模型与运动学模型，获取水下机器人周围障碍物的信息；
步骤2：根据水下机器人周围障碍物的信息，在笛卡尔坐标系下建立水下峡谷模拟训练环境，生成避障策略；
步骤3：将生成的避障策略转化为MDP过程，并进行训练得到水下自主航行器连续性障碍物避障MDP模型的状态集；
步骤4：根据状态集进行深度强化学习训练，直至无碰撞到达目标区域，并保存避障策略；
步骤5：将深度强化学习后保存的避障策略，通过数据传输给下位机模块，指导控制模块融合传感器数据信息，实时计算出水下自主航行器偏航角及速度，此时的水下自主航行器偏航角及速度，引导水下自主航行器避开水下峡谷岩壁，并到达目标区域，完成航行任务。
优选地，所述步骤1具体为：
建立水平面3自由度水下自主航行器模型与运动学模型，设置7个避障声呐分别为Si，其中，i＝1,2,3…7，测距声纳采样频率为2Hz，探测距离为150m，通过避障声呐获取水下机器人周围障碍物的信息；
将水平面3自由度水下自主航行器模型与运动学模型进行简化，简化为等尺度的长方形，并用对称分布在长方形周边的7条虚线，随体坐标系下分布角度依次为θi，通过θi＝90°,60°,30°,0°,-30°,-60°,90°表示7个避障声呐，通过下式表示水平面3自由度水下自主航行器与运动学模型方程：
υ＝[u,v,r]T∈R3
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ)为转换矩阵，ψ为水下自主航行器的偏航角；u、v和r分别水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度。
优选地，所述步骤2具体为：
在笛卡尔坐标系下建立水下峡谷模拟训练环境，根据水下机器人周围障碍物的信息在上位机模块的界面及仿真系统模块中建立水下自主航行器水下峡谷环境二维模拟训练环境模型，并且在模拟环境搭建过程需要融合连续性静态障碍物避障方法，
确定水下自主航行器的声呐与连续障碍物表面的探测交点，通过Ti表示水下自主航行器的声呐与连续障碍物表面的探测交点，Di是水下自主航行器到T点的距离，通过控制Di≥ρs，ρs为水下自主航行器最小安全距离，实现水下自主航行器对连续性障碍物进行规避，生成避障策略。
优选地，所述步骤3具体为：
为了进行深度强化学习训练，将生成的避障策略转化为MDP模型，MDP模型由四元组构成，通过下式表示MDP模型：
MDP＝(S,A,Psa,R)
其中A表示动作集，S表示状态集，Psa表示状态转移概率，R为奖赏函数；
对MDP模型进行设定，所述MDP模型设定运动规划任务是水平的，有三个自由度；同时将时间作离散化处理，避障系统以采样率为TS＝0.5s的间隔定期输出，由此水下自主航行器在t时刻接收到状态信息后，输出动作μt∈A，产生的t时刻的奖赏值Rt＝f(st)，状态变为st+1；
Agent输出的动作ut由策略π决定，策略π为状态st映射到每个动作的概率：S→P(A)，得到水下自主航行器大尺度连续性障碍物避障MDP模型的状态集。
优选地，所述步骤4具体为：
步骤4.1：在仿真系统模块中，基于水下自主航行器大尺度连续性障碍物避障MDP模型的状态集，建立仿真试验平台，搭建python编译环境，编写分层强化学习训练模块，利用pyglet库建立水下峡谷海洋环境，建立水下自主航行器运动学模型和障碍物运动学模型，导入利用Python语言编写实现基于SumTree-DDPG的连续性障碍物环境避障控制器，设置水下自主航行器的初始参数及神经网络训练超参数，进行训练；
步骤4.2：进行训练：水下自主航行器在水下峡谷环境中根据初始速度和初始偏航角运动，并且水下自主航行器的7个声呐探测的环境数据作为深度强化学习的状态，当7个声呐探测的范围中没有障碍物，水下自主航行器允许继续学习探索，直到到达目标区域，结束该回合学习；
步骤4.3：当7个声呐探测的范围存在障碍物且探测线的最小距离小于安全距离，表明水下自主航行器与障碍物相撞，该回合结束，回到起点重新开始学习；
不断的循环学习，直到每个回合都是无碰撞到达目标区域而触发的回合结束，此时表明训练收敛，保存学习到的策略。
优选地，所述SumTree-DDPG避障控制器具体实现过程为：
步骤4.1.1：随机初始化评论家网络Q(s,a|θQ)和演员网络μ(s|θu)的权重参数θQ和θμ；初始化目标网络Q'和μ'，网络权重参数为θQ'←θQ,θμ'←θμ；初始化SumTree并定义容量为H＝φ，记忆库数据存储到达最大容量为lenMax(Data)＝M；设置初始位置为当前状态，到达目标区域为目标状态；
状态空间为水下自主航行器7个避障声呐的实时探测到与障碍物或目标的距离，确定在时刻t的状态，通过下式表示在时刻t的状态：
步骤4.1.2：初始化动作空间，在时刻t定义动作空间为偏航角速度ω(t)和水平速度V(t)，确定at,通过下式表示动作集at：
训练回合数Ep最大回合数10000初始化为Ep＝1；
步骤4.1.3：Ep回合中时间步t最大时间步2000为初始化t＝1；
步骤4.1.4：在线Actor策略网络根据当前状态st策略选择出包含水下自主航行器的偏航角速度及水平速度的动作集，通过下式表示当前状态下动作集：
at＝μ(st|θμ)+Nt；
根据输出的动作
结合水下自主航行器的水平面3自由度的运动学模型，得到微分式，通过下式表示微分式：
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ(t)为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ(t))为转换矩阵；ψ(t)为时间步t时水下自主航行器的偏航角，并且
为ψ(t)对时间步t的微分；u(t)，v(t)，r(t)分别时间步t时水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度；
根据四阶龙格-库塔法求解微分式，得到执行动作后的新位置向量η(t+1)，通过下式表示所述向量：
η(t+1)＝[x(t+1),y(t+1),ψ(t+1)]T∈R3
由执行动作后的新位置向量转到下一个状态st+1，获得即时奖励值rt；
将四元组(st,at,rt,st+1)存入SumTree结构H＝φ，当数据存储到达最大容量时，len(Data)＞M，从SumTree容量中H＝φ中依据采样概率
权重ωk＝(P(k)/minjP(j))-β采样小批量的N个经验样本
其中，
表示时间步t时的第k条经验样本，并且k＝1,2,…,N，N为小批量样本总数；
从SumTree容量中采样小批量的N个经验样本
构成数据集，发送给在线策略网络、目标策略网络、在线评价网络和目标评价网络；
根据采样的数据集，目标策略网络根据状态st+1输出动作a′t+1，计算目标Q值，记为yi，通过下式表示yi：
yi＝ri+γQ'(si+1,μ'(si+1|θμ')|θQ')；
目标评价网络根据状态st+1、目标策略网络输出的动作a′t+1和目标Q值的yi，更新损失函数更新评论家在线评价网络参数θ，通过下式进行在线评价:
其中，L为损失函数；
将小批量的N个经验样本结合随机梯度下降法，更新演员网络的策略和在线策略网络参数δ，通过下式进行更新:
其中，
为抽样策略梯度；
根据在线网络参数θ和δ以软更新的形式更新θ'和δ'：
步骤4.1.5：更新SumTree结构中每个节点的优先值，pk←|δj|；
步骤4.1.6：回合数用Ep表示，Ep＝1回合中时间步t＝t+1，重复运行步骤4.1.4到步骤4.1.5；
当在t≤2000时水下自主航行器进行探索过程碰撞障碍物或者到达目标区域转，回合数Ep＝Ep+1，重复运行步骤4.1.3到步骤4.1.6；
当Ep＝10000，水下自主航行器在大尺度连续性障碍物环境训练完成，保存学习后的避障策略。
优选地，即时奖励值rt具体计算步骤如下；
步骤4.2.1：抗海流干扰稳定性模块奖励值更新r1(st,at,st+1)←r1(st,at,st+1)-0.01×(|ωt+1-ωt|+|sin(ψt-φ)|)，当水下自主航行器从安全区域驶向的下一个状态仍然是安全区域，目标模块奖励值更新
其中，r1(st,at,st+1)表示时间步t时奖励值rt的第一个分量抗海流干扰稳定性模块奖励值，式中ωt、ωt+1分别为当前时刻与下一时刻水下自主航行器的偏航角速度，ψt、φ分别在笛卡尔坐标系下的为水下自主航行器当前时刻的偏航角、水流方向角；r3(st,at,st+1)表示时间步t时奖励值rt的第三个分量目标模块奖励值，d0和dt分别为在大地坐标系下水下自主航行器的起始点与目标区域中心处距离，以及在大地坐标系下水下自主航行器在时间步t时位置与目标区域中心处距离；
步骤4.2.2：当水下自主航行器从安全区域驶向的下一个状态不是安全区域，安全模块奖励值r2(st,at,st+1)更新，当不安全区域为碰撞峡谷谷壁，安全模块奖励值更新为：r2(st,at,st+1)←r2(st,at,st+1)-R并且重新回到起点探索，结束回合；
否则处于避碰峡谷谷壁的危险区域，当声纳波束探测到水下自主航行器与障碍物距离小于1.5倍安全距离大于安全距离时，对安全模块奖励值进行更新，通过下式进行更新：
r2(st,at,st+1)←r2(st,at,st+1)-(Dt-1.5×rs)2；
其中，r2(st,at,st+1)表示时间步t时奖励值rt的第二个分量安全模块奖励值，R为正常数，Dt为时间步t时7个避障声呐探索到水下自主航行器与大尺度连续性障碍物之间的欧氏距离欧式值中最小值，rs为水下自主航行器的最小安全距离；
步骤4.2.3：当下一个状态是抵达目标区域，目标模块奖励值更新r3(st,at,st+1)←r3(st,at,st+1)+R，结束回合；
其中，R为正常数；
步骤4.2.4：结束回合后，得到即时奖励值rt←rt+τ1r1(st,at,st+1)+τ2r2(st,at,st+1)+τ3r3(st,at,st+1)；
其中，τ1、τ2和τ3分别表示三个奖励模块的比例系数。
本发明具有以下有益效果：
本发明根据经验样本重要性的不同，结合SumTree结构对样本进行分类存储并不断抽取优质样本，提高了模型收敛速度。DDPG算法采用经验回放的方法，将Agent与环境交互产生的经验样本存放于经验缓冲池中，并从中随机抽取样本用来训练网络。这种随机抽取样本的方法，既没有考虑到不同数据所具有的不同的重要性，又没有充分考虑到被抽取样本应具有的多样性，导致模型收敛较慢。针对这一问题，本发明的样本存储和抽取策略，不同于原生DDPG算法的单个经验缓冲池随机抽取样本方法，而是按照数据重要性采取按优先级抽取的方法，有效提高了模型的收敛速度。
目前多数水下自主航行器路径规划系统都是靠人工在上位机点击选取路径节点的方法规划全局路径，并且不具备实时避障能力，并且面对大尺度连续性障碍物传统的方法也很难实现避障。该系统利用深度强化学习的算法对水下自主航行器进行避障训练，并且通过仿真训练得到的避障策略可以直接通过无线网络写入下位机模块，结合下位机的控制模块输出偏航角、速度实现规避障碍物，到达目标区域。由此该方法不仅具有全局规划能力而且具备实时避障能力。
在仿真系统模块中对水下自主航行器进行避障训练避免了机器人的碰撞损坏，并且通过仿真训练时直接结合了水下自主航行器的运动学模型，由此得到的避障策略可以直接应用于实际的机器人，不需要进行二次动作规划，相对于传统路径规划、动作规划分开的模式，该方法可节省一定的人力物力。
本发明提出的基于SumTree-DDPG算法的大尺度连续性障碍物环境避障控制器对水下自主航行器的连续动作的控制具有良好的鲁棒性，可以改变训练环境模型，将该控制器用于水下自主航行器其他类似控制领域，如：路径跟踪。并且其中的SumTree-DDPG算法不断地择优抽取样本，减少了系统学习时间，提高了学习效率，收敛速度快，该算法也可迁移解决类似的连续控制问题，如：机械臂控制。
针对水下自主航行器路径规划、避障控制运用深度强化学习训练时奖励函数设计困难，该发明通过模块化设计并结合人工势场法处理奖励稀疏问题。本发明的奖励工程同时考虑了障碍物、目标点及海流的因素，可以训练水下自主航行器避开大型连续性障碍物抵达目标点的同时，还考虑了海流等环境因素的干扰。并且奖励工程是模块化设计，本发明可以针对不同需求仅调节比例系数即可实现任务，如将距目标点距离的比例系数调为最大可以保证水下自主航行器人优先考虑到达目标点。
附图说明
图1为水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制系统流程图；
图2为用于水下峡谷航行的水下自主航行器示意图；
图3为水下峡谷避障模型示意图；
图4为SumTree结构图；
图5为基于SumTree-DDPG算法的大尺度连续性障碍物环境避障控制器；
图6为基于某海域的水下峡谷模拟环境仿真训练结果图。
具体实施方式
以下结合具体实施例，对本发明进行了详细说明。
具体实施例一：
根据图1至图6所示，本申请提供一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，包括以下步骤：
一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，包括以下步骤：
步骤1：建立水下自主航行器模型与运动学模型，获取水下机器人周围障碍物的信息；
所述步骤1具体为：
建立水平面3自由度水下自主航行器模型与运动学模型，设置7个避障声呐分别为Si，其中，i＝1,2,3…7，测距声纳采样频率为2Hz，探测距离为150m，通过避障声呐获取水下机器人周围障碍物的信息；如附图2所示是用于实现大尺度连续性障碍物环境中行驶的水下自主航行器的实际模型
将水平面3自由度水下自主航行器模型与运动学模型进行简化，简化为等尺度的长方形，并用对称分布在长方形周边的7条虚线，随体坐标系下分布角度依次为θi，通过θi＝90°,60°,30°,0°,-30°,-60°,90°表示7个避障声呐，通过下式表示水平面3自由度水下自主航行器与运动学模型方程：
υ＝[u,v,r]T∈R3
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ)为转换矩阵，ψ为水下自主航行器的偏航角；u、v和r分别水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度。
该型水下自主航行器有3个推进器一个尾推，2个侧推，可实现水下自主航行器的转艏、前进与后退。为了利于仿真训练，本发明将水下自主航行器简化为长方形，结合水下自主航行器的运动学模型保证了仿真训练系统训练出的策略可直接指导水下自主航行器的实际航行。
步骤2：根据水下机器人周围障碍物的信息，在笛卡尔坐标系下建立水下峡谷模拟训练环境，生成避障策略；
所述步骤2具体为：
如附图3所示为在笛卡尔坐标系下水下自主航行器和水下峡谷壁面的几何关系。附图3中黑色粗线部分代表不可穿透的连续障碍物峡谷壁面表面，G表示水下自主航行器的位置，在笛卡尔坐标系下建立水下峡谷模拟训练环境，根据水下机器人周围障碍物的信息在上位机模块的界面及仿真系统模块中建立水下自主航行器水下峡谷环境二维模拟训练环境模型，并且在模拟环境搭建过程需要融合连续性静态障碍物避障方法，
确定水下自主航行器的声呐与连续障碍物表面的探测交点，通过Ti表示水下自主航行器的声呐与连续障碍物表面的探测交点，Di是水下自主航行器到T点的距离，通过控制Di≥ρs，ρs为水下自主航行器最小安全距离，实现水下自主航行器对连续性障碍物进行规避，生成避障策略。
步骤3：将生成的避障策略转化为MDP过程，并进行训练得到水下自主航行器连续性障碍物避障MDP模型的状态集；
所述步骤3具体为：
为了进行深度强化学习训练，将生成的避障策略转化为MDP模型，MDP模型由四元组构成，如图3所示，通过下式表示MDP模型：
MDP＝(S,A,Psa,R)
其中A表示动作集，S表示状态集，Psa表示状态转移概率，R为奖赏函数；
对MDP模型进行设定，所述MDP模型设定运动规划任务是水平的，有三个自由度；同时将时间作离散化处理，避障系统以采样率为TS＝0.5s的间隔定期输出，由此水下自主航行器在t时刻接收到状态信息后，输出动作μt∈A，产生的t时刻的奖赏值Rt＝f(st)，状态变为st+1；
Agent输出的动作ut由策略π决定，策略π为状态st映射到每个动作的概率：S→P(A)，得到水下自主航行器大尺度连续性障碍物避障MDP模型的状态集。水下自主航行器大尺度连续性障碍物避障MDP模型的状态集S如附表1，动作集A如附表2
表1状态集
表2动作集
步骤4：根据状态集进行深度强化学习训练，直至无碰撞到达目标区域，并保存避障策略；
所述步骤4具体为：
步骤4.1：在仿真系统模块中，基于水下自主航行器大尺度连续性障碍物避障MDP模型的状态集，建立仿真试验平台，搭建python编译环境，编写分层强化学习训练模块，利用pyglet库建立水下峡谷海洋环境，建立水下自主航行器运动学模型和障碍物运动学模型，导入利用Python语言编写实现基于SumTree-DDPG的连续性障碍物环境避障控制器，如附图5所示。设置水下自主航行器的初始参数及神经网络训练超参数，进行训练；
步骤4.2：进行训练：水下自主航行器在水下峡谷环境中根据初始速度和初始偏航角运动，并且水下自主航行器的7个声呐探测的环境数据作为深度强化学习的状态，当7个声呐探测的范围中没有障碍物，水下自主航行器允许继续学习探索，直到到达目标区域，结束该回合学习；
步骤4.3：当7个声呐探测的范围存在障碍物且探测线的最小距离小于安全距离，表明水下自主航行器与障碍物相撞，该回合结束，回到起点重新开始学习；
不断的循环学习，直到每个回合都是无碰撞到达目标区域而触发的回合结束，此时表明训练收敛，保存学习到的策略。某海域的水下峡谷模拟环境仿真训练结果图，如附图6所示
优选地，所述SumTree-DDPG避障控制器具体实现过程为：
步骤4.1.1：随机初始化评论家网络Q(s,a|θQ)和演员网络μ(s|θu)的权重参数θQ和θμ；初始化目标网络Q'和μ'，网络权重参数为θQ'←θQ,θμ'←θμ；初始化SumTree并定义容量为H＝φ，记忆库数据存储到达最大容量为lenMax(Data)＝M；设置初始位置为当前状态，到达目标区域为目标状态；
状态空间为水下自主航行器7个避障声呐的实时探测到与障碍物或目标的距离，确定在时刻t的状态，通过下式表示在时刻t的状态：
步骤4.1.2：初始化动作空间，在时刻t定义动作空间为偏航角速度ω(t)和水平速度V(t)，确定at,通过下式表示动作集at：
训练回合数Ep最大回合数10000初始化为Ep＝1；
步骤4.1.3：Ep回合中时间步t最大时间步2000为初始化t＝1；
步骤4.1.4：在线Actor策略网络根据当前状态st策略选择出包含水下自主航行器的偏航角速度及水平速度的动作集，通过下式表示当前状态下动作集：
at＝μ(st|θμ)+Nt；
根据输出的动作
结合水下自主航行器的水平面3自由度的运动学模型，得到微分式，通过下式表示微分式：
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ(t)为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ(t))为转换矩阵；ψ(t)为时间步t时水下自主航行器的偏航角，并且
为ψ(t)对时间步t的微分；u(t)，v(t)，r(t)分别时间步t时水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度；
根据四阶龙格-库塔法求解微分式，得到执行动作后的新位置向量η(t+1)，通过下式表示所述向量：
η(t+1)＝[x(t+1),y(t+1),ψ(t+1)]T∈R3
由执行动作后的新位置向量转到下一个状态st+1，获得即时奖励值rt；
即时奖励值rt具体计算步骤如下；
步骤4.2.1：抗海流干扰稳定性模块奖励值更新r1(st,at,st+1)←r1(st,at,st+1)-0.01×(|ωt+1-ωt|+|sin(ψt-φ)|)，当水下自主航行器从安全区域驶向的下一个状态仍然是安全区域，目标模块奖励值更新
其中，r1(st,at,st+1)表示时间步t时奖励值rt的第一个分量抗海流干扰稳定性模块奖励值，式中ωt、ωt+1分别为当前时刻与下一时刻水下自主航行器的偏航角速度，ψt、φ分别在笛卡尔坐标系下的为水下自主航行器当前时刻的偏航角、水流方向角；r3(st,at,st+1)表示时间步t时奖励值rt的第三个分量目标模块奖励值，d0和dt分别为在大地坐标系下水下自主航行器的起始点与目标区域中心处距离，以及在大地坐标系下水下自主航行器在时间步t时位置与目标区域中心处距离；
步骤4.2.2：当水下自主航行器从安全区域驶向的下一个状态不是安全区域，安全模块奖励值r2(st,at,st+1)更新，当不安全区域为碰撞峡谷谷壁，安全模块奖励值更新为：r2(st,at,st+1)←r2(st,at,st+1)-R并且重新回到起点探索，结束回合；
否则处于避碰峡谷谷壁的危险区域，当声纳波束探测到水下自主航行器与障碍物距离小于1.5倍安全距离大于安全距离时，对安全模块奖励值进行更新，通过下式进行更新：
r2(st,at,st+1)←r2(st,at,st+1)-(Dt-1.5×rs)2；
其中，r2(st,at,st+1)表示时间步t时奖励值rt的第二个分量安全模块奖励值，R为正常数，Dt为时间步t时7个避障声呐探索到水下自主航行器与大尺度连续性障碍物之间的欧氏距离欧式值中最小值，rs为水下自主航行器的最小安全距离；
步骤4.2.3：当下一个状态是抵达目标区域，目标模块奖励值更新r3(st,at,st+1)←r3(st,at,st+1)+R，结束回合；
其中，R为正常数；
步骤4.2.4：结束回合后，得到即时奖励值rt←rt+τ1r1(st,at,st+1)+τ2r2(st,at,st+1)+τ3r3(st,at,st+1)；
其中，τ1、τ2和τ3分别表示三个奖励模块的比例系数。
将四元组(st,at,rt,st+1)存入SumTree结构H＝φ，当数据存储到达最大容量时，len(Data)＞M，从SumTree容量中H＝φ中依据采样概率
权重ωk＝(P(k)/minjP(j))-β采样小批量的N个经验样本
其中，
表示时间步t时的第k条经验样本，并且k＝1,2,…,N，N为小批量样本总数；
从SumTree容量中采样小批量的N个经验样本
构成数据集，发送给在线策略网络、目标策略网络、在线评价网络和目标评价网络；
根据采样的数据集，目标策略网络根据状态st+1输出动作a′t+1，计算目标Q值，记为yi，通过下式表示yi：
yi＝ri+γQ'(si+1,μ'(si+1|θμ')|θQ')；
目标评价网络根据状态st+1、目标策略网络输出的动作a′t+1和目标Q值的yi，更新损失函数更新评论家在线评价网络参数θ，通过下式进行在线评价:
其中，L为损失函数；
将小批量的N个经验样本结合随机梯度下降法，更新演员网络的策略和在线策略网络参数δ，通过下式进行更新:
其中，
为抽样策略梯度；
根据在线网络参数θ和δ以软更新的形式更新θ'和δ'：
步骤4.1.5：更新SumTree结构中每个节点的优先值，pk←|δj|；
步骤4.1.6：回合数用Ep表示，Ep＝1回合中时间步t＝t+1，重复运行步骤4.1.4到步骤4.1.5；
当在t≤2000时水下自主航行器进行探索过程碰撞障碍物或者到达目标区域转，回合数Ep＝Ep+1，重复运行步骤4.1.3到步骤4.1.6；
当Ep＝10000，水下自主航行器在大尺度连续性障碍物环境训练完成，保存学习后的避障策略。
步骤5：将深度强化学习后保存的避障策略，通过数据传输给下位机模块，指导控制模块融合传感器数据信息，实时计算出水下自主航行器偏航角及速度，此时的水下自主航行器偏航角及速度，引导水下自主航行器避开水下峡谷岩壁，并到达目标区域，完成航行任务。
具体实施例二：
本发明公开了一种可用于水下自主航行器在有大尺度连续性障碍物(如：水下峡谷环境中岩壁、航道水域、人工运河和渠道环境中墙壁)环境中路径规划避障控制系统。该系统是一个端到端的运动规划系统，并基于深度强化学习算法对策略进行训练学习优化。它以传感器信息为状态输入，水下自主航行器航行速度和偏航角速度为动作输出。该水下自主航行器路径规划避障控制系统主要过程为，首先建立一个存在大尺度连续性障碍物模拟训练环境中，本发明以典型的存在大尺度连续性障碍物的环境：水下峡谷为例进行说明，水下峡谷岩壁视为大尺度连续性静态障碍物。然后以避障传感器信息为输入，航行速度和偏航角速度为输出搭建深度强化学习神经网络的状态与动作，并且针对运动规划避障控制过程的多目标结构，对奖赏函数进行了模块化设计，为了避免稀疏奖励引起系统不为稳定，本发明结合人工势场法设置连续性奖励。本发明还利用基于改进的深度确定性策略梯度算法(本文命名为：SumTree-DDPG)对水下自主航行器进行避障训练，并将训练所得到的避障策略写入机器人下位机控制系统；最后水下自主航行器在水下峡谷中行驶时，可利用训练学习到的避障策略进行避障，最终安全的到达目标区域。
奖励值函数模块设计：在对奖励值函数设计时，根据多目标多约束进行模块化设计，本发明考虑稳定性、安全、目标三个方面。
为了提高水下自主航行器避障系统的鲁棒性，防止控制过程中,艏向转角的抖动,此处引入转艏变化率的影响,以实现平顺的水下自主航行器转艏控制效果。水下自主航行器在航行过程中不可避免地会受到海流的影响。水下自主航行器受到的影响最大侧流，其他方向影响很小，可忽略。因此，考虑系统干扰与外界环境干扰的稳定性模块的奖励值函数设计如式(1)。
式中ωt、ωt+1分别为当前时刻与下一时刻水下自主航行器的偏航角速度，ψt、φ分别在笛卡尔坐标系下的为水下自主航行器当前时刻的偏航角，水流方向角。
在安全模块方面，本文为了减少训练过程中碰撞障碍物的次数，在传统的碰撞障碍物给与离散的负奖励基础上，引入人工势场法设计出中间奖励，即当声纳波束探测到水下自主航行器与障碍物距离小于1.5倍安全距离即将碰撞时，给与水下自主航行器奖励值f2(Dt)，函数f2(Dt)借鉴于斥力函数，表达式如式(2)。如水下自主航行器未逃离非安全区域，碰撞了水下峡谷谷壁，则得到一个负奖励值-R(R为正常数)，整个安全模块的奖励值函数表达式如式(3)。
f2(Dt)＝-(Dt-1.5×rs)2 (2)
在目标模块中引入人工势场法设计正奖励，使水下机器人更快地找到目标位置。我们设计了以水下机器人当前位置与目标位置的距离为自变量的奖励函数f3(dt)，表达式如式(4)，并且当水下机器人当前位置与目标位置的距离小于上一时刻位置与目标位置的距离时，水下机器人将得到对应的正向奖励。如果水下自主航行器到达目标区域，则得到一个正奖励值R(R为正常数)，整个目标模块的奖励值函数表达式如式(5)。
式中d0、dt分别为起始点与目标区域中心处距离，以及水下自主航行器当前位置与目标区域中心处距离。
水下自主航行器得到的总奖励值为式(6)。
r(st,at,st+1)＝τ1r1(st,at,st+1)+τ2r2(st,at,st+1)+τ3r3(st,at,st+1) (6)
式中τ1、τ2和τ3分别表示三个奖励模块的比例系数。
奖励函数算法伪代码如算法1所示：
SumTree是一种树形结构，如附图4所示，叶节点内存放样本的优先级P，每个叶节点对应一个索引值，利用索引值，可以实现对应样本的存取。每两个叶节点对应一个上级的父节点，父节点的优先级等于左右两个子节点优先级之和，所以SumTree结构的顶端就是所有P的和。
SumTree-DDPG算法重点就在本发明在batch抽样的时候并不是随机抽样，而是按照Memory中的样本优先级来抽。这能更有效地找到我们需要学习的样本。由于DDPG算法中，策略网络的参数依赖价值网络的选取，而价值网络中的参数由价值网络的损失函数，即目标Q值与现实Q值差值的期望来更新。差值越大，代表网络参数的预测准确度还很远,即该样本更需要被学习，也就是优先级P越高。有了优先级P，本发明使用SumTree结构的方法有效地根据P来抽样。抽样时，本研究首先用根节点的优先级(所有叶节点优先级之和)除以样本数N,将优先级按顺序从0到优先级之和分成N个区间。然后在每个区间里随机取一个数。因为优先级较大的节点也会占据较长的区间，因此被抽到的概率也会较高，这样便达到了优先级取样的目的。每抽到一个叶节点，返回其优先级，及对应的样本池数据。从SumTree结构中采集N个样本
每个样本的采样概率和权重分别如下式(9)、(10)所示。
通过对DDPG的经验池改进，并结合算法1，得到用于水下自主航行器在大尺度连续性障碍物环境中避障仿真训练的算法2。
以上所述仅是一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法的优选实施方式，一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法的保护范围并不仅局限于上述实施例，凡属于该思路下的技术方案均属于本发明的保护范围。应当指出，对于本领域的技术人员来说，在不脱离本发明原理前提下的若干改进和变化，这些改进和变化也应视为本发明的保护范围。
Claims (5)
1.一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，其特征是：包括以下步骤：
步骤1：建立水下自主航行器模型与运动学模型，获取水下自助航行器周围障碍物的信息；
步骤2：根据水下自助航行器周围障碍物的信息，在笛卡尔坐标系下建立水下峡谷模拟训练环境，生成避障策略；
步骤3：将生成的避障策略转化为MDP过程，并进行训练得到水下自主航行器连续性障碍物避障MDP模型的状态集；
步骤4：根据状态集进行深度强化学习训练，直至无碰撞到达目标区域，并保存避障策略；
步骤5：将深度强化学习后保存的避障策略，通过数据传输给下位机模块，指导控制模块融合传感器数据信息，实时计算出水下自主航行器偏航角及速度，此时的水下自主航行器偏航角及速度，引导水下自主航行器避开水下峡谷岩壁，并到达目标区域，完成航行任务；
所述步骤4具体为：
步骤4.1：在仿真系统模块中，基于水下自主航行器大尺度连续性障碍物避障MDP模型的状态集，建立仿真试验平台，搭建python编译环境，编写分层强化学习训练模块，利用pyglet库建立水下峡谷海洋环境，建立水下自主航行器运动学模型和障碍物运动学模型，导入利用Python语言编写实现基于SumTree-DDPG的连续性障碍物环境避障控制器，设置水下自主航行器的初始参数及神经网络训练超参数，进行训练；
步骤4.2：进行训练：水下自主航行器在水下峡谷环境中根据初始速度和初始偏航角运动，并且水下自主航行器的7个声呐探测的环境数据作为深度强化学习的状态，当7个声呐探测的范围中没有障碍物，水下自主航行器允许继续学习探索，直到到达目标区域，结束该回合学习；
步骤4.3：当7个声呐探测的范围存在障碍物且探测线的最小距离小于安全距离，表明水下自主航行器与障碍物相撞，该回合结束，回到起点重新开始学习；
不断的循环学习，直到每个回合都是无碰撞到达目标区域而触发的回合结束，此时表明训练收敛，保存学习到的策略；
所述SumTree-DDPG避障控制器具体实现过程为：
步骤4.1.1：随机初始化评论家网络Q(s,a|θQ)和演员网络μ(s|θu)的权重参数θQ和θμ；初始化目标网络Q'和μ'，网络权重参数为θQ'←θQ,θμ'←θμ；初始化SumTree并定义容量为H＝φ，记忆库数据存储到达最大容量为lenMax(Data)＝M；设置初始位置为当前状态，到达目标区域为目标状态；
状态空间为水下自主航行器7个避障声呐的实时探测到与障碍物或目标的距离，确定在时刻t的状态，通过下式表示在时刻t的状态：
步骤4.1.2：初始化动作空间，在时刻t定义动作空间为偏航角速度ω(t)和水平速度V(t)，确定at,通过下式表示动作集at：
训练回合数Ep最大回合数10000初始化为Ep＝1；
步骤4.1.3：Ep回合中时间步t最大时间步2000为初始化t＝1；
步骤4.1.4：在线Actor策略网络根据当前状态st策略选择出包含水下自主航行器的偏航角速度及水平速度的动作集，通过下式表示当前状态下动作集：
at＝μ(st|θμ)+Nt；
根据输出的动作
结合水下自主航行器的水平面3自由度的运动学模型，得到微分式，通过下式表示微分式：
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ(t)为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ(t))为转换矩阵；ψ(t)为时间步t时水下自主航行器的偏航角，并且
为ψ(t)对时间步t的微分；u(t)，v(t)，r(t)分别时间步t时水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度；
根据四阶龙格-库塔法求解微分式，得到执行动作后的新位置向量η(t+1)，通过下式表示所述向量：
η(t+1)＝[x(t+1),y(t+1),ψ(t+1)]T∈R3
由执行动作后的新位置向量转到下一个状态st+1，获得即时奖励值rt；
将四元组(st,at,rt,st+1)存入SumTree结构H＝φ，当数据存储到达最大容量时，len(Data)＞M，从SumTree容量中H＝φ中依据采样概率
权重ωk＝(P(k)/minjP(j))-β采样小批量的N个经验样本
其中，
表示时间步t时的第k条经验样本，并且k＝1,2,…,N，N为小批量样本总数；
从SumTree容量中采样小批量的N个经验样本
构成数据集，发送给在线策略网络、目标策略网络、在线评价网络和目标评价网络；
根据采样的数据集，目标策略网络根据状态st+1输出动作a′t+1，计算目标Q值，记为yi，通过下式表示yi：
yi＝ri+γQ'(si+1,μ'(si+1|θμ')|θQ')；
目标评价网络根据状态st+1、目标策略网络输出的动作a′t+1和目标Q值的yi，更新损失函数更新评论家在线评价网络参数θ，通过下式进行在线评价:
其中，L为损失函数；
将小批量的N个经验样本结合随机梯度下降法，更新演员网络的策略和在线策略网络参数δ，通过下式进行更新:
其中，
为抽样策略梯度；
根据在线网络参数θ和δ以软更新的形式更新θ'和δ'：
步骤4.1.5：更新SumTree结构中每个节点的优先值，pk←|δj|；
步骤4.1.6：回合数用Ep表示，Ep＝1回合中时间步t＝t+1，重复运行步骤4.1.4到步骤4.1.5；
当在t≤2000时水下自主航行器进行探索过程碰撞障碍物或者到达目标区域转，回合数Ep＝Ep+1，重复运行步骤4.1.3到步骤4.1.6；
当Ep＝10000，水下自主航行器在大尺度连续性障碍物环境训练完成，保存学习后的避障策略。
2.根据权利要求1所述的一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，其特征是：所述步骤1具体为：
建立水平面3自由度水下自主航行器模型与运动学模型，设置7个避障声呐分别为Si，其中，i＝1,2,3…7，测距声纳采样频率为2Hz，探测距离为150m，通过避障声呐获取水下自助航行器周围障碍物的信息；
将水平面3自由度水下自主航行器模型与运动学模型进行简化，简化为等尺度的长方形，并用对称分布在长方形周边的7条虚线，随体坐标系下分布角度依次为θi，通过θi＝90°,60°,30°,0°,-30°,-60°,90°表示7个避障声呐，通过下式表示水平面3自由度水下自主航行器与运动学模型方程：
υ＝[u,v,r]T∈R3
其中，
为水下自主航行器大地坐标系下的水平面位置矢量包含水平面位置坐标和偏航角；υ为水下自主航行器在载体下的水平面速度矢量包含水平速度和偏航角速度；R(ψ)为转换矩阵，ψ为水下自主航行器的偏航角；u、v和r分别水下自主航行器在随体坐标系下的水平速度矢量的X轴向分量、Y轴向分量和偏航角速度。
3.根据权利要求1所述的一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，其特征是：所述步骤2具体为：
在笛卡尔坐标系下建立水下峡谷模拟训练环境，根据水下自助航行器周围障碍物的信息在上位机模块的界面及仿真系统模块中建立水下自主航行器水下峡谷环境二维模拟训练环境模型，并且在模拟环境搭建过程需要融合连续性静态障碍物避障方法，
确定水下自主航行器的声呐与连续障碍物表面的探测交点，通过Ti表示水下自主航行器的声呐与连续障碍物表面的探测交点，Di是水下自主航行器到T点的距离，通过控制Di≥ρs，ρs为水下自主航行器最小安全距离，实现水下自主航行器对连续性障碍物进行规避，生成避障策略。
4.根据权利要求1所述的一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，其特征是：所述步骤3具体为：
为了进行深度强化学习训练，将生成的避障策略转化为MDP模型，MDP模型由四元组构成，通过下式表示MDP模型：
MDP＝(S,A,Psa,R)
其中A表示动作集，S表示状态集，Psa表示状态转移概率，R为奖赏函数；
对MDP模型进行设定，所述MDP模型设定运动规划任务是水平的，有三个自由度；同时将时间作离散化处理，避障系统以采样率为TS＝0.5s的间隔定期输出，由此水下自主航行器在t时刻接收到状态信息后，输出动作μt∈A，产生的t时刻的奖赏值Rt＝f(st)，状态变为st+1；
Agent输出的动作ut由策略π决定，策略π为状态st映射到每个动作的概率：S→P(A)，得到水下自主航行器大尺度连续性障碍物避障MDP模型的状态集。
5.根据权利要求1所述的一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法，其特征是：即时奖励值rt具体计算步骤如下；
步骤4.2.1：抗海流干扰稳定性模块奖励值更新r1(st,at,st+1)←r1(st,at,st+1)-0.01×(|ωt+1-ωt|+|sin(ψt-φ)|)，当水下自主航行器从安全区域驶向的下一个状态仍然是安全区域，目标模块奖励值更新
其中，r1(st,at,st+1)表示时间步t时奖励值rt的第一个分量抗海流干扰稳定性模块奖励值，式中ωt、ωt+1分别为当前时刻与下一时刻水下自主航行器的偏航角速度，ψt、φ分别在笛卡尔坐标系下的为水下自主航行器当前时刻的偏航角、水流方向角；r3(st,at,st+1)表示时间步t时奖励值rt的第三个分量目标模块奖励值，d0和dt分别为在大地坐标系下水下自主航行器的起始点与目标区域中心处距离，以及在大地坐标系下水下自主航行器在时间步t时位置与目标区域中心处距离；
步骤4.2.2：当水下自主航行器从安全区域驶向的下一个状态不是安全区域，安全模块奖励值r2(st,at,st+1)更新，当不安全区域为碰撞峡谷谷壁，安全模块奖励值更新为：r2(st,at,st+1)←r2(st,at,st+1)-R并且重新回到起点探索，结束回合；
否则处于避碰峡谷谷壁的危险区域，当声纳波束探测到水下自主航行器与障碍物距离小于1.5倍安全距离大于安全距离时，对安全模块奖励值进行更新，通过下式进行更新：
r2(st,at,st+1)←r2(st,at,st+1)-(Dt-1.5×rs)2；
其中，r2(st,at,st+1)表示时间步t时奖励值rt的第二个分量安全模块奖励值，R为正常数，Dt为时间步t时7个避障声呐探索到水下自主航行器与大尺度连续性障碍物之间的欧氏距离欧式值中最小值，rs为水下自主航行器的最小安全距离；
步骤4.2.3：当下一个状态是抵达目标区域，目标模块奖励值更新r3(st,at,st+1)←r3(st,at,st+1)+R，结束回合；
其中，R为正常数；
步骤4.2.4：结束回合后，得到即时奖励值rt←rt+τ1r1(st,at,st+1)+τ2r2(st,at,st+1)+τ3r3(st,at,st+1)；
其中，τ1、τ2和τ3分别表示三个奖励模块的比例系数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011109095.2A CN112241176B (zh) | 2020-10-16 | 2020-10-16 | 一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011109095.2A CN112241176B (zh) | 2020-10-16 | 2020-10-16 | 一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112241176A CN112241176A (zh) | 2021-01-19 |
| CN112241176B true CN112241176B (zh) | 2022-10-28 |
Family
ID=74169262
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011109095.2A Active CN112241176B (zh) | 2020-10-16 | 2020-10-16 | 一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112241176B (zh) |
Families Citing this family (28)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112925319B (zh) * | 2021-01-25 | 2022-06-07 | 哈尔滨工程大学 | 一种基于深度强化学习的水下自主航行器动态避障方法 |
| CN112904350A (zh) * | 2021-01-27 | 2021-06-04 | 天津蓝鳍海洋工程有限公司 | 一种水下航行器立体航行自主避让障碍物的方法 |
| CN112947421B (zh) * | 2021-01-28 | 2022-09-13 | 西北工业大学 | 一种基于强化学习的auv自主避障方法 |
| CN113010963B (zh) * | 2021-03-04 | 2022-04-29 | 山东大学 | 基于深度强化学习的变质量水下航行器避障方法及系统 |
| CN113033119B (zh) * | 2021-03-11 | 2022-03-25 | 山东大学 | 一种基于双评论家强化学习技术的水下航行器目标区域上浮控制方法 |
| CN113095463A (zh) * | 2021-03-31 | 2021-07-09 | 南开大学 | 一种基于进化强化学习的机器人对抗方法 |
| CN113065709B (zh) * | 2021-04-13 | 2023-06-30 | 西北工业大学 | 一种基于强化学习的跨域异构集群路径规划方法 |
| CN113156954B (zh) * | 2021-04-25 | 2023-03-24 | 电子科技大学 | 一种基于增强学习的多智能体集群避障方法 |
| CN113503885B (zh) * | 2021-04-30 | 2024-02-20 | 山东师范大学 | 一种基于采样优化ddpg算法的机器人路径导航方法及系统 |
| CN113220001B (zh) * | 2021-05-13 | 2022-11-25 | 青岛理工大学 | 一种潜航器及潜航器的实时避障方法 |
| CN113391633A (zh) * | 2021-06-21 | 2021-09-14 | 南京航空航天大学 | 一种面向城市环境的移动机器人融合路径规划方法 |
| CN113342047B (zh) * | 2021-06-23 | 2023-10-17 | 大连大学 | 未知环境中基于障碍物位置预测改进人工势场法的无人机路径规划方法 |
| CN113625733A (zh) * | 2021-08-04 | 2021-11-09 | 北京工业大学 | 一种基于ddpg多目标三维无人机路径规划方法 |
| CN113534668B (zh) * | 2021-08-13 | 2022-06-10 | 哈尔滨工程大学 | 基于最大熵的演员-评论家框架的auv运动规划方法 |
| CN113486871B (zh) * | 2021-09-07 | 2021-11-16 | 中国人民解放军国防科技大学 | 基于深度图的无人车局部自主控制方法、装置和设备 |
| CN114822141B (zh) * | 2021-12-30 | 2024-03-22 | 宜昌测试技术研究所 | 一种auv模拟训练系统及模拟训练方法 |
| CN114609925B (zh) * | 2022-01-14 | 2022-12-06 | 中国科学院自动化研究所 | 水下探索策略模型的训练方法及仿生机器鱼水下探索方法 |
| CN114610021A (zh) * | 2022-02-07 | 2022-06-10 | 哈尔滨工程大学 | 一种自主式水下航行器归航路径规划方法和装置 |
| CN114625151B (zh) * | 2022-03-10 | 2024-05-28 | 大连理工大学 | 一种基于强化学习的水下机器人避障路径规划方法 |
| CN114734446B (zh) * | 2022-05-10 | 2024-06-18 | 南京理工大学 | 基于改进的强化学习算法的机械手高精度位置控制方法 |
| CN114967721B (zh) * | 2022-06-08 | 2024-04-16 | 西北工业大学 | 一种基于DQ-CapsNet的无人机自助路径规划和避障策略方法 |
| CN115206157A (zh) * | 2022-08-05 | 2022-10-18 | 白杨时代(北京)科技有限公司 | 一种无人潜航器寻路训练方法、装置及无人潜航器 |
| CN115493595A (zh) * | 2022-09-28 | 2022-12-20 | 天津大学 | 一种基于局部感知与近端优化策略的auv路径规划方法 |
| CN115494733A (zh) * | 2022-10-27 | 2022-12-20 | 南方电网调峰调频发电有限公司储能科研院 | 一种基于gazebo的水下机器人自适应控制方法 |
| CN115657678B (zh) * | 2022-10-28 | 2024-04-26 | 中国船舶重工集团公司第七一九研究所 | 面向复杂动态环境的水下无人潜航器航迹生成方法及系统 |
| CN115657683B (zh) * | 2022-11-14 | 2023-05-02 | 中国电子科技集团公司第十研究所 | 一种可用于巡检作业任务的无人无缆潜水器实时避障方法 |
| CN117406757B (zh) * | 2023-12-12 | 2024-04-19 | 西北工业大学宁波研究院 | 一种基于三维全局视觉的水下自主航行方法 |
| CN117873118B (zh) * | 2024-03-11 | 2024-05-28 | 中国科学技术大学 | 一种基于sac算法与控制器的仓储物流机器人导航方法 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107102644A (zh) * | 2017-06-22 | 2017-08-29 | 华南师范大学 | 基于深度强化学习的水下机器人轨迹控制方法及控制系统 |
| CN108762281A (zh) * | 2018-06-08 | 2018-11-06 | 哈尔滨工程大学 | 一种基于记忆关联强化学习的嵌入式实时水下机器人智能决策方法 |
| WO2018204642A1 (en) * | 2017-05-03 | 2018-11-08 | The Government Of The United States Of America, As Represented By The Secretary Of The Navy | Floating base vector sensor |
| CN108803321A (zh) * | 2018-05-30 | 2018-11-13 | 清华大学 | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 |
| CN109540151A (zh) * | 2018-03-25 | 2019-03-29 | 哈尔滨工程大学 | 一种基于强化学习的auv三维路径规划方法 |
| CN110333739A (zh) * | 2019-08-21 | 2019-10-15 | 哈尔滨工程大学 | 一种基于强化学习的auv行为规划及动作控制方法 |
| WO2020005875A1 (en) * | 2018-06-29 | 2020-01-02 | Nissan North America, Inc. | Orientation-adjust actions for autonomous vehicle operational management |
| CN110794842A (zh) * | 2019-11-15 | 2020-02-14 | 北京邮电大学 | 基于势场的强化学习路径规划算法 |
| CN110806759A (zh) * | 2019-11-12 | 2020-02-18 | 清华大学 | 一种基于深度强化学习的飞行器航线跟踪方法 |
| CN111093203A (zh) * | 2019-12-30 | 2020-05-01 | 重庆邮电大学 | 一种基于环境感知的服务功能链低成本智能部署方法 |
| CN111123963A (zh) * | 2019-12-19 | 2020-05-08 | 南京航空航天大学 | 基于强化学习的未知环境自主导航系统及方法 |
| US10671076B1 (en) * | 2017-03-01 | 2020-06-02 | Zoox, Inc. | Trajectory prediction of third-party objects using temporal logic and tree search |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107479368B (zh) * | 2017-06-30 | 2021-09-21 | 北京百度网讯科技有限公司 | 一种基于人工智能的训练无人机控制模型的方法及系统 |
-
2020
- 2020-10-16 CN CN202011109095.2A patent/CN112241176B/zh active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10671076B1 (en) * | 2017-03-01 | 2020-06-02 | Zoox, Inc. | Trajectory prediction of third-party objects using temporal logic and tree search |
| WO2018204642A1 (en) * | 2017-05-03 | 2018-11-08 | The Government Of The United States Of America, As Represented By The Secretary Of The Navy | Floating base vector sensor |
| CN107102644A (zh) * | 2017-06-22 | 2017-08-29 | 华南师范大学 | 基于深度强化学习的水下机器人轨迹控制方法及控制系统 |
| CN109540151A (zh) * | 2018-03-25 | 2019-03-29 | 哈尔滨工程大学 | 一种基于强化学习的auv三维路径规划方法 |
| CN108803321A (zh) * | 2018-05-30 | 2018-11-13 | 清华大学 | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 |
| CN108762281A (zh) * | 2018-06-08 | 2018-11-06 | 哈尔滨工程大学 | 一种基于记忆关联强化学习的嵌入式实时水下机器人智能决策方法 |
| WO2020005875A1 (en) * | 2018-06-29 | 2020-01-02 | Nissan North America, Inc. | Orientation-adjust actions for autonomous vehicle operational management |
| CN110333739A (zh) * | 2019-08-21 | 2019-10-15 | 哈尔滨工程大学 | 一种基于强化学习的auv行为规划及动作控制方法 |
| CN110806759A (zh) * | 2019-11-12 | 2020-02-18 | 清华大学 | 一种基于深度强化学习的飞行器航线跟踪方法 |
| CN110794842A (zh) * | 2019-11-15 | 2020-02-14 | 北京邮电大学 | 基于势场的强化学习路径规划算法 |
| CN111123963A (zh) * | 2019-12-19 | 2020-05-08 | 南京航空航天大学 | 基于强化学习的未知环境自主导航系统及方法 |
| CN111093203A (zh) * | 2019-12-30 | 2020-05-01 | 重庆邮电大学 | 一种基于环境感知的服务功能链低成本智能部署方法 |
Non-Patent Citations (6)
| Title |
|---|
| A Plume-Tracing Strategy via Continuous State-action Reinforcement Learning;Niu, LY等;《2017 CHINESE AUTOMATION CONGRESS》;20171231;全文 * |
| AUV path following controlled by modified Deep Deterministic Policy Gradient;Sun, YS等;《OCEAN ENGINEERING》;20200815;全文 * |
| Wang, C等.AUTONOMOUS NAVIGATION OF UAV IN LARGE-SCALE UNKNOWN COMPLEX ENVIRONMENT WITH DEEP REINFORCEMENT LEARNING.《2017 IEEE GLOBAL CONFERENCE ON SIGNAL AND INFORMATION PROCESSING》.2017, * |
| 一种小型水下自主航行器的操纵性;唐同泽等;《中国海洋平台》;20181030;第33卷(第5期);全文 * |
| 基于增强学习的水下无人航行器控制方法研究;李严;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20200215(第02期);全文 * |
| 智能协同算法研究及应用;黄利伟;《中国博士学位论文全文数据库 信息科技辑》;20200115(第01期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112241176A (zh) | 2021-01-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112241176B (zh) | 一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法 | |
| Li et al. | Path planning technologies for autonomous underwater vehicles-a review | |
| CN110333739B (zh) | 一种基于强化学习的auv行为规划及动作控制方法 | |
| Chen et al. | Path planning and obstacle avoiding of the USV based on improved ACO-APF hybrid algorithm with adaptive early-warning | |
| Sun et al. | Mapless motion planning system for an autonomous underwater vehicle using policy gradient-based deep reinforcement learning | |
| CN112925319B (zh) | 一种基于深度强化学习的水下自主航行器动态避障方法 | |
| CN108319293B (zh) | 一种基于lstm网络的uuv实时避碰规划方法 | |
| Leonard et al. | Decoupled stochastic mapping [for mobile robot & auv navigation] | |
| Cao et al. | Target search control of AUV in underwater environment with deep reinforcement learning | |
| CN109765929B (zh) | 一种基于改进rnn的uuv实时避障规划方法 | |
| CN114625151B (zh) | 一种基于强化学习的水下机器人避障路径规划方法 | |
| CN113534668B (zh) | 基于最大熵的演员-评论家框架的auv运动规划方法 | |
| Hadi et al. | Deep reinforcement learning for adaptive path planning and control of an autonomous underwater vehicle | |
| CN113848974B (zh) | 一种基于深度强化学习的飞行器轨迹规划方法及系统 | |
| CN111240345A (zh) | 一种基于双bp网络增强学习框架的水下机器人轨迹跟踪方法 | |
| Wang et al. | Cooperative collision avoidance for unmanned surface vehicles based on improved genetic algorithm | |
| CN111930141A (zh) | 一种水下机器人三维路径可视化跟踪方法 | |
| Lan et al. | Path planning for underwater gliders in time-varying ocean current using deep reinforcement learning | |
| Du et al. | An optimized path planning method for coastal ships based on improved DDPG and DP | |
| Wang et al. | Path-following optimal control of autonomous underwater vehicle based on deep reinforcement learning | |
| CN117590867B (zh) | 基于深度强化学习的水下自主航行器接驳控制方法和系统 | |
| Zhang et al. | Intelligent vector field histogram based collision avoidance method for auv | |
| Huang et al. | The USV path planning based on an improved DQN algorithm | |
| CN115657683B (zh) | 一种可用于巡检作业任务的无人无缆潜水器实时避障方法 | |
| CN116774712A (zh) | 一种欠驱动auv三维环境下的实时动态避障方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |