CN110134130A - 一种基于改进角度势场法的无人船自主避障方法 - Google Patents

Abstract

本发明公开了一种基于改进角度势场法的无人船自主避障方法，在传统角度势场法的基础上，考虑无人船运动学特点对角度和速度的计算方法进行改进；提出反向避障方法处理狭窄空间无法转向问题；将两点航行避障拓展到多目标点航行避障，方法包括：根据用户输入的目标点顺序，获取第一个目标点信息，通过IMU获取当前无人船的位置和航向信息；根据雷达获取当前位置的环境信息进行环境建模；根据改进角度势场法避障航行直到航行至当前目标点；根据用户输入的目标点顺序，更新目标点位置信息，继续航行，直到航行至最终目标点。本发明保证无人船能够在复杂障碍空间下有效避开障碍物，进行多目标点自主航行和避障。

Description

一种基于改进角度势场法的无人船自主避障方法

技术领域

本发明属于无人船自主避障技术领域，具体涉及一种基于改进角度势场法的无人船自主避障方法。

背景技术

近年来，无人智能平台迅速发展，但如何通过探测到的障碍物信息，实现无人船的自主避障一直是无人船自动巡航过程中急需解决的问题。尤其是针对小型无人船更需要研究出计算量小、安全性能高、适用于各种复杂环境的自主避障算法。

针对无人船自主避障问题，国内外研究学者开展了各种研究。Khatib提出了人工势场法(Potential Field Method)，将机器人视角中障碍物对机器人的影响描述为“排斥力”，目标点对无人船的影响描述为“牵引力”，将排斥力与牵引力合成，得到合力，引导机器人躲避障碍物，向目标点前进。人工势场法因为简单、计算量小等优点得到了广泛应用。但人工势场法将所有障碍物对机器人的影响统一描述为一个排斥力，所以存在局部最优解问题。W.E Howden提出了栅格法，将机器人行进空间划分为多个大小相等、具有二值信息的栅格单元，从起始点到目标点进行栅格路径搜索，搜索到的路径就是规避障碍物的无碰路径。在划分栅格时，栅格单元越大，计算量越小，但环境信息表示也会越模糊，从而会导致路径规划不准确；栅格单元越小，环境信息刻画就越清晰，但计算量和存储空间会成指数增加。所以运用栅格法进行避障时需要划分大小合适的栅格单元。

随着现代人工智能的发展，一些智能算法也得到了广泛应用。W.mcculloch和W.pitts提出神经网络算法后，神经网络算法逐渐被大量应用于机器人视觉，无人系统避障等各个领域，禹建丽提出基于神经网络的路径规划算法，利用神经网络结构定义能量函数，然后通过路径点位置选取动态运动方程，从而规划出折线形无碰路径。Eberhart和Kennedy提出粒子群算法。粒子群算法的基本概念源于对鸟群觅食行为的研究，通过一个随机解以及群体抽象出的数学模型进行迭代从而求取最优解。粒子群算法可以规划出两点间最优路径。但智能算法计算量过大，不适用于小型无人船系统。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种基于改进角度势场法的无人船自主避障方法，实现多目标点间复杂空间内的避障航行。

为实现上述技术目的，本发明采取的技术方案为：

一种基于改进角度势场法的无人船自主避障方法，包括以下步骤：

步骤一：根据用户输入的目标点顺序，获取第一个目标点信息；

步骤二：通过IMU获取当前无人船位置信息、当前航向信息；

步骤三：根据雷达获取当前位置的环境信息进行环境建模；

步骤四：根据改进角度势场法避障航行直到航行至当前目标点；

步骤五：根据用户输入的目标点顺序，更新目标点位置信息，继续航行，直到航行至最终目标点。

为优化上述技术方案，采取的具体措施还包括：

优选地，上述的步骤一具体为：

用户根据自身需求，按照顺序依次输入目标点的位置信息；

