一种适用于高速环境的无人机动态路径规划方法

摘要

一种适用于高速环境的无人机路径规划方法，结合运动力学理论，建立了和速度相关的引力、斥力函数，并增加了虚拟阻力，抛弃了“势场”的概念，直接通过虚拟力函数计算无人机所受合力，通过仿真实验，验证了该方法在无人机高速运动时的有效性和安全性。本发明有益效果：本发明方法无需生成无人机的运动路径，仅需计算无人机当前受到的期望合力，不仅降低了算法的复杂度，而且将期望加速度取代期望位置，作为控制系统的输入，从而减少一个闭环回路，简化系统复杂度，大大缩短稳定时间，解决了现有路径规划方法不适用于高速环境的问题。

说明书

技术领域

本发明涉及无人机路径规划领域，具体地说是一种适用于高速环境的无人机动态路径规划方法。

背景技术

路径规划是无人机领域的核心技术之一。目前应用于无人机领域的路径规划方法主要有可视图法、模糊逻辑、神经网络、遗传算法、蚁群算法、人工势场法等。在众多的路径规划算法中，人工势场法因其需要信息量少、容易实现、表达式简洁、数学分析简单、计算量小等诸多优点，被广泛研究与应用。人工势场法(简称APF)也存在明显的先天缺陷，那就是“目标不可达”和“局部极小点”问题。目前，通过人工势场法和其他方法的结合，已有多种途径可以解决该问题，例如：优化函数法、障碍物填平法、虚拟子目标法、快速拓展随机树法、沿墙走策略等。上述方法各有优劣，但基本上都能顺利的引导无人机避开障碍物，到达目标点。

人工势场法是上个世纪八十年代中后期由斯坦福大学教授Khatib最先提出的。这种方法的基本思想是在目标点位置构造一个引力场，在障碍物的周围构造一个斥力场，它们共同作用形成一个虚拟的人工势场，再通过求势场的负梯度值得出无人机受到的虚拟引力和斥力。令无人机沿着合力的作用方向运动，通过这种方式来寻找一条无碰撞的最优路径，这就是人工势场法的基本原理。但利用人工势场法进行路径规划仅适用于低速环境，这主要由三方面原因：一是处理器、传感器和动力系统等硬件性能的瓶颈；第二是由于人工势场法自身的理论缺陷。由于势场中的物体受到势场的作用力仅和物体的位置有关，和物体当前的运动状态无关，因此人工势场法无法根据无人机的运动状态调整引力和斥力的大小。第三是由于人工势场法以“期望位置”作为输出，设计相应的控制系统时需要位置控制和姿态控制两个闭环回路，系统设计复杂，稳定时间较长，不能及时得到期望的位置输出。

从理论层面上讲，由于势场中的物体受到势场的作用力仅和物体的位置有关，和物体当前的运动状态无关，因此人工势场法无法根据无人机的运动状态调整引力和斥力的大小。从引力函数和斥力函数的表达式中也可以看出，引力和斥力的大小都仅和无人机位置有关。

从应用层面上讲，人工势场法进行路径规划的步骤是：首先通过引力、斥力函数计算出当前无人机受到的引力和斥力，然后在合力的方向移动一个步进值，生成一个临时目标点，通过控制系统控制无人机运动到临时目标点，在进行下一轮的计算，无人机不断生成并运动到临时目标点，最终到达终点。低速环境下，无人机到达一个临时目标点后，可以在短时间内改变运动方向向着下一个临时目标点运动，因此可以近似的沿着理想的路径运动。但在高速环境下，由于动力系统提供的加速度有上限，控制系统也需要一定的稳定时间，因此无人机不能在短时间内改变运动方向，其实际运动路径会与理想路径有较大的偏差。

发明内容

本发明所要解决的技术问题是提供一种适用于高速环境的无人机动态路径规划方法，使无人机保持较高速度的状态下，在未知环境中自动躲避静止障碍物或速度较低(低于无人机的最高速度)的移动障碍物，到达预先设定的目标点，解决了目前其他路径规划方法存在的问题。

本发明为解决上述技术问题所采用的技术方案是：一种适用于高速环境的无人机动态路径规划方法，将无人机一次完整的路径规划分为三个阶段：一般情况下为一般阶段；无人机接近目标点后进入到达阶段；无人机陷入局部极小点后到脱困之前为脱困阶段；到达阶段和脱困阶段之外的均为一般阶段，完整的路径规划方法包括以下步骤：

(1)获取无人机当前位置和速度信息，包括无人机当前位置c、无人机指向障碍物的向量无人机当前的瞬时速度以及无人机速度上限v_max，计算无人机指向目标点的向量

