[发明专利]一种基于位置与速度人工势场法的无人机路径规划方法

权利要求书

1.一种基于位置与速度人工势场法的无人机路径规划方法，其特征在于，包括：

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

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

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

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

3.根据权利要求2所述的基于位置与速度人工势场法的无人机路径规划方法,其特征在于，所述位置侦测器接收所述无人机的三维空间位置记为q＝[x,y,z]^T，其中x,y,z表示所述无人机在三维空间O_xyz的位置坐标，O_x的方向为水平面内正东方向，O_z的方向为垂直于地面指向正上方，O_y的方向为在水平面内垂直于O_x轴并满足右手法则；

所述位置侦测器测量所述各障碍物的三维空间位置记为q_oj＝[x_oj,y_oj,z_oj]^T，其中x_oj,y_oj,z_oj表示第j个障碍物在三维空间O_xyz的位置坐标，j＝1,...,N，其中N表示空间内障碍物的总个数；所述中央服务器存储所述无人机目标区域的三维空间位置q_g＝[x_g,y_g,z_g]^T，其中x_g,y_g,z_g表示目标区域的中心点在三维空间O_xyz的位置坐标；

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

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

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

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

其中

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

其中α_p,xy为xy平面上的正增益引力因子，α_p,z为z轴上的正增益引力因子；E_p,xy表示所述无人机与目标在xy投影平面内的单位向量，方向由目标的xy投影中心指向所述无人机的xy投影中心，E_p,z表示所述无人机与目标在z轴投影的单位向量，方向由目标的z轴投影点指向所述无人机的z轴投影点，ρ_g,xy＝||q_xy-q_g,xy||表示所述无人机与目标在xy投影平面内的距离，其中q_g,xy＝[x_g,y_g]^T，ρ_g,z＝||z-z_g||表示所述无人机与目标在z轴投影上的距离。