本发明提供了一种基于人类避障行为的无人机避障方法及其装置，其包括：实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在障碍物或突发威胁，则建立所述障碍物或突发威胁所在的障碍区，并根据所述障碍区中离无人机最近的障碍边建立避碰行为区；如果无人机进入所述避碰行为区，则根据所述最近的障碍边计算避障速度矢量；所述避障速度矢量包括无人机避障速度以及避障航向；根据计算所得到的避障速度矢量，计算无人机前进的步长；如果没有探测到任何的障碍物或者突发威胁，则根据目标位置计算目标引导速度矢量，并根据该目标引导速度矢量计算前进的步长；判断是否到达目标，如果没有到达，则重新探测并计算；否则结束。
1.一种基于人类避障行为的无人机避障方法，其包括：
步骤1、实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在，转向步骤2；否则转向步骤5；
步骤2、如果存在障碍物或突发威胁，则建立所述障碍物或突发威胁所在的障碍区，并根据所述障碍区中离无人机最近的障碍边建立避碰行为区；
步骤3、如果无人机进入所述避碰行为区，则根据所述最近的障碍边计算避障速度矢量；所述避障速度矢量包括无人机避障速度以及避障航向；
步骤4、根据所计算得到的避障速度矢量，计算无人机前进的步长；
步骤5、如果没有探测到任何的障碍物或者突发威胁，则根据目标位置计算目标引导速度矢量，并根据该目标引导速度矢量计算前进的步长；
步骤6、判断是否到达目标，如果没有到达，则转步骤1；否则结束；
其中，步骤2中如下建立避碰行为区：将所述障碍区离无人机最近的障碍边外扩安全距离阈值τ，再将τ区域外扩一定范围，得到的连通区域[τ，τ+Disaction]作为无人机的避碰行为区；其中，Disaction为避障行为区的宽度，且Disaction＝vcurrent·π/θmax，vcurrent为无人机当前速度的大小，θmax为无人机的最大转弯角度。
2.如权利要求1所述的无人机避障方法，其特征在于，步骤2中通过如下方法建立障碍区：将障碍物近似为矩形区域作为所述障碍区，该障碍区用障碍物的左上顶点和右下顶点来表示。
3.如权利要求1所述的无人机避障方法，其特征在于，步骤3中所述无人机避障速度为无人机的当前行驶速度，所述避障航向如下计算：其中，θavoidcollision为避障航向，xf为到障碍区离无人机最近的障碍边L0的平行逆时针方向；xe为航向控制常量，满足xe∈(0，π/2]，ε为无人机当前位置Positioncurrent到所述障碍区离无人机最近的障碍边L0的垂直距离；无人机当前位置点在障碍区最近边L0的左侧时，ρ＝1；无人机当前位置点在障碍区最近变L0的右侧时，ρ＝-1；k为航向调整速率参数，k≥1且为奇数；vcurrent为无人机当前速度的大小；θmax为无人机的最大转弯角度。
4.如权利要求3所述的无人机避障方法，其特征在于，k＝3，xe＝π/6。
5.如权利要求1所述的无人机避障方法，其特征在于，τ＝10。
6.如权利要求1-5任一项所述的无人机避障方法，其特征在于，步骤5中目标引导速度矢量包括无人机速度航向角和飞行速度，所述飞行速度为当前飞行速度；所述速度航向角如下计算：其中，θguideDirection为无人机速度航向角，(xgoal，ygoal)表示目标位置，(xcurrent，ycurrent)表示无人机当前位置。
7.如权利要求1-5任一项所述的无人机避障方法，其特征在于，当存在多个障碍物或突发威胁物时，选择离无人机最近的障碍物或突发威胁建立障碍区。
8.一种基于人类避障行为的无人机避障装置，其包括：
初始判断模块，其用于实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在，则转向建立模块；否则转向第二步长计算模块；
避障行为区建立模块，其用于存在障碍物或突发威胁时，则建立所述障碍物或突发威胁所在的障碍区，并根据所述障碍区中离无人机最近的障碍边建立避碰行为区，其中建立避碰行为区具体为：将所述障碍区离无人机最近的障碍边外扩安全距离阈值τ，再将τ区域外扩一定范围，得到的连通区域[τ，τ+Disaction]作为无人机的避碰行为区，Disaction为避障行为区的宽度，且Disaction＝vcurrentπ/θmax，vcurrent为无人机当前速度的大小，θmax为无人机的最大转弯角度；
避障速度矢量模块，其用于如果无人机进入所述避碰行为区，则根据所述最近的障碍边计算避障速度矢量；所述避障速度矢量包括无人机避障速度以及避障航向；
避障行为步长计算模块，其根据所计算得到的避障速度矢量，计算无人机前进的步长；
目标引导行为步长计算模块，其用于在没有探测到任何的障碍物或者突发威胁，根据目标位置计算目标引导速度矢量，并根据该目标引导速度矢量计算无人机前进的步长；
结果判断模块，其用于判断无人机是否到达目标，如果没有到达，则转初始判断模块；
一种无人机的避障方法及其装置
技术领域
[0001] 本发明涉及航空科学技术领域，尤其涉及一种基于人类避障行为的无人机避障方法及其装置。
背景技术
[0002] 无人机已经成为当前各国投入大量人力和财力研发的作战装备，随着战场环境逐渐复杂和任务要求的提高，多无人机协同执行侦察任务的需求日益重要和普遍。本世纪70年代之后，国内外研究者们陆续提出了很多典型的无人机实时航路规划方法，并取得了显著的研究成果，但大多数方法都存在不符合实际需求的缺陷。
[0003] 无人真实的飞行环境信息很难完全预知，经常会遇到未知的突发威胁或障碍，这时预先规划的全局航路已不能满足要求。即使没有突发威胁出现，全局航路规划方法也会由于需要离线预处理和对环境信息的高依赖性，而不适用于复杂时变的战场环境，欠缺实用性。对应离线预处理的全局航路规划方法的就是实时航路规划方法，主要根据实时获取的环境信息进行快速航路规划，达到最小能耗实时避障的目的。学者们已经提出一些动态环境下实时航路规划的方法和策略，主要分为两类，一是基于已知环境信息规划初始航路，在出现突发威胁障碍时局部调整初始航路，绕过威胁后重新回到预先规划的航路上；另一种方法是在发现新威胁时从飞行器当前位置到目标位置之间完全重规划一条新的航路。以上两种方法虽然弥补了全局航路规划缺乏应变能力的不足，但牺牲了解决问题的时间，不能满足无人机航路重规划安全快速的要求。
[0004] 在无人飞行器航路规划算法方面，国内外的学者已经进行了大量的研究工作，典型算法有图搜索法、进化法、人工势场法等，每一种算法都有自身的优势和不足。图搜索法包括Voronoi图法、快速随机搜索树法、A*算法，这些方法可以快速在构造图中搜索到航路，但其搜索的复杂度随着地理空间维度的增加成几何级数增长，不适用于复杂地形和实时计算；另外这些方法得到的航路由折线构成，不满足无人飞行器的机动能力约束条件；进化算法可以满足三维地形和飞行性能的约束要求，但是其迭代进化过程中存在盲目性，计算量很大，不满足实时性要求，并且对机载或地面站处理器的性能要求较高；人工势场法将目标和障碍物分别看做引力场源和斥力场源，无人机沿着引力和斥力的合力方向运动，可以得到平滑的飞行轨迹，该方法能够用于实时控制，不足之处在于存在局部陷阱，解算出的航路容易出现抖动现象。
发明内容
[0005] 本发明的目的在于克服现有技术的不足，借鉴人类躲避障碍物的机理，提出一种适合于无人机机载实时解算的航路规划方法。人类避障行为具有几点特征，特别适用于无人机实时航路规划技术：(1)人类通过肉眼只能看到距离自己比较近的环境信息，正好符合无人机飞行过程中实时探测局部环境信息的实际情况；(2)没有障碍物或距离障碍物较远时，人类会保持朝向目标点的运动状态；(3)人类在观察到存在障碍物时，会在必要的范围内采取避障行为，按照尽可能省时省力的原则调整自身运动状态来避障，直至到达目标点。
[0006] 人类避障行为不会受环境中障碍物数量多少的影响，会以局部探测信息作为依据，结合自身机动性能来实施避障，本发明以这些特征为出发点，提出快速有效的无人机实时航路规划方法。
[0007] 为此，本发明提出一种基于人类避障行为的无人机避障方法，其包括：
[0008] 步骤1、实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在，转向步骤2；否则转向步骤5；
[0009] 步骤2、如果存在障碍物或突发威胁，则建立所述障碍物或突发威胁所在的障碍区，并根据所述障碍区中离无人机最近的障碍边建立避碰行为区；
[0010] 步骤3、如果无人机进入所述避碰行为区，则根据所述最近的障碍边计算避障速度矢量；所述避障速度矢量包括无人机避障速度以及避障航向；
[0011] 步骤4、根据所计算得到的避障速度矢量，计算无人机前进的步长；
[0012] 步骤5、如果没有探测到任何的障碍物或者突发威胁，则根据目标位置计算目标引导速度矢量，并根据该目标引导速度矢量计算前进的步长；
[0013] 步骤6、判断是否到的目标，如果没有到达，则转步骤1；否则结束。
[0014] 本发明还提出了一种基于人类避障行为的无人机避障装置，其包括：
[0015] 初始判断模块，其用于实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在，则转向建立模块；否则转向第二步长计算模块；
[0016] 避障行为区建立模块，其用于存在障碍物或突发威胁时，则建立所述障碍物或突发威胁所在的障碍区，并根据所述障碍区中离无人机最近的障碍边建立避碰行为区；
[0017] 避障速度矢量模块，其用于如果无人机进入所述避碰行为区，则根据所述最近的障碍边计算避障速度矢量；所述避障速度矢量包括无人机避障速度以及避障航向；
[0018] 避障行为步长计算模块，其根据所计算得到的避障速度矢量，计算无人机前进的步长；
[0019] 目标引导行为步长计算模块，其用于在没有探测到任何的障碍物或者突发威胁，根据目标位置计算目标引导速度矢量，并根据该目标引导速度矢量计算无人机前进的步长；
[0020] 结果判断模块，其用于判断无人机是否到的目标，如果没有到达，则转初始判断模块；否则结束操作。
[0021] 本发明根据人类避碰行为的思想，能够快速解算出平滑有效的安全航路，且满足无人机机动性能约束，计算耗时小，容易实现，为工程应用提供了一种实用性强的无人机实时航路规划方法。其优点是方法简单，实时性强，可以实现无人机以尽量少的飞行能耗代价，快速安全的避障。
附图说明
[0022] 图1是本发明中基于人类避障行为的无人机避障方法流程示意图；
[0023] 图2是本发明中计算多障碍物最近边缘示意图；
[0024] 图3是本发明中无人机避碰行为区和避障速度矢量模型示意图；
[0025] 图4是本发明中无人机飞行环境示意图；
[0026] 图5是本发明中无人机障碍物模型示意图；
[0027] 图6是根据本发明方案的仿真结果图。
具体实施方式
[0028] 下面结合附图和具体实例，说明本发明技术方案作进一步说明。
[0029] 图1示出了本发明公开的一种基于人类避障行为的无人机避障方法流程图。如图1所示，该方法具体包括以下步骤：
[0030] 步骤1、传感器实时探测并判断无人机前进方向上是否存在障碍物及突发威胁；如果存在，转向步骤2；如果不存在障碍物及威胁，则转向步骤5；
[0031] 步骤2、如果存在障碍物或突发威胁，则对障碍物或突发威胁所在的障碍区进行建模，判断障碍区模型中最近的障碍边缘，利用最近边缘构建避碰行为区Γ，当无人机飞到避碰行为区Γ范围内就采取避障行为，具体方法将在步骤3中阐述，这里先介绍避碰行为区Γ的求解方法。
[0032] (1)所述障碍区如下建模：
[0033] 将障碍物近似为矩形区域，第i个矩形障碍物用左上顶点和右下顶点来表示，即矩形障碍物的宽度和长度为widthi和lengthi，设共发现M个障碍物，构成的禁飞区表示为：
[0034]
[0035] 根据障碍物集合组成的禁飞区域Forbidden_zone，可以在禁飞区外围构建无人机避碰行为区Γ。
[0036] (2)解算出所有障碍区边缘中最近的障碍边缘
[0037] 图2表示出了本发明中计算多障碍物最近边缘示意图。如图2所示，最近的威胁障碍边的求解方法是：建立无人机飞行空间所在的X-Y直角坐标系，逐一计算无人机距离探测到的障碍物所有边的最短距离，设无人机距离第i条障碍物边最短距离为MinDisi，如果无人机到第i条障碍物边的垂线的垂心在障碍物边上，则MinDisi即为距离第i条边垂直距离Disi；如果垂心没有在此障碍物边上，则有MinDisi＝min{DisLeft_i，DisRight_i}，DisLeft_i、DisRight_i为第i条障碍物边的两个端点到无人机的距离。比较无人机到障碍物所有边MinDisi的大小，得到距离最近的威胁障碍边缘L0，将L0构造成矢量边，其大小即为边的长度||L0||，方向为xf，取L0边逆时针走向。
[0038] (3)所述无人机避碰行为区Γ如下建立：
[0039] 根据人类在观察到障碍物之后，会先有个预判过程，考虑到自身避障行为能力，从而判断运动到什么位置采取避障措施。如果躲避障碍物的行为很灵活，就可以在离障碍物很近的地方再采取避碰行为，反之则远离威胁时尽早避障。根据这个思想建立无人机探测到的障碍物的避碰行为区Γ，当无人机在避碰行为区Γ之内采取必要的避障行为，避碰行为区Γ之外保持朝向目标的运动状态不变。避碰行为区Γ的大小同无人机避障的机动性能有关，主要依靠无人机转弯绕行来实现。
[0040] 图3示出了本发明中无人机避碰行为区的示意图。无人机主要依靠调整航向，绕行障碍物来实现避障，因此避碰行为区Γ大小的选取同无人机避障时的转弯机动性能有关。如图3所示，将障碍区周边外扩安全距离阈值τ，τ范围内同样为无人机禁飞区，障碍物及τ区域再外扩一定范围，即连通区域[τ，τ+Disaction]作为无人机的避碰行为区Γ。由于避障行为所需要的区域同无人机当前速度成正比，同无人机机动性能中的最大转弯角度成反比，因此这里将避障行为区Γ宽度定义为：Disaction＝vcurrent·π/θmax，则得到宽度为[τ，τ+vcurrent·π/θmax]的连通区域作为无人机的避碰行为区Γ，其中vcurrent为无人机当前速度的大小，θmax为无人机的最大转弯角度。
[0041] 步骤3、根据所建立的所有障碍区组成的障碍群集合信息Forbidden_zone和步骤2中得到的避碰行为区判断无人机是否进入了避碰行为区，如果进入就计算避障航向θavoidcollision，得到无人机的避障速度矢量 如果没有进入避碰行为区，就保持原来的运动状态继续飞行。
[0042] 该步骤中，由于本发明主要考虑无人机在线实时航路规划问题，因此只对最近的威胁障碍边进行快速规避。
[0043] 另外，所述无人机的避障速度矢量如下计算：
[0044] 本发明提出一种无人机的避障速度矢量模型，借鉴人类以尽可能少的能耗行为来调整自身前进方向，实现安全绕行障碍物的避障思想，因此无人机也根据障碍物的距离和自身机动性能来计算避障绕行航向。
[0045] 图3为本发明中无人机避障速度矢量模型的示意图。如图3所示，vcurrent为无人机当前速度，θcurrent为无人机当前航向，无人机需要在避碰行为区Γ内，通过逐渐调整飞行方向θcurrent，直至绕出危险区域或调整为同障碍边缘平行的xf方向。这里通过解算出在避碰行为区Γ内无人机的避障速度矢量，实现将无人机引导至障碍禁飞区外。如果计算出所有位置的避障速度矢量，就组成了威胁周边的避障速度矢量场。
[0046] 为了保证飞行状态稳定安全，无人机首选的避障方式就是在机动性能允许的范围内调整本身的飞行航向，因此避障速度矢量中的速度大小保持不变，依旧为vcurrent，主要是更新避障航向θavoidcollision，具体计算方法如式(2)所示，其中xe为航向控制常量，满足xe∈(0，π/2]，ε为无人机当前位置Positioncurrent到障碍区最近边L0的垂直距离，表示为ε＝dis(Positoncurrent，L0)。
[0047]
[0048] 式(2)中，无人机当前位置点在障碍物矢量边L0的左侧时，ρ＝1；无人机当前位置点在障碍物矢量边L0的右侧时，ρ＝-1；k为航向调整速率参数，k≥1且为奇数。其中，优选地，k＝3，xe＝π/6，τ＝10。
[0049] 步骤4、按照步骤3中计算得到的避障速度矢量，无人机前进一个步长，即一个飞行控制周期的长度；
[0050] 步骤5、如果没有探测到障碍物或突发威胁，根据公式(3)计算无人机的目标引导速度矢量 并前进一个步长。
[0051] 公式(3)为速度航向角θguideDirection的计算方法，其中目标位置(xgoal，ygoal)已知，无人机当前位置为(xcurrent，ycurrent)。其中飞行速度大小vguideDirection保持不变，有利于无人机平稳状态飞行。
[0052]
[0053] 步骤6、判断是否到达目标点，如果没有到达目标点，转向步骤1，继续实时探测周围环境信息并实时解算避障速度；如果达到目标点，则结束计算。
[0054] 为验证本发明的方法和装置，进行试验仿真。
[0055] 图4示出了本试验中障碍物的分布情况示意图。如图4所示，无人机飞行环境中存在9个已知高山障碍物，在无人机飞行将至目标点Goal时，将会有一个突发威胁出现。
[0056] 图5示出了上述试验中二维化的障碍物示意图。如图5所示，首先将9个已知高山和1个突发威胁简化为二维平面的10个矩形障碍区，再利用本发明的方法和装置，随着无人机的飞行实时探测且解算，从而得到如图6所示的飞行航线结果，通过试验可以验证本发明的方法和装置是有效的。
[0057] 以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
