CN111595343B - 一种基于定位误差校正的无人机航迹规划方法 - Google Patents

Abstract

本发明属于无人机航迹规划技术领域，公开了一种基于定位误差校正的无人机航迹规划方法，采用数学建模思想，最优化方法理论，针对现有技术存在的问题，将航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题。该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度。同时本发明采用A^*算法与Q‑learning方法来对模型进行求解与误差校正，可有效保证无人机飞行定位精度的同时，降低无人机的航迹长度，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

Description

一种基于定位误差校正的无人机航迹规划方法

技术领域

本发明属于无人机航迹规划技术领域，尤其涉及一种基于定位误差校正的无人机航迹规划方法。

背景技术

随着计算机、通信技术、自动化控制技术的进步，近年来无人机发展迅速。无人机具有适应性强，灵活便携等优势，在一定程度上可以替代有人作业，以更高的效率和更低的成本完成工作任务，对军用和民用领域都有重要影响。无人机的航迹规划就是根据任务目标搜索出一条从源点到目标点的飞行航迹，是无人机任务规划问题中的核心技术。

现有技术一提出了基于几何图形的航迹规划方法，在静态环境下解决了无人机行进中的初始点方向问题，该技术对环境地图信息进行几何建模从而规划路径，可获取较优的无人机航迹规划路线。但该方法没有考虑定位误差对无人机航迹规划的影响，在实际的无人机飞行过程中，定位误差是会逐渐累积的，一旦超过定位误差阈值则可能会导致无人机偏离预定轨迹，从而影响预期任务目标的实现。

现有技术二提出了一种基于动态切点调整的无人机航迹规划方法，通过对无人机航迹规划区域进行研究，构建校正点选择策略，并基于校正点选择策略，构建航迹规划方案，该技术考虑了定位误差对无人机航迹规划的影响，但该技术没有考虑到无人机的飞行环境可能随时间动态变化，虽然校正点在飞行前已经确定，但无人机在部分校正点进行误差校正时存在无法达到理想校正的情况，例如天气等不可控因素会导致无人机到达校正点也无法进行理想的误差校正。

综上所述，现有技术存在的问题是：

(1)现有技术一运用几何建模的手段对无人机航迹进行规划，在获取较优航迹的同时却没有考虑无人机定位存在误差的问题，而误差会逐步积累可能导致任务失败的情况发生。

(2)现有技术二通过设置校正点的方式来对无人机定位误差进行校正，但没有考虑到无人机的飞行环境可能随时间动态变化，校正点可能会由于外部因素失效，无法对定位误差进行校正。

解决上述技术问题的难度：现有技术一：该技术运用几何数学的手段对无人机航迹进行规划时需假设无人机时刻按预定路线进行飞行，而在飞行过程中产生定位误差是必然的，因此在规划航迹过程中需考虑定位误差所带来的影响。现有技术二：该技术假设校正点的校正概率为100％，并通过A*算法对航迹进行规划，但飞行环境可能随时间动态变化，而无人机航迹一旦确定，不论校正成功与否，均不能改变规划路径。因此校正次数过多会增加航迹长度，校正次数过少会超过定位误差阈值。解决上述技术问题的难度在于如何使航迹尽可能短的前提下校正次数尽可能少，且无人机飞行误差不超过定位误差阈值。

解决上述技术问题的意义：本发明设计了一种基于定位误差校正的无人机航迹规划方法，采用数学建模思想，最优化方法理论，针对现有技术存在的问题，将航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题。该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度。同时本发明采用A*算法与Q-learning方法来对模型进行求解与误差校正，可有效保证无人机飞行定位精度的同时，降低无人机的航迹长度，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

发明内容

针对现有技术存在的问题，本发明提供了一种基于定位误差校正的无人机航迹规划方法。

本发明是这样实现的，一种基于定位误差校正的无人机航迹规划方法，所述基于定位误差校正的无人机航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题。该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度。

进一步，所述基于定位误差校正的无人机航迹规划方法包括以下步骤：

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

第二步，基于无人机航迹规划区域，构建航迹规划模型；

