一种无人飞行器避障路径规划算法、路径规划算法制造技术

发明专利技术公开了一种无人飞行器避障路径规划算法、路径规划算法，包括：根据无人飞行器位置、障碍物位置、目标位置，检测无人飞行器位置是否是局部最小值位置，若是，对局部最小值位置的无人飞行器施加逃逸力，并重新初始化势场使无人飞行器离开局部最小位置；其中，局部最小值位置是，人工势场法中无人飞行器的吸引力和排斥力在预设的误差范围内大小相等方向相反，且非目标位置的位置。本公开通过检测无人飞行器是否处于局部最小值位置，设置逃逸力解决局部最小值问题，每次检测到局部最小问题时，都会激活逃逸力，也就是将外部排斥力作为摆脱局部最小值的解决方案。摆脱局部最小值的解决方案。摆脱局部最小值的解决方案。

【技术实现步骤摘要】
一种无人飞行器避障路径规划算法、路径规划算法


[0001]本公开属于路径规划领域，具体涉及一种无人飞行器避障路径规划算法、路径规划算法。

技术介绍

[0002]人工势场法(APF)作为现在常见的一种避障路径规划算法，是80年代中期首次提出的。相比于A*和遗传算法等多层的搜索算法，人工势场法具有建模简单、数学原理简单、响应速度快、实时性高等特点，是一种应用非常广泛的经典方法。此外，用人工势场法计算的路径具有良好的光滑性和连续性，在实时避障中得到了广泛的应用。但直接应用人工势场法进行无人机避障时，往往存在避障角度过大的问题。有研究人员通过优化势场作用域实现了小角度避障，但该方法只能进行平面避障。而且目前多数人工势场法进行仿真时，将障碍物视为质点或球形等简单几何图形，场景设置也较为简单，缺乏应用价值。同时，传统人工势场法还容易陷入局部最优解，同时存在障碍附近目标不可达(Goal Non
‑
reachable with Obstacles Nearby，GNRON)的问题。
[0003]参阅图1所示，局部最小值问题出现在无人飞行器1、障碍物2和目标位置3于同一条线上时。无人飞行器1向目标位置移动的过程中，一旦进入障碍物2的影响区域，就会被挡在一个局部最小的位置。在该位置上，吸引力与排斥力大小相等，方向相反，势场的合力为0，但无人飞行器1还未到达目标位置。
[0004]参阅图2所示，当目标位于障碍物2的影响范围内时，就会出现不能到达目标(Goal Non
‑
reachable with Obstacles Nearby，GNRON)的问题。产生这种问题的原因是，当无人飞行器1接近目标的同时，也会接近障碍物2。此时吸引势函数减小，而排斥势函数增大，使得目标位置并不是总势场的全局最小值位置，无人飞行器永远不可能成功到达目标。

技术实现思路

