CN116774712A - 一种欠驱动auv三维环境下的实时动态避障方法 - Google Patents

Abstract

本发明属于水下机器人的避障领域，具体说是一种欠驱动AUV三维环境下的实时动态避障方法，包括以下步骤：构建AUV的操纵性模型，计算AUV与障碍物间的相对位置及姿态；建立障碍物风险评估模型，并判断出不同障碍物的风险等级，筛选出对AUV威胁最大的障碍物；基于深度确定性策略梯度算法，搭建动态避障系统的网络架构；设计动态避障系统的输入与输出，实现状态到动作的映射；将得到的动作输入至AUV的操纵性模型，从而实现AUV的运动；设置动态避障系统的强化学习奖励函数；根据实际场景进行虚拟仿真环境搭建，对动态避障系统进行训练，保存训练好的模型，通过搭载该动态避障系统，实现欠驱动AUV在现实中海洋环境的实时动态避障。

Description

一种欠驱动AUV三维环境下的实时动态避障方法

技术领域

本发明属于水下机器人的避障领域，具体说是一种欠驱动AUV三维环境下的实时动态避障方法。

背景技术

海洋中拥有着丰富的资源，因此近年来世界各国加大了对海洋的开发利用，然而由于海洋环境复杂多变，且较为恶劣，因此无论是人还是陆地上的大多数设备都无法在水下环境中进行操作。自主水下机器人(autonomous underwater vehicle，AUV)由于其高机动性、自主性及安全性等特点，广泛用于水下任务中。

在水下环境中，AUV避障不仅取决于AUV的运动特性，即运动学与动力学约束，还取决于环境因素，除了已知的地形和静态障碍外，由于水下环境的复杂性与不可预测性，AUV无法在任务前获得完整的环境信息，在航行过程中可能遭遇未知的障碍物，在这些障碍中，动态障碍物由于其运动状态难以预测，同时动能较大，因此与AUV发生碰撞时，危险性高，严重威胁着AUV的安全。

基于上述需求，AUV必须具有实时避障能力，以确保自身安全并提高任务效率。因此需要一个实时的动态避障系统，使AUV能够应对复杂多变的水下环境，躲避航行过程中遭遇的静态及动态障碍物，确保任务的顺利进行。

目前现有的方法多数只应用于二维平面，有的只是对三维空间进行解耦，在实际过程中仍然是当作二维平面进行处理，没有真正地考虑三维环境下的避障问题，影响了避障效率；同时现有的方法考虑的障碍物场景较为简单，多数为静态障碍物，且事先已经获取了障碍物的相关信息，避障能力有限；一些方法考虑动态障碍物后，只是将障碍物设置简单的运动状态，训练后的模型具有偶然性，不具有普适性；同时当AUV附近存在多个障碍物时，由于状态维数过大，模型搜索能力较低，导致系统难以制定出最优策略。

发明内容

本发明目的是提供一种欠驱动水下机器人三维环境下的实时动态避障方法，可以使AUV在三维水下环境作业的过程中，实时躲避静态及动态障碍物，当AUV附近存在多个障碍物时，通过风险评估模型计算各个障碍物的风险等级，从而可以筛选出对AUV威胁最大的障碍物，大大提高了避障能力，使用深度强化学习算法，构建出状态输入到动作输出的端到端模型，基于传感器获取到的信息，AUV即可完成决策从而实现自主避障，以克服上述现有技术中水下机器人的缺陷。

本发明为实现上述目的所采用的技术方案是：一种欠驱动AUV三维环境下的实时动态避障方法，包括以下步骤：

S1：构建AUV操纵性模型；

S2：通过声纳探测的障碍物信息，计算AUV与障碍物间的相对位置及姿态；

S3：根据步骤S2中获得的AUV与障碍物间的相对位置及姿态，建立障碍物风险评估模型，并判断出不同障碍物的风险等级，筛选出对AUV威胁最大的障碍物；

S4：基于深度确定性策略梯度算法，搭建动态避障系统的网络架构；

S5：设计动态避障系统的输入与输出，将状态信息传输至动态避障系统的网络架构中，得到系统的动作输出，构建出状态输入到动作输出的端到端的动态避障系统，实现状态到动作的映射；将得到的动作输入至步骤S1构建的AUV操纵性模型，从而实现AUV的运动；

S6：设置动态避障系统的强化学习奖励函数；

S7：根据实际场景进行虚拟仿真环境搭建，对动态避障系统进行训练，保存训练好的模型，通过搭载该动态避障系统，实现欠驱动AUV在现实中海洋环境的实时动态避障。

所述步骤S1，具体为：

构建AUV的操纵性模型，忽略横摇，AUV在三维空间下由进退、横移、升沉、俯仰以及偏航五部分运动形式组成，则AUV的位置姿态为：

η＝[x,y,z,θ,ψ]^T

其中，(x,y,z)代表大地坐标系下的位置坐标，θ与ψ分别代表纵倾角与艏向角；

AUV的速度由v＝[u,v,w,q,r]^T表示；

其中，(u,v,w)分别代表AUV的纵向速度、横向速度与垂向速度，q与r分别表示纵倾角速度与转艏角速度；

AUV的非线性运动学及动力学方程表示为：

其中，J(η)是AUV空间运动五自由度坐标转换矩阵；M表示惯性矩阵，C(v)为科式向心力矩阵，D(v)代表阻尼矩阵，g(η)为恢复力矩阵，τ表示控制力、力矩，针对欠驱动类型的AUV，矩阵τ表示为：

τ＝[τ_u,0,0,τ_q,τ_r]^T

其中τ_u、τ_q、τ_r分别代表AUV的纵向推力、纵倾力矩、偏航力矩。

所述步骤S2，包括以下步骤：

2-1)通过AUV搭载的前视声纳及侧扫声纳，获取障碍物在大地坐标系下的位置信息η_o＝(x_o,y_o,z_o)^T；