第三步，基于航迹规划模型，构建航迹规划方案；

第四步，基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法。

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

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

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

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

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正；

步骤(1.6)，无人机在部分校正点进行误差校正时存在无法达到理想校正的情况，在该校正点将某个误差校正为0的概率为P，如果校正失败，则校正后的剩余误差为min(error，5)个单位，error为校正前误差；无人机到达该校正点时即可知道在该点处是否能够校正成功，但不论校正成功与否，均不能改变规划路径。

进一步，所述第二步基于无人机航迹规划区域，构建航迹规划模型，具体包括：

步骤(2.1)，构建航迹规划模型目标函数：

根据两点间距离公式，N_i点与N_j点间的直线距离

可由下式进行计算：

其中，N_i与N_j为所述第一步无人机航迹规划区域内的任意两点，

为点N_i的x、y、z轴的坐标，

为点N_j的x、y、z轴的坐标；

无人机由A点飞至B点，将经过n个校正点，具体而言，起始校正点为N_i，最后校正点为N_k，无人机所经过的校正点总数为n，因此点A到点B之间的航迹长度

是多个校正点之间的直线距离累加，可表示为：

基于所述步骤(1.6)可知，所述第一步航迹规划区域内的校正点可能无法实现理想校正，成功校正概率为P，将无人机航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题，因此其目标函数可表示为：

步骤(2.2)，构建航迹规划模型约束条件：

基于所述步骤(1.1)可知，无人机到达B点时的垂直/水平误差应满足以下条件：

基于所述步骤(1.3)可知，无人机在A点时的垂直/水平误差为：

基于所述步骤(1.1)、步骤(1.4)、步骤(1.5)可知，

与两点间垂直/水平误差之间的关系应满足以下条件：

其中，

为无人机到达校正点N_j后的垂直误差，

为无人机离开校正点N_i后的垂直误差，

为无人机到达校正点N_j后的水平误差，

为无人机离开校正点N_i后的水平误差；

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，经N_i校正后的垂直/水平误差为：

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，需满足以下关系才能进行垂直校正：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为垂直误差校正点时，经N_j校正后的垂直/水平误差为：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为水平误差校正点时，需满足以下关系才能进行水平校正：

基于所述步骤(1.6)可知，当N_i为垂直误差校正点，并且N_i＝1时，经N_i校正后的垂直/水平误差为：

当N_i为垂直误差校正点，并且N_i＝0时，经N_i校正后的垂直/水平误差为：

当N_j为垂直误差校正点时，并且N_j＝1时，经N_j校正后的垂直/水平误差为：

当N_j为垂直误差校正点时，并且N_j＝1时，经N_j校正后的垂直/水平误差为：

步骤(2.3)，航迹规划模型

基于步骤(2.1)、步骤(2.2)所述的目标函数与约束条件，可得本发明的航迹规划模型为：

进一步，所述第三步基于航迹规划模型，构建航迹规划方案，具体包括：

步骤(3.1)，数据预处理：

建立两数组，分别存放垂直校正点坐标以及水平校正点坐标，数组名称为Tag_0和Tag_1，Tag_0为水平校正点，Tag_1为垂直校正点；

步骤(3.2)，校正点选择策略：

采用垂直/水平交替校正方案来对无人机航迹进行校正；

步骤(3.3)，航迹规划方案：

基于A*算法进行航迹规划，航迹规划方案主要分为代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.3.1)，代价函数构建：

A*算法的代价函数可表示为：

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

其中，f(n)是从初始点经由节点n到目标点的代价函数，g(n)是在状态空间中从初始节点到n节点的实际代价，h(n)是从n到目标节点最佳路径的估计代价；

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

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

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

步骤(3.3.2)，校正点的范围构建：

根据构建的代价函数和校正点选择策略，每次选择的校正点都将会是数组Tag_0或Tag_1中所有的点；但根据约束条件，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此本发明可以通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.3.2.1)，确定搜索范围：

假设无人机从A点出发先选择垂直校正点，即数组Tag_1中所有的点都可作为备选点，但根据所建立的约束条件，无人机最远飞行距离D_Amax应满足以下条件：

