[发明专利]一种基于动态切点调整的无人机航迹规划方法

权利要求书

1.一种基于动态切点调整的无人机航迹规划方法，其特征在于，所述一种基于动态切点调整的无人机航迹规划方法将航迹规划问题形式化为一个与定位误差校正次数、航迹长度和转弯半径相关的多目标优化问题，在保证定位误差得到有效校正的前提下优化航迹长度，并利用动态切点调整的方式来制定无人机的转弯策略；本方法可有效降低无人机航迹长度与算法复杂度并能符合无人机动力结构，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划；

所述一种基于动态切点调整的无人机航迹规划方法包括以下步骤：

第一步，构建由1个出发点A、1个目的地B、M个水平校正点、N个垂直校正点组成的无人机航迹规划区域；

第二步，基于无人机航迹规划区域，构建校正点选择策略；

第三步，基于校正点选择策略，构建航迹规划方案；

第四步，基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法；

所述第一步构建由1个出发点A、1个目的点B、R个水平校正点、L个垂直校正点组成的无人机航迹规划区域，具体包括：

步骤(1.1)，构建包含2+R+L点的无人机航迹规划区域，无人机在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差，无人机每飞行1m，垂直误差和水平误差将各增加δ个专用单位，以下简称单位，其中δ为无人机飞行1m，所各增加的垂直误差和水平误差，到达目的点时垂直误差和水平误差均应小于等于θ个单位，当垂直误差和水平误差均小于等于θ个单位时，无人机仍能够按照规划航迹飞行；

步骤(1.2)，无人机在飞行过程中需要对定位误差进行校正，航迹规划区域内存在校正点可用于误差校正，当无人机到达校正点即能够根据校正点的误差校正类型进行误差校正，校正垂直和水平误差的位置可根据地形在航迹规划前确定，若垂直误差、水平误差都能得到及时校正，则无人机可以按照预定航线飞行，并通过若干个校正点进行误差校正后最终到达目的地；

步骤(1.3)，无人机在出发点的垂直和水平误差均为0，在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变，在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正，其中α₁为无人机到达垂直校正点时所允许的最大垂直误差，α₂为无人机到达垂直校正点时所允许的最大水平误差；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正，其中β₁为无人机到达水平校正点时所允许的最大垂直误差，β₂为无人机到达水平校正点时所允许的最大水平误差；

步骤(1.6)，无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯，定义无人机的最小转弯半径为r_min＝200m；

所述第二步基于无人机航迹规划区域，构建校正点选择策略具体包括：

步骤(2.1)，经垂直/水平校正点交替校正后，无人机能按照规划路径飞行的最远距离D₁应满足以下关系：

应在该距离内寻找新的垂直校正点进行垂直校正，否则根据第一步约束条件无法按规划航迹飞行；

步骤(2.2)，经水平/垂直校正点交替校正后，无人机能按照规划路径飞行的最远距离D₂应满足以下关系：

应在该距离内寻找新的水平校正点进行水平校正，否则根据第一步约束条件无法按规划航迹飞行；

步骤(2.3)，若经垂直校正点校正后，无人机不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₃应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行垂直校正；

步骤(2.4)，若经水平校正点校正后，无人机不寻找垂直校正点进行校正，仍寻找水平校正点进行校正，那么无人机能按照规划路径飞行的最远距离D₄应满足以下关系：

若飞行超过该距离未进行校正，则在未来将无法进行水平校正；

步骤(2.5)，综上所述，无人机选择垂直/水平交替校正方案可在飞行相同距离的条件下，取得最少校正次数，并且该方案在选择下一校正点时，仅需对不同组别的点进行计算即可，无需计算同组别的校正点，因此可以降低算法计算量，减少搜索次数；

所述第三步基于校正点选择策略，构建航迹规划方案具体包括代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.1)，采用改进的A*算法进行航迹规划，首先构建代价函数，代价函数可表示为：

f(n)＝g(n)+h(n) (5)

其中，g(n)为备选校正点到出发点A的欧式距离，h(n)为备选校正点到目的点B的欧式距离，假设起点A周围存在m个同类型校正点可作为备选，将这些校正点编号为N₁,...,N_m，根据式(5)可计算每个点的代价函数，可表示为：

其中，为起点A到校正点N₁之间的欧式距离，为校正点N₁到目的点B之间的欧式距离，为起点A到校正点N_m之间的欧式距离，为校正点N_m到目的点B之间的欧式距离；

根据式(6)的计算结果，可选出最优的校正点N_i，选择方案如下所示：

f(N_i)＝min{f(N₁),...,f(N_m)}i∈(1,m) (7)

步骤(3.2)，基于构建的代价函数和校正点选择策略，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此本方法通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.2.1)，从出发点A出发先选择垂直校正点，无人机最远飞行距离D_Amax应满足以下条件：

D_Amax≤D₃ (8)

其中，D₃为无人机经垂直校正点校正后，不寻找水平校正点进行校正，仍寻找垂直校正点进行校正，能按照规划路径飞行的最远距离；

若D_Amax＞D₃，即使找到垂直校正点也无法进行垂直校正，因此以出发点A为球心，以半径为D₃做球，在球内所有水平校正点可作为备选校正点，则在球外的校正点则舍去；

