本发明专利涉及移动机器人局部路径规划领域，具体是一种基于速度障碍法和动态窗口法的无人艇动态避障算法。
背景技术：
水面无人艇，简称无人艇，是一种轻型智能水面运载工具，具有体积小、造价低、速度快、机动性强等特点。随着控制技术、传感技术、无线通信技术的进步，水面无人艇获得了很大的发展。通过搭载不同的设备，无人艇可以应用在不同的领域，比如，当搭载单波束、多波束、浅底层剖面仪等声呐设备时，可以用于海底测绘、探雷反潜等；当搭载水质采样或检测设备时，可以用于环境检测；当搭载武器时，可以用于区域巡逻、海岸保护、护航、作战等任务。
要想保证无人艇能够在海洋中正常安全地航行，无人艇必须能够对航行过程中遇到的岛屿、暗礁、灯塔、浮标和航行的船只等其他障碍物进行自主避障。无人艇自主避障属于路径规划领域。
通常将路径规划分为全局路径规划和局部路径规划。全局路径规划通常假设环境信息完全已知，在无人艇开始行驶前，离线计算出一条从起点到终点的完全路径；但当环境不完全可知时全局路径规划不能规划出安全的路径，且实时性不好，当环境改变时，不能快速的计算出新的路径。局部路径规划通过传感器获得的部分环境信息在线实时计算出可行路径，局部路径规划可分为两类：当障碍物为静态时称为静态避障，当障碍物为动态时称为动态避障，动态避障算法也可用于静态避障。其中动态避障比较困难，因为通过无人艇自身携带的传感器很难精确的获得动态障碍物的运动信息，无法对障碍物的运动趋势进行准确的预测。局部路径规划计算量小，实时性好，但由于环境信息不完全可知，因此容易陷入极小值点。根据全局路径与局部路径的特点，目前通常采用全局与局部路径规划相结合的两层避障结构，在第一层，通过已知的环境信息，通常为地图，离线规划出一条从起点到目标点的可行路径，该路径应避开地图上已知的静态障碍物；在第二层，无人艇沿着第一层规划的路径行驶，当在行驶过程中通过传感器检测到新的障碍物时，再通过传感器获得的障碍物的详细信息进行局部避障。也有一种三层避障结构，在两层避障结构的基础上又增加了第三层，进行反应式避障，来应对障碍物运动信息不可知的避障场景。
因此，不管在何种避障结构中，为了保证无人艇顺利安全的执行任务，都需要一种局部避障算法来保证无人艇避开遇到的静态和动态障碍物。
技术实现要素：
本发明针对已有技术的不足，提供一种基于速度障碍法和动态窗口法的无人艇动态避障算法，该方法安全、有效、可靠，保证无人艇在自主航行的过程中，可以安全稳定的避开遇到的小型岛屿、灯塔、浮标等静态障碍物，和其他航行的船只等动态障碍物，避免无人艇发生碰撞。
本发明的目的是通过以下步骤实现的：
步骤一，获取障碍物信息、无人艇信息、期望的速度和方向：
获取无人艇周围障碍物的形状位置与运动信息，形状位置信息通过无人艇自身携带的多传感器数据融合进行环境建模得到，根据障碍物的实际尺寸情况将障碍物建模为大小不同的椭圆形状，运动信息通过相关的算法得出。获取无人艇船体的位置、运动与姿态信息，位置信息通过gps或北斗获得，运动与姿态信息通过惯导传感器获得。获取无人艇期望的速度与方向信息，根据任务的不同，该信息从上层算法获得。
步骤二，障碍物处理：
将无人艇看成椭圆，对椭圆障碍物进行膨胀，即将椭圆障碍物的半长轴与半短轴的长度分别加上椭圆无人艇的半长轴的长度，将障碍物放大。将障碍物的速度矢量误差离散成一个误差矢量集合ve，该集合中的每一个误差矢量加上传感器获得的障碍物速度矢量会得到一个矢量集合vp，该集合中每一个矢量都看成一个虚拟障碍物的速度矢量，虚拟障碍物的位置、尺寸与传感器获得的障碍物的位置、尺寸相同。
步骤三，判断无人艇是否会与障碍物发生碰撞：
首先求出椭圆障碍物相对于无人艇中心的左右两条切线，然后求出无人艇与该障碍物的相对速度大小与方向，若相对速度方向落入左右两条切线夹角内，则无人艇会与障碍物发生碰撞，进入步骤四；若相对速度方向没有落入左右两条切线夹角内，则无人艇不会与障碍物发生碰撞，进入步骤九。
步骤四，判断是否需要开始避障：
假设无人艇与障碍物都为匀速直线运动，求出无人艇从当前时刻开始到发生碰撞所需的时间tcpa；假设无人艇速度大小不改变，无人艇以恒定的角加速度转弯进行避障，求出避开障碍物所需的时间为ta。若tcpa＞k×ta，则不需要开始避障，进入步骤九；若tcpa≤k×ta，则需要开始避障，进入计算避障速度与方向模块，即进入步骤五。
步骤五，计算速度窗口与艏向窗口
假设无人艇的加速度与角加速度恒定，计算δt时间内无人艇能达到的线速度，称为速度窗口；计算δt时间内无人艇艏向能够达到的角度，称为艏向窗口。将速度窗口与艏向窗口离散化，组成无人艇可达的速度矢量集合rv。
步骤六，计算可达的避障速度矢量
rv集合中会与障碍物发生碰撞的速度矢量组成的集合称为vo集合，从rv集合中排除vo集合即为可达的避障速度矢量集合rav。
步骤七，选择最优的避障速度矢量
rav集合中所有的速度矢量都可以作为避障速度矢量，构建评价函数g，从rav集合中选择一个使得评价函数值最小的速度矢量作为避障速度矢量。
步骤八，判断是否可结束避障
判断无人艇沿初始期望的速度与方向行驶不会发生碰撞，且以当前速度大小沿到目标点方向运动也不会发生碰撞时，即可结束避障。当可结束避障时，进入步骤九；当不可结束避障时，输出选择的避障速度与方向，再次进入步骤五。
步骤九，输出初始的期望速度和方向
进入该步骤时，认为无人艇沿着初始的期望速度和方向行驶时不会发生碰撞，则直接输出初始的期望速度和方向。
本发明专利与现有技术相比较，具有如下显而易见的突出实质性特点和显著技术进步：本发明在速度障碍法的基础上，考虑船只长宽比大的特点，用椭圆表示无人艇与障碍物，给出一种求解椭圆切线的方法；考虑无人艇的运动学性能，只使用无人艇在给定时间内能到达的速度和方向进行避障计算。通过比较碰撞时间与无人艇避开障碍物所需的时间来确定何时开始避障，通过确定无人艇沿导航期望的方向运动与到目标点方向运动都安全时来结束避障。并根据障碍物的速度与运动方向误差增加虚拟障碍物来减小障碍物运动信息误差的影响。本发明专利保证无人艇在海面上自主航行的过程中能有效避开遇到的静态和动态障碍物。
附图说明
图1是本发明开始避障前的流程图；
图2是本发明开始避障后的路程图；
图3a是在速度空间中，无人艇可达到的速度矢量示意图，图3b是在速度空间中，无人艇可达到的避障速度矢量示意图；
图4是存在一个动态障碍物时，无人艇避障仿真的效果图；
图5是存在三个动态障碍物时，无人艇避障仿真的效果图。
具体实施方式
下面结合附图和优选实施例对本发明专利作进一步说明。
实施例一
参见图1、图2、图3，本基于速度障碍法和动态窗口法的无人艇动态避障算法，包括以下步骤：
步骤一：获取障碍物信息、无人艇信息、期望的速度和方向；
步骤二：障碍物处理；
步骤三：判断无人艇是否会与障碍物发生碰撞；
步骤四：判断是否需要开始避障；
步骤五：计算速度窗口与艏向窗口；
步骤六：计算可达的避障速度矢量；
步骤七：选择最优的避障速度矢量；
步骤八：判断是否可结束避障；
步骤九：输出初始的期望速度和方向。
实施例二
本实施例与实施例一基本相同，特别之处如下：
获取障碍物信息、无人艇信息、期望的速度和方向。获取的障碍物信息包括位置信息、尺寸信息、运动信息。位置信息在无人艇船体坐标系下用距离和角度表示，运动信息用速度和运动方向表示，运动方向以正北方向为基准。获取的无人艇信息包括位置信息、运动与姿态信息。位置信息在北东地坐标系下用(x,y)坐标表示，运动信息包括速度大小、速度方向、加速度、角速度和角加速度。姿态信息为艏向角，以正北方向为基准。期望的速度和方向由上层算法给出，方向以正北方向为基准。以上所述的角度与方向都是顺时针方向为正值。
障碍物处理。将椭圆障碍物的半长轴与半短轴的长度分别加上椭圆无人艇的半长轴的长度。根据障碍物的速度与方向误差，求出障碍物实际可能的速度矢量集合vp为：
式中为
判断无人艇是否会与障碍物发生碰撞。建立无人艇船体坐标系与障碍物坐标系，障碍物坐标系以障碍物中心为原点，长轴方向为x轴，x轴顺时针旋转90度为y轴。将无人艇在船体坐标系中的坐标转到障碍物坐标系中，在障碍物坐标系中求出椭圆障碍物相对于无人艇中心两条切线的切点，将两个切点坐标转到船体坐标系下，求出在船体坐标系下椭圆障碍物相对于无人艇中心的左右两条切线。求出无人艇相对于障碍物的相对速度，若相对速度方向落入左右两条切线夹角内，则无人艇会与障碍物发生碰撞。
判断是否需要开始避障。根据下式计算出无人艇与障碍物发生碰撞所需的时间tcpa：
式中
式中式中(x,y)，(xa,ya)分别为p点、a点在障碍物坐标系中的坐标，v,θ0,ω0,α分别为无人艇当前的速度，艏向，角速度，角加速度，pc1+pc2为在障碍物坐标系中p点到椭圆两焦点的距离和，a为椭圆半长轴。当tcpa≤k×ta时无人艇开始避障，否则无人艇不开始避障。
计算速度窗口与艏向窗口。计算δt时间内无人艇能达到的速度，称为速度窗口；计算δt时间内无人艇艏向能够达到的角度，称为艏向窗口。将速度窗口离散为m个速度，艏向窗口离散为n个方向，离散的速度和方向组成m×n个速度矢量，将这些无人艇在δt时间内可达到的离散的速度矢量组成的集合称为rv。
计算可达的避障速度矢量。将rv集合中会与障碍物发生碰撞的速度矢量组成的集合称为vo集合，通过下式计算可达的避障速度矢量集合rav：
选择最优的避障速度矢量。构建评价函数：
式中vc，θh分别表示无人艇当前的速度和艏向，(vi,θj)表示离散的一个速度矢量。从rav集合中选择一个使得评价函数值最小的速度矢量作为避障速度矢量。
判断是否可结束避障。判断无人艇沿初始期望的速度与方向行驶不会发生碰撞，且以当前速度沿到目标点方向运动也不会发生碰撞时，即可结束避障。
实施例三
本基于速度障碍法和动态窗口法的无人艇动态避障算法具体操作步骤如下：
步骤一，如图1，获取障碍物信息、无人艇信息、期望的速度和方向：
通过无人艇自身携带的雷达、视觉、激光雷达、声呐传感器获得环境信息，进行数据融合与环境建模得到障碍物的形状与位置信息，根据障碍物的实际尺寸情况将障碍物建模为大小不同的椭圆形状，障碍物的形状通过椭圆的半长轴长与半短轴长表示，障碍物的位置信息在船体坐标系下，用距离与角度来表示，距离为无人艇中心到障碍物中心的距离，角度为障碍物方向相对于无人艇艏向的角度，角度范围为[-180.0,180.0)，单位为度，顺时针方向角度为正值；通过跟踪算法计算出障碍物的运动信息，障碍物的运动信息用速度大小与速度方向表示，速度方向以正北方向为基准，方向范围为[-180.0,180.0)，单位为度，顺时针方向为正值。
通过gps或北斗传感器获得无人艇的经纬度位置信息，将无人艇的经纬度位置信息转为北东地系下的(x,y)坐标，北东地坐标系以正北方向为x轴，正东方向为y轴，地球表面一点为坐标原点。通过惯导传感器获得无人艇的运动与姿态信息，运动信息用速度大小、速度方向、加速度、角速度和角加速度表示；姿态信息主要为无人艇艏向角。速度方向与艏向角的单位为度，范围为[-180.0,180.0)，角速度的单位为度/秒，角加速度的单位为度/秒2，都是顺时针方向为正值。
通过上层算法获得无人艇初始的期望速度与方向，即在不需要避障的情况下，无人艇为了执行任务，应该航行的速度与方向。
步骤二，如图1，障碍物处理
将无人艇看成椭圆，对椭圆障碍物进行膨胀，即将椭圆障碍物的半长轴与半短轴的长度分别加上椭圆无人艇的半长轴的长度，将障碍物放大。
通过传感器获得的障碍物的速度矢量为
式中
将vp集合中的每个速度矢量都看为一个虚拟障碍物的速度矢量，虚拟障碍物的位置、尺寸与传感器获得的障碍物的位置、尺寸相同。
步骤三，判断无人艇是否会与障碍物发生碰撞：
建立无人艇船体坐标系和障碍物坐标系，船体坐标系以船体中心为坐标原点，船艏方向为x轴，右舷方向为y轴，障碍物坐标系以障碍物中心为原点，长轴方向为x轴，x轴顺时针旋转90度为y轴。在障碍物坐标系中用椭圆标准方程表示椭圆障碍物，首先将无人艇在船体坐标系中的坐标转到障碍物坐标系中，如下式：
usvobs＝r-1(usvboat-c)
式中usvboat为无人艇在船体坐标系中的坐标，usvobs为无人艇在障碍物坐标系中的坐标，c为障碍物中心点在船体坐标系中的坐标，r为障碍物坐标系相对于船体坐标系的旋转矩阵：
在障碍物坐标系中联立标准椭圆方程与椭圆切线方程，如下式：
即可求得在障碍物坐标系中椭圆障碍物相对与无人艇中心两条切线切点的坐标(xt,yt)，然后再通过下式：
tboat＝rtobs+c
计算切点(xt,yt)在船体坐标系中的坐标，求出船体坐标系中两切点坐标后即可计算在船体坐标系中椭圆障碍物相对于无人艇的左右两条切线。
求无人艇相对于障碍物的相对速度：
式中
若相对速度方向落入左右两条切线夹角内，则无人艇会与障碍物发生碰撞，进入步骤四；若相对速度方向没有落入左右两条切线夹角内，则无人艇不会与障碍物发生碰撞，进入步骤九。
步骤四，如图1，判断是否需要开始避障：
假设无人艇与障碍物都为匀速直线运动，求出无人艇从当前时刻开始到与障碍物发生碰撞所需的时间tcpa：
式中
当无人艇与障碍物发生碰撞时，将无人艇中心点的位置称为cpausv。假设无人艇速度大小保持不变，可以以恒定的角加速度转弯。无人艇以恒定速度和方向驶向cpausv点的过程中，在某点a以恒定角加速度转弯，无人艇的曲线轨迹恰好与椭圆障碍物相切于p点，则认为无人艇从p点开始恰好避开了障碍物，求无人艇从a点行驶到p点所需的时间为ta：
式中式中(x,y)，(xa,ya)分别为p点、a点在障碍物坐标系中的坐标，v,θ0,ω0,α分别为无人艇当前的速度，艏向，角速度，角加速度，pc1+pc2为在障碍物坐标系中p点到椭圆两焦点的距离和，a为椭圆半长轴。
ta即为无人艇避开障碍物所需的时间。若tcpa＞k×ta，则不需要开始避障，进入步骤九,其中k为大于1的系数；若tcpa≤k×ta，则需要开始避障，进入计算避障速度与方向模块，即进入步骤五。
步骤五，如图2，计算速度窗口与艏向窗口
假设无人艇的加速度与角加速度恒定，计算δt时间内无人艇能达到的线速度，称为速度窗口：
计算δt时间内无人艇艏向能够达到的角度，称为艏向窗口：
式(9)中vc为无人艇当前的移动速度，
将速度窗口vd离散为m个速度，艏向窗口θd离散为n个方向，离散后的一个速度vi和一个方向θi组成一个速度矢量(vi,θi)，因此总共有m×n个速度矢量，将这些无人艇在δt时间内可达到的离散的速度矢量组成的集合称为rv，如图3a所示。
步骤六，如图2，计算可达的避障速度矢量
使用步骤三中的方法计算rv集合中会与障碍物发生碰撞的速度矢量，会与障碍物发生碰撞的速度矢量组成的集合称为vo集合，计算可达的避障速度矢量集合rav，如图3b所示：
步骤七，如图2，选择最优的避障速度矢量
rav集合中所有的速度矢量都可以作为避障速度矢量，构建评价函数g：
式中vc，θh分别表示无人艇当前的速度和艏向，(vi,θj)表示离散的一个速度矢量。
从rav集合中选择一个使得评价函数值最小的速度矢量作为避障速度矢量。
步骤八，如图2，判断是否可结束避障
判断无人艇沿初始期望的速度与方向行驶不会发生碰撞，且以当前速度沿到目标点方向运动也不会发生碰撞时，认为可结束避障，即满足下式时：
可结束避障，进入步骤九；当不可结束避障时，输出选择的避障速度与方向，再次进入步骤五。式中
步骤九，输出初始的期望速度和方向
进入该步骤时，认为无人艇沿着初始的期望速度和方向行驶时不会发生碰撞，则直接输出初始的期望速度和方向。
通过以上步骤可以保证无人艇安全、有效、可靠的避开航行中遇到的静态和动态障碍物。
图4为存在一个动态障碍物时，无人艇避障仿真的效果图，图中无人艇沿航迹线向正北方向航行，一个障碍物向正西方向匀速运动，无人艇从障碍物后方进行避障，避开障碍物后，无人艇继续沿着初始的期望速度与方向运动。
图5为存在三个动态障碍物时，无人艇避障仿真的效果图，图中无人艇沿航迹线向正北方向航行，障碍物1与障碍物3向正西方向匀速运动，障碍物2向正东方向匀速运动，无人艇分别从三个障碍物的后方避开障碍物，然后沿着初始的期望速度与方向运动。