(2)判断无人机当前所处的阶段为一般阶段、脱困阶段或到达阶段；

(3)根据无人机所处的阶段，运用虚拟力函数计算期望的合力值；

(4)通过控制系统控制动力系统输出期望的合力值作用于无人机，无人机受到合力后按照牛顿第二定律运动，由此得到无人机的动态路径。

本发明所述无人机进入到达阶段的条件为：无人机离开到达阶段达到目标点的条件为：其中ε₂、ε_v和ε_d是根据实际情况设置的阈值。

本发明所述无人机进入脱困阶段的条件为：所述无人机结束脱困阶段的条件为：其中，和分别表示t时刻和t-1时刻的速度，ε₁为根据实际情况设定的阈值，和分别表示t时刻和t-1时刻无人机距离目标点的距离。

本发明所述无人机处于一般阶段时用于计算期望合力的虚拟力函数为：

期望合力：

引力函数：

斥力函数：

其中，表示期望合力，表示引力，表示斥力，K_a为引力系数，表示由无人机当前位置指向目标点的单位向量，d_v表示斥力的影响范围，d_s为无人机的半径，表示无人机与障碍物的相对速度，静态障碍物环境下动态障碍物环境下α表示和的夹角，K_r表示斥力系数，表示无人机指向障碍物的单位向量。

本发明所述无人机处于到达阶段时用于计算期望合力的虚拟力函数为：

期望合力：

引力函数：

斥力函数：

虚拟阻力函数：

其中，表示期望合力，表示引力，表示斥力，表示虚拟阻力，k_f表示虚拟阻力系数，表示速度的单位向量。

本发明所述无人机处于脱困阶段时用于计算期望合力的虚拟力函数为：

期望合力：

束缚力函数：

斥力函数：

引力函数：

其中，表示期望合力，表示引力，表示束缚力，表示斥力，D_min是刚进入脱困阶段时无人机与障碍物的最小距离，K_a'是引力系数，是将顺时针旋转90°后的单位向量。

本发明的有益效果是：本发明结合运动力学理论，通过产生虚拟力引导无人机到达目标点，虚拟力不仅和无人机的位置有关也和运动状态密切相关，本发明方法无需生成无人机的运动路径，仅需计算无人机当前受到的期望合力，不仅降低了算法的复杂度，而且将期望加速度取代期望位置，作为控制系统的输入，从而减少一个闭环回路，简化系统复杂度，大大缩短稳定时间，解决了现有路径规划方法不适用于高速环境的问题。本发明通过建立新的斥力函数，减小了产生碰撞的风险；通过引入角度因子解决了“反弹”问题；通过引入速度因子使无人机可以顺利通过狭窄通道，并解决了“目标不可达”问题；通过在到达阶段中引入虚拟阻力解决了盘旋问题；通过仿真实验，验证了该方法在无人机高速运动时的有效性和安全性。

附图说明

图1为本发明路径规划方法的流程示意图；

图2为本发明仿真实验环境模型图；

图3、图4分别为APF规划与本发明规划方法下无人机正面接近障碍物的运动轨迹仿真图；

图5、图6分别为APF规划与本发明规划方法下无人机正面接近障碍物的受力分析图；

图7、图8分别为APF规划与本发明规划方法下无人机侧面接近障碍物的运动轨迹仿真图；

图9、图10分别为APF规划与本发明规划方法下无人机接近对称障碍物的运动轨迹仿真图；

图11、图12分别为APF规划与本发明规划方法下无人机接近凹型障碍物的运动轨迹仿真图；

图13、图14、图15分别为APF规划下K_r＝1100、K_r＝1200以及本发明规划方法下无人机接近目标点附近的障碍物的运动轨迹仿真图；

图16、图17分别为APF规划与本发明规划方法下无人机在综合环境下的运动轨迹仿真图；

图18、图19、图20、图21分别为本发明方法下时间t为0s、8.6s、14.5s和30.8s时无人机面对动态障碍物的运动轨迹仿真图。

具体实施方式

路径规划的具体步骤如下：

(1)设置目标点坐标c_g和计算参数，继续执行步骤(2)；

(2)通过传感器获取无人机当前位置c、速度无人机指向障碍物的向量继续执行步骤(3)；

(3)判断是否达到目标点，判断条件为：

其中，ε_v和ε_d是设置的阈值，即无人机到目标点的距离；如果满足判定条件视为到达终点，结束路径规划，如果不满足执行步骤(4)；

(4)判断是否进入脱困阶段，判断条件为：

