CN109784201A - 基于四维风险评估的auv动态避障方法 - Google Patents

Abstract

本发明所述基于四维风险评估的AUV动态避障方法，建立增加相对时间信息的四维风险评估模型，应用采取连续状态离散动作空间的强化学习算法DQN，以适应不确定的环境空间，实现提高AUV自学习能力与自适应性能力、以自主动态避障提高AUV航行的安全性能。主要包括有以下步骤：1)构建实用地图系统，在巡航状态下，根据前视声呐采集的数据构建障碍物相对于AUV的三维位置地图；2)建立四维风险评估模型；筛选出重点障碍物进行避碰，根据风险系数，选择避障动作与否；3)根据AUV当前状态，经强化学习，输出避碰动作行为；输出的动作行为，经底层控制模块传输至执行机构，形成具体的避碰行为作用于AUV；4)处理将强化学习样本处理后存储在样本库中；抽取样本，强化学习进行学习更新。

Description

基于四维风险评估的AUV动态避障方法

技术领域

本发明涉及一种水下机器人的动态避碰方法，具体地采用四维风险评估与改进的DQN强化学习组合的方式，属于机器人智能控制技术领域。

背景技术

面对深海环境的未知性和复杂性，AUV的航行控制不能完全按照预先规划的路径航 行。要保证AUV在海洋中长期安全地航行，必须能够对航行过程中遇到的各种静、动态障碍物进行自主避障。目前，在海洋环境中对于已知的静态障碍，AUV的避障技术已经 得到应用，但是针对动态障碍物的威胁，AUV的避障技术还不够成熟，因此，为了使AUV 在深海环境下安全航行，需要设计动态避障技术来避免未知障碍物的干扰，这也成为AUV 避障问题上的研究热点。

在目前研究较多的诸多的避障方法中，主要方法有人工势场法、模板匹配方法、地图构建方法和神经网络等人工智能路径规划方法等。而以上算法通常依赖于被控对象精确的数学模型和完整全面的环境信息。当避障过程相对复杂,障碍物多,环境信息不全, 被控对象的数学模型难以确定时,避障效果往往不能令人满意。

如公开以下方案在先申请专利，申请号CN201210539473.X，名称为一种智能水下机器人的自主避障方法，其方案是将水下机器人运动目标、障碍物和水下机器人控制性 能进行统一考虑来实现避障。包括以下步骤：根据作业任务和海图数据库信息进行全局 路径规划；读取水下机器人运动传感器信息、避碰声纳数据信息；计算水下机器人警戒 安全距离，建立水下机器人安全虚拟外壳；判断是否需要避障；判断是否达到目标，若 到达目标，任务完成。本发明将水下机器人速度息的影响引入机器人避障策略中，将水 下机器人局部避碰规划与控制、水动力性能相结合，制定的避障策略能够反映智能水下 机器人动态避障能力。

上述现有技术主要存在的缺点有，一是，如果AUV把所有检测到的物体都归为障碍物来进行避障，那么无疑会加大避障的难度和计算复杂度，增加了避障的时间，进而会 影响AUV整个任务的执行。事实上检测到的物体中有一部分对AUV是没有威胁的，比如 其移动方向是远离AUV的，这类物体对AUV是没有威胁的，可以选择不避障。二是，目 前研究的大多方法通常需要假设完整的环境配置信息和充分的经验，然而，在大量的实 际应用中，AUV往往需要作业于未知的环境中，复杂且难以预测的水下环境，对AUV的 提出了极高的要求。三是，目前采用的强化学习避障方法中，大多基于离散的状态空间 的，这会产生维数灾难问题，又AUV与障碍物的相对状态比较复杂多样，连续的状态空 间是必要的，连续的动作是没有必要的。

有鉴于此，特提出本专利申请。

发明内容

本发明所述基于四维风险评估的AUV动态避障方法，其目的在于解决上述现有技术存在的问题而建立增加相对时间信息的四维风险评估模型，应用采取连续状态离散动作空间的强化学习算法DQN，以适应不确定的环境空间，实现提高AUV自学习能力与自 适应性能力、以自主动态避障提高AUV航行的安全性能。

为实现上述设计目的，所述的基于四维风险评估的AUV动态避障方法，主要包括有以下步骤：

1)构建实用地图系统

在巡航状态下，根据前视声呐采集的数据构建障碍物相对于AUV的三维位置地图，障碍物相对AUV方位s_obsψ和距离s_obsdist，AUV相对子目标点的方位s_goalψ和距离s_goaldist；

