一种基于速度障碍法和动态窗口法的无人艇动态避障算法制造技术

本发明专利技术涉及一种基于速度障碍法和动态窗口法的无人艇动态避障算法。本发明专利技术在速度障碍法的基础上，考虑船只长宽比大的特点，用椭圆表示无人艇与障碍物，给出一种求解椭圆切线的方法；考虑无人艇的运动学性能，只使用无人艇在给定时间内能到达的速度和方向进行避障计算。通过比较碰撞时间与无人艇避开障碍物所需的时间来确定何时开始避障，通过确定无人艇沿导航期望的方向运动与到目标点方向运动都安全时来结束避障。并根据障碍物的速度与运动方向误差增加虚拟障碍物来减小障碍物运动信息误差的影响。本发明专利技术保证无人艇在海面上自主航行的过程中能有效避开遇到的静态和动态障碍物。

A dynamic obstacle avoidance algorithm for unmanned craft based on speed obstacle method and dynamic window method

The invention relates to a dynamic obstacle avoidance algorithm for unmanned craft based on speed obstacle method and dynamic window method. On the basis of the velocity obstacle method and considering the characteristics of large ratio of ship length to width, an ellipse is used to represent the unmanned ship and obstacles, and a method for solving the ellipse tangent is given. By comparing the collision time with the time required for the UAV to avoid obstacles, we can determine when to start the obstacle avoidance, and end the obstacle avoidance by determining when the UAV is safe in the desired direction of navigation and in the direction to the target. According to the velocity and motion direction errors of obstacles, the virtual obstacles are added to reduce the influence of obstacle motion information errors. The invention ensures that the unmanned aerial vehicle can effectively avoid the static and dynamic obstacles encountered in the course of autonomous navigation on the sea surface.

【技术实现步骤摘要】
一种基于速度障碍法和动态窗口法的无人艇动态避障算法
本专利技术专利涉及移动机器人局部路径规划领域，具体是一种基于速度障碍法和动态窗口法的无人艇动态避障算法。
技术介绍
水面无人艇，简称无人艇，是一种轻型智能水面运载工具，具有体积小、造价低、速度快、机动性强等特点。随着控制技术、传感技术、无线通信技术的进步，水面无人艇获得了很大的发展。通过搭载不同的设备，无人艇可以应用在不同的领域，比如，当搭载单波束、多波束、浅底层剖面仪等声呐设备时，可以用于海底测绘、探雷反潜等；当搭载水质采样或检测设备时，可以用于环境检测；当搭载武器时，可以用于区域巡逻、海岸保护、护航、作战等任务。要想保证无人艇能够在海洋中正常安全地航行，无人艇必须能够对航行过程中遇到的岛屿、暗礁、灯塔、浮标和航行的船只等其他障碍物进行自主避障。无人艇自主避障属于路径规划领域。通常将路径规划分为全局路径规划和局部路径规划。全局路径规划通常假设环境信息完全已知，在无人艇开始行驶前，离线计算出一条从起点到终点的完全路径；但当环境不完全可知时全局路径规划不能规划出安全的路径，且实时性不好，当环境改变时，不能快速的计算出新的路径。局部路径规划通过传感器获得的部分环境信息在线实时计算出可行路径，局部路径规划可分为两类：当障碍物为静态时称为静态避障，当障碍物为动态时称为动态避障，动态避障算法也可用于静态避障。其中动态避障比较困难，因为通过无人艇自身携带的传感器很难精确的获得动态障碍物的运动信息，无法对障碍物的运动趋势进行准确的预测。局部路径规划计算量小，实时性好，但由于环境信息不完全可知，因此容易陷入极小值点。根据全局路径与局部路径的特点，目前通常采用全局与局部路径规划相结合的两层避障结构，在第一层，通过已知的环境信息，通常为地图，离线规划出一条从起点到目标点的可行路径，该路径应避开地图上已知的静态障碍物；在第二层，无人艇沿着第一层规划的路径行驶，当在行驶过程中通过传感器检测到新的障碍物时，再通过传感器获得的障碍物的详细信息进行局部避障。也有一种三层避障结构，在两层避障结构的基础上又增加了第三层，进行反应式避障，来应对障碍物运动信息不可知的避障场景。因此，不管在何种避障结构中，为了保证无人艇顺利安全的执行任务，都需要一种局部避障算法来保证无人艇避开遇到的静态和动态障碍物。
技术实现思路
本专利技术针对已有技术的不足，提供一种基于速度障碍法和动态窗口法的无人艇动态避障算法，该方法安全、有效、可靠，保证无人艇在自主航行的过程中，可以安全稳定的避开遇到的小型岛屿、灯塔、浮标等静态障碍物，和其他航行的船只等动态障碍物，避免无人艇发生碰撞。本专利技术的目的是通过以下步骤实现的：步骤一，获取障碍物信息、无人艇信息、期望的速度和方向：获取无人艇周围障碍物的形状位置与运动信息，形状位置信息通过无人艇自身携带的多传感器数据融合进行环境建模得到，根据障碍物的实际尺寸情况将障碍物建模为大小不同的椭圆形状，运动信息通过相关的算法得出。获取无人艇船体的位置、运动与姿态信息，位置信息通过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，本基于速度障碍法和动态窗口法的无人艇动态避障算法，包括以下步骤：步骤一：获取障碍物信息、无人艇信息、期望的速度和方向；步骤二：障碍物处理；步骤三：判断无人艇是否会与障碍物发生碰撞；步骤四：判断是否需要开始避障；步骤五：计算速度窗口与艏向窗口；步骤六：计算可达的避障速度矢量；步骤七：选择最优的避障速度矢量；步骤八：判断是否可结束避障；步骤九：输出初始的期望速度和方向。实施例二本实施例与实施例一基本相同，特别之处如下：获取障碍物信息、无人艇信息、期望的速度和方向。获取的障碍物信息包括本文档来自技高网...

