一种基于虚拟膨化的运动障碍与uuv相向航行的规避方法
【技术领域】
[0001 ]本发明涉及基于虚拟膨化的运动障碍与UUV相向航行的规避方法。
【背景技术】
[0002] 近年来，关于运动障碍物规避策略的成果不计其数，总结起来大致分为3大类，第 一类是先预测运动障碍的运动趋势，然后根据不同的趋势作出相应的对策;第二类是在动 态规避的瞬间，将运动障碍固化成静态障碍规避，只要作出动态规避的频率较快，算法就能 不断更新规划路线，直到避过运动障碍;第三类是采用学习算法，通过训练使机器人具有智 能避障的能力；
[0003] 基于趋势预测的策略能较好地规避运动状态相对平稳的运动障碍，但对于运动速 度或方向变化频繁的运动障碍(或目标)则难以准确预测。将运动障碍固化成静态障碍的策 略，没有考虑运动趋势，导致规划出的航路不一定沿着威胁减小的方向生成。基于学习算法 的避碰策略需要大量的训练样本，当在复杂的动态程度较高的未知环境中，其避碰效果难 以保证。
【发明内容】
[0004] 本发明是为了解决目前采用的相向航行的运动障碍规避方法难以准确预测运动 障碍的运动状态的问题，而提出的一种基于虚拟膨化的运动障碍与UUV(无人水下航行器） 相向航行的规避方法。
[0005] -种基于虚拟膨化的运动障碍与UUV相向航行的规避方法按以下步骤实现：
[0006] 步骤一：设运动障碍航向与引导航向的夹角为headAng 1 e : 时，UUV与运动障碍为相向航行;所述引导航向是指从UUV当前位置到下一个非运动障碍航 路点的连线所形成的向量，非运动障碍航路点是指不是依托于运动障碍形成的环境中静态 环境信息的顶点；
[0007] 步骤二：当UUV检测到与运动障碍物相向航行时，运动障碍进行圆形膨化；
[0008] 步骤三:根据步骤二中运动障碍的圆形膨化，生成矩形虚拟障碍；
[0009] 步骤四：根据步骤二和步骤三，计算触发航路规划算法的衡量距离nextL，当UUV与 运动障碍中心点的直线距离Μ满足M〈nextL时触发航路规划算法，计算下一次触发规划的衡 量距nextL，直到nextL = abs_r+safe_c^t，航路规划算法不再被触发;其中所述obs_r为运 动障碍半径，safe_d为膨化距离。
[0010] 发明效果：
[0011] 本发明使用蚁群算法作为最基本的动态规划算法，其中规划算法的触发机制是动 态规划的关键，首先该机制要控制好动态规划算法的触发时机，使UUV能及时规划出新航路 避开运动障碍物，或者在没有运动障碍物时合理触发算法修正原有航路。其次该机制还应 能根据UUV所处的具体环境，抑制不必要的触发，因为算法触发过于频繁会降低UUV感知环 境的灵敏性。
[0012] 本发明的目的是在运动障碍静态化策略的基础上，使用虚拟膨化方法将运动障碍 的趋势考虑到动态规划当中，使UUV与运动障碍相向航行时总能沿着威胁减小的方向生成 规避航路。本发明的仿真效果图如图5所示。
【附图说明】
[0013] 图1为引导航向示意图；图中1是静态障碍，2是运动障碍，3是引导航向，4是布放 点，5是回收点，必1是必经点1;
[0014] 图2为相向航行示意图；图中1是headAngle，2是运动障碍，3引导航向，4是UUV艏 向，5是运动障碍航向，0是UUV;
[0015] 图3为运动障碍膨化并生成虚拟障碍示意图；图中1是headAngle，2是运动障碍，3 是引导航向，4是L，5是运动障碍航向，6是Μ，7是运动障碍，0是UUV;
[0016] 图4为UUV与运动障碍的位置关系示意图；图中1是headAngle，2是运动障碍，3是运 动障碍航向，4是引导航向，5是L，6是M，7是运动障碍，0是UUV;
[0017] 图5为仿真效果图，图中1为运动障碍。
【具体实施方式】
【具体实施方式】 [0018] 一：一种基于虚拟膨化的运动障碍与UUV相向航行的规避方法包括 以下步骤：
[0019] 弓丨导航向是指从UUV当前位置到下一个非运动障碍航路点的连线所形成的向量的 方向。所谓非运动障碍航路点，是指该航路点不是依托于运动障碍形成的，而是环境中静态 环境信息的顶点，如静态目标的顶点(P2)、必经点，回收点等。当UUV运行到图1所示位置时， 遭遇1号运动障碍，UUV触发动态规划算法后形成新的规避航路，点 ？1是1]1^为避开1号运动 障碍形成的依托于运动障碍形成的航路点，而点P2是静态目标膨化顶点。于是根据定义此 时引导航向为向量$的方向。UUV为无人水下航行器。
[0020] 步骤一：设运动障碍航向与引导航向的夹角为headAngl e如图2所示，当
，UUV与运动障碍为相向航行;所述引导航向是指从UUV当前位置到下 一个非运动障碍航路点的连线所形成的向量，非运动障碍航路点是指不是依托于运动障碍 形成的环境中静态环境信息的顶点；
[0021 ]步骤二:当UUV检测到与运动障碍物相向航行时，运动障碍进行圆形膨化；
[0022]步骤三:根据步骤二中运动障碍的圆形膨化，生成矩形虚拟障碍；
[0023] 步骤四：根据步骤二和步骤三，计算触发航路规划算法的衡量距离nextL，设UUV与 运动障碍的位置关系如图4所示，当UUV与运动障碍中心点的直线距离Μ满足M〈nextL时触发 航路规划算法，计算下一次触发规划的衡量距nextL，直到nextL = abs_r+safe_c^t，航路规 划算法不再被触发（即规避了运动障碍）；其中所述〇bs_rS运动障碍半径(在建模处理的时 候，按照障碍物的最大宽度将障碍物都建成圆形模型），safe_d为膨化距离。由于运动障碍 物的膨化半径为R，障碍物到UUV的距离Μ始终满足Μ 2 R，而R>obs_r+saf e_d，至此规划算法 不再被触发。
【具体实施方式】 [0024] 二:本实施方式与一不同的是:所述步骤二中运动障 碍进行圆形膨化后区域半径为R，R>〇bs_r+saf e_d。
【具体实施方式】 [0025] 三:本实施方式与一或二不同的是:所述步骤三中生 成矩形虚拟障碍具体为：
[0026]当UUV检测到运动障碍相向驶来时，运动障碍沿运动障碍前进方向生成长为L、宽 为2R的矩形虚拟障碍(如图3所示），所述矩形虚拟障的长与UUV到运动障碍中心点的直线距 离Μ关系为：
[0027] L = R+A(M-R)
[0028] 其中所述0〈λ〈1，λ是相向航行时运动障碍生成的矩形虚拟障碍长度系数，λ越大， 矩形虚拟障碍的初始长度越大，相邻两次生成的长度变化越大，UUV产生的规避路径越不平 滑，但能为UUV再次规划留足航行缓冲时间，适合在UUV与运动障碍相对航速较大时使用。反 之，λ越小，矩形虚拟障碍的初始长度越短，相邻两次生成的长度变化越小，UUV产生的规避 路径越平滑，适合在UUV与运动障碍相对航速较小时使用。
[0029] 运动障碍物膨化后的区域以及虚拟障碍物覆盖的区域都是机器人航路不可经过 的区域。此时相当于将所有环境信息瞬间静态化，调用航路规划算法可以得到一条在这一 瞬间安全的航路。由于在运动障碍物前方生成了虚拟障碍，可以保证在障碍物行驶完虚拟 障碍覆盖的区域前不会和UUV相撞。然而，机器人和障碍物都在不断运动，相对位置不断发 生变化，且它们各自航行速度也是随机的，一次规划并不能保证UUV能安全避过运动障碍。 这就需要合理地再次触发规划算法，及时修正航路。
[0030] 【具体实施方式】四：本实施方式与【具体实施方式】一至三之一不同的是:所述步骤四 中的航路规划算法米用蚁群算法，触发航路规划算法的衡量距nextL为：
[0032]其中所述D为常数，0〈ri〈l，ri是相向航行时，触发路径规划算法的衡量距离nextL的 更新系数，η越小，触发路径规划算法的频率越快。
[0033] 实施例一：
[0034]相向航行的规避方法是航向控制，但是为了加快避碰速度，在更改航向的同时，对 UUV进行了加速使其航速为ν = vu_max，当避过障碍物后再恢复UUV原本速度，如图5所示。此时 UUV的初始航速为4节，运动障碍的航速为4节，λ = 〇. 75，safe_d= 10(像素根据任务安全要 求设定），UUV最大直径=18(像素根据UUV尺寸设定），n = 〇.95，D = 2R，UUV巡航速度~=4kn (节），UUV最高航速Vu_max = 8kn(节）。
【主权项】
1. 一种基于虚拟膨化的运动障碍与uuv相向航行的规避方法，其特征在于，所述基于虚 拟膨化的运动障碍与UUV相向航行的规避方法包括W下步骤： 步骤一:设运动障碍航向与引导航向的夹角为headAngle，当知,"沁/"如,时，UUV 与运动障碍为相向航行;所述引导航向是指从UUV当前位置到下一个非运动障碍航路点的 连线所形成的向量，非运动障碍航路点是指不是依托于运动障碍形成的环境中静态环境信 息的顶点； 步骤二：当UUV检测到与运动障碍物相向航行时，运动障碍进行圆形膨化； 步骤Ξ:根据步骤二中运动障碍的圆形膨化，生成矩形虚拟障碍； 步骤四：根据步骤二和步骤Ξ，计算触发航路规划算法的衡量距离nextL，当UUV与运动 障碍中屯、点的直线距离Μ满足M<nextL时触发航路规划算法，计算下一次触发规划的衡量距 nextL，直到nextL = abs_r+safe_d时，航路规划算法不再被触发；其中所述obs_;r为运动障 碍半径，saf e_d为膨化距离。2. 根据权利要求1所述的一种基于虚拟膨化的运动障碍与UUV相向航行的规避方法，其 特征在于所述步骤二中运动障碍进行圆形膨化后区域半径为R，R〉〇bs_r+saf e_d。3. 根据权利要求2所述的一种基于虚拟膨化的运动障碍与UUV相向航行的规避方法，其 特征在于所述步骤Ξ中生成矩形虚拟障碍具体为： 沿运动障碍前进方向生成长为L、宽为2R的矩形虚拟障碍，所述矩形虚拟障的长与UUV 到运动障碍中屯、点的直线距离Μ关系为： L = R+A(M-R) 其中0<λ<1。4. 根据权利要求3所述的一种基于虚拟膨化的运动障碍与UUV相向航行的规避方法，其 特征在于所述步骤四中的航路规划算法采用蚁群算法，触发航路规划算法的衡量距nextL 为：其中D为常数，0<11<1。
【专利摘要】一种基于虚拟膨化的运动目标与UUV相向航行的规避方法，本发明涉及基于虚拟膨化的运动目标与UUV相向航行的规避方法。本发明是为了目前采用的相向航行的运动障碍规避方法难以准确预测运动障碍的运动状态的问题。本发明根据运动障碍航向与引导航向的夹角为headAngle，确定UUV与运动障碍相向航行，当UUV检测到与运动障碍物相向航行时，运动障碍进行圆形膨化后生成矩形虚拟障碍，计算触发航路规划算法的衡量距离nextL，当UUV与运动障碍中心点的直线距离M满足M<nextL时触发航路规划算法，计算下一次触发规划的衡量距nextL，直到航路规划算法不再被触发。本发明应用于UUV的运动障碍规避领域。
【IPC分类】G05D1/06
【公开号】CN105549600
【申请号】CN201610082237
【发明人】王宏健, 张雪莲, 崔保华, 张宏瀚, 刘向波
【申请人】哈尔滨工程大学
【公开日】2016年5月4日
【申请日】2016年2月5日