检测并存储障碍物动态信息，包括障碍物在Δt内的速度v_obs、俯仰角θ_obs和航向ψ_obs信息，形成用于构建四维风险评估模型的相对于AUV视角的局部三维环境地图；

2)建立四维风险评估模型，筛选重点障碍物

以AUV当前姿态和速度航行Δt之后的距离定义为碰撞距离dist，建立四维风险评估 模型以设定模型参数，包括设定最小碰撞距离dist_min、最大碰撞距离dist_max、判断避障失败的距离dist_fail、最小风险系数risk_min和最大风险系数risk_max；根据风险系数，选择 避障动作与否；

3)强化学习输出避碰动作行为

将全局规划的路径点作为避障的子目标点，通过规划姿态控制策略来优化到达子目 标点的局部路径；

根据AUV与子目标点和障碍物的相对状态信息s_t＝(s_obsψ,s_obsdist,s_goalψ,s_goaldist)，经强 化学习，输出避碰动作行为；

输出的动作行为a_t＝(ψ_des,θ_des,v_des)，期望航向ψ_des，期望俯仰θ_des，期望速度v_des，经 底层控制模块传输至执行机构，形成AUV具体的避碰行为；

4)强化学习样本处理和学习更新

计算奖赏r_t，并将上一时刻的状态信息s_t下一障碍物状态s_t+1动作a_t和奖赏r_t存储于 样本库中；

采用DQN强化学习方法与经验回放方式，随机抽样进行学习，更新DQN的网络参数。

如上述基本设计构思，将时间信息包括在内的四维碰撞风险评估方式，对检测到的 物体进行一定时间内的风险评估，得到相应障碍物的风险系数，根据避碰距离产生的危险系数risk对避碰效果进行量化，筛选重点的障碍物进行避碰，这样能够减少避障的时间，避免不必要的障碍物的处理，提高避障和AUV航行效率。

为了提高AUV避障动作的可靠稳定性能，进一步地改进与优化方案是，在所述的步骤4)中，DQN学习方法采取离线训练与在线学习相结合的方式；

采用离线训练和在线学习结合的方法，在实际应用前，模拟各种场景进行训练并保 存到模型库。在实际应用的同时，保存各种在线数据，当出现长时间避障不成功的情况，开始在线学习。所述的DQN强化学习方法，学习时采取经验回放和专家知识辅助的方 式，对回报函数进行设置，将历史经验数据和专家经验进行存储，随机获取样本数据结 合专家知识进行训练，保证该算法的收敛性和全局最优性。

为提高所述DQN强化学习方法的效果，针对性地指导避障动作优化的方向，即获得某一状态下所执行动作的优劣结果，可在所述回报函数的设置时，综合避碰效果和趋近 子目标点效果；

取两者结合，则回报函数为

上述步骤3)中形成的障碍物信息和动作信息、步骤4)中形成的奖赏信息存储到样本库中。

所述的障碍物信息包括，障碍物相对AUV方位s_obsψ和距离s_obsdist，AUV相对子目 标点的方位s_goalψ和距离s_goaldist。

针对所述在线学习的优选实施方式是，在线学习方法包括以下步骤：

初始化，Q函数网络Q(s,a|θ^Q)←θ^Q；

初始化数据库，强化学习样本库D_RL；

将障碍物信息s_t＝(s_obsψ,s_obsdist,s_goalψ,s_goaldist)输入DQN模型，并输出AUV的避碰动 作a_t＝(ψ_des,θ_des,v_des)以调整其航速、航向与俯仰角；

AUV执行避碰动作，并更新下一障碍物状态s_t+1；