根据用户输入的目标点顺序，获取第一个目标点位置信息，并设为当前航行目标点。

步骤二所述IMU集成AHRS和GPS功能，所述AHRS通过测量三轴方向上的加速度、角速率和磁场，获知无人船当前的三维姿态；所述GPS提供自身位置信息；AHRS和GPS功能的结合用于获取无人船实时位置和行进方向。

优选地，上述的步骤三具体为：

激光雷达可获取当前位置360度的环境信息，所述环境信息包括障碍物所在角度以及障碍物与当前位置相距的距离；

将激光雷达提供的环境信息根据当前航向转化为无人船视角范围内的障碍物距离、角度信息。

优选地，上述的步骤四具体为：

根据环境信息进行可航行空间判断；

若当前位置存在可航行空间，则根据环境信息计算通行程度，确定输出角度，根据无人船特性确定待转向角度，使用PID(比例proportion、积分integral、微分differential)算法进行舵机角度调整航行；若无人船当前位置不存在可航行空间，则进行反向避障；

直到到达当前目标点。

优选地，上述的通行程度K_通行计算公式如下：

式中，K_引(θ)、K_阻(θ)分别表示角度θ受到的引力和阻力；

所述输出角度为最大通行程度所在的角度；

所述待转向角度确定方法为：若输出角度大于无人船1秒内转向角度δ，则待转向角度为δ，若输出角度小于δ，则待转向角度为输出角度。

优选地，上述的输出角度为最大通行程度所在的角度；

优选地，上述的待转向角度确定方法为：若输出角度大于无人船1秒内转向角度δ，则待转向角度为δ，若输出角度小于δ，则待转向角度为输出角度。

上述的K_引(θ)、K_阻(θ)的计算公式如下：

K_引(θ)＝cos(θ-θ_obj) (12)

式中，为对于角度的障碍点在角度θ产生的阻力，θ_obj为目标点相对于当前位置的方位角。

优选地，上述的PID算法为增量式PID，其输出Δu[n]计算公式如下：

Δu[n]＝k_p×{e[n]-e[n-1]}+k_i×e[n]+k_d×{e[n]-2e[n-1]+e[n-2]} (4)

式中k_p表示比例系数，k_i表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；通过三个时刻的船航行与目标航行的差值作为输入得出舵机转向输出，不断调整转向角度。

优选地，上述的反向避障具体为：

将目标点转化为与当前无人船的位置对称的目标点，将当前航向方向从船头航向转变为船尾航向--以船尾为船的航向，障碍物探测区域转变为船尾视角；

根据新目标点、新航向、新障碍物探测区域进行角度势场法避障；

在确定后退停止位置时，采用圆弧的弦长度(SubtenseDistance)作为判断该角度是否满足船体转向通过的条件。

上述的弦长度SubtenseDistance(θ)计算公式如下：

式中，θ为判定位置航向与当前航向的夹角，Radius为无人船的转向半径，Size为无人船的长度，角度势场法输出的正向转向角度范围内有任一角度的弦长度小于该角度障碍物距离，则当前位置不允许船体转向，继续反向避障。

本发明具有以下有益效果：

本发明保证无人船在航行过程中能够在复杂障碍空间下有效避开障碍物，进行多目标点航行，具有计算量小，实时避障等优点。

附图说明

图1是本发明一种基于改进角度势场法的无人船自主避障方法的步骤框图。

图2是本发明实施例的危险角度示意图。

图3是本发明实施例的反向避障示意图。

图4本发明一种基于改进角度势场法的无人船自主避障方法流程图。

具体实施方式

以下结合附图对本发明的实施例作进一步详细描述。

如图1所示，本发明的一种基于改进角度势场法的无人船自主避障方法，包含以下步骤：

步骤一：根据用户输入的目标点顺序，获取第一个目标点信息；

实施例中，用户根据自身需求，按照顺序依次输入目标点的位置信息；