和分别表示t时刻和t-1时刻的速度，ε₁是设置的阈值；如果满足判定条件，视为进入脱困阶段，执行步骤(9)，如果不满足，执行步骤(5)；

(5)判断是否进入到达阶段，判断条件为：

ε₂是设置的阈值，如果满足判定条件，视为进入到达阶段，执行步骤(6)，如果不满足，执行步骤(7)；

(6)根据到达阶段虚拟力函数计算期望合力：

期望合力：

引力函数：

斥力函数：

虚拟阻力函数：

其中，表示期望合力，表示引力，表示斥力，表示虚拟阻力，k_f表示虚拟阻力系数，表示速度的单位向量，执行步骤(8)；

(7)根据一般阶段虚拟力函数公式计算期望合力：

期望合力：

引力函数：

斥力函数：其中，表示期望合力，表示引力，表示斥力，K_a为引力系数，表示由无人机当前位置指向目标点的单位向量，d_v表示斥力场的影响范围，表示无人机与障碍物的相对速度，静态障碍物环境下动态障碍物环境下α表示和的夹角，K_r表示斥力系数，表示无人机指向障碍物的单位向量；继续执行步骤(8)；

其中，称斥力函数中为“距离因子”，记为D；称为“速度因子”，记为V；称max(0,cos(α))为角度因子，记为A；斥力公式可简写为：

(8)控制系统调整螺旋桨转速，使无人机受到的合力等于求出的期望合力，无人机在合理的作用下按照牛顿第二定律运动，间隔时间Δt后执行步骤(2)，Δt是设置的计算周期；

(9)根据脱困阶段虚拟力函数计算期望合力：

期望合力：

束缚力函数：

斥力函数：

引力函数：

其中，表示期望合力，表示引力，表示束缚力，表示斥力，D_min是刚进入脱困阶段时无人机与障碍物的最小距离，K_a'是引力系数，是将顺时针旋转90°后的单位向量；继续执行步骤(10)；

(10)无人机移动，过程类似步骤(8)；继续执行步骤(11)；

(11)获取数据，过程类似步骤(2)；继续执行步骤(12)；

(12)判断是否结束脱困条件，判断条件为：

和分别表示t时刻和t-1时刻无人机距离目标点的距离；如果满足判定条件，结束脱困阶段，执行步骤(5)，如果不满足，执行步骤(9)。仿真实验

利用MATLAB进行仿真实验，建立二维平面坐标系，以无人机起始位置作为坐标原点；一个单位距离对应实际环境中1m；将无人机外形简化为半径为d_s的圆，无人机的运动轨迹用曲线表示，障碍物简化为一系列点，用圆圈表示，目标点为一个点，用三角型表示，与无人机相关的坐标、距离计算均以无人机中心点为准，如图2所示：

建立了5个典型静态障碍物环境，1个综合静态障碍物环境和2个动态障碍物环境，分别采用人工势场法(简称APF)和本发明虚拟力算法(简称VF)进行仿真。两种方法均以期望合力为输出，再根据运动力学方程计算出运动路径，运动力学方程如下：

其中，表示t时刻加速度，m表示无人机的质量，表示t时刻速度变化量，Δt表示计算周期，表示t时刻速度，表示t时刻位移，表示t时刻无人机的坐标。

另外，考虑到无人机的运动性能，设定了一些边界条件：

即

即

F_max和V_max分别表示实际系统中可以获得的最大水平动力和最大速度，分别为和的单位向量。

最后以是否能够顺利到达和到达时间作为评价依据并进行对比。

仿真参数设置如表1所示：

表1仿真参数表

仿真一 无人机正面接近障碍物

两种方法下无人机正面接近障碍物的仿真运动轨迹图如图3、图4所示，两种方法下无人机的受力分析如图5、图6所示。由图3和图4可以看出，APF规划下产生了碰撞，而VF规划下顺利到达。从图5和图6中可以看出，无人机均在4.5s时探测到障碍物，并受到斥力影响；APF规划下，4.5s～4.9s期间斥力都很小，合力仍为正，从5s开始合力才为负值，开始进行制动；VF规划下，4.5s就开始进行制动。也就是说，APF比VF规划下晚了0.4秒进行制动，而在这0.4s中无人机前进了7.73m，以至于最终产生碰撞。

我们希望无人机探测到障碍物后可以尽早进行制动。易知，最易出现碰撞的情况是无人机以最高速度正面飞向障碍物。此时A＝1,V＝1，D>1，令Kr＝2Fmax，则

上式说明无人机刚探测到障碍物时就可以获得较大斥力，并以最大加速度制动，从而有效降低碰撞的风险。

仿真二 无人机侧面接近障碍物