2-2)根据AUV的位置坐标与障碍物的位置坐标，计算AUV与障碍物间的相对位置关系，即：

p_ao＝(x_ao,y_ao,z_ao)^T＝(x_o,y_o,z_o)^T-(x,y,z)^T

其中，p_ao表示由AUV指向障碍物的位置向量，(x_ao,y_ao,z_ao)代表向量的坐标；

2-3)根据AUV指向障碍物的位置向量p_ao，获取AUV与障碍物间的距离，即p_ao的模长：

2-4)根据AUV指向障碍物的位置向量p_ao，通过坐标转换矩阵R(θ,ψ)，得到相对位置向量p_ao在AUV自身坐标系上的投影p_r为：

2-5)根据投影p_r获取AUV与障碍物间的相对位姿，相对位姿包括：相对纵倾角θ_r与相对艏向角为ψ_r，即：

2-6)基于目标点的位置坐标(x_g,y_g,z_g)，根据步骤2-1)～步骤2-5)，获取AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg。

所述步骤S3，具体为：

3-1)通过判断障碍物的坐标是否发生变化，划分静态障碍物与动态障碍物，用O_i来表示障碍物i的特性，即：

3-2)构建障碍物的风险评估模型，以判断障碍物的风险等级；根据AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg，则标号为i的障碍物的风险等级M_i表示为：

其中，k₁、k₂、k₃、m₁、m₂、m₃均为大于零的比例系数，表示AUV与障碍物表面间距离的安全阈值，/>代表AUV与障碍物i的距离，/>代表障碍物i的半径，f()为自定义函数；

将上式简化为：

M_i＝M_com+M_con

其中，M_com定义为常规项，M_con定义为条件项；

3-3)当声纳探测到AUV附近存在多个障碍物时，获取不同障碍物的风险等级，通过比较各个障碍物的风险等级M_i，筛选出M_i最大的障碍物，即：

其中，i_max为对应的障碍物即为当前对AUV威胁最大的障碍物。

判断出不同障碍物的风险等级，具体为：

a.当AUV能够探测到障碍物时，M_com始终有值，M_com与AUV和障碍物间的距离d_ao、相对纵倾角θ_r、相对艏向角为ψ_r有关，且值随AUV与障碍物间的距离、相对纵倾角、相对艏向角的增大而减小，成负相关，则当以下条件满足时，风险等级最大；

b.在障碍物风险评估模型中，条件项M_con，表示为：

条件项M_con中，k₂O_i∈[0,k₂]与障碍物的动、静态特性相关，判定动态障碍物的威胁性相对于静态障碍物高，增大了动态障碍物的风险等级；

当障碍物为静态障碍物时，k₂O_i为0；

当障碍物为动态障碍物时，k₂O_i的值为k₂；

c.条件项M_con中，在中，其自定义函数f(t)表示为：

当时，/>则该项值为/>

当时，/>该项的值为：/>

判定当AUV与障碍物之间的距离超出设定的安全阈值与障碍物自身半径之和时，/>为常数，不影响各个障碍物的风险等级；随着AUV与障碍物之间距离的减小，当小于阈值与障碍物半径之和时，/>的数值随着二者之间距离减小而增大；

当时，此时M_i→+∞，将AUV视为质点，障碍物视为球体，AUV与物体表面之间的距离为障碍物自身半径，表示AUV与障碍物已发生碰撞，此时风险等级最大。

所述深度确定性策略梯度算法为基于AC框架的算法；

AC框架包含策略网络Actor与价值网络Critic；其中，策略网络Actor与价值网络Critic分别设有一个估计网络和一个目标网络。

所述步骤S4，具体为：

4-1)策略网络Actor的输入为AUV的状态，输出为动作；

对于策略网络Actor中的估计网络，表示为：a_t＝μ_θ(s_t)，其中，μ_θ(s_t)代表策略，s_t与a_t分别代表当前时刻的状态与动作，θ代表Actor中估计网络的参数；

对于策略网络Actor中的目标网络，输入为下一时刻的状态s_t+1，输出为下一时刻的动作a_t+1，表示为：其中，/>代表目标网络参数；

对策略网络Actor中的估计网络引入OU噪声，以增加动作的随机性，即：

a_t＝μ_θ(s_t)+N_t

其中，N_t代表OU噪声；

4-2)通过价值网络Critic拟合价值函数Q(s,a)，对AUV执行的动作进行评价；其中价值估计网络的输入为s_t与a_t，输出为Q_w(s_t,a_t)，价值目标网络的输入为s_t+1、a_t+1，输出

4-3)在网络结构中设置了一个记忆库，当每次训练时，将AUV与环境的交互数据(s_t,a_t,r_t,s_t+1)存储在记忆库中，在更新过程，从记忆库中随机抽取N个样本更新网络参数θ和w；

4-4)基于价值网络Critic拟合的价值函数Q_w(s_t,a_t)，进行策略网络的更新，利用梯度上升，使价值网络Critic输出的Q值最大化，则梯度表示为：

其中，代表梯度符号；

4-5)通过计算价值网络Critic的时序差分，再计算均方误差，利用梯度下降，最小化目标函数，更新自身网络参数w，其中，最小化目标函数L(w)表示为：

其中，γ∈(0,1)代表折扣因子；

4-6)通过软更新的方式，更新策略网络Actor与价值网络Critic中的目标网络参数和/>即：

其中，ξ∈(0,1)代表滑动平均系数，能够影响和/>更新的幅度；

4-7)完成AUV动态避障系统中策略网络Actor和价值网络Critic网络框架的建立。

所述设计动态避障系统的输入与输出，并构建出状态输入到动作输出的端到端模型，实现二者之间的映射，具体为：

