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

本发明专利技术公开了一种基于位置与速度人工势场法的无人机路径规划方法，主要包括：步骤一是建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数；步骤二是建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数；步骤三是建立基于位置与速度人工势场函数的数学模型，求解出无人机动力学方程进行路径规划。与传统二维的基于位置的人工势场法相比不仅解决了二维空间的局限问题，而且引入速度信息提高无人机路径规划的可靠性。

A path planning method of UAV Based on artificial potential field of position and velocity

【技术实现步骤摘要】
一种基于位置与速度人工势场法的无人机路径规划方法
本专利技术属于无人机路径规划和无人机控制领域，具体涉及一种基于位置与速度人工势场法的无人机路径规划方法。
技术介绍
随着计算机技术与电子电路技术的发展，无人机从军事领域逐渐走入民用、商用领域，特别是应用于农业监测、民用航拍、灯光表演等场合。无人机作为一种新型的具备极高机动性的设备，在飞行的过程中需要对其运动轨迹进行路径规划，这是保障飞行器自身不被障碍物损坏与高效到达目标区域的重要措施。人工势场法因具有建模简单、路径规划速度快等优点被广泛用于进行无人机路径规划，目前在应用人工势场法对无人机进行路径规划时存在若干研究上的难题，主要包括两个方面，一是传统的人工势场法局限于二维空间也即平面上的运动，而无人机的运动遍历三维空间，如何将人工势场法扩展至三维空间并单独对其每个轴进行独立权重控制成为无人机路径规划的重要研究方向；二是传统的人工势场法局限于只采用位置信息构建势力函数而忽略速度信息，在障碍物与目标区域都在动态移动的场合难以保证路径规划的安全性与精确度。
技术实现思路
本专利技术的目的在于提供种一种基于位置与速度人工势场法的无人机路径规划方法，以解决上述
技术介绍
中提出的问题。为实现上述目的，本专利技术提供如下技术方案：种基于位置与速度人工势场法的无人机路径规划方法，包括:步骤一，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数；步骤二，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数；>步骤三，建立基于位置与速度人工势场函数的数学模型，求解出无人机动力学方程进行路径规划。进一步地，所述步骤一的建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数的过程，包括：位置侦测器接收无人机的三维空间位置，并测量各障碍物的三维空间位置，同时将所述无人机与所述各障碍物的三维空间位置传输至中央服务器，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数。进一步地，所述位置侦测器接收所述无人机的三维空间位置记为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.一种基于位置与速度人工势场法的无人机路径规划方法，其特征在于，包括：/n步骤一，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数；/n步骤二，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数；/n步骤三，建立基于位置与速度人工势场函数的数学模型，求解出无人机动力学方程进行路径规划。/n

【技术特征摘要】
1.一种基于位置与速度人工势场法的无人机路径规划方法，其特征在于，包括：
步骤一，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数；
步骤二，建立基于障碍物速度的动态斥力势函数和基于目标速度的动态引力势函数；
步骤三，建立基于位置与速度人工势场函数的数学模型，求解出无人机动力学方程进行路径规划。


2.根据权利要求1所述的基于位置与速度人工势场法的无人机路径规划方法,其特征在于，所述步骤一的建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数的过程，包括：位置侦测器接收无人机的三维空间位置，并测量各障碍物的三维空间位置，同时将所述无人机与所述各障碍物的三维空间位置传输至中央服务器，建立基于障碍物三维空间位置的斥力势函数和基于目标三维空间位置的引力势函数。


3.根据权利要求2所述的基于位置与速度人工势场法的无人机路径规划方法,其特征在于，所述位置侦测器接收所述无人机的三维空间位置记为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轴投影上的距离。


4.根据权利要求1所述的基于位置与速度人工势场法的无人机路径规划方法,其特征在于，所述步骤二的建立基于障...

【专利技术属性】
技术研发人员：王福杰，张佳宁，姚智伟，秦毅，李裕荣，任斌，
申请(专利权)人：东莞理工学院，
类型：发明
国别省市：广东;44