一种基于位置与速度人工势场法的无人机路径规划方法与流程

本发明属于无人机路径规划和无人机控制领域，具体涉及一种基于位置与速度人工势场法的无人机路径规划方法。

背景技术：

随着计算机技术与电子电路技术的发展，无人机从军事领域逐渐走入民用、商用领域，特别是应用于农业监测、民用航拍、灯光表演等场合。无人机作为一种新型的具备极高机动性的设备，在飞行的过程中需要对其运动轨迹进行路径规划，这是保障飞行器自身不被障碍物损坏与高效到达目标区域的重要措施。人工势场法因具有建模简单、路径规划速度快等优点被广泛用于进行无人机路径规划，目前在应用人工势场法对无人机进行路径规划时存在若干研究上的难题，主要包括两个方面，一是传统的人工势场法局限于二维空间也即平面上的运动，而无人机的运动遍历三维空间，如何将人工势场法扩展至三维空间并单独对其每个轴进行独立权重控制成为无人机路径规划的重要研究方向；二是传统的人工势场法局限于只采用位置信息构建势力函数而忽略速度信息，在障碍物与目标区域都在动态移动的场合难以保证路径规划的安全性与精确度。

技术实现要素：

本发明的目的在于提供种一种基于位置与速度人工势场法的无人机路径规划方法，以解决上述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：种基于位置与速度人工势场法的无人机路径规划方法，包括:

步骤一，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数；

步骤二，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数；

步骤三，建立基于位置与速度人工势场函数的数学模型，求解出无人机动力学方程进行路径规划。

进一步地，所述步骤一的建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数的过程，包括：位置侦测器接收无人机的三维空间位置，并测量各障碍物的三维空间位置，同时将所述无人机与所述各障碍物的三维空间位置传输至中央服务器，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数。

进一步地，所述位置侦测器接收所述无人机的三维空间位置记为q＝[x,y,z]^t，其中x,y,z表示所述无人机在三维空间oxyz的位置坐标，ox的方向为水平面内正东方向，oz的方向为垂直于地面指向正上方，oy的方向为在水平面内垂直于ox轴并满足右手法则；

所述位置侦测器测量所述各障碍物的三维空间位置记为qoj＝[xoj,yoj,zoj]^t，其中xoj,yoj,zoj表示第j个障碍物在三维空间oxyz的位置坐标，j＝1,...,n，其中n表示空间内障碍物的总个数；所述中央服务器存储所述无人机目标区域的三维空间位置qg＝[xg,yg,zg]^t，其中xg,yg,zg表示目标区域的中心点在三维空间oxyz的位置坐标；

所述无人机发送自身三维空间位置的时间间隔是50毫秒，所述位置侦测器的采样周期为50毫秒，所述中央服务器的采样周期为25毫秒；

基于第j个障碍物三维空间位置的斥力势函数的表达式为：

其中表示由所述第j个障碍物引起的斥力势函数，βp,xy为xy平面上的正增益斥力因子，βp,z为z轴上的正增益斥力因子，kr1和kr2表示斥力常数；表示所述无人机与所述第j个障碍物在xy投影平面内的单位向量，方向由所述无人机的xy投影中心指向障碍物的xy投影中心，表示所述无人机与所述第j个障碍物在z轴投影的单位向量，方向由所述无人机的z轴投影点指向障碍物的z轴投影点，表示所述无人机与所述第j个障碍物在xy投影平面内的距离，其中qxy＝[x,y]^t，表示所述无人机与所述第j个障碍物在z轴投影上的距离，表示所述第j个障碍物在xy投影平面内的位置影响距离，表示所述第j个障碍物在z轴上的位置影响距离；

基于障碍物三维空间位置的斥力势函数的表达式为：

其中

基于目标三维空间位置的引力势函数的表达式为：