若D_Amax＞α₂/δ即使找到垂直校正点也无法进行垂直校正，以点A为球心，以半径为α₂/δ做球，在球内的Tag_1点均可作为备选校正点，而在球外的Tag_1点则进行舍去；

步骤(3.3.2.2)，计算梯度方向：

在球内所有的点也非全是有效点，可通过计算梯度方向来判断该点是否为有效点；点N_C、N_D、N_E均为点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算

其中，参数

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.3.2.3)，预测未来轨迹：

采用垂直/水平交替校正的方案，但校正点位置是随机布置的，因此将有可能如下情况：

假设校正N_C为垂直校正点，无人机经点N_C垂直校正后根据A^*算法应选择水平校正点N_D进行水平校正，但在水平校正点N_D附近无可达的垂直校正点，故A^*算法无法成功规划轨迹，此时为保证航迹正常规划，应选择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.3.3)，循环体构建：

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

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

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

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

步骤(3.3.4)，终止条件构建：

根据约束条件可知，只要到达B点时的垂直/水平误差均小于误差阈值，即视为成功到达，因此在循环体部分选出校正点后，需计算该点是否能直接到达B点，若可以则终止循环，否则继续循环；

假设点N_N为水平校正点，点N_M为垂直校正点，飞行器由点N_M飞向点N_N，此时经点N_N校正后的飞行器误差为：

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

若

则允许直接飞至B点，完成循环，否则在N点继续循环，直至达到终止条件；

步骤(3.3.5)，优化方案比较：

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

1.先选择垂直校正点进行校正；

2.先选择水平校正点进行校正；

对模型求解后，将产生2种航迹规划方案，需对这2种方案进行比较，选择更符合目标函数的方案。

进一步，所述第四步基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法，具体包括：

采用Q-learning方法对所述第三步的航迹规划方案进行误差校正，构建一个Q值表Q_table，Q_table是一个二维矩阵，由状态s_g和动作a_g构成，如下所示：

其中，Re(s_g,a_g)为回报函数，s_g+1为下一个状态，a_g+1为下一个动作，

为折扣因子，

α为学习速率，

和α可自行按需设定；

动作a_g代表对航迹规划区域内的某一校正点的选择，即通过航迹规划方案选择的该校正点是否满足所述航迹规划模型中的约束条件，若满足则进行下一校正点的选择，若不满足则放弃该校正点，重新对校正点进行选择；采用Q-learning方法，可以使无人机由起点A按照航迹规划方案进行校正点选择，并通过Q_table进行误差校正生成一条由起点A→垂直/水平校正点→终点B的无人机航迹规划路径。

综上所述，本发明的优点及积极效果为：本发明采用采用数学建模思想，最优化方法理论，针对现有技术存在的问题，将航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题。该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度。同时本发明采用A*算法与Q-learning方法来对模型进行求解与误差校正，可有效保证无人机飞行定位精度的同时，降低无人机的航迹长度，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

本发明采用CPGMCM2019-Date1数据集进行航迹规划的效果图如图3所示。

附图说明

图1是本发明实施例提供的基于定位误差校正的无人机航迹规划方法的流程图。

图2是本发明实施例提供的航迹规划模型求解流程图。

图3是本发明实施例提供的采用CPGMCM2019-Date1数据集。

图4是本发明实施例提供的采用CPGMCM2019-Date1数据集进行航迹规划的效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

针对现有技术存在的问题，本发明提供了一种基于定位误差校正的无人机航迹规划方法，下面结合附图对本发明作详细的描述。

如图1所示，本发明实施例提供的基于定位误差校正的无人机航迹规划方法包括以下步骤：

S101：构建由1个出发点A、1个目的地B、R个水平校正点、L个垂直校正点组成的无人机航迹规划区域；

S102：基于无人机航迹规划区域，构建航迹规划模型；

S103：基于航迹规划模型，构建航迹规划方案；

S104：基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法；

下面结合具体实施例对本发明的技术方案作进一步的描述。