获得奖赏反馈r_t，进行学习并更新DQN模型的网络参数；设置y_t＝r_t+γ(Q'(s_t+1,a_t)， 通过最小化loss更新critic网络L(θ^Q)＝E[(Q(s_t,a_t|θ^Q)-y_t)²]；

重复以上步骤，直到任务完成。

应用上述在线学习方法，当AUV陷入复杂障碍物群而导致长时间避障不成功之后，即 启动在线学习模式。其中，输入为AUV与障碍物的相对方位和AUV与目标点的相对方位，输出为期望的航向，俯仰和速度值。

AUV的当前位置作为反馈参数可直接从导航系统获得，障碍物的三维位置从前视声呐 获得，AUV与障碍物的相对方位和AUV与目标点的相对方位可以从位置解算模块获得。在 经本申请所述的方法训练和学习之后，获得相应的期望航向，俯仰和速度，同时存储强化学习历史数据，根据专家知识，设置合理的回报函数，能够加快学习速度。AUV底层 控制模块可以根据当前和期望信息，计算执行机构的期望指令，最终实现AUV的自主动 态避障。

综上内容，所述基于四维风险评估的AUV动态避障方法具有的优点是：

1、基于无模型的控制，而不依赖于完整的环境配置信息与充分的经验信息；同时，基 于强化学习方法可以无需先验知识，因此具有很强的自学习能力与未知环境下的自 适应能力。

2、强化学习具有较好的鲁棒性，能适应水下环境的复杂多样性，且在环境模型未知领 域中具有较高的应用价值。

3、建立增加相对时间信息的四维风险评估模型，筛选重点的障碍物进行避碰，提高避 障和航行效率，应用采取连续状态离散动作空间的强化学习算法DQN，真正实现了自主、动态地避障能力，有利于提高AUV航行的安全性能。

附图说明

图1是应用所述动态避障方法的AUV模块执行示意图；

图2是实用地图框架的构建示意图；

图3是四维风险评估模型的示意图；

图4是四维风险评估模型的参数设置示意图；

图5是四维风险评估结果示意图；

图6是所述AUV动态避障方法的流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

实施例1，如图1和图6所示，所述基于四维风险评估的AUV动态避障方法，包括 有以下实施步骤：

1)构建实用地图系统

在巡航状态下，根据前视声呐采集的数据构建障碍物相对于AUV的三维位置地图，障碍物相对AUV方位s_obsψ和距离s_obsdist，AUV相对子目标点的方位s_goalψ和距离s_goaldist；

检测并存储障碍物动态信息，包括障碍物在Δt内的速度v_obs、俯仰角θ_obs和航向ψ_obs信息，形成用于构建四维风险评估模型的相对于AUV视角的局部三维环境地图；

如图2所示，对于避障来说，重要的是知道障碍物相对于AUV的位置，而不是障 碍物的全局位置，构建相对于AUV位置的地图可以避免导航精度带来障碍物位置误差 的问题。

地图构建说明如下：

以AUV声呐安装位置为原点(0,0,0)，体坐标系(X_b,Y_b,Z_b)AUV航向为Y轴，水 平垂直于航向方向为X轴，Z轴，构建坐标系，Δt时间内，假设AUV航向俯仰和航速 是定值速度v_auv俯仰θ_auv和航向ψ_auv，当AUV沿着一定航向俯仰航行时，坐标系进行相 应更新，则坐标系原点移动的X轴Y轴Z轴的量分别为， (Δx,Δy,Δz),Δx,＝v_auv*Δt*cos(θ_auv)*cos(ψ_auv),Δy＝v_auv*Δt*cos(θ_auv)*sin(ψ_auv),Δz＝v_auv*Δt*sin(θ_auv) 快速地检测动态障碍物信息，包括障碍物的位置，大小和速度等信息。

检测声呐图像显示的障碍物轮廓，找出左右最远点和上下最远点，构造合适的椭圆， 将障碍物包括在内。

假设障碍物在Δt内速度v_obs俯仰θ_obs和航向ψ_obs也是固定的，声呐测得障碍物位置相对于当前坐标系位置为(x1,y1,z1)，障碍物新测得的位置(x2,y2,z2)，而上一时刻障碍物相对于当前坐标系的位置为(x₂₁,y₂₁,z₂₁)＝(x1-Δx,y1-Δy,z1-Δz)，则障碍物航行的速 度为

方向为ψ_obs＝arctan((y21-y2)/(x21-x2))，

据此可以判断出障碍物的动态信息；

存储以上信息，形成相对于AUV视角的局部地图。

2)建立四维风险评估模型，筛选重点障碍物

以AUV当前姿态和速度航行Δt之后的距离定义为碰撞距离，建立四维风险评估模型 以设定模型参数，包括设定最小碰撞距离dist_min、最大碰撞距离dist_max、判断避障失败的距离dist_fail、最小风险系数risk_min和最大风险系数risk_max；

如图3所示，建立四维风险碰撞评估模型，以计算出碰撞距离。

设AUV位置，航速，航向和俯仰(x_auv,y_auv,v_auv,ψ_auv,θ_auv)，动态障碍物 (x_obs,y_obs,v_obs,ψ_obs,θ_obs)，以当前的姿态和速度航行Δt之后的距离定义为碰撞距离。

AUV经过Δt时间后在x轴移动量为：Δx_auv＝x_auv+v_auvΔtcosθ_auvcosψ_auv