步骤(3.2.2)，在球内所有的点也非全是有效点，通过计算梯度方向来判断该点是否为有效点，点N_C、N_D、N_E均为出发点A的待选校正点，分别计算与之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算：

其中，ω为向量夹角，和表示向量，具体指代为向量与即参数可为向量与之一，参数可为向量与之一，参数和不可相等，依次求解各向量之间夹角；

步骤(3.2.3)，采用垂直/水平交替校正的方案，但数据集中的校正点位置是随机布置的，若校正点N_C为垂直校正点，无人机经点N_C垂直校正后根据本方法应选择水平校正点，但若水平校正点附近无可达的垂直校正点时，应选择择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.2.4)，通过确定搜索范围、计算梯度方向和预测未来轨迹可以减少备选校正点的个数，从而在下一步循环体构建中减少搜索次数；

步骤(3.3)，对目标函数和校正点的范围完成构建后，对循环体予以构建，假设无人机从出发点A出发后先选择的校正点为垂直校正点N_C，无人机到达N_C时的误差为：

其中，为无人机到达校正点N_C后的垂直误差，为无人机到达校正点N_C后的水平误差，为起点A到校正点N_C之间的欧式距离；

经点N_C进行垂直校正后，无人机的垂直/水平误差为：

其中，为无人机离开校正点N_C后的垂直误差，为无人机离开校正点N_C后的水平误差；

之后将选择水平校正点进行水平校正，待选的水平校正点处于以点N_C为球心，半径为r′球内，r′满足以下约束条件：

再根据构建的校正点范围和目标函数，即可选出最优的下一校正点；

步骤(3.4)，因采用循环体结构，需对终止条件予以构建，由第一步可知当无人机飞到目的点B时的垂直/水平误差均小于预设值θ，因此在循环体部分选出校正点后，需计算该点是否能直接到达目的点B，若可以则终止循环，否则继续循环；

设点N_N为水平校正点，点N_M为垂直校正点，无人机由点N_M飞向点N_N，此时经点N_N校正后的无人机误差为：

其中，为无人机离开校正点N_N后的垂直误差，为无人机离开校正点N_N后的水平误差；

若无人机由点N_N直接飞至目的点B，则飞至目的点B时的误差为：

其中，为校正点N_M与校正点N_N之间的欧式距离，为校正点N_N与目的点B之间的欧式距离，为无人机到达目的点B的水平误差，为无人机到达目的点B的垂直误差；

若则允许直接飞至目的点B，完成循环，否则在校正点N_N继续循环，直至达到终止条件；

步骤(3.5)，由于采用垂直/水平交替校正的方案，因此从出发点A出发的情况将有2种，分别是：

(a)先选择垂直校正点进行校正；

(b)先选择水平校正点进行校正；

因此将产生2种航迹规划方案，需对这2种方案进行比较，选择航迹更短的方案；

所述第四步基于航迹规划方案，构建基于动态切点调整的无人机航迹规划方法具体包括起点飞向校正点、校正点飞向校正点、校正点飞向终点三部分：

步骤(4.1)，由于无人机的初始飞行方向可自行设定，因此本方法使无人机的飞行方向dir_A与出发点A，校正点N_a构成的向量方向一致即

步骤(4.2)，无人机经过校正点N_a后与出发点A、校正点N_a构成的向量方向应具有如下关系：

以N_a为切点做圆圆半径为，无人机沿圆弧飞行，飞行的圆心角ξ应满足如下关系：

根据圆心角可计算无人机在N_a点沿圆弧飞行的航迹长度为：

之后在N′_a点飞出，飞向方向应与校正点N_a和校正点N_b构成的向量方向一致，即再沿方向飞行l长度，l可由下式进行计算：

此时无人机的位置为点N′_b，以点N′_b为切点做圆圆半径仍为无人机沿圆飞行即可到达下一校正点N_b；

因此点N_a飞向点N_b采用航迹平滑方案所航行的航迹长度与之间的关系如下所示：

在式(19)中，中ξ为弧度，中ξ为角度；

在上式中是一个定值，为点N_a与N_b之间的欧式距离，圆心角ξ也是一个定值，由与决定，因此若要使航迹最短，即使最小；因此，无人机按最小转弯半径进行转弯，即满足以下关系：

其中，为无人机在校正点N_i时的转弯半径；

无人机此时离开点N_a时的飞向方向与校正点N_a和校正点N_b构成的向量垂直；

步骤(4.3)，在圆上重新寻找一个切点N″_m使之满足以下条件：

其中O_m为圆的圆心，为圆的圆心O_m与切点N″_m所组成的向量；

由点N_m″直接飞向目的点B，到达终点；该段航迹长度可表示为：

其中，为切点N″_m与目的点B之间的欧式距离，φ为无人机在圆处转过的圆心角，可由下式进行计算：

其中，为圆的圆心O_m与无人机经过的最后一个校正点N_m所组成的向量；

因此规划后的航迹长度可由式(24)进行表示；在式(24)中，点N_a为第一个校正点，点N_m为最后一个校正点，无人机共经过k个校正点；

其中为起点A到校正点N_a之间的欧式距离，为校正点N_i与校正点N_j之间的欧式距离。