CN102999050A - 一种智能水下机器人的自主避障方法 - Google Patents

Abstract

本发明涉及一种智能水下机器人避障方法，具体是一种将水下机器人运动目标、障碍物和水下机器人控制性能进行统一考虑来实现避障的方法。本发明包括以下步骤：根据作业任务和海图数据库信息进行全局路径规划；读取水下机器人运动传感器信息、避碰声纳数据信息；计算水下机器人警戒安全距离，建立水下机器人安全虚拟外壳；判断是否需要避障；判断是否达到目标，若到达目标，任务完成。本发明将水下机器人速度息的影响引入机器人避障策略中，将水下机器人局部避碰规划与控制、水动力性能相结合，制定的避障策略能够反映智能水下机器人动态避障能力，提高了水下机器人的生存能力。

Description

一种智能水下机器人的自主避障方法

技术领域

本发明涉及一种智能水下机器人避障方法，具体是一种将水下机器人运动目标、障碍物和水下机器人控制性能进行统一考虑来实现避障的方法。

背景技术

智能水下机器人在海洋科学研究、海洋开发、水下工程以及军事等方面都有广泛的应用前景。智能水下机器人一般工作在复杂海洋环境下，为了更好地完成各种作业使命和其自身生命安全，其需要具有自主避障的能力，能够对周围环境障碍物可能造成碰撞危险进行感知并做出相应响应。

水下机器人的自主避障方式有很多种，如势场法、模糊避碰法等等。势场法是一种虚拟力法，其基本思想是将机器人在环境中的运动视为一种虚拟的受力场中的运动。障碍物对机器人产生斥力，目标点产生引力，引力和斥力的合力作为机器人的加速力，控制机器人的运动方向和机器人的位置。该方法结构简单，便于底层的实时控制、实时避障，但其仅仅根据一些相对距离来确定控制力的大小比较粗糙，并且存在局部最优解的问题，容易产生死锁现象。近些年一些学者将模糊推理的思想引入水下机器人避障技术方法中，其原理就是基于实时传感器信息的模糊逻辑参考人的经验，通过查表得到规划信息，实现局部路径规划，该方法克服了势场法易产生的局部极小问题，适用于时变未知环境下的路径规划，实时性较好，取得了一些成果，其缺点是人的经验不一定完备的，输入量增多时，推理规则和模糊表会急剧膨胀。但大多都未能够充分考虑水下机器人的动力学模型与运动控制能力，因此将这些因素引入其避障策略中使其能够真实反映动态避障能力使之安全可靠地执行各种作业任务具有重要的意义。

2001年5月《机器人》第23卷第3期的“基于运动平衡点的水下机器人自主避障方式”提出将水下机器人的自主避障规划和运动控制结合起来的自主避障规划方法。但该方法在确定运动平衡点安全距离时虽然考虑了水下机器人的运动控制性能等因素，但其设计的安全距离值是一个定值，尤其没有考虑水下机器人速度对避障能力的影响，并且在考虑水下机器人的运动控制能力时也认为是一成不变的，这些其实并不能真实反应水下机器人在实际工作中的动态变化情况。

发明内容

本发明的目的在于提供一种考虑水下机器人动力学模型与运动控制能力的智能水下机器人避障方法。

本发明的目的是这样实现的：

智能水下机器人的自主避障方法，包括以下步骤：

（1）根据作业任务和海图数据库信息进行全局路径规划；

（2）读取水下机器人运动传感器信息、避碰声纳数据信息；

（3）计算水下机器人警戒安全距离，建立水下机器人安全虚拟外壳；

（4）判断是否需要避障，若需要避障，执行避障策略，重新执行步骤（2），若不需要避障，继续执行步骤（5）；

（5）判断是否达到目标，若未到达目标重新执行步骤（2），若到达目标，任务完成。

将水下机器人的速度矢量信息引入所述的警戒安全距离，以安全警戒距离点构建水下机器人安全虚拟外壳，水下机器人第i个自由度安全警戒距离h_i为：

h_i＝h_i0+α_i·v_i

其中，h_i0，为水下机器人第i个自由度的静态避碰安全警戒距离，i＝1,2,3分别代表机器人纵向、横向和垂向，v_i为水下机器人第i个自由度方向上速度，α_i为第i个自由度方向上速度敏感系数。