在y轴移动量为：Δy_auv＝y_auv+v_auvΔtcosθ_auvsinψ_auv

在z轴移动量为：Δz_auv＝z_auv+v_auvΔtsinθ_auv

障碍物经过Δt时间后在x轴移动量为：Δx_obs＝x_obs+v_obsΔtcosθ_obscosψ_obs

在y轴移动量为：Δy_obs＝y_obs+v_obsΔtcosθ_obssinψ_obs

在z轴移动量为：Δz_obs＝z_obs+v_obstsinθ_obs

则经过后，AUV与障碍物之间的距离的平方即碰撞距离为：

dist²＝(Δx_auv-Δx_obs)²+(Δy_auv-Δy_obs)²+(Δz_auv-Δz_obs)²

如图4所示，在建立碰撞风险评估模型中，对评估模型的参数进行如下设置：

如图4所示，在建立碰撞风险评估模型中，对评估模型的参数进行如下设置：

将该函数关于Y轴对称，原点右移到risk_min和risk_max的中点，σ按照实际情况选取使横坐标伸长以使函数变化符合实际需求，在这里选取5.5，则得到如下结果：

如图5所示的风险评估结果示意图。

根据风险系数，选择避障动作与否，筛选出重点障碍物进行避碰，危险系数等于0的物体可以忽略；

3)强化学习输出避碰动作行为 将全局规划的路径点作为避障的子目标点，通过规划姿态控制策略来优化到达子目 标点的局部路径；

根据AUV与子目标点和障碍物的相对状态信息s_t＝(s_obsψ,s_obsdist,s_goalψ,s_goaldist)，经强 化学习，输出避碰动作行为；

输出的动作行为a_t＝(ψ_des,θ_des,v_des)，期望航向ψ_des，期望俯仰θ_des，期望速度v_des，经 底层控制模块传输至执行机构，形成AUV具体的避碰行为；

4)强化学习样本处理和学习更新

计算奖赏r_t，并将上一时刻的状态信息s_t下一障碍物状态s_t+1动作a_t和奖赏r_t存储于 样本库中；

采用DQN强化学习方法与经验回放方式，随机抽样进行学习，更新DQN的网络参数。

DQN学习方法采取离线训练与在线学习相结合的方式，在实际应用前，模拟各种场景进行训练并保存到模型库。在实际应用的同时，保存各种在线数据，当出现长时间避 障不成功的情况，开始在线学习；所述的DQN强化学习方法，引入专家知识对回报函 数进行设置，根据避碰距离产生的危险系数risk对避碰效果进行量化。

为了既考虑全局优化指标，又能根据局部的动态海洋环境信息进行实时的优化，将 全局规划的路径点作为避障的子目标点，通过规划姿态控制策略来优化到达子目标点的 局部路径。为了避免陷入局部最优值，采用经验回放的方式，将历次经验数据进行保存，每次随机抽样进行学习。

基本参数设置如下：

设置动作空间：动作空间进行离散化a_t＝(ψ_des,θ_des,v_des),其中，期望航向ψ_des：(-90，-60,-30，0,+30,+60,+90)+为右转、-为左转；期望俯仰：(-30,-15,0,+15,+30)、+ 为上浮、-为下潜；期望速度v_des(0,0.5,1,1.5,2)米/秒；

设置状态空间：s_t＝(s_obsψ,s_obsdist,s_goalψ,s_goaldist)，其中，障碍物相对AUV方位s_obsψ和 距离s_obsdist，AUV相对子目标点的方位s_goalψ和距离s_goaldist；

神经网络设置：为了提高稳定性，搭建两个神经网络,一个用于预测目标值,他不会及时更新参数，另用于预测估计值，会定时被目标值神经网络的参数更新。两个拟合 状态值的神经网络使用多层完全连接的深度神经网络结构，其中三个隐藏的完全连接 层。

回报函数设置：综合避碰效果和趋近子目标点效果，避碰效果为AUV到障碍物的距离d_obs(距离越远越好，取正值)，趋近子目标点效果为AUV到目标点的距离-d_goal(距 离越近越好，取负值)，若碰撞则给定-1000。引入专家知识将两者结合，根据避碰距离 产生的危险系数risk对避碰效果进行量化：危险系数risk最大100％，最小0；则在小于 30米时，risk＝100％，即回报完全由避碰效果产生；大于100米，risk＝0，即回报完 全由去向目标点效果产生，中间函数变化处理。

将样本信息{s_t,a_t,s_t+1,r_t}存储到数据库；

在一定时间之后，离线训练的结果仍然没有使避障成功后，进行在线学习。