根据用户输入的目标点顺序，获取第一个目标点位置信息，并设为当前航行目标点。

步骤二：通过IMU获取当前无人船的位置信息、当前航向信息；

实施例中，无人船搭载的IMU集成AHRS和GPS功能，AHRS主要是通过测量三轴方向上的加速度、角速率和磁场，可以获知无人船当前的三维姿态；GPS可以提供自身位置信息；两者结合可以获取无人船的实时位置和行进方向。

IMU采用的是北京三驰惯性科技股份有限公司的SC-INS-100S。SC-INS-100S由惯性测量单元、磁传感器等构成。产品内部嵌入扩展卡尔曼滤波算法，提供精确的航向(带GPS)、姿态等角度信息。相对于传统的航姿参考系统，通过产品内部各传感器相互作用，提高了产品的精度和可靠性。提供动静态环境下实时的、高精度的横滚角、俯仰角、航向角和位置信息；具有抗振动、抗短时外部磁场干扰、高带宽等特性；尺寸小、重量轻，功耗低。

IMU通过USB和树莓派连接，通信方式为UART_TTL全双工串口，115200bps，8位数据位，1位停止位，无校验。IMU上电启动准备完成后，等待START报文才可以开始按固定频率自动发送数据包，收到STOP报文后停止发送数据包。根据数据包格式，不断的读取串口，将接受到的数据整理成数据包并校验，检验通过则读取数据报，获得传感器返回的位置和航向等信息。

步骤三：根据雷达获取当前位置的环境信息进行环境建模；

实施例中，激光雷达采用砝石公司FaseLase最新研制的一款小型、经济型二维激光扫描雷达。它可探测到周边360度范围10米内的环境信息。包括障碍物所在角度以及障碍物与当前位置相距的距离。

该激光雷达通信方式为UART_TTL串口，比特率为230400bps。其二进制输出为4字节一组，包含距离值和角度值，角度精度为十六分之一度。设机器人所在航向为90度，当前航向顺时针90度以及当前航向逆时针90度共同组成机器人视角。将激光雷达提供的环境信息根据当前航向转化为无人船视角范围内的障碍物距离、角度信息。

步骤四：根据改进角度势场法避障航行直到航行至当前目标点；

实施例中，根据环境信息进行可航行空间判断，若当前位置存在可航行空间，则根据环境信息计算通行程度，确定输出角度，根据无人船特性确定待转向角度，使用PID算法进行舵机角度调整航行；若无人船当前位置不存在可航行空间，则进行反向避障；直到达到当前目标点。

使用引力与阻力的比值表示某一角度θ的可通行程度K_通行，见下式：

式中K_引(θ)、K_阻(θ)分别表示角度θ受到的引力和阻力。引力不变的情况下，在阻力较大时，可通行程度较小；在阻力较小时，可通行程度较大；当无人船与障碍物距离在刹车距离范围内时，通行程度降为0。若最大可通行程度K_out为零则停车。最大可通行程度K_out见下式：

K_out＝max(K_通行) (2)

根据环境信息计算各角度的阻力与引力，生成各角度的通行程度。通行程度最大的角度即为输出角度。根据无人船的运动学特点，设定无人船1S内转向角度δ度，若输出角度大于δ度，则待转向角度为δ度，若输出角度小于δ度，则待转向角度为输出角度。无人船速度不容易进行精准控制，所以不进行速度的调节。

PID控制是工业中最常用的控制算法。PID控制器的基本思想是读取传感器，然后通过计算比例，积分和微分单元计算所需的执行器输出，并将这三个分量相加以计算输出。PID控制器输出u(t)见下式：

式中k_p表示比例系数，e(t)为输入，k_i表示积分时间常数，k_d表示微分时间常数。PID控制的基础是比例控制，当仅有比例控制时系统输出存在稳态误差；积分控制可消除稳态误差，但可能增加超调；在微分控制中，控制器的输出与输入误差信号的微分(即误差的变化率)成正比关系。考虑实际条件，使用增量式PID控制进行无人船控制。增量式PID控制以传统PID控制为基础，将当前时刻的控制量和上一时刻的控制量做差，以差值为新的控制量，是一种递推式的算法。

