[发明专利]一种水下自主航行器在大尺度连续性障碍物环境中路径规划避障控制方法

权利要求书

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＝φ，记忆库数据存储到达最大容量为len_Max(Data)＝M；设置初始位置为当前状态，到达目标区域为目标状态；

状态空间为水下自主航行器7个避障声呐的实时探测到与障碍物或目标的距离，确定在时刻t的状态，通过下式表示在时刻t的状态：

步骤4.1.2：初始化动作空间，在时刻t定义动作空间为偏航角速度ω(t)和水平速度V(t)，确定a_t,通过下式表示动作集a_t：

训练回合数Ep最大回合数10000初始化为Ep＝1；

步骤4.1.3：Ep回合中时间步t最大时间步2000为初始化t＝1；

步骤4.1.4：在线Actor策略网络根据当前状态s_t策略选择出包含水下自主航行器的偏航角速度及水平速度的动作集，通过下式表示当前状态下动作集：

a_t＝μ(s_t|θ^μ)+N_t；

根据输出的动作结合水下自主航行器的水平面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∈R³

由执行动作后的新位置向量转到下一个状态s_t+1，获得即时奖励值r_t；

将四元组(s_t,a_t,r_t,s_t+1)存入SumTree结构H＝φ，当数据存储到达最大容量时，len(Data)＞M，从SumTree容量中H＝φ中依据采样概率权重ω_k＝(P(k)/min_jP(j))^-β采样小批量的N个经验样本其中，表示时间步t时的第k条经验样本，并且k＝1,2,…,N，N为小批量样本总数；

从SumTree容量中采样小批量的N个经验样本构成数据集，发送给在线策略网络、目标策略网络、在线评价网络和目标评价网络；

根据采样的数据集，目标策略网络根据状态s_t+1输出动作a′_t+1，计算目标Q值，记为y_i，通过下式表示y_i：

y_i＝r_i+γQ'(s_i+1,μ'(s_i+1|θ^μ')|θ^Q')；

目标评价网络根据状态s_t+1、目标策略网络输出的动作a′_t+1和目标Q值的y_i，更新损失函数更新评论家在线评价网络参数θ，通过下式进行在线评价:

其中，L为损失函数；

将小批量的N个经验样本结合随机梯度下降法，更新演员网络的策略和在线策略网络参数δ，通过下式进行更新:

其中，为抽样策略梯度；

根据在线网络参数θ和δ以软更新的形式更新θ'和δ'：

其中τ在线网络参数的权重；

步骤4.1.5：更新SumTree结构中每个节点的优先值，p_k←|δ_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，水下自主航行器在大尺度连续性障碍物环境训练完成，保存学习后的避障策略。