其中αp,xy为xy平面上的正增益引力因子，αp,z为z轴上的正增益引力因子；ep,xy表示所述无人机与目标在xy投影平面内的单位向量，方向由目标的xy投影中心指向所述无人机的xy投影中心，ep,z表示所述无人机与目标在z轴投影的单位向量，方向由目标的z轴投影点指向所述无人机的z轴投影点，ρg,xy＝||qxy-qg,xy||表示所述无人机与目标在xy投影平面内的距离，其中qg,xy＝[xg,yg]^t，ρg,z＝||z-zg||表示所述无人机与目标在z轴投影上的距离。

进一步地，所述步骤二的建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数的过程，包括：速度侦测器接收所述无人机的速度，并测量各障碍物的速度，同时将所述无人机与所述各障碍物的速度传输至所述中央服务器，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数。

进一步地，所述速度侦测器接收所述无人机的速度记为测量所述各障碍物的速度记为所述中央服务器存储无人机目标的速度记为所述无人机发送自身速度的时间间隔是50毫秒，所述速度侦测器的采样周期为50毫秒；

基于所述第j个障碍物速度的动态斥力势函数的表达式为：

其中表示由所述第j个障碍物引起的动态斥力势函数，βv为速度斥力因子，表示所述无人机与所述第j个障碍物在三维空间内的单位向量，方向由所述无人机的中心指向障碍物的中心，表示所述无人机与所述第j个障碍物的相对速度大小，表示所述无人机与所述第j个障碍物在三维空间的距离，表示所述第j个障碍物在三维空间内的速度影响距离；

所述基于障碍物速度的动态斥力势函数的表达式为：

其中

所述基于目标速度的动态引力势函数的表达式为：

其中αv为速度引力因子，evg表示所述无人机与目标在三维空间内的单位向量，方向由目标的中心指向所述无人机的中心，ρvg＝||v-vg||表示所述无人机与目标的相对速度大小。

进一步地，所述步骤三的基于位置与速度人工势场函数的数学模型为：

λ(q,qo,qg,v,vo,vg)＝λrep,p(q,qo)+λat,p(q,qg)+λrep,v(q,qo,v,vo)+λat,v(q,qg,v,vg)

进一步地，求解所述无人机动力学方程进行路径规划包括：对所述无人机飞行环境进行建模、求解出所述无人机受到的虚拟合外力、求解所述无人机在受到虚拟合外力的作用下向目标飞行的正向动力学和判断所述无人机是否到达目标区域并判断所述目标区域是否停止移动。

进一步地，所述无人机受到的虚拟合外力为：

其中和分别表示函数队q和v进行负梯度操作，frep,p(q,v)表示由障碍物三维空间位置产生的斥力，fat,p(q,v)表示由目标三维空间位置产生的引力，frep,v(q,v)表示由障碍物速度产生的斥力，fat,v(q,v)表示由目标速度产生的引力。

进一步地，所述无人机在受到虚拟合外力的作用下向目标飞行的正向动力学，具体如下：

式中表示由虚拟合外力产生加速度，m表示所述无人机的对角权重矩阵，kf和kq表示正比例增益；解出无人机在虚拟合外力驱动后的下一个位置点和飞行速度信息。

进一步地，判断所述无人机到达所述目标区域且所述目标区域停止移动，所述无人机路径规划结束，所述无人机进行悬停操作；判断所述无人机未到达目标区域或目标区域正在移动，所述无人机重新对飞行环境进行建模，求解动力学方程进行路径规划。

与现有技术相比，本发明的有益效果是：本发明的基于位置与速度人工势场法的无人机路径规划方法，本发明中的位置与速度人工势场法算法的基本思想是将无人机在空间的运动看作是在障碍物的斥力与目标区域的引力所组成的虚拟合外力驱动下的空间运动，可以不断适应外界空间环境的变化来进行轨迹规划。该方法旨在突破传统的基于位置的二维人工势场函数法的局限性，引入速度信息与三维位置信息解决无人机轨迹规划以及障碍物躲避等问题，利用三维空间位置信息与速度信息对无人机进行高精度路径规划与轨迹控制，同时为无人机队形保持、编队控制提供借鉴作用。

附图说明

图1为本发明基于位置与速度人工势场法的无人机路径规划方法结构图

图2为本发明设有3个移动障碍物的无人机飞行环境简化图；

图3为本发明基于位置与速度人工势场函数的构建流程图；