增量式PID输出Δu[n]见下式：

Δu[n]＝k_p×{e[n]-e[n-1]}+k_i×e[n]+k_d×{e[n]-2e[n-1]+e[n-2]} (4)

式中k_p表示比例系数，k_i表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入。通过三个时刻的船航行与目标航行的差值作为输入得出舵机转向输出，不断调整转向角度。

障碍物对无人船产生的阻力构成阻力场，阻力随无人船和障碍物的距离减小而增大。同一个障碍点危险角度由无人船能够安全通过障碍物的横向安全距离D_sf决定。

如图2，障碍物1的危险角度由D_sf与障碍物距离决定。具体计算见下式：

式中，D_sf表示无人船的横向安全距离，表示角度的障碍点距离，k_sf表示放大系数，W表示无人船的宽度。设定大于探测距离的障碍点产生的阻力为固定的较小阻力；在小于最大探测距离且在安全航行范围内，障碍点产生的阻力大小与障碍点到无人船的距离成反比例关系；小于安全航行距离(刹车距离)的障碍物均产生极大的阻力。对于角度的障碍点在角度θ产生的阻力k_RF(θ)具体见下式：

式中，表示角度的障碍点的危险角度，表示角度的障碍点距离，D_m表示设定的最大评估距离，D_sf表示无人船以v速度航行的刹车距离，a表示无人船的减速时的加速度，k_sr表示放大系数。式中传统角度势场法将在危险角度范围之外的障碍物对θ角度产生的阻力置为0，但是若目标点周围没有障碍物，则所有角度阻力均为0，通行函数均为_+∞，输出角度就是机器人视角内为0度的方向，无人船将一直绕着目标点转圈。为了避免在无障碍物的情况下无人船转圈航行的情况，将未探测到的障碍物的障碍物与当前位置的距离设置为比较大的常数，则阻力为D_m-D_sr，如下式：

式中，表示角度的障碍点的危险角度，表示角度的障碍点距离，D_m表示设定的最大评估距离，D_sr表示无人船的刹车距离。对于某一角度θ，无人船所受的阻力为其视角范围内([0，π])所有障碍物对于当前位置的最大阻力，阻力值K_阻(θ)见下式：

引力场由目标点对无人船视角范围内所有角度的影响构成。使用余弦函数表示目标点对每个航行角度产生的引力，则对于角度θ的引力K_引(θ)，见下式：

K_引(θ)＝cos(θ-θ_obj) (12)

式中θ_obj表示目标点相对于当前位置的角度。使用余弦函数表示引力优点在于：当无人船行进角度与目标点角度相差较大时，值变化较大，可以使无人船更快向着目标点方向行进；当无人船行进角度与目标点角度相差较小时，值变化较小，目标点角度对无人船的行进影响较小，防止过度吸引而撞到障碍物。

反向避障流程为：将目标点转化为与当前无人船的位置对称的目标点，将当前航向方向从船头航向转变为船尾航向--以船尾为船的航向，障碍物探测区域转变为船尾视角，根据新目标点、新航向、新障碍物探测区域进行角度势场法避障。在确定后退停止位置时，采用圆弧的弦长度作为判断该角度是否满足船体转向通过的条件。弦长度SubtenseDistance(θ)见下式：

式中θ表示判定位置航向与当前航向的夹角，Radius表示无人船的转向半径，Size表示无人船的长度。

首先根据当前目标点以及当前航向角，采用角度势场法获得理想航向，依次判断理想区域(当前角度到理想输出角度)中每个角度的弦与该角度障碍物距离大小关系。如图3，理想区域a内的所有角度的弦长度均需大于障碍物距离Distance无人船才能在当前位置完成调向。理想区域有任一角度弦长度小于该角度障碍物距离，则当前位置不满足船体的转向，继续反向避障。