5-1)动态避障系统的输入包含AUV自身的位置信息及姿态信息η＝[x,y,z,θ,ψ]^T，和AUV的速度信息v＝[u,v,w,q,r]^T；

5-2)根据障碍物风险评估模型，找出最具威胁的障碍物i后，将障碍物自身动静态特性O_i、与AUV间的距离相对纵倾角/>相对艏向角/>障碍物半径/>该障碍物的风险等级M_i全部输入到动态避障系统中；同时，将获取到的AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg也作为动态避障系统的输入，则动态避障系统的总输入为：

5-3)将s_t作为一个整体进行归一化处理后，通过模型堆叠，再将上一时刻的输入状态s_t-1与当前时刻输入状态s_t作为整体输入到动态避障系统中；

5-4)将动态避障系统输出的动作值控制在(-1,+1)之间，对其进行数学变换，从而输出与AUV模型适配的动作；

5-5)基于动态避障系统，实现状态s_t到动作a_t的映射，即：

a_t＝f(s_t) s_t→a_t

5-6)AUV根据动态避障系统输出的动作，完成避障任务。

所述根据动态避障系统及设计动态避障系统的输入与输出，设置强化学习奖励函数，具体为：

6-1)AUV在执行任务的过程中，需要到达指定目标位置，因此设立终端奖励项，当AUV到达指定位置后，获得奖励，表示为：

r＝+r₁ r₁＞0

当AUV与障碍物发生碰撞时，给予其惩罚，表示为：

r＝-r₂ r₂＞0

6-2)基于AUV与目标点间的距离d_ag、相对纵倾角θ_ag及相对艏向角ψ_ag，设置奖励项为：

其中，-w₁d_ag以使AUV减少与目标点之间的距离，以指导AUV调整自身的姿态，使其朝向目标位置航行；

6-3)根据障碍物风险评估模型，将避障任务分解为：减小障碍物对AUV带来的威胁，将风险值M_i进行变换：

其中，/>与/>分别表示通过障碍物风险评估模型获取的上一时刻与当前时刻威胁最大的障碍物的风险等级；

如果当前时刻最大的障碍物风险等级小于上一时刻最大的障碍物风险等级，给予AUV正向的奖励值，反之给予其负向的惩罚；

6-4)最终，设置强化学习奖励函数表示为：

所述步骤S6，具体为：

(1)训练开始，初始化网络；

(2)进行循环条件判断，当前训练回合数是否小于最大回合数，如果是，则执行步骤(3)，否则训练结束，执行步骤(9)；

(3)初始化AUV位置、姿态、速度、输出力/力矩、目标点位置、障碍物位置以及障碍物运动状态；

(4)根据动态避障系统，AUV基于当前输入的状态s_t选择动作a_t，环境给予其奖励值r，而后处于下一状态s_t+1；

(5)将下一状态s_t+1赋值给当前状态s_t，准备下一次的循环；

(6)判断是否达到动态避障系统的网络更新步，如果是，则执行(7)，否则跳过，直接执行步骤(8)；

(7)从记忆库中抽取样本，对动态避障系统的网络进行更新；

(8)进行循环判断，是否达到回合终止条件(抵达目标点、触碰障碍物、当前步数到达回合内最大步数)，如果是，本回合结束，开始下一回合的训练，返回到步骤(2)，否则开始下一时间步，返回到步骤(4)；

(9)训练结束后，保存训练好的动态避障系统的网络模型，通过查看任务完成情况及奖励值曲线走势，判断训练是否成功；

若系统还未获得完备的避障策略，则重新调整各参数，继续执行设定次数的训练；若系统已经能够成功抵达目标点，同时躲避各类静、动态障碍物，则AUV能够在现实的海洋环境中实时动态避障。

本发明具有以下有益效果及优点：

1.本发明提供了欠驱动水下机器人三维环境下的实时动态避障方法，通过构建欠驱动AUV的操纵性模型，计算AUV与障碍物间的距离、相对纵倾角、相对艏向角，从而获取AUV与障碍物间的相对位置及姿态信息，提出的方法可以直接运用于三维环境中，不需要解耦到二维平面；

2.本发明提供了一种障碍物风险评估方法，当AUV附近存在多个障碍物时，通过风险评估模型计算各个障碍物的风险等级，从而可以筛选出对AUV威胁最大的障碍物，大大提高了避障能力及避障效率，相比于将所有障碍物的信息输入到避障系统中，降低输入数据的维度。

3.本发明提出的基于深度确定性策略梯度算法的动态避障系统，只需要将状态信息输入到系统中，即可端到端地输出对应决策动作，与传统的分析方法相比，搜索能力更强，具有更高的实时性；

4.本发明通过随机化初始环境，设置不同运动状态的动态障碍物，能够增强系统应对复杂环境的能力，提高模型迁移性。

附图说明

图1为本发明的方法流程图；

图2为本发明的AUV运动坐标系示意图；

图3为本发明AUV与障碍物间的相对位置示意图；

图4为本发明基于DDPG算法动态避障系统的网络结构框架图；

图5为本发明AUV的训练的方法流程图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1所示，为本发明的方法流程图，本发明一种欠驱动AUV三维环境下的实时动态避障方法，包括以下步骤：

S1：构建AUV操纵性模型；

S2：基于声纳探测的障碍物信息，计算AUV与障碍物间的相对位置及姿态，用于步骤S3障碍物风险等级判断；

S3：根据步骤S2中获得的AUV与障碍物间的相对位置及姿态，建立障碍物风险评估模型，并判断出不同障碍物的风险等级，筛选出对AUV威胁最大的障碍物；

S4：基于深度确定性策略梯度算法，搭建动态避障系统的网络架构；

S5：设计动态避障系统的输入与输出，将状态信息传输至动态避障系统的网络架构中，得到系统的动作输出，构建出状态输入到动作输出的端到端的动态避障系统，实现状态到动作的映射；将得到的动作输入至步骤S1构建的AUV操纵性模型，从而实现AUV的运动；