本发明实施例提供的基于定位误差校正的无人机航迹规划方法包括以下步骤：

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

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

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

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

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正；

步骤(1.6)，无人机在部分校正点进行误差校正时存在无法达到理想校正的情况，在该校正点将某个误差校正为0的概率为P，如果校正失败，则校正后的剩余误差为min(error，5)个单位，error为校正前误差；无人机到达该校正点时即可知道在该点处是否能够校正成功，但不论校正成功与否，均不能改变规划路径。

第二步，基于无人机航迹规划区域，构建航迹规划模型，具体地步骤如下：

步骤(2.1)，构建航迹规划模型目标函数：

根据两点间距离公式，N_i点与N_j点间的直线距离

可由下式进行计算：

其中，N_i与N_j为所述第一步无人机航迹规划区域内的任意两点，

为点N_i的x、y、z轴的坐标，

为点N_j的x、y、z轴的坐标；

无人机由A点飞至B点，将经过n个校正点，具体而言，起始校正点为N_i，最后校正点为N_k，无人机所经过的校正点总数为n，因此点A到点B之间的航迹长度

是多个校正点之间的直线距离累加，可表示为：

基于所述步骤(1.6)可知，所述第一步航迹规划区域内的校正点可能无法实现理想校正，成功校正概率为P，将无人机航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题，因此其目标函数可表示为：

步骤(2.2)，构建航迹规划模型约束条件：

基于所述步骤(1.1)可知，无人机到达B点时的垂直/水平误差应满足以下条件：

基于所述步骤(1.3)可知，无人机在A点时的垂直/水平误差为：

基于所述步骤(1.1)、步骤(1.4)、步骤(1.5)可知，

与两点间垂直/水平误差之间的关系应满足以下条件：

其中，

为无人机到达校正点N_j后的垂直误差，

为无人机离开校正点N_i后的垂直误差，

为无人机到达校正点N_j后的水平误差，

为无人机离开校正点N_i后的水平误差；

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，经N_i校正后的垂直/水平误差为：

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，需满足以下关系才能进行垂直校正：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为垂直误差校正点时，经N_j校正后的垂直/水平误差为：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为水平误差校正点时，需满足以下关系才能进行水平校正：

基于所述步骤(1.6)可知，当N_i为垂直误差校正点，并且N_i＝1时，经N_i校正后的垂直/水平误差为：

当N_i为垂直误差校正点，并且N_i＝0时，经N_i校正后的垂直/水平误差为：

当N_j为垂直误差校正点时，并且N_j＝1时，经N_j校正后的垂直/水平误差为：

当N_j为垂直误差校正点时，并且N_j＝1时，经N_j校正后的垂直/水平误差为：

步骤(2.3)，航迹规划模型

基于步骤(2.1)、步骤(2.2)所述的目标函数与约束条件，可得航迹规划模型为：

第三步，基于航迹规划模型，构建航迹规划方案，具体包括：

步骤(3.1)，数据预处理：

建立两数组，分别存放垂直校正点坐标以及水平校正点坐标，数组名称为Tag_0和Tag_1，Tag_0为水平校正点，Tag_1为垂直校正点；

步骤(3.2)，校正点选择策略：

采用垂直/水平交替校正方案来对无人机航迹进行校正；

步骤(3.3)，航迹规划方案：

基于A*算法进行航迹规划，航迹规划方案主要分为代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.3.1)，代价函数构建：

A*算法的代价函数可表示为：

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

其中，f(n)是从初始点经由节点n到目标点的代价函数，g(n)是在状态空间中从初始节点到n节点的实际代价，h(n)是从n到目标节点最佳路径的估计代价；

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

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

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

步骤(3.3.2)，校正点的范围构建：

根据构建的代价函数和校正点选择策略，每次选择的校正点都将会是数组Tag_0或Tag_1中所有的点；但根据约束条件，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此可以通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.3.2.1)，确定搜索范围：

假设无人机从A点出发先选择垂直校正点，即数组Tag_1中所有的点都可作为备选点，但根据所建立的约束条件，无人机最远飞行距离D_Amax应满足以下条件：