所述的在线学习包括以下步骤：

初始化，Q函数网络Q(s,a|θ^Q)←θ^Q；

初始化数据库，强化学习样本库D_RL；

将障碍物信息s_t输入DQN模型，并输出AUV的避碰动作a_t以调整其航速、航向与 俯仰角；

AUV执行避碰动作，并更新下一障碍物状态s_t+1；

获得奖赏反馈r_t，进行学习并更新DQN模型的网络参数；设置y_t＝r_t+γ(Q'(s_t+1,a_t)， 通过最小化loss更新critic网络L(θ^Q)＝E[(Q(s_t,a_t|θ^Q)-y_t)²]；

重复以上步骤，直到任务完成。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (6)

1.一种基于四维风险评估的AUV动态避障方法，其特征在于：包括有以下步骤，

1)构建实用地图系统

在巡航状态下，根据前视声呐采集的数据构建障碍物相对于AUV的三维位置地图，障碍物相对AUV方位s_obsψ和距离s_obsdist，AUV相对子目标点的方位s_goalψ和距离s_goaldist；

检测并存储障碍物动态信息，包括障碍物在Δt内的速度v_obs、俯仰角θ_obs和航向ψ_obs信息，形成用于构建四维风险评估模型的相对于AUV视角的局部三维环境地图；

2)建立四维风险评估模型，筛选重点障碍物

以AUV当前姿态和速度航行Δt之后的距离定义为碰撞距离dist，建立四维风险评估模型以设定模型参数，包括设定最小碰撞距离dist_min、最大碰撞距离dist_max、判断避障失败的距离dist_fail、最小风险系数risk_min和最大风险系数risk_max；根据风险系数，选择避障动作与否；

3)强化学习输出避碰动作行为将全局规划的路径点作为避障的子目标点，通过规划姿态控制策略来优化到达子目标点的局部路径；

根据AUV与子目标点和障碍物的相对状态信息s_t＝(s_obsψ,s_obsdist,s_goalψ,s_goaldist)，经强化学习，输出避碰动作行为；

输出的动作行为a_t＝(ψ_des,θ_des,v_des)，期望航向ψ_des，期望俯仰θ_des，期望速度v_des，经底层控制模块传输至执行机构，形成AUV具体的避碰行为；

4)强化学习样本处理和学习更新

计算奖赏r_t，并将上一时刻的状态信息s_t下一障碍物状态s_t+1动作a_t和奖赏r_t存储于样本库中；

采用DQN强化学习方法与经验回放方式，随机抽样进行学习，更新DQN的网络参数。

2.根据权利要求1所述的基于四维风险评估的AUV动态避障方法，其特征在于：在所述的步骤4)中，DQN学习方法采取离线训练与在线学习相结合的方式；

所述的DQN强化学习方法，引入专家知识对回报函数进行设置，根据避碰距离产生的危险系数risk对避碰效果进行量化。

3.根据权利要求2所述的基于四维风险评估的AUV动态避障方法，其特征在于：在所述回报函数的设置时，综合避碰效果和趋近子目标点效果，避碰效果为AUV到障碍物的距离d_obs(距离越远越好，取正值)，趋近子目标点效果为AUV到目标点的距离-d_goal(距离越近越好，取负值)，若碰撞则给定-1000；

取两者结合，则回报函数为

4.根据权利要求1所述的基于四维风险评估的AUV动态避障方法，其特征在于：将所述步骤2)和3)中形成的障碍物信息和动作信息、步骤4)中形成的奖赏信息存储到样本库中。

5.根据权利要求4所述的基于四维风险评估的AUV动态避障方法，其特征在于：所述的障碍物信息包括，重点障碍物距AUV的方位信息、以及AUV距离目标点的方位信息。

6.根据权利要求1或3所述的基于四维风险评估的AUV动态避障方法，其特征在于：所述的在线学习包括以下步骤，

初始化，Q函数网络Q(s,a|θ^Q)←θ^Q；

初始化数据库，强化学习样本库D_RL；

将障碍物信息s_t输入DQN模型，并输出AUV的避碰动作a_t以调整其航速、航向与俯仰角；

AUV执行避碰动作，并更新下一障碍物状态s_t+1；

获得奖赏反馈r_t，进行学习并更新DQN模型的网络参数；设置y_t＝r_t+γ(Q'(s_t+1,a_t)，通过最小化loss更新critic网络L(θ^Q)＝E[(Q(s_t,a_t|θ^Q)-y_t)²]；

重复以上步骤，直到任务完成。