S6：设置动态避障系统的强化学习奖励函数；

S7：根据实际场景进行虚拟仿真环境搭建，对动态避障系统进行训练，保存训练好的模型，通过搭载该动态避障系统，实现欠驱动AUV在现实中海洋环境的实时动态避障。

(1)其中，步骤S1中构建水下机器人的操作性模型，具体包括以下步骤：

首先构建AUV的操纵性模型，忽略横摇，AUV在三维空间下由进退、横移、升沉、俯仰以及偏航五部分运动形式组成，运动参考坐标系如图2所示，AUV的位置姿态由η＝[x,y,z,θ,ψ]^T表示，其中，(x,y,z)代表大地坐标系下的位置坐标，θ与ψ分别代表纵倾角与艏向角；速度由v＝[u,v,w,q,r]^T表示，其中(u,v,w)分别代表AUV的纵向速度、横向速度与垂向速度，q与r分别表示纵倾角速度与转艏角速度，AUV的非线性运动学及动力学方程可以表示为：

其中，J(η)是AUV空间运动五自由度坐标转换矩阵；M表示惯性矩阵，由刚体自身惯性矩阵M_RB与附加质量矩阵M_A组成；C(v)为科式向心力矩阵，由刚体科式向心力矩阵C_RB和附加质量引起的科式向心力矩阵C_A组成；D(v)代表阻尼矩阵，由线性阻尼D和非线性二次阻尼D_n组成；g(η)为恢复力矩阵，由重力浮力产生；τ表示控制力、力矩；

J(η)表示为：

为简化模型，便于分析与控制，假设：

AUV具有良好的对称性，自身关于三个剖面对称，重心浮心位置重合，同时AUV具有中性浮力，即重力W与浮力B相等；则惯性积I_xy＝I_yx＝I_xz＝0，重心坐标(x_g,y_g,z_g)＝(0,0,0)，同时恢复力矩阵g(η)所产生的影响忽略不计；

基于上述假设，刚体自身惯性矩阵表示为M_RB＝diag(m,m,m,I_y,I_z)，附加质量矩阵

其中I_y、I_z是AUV绕y、z轴旋转的转动惯量，X_.、Y_.、Z_.、M_.、N_.为水动力系数，与AUV自身的形状等因素有关；

科式向心力矩阵可以表示为：

其中，m为AUV自身质量；

阻尼矩阵表示为：

在本发明中，该AUV为舵桨联合驱动型，它具有兼顾长航程、高速下易操控、成本低等优点，具有更广泛的适用性和更少的硬件要求；

因此，针对欠驱动类型的AUV，矩阵τ可以表示为：

τ＝[τ_u,0,0,τ_q,τ_r]^T (8)

其中τ_u、τ_q、τ_r分别代表纵向推力、纵倾力矩、偏航力矩。

(2)步骤S2中，基于声纳探测的障碍物信息，计算AUV与障碍物间的相对位置及姿态，用于步骤S3障碍物风险等级判断，具体包括以下步骤：

图3为AUV与障碍物间的相对位置示意图，基于前视声纳及侧扫声纳，获取障碍物在大地坐标系下的位置信息η_o＝(x_o,y_o,z_o)^T；

基于AUV与障碍物的位置坐标，计算AUV与障碍物间的相对位置关系：

p_ao＝(x_ao,y_ao,z_ao)^T＝(x_o,y_o,z_o)^T-(x,y,z)^T

其中，p_ao表示由AUV指向障碍物的位置向量，(x_ao,y_ao,z_ao)代表向量的坐标；

基于向量p_ao，可以计算出AUV与障碍物间的距离，即p_ao的模长，表示为：

基于向量p_ao，通过坐标转换矩阵R(θ,ψ)，得到相对位置向量p_ao在刚体自身坐标系上的投影p_r为：

进一步地，基于p_r，得到相对纵倾角θ_r与相对艏向角为ψ_r为：

同理，设定目标点的位置坐标为(x_g,y_g,z_g),基于上述设定，求出AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg；

(3)步骤S3中，建立障碍物风险评估模型，基于AUV与障碍物之间的相对距离、相对纵倾角、相对艏向角几个因素，判断出不同障碍物的风险等级，筛选出对AUV威胁最大的障碍物；

同一时间，声纳可能探测多个障碍物，将其信息全部输入到避障系统中，数据维度过大，难度较高，因此需要建立障碍物风险评估模型，筛选出最具威胁的障碍物；

基于声纳图像算法及位置推算原理，通过判断障碍物的坐标是否发生变化，来划分静态障碍物与动态障碍物，用O_i来表示障碍物i的特性，表示为：

进一步地，构建障碍物的风险评估模型，判断障碍物的风险等级，基于步骤2计算的AUV与障碍物间的相对距离、相对纵倾角及相对艏向角，标号为的i障碍物的风险等级M_i表示为：

/>

其中，k₁、k₂、k₃、m₁、m₂、m₃＞0代表比例系数，表示AUV与障碍物表面间距离的安全阈值，f()为自定义函数；

针对障碍物风险评估模型，下面对表达式的各个部分进行依次介绍：

第一项：

该项与AUV和障碍物间的相对位置、相对姿态有关，且值随着AUV与障碍物间的距离、相对纵倾角、相对艏向角的增大而减小，成负相关，当以下条件满足时，风险等级最大，

当AUV与障碍物间的距离、相对纵倾角与相对艏向角较大时，AUV此时相对安全，该项的数值较小。当AUV能够探测到障碍物时，M_com始终有值，因此被称为常规项，而在障碍物风险评估模型中，第二项与第三项只有在一定条件下才会触发，因此统称为条件项M_con，表示为：