若D_Amax＞α₂/δ即使找到垂直校正点也无法进行垂直校正，以点A为球心，以半径为α₂/δ做球，在球内的Tag_1点均可作为备选校正点，而在球外的Tag_1点则进行舍去；

步骤(3.3.2.2)，计算梯度方向：

在球内所有的点也非全是有效点，可通过计算梯度方向来判断该点是否为有效点；点N_C、N_D、N_E均为点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算

其中，参数

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.3.2.3)，预测未来轨迹：

采用垂直/水平交替校正的方案，但校正点位置是随机布置的，因此将有可能如下情况：

假设校正N_C为垂直校正点，无人机经点N_C垂直校正后根据A^*算法应选择水平校正点N_D进行水平校正，但在水平校正点N_D附近无可达的垂直校正点，故A^*算法无法成功规划轨迹，此时为保证航迹正常规划，应选择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.3.3)，循环体构建：

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

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

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

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

步骤(3.3.4)，终止条件构建：

根据约束条件可知，只要到达B点时的垂直/水平误差均小于误差阈值，即视为成功到达，因此在循环体部分选出校正点后，需计算该点是否能直接到达B点，若可以则终止循环，否则继续循环；

假设点N_N为水平校正点，点N_M为垂直校正点，飞行器由点N_M飞向点N_N，此时经点N_N校正后的飞行器误差为：

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

若

则允许直接飞至B点，完成循环，否则在N点继续循环，直至达到终止条件；

步骤(3.3.5)，优化方案比较：

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

1.先选择垂直校正点进行校正；

2.先选择水平校正点进行校正；

对模型求解后，将产生2种航迹规划方案，需对这2种方案进行比较，选择更符合目标函数的方案。

第四步，基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法，具体地步骤如下：

采用Q-learning方法对所述第三步的航迹规划方案进行误差校正，构建一个Q值表Q_table，Q_table是一个二维矩阵，由状态s_g和动作a_g构成，如下所示：

其中，Re(s_g,a_g)为回报函数，s_g+1为下一个状态，a_g+1为下一个动作，

为折扣因子，

α为学习速率，

和α可自行按需设定；

动作a_g代表对航迹规划区域内的某一校正点的选择，即通过航迹规划方案选择的该校正点是否满足所述航迹规划模型中的约束条件，若满足则进行下一校正点的选择，若不满足则放弃该校正点，重新对校正点进行选择；采用Q-learning方法，可以使无人机由起点A按照航迹规划方案进行校正点选择，并通过Q_table进行误差校正生成一条由起点A→垂直/水平校正点→终点B的无人机航迹规划路径。

下面结合仿真对本发明的技术效果作详细的描述。

本实验对基于定位误差校正的无人机航迹规划方法进行仿真，验证本发明方法的航迹规划能力，航迹规划区域采用CPGMCM2019-Date1数据集。具体地参数如下：

α₁＝25,α₂＝15,β₁＝20,β₁＝25,θ＝30,δ＝0.001,P＝80％

采用CPGMCM2019-Date1数据集如图3所示，采用CPGMCM2019-Date1数据集进行航迹规划的效果图如图4所示；不考虑校正点失效的航迹规划路径如表1所示，考虑校正点失效的航迹规划路径如表2所示：

表1 CPGMCM2019-Date1数据集不考虑校正点失效的航迹规划路径

表2 CPGMCM2019-Date1数据集考虑校正点失效的航迹规划路径

其中，11表示垂直误差校正成功，01表示水平误差校正成功，12表示垂直误差校正不成功，02表示水平误差校正不成功。

综上，本发明实施例提供的基于定位误差校正的无人机航迹规划方法，采用数学建模思想，最优化方法理论，针对现有技术存在的问题，将航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题。该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度。同时本发明采用A*算法与Q-learning方法来对模型进行求解与误差校正，可有效保证无人机飞行定位精度的同时，降低无人机的航迹长度，相较于其他无人机航迹规划方法更能适用于实际场景应用，实现多约束条件下的无人机航迹快速规划。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于定位误差校正的无人机航迹规划方法，其特征在于，所述基于定位误差校正的无人机航迹规划方法将信息传输问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题，该优化问题考虑无人机飞行环境动态变化对航迹规划所带来的影响，并在保证定位误差得到有效校正的前提下优化航迹长度，同时采用A*算法与Q-learning方法来对模型进行求解与误差校正；