两种方法下无人机侧面接近障碍物时的运动轨迹如图7、图8所示。从图7可以看出APF方法下，无人机接近障碍物后，并没有沿着障碍物边缘前进，而是被弹开，我们称这种现象为“反弹效应”；无人机接近目标点后，围绕目标点往复运动，我们称这种现象为“盘旋效应”。从图8可以看出VF很好的解决了这两个问题。

“反弹效应”实际上就是斥力做正功的共过程，而斥力做正功的条件是：力和速度夹角为锐角。从VF的斥力函数可以看出，斥力方向始终和n_o相反；取α∈(-π，π]，角度因子A的取值如下：

与成锐角时为零，因此不会出现反弹现象。

“盘旋效应”其实就是无人机的动能和势能不断转换的过程。由能量守恒定律可知，在没有阻力的情况下，无人机会不停的在目标点附近盘旋，永不停止。在实际系统中，无人机会受到一定阻力，其主要来自于空气阻力。当空气阻力将无人机的机械能(即动能和势能综合)消耗殆尽它才会停止运动。空气阻力公式可近似表示为以下形式：

其中k代表空气阻力系数。在实际系统中，无人机的空气阻力系数往往很小，通过空气阻力消耗机械能需要很长的时间，因此就出现了盘旋的情况。

VF中虚拟阻力的形式和空气阻力公式相同，但阻力系数较大，因此可以快速消耗机械能，从而避免出现盘旋的情况或是缩短盘旋的时间。

实际情况下APF均会出现盘旋效应，从而长时间不能到达目标点，在这一方面VF有明显的优势。为更好的比较APF和VF其他方面的的优劣，以下实验人为忽略了盘旋效应。

仿真三 无人机接近对称障碍物

两种方法下无人机接近对称障碍物时的运动轨迹仿真图如图9、图10所示。由图可以看出，APF规划下，无人机最终到达了目的地，但运动路程较远；而VF规划下，无人机直接从通道中通过会大量节省时间。

VF斥力函数中引入速度因子V使斥力随着速度的减小而减小，因此无人机可以缓慢的通过狭窄的通道。速度因子中增加一个是为了避免无人机速度趋近于零时斥力也为零从而产生碰撞。

仿真四 无人机接近凹型障碍物

两种方法下无人机接近凹形障碍物时的运动轨迹规划图如图11、图12所示。由图可以看出，APF和VF规划下均可顺利到达。APF的脱困过程采用期望位置作为输出，无人机运动速度较慢，脱困时间也较长。VF采用期望合力作为输出，无人机运动速度较快，脱困时间较短。从VF中GP的虚拟力函数中可以看出，当d_o>D_min时，方向指向障碍物，形成引力；当d_o<D_min时，方向背离障碍物，形成斥力，因此的作用是将无人机的运动轨迹束缚在和障碍物的距离D_min的平行轨道附近，束缚力的名称也因此而来。的方向始终沿垂直于斥力的方向，即平行于障碍物的方向，其作用是引导无人机沿着平行于障碍物的方向前进。在和的共同作用下，引导无人机完成沿墙走行为。

考虑到实际情况下，脱困过程可能是曲折的沿墙走，为防止误判，可以采用当判定条件连续出现n次才判定脱困过程结束。

仿真五 无人机接近目标点附近的障碍物

两种方法下无人机接近目标点附近的障碍物时的运动轨迹规划仿真图如图13、图14、图15所示。由图可以看出，APF规划下，目标点附近存在障碍物时很容易出现目标不可达，一般可以通过调整Kr解决该问题，但当障碍物离目标点特别近时则不能顺利到达，Kr较小会出现碰撞，如图13，Kr较大会出现目标不可达，如图14；VF规划下，当无人机接近目标点时，很小，在速度因子V的影响下也较小，从而避免了目标不可达情况。

仿真六 综合环境

两种方法下无人机在综合环境下的运动轨迹规划仿真图如图16、图17所示。

表2不同环境下两种方法到达目标点的时间及情况汇总表

仿真七 无人机接近动态障碍物

VF可以计算无人机与障碍物的相对速度与正相关，当障碍物朝着接近无人机的方向运动时增大，也相应增大，因此无人机可以根据障碍物的运动趋势相应的改变运动轨迹，以避免碰撞。

建立动态障碍物实验环境，障碍物A以a为圆心旋转，障碍物B沿平行于Y轴的方向，在y＝40和y＝80之间往复直线运动，如图18所示。图19和图20中，箭头表示障碍物当前的运动方向，可以看出无人机随着障碍物的运动，相应的改变运动轨迹，顺利躲避障碍物最终到达目标点，如图21所示。