其中，第二项k₂O_i∈[0,k₂]与障碍物动、静态特性有关，当为静态障碍物时，该项为0，当为动态障碍物时，该项的值为k₂；因为动态障碍物更难被预测到下一时刻的运动状态，且发生碰撞时危害更大，因此通常情况下认为动态障碍物的威胁性相对于静态障碍物更高，故而增大了动态障碍物的风险等级；

进一步地，第三项作为条件项M_con的一部分，目的是增大AUV与障碍物表面间的距离小于安全阈值情况时的风险等级；因为安全阈值代表AUV与障碍间表面保持安全的最小距离，当二者之间的距离小于阈值/>时，此时代表AUV目前的状况十分危险，与障碍物发生碰撞的概率极高，而常规项M_com并不能满足该需求，因为/>的变化是线性的，它与/>共同影响着第一项的大小，而/>在阈值附近左右变化时，并不能引起障碍物危险等级M_i的突变，因此添加了第三项，增大距离小于安全阈值时的危险等级；

其中自定义函数f(t)表示为：

因此当时，/>第三项的值为/>当时，/>该项的值为/>分析可得，当AUV与障碍物之间的距离超出设定的安全阈值与障碍物自身半径之和时，该项为常数，不影响各个障碍物的危险等级；而随着距离的减小，小于阈值与障碍物半径之和时，第三项的数值发生变化，随着二者之间距离减小而增大；当/>时，此时M_i→+∞，将AUV视为质点，障碍物视为球体，AUV与物体表面之间的距离为障碍物自身半径，表示AUV与障碍物已发生碰撞，此时风险等级最大；

以上分析了风险评估模型中每项的含义，进一步地，当传感器探测到AUV附件存在多个障碍物时，通过比较各个障碍物的威胁等级M_i，筛选出其中最大的：

i_max对应的障碍物即为当前对AUV威胁最大的障碍物。

(4)步骤S4中，基于深度确定性策略梯度算法，搭建水下机器人动态避障系统；

具体步骤为：

深度确定性策略梯度算法(Deep deterministic policy gradient，DDPG)是一种基于演员-评论家(Actor-Critic，AC)框架的算法，包含Actor与Critic网络，即策略网络和价值网络，如图4所示，Actor与Critic中分别有一个估计网络和目标网络，因此该算法框架共包含4个神经网络；

其中，策略网络的输入为智能体状态，输出为动作；对于估计网络，表示为a_t＝μ_θ(s_t)，其中，μ_θ(s_t)代表策略，s_t与a_t分别代表当前时刻的状态与动作，θ代表Actor中估计网络的参数；

对于Actor中的目标网络，输入为下一时刻的状态s_t+1，输出为下一时刻的动作a_t+1，表示为：其中，/>代表目标网络参数；

因为DDPG算法作为一种确定性策略的算法，随机性较差，因此引入Ornstein-Uhlenbeck(OU)噪声用于增加动作的随机性，表示为：

a_t＝μ_θ(s_t)+N_t (19)

其中，N_t代表OU噪声；

进一步地，通过价值网络拟合价值函数Q(s,a)，从而对智能体执行的动作进行评价；其中价值估计网络的输入为s_t与a_t，输出为Q_w(s_t,a_t)，目标网络的输入为s_t+1、a_t+1，输出

进一步地，在网络结构中设置了一个记忆库，每次训练时，将AUV与环境的交互数据(s_t,a_t,r_t,s_t+1)存储其中，在更新过程，从记忆库中随机抽取N个样本来更新网络参数θ和w；

基于价值网络拟合的价值函数，进行策略网络的更新，利用梯度上升，最大化价值网络输出的Q值，梯度表示为：

其中，代表梯度符号；

价值网络通过计算时序差分，而后通过计算均方误差，利用梯度下降，最小化目标函数，来更新自身网络参数w，目标函数L(w)表示为：

式中，γ∈(0,1)代表折扣因子；

进一步地，通过软更新的方式，更新Actor与Critic中的目标网络参数和/>

其中，ξ∈(0,1)代表滑动平均系数，能够影响和/>更新的幅度；

(5)设计动态避障系统的输入与输出，将状态信息传输至动态避障系统的网络架构中，得到系统的动作输出，构建出状态输入到动作输出的端到端的动态避障系统，实现状态到动作的映射；将得到的动作输入至步骤S1构建的AUV操纵性模型，从而实现AUV的运动；

动态避障系统的输入应包含AUV自身的位置信息及姿态信息η＝[x,y,z,θ,ψ]^T、AUV的速度信息v＝[u,v,w,q,r]^T；

进一步地，基于步骤S3可知，将所有的障碍物信息都输入到系统中，会造成输入维度过大，同时一些距离较远，对AUV威胁较小的障碍物作为干扰项，输入系统后将影响AUV的决策；

因此，基于风险评估模型，找出最具威胁的障碍物i后，将障碍物自身动静态特性O_i、与AUV间的距离相对纵倾角/>相对艏向角/>障碍物半径/>该障碍物的风险等级M_i全部输入到动态避障系统中；同时，将获取到的AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg也作为动态避障系统的输入，则动态避障系统的总输入为：

将s_t作为一个整体，在输入到动态避障系统前，进行归一化和模型堆叠处理，进行归一化处理的目的是提高计算效率，防止梯度爆炸；通过模型堆叠，再将上一时刻的输入状态s_t-1与当前时刻输入状态s_t作为整体输入到动态避障系统中；

进一步地，在本发明中AUV为欠驱动类型，根据步骤S1可知，系统需要输出纵向推力、纵倾力矩及偏航力矩来控制AUV的航行；

实际操作时，需要对系统输出的动作值施加一定的约束，否则输出的动作值可能没有实际物理意义或超出执行机构的最大输出范围，因此将系统输出的动作值控制在(-1,+1)之间，而后对其进行数学变换，这样可以获得理想的动作区间，从而输出与AUV模型适配的动作；