所述基于定位误差校正的无人机航迹规划方法包括以下步骤：

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

第二步，基于无人机航迹规划区域，构建航迹规划模型；

第三步，基于航迹规划模型，构建航迹规划方案；

第四步，基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法；

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

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

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

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

步骤(1.4)，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正；

步骤(1.5)，当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正；

步骤(1.6)，无人机在部分校正点进行误差校正时存在无法达到理想校正的情况，在该校正点将某个误差校正为0的概率为P，如果校正失败，则校正后的剩余误差为min(error，5)个单位，error为校正前误差；无人机到达该校正点时即可知道在该点处是否能够校正成功，但不论校正成功与否，均不能改变规划路径；

所述第二步基于无人机航迹规划区域，构建航迹规划模型具体包括：

步骤(2.1)，构建航迹规划模型目标函数：

根据两点间距离公式，N_i点与N_j点间的直线距离

可由下式进行计算：

其中，N_i与N_j为所述第一步无人机航迹规划区域内的任意两点，

为点N_i的x、y、z轴的坐标，

为点N_j的x、y、z轴的坐标；

无人机由A点飞至B点，将经过n个校正点，具体而言，起始校正点为N_i，最后校正点为N_k，无人机所经过的校正点总数为n，因此点A到点B之间的航迹长度

是多个校正点之间的直线距离累加，可表示为：

基于所述步骤(1.6)可知，所述第一步无人机航迹规划区域内的校正点可能无法实现理想校正，成功校正概率为P，将无人机航迹规划问题形式化为一个以航迹长度、校正次数为优化目标，定位误差阈值为约束条件的多目标优化问题，因此其目标函数可表示为：

其中，L为航迹长度；

步骤(2.2)，构建航迹规划模型约束条件：

基于所述步骤(1.1)可知，无人机到达B点时的垂直/水平误差应满足以下条件：

基于所述步骤(1.3)可知，无人机在A点时的垂直/水平误差为：

基于所述步骤(1.1)、步骤(1.4)、步骤(1.5)可知，

与两点间垂直/水平误差之间的关系应满足以下条件：

其中，

为无人机到达校正点N_j后的垂直误差，

为无人机离开校正点N_i后的垂直误差，

为无人机到达校正点N_j后的水平误差，

为无人机离开校正点N_i后的水平误差；

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，经N_i校正后的垂直/水平误差为：

基于所述步骤(1.4)可知，当N_i为垂直误差校正点时，需满足以下关系才能进行垂直校正：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为水平误差校正点时，经N_j校正后的垂直/水平误差为：

基于所述步骤(1.4)、步骤(1.5)可知，当N_j为水平误差校正点时，需满足以下关系才能进行水平校正：

当N_i为垂直误差校正点，并且N_i＝0时，经N_i校正后的垂直/水平误差为：

当N_j为水平误差校正点时，并且N_j＝1时，经N_j校正后的垂直/水平误差为：

步骤(2.3)，航迹规划模型

基于步骤(2.1)、步骤(2.2)所述的目标函数与约束条件，可得航迹规划模型为：

所述第三步基于航迹规划模型，构建航迹规划方案具体包括：

步骤(3.1)，数据预处理：

建立两数组，分别存放垂直校正点坐标以及水平校正点坐标，数组名称为Tag_0和Tag_1，Tag_0为水平校正点，Tag_1为垂直校正点；

步骤(3.2)，校正点选择策略：

采用垂直/水平交替校正方案来对无人机航迹进行校正；

步骤(3.3)，航迹规划方案：

基于A*算法进行航迹规划，航迹规划方案主要分为代价函数构建、校正点的范围构建、循环体构建、终止条件构建和优化方案比较五部分：

步骤(3.3.1)，代价函数构建：