【技术保护点】
1.一种基于速度障碍法和动态窗口法的无人艇动态避障算法，其特征在于包括以下步骤：步骤一：获取障碍物信息、无人艇信息、期望的速度和方向；步骤二：障碍物处理；步骤三：判断无人艇是否会与障碍物发生碰撞；步骤四：判断是否需要开始避障；步骤五：计算速度窗口与艏向窗口；步骤六：计算可达的避障速度矢量；步骤七：选择最优的避障速度矢量；步骤八：判断是否结束避障；步骤九：输出初始的期望速度和方向。

【技术特征摘要】
1.一种基于速度障碍法和动态窗口法的无人艇动态避障算法，其特征在于包括以下步骤：步骤一：获取障碍物信息、无人艇信息、期望的速度和方向；步骤二：障碍物处理；步骤三：判断无人艇是否会与障碍物发生碰撞；步骤四：判断是否需要开始避障；步骤五：计算速度窗口与艏向窗口；步骤六：计算可达的避障速度矢量；步骤七：选择最优的避障速度矢量；步骤八：判断是否结束避障；步骤九：输出初始的期望速度和方向。2.根据权利要求1所述的基于速度障碍法和动态窗口法的无人艇动态避障算法，其特征是：所述步骤一，获取障碍物信息、无人艇信息、期望的速度和方向：获取的障碍物信息包括位置信息、尺寸信息、运动信息；位置信息在无人艇船体坐标系下用距离和角度表示，运动信息用速度和运动方向表示，运动方向以正北方向为基准，获取的无人艇信息包括位置信息、运动与姿态信息，位置信息在北东地坐标系下用(x,y)坐标表示，运动信息包括速度大小、速度方向、加速度、角速度和角加速度，姿态信息为艏向角，以正北方向为基准，期望的速度和方向由上层算法给出，方向以正北方向为基准，以上所述的角度与方向都是顺时针方向为正值。3.根据权利要求1所述的基于速度障碍法和动态窗口法的无人艇动态避障算法，其特征是：所述步骤二，障碍物处理：将椭圆障碍物的半长轴与半短轴的长度分别加上椭圆无人艇的半长轴的长度，根据障碍物的速度与方向误差，求出障碍物实际可能的速度矢量集合VP为：式中为为通过传感器获得的障碍物的速度矢量，VE为障碍物速度矢量误差集合，为闵可夫斯基矢量和运算。4.根据权利要求1所述的基于速度障碍法和动态窗口法的无人艇动态避障算法，其特征是：所述步骤三，判断无人艇是否会与障碍物发生碰撞：建立无人艇船体坐标系与障碍物坐标系，障碍物坐标系以障碍物中心为原点，长轴方向为x轴，x轴顺时针旋转90度为y轴，将无人艇在船体坐标系中的坐标转到障碍物坐标系中，在障碍物坐标系中求出椭圆障碍物相对于无人艇中心两条切线的切点，将两个切点坐标转到船体坐标系下，求出在船体坐标系下椭圆障碍物相对于无人艇中心的左右两条切线，求出无人艇相对于...

【专利技术属性】
技术研发人员：谢少荣，张洋洋，罗均，彭艳，
申请(专利权)人：上海大学，
类型：发明
国别省市：上海,31