[0005]为了解决上述技术问题中的至少一个，本公开的第一目的在于提供了一种能够解决无人飞行器路径规划中的局部最小值问题的无人飞行器避障路径规划算法。
[0006]本公开的第二目的在于提供一种无人飞行器路径规划算法
[0007]为了实现本公开的目的，本公开所采用的技术方案如下：
[0008]一种无人飞行器避障路径规划算法，包括：
[0009]获取无人飞行器位置、障碍物位置、目标位置，当无人飞行器位于局部最小值位置时，对无人飞行器施加逃逸力，并重新初始化势场使无人飞行器离开局部最小位置；
[0010]其中，局部最小值位置是，人工势场法中无人飞行器的吸引力和排斥力在预设的误差范围内大小相等方向相反，且非目标位置的位置。
[0011]可选地，对无人飞行器施加逃逸力具体方法是，将三维空间的人工势场转化为二维空间的人工势场，再对二维空间的人工势场中的无人飞行器施加逃逸力。
[0012]可选地，对无人机施加逃逸力的大小和方向通过以下方程组计算；
[0013][0014][0015][0016][0017]其中，是无人飞行器处于q点位置施加的逃逸力，是无人飞行器处于q点位置逃逸力在x轴上的分量，是无人飞行器处于q点位置逃逸力在y轴上的分量，F
rep,i
是无人飞行器的排斥力，是从障碍物位置指向无人飞行器位置的单位向量，α是旋转角，x是无人飞行器位置的x轴坐标，y是无人飞行器位置的y轴坐标，x
goal
是目标位置的x轴坐标，y
goal
是目标位置的y轴坐标。
[0018]可选地，无人飞行器受到的总排斥力的大小和方向通过以下公式计算：
[0019][0020]其中，是无人飞行器处于q点位置的总排斥力。
[0021]可选地，当吸引力和排斥力满足以下公式时，判断吸引力和排斥力在误差范围内大小相等：
[0022][0023]其中F(q)表示吸引力与斥力的合力，F
rep,i
是无人飞行器的排斥力，ε表示预设的受力相同误差系数。
[0024]可选地，当吸引力和排斥力满足以下公式时，判断吸引力和排斥力在误差范围内方向相反：
[0025][0026]其中，F
att
表示无人飞行器吸引力，F
rep,i
表示无人飞行器的排斥力，θ表示预设的受力相反误差系数。
[0027]为了实现本公开的第二目的，本公开所采用的技术方案如下：
[0028]一种无人飞行器路径规划算法，包括：
[0029]获取无人飞行器的初始位置、目标位置，其中，初始位置作为无人飞行器位置；
[0030]路径规划：通过人工势场法计算无人飞行器位置至目标位置的飞行路径，当无人飞行器避障逃逸算法检测到无人飞行器位置是局部最小值位置，对飞行路径中的无人飞行器位置应用权利要求1所述的无人飞行器避障逃逸算法，使无人飞行器离开局部最小位置；当无人飞行器位于非目标位置时，继续执行路径规划步骤直至无人飞行器位于目标位置。
[0031]可选地，通过人工势场法计算无人飞行器的路径时，吸引势函数为：
[0032][0033]其中，U
att
(q)表示无人飞行器处于q点位置时的吸引势，ζ表示引力增益，d(q,q
goal
)表示无人飞行器位置与目标位置之间的距离，D表示障碍物位置与目标位置的最小距离。
[0034]可选地，通过人工势场法计算无人飞行器的路径时，吸引力函数为：
[0035][0036]其中，F
att
(q)表示无人飞行器处于q点位置时的吸引势。
[0037]可选地，通过人工势场法计算无人飞行器的路径时，吸引势函数为：
[0038][0039]其中，U
att
(q)表示无人飞行器处于q点位置时的吸引势，ζ表示引力增益，d(q,q
goal
)表示无人飞行器位置与目标位置之间的距离，D表示障碍物位置与目标位置的最小距离，其中K是预设的距离系数。
[0040]本公开中，针对局部最小值问题，通过检测无人飞行器是否处于局部最小值位置，设置逃逸力解决局部最小值问题，每次检测到局部最小问题时，都会激活逃逸力，也就是将外部排斥力作为摆脱局部最小值的解决方案。
附图说明
[0041]附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
[0042]图1是本公开中无人飞行器处于局部最小值位置时的位置示意图；
[0043]图2是本公开中无人飞行器处于不能到达目标时的位置示意图；
[0044]图3是本公开中无人飞行器避障路径规划算法的方法流程图；
[0045]图4是本公开中优选实施例的飞行器路径规划算法的方法流程图。
具体实施方式
[0046]下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种无人飞行器避障路径规划算法，其特征在于，包括：获取无人飞行器位置、障碍物位置、目标位置，当无人飞行器位于局部最小值位置时，对无人飞行器施加逃逸力，并重新初始化势场使无人飞行器离开局部最小位置；其中，局部最小值位置是，人工势场法中无人飞行器的吸引力和排斥力在预设的误差范围内大小相等方向相反，且非目标位置的位置。2.如权利要求1所述的无人飞行器避障路径规划算法，其特征在于：对无人飞行器施加逃逸力具体方法是，将三维空间的人工势场转化为二维空间的人工势场，再对二维空间的人工势场中的无人飞行器施加逃逸力。3.如权利要求1所述的无人飞行器避障路径规划算法，其特征在于，对无人机施加逃逸力的大小和方向通过以下方程组计算；方程组计算；方程组计算；方程组计算；其中，是无人飞行器处于q点位置施加的逃逸力，是无人飞行器处于q点位置逃逸力在x轴上的分量，是无人飞行器处于q点位置逃逸力在y轴上的分量，F
rep,i
是无人飞行器的排斥力，是从障碍物位置指向无人飞行器位置的单位向量，α是旋转角，x是无人飞行器位置的x轴坐标，y是无人飞行器位置的y轴坐标，x
goal
是目标位置的x轴坐标，y
goal
是目标位置的y轴坐标。4.如权利要求3所述的无人飞行器避障路径规划算法，其特征在于：无人飞行器受到的总排斥力的大小和方向通过以下公式计算：其中，是无人飞行器处于q点位置的总排斥力。5.如权利要求1所述的无人飞行器避障路径规划算法，其特征在于，当吸引力和排斥力满足以下公式时，判断吸引力和排斥力在误差范围内大小相等：其中F(q)表示吸引力与斥力的合力，F
rep,i
是无人飞行器的排斥力，ε表示预设的受力相同误差系数。6.如权利要求1所述的无人飞行器避障路径规划算法，其特征在于：当吸...

【专利技术属性】
技术研发人员：董韶鹏，吴泽炎，袁梅，赵龙飞，崔晋，童成彬，屈玉丰，
申请(专利权)人：北京航空航天大学宁波创新研究院，
类型：发明
国别省市：