A*算法的代价函数可表示为：

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

其中，f(n)是从初始点经由节点n到目标点的代价函数，g(n)为备选校正点到起点A的欧式距离，h(n)为备选校正点到终点B的欧式距离；假设起点A周围存在m个同类型校正点可作为备选，将这些校正点编号为N₁,...,N_m，根据代价函数表示形式可计算每个点的代价函数，可表示为：

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

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

步骤(3.3.2)，校正点的范围构建：

根据构建的代价函数和校正点选择策略，每次选择的校正点都将会是数组Tag_0或Tag_1中所有的点；但根据约束条件，无人机单次飞行且不进行航迹校正的飞行距离是有限的，因此可以通过确定搜索范围、计算梯度方向和预测未来轨迹来减少备选校正点个数；

步骤(3.3.2.1)，确定搜索范围：

假设无人机从A点出发先选择垂直校正点，即数组Tag_1中所有的点都可作为备选点，但根据所建立的约束条件，无人机最远飞行距离D_Amax应满足以下条件：

若D_Amax＞α₂/δ即使找到垂直校正点也无法进行垂直校正，以点A为球心，以半径为α₂/δ做球，在球内的Tag_1点均可作为备选校正点，而在球外的Tag_1点则进行舍去；

步骤(3.3.2.2)，计算梯度方向：

在球内所有的点也非全是有效点，可通过计算梯度方向来判断该点是否为有效点；点N_C、N_D、N_E均为点A的待选校正点，分别计算

与

之间梯度，此处梯度具体为向量夹角，若夹角＞90°则将该点舍去，夹角可采用如下的向量夹角公式进行计算

其中，参数

和

表示向量，具体指代为向量

与

即参数

可为向量

与

之一，参数

可为向量

与

之一，参数

和

不可相等，依次求解各向量之间夹角；

步骤(3.3.2.3)，预测未来轨迹：

采用垂直/水平交替校正的方案，但校正点位置是随机布置的，因此将有可能如下情况：

假设校正N_C为垂直校正点，无人机经点N_C垂直校正后根据A^*算法应选择水平校正点N_D进行水平校正，但在水平校正点N_D附近无可达的垂直校正点，故A^*算法无法成功规划轨迹，此时为保证航迹正常规划，应选择次优的水平校正点N_E，并将点N_D舍去；

步骤(3.3.3)，循环体构建：

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

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

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

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

步骤(3.3.4)，终止条件构建：

根据约束条件可知，只要到达B点时的垂直/水平误差均小于误差阈值，即视为成功到达，因此在循环体部分选出校正点后，需计算该点是否能直接到达B点，若可以则终止循环，否则继续循环；

假设点N_N为水平校正点，点N_M为垂直校正点，飞行器由点N_M飞向点N_N，此时经点N_N校正后的飞行器误差为：

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

若

则允许直接飞至B点，完成循环，否则在N点继续循环，直至达到终止条件；

步骤(3.3.5)，优化方案比较：

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

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

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

对模型求解后，将产生2种航迹规划方案，需对这2种方案进行比较，选择更符合目标函数的方案；

所述第四步基于航迹规划方案，构建基于定位误差校正的无人机航迹规划方法具体包括：

采用Q-learning方法对所述第三步的航迹规划方案进行误差校正，构建一个Q值表Q_table，Q_table是一个二维矩阵，由状态s_g和动作a_g构成，如下所示：

其中，Re(s_g,a_g)为回报函数，s_g+1为下一个状态，a_g+1为下一个动作，

为折扣因子，

α为学习速率，

和α可自行按需设定；

动作a_g代表对航迹规划区域内的某一校正点的选择，即通过航迹规划方案选择的该校正点是否满足所述航迹规划模型中的约束条件，若满足则进行下一校正点的选择，若不满足则放弃该校正点，重新对校正点进行选择；采用Q-learning方法，可以使无人机由起点A按照航迹规划方案进行校正点选择，并通过Q_table进行误差校正生成一条由起点A→垂直/水平校正点→终点B的无人机航迹规划路径。

Images