图4为本发明求解无人机正向动力学的流程图；

图5为本发明方法实施无人机路径规划的仿真示意图。

具体实施方式

下面结合实施例对本发明所提出的一种基于位置与速度人工势场法的无人机路径规划方法做进一步的描述。

以下实施例用于说明本发明，但不能用来限制本发明的保护范围。实施例中的条件可以根据具体条件做进一步的调整，在本发明的构思前提下对本发明的方法简单改进都属于本发明要求保护的范围。

请参阅图1所示，其为本发明基于位置与速度人工势场法的无人机路径规划方法的结构图，本发明实施例主要是根据位置采集器收集各无人机与障碍物的位置信息并传输至中央服务器，并根据速度收集器收集各无人机与障碍物的速度信息并传输至中央服务器，然后中央服务器根据接收到的信息建立基于位置与速度的三维人工势场函数的数学模型，最后通过中央服务器计算无人机受到的合力并发送至对应的无人机进行路径规划模块。

请参阅图2所示，三维空间环境中主要包括移动障碍物1、移动障碍物2、移动障碍物3、移动目标区域4、位置侦测器5、速度侦测器6和无人机7，无人机7的位置与速度信号由其自身的传感器测量，障碍物1、障碍物2和障碍物3的位置与速度由位置侦测器5和速度侦测器6测量，在进行无人机路径规划时要求无人机7能躲避移动的多个障碍物同时高效到达目标区域。

请参阅图3所示，建立无人机7基于位置与速度人工势场函数的过程分三步进行。

步骤一，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数。

具体而言，位置侦测器5接收无人机7的三维空间位置，并测量障碍物1、障碍物2和障碍物3的三维空间位置，同时将无人机7与障碍物1、障碍物2和障碍物3的三维空间位置传输至中央服务器，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数。

上述步骤一的具体操作过程是位置侦测器5接收无人机的三维空间位置记为q＝[x,y,z]^t，其中x,y,z表示无人机在三维空间oxyz的位置坐标，ox的方向为水平面内正东方向，oz的方向为垂直于地面指向正上方，oy的方向为在水平面内垂直于ox轴并满足右手法则；位置侦测器5测量各障碍物的三维空间位置记为qoj＝[xoj,yoj,zoj]^t，其中xoj,yoj,zoj表示第j个障碍物在三维空间oxyz的位置坐标，在本发明实施例中，j＝{1,2,3}。中央服务器存储无人机目标区域的三维空间位置qg＝[xg,yg,zg]^t，其中xg,yg,zg表示目标区域的中心点在三维空间oxyz的位置坐标。无人机7发送自身三维空间位置的时间间隔是50毫秒，位置侦测器5的采样周期为50毫秒，中央服务器的采样周期为25毫秒。

基于第j个障碍物三维空间位置的斥力势函数的表达式为：

其中表示由第j个障碍物引起的斥力势函数，βp,xy为xy平面上的正增益斥力因子，βp,z为z轴上的正增益斥力因子，kr1和kr2表示斥力常数；表示无人机7与第j个障碍物在xy投影平面内的单位向量，方向由无人机7的xy投影中心指向障碍物的xy投影中心，表示无人机7与第j个障碍物在z轴投影的单位向量，方向由无人机7的z轴投影点指向障碍物的z轴投影点，表示无人机7与第j个障碍物在xy投影平面内的距离，其中qxy＝[x,y]^t，表示无人机7与第j个障碍物在z轴投影上的距离，表示第j个障碍物在xy投影平面内的位置影响距离，表示第j个障碍物在z轴上的位置影响距离。

基于障碍物三维空间位置的斥力势函数的表达式为：

其中

基于目标三维空间位置的引力势函数的表达式为：

其中αp,xy为xy平面上的正增益引力因子，αp,z为z轴上的正增益引力因子，ep,xy表示无人机7与目标在xy投影平面内的单位向量，方向由目标的xy投影中心指向无人机7的xy投影中心，ep,z表示无人机7与目标在z轴投影的单位向量，方向由目标的z轴投影点指向无人机7的z轴投影点，ρg,xy＝||qxy-qg,xy||表示无人机7与目标在xy投影平面内的距离，其中qg,xy＝[xg,yg]^t；ρg,z＝||z-zg||表示无人机7与目标在z轴投影上的距离。