进一步地，基于动态避障模型，实现状态s_t到动作a_t的映射，即：

a_t＝f(s_t) s_t→a_t (25)

AUV根据动态避障系统输出的动作，完成避障任务。

(6)步骤S6中，根据动态避障系统及系统的输入与输出，设置强化学习奖励函数，具体为：

奖励函数的设定与任务需求息息相关，因此在本实施例中，奖励函数设置如下：

AUV在执行任务的过程中，需要到达指定目标位置，因此设立终端奖励项，当AUV到达指定位置后，获得较大的奖励，表示为：

r＝+r₁ r₁＞0 (26)

同理，当AUV与障碍物发生碰撞时，给予其较大的惩罚，表示为：

r＝-r₂ r₂＞0 (27)

进一步地，为了改善避障任务中奖励稀疏的问题，使奖励变得稠密，将任务目标进行分解，设立子目标；

抵达目标点分解为趋近目标点的任务，基于AUV与目标点间的距离d_ag、相对纵倾角θ_ag及相对艏向角ψ_ag，设置奖励项为：

该公式由三部分组成，-w₁d_ag项以使AUV减少与目标点之间的距离，以指导AUV调整自身的姿态，使其朝向目标位置航行，二者的目的都是为了减少受到的惩罚；

进一步地，对于避障而言，训练的目的是使AUV通过调整速度和姿态躲避障碍物带来的威胁，因此基于步骤S3的障碍物风险评估模型，不需要重新设置奖励函数，只需要将避障任务分解为减小障碍物对AUV带来的威胁这一目标，将风险值M_i进行变换：

式中，与/>分别表示通过障碍物风险评估模型获取的上一时刻与当前时刻威胁最大的障碍物的风险等级，注意的是，上一时刻与当前时刻的i_max可能不是同一个障碍物，必须通过风险评估模型计算得来，不能因为当前时刻对AUV威胁最大的障碍物为i，后续过程就仅计算障碍物i的风险等级，否则AUV可能为躲避当前障碍，在决策过程中，转而陷入更大的危险中；

上述可以理解为，上一时刻基于风险评估模型计算出风险等级最大的障碍物是m₁,风险等级为AUV执行动作a₁后，当前时刻m₁的风险等级变为/> 从结果上来看，AUV的决策是正确的，因为障碍物m₁的风险等级变小了；若当前时刻风险等级最大的障碍物为m₂，风险等级为/>且/>虽然AUV通过决策减少了障碍物m₁所带来的威胁，然而却转移到了更大危险中，因此，公式(28)中t-1与t时刻中的i_max都是当前决策时刻风险等级最大的障碍物；

如果当前时刻最大的障碍物风险等级小于上一时刻最大的障碍物风险等级，给予AUV正向的奖励值，反之给予其负向的惩罚。

综上，奖励函数可以表示为：

(7)步骤S7：根据实际场景进行虚拟仿真环境搭建，对动态避障系统进行训练，保存训练好的模型，通过搭载该动态避障系统，即可实现欠驱动AUV在现实中海洋环境的实时动态避障。

基于实际场景搭建虚拟仿真环境，通过设定目标点生成位置、改变AUV初始位置及姿态、设立静态、运动形式各异的动态障碍物(直线运动、曲线运动、匀速运动、匀加速等)，从而增强环境的随机性，提高训练后的系统对环境的适应力，以应对复杂多变的环境；

进一步地，设置实验相关参数(最大训练回合数、每回合最大训练步数等)及系统参数(网络参数、障碍物风险评估模型系数、奖励函数系数)等，而后对系统进行训练；

如图5为AUV的训练流程，可以描述为：

(1)训练开始，初始化网络；

(2)进行循环条件判断，当前训练回合数是否小于最大回合数，如果是，则执行步骤(3)，否则训练结束，执行步骤(9)；

(3)初始化AUV位置、姿态、速度、输出力/力矩、目标点位置、障碍物位置以及障碍物运动状态；

(4)根据动态避障系统，AUV基于当前输入的状态s_t选择动作a_t，环境给予其奖励值r，而后处于下一状态s_t+1；

(5)将下一状态s_t+1赋值给当前状态s_t，准备下一次的循环；

(6)判断是否达到动态避障系统的网络更新步，如果是，则执行(7)，否则跳过，直接执行步骤(8)；

(7)从记忆库中抽取样本，对动态避障系统的网络进行更新；

(8)进行循环判断，是否达到回合终止条件(抵达目标点、触碰障碍物、当前步数到达回合内最大步数)，如果是，本回合结束，开始下一回合的训练，返回到步骤(2)，否则开始下一时间步，返回到步骤(4)；

(9)训练结束后，保存训练好的动态避障系统的网络模型，通过查看任务完成情况及奖励值曲线走势，判断训练是否成功；

若系统还未获得完备的避障策略，则重新调整各参数，继续执行设定次数的训练，若系统已经能够成功抵达目标点，同时躲避各类静、动态障碍物，则该动态避障系统已经能够应对复杂多变的海洋环境，将其运动到真实环境中，通过机器人搭载的各类传感器获取状态信息，输入到系统中后，AUV即可获得避障策略，通过调整速度及角速度，从而改变自身航线及姿态，实现在现实海洋环境中的实时动态避障。

以上所述仅为本发明的实施方式，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进、扩展等，均包含在本发明的保护范围内。

Claims (10)

1.一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，包括以下步骤：

S1：构建AUV操纵性模型；

S2：通过声纳探测的障碍物信息，计算AUV与障碍物间的相对位置及姿态；

S3：根据步骤S2中获得的AUV与障碍物间的相对位置及姿态，建立障碍物风险评估模型，并判断出不同障碍物的风险等级，筛选出对AUV威胁最大的障碍物；