步骤五：根据用户输入的目标点顺序，载入新的目标点位置信息，继续运用改进角度势场法进行航行，直到航行至最终目标点。

如图4，无人船在航行达到当前目标点后，载入下一目标点，重新进行角度势场法开始航行。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (10)

1.一种基于改进角度势场法的无人船自主避障方法，其特征在于，包括以下步骤：

步骤一：根据用户输入的目标点顺序，获取第一个目标点信息；

步骤二：通过IMU获取当前无人船位置和航向信息；

步骤三：根据雷达获取当前位置的环境信息进行环境建模；

步骤四：根据改进角度势场法避障航行直到航行至当前目标点；

步骤五：根据用户输入的目标点顺序，更新目标点位置信息，继续航行，直到航行至最终目标点。

2.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤一具体为：

用户根据实际需求，按照顺序依次输入目标点的位置信息；

根据用户输入的目标点顺序，获取第一个目标点位置信息，并设为当前航行目标点。

3.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，步骤二所述IMU集成AHRS和GPS功能，所述AHRS通过测量三轴方向上的加速度、角速率和磁场，获知无人船当前的三维姿态；所述GPS提供自身位置信息；AHRS和GPS功能的结合用于获取无人船实时位置和行进方向。

4.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤三具体为：

激光雷达可获取当前位置360度的环境信息，所述环境信息包括障碍物所在角度以及障碍物与当前位置相距的距离；

将激光雷达提供的环境信息根据当前航向转化为无人船视角范围内的障碍物距离、角度信息。

5.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤四具体为：

根据环境信息进行可航行空间判断；

若当前位置存在可航行空间，则根据环境信息计算通行程度，确定输出角度，根据无人船特性确定待转向角度，使用PID算法进行舵机角度调整航行；若无人船当前位置不存在可航行空间，则进行反向避障；

直到到达当前目标点。

6.根据权利要求5所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述通行程度K_通行计算公式如下：

式中，K_引(θ)、K_阻(θ)分别表示角度θ受到的引力和阻力；

所述输出角度为最大通行程度所在的角度；

所述待转向角度确定方法为：若输出角度大于无人船1秒内转向角度δ，则待转向角度为δ，若输出角度小于δ，则待转向角度为输出角度。

7.根据权利要求6所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述K_引(θ)、K_阻(θ)的计算公式如下：

K_引(θ)＝cos(θ-θ_obj) (12)

式中，为对于角度的障碍点在角度θ产生的阻力，θ_obj为目标点相对于当前位置的方位角。

8.根据权利要求5所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述PID算法为增量式PID，其输出Δu[n]计算公式如下：

Δu[n]＝k_p×{e[n]-e[n-1]}+k_i×e[n]+k_d×{e[n]-2e[n-1]+e[n-2]} (4)

式中k_p表示比例系数，k_i表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；通过三个时刻的船航行与目标航行的差值作为输入得出舵机转向输出，不断调整转向角度。

9.根据权利要求5所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述反向避障具体为：

将目标点转化为与当前无人船位置对称的目标点，将当前航向方向从船头航向转变为船尾航向--以船尾为船的航向，障碍物探测区域转变为船尾视角；

根据新目标点、新航向、新障碍物探测区域进行角度势场法避障；

在确定后退停止位置时，采用圆弧的弦长度作为判断该角度是否满足船体转向通过的条件。

10.根据权利要求9所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述弦长度SubtenseDistance(θ)计算公式如下：

式中，θ为判定位置航向与当前航向的夹角，Radius为无人船的转向半径，Size为无人船的长度，角度势场法输出的正向转向角度范围内有任一角度的弦长度小于该角度障碍物距离，则当前位置不允许船体转向，继续反向避障。