所述的避障策略包括远距离制导和近距离逼近与定位两部分：

1）障碍物和目标点均在警戒距离以外，进行远距离制导

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& l_{\mathrm{ox}}\≥E_{x\max }\\ l_{\mathrm{ox}},& {-E}_{x\max }\≤l_{\mathrm{ox}}\≤E_{x\max },\\ {-E}_{x\max },& l_{\mathrm{ox}}\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& l_{\mathrm{oy}}\≥E_{y\max }\\ l_{\mathrm{oy}},& {-E}_{y\max }\≤l_{\mathrm{oy}}\≤E_{y\max },\\ {-E}_{y\max },& l_{\mathrm{oy}}\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≥1& \\ E_{\mathrm{\ψ}\max }\·(e_x-d)/E_{x\max },& -1\≤e_x/E_{x\max }\≤1,\\ {-E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≤-1& \end{array}\right.$

其中e_x为机器人纵向运动控制偏差，e_y为机器人横向运动控制偏差，e_w为机器人艏向运动控制偏差，E_xmax是纵向控制参数，E_ymax是横向控制参数，E_ψmax是艏向控制参数，d为转艏规划提前量，l_ox为目标点或障碍物对机器人的影响在艇体坐标上的纵向分量距离、l_oy为目标点或障碍物对机器人的影响在艇体坐标上的横向分量距离，d＝d₀+bv_x，其中d₀为静态转艏提前量，b为速度敏感系数，v_x为水下机器人纵向速度；

2）近距离逼近与定位

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& x\≥E_{x\max }\\ x,& {-E}_{x\max }\≤x\≤E_{x\max },\\ {-E}_{x\max },& x\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& y\≥E_{y\max }\\ y,& {-E}_{y\max }\≤y\≤E_{y\max },\\ {-E}_{y\max },& y\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≥E_{\mathrm{\ψ}\max }\\ \mathrm{\ψ},& {-E}_{\mathrm{\ψ}\max }\≤\mathrm{\ψ}\≤E_{\mathrm{\ψ}\max }\\ {-E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≤{-E}_{\mathrm{\ψ}\max }\end{array}\right.,$

其中x为目标点在机器人艇体坐标下的纵坐标值，y为目标点在机器人艇体坐标下的横向坐标值，Ψ为目标点在机器人艇体坐标下的艏向坐标值。

所述艏向角偏差e_ψ的修正值为：

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{e}_{\mathrm{\ψ}}& v\≤v_0\\ e_{\mathrm{\ψ}}/(1+\mathrm{\β}(v-v_0))& v>v_0\end{array}\right.,$

其中，e_ψ为规划器的规划转艏角度，v为水下机器人当前航速，v₀为转艏运动控制时考虑纵向速度影响的临界速度，β为艏向运动与纵向运动之间耦合程度的参数。

本发明的有益效果在于：

本发明将水下机器人速度和电压能源信息的影响引入机器人避障策略中，根据机器人速度信息计算警戒安全距离和避障规划控制偏差，执行避障控制，将水下机器人局部避碰规划与控制、水动力性能相结合，制定的避障策略能够反映智能水下机器人动态避障能力，提高了水下机器人的生存能力。

附图说明

图1为智能水下机器人自主避障流程图；

图2为速度对机器人安全警戒距离影响示意图；

图3为智能水下机器人避障控制偏差计算示意图。

具体实施方式

下面结合附图举例对本发明作更详细地描述：

智能水下机器人工作在复杂的海洋环境里，可能存在未知的障碍物－如暗礁、堤坝、航行路线上驶过的船只、海中浮游物体等等，这些都可能造成水下机器人执行任务的失败甚至威胁水下机器人的生命。而水下机器人在不同的运动状态下，其避障能力会有所不同，针对此情况本发明提出了一种智能水下机器人的自主避障方法，该方法将水下机器人运动目标、障碍物和水下机器人控制性能进行统一考虑，水下机器人的避障能力和其运动状态息息相关。

图1是本发明的流程图，首先，智能水下机器人根据其当前位置信息、作业任务目标点和海图数据库信息进行全局路径规划得到一个路径，很容易想到，在备选的路线中可能会有不止一条的可选择线路，那么路线的选择是从起始点出发到目标点（需要考虑海图信息，如岛礁，水下设施等）的最短路线。

此时给出的全局规划路线是在已知的海图信息根据障碍物信息制定的规划路线，当然，复杂的水下还有可能存在其它未知的障碍物（如水下漂流的废弃物、布设的水雷等），水下机器人还会根据实时障碍物信息对原有规划的路线进行调整。

水下机器人读取避碰声纳信息，并根据机器人速度信息计算机器人安全警戒距离，在考虑艇形的基础上构造一个机器人安全警戒虚拟外壳。

如图2所示，在这里，考虑到机器人速度对其避障能力的影响，将机器人速度信息引入到水下机器人安全警戒距离计算方法中，则机器人第i个自由度安全警戒距离h_i（i＝1，2，3分别代表机器人纵向、横向和垂向）为：

h_i＝h_i0+α_i·v_i

其中，h_i0（i＝1，2，3）为水下机器人第i个自由度的静态避碰安全警戒距离，v_i水下机器人第i个自由度方向上速度,α_i为第i个自由度方向上速度敏感系数（i＝1，2，3）。

这里要说明的是h_i0要小于避碰传感器的最大探测范围，为了节约能源，h_i0的值在保证机器人安全的前提下要尽可能小；另外考虑到机器人的基础运动控制能力在不同方向上的差别（一般水下机器人纵向控制能力强于横向和垂向控制能力），α_i的取值在不同自由度上也有所不同，纵向自由度上的值较大一些，一般可在0~5之间取值，α=0则代表安全警戒距离的计算未考虑速度的影响。例如，我们机器人配置的避碰声纳最大探测范围80m，我们可设h₁₀=68m，h₂₀=40m，h₃₀=56m，α₁₀＝2.0；α₂₀＝1.0；α₃₀＝1.5。

接着，水下机器人需要根据障碍物信息和计算得到的警戒安全距离判断机器人是否需要避障，若需要避障则执行避障策略，并循环执行：实时接收传感器信息，判断是否脱离障碍物，再执行避障策略直至脱离障碍物。

若机器人不需要避障，则判断是否到达目标，到达目标点，任务结束，若未到达目标点则重复以上过程直至到达目标点。

智能水下机器人避障策略分为远距离制导和近距离逼近与定位，具体为：

（1）远距离制导（障碍物和目标点均在警戒安全距离以外）：

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& l_{\mathrm{ox}}\≥E_{x\max }\\ l_{\mathrm{ox}},& {-E}_{x\max }\≤l_{\mathrm{ox}}\≤E_{x\max },\\ {-E}_{x\max },& l_{\mathrm{ox}}\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& l_{\mathrm{oy}}\≥E_{y\max }\\ l_{\mathrm{oy}},& {-E}_{y\max }\≤l_{\mathrm{oy}}\≤E_{y\max },\\ {-E}_{y\max },& l_{\mathrm{oy}}\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≥1& \\ E_{\mathrm{\ψ}\max }\·(e_x-d)/E_{x\max },& -1\≤e_x/E_{x\max }\≤1,\\ {-E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≤-1& \end{array}\right.$

其中e_x,e_y,e_ψ分别为机器人纵向、横向、艏向运动控制偏差，E_xmax，E_ymax和E_ψmax则分别是纵向、横向和艏向的控制参数，即控制的最大偏差，控制的最大偏差既体现了控制器的能力，同时也反映了控制器的性能，d为转艏规划提前量，l_ox、l_oy分别为目标点或障碍物对机器人的影响在艇体坐标分别至OX、OY方向上警戒安全虚拟外壳的距离。

如图3所示，B点为机器人探测到的障碍物位置，A点为B点至机器人中心O与机器人警戒安全虚拟外壳交点，A₁、A₂点分别为机器人纵向、横向警戒安全虚拟外壳交点。AA₁//BB₁，AA₂//BB₂，即B点障碍物对机器人分解成纵向B₁和横向B₂两处有障碍物对机器人的影响（三维的也可同样分析）。由于障碍物Ｂ点距避碰声纳距离可由避碰声纳获得，另外，避碰声纳在机器人上的安装位置、机器人几何信息及警戒安全虚拟外壳信息均已知，显然，OA、AB、AA₁、AA₂均已知，再根据几何关系，则A₁B₁（即l_ox）、A₂B₂（即l_oy）可以求得。

水下机器人的速度与其转艏和避碰能力息息相关，取d＝d₀+bv_x，其中d₀为静态转艏提前量，为一常值，需要在实验中进行调整，b为速度敏感系数，v_x为水下机器人纵向速度。

关于E_xmax，E_ymax，E_ψmax，d₀的取值机器人的运动控制性能有关，E_xmax，E_ymax，E_ψmax为在纵向、横向、转艏的单位时间内可实现的最大控制偏差，例如我们可取E_xmax=3.5，E_ymax=1.5，E_ψmax=20，d₀=16，而b的取值一般在0~3之间，当b=0则未考虑机器人速度对转艏能力的影响，例如可取b=1.1。

（2）近距离逼近与定位（目标点在警戒距离以内）

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& x\≥E_{x\max }\\ x,& {-E}_{x\max }\≤x\≤E_{x\max },\\ {-E}_{x\max },& x\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& y\≥E_{y\max }\\ y,& {-E}_{y\max }\≤y\≤E_{y\max },\\ {-E}_{y\max },& y\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≥E_{\mathrm{\ψ}\max }\\ \mathrm{\ψ},& {-E}_{\mathrm{\ψ}\max }\≤\mathrm{\ψ}\≤E_{\mathrm{\ψ}\max }\\ {-E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≤{-E}_{\mathrm{\ψ}\max }\end{array}\right.,$

其中x,y,ψ为目标点在机器人艇体坐标下的坐标值，分别为纵向、横向、艏向信息。

考虑到水下机器人的动力学特性，引入水下机器人速度对规划的艏向角偏差e_ψ进行修正。根据水下机器人的动力学方程可知：水下机器人的水动力与纵向速度成增函数的关系，高速航行下水下机器人各个自由度之间的耦合效应明显增强。所以在机器人高速航行状态下，各个自由度上的水动力（矩）都会增大，艇体所受转艏力矩也将明显增大，另外如果是采用推进器执行机构来控制水下机器人姿态时，由于在水下机器人高速航行时推进器推力减额的影响，推进器所能够提供的转艏力矩明显减小。而从运动控制的角度出发，为了保证控制系统的稳定，在高速运动的情况下不希望有过大的转艏角度。机器人转艏角度的规划应该与纵向运动速度、当前机器人的艏向角相关。因此，e_ψ进行如下修正：

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{e}_{\mathrm{\ψ}}& v\≤v_0\\ e_{\mathrm{\ψ}}/(1+\mathrm{\β}(v-v_0))& v>v_0\end{array}\right.$

其中：e_ψ为规划器的规划角度，v为水下机器人当前航速，当机器人运动速度小于v₀时各个自由度之间耦合效应已经较小，可以不予考虑，一般在0.5~1.5之间取值，如可取v₀=1.0。β为艏向运动与纵向运动之间耦合程度的参数，一般在0~1.0之间取值，当其值取0时，即不考虑速度对艏向角规划的影响。

上述仅是为说明本发明而列举的实施例，在本发明基本构思的基础上可以进行的各种替换、变化和修改，这些替换、变化和修改不应排除在本发明的保护范围之外。

Claims (5)

1.一种智能水下机器人的自主避障方法，其特征在于包括以下步骤：

（1）根据作业任务和海图数据库信息进行全局路径规划；

（2）读取水下机器人运动传感器信息、避碰声纳数据信息；

（3）计算水下机器人警戒安全距离，建立水下机器人安全虚拟外壳；

（4）判断是否需要避障，若需要避障，执行避障策略，重新执行步骤（2），若不需要避障，继续执行步骤（5）；

（5）判断是否达到目标，若未到达目标重新执行步骤（2），若到达目标，任务完成。

2.根据权利要求1所述的一种智能水下机器人的自主避障方法，其特征在于：将水下机器人的速度矢量信息引入所述的警戒安全距离，以安全警戒距离点构建水下机器人安全虚拟外壳，水下机器人第i个自由度安全警戒距离h_i为：

h_i＝h_i0+α_i·v_i

其中，h_i0，为水下机器人第i个自由度的静态避碰安全警戒距离，i＝1,2,3分别代表机器人纵向、横向和垂向，v_i为水下机器人第i个自由度方向上速度，α_i为第i个自由度方向上速度敏感系数。

3.根据权利要求1或2所述的一种智能水下机器人的自主避障方法，其特征在于：所述的避障策略包括远距离制导和近距离逼近与定位两部分：

1）障碍物和目标点均在警戒距离以外，进行远距离制导

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& l_{\mathrm{ox}}\≥E_{x\max }\\ l_{\mathrm{ox}},& {-E}_{x\max }\≤l_{\mathrm{ox}}\≤E_{x\max },\\ {-E}_{x\max },& l_{\mathrm{ox}}\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& l_{\mathrm{oy}}\≥E_{y\max }\\ l_{\mathrm{oy}},& {-E}_{y\max }\≤l_{\mathrm{oy}}\≤E_{y\max },\\ {-E}_{y\max },& l_{\mathrm{oy}}\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≥1& \\ E_{\mathrm{\ψ}\max }\·(e_x-d)/E_{x\max },& -1\≤e_x/E_{x\max }\≤1,\\ {-E}_{\mathrm{\ψ}\max },(e_x-d)/E_{x\max }\≤-1& \end{array}\right.$

其中e_x为机器人纵向运动控制偏差，e_y为机器人横向运动控制偏差，e_w为机器人艏向运动控制偏差，E_xmax是纵向控制参数，E_ymax是横向控制参数，E_ψmax是艏向控制参数，d为转艏规划提前量，l_ox为目标点或障碍物对机器人的影响在艇体坐标上的纵向分量距离、l_oy为目标点或障碍物对机器人的影响在艇体坐标上的横向分量距离，d＝d₀+bv_x，其中d₀为静态转艏提前量，b为速度敏感系数，v_x为水下机器人纵向速度；

2）近距离逼近与定位

$e_x=\left\{\begin{array}{cc}{E}_{x\max },& x\≥E_{x\max }\\ x,& {-E}_{x\max }\≤x\≤E_{x\max },\\ {-E}_{x\max },& x\≤{-E}_{x\max }\end{array}\right.,$

$e_y=\left\{\begin{array}{cc}{E}_{y\max },& y\≥E_{y\max }\\ y,& {-E}_{y\max }\≤y\≤E_{y\max },\\ {-E}_{y\max },& y\≤{-E}_{y\max }\end{array}\right.,$

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≥E_{\mathrm{\ψ}\max }\\ \mathrm{\ψ},& {-E}_{\mathrm{\ψ}\max }\≤\mathrm{\ψ}\≤E_{\mathrm{\ψ}\max }\\ {-E}_{\mathrm{\ψ}\max },& \mathrm{\ψ}\≤{-E}_{\mathrm{\ψ}\max }\end{array}\right.,$

其中x为目标点在机器人艇体坐标下的纵坐标值，y为目标点在机器人艇体坐标下的横向坐标值，Ψ为目标点在机器人艇体坐标下的艏向坐标值。

4.根据权利要求1或2所述的一种智能水下机器人的自主避障方法，其特征在于：所述艏向角偏差e_ψ的修正值为：

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{e}_{\mathrm{\ψ}}& v\≤v_0\\ e_{\mathrm{\ψ}}/(1+\mathrm{\β}(v-v_0))& v>v_0\end{array}\right.,$

其中，e_ψ为规划器的规划转艏角度，v为水下机器人当前航速，v₀为转艏运动控制时考虑纵向速度影响的临界速度，β为艏向运动与纵向运动之间耦合程度的参数。

5.根据权利要求3所述的一种智能水下机器人的自主避障方法，其特征在于：所述艏向角偏差e_ψ的修正值为：

$e_{\mathrm{\ψ}}=\left\{\begin{array}{cc}{e}_{\mathrm{\ψ}}& v\≤v_0\\ e_{\mathrm{\ψ}}/(1+\mathrm{\β}(v-v_0))& v>v_0\end{array}\right.,$

其中，e_ψ为规划器的规划转艏角度，v为水下机器人当前航速，v₀为转艏运动控制时考虑纵向速度影响的临界速度，β为艏向运动与纵向运动之间耦合程度的参数。

Images