S4：基于深度确定性策略梯度算法，搭建动态避障系统的网络架构；

S5：设计动态避障系统的输入与输出，将状态信息传输至动态避障系统的网络架构中，得到系统的动作输出，构建出状态输入到动作输出的端到端的动态避障系统，实现状态到动作的映射；将得到的动作输入至步骤S1构建的AUV操纵性模型，从而实现AUV的运动；

S6：设置动态避障系统的强化学习奖励函数；

S7：根据实际场景进行虚拟仿真环境搭建，对动态避障系统进行训练，保存训练好的模型，通过搭载该动态避障系统，实现欠驱动AUV在现实中海洋环境的实时动态避障。

2.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述步骤S1，具体为：

构建AUV的操纵性模型，忽略横摇，AUV在三维空间下由进退、横移、升沉、俯仰以及偏航五部分运动形式组成，则AUV的位置姿态为：

η＝[x,y,z,θ,ψ]^T

其中，(x,y,z)代表大地坐标系下的位置坐标，θ与ψ分别代表纵倾角与艏向角；

AUV的速度由v＝[u,v,w,q,r]^T表示；

其中，(u,v,w)分别代表AUV的纵向速度、横向速度与垂向速度，q与r分别表示纵倾角速度与转艏角速度；

AUV的非线性运动学及动力学方程表示为：

其中，J(η)是AUV空间运动五自由度坐标转换矩阵；M表示惯性矩阵，C(v)为科式向心力矩阵，D(v)代表阻尼矩阵，g(η)为恢复力矩阵，τ表示控制力、力矩，针对欠驱动类型的AUV，矩阵τ表示为：

τ＝[τ_u,0,0,τ_q,τ_r]^T

其中τ_u、τ_q、τ_r分别代表AUV的纵向推力、纵倾力矩、偏航力矩。

3.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述步骤S2，包括以下步骤：

2-1)通过AUV搭载的前视声纳及侧扫声纳，获取障碍物在大地坐标系下的位置信息η_o＝(x_o,y_o,z_o)^T；

2-2)根据AUV的位置坐标与障碍物的位置坐标，计算AUV与障碍物间的相对位置关系，即：

p_ao＝(x_ao,y_ao,z_ao)^T＝(x_o,y_o,z_o)^T-(x,y,z)^T

其中，p_ao表示由AUV指向障碍物的位置向量，(x_ao,y_ao,z_ao)代表向量的坐标；

2-3)根据AUV指向障碍物的位置向量p_ao，获取AUV与障碍物间的距离，即p_ao的模长：

2-4)根据AUV指向障碍物的位置向量p_ao，通过坐标转换矩阵R(θ,ψ)，得到相对位置向量p_ao在AUV自身坐标系上的投影p_r为：

2-5)根据投影p_r获取AUV与障碍物间的相对位姿，相对位姿包括：相对纵倾角θ_r与相对艏向角为ψ_r，即：

2-6)基于目标点的位置坐标(x_g,y_g,z_g)，根据步骤2-1)～步骤2-5)，获取AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg。

4.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述步骤S3，具体为：

3-1)通过判断障碍物的坐标是否发生变化，划分静态障碍物与动态障碍物，用O_i来表示障碍物i的特性，即：

3-2)构建障碍物的风险评估模型，以判断障碍物的风险等级；根据AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg，则标号为i的障碍物的风险等级M_i表示为：

其中，k₁、k₂、k₃、m₁、m₂、m₃均为大于零的比例系数，d表示AUV与障碍物表面间距离的安全阈值，d_aoi代表AUV与障碍物i的距离，r_oi代表障碍物i的半径，f()为自定义函数；

将上式简化为：

M_i＝M_com+M_con

其中，M_com定义为常规项，M_con定义为条件项；

3-3)当声纳探测到AUV附近存在多个障碍物时，获取不同障碍物的风险等级，通过比较各个障碍物的风险等级M_i，筛选出M_i最大的障碍物，即：

其中，i_max为对应的障碍物即为当前对AUV威胁最大的障碍物。

5.根据权利要求4所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，判断出不同障碍物的风险等级，具体为：

a.当AUV能够探测到障碍物时，M_com始终有值，M_com与AUV和障碍物间的距离d_ao、相对纵倾角θ_r、相对艏向角为ψ_r有关，且值随AUV与障碍物间的距离、相对纵倾角、相对艏向角的增大而减小，成负相关，则当以下条件满足时，风险等级最大；

b.在障碍物风险评估模型中，条件项M_con，表示为：

条件项M_con中，k₂O_i∈[0,k₂]与障碍物的动、静态特性相关，判定动态障碍物的威胁性相对于静态障碍物高，增大了动态障碍物的风险等级；

当障碍物为静态障碍物时，k₂O_i为0；

当障碍物为动态障碍物时，k₂O_i的值为k₂；

c.条件项M_con中，在中，其自定义函数f(t)表示为：

当时，/>则该项值为/>

当时，/>该项的值为：/>

判定当AUV与障碍物之间的距离超出设定的安全阈值与障碍物自身半径之和时，为常数，不影响各个障碍物的风险等级；随着AUV与障碍物之间距离的减小，当小于阈值与障碍物半径之和时，/>的数值随着二者之间距离减小而增大；

当时，此时M_i→+∞，将AUV视为质点，障碍物视为球体，AUV与物体表面之间的距离为障碍物自身半径，表示AUV与障碍物已发生碰撞，此时风险等级最大。

6.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述深度确定性策略梯度算法为基于AC框架的算法；

AC框架包含策略网络Actor与价值网络Critic；其中，策略网络Actor与价值网络Critic分别设有一个估计网络和一个目标网络。

7.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述步骤S4，具体为：

4-1)策略网络Actor的输入为AUV的状态，输出为动作；