步骤二，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数。

具体而言，上述步骤二的操作过程是速度侦测器6接收无人机7的速度，并测量障碍物1、障碍物2和障碍物3的速度，同时将无人机7与障碍物1、障碍物2和障碍物3的速度传输至中央服务器，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数。

速度侦测器6接收无人机7的速度记为测量各障碍物的速度记为中央服务器存储无人机目标的速度记为无人机7发送自身速度的时间间隔是50毫秒，速度侦测器6的采样周期为50毫秒。

基于第j个障碍物速度的动态斥力势函数的表达式为：

其中表示由第j个障碍物引起的动态斥力势函数，βv为速度斥力因子，表示无人机7与第j个障碍物在三维空间内的单位向量，方向由无人机7的中心指向障碍物的中心，表示无人机7与第j个障碍物的相对速度大小，表示无人机7与第j个障碍物在三维空间的距离，表示第j个障碍物在三维空间内的速度影响距离。

基于障碍物速度的动态斥力势函数的表达式为：

其中

基于目标速度的动态引力势函数的表达式为：

其中αv为速度引力因子，evg表示无人机7与目标在三维空间内的单位向量，方向由目标的中心指向无人机7的中心，ρvg＝||v-vg||表示无人机7与目标的相对速度大小。

步骤三，建立基于位置与速度人工势场函数的数学模型。

具体而言，基于位置与速度人工势场函数的数学模型为：

λ(q,qo,qg,v,vo,vg)＝λrep,p(q,qo)+λat,p(q,qg)+λrep,v(q,qo,v,vo)+λat,v(q,qg,v,vg)

请参阅图4所示，本实施例中，利用上述步骤三中基于位置与速度人工势场函数的数学模型，求解无人机动力学方程进行路径规划的具体步骤分五步进行，按操作顺序分别标记为步骤s31、步骤s32、步骤s33、步骤s34、步骤s35。

步骤s31是对无人机飞行环境进行建模，包括目标区域的定位和速度获取，以及障碍物个数的确定。

步骤s32是根据基于位置与速度人工势场函数的数学模型，求解出无人机受到的虚拟合外力：

其中和分别表示函数队q和v进行负梯度操作，frep,p(q,v)表示由障碍物三维空间位置产生的斥力，fat,p(q,v)表示由目标三维空间位置产生的引力，frep,v(q,v)表示由障碍物速度产生的斥力，fat,v(q,v)表示由目标速度产生的引力。

步骤s33是求解无人机在受到虚拟合外力的作用下向目标飞行的正向动力学，具体如下：

其中，式中表示由虚拟合外力产生加速度，m表示无人机的对角权重矩阵，kf和kq表示正比例增益。在此步骤求解出无人机在虚拟合外力驱动后的下一个位置点和飞行速度信息。

步骤s34是判断无人机是否到达目标区域与目标区域是否停止移动，若已到达且目标区域停止移动则跳转至步骤s35；若未到达目标区域或目标区域正在移动，则跳转至步骤s31。

步骤s35是无人机路径规划结束，无人机进行悬停操作。

请参阅图5所示，在此仿真实验中，参数设定如下：kf＝20，kq＝10，βp,xy＝10，βp,z＝5，kr1＝2，kr2＝2，αp,xy＝15，αp,z＝8，βv＝5，αv＝5。对障碍物、无人机和目标区域做出以下具体抽象与简化：障碍物中心采用“五角星”表示，目标区域采用“菱形”表示，无人机中心采用“三角形”表示。

障碍物与目标区域的运动轨迹分别为：

从图5中的运动轨迹可知，采用本发明提出的方法可以有效躲闪移动的障碍物，无人机在运动的过程中能有效根据障碍物的移动速度修正自身运动轨迹并远离障碍物，同时无人机能高效到达目标区域，对于移动的目标区域也可实现跟踪控制。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。