对于策略网络Actor中的估计网络，表示为：a_t＝μ_θ(s_t)，其中，μ_θ(s_t)代表策略，s_t与a_t分别代表当前时刻的状态与动作，θ代表Actor中估计网络的参数；

对于策略网络Actor中的目标网络，输入为下一时刻的状态s_t+1，输出为下一时刻的动作a_t+1，表示为：其中，/>代表目标网络参数；

对策略网络Actor中的估计网络引入OU噪声，以增加动作的随机性，即：

a_t＝μ_θ(s_t)+N_t

其中，N_t代表OU噪声；

4-2)通过价值网络Critic拟合价值函数Q(s,a)，对AUV执行的动作进行评价；其中价值估计网络的输入为s_t与a_t，输出为Q_w(s_t,a_t)，价值目标网络的输入为s_t+1、a_t+1，输出

4-3)在网络结构中设置了一个记忆库，当每次训练时，将AUV与环境的交互数据(s_t,a_t,r_t,s_t+1)存储在记忆库中，在更新过程，从记忆库中随机抽取N个样本更新网络参数θ和w；

4-4)基于价值网络Critic拟合的价值函数Q_w(s_t,a_t)，进行策略网络的更新，利用梯度上升，使价值网络Critic输出的Q值最大化，则梯度表示为：

其中，代表梯度符号；

4-5)通过计算价值网络Critic的时序差分，再计算均方误差，利用梯度下降，最小化目标函数，更新自身网络参数w，其中，最小化目标函数L(w)表示为：

其中，γ∈(0,1)代表折扣因子；

4-6)通过软更新的方式，更新策略网络Actor与价值网络Critic中的目标网络参数和即：

其中，ξ∈(0,1)代表滑动平均系数，能够影响和/>更新的幅度；

4-7)完成AUV动态避障系统中策略网络Actor和价值网络Critic网络框架的建立。

8.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述设计动态避障系统的输入与输出，并构建出状态输入到动作输出的端到端模型，实现二者之间的映射，具体为：

5-1)动态避障系统的输入包含AUV自身的位置信息及姿态信息η＝[x,y,z,θ,ψ]^T，和AUV的速度信息v＝[u,v,w,q,r]^T；

5-2)根据障碍物风险评估模型，找出最具威胁的障碍物i后，将障碍物自身动静态特性O_i、与AUV间的距离相对纵倾角/>相对艏向角/>障碍物半径/>该障碍物的风险等级M_i全部输入到动态避障系统中；同时，将获取到的AUV与目标点间的距离d_rg、相对纵倾角θ_rg与相对艏向角ψ_rg也作为动态避障系统的输入，则动态避障系统的总输入为：

5-3)将s_t作为一个整体进行归一化处理后，通过模型堆叠，再将上一时刻的输入状态s_t-1与当前时刻输入状态s_t作为整体输入到动态避障系统中；

5-4)将动态避障系统输出的动作值控制在(-1,+1)之间，对其进行数学变换，从而输出与AUV模型适配的动作；

5-5)基于动态避障系统，实现状态s_t到动作a_t的映射，即：

a_t＝f(s_t)s_t→a_t

5-6)AUV根据动态避障系统输出的动作，完成避障任务。

9.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述根据动态避障系统及设计动态避障系统的输入与输出，设置强化学习奖励函数，具体为：

6-1)AUV在执行任务的过程中，需要到达指定目标位置，因此设立终端奖励项，当AUV到达指定位置后，获得奖励，表示为：

r＝+r₁ r₁＞0

当AUV与障碍物发生碰撞时，给予其惩罚，表示为：

r＝-r₂ r₂＞0

6-2)基于AUV与目标点间的距离d_ag、相对纵倾角θ_ag及相对艏向角ψ_ag，设置奖励项为：

其中，-w₁d_ag以使AUV减少与目标点之间的距离，以指导AUV调整自身的姿态，使其朝向目标位置航行；

6-3)根据障碍物风险评估模型，将避障任务分解为：减小障碍物对AUV带来的威胁，将风险值M_i进行变换：

其中，与/>分别表示通过障碍物风险评估模型获取的上一时刻与当前时刻威胁最大的障碍物的风险等级；

如果当前时刻最大的障碍物风险等级小于上一时刻最大的障碍物风险等级，给予AUV正向的奖励值，反之给予其负向的惩罚；

6-4)最终，设置强化学习奖励函数表示为：

10.根据权利要求1所述的一种欠驱动AUV三维环境下的实时动态避障方法，其特征在于，所述步骤S6，具体为：

(1)训练开始，初始化网络；

(2)进行循环条件判断，当前训练回合数是否小于最大回合数，如果是，则执行步骤(3)，否则训练结束，执行步骤(9)；

(3)初始化AUV位置、姿态、速度、输出力/力矩、目标点位置、障碍物位置以及障碍物运动状态；

(4)根据动态避障系统，AUV基于当前输入的状态s_t选择动作a_t，环境给予其奖励值r，而后处于下一状态s_t+1；

(5)将下一状态s_t+1赋值给当前状态s_t，准备下一次的循环；

(6)判断是否达到动态避障系统的网络更新步，如果是，则执行(7)，否则跳过，直接执行步骤(8)；

(7)从记忆库中抽取样本，对动态避障系统的网络进行更新；

(8)进行循环判断，是否达到回合终止条件(抵达目标点、触碰障碍物、当前步数到达回合内最大步数)，如果是，本回合结束，开始下一回合的训练，返回到步骤(2)，否则开始下一时间步，返回到步骤(4)；

(9)训练结束后，保存训练好的动态避障系统的网络模型，通过查看任务完成情况及奖励值曲线走势，判断训练是否成功；

若系统还未获得完备的避障策略，则重新调整各参数，继续执行设定次数的训练；若系统已经能够成功抵达目标点，同时躲避各类静、动态障碍物，则AUV能够在现实的海洋环境中实时动态避障。