一种定位误差约束下的飞行器航迹快速规划方法与流程

本发明属航空规划设计中的智能飞行器控制领域，具体涉及一种定位误差约束下的飞行器航迹快速规划方法。

背景技术：

飞行器航迹规划的实质就是在一定的环境中，在特定的约束条件下，寻找一条从起始点到目的地满足某种性能指标的最优或次优飞行航迹。由于受到飞行到达时间、油耗和地形环境等因素影响，这类飞行器的定位系统无法对自身进行精准定位，一旦定位误差积累到一定程度可能导致任务失败。因此，在飞行过程中对定位误差进行校正是智能飞行器航迹规划中一项重要任务。

但是飞行器的误差校正过程中存在诸多约束：可校正的飞行区域分布位置依赖于地形，无统一规律；垂直误差校正点及水平误差校正点均只可校正一类误差，且要求误差不超过规定的可校正范围；飞行器前进方向不能突然改变等。本发明研究飞行器在复杂环境中的航迹快速规划问题，在满足各类约束的条件下，求取同时满足航迹长度尽可能小、经过校正区域进行校正的次数尽可能少这两个目标的航迹。

技术实现要素：

本发明所要解决的技术问题是：提供一种飞行器航迹快速规划方法，该方法在满足各类约束的条件下，快速求取同时满足航迹长度尽可能小、经过校正区域进行校正的次数尽可能少这两个目标的航迹。该飞行器航迹快速规划方法根据投影值大小对空间中散乱无序的校正点作有序化，并以此为依据减少每一次选取校正点时的候选点数量、尽可能减少航迹所经过的校正点数量、尽可能减少航迹的总长度。为复杂环境中多约束条件下智能飞行器航迹快速规划提供了依据。

为解决上述技术问题，本发明采用的一种定位误差约束下的飞行器航迹快速规划方法，该飞行器航迹快速规划方法包括以下步骤：

步骤(1)进行适当的简化假设，以为方便问题求解；

步骤(2)根据校正点与出发点所构成的向量在飞行器直线航迹上的投影值大小，给定每一校正点对应的“序”；

步骤(3)通过“禁止后退搜索”方法，控制飞行器飞行过程中经过的校正点的“序”依次增大，确保飞行器不经过多余校正点；

步骤(4)分析理想状况下飞行器的最大前进距离，并以此为依据划定每一步搜索校正点的最大前进距离；

步骤(5)在划定的最大距离范围内，筛选满足水平误差校正和垂直误差校正的校正点；

步骤(6)在所有满足条件的校正点中，选取“序”最大的k个点；

步骤(7)在所选k个点中，进一步选取使得飞行器航迹长度最短的点；

步骤(8)判断最后一个校正点与终点的距离是否可保证飞船沿规划轨迹飞行而不产生偏差。

进一步的，本发明中，步骤(1)中，所作的简化假设如下：

忽略飞行器的体积与形状，将其看做空间中的一个质点；当垂直误差和水平误差均小于θ个单位时，飞行器仍能够按照规划路径飞行；不考虑飞行器在航行过程中遇到的避障问题；不考虑飞机到达时间、油耗、空中威胁和地形环境等因素对轨迹规划的影响；假设航迹中校正点数量和航迹长度对航迹规划同样重要，即影响程度相同；假设飞行器可以即时转弯(即飞行器前进方向可以立即改变)。

飞行器每飞行1m，垂直误差和水平误差将各增加δ个专用单位(简称单位)；在出发地a点，飞行器的垂直和水平误差均为0；飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变；飞行器在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；当飞行器的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正；当飞行器的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正。

进一步的，本发明中，步骤(2)中，确定每一校正点“序”的方法如下：

给定飞行器的出发点a和目的地b，以及所有校正点ji(i＝1,2,3…,n)的在三维空间中的坐标(xi,yi,zi)。以a为起点，b为终点可以确定一个向量同样的，以a为起点，以任一校正点ji为终点可以确定向量以为基准，求(i＝1,2,3…,n)在上的投影值pi。

将投影值pi(i＝1,2,3…,n)按升序排列，此时pi的顺序即决定了ji的“序”：若pi是最小的，则校正点ji的“序”取为1，以此类推。为方便叙述，后文用ji+1指代序比ji大的校正点。

进一步的，本发明中，步骤(3)中，“禁止后退搜索”的方法如下：

飞行器从a点出发，依次经过一系列水平或垂直校正点，最终达到目的地b。为了确保总航迹最短，飞行器可能从当前校正点ji飞行至下一校正点ji+1的前提是下一校正点ji+1“更靠近”目的地b，即pi+1＞pi，或校正点ji+1的序大于校正点ji的序，可以理解为飞行器在向量方向上有所前进。

因此，从当前校正点搜索下一校正点时，仅需考虑“序”更大的点，防止搜索回溯，以避免飞行器在向量方向上的迂回，增加不必要的航迹长度。

进一步的，本发明中，步骤(4)中，每一步搜索校正点的最大前进距离计算方法如下：

首先讨论不考虑校正点地理位置的理想状况。

理想状况下，即飞行器在任意点都可以进行任意校正(水平或者垂直)，飞行器的定位校正是一个有规律的循环过程，该过程由α1，α2，β1，β2共同决定；而决定飞行器是否能够按照规划路径飞行的θ不小于上述4个阈值，在到达终点之前均无需考虑。因此理想状况下，飞行器在每两个相邻校正点之间可以飞行的最大距离有四种情况：α1-β1，α2，β1，β2-α2，而在每两个最近的同类校正点之间可以飞行的最大距离有两种情况：α1和β2。

理想状况下，由于飞行器可以在任意点处做任意校正，故直接可以得到最短航迹长是ab两点间的欧氏距离，最小校正点数量也可以通过循环规律得到。这两个结果可以作为检验最终结果是否正确的依据。计算步骤是：

步骤(41)循环数量＝ceil((ab间的欧氏距离-θ/δ)/一次循环的航行距离)；

步骤(42)基准校正点数量＝循环数量×2；

步骤(43)计算倒数第二个校正点与目的地b之间的距离，判断航行该距离所产生的误差是否小于θ。如果“是”，那么得到最小校正点数量＝基准校正点数量-1；如果“否”，那么得到最小校正点数量＝基准校正点数量。

以理想状况为基础，考虑实际状况。

实际状况下，由于校正点位置的限制，飞行器不能在任意点处进行校正。也就是说，即使飞行器没有达到不校正就无法继续按规划航迹飞行的极限，但在达到极限处没有相应的校正点，飞行器就必须在达到极限之前的校正点提前校正。所以，在实际状况下，飞行器在每两个相邻校正点之间、每两个最近的同类校正点之间可以飞行的最大距离一定不超过理想状况，可以用理想情况下的最大可飞行距离作为从当前点向前搜索校正点的最大前进步长。

需要分为以下三类进行讨论：

第一类：当前点为出发点a：由于在a点飞行器的垂直和水平误差均为0，飞行器最大可飞行α2/δm至垂直校正点进行垂直校正，或者飞行β1/δm至水平校正点进行水平校正。因为目前不能确定下一点是何种校正，所以飞行器从a出发最大可飞行的距离为max{α2,β1}/δm。

第二类：当前点为垂直校正点：当前点仅能校正垂直定位误差，所以上一个水平校正点之后累积的水平误差err2都不能得到校正。但是，在到达下一个水平校正点之前，总的水平误差不能超过β2，否则将再也无法进行水平校正。所以，要满足水平校正的要求，最远可以航行的距离为(β2-err2)/δm。

同时，若err2过小(极端情况为err2＝0，即退化为出发点a)，飞行器的最大可飞行的距离还要受到max{α2,β1}/δ的约束。

综合以上两种情况，取约束更紧的一种情况作为最大前进步长，即min{max{α2，β1},(β2-err2)}/δm。

第三类：当前点为水平校正点：与垂直校正点同理，为防止再也无法进行垂直校正，最远可以航行的距离为(α1-err1)/δm，同时也要受到max{α2,β1}/δ的约束。

综合以上两种情况，最大前进步长取值为min{max{α2,β1},(α1-err1)}/δm。

进一步的，本发明中，步骤(5)中，筛选满足水平误差校正和垂直误差校正约束的校正点的方法如下：

计算当前校正点ji与下一校正点ji+1之间的欧氏距离di+1，计算飞行器航行至ji+1的累积垂直误差和累积水平误差，分别为：err1+di+1/δ，err2+di+1/δ。校正点ji+1为垂直校正点时，若

err1+di+1/δ≤α1且err2+di+1/δ≤α2，则ji+1满足误差校正约束，予以保留；校正点ji+1为水平校正点时，若err1+di+1/δ≤β1且err2+di+1/δ≤β2，则ji+1满足误差校正约束，予以保留；否则在后续步骤中不进行考虑。

进一步的，本发明中，步骤(6)中，选取“序”最大的k个点的方法如下：

由于飞行器航迹所经过的校正点数量与步骤(4)中给定的最大前进距离直接相关，所以要保证每次前进距离尽量大，即每两个相邻校正点间的距离尽量的大。在所有符合约束的校正点中，“序”最大的点即为与当前校正点距离最大的点(向量方向上)，也是与b的距离相对更近的点(向量方向上)。

其中k的选取可以根据情况进行调整，初始值可以由先验知识确定。

进一步的，本发明中，步骤(7)中，选取使得飞行器航迹长度最短的校正点的方法如下：

在步骤(6)选取的k个校正点中，可能存在投影值很大，但严重偏离向量的点，若飞行器航行至该点校正，会造成航迹总长度s过长。因此，从当前校正点ji选取下一校正点ji+1时，还需保证两点间的欧氏距离di+1尽量小。因此，在k个投影最大的校正点中，选取di+1最小的校正点作为飞行器航迹中的校正点。

进一步的，本发明中，步骤(8)中，判断最后一个校正点与终点的距离是否可保证飞船沿规划轨迹飞行而不产生偏差的方法如下：

在航迹中已有校正点的数量达到步骤(4)中计算的最小校正点数量后，计算最后一个校正点与目的地b之间的欧氏距离db，并与(θ+max{err1,err2})/δ比较。若db≤(θ+max{err1,err，则飞行器在到达b点之前无需再进行校正；若db＞(θ+max{err1,er，返回步骤(4)，继续计算最大前进距离并寻找每一步中的最优校正点。

与现有技术相比，本发明的技术方案具有以下有益效果：

1)该技术方案搜索空间得到减小，本发明巧妙地利用“禁止后退搜索”及“控制前进步长”来减小搜索空间，开创性地使用“投影值最大的k个点”策略对航迹搜索树的扩展进行有效剪枝，从而大大提高了搜索效率，同时，搜索过程中，利用先验知识对k的取值进行调整，可以有效提高搜索过程的收敛性；

2)计算简单，空间几何关系仅需利用简单的线性代数知识解决，过程简洁，无需多次迭代，结果精确且成本较低；

3)针对航迹规划问题量身定制，本发明提供的方法并未使用任何现有的算法工具箱及已有技术框架，相较于以“遗传算法、蚁群算法”为典型的“概率随机搜索”模式，本文涉及的算法可以保证较高的计算效率与较快的时间速度，并不需要以牺牲时间维度的快速性为代价，避免了全局寻优的低效与高昂的计算复杂度。在以“快速轨迹规划”为目标的前提下，本发明的方法更具优越性。

附图说明

图1是本发明的流程图。

图2是本发明实施例中出发点a、目的地b及校正点的分布情况。

图3是本发明所述的“禁止后退搜索”和“控制前进步长”策略对搜索空间的影响。

图4是本发明实施例中出发点a、目的地b之间的航迹规划结果。

具体实施方式

下面结合说明书附图和实施例，对本发明的技术方案作进一步详细说明。

实施例：参见图1—图4，一种定位误差约束下的飞行器航迹快速规划方法，该飞行器航迹快速规划方法包括以下步骤：

步骤(1)进行适当的简化假设，以为方便问题求解；

步骤(2)根据校正点与出发点所构成的向量在飞行器直线航迹上的投影值大小，给定每一校正点对应的“序”；

步骤(3)通过“禁止后退搜索”方法，控制飞行器飞行过程中经过的校正点的“序”依次增大，确保飞行器不经过多余校正点；

步骤(4)分析理想状况下飞行器的最大前进距离，并以此为依据划定每一步搜索校正点的最大前进距离；

步骤(5)在划定的最大距离范围内，筛选满足水平误差校正和垂直误差校正的校正点；

步骤(6)在所有满足条件的校正点中，选取“序”最大的k个点；

步骤(7)在所选k个点中，进一步选取使得飞行器航迹长度最短的点；

步骤(8)判断最后一个校正点与终点的距离是否可保证飞船沿规划轨迹飞行而不产生偏差；具体如下：

(1)确定具体的简化假设参数。飞行器每飞行1m，垂直误差和水平误差将各增加0.001个专用单位；在出发地a点，飞行器的垂直和水平误差均为0；飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变；飞行器在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变；当飞行器的垂直误差不大于25个单位，水平误差不大于15个单位时才能进行垂直误差校正；当飞行器的垂直误差不大于20个单位，水平误差不大于25个单位时才能进行水平误差校正；当垂直误差和水平误差均小于30个单位时，飞行器仍能够按照规划路径飞行。空间中散乱分布的校正点见图1。

(2)确定每一校正点的“序”。给定飞行器的出发点a(0.00,50000.00,5000.00)和目的地b(100000.00,59652.34,5022.00)，以及所有校正点ji(i＝1,2,3…,n)的在三维空间中的坐标(xi,yi,zi)。以a为起点，b为终点可以确定向量为(100000.00,9652.34,22.00)；同样的，以a为起点，以任一校正点ji为终点可以确定向量以为基准，求(i＝1,2,3…,n)在上的投影值pi，并将投影值pi(i＝1,2,3…,n)按升序排列。此后以校正点的“序”作为索引。

(3)飞行器从a点出发，依次经过一系列水平或垂直校正点，最终达到目的地b。为了确保总航迹最短，飞行器可能从当前校正点ji飞行至下一校正点ji+1的前提是下一校正点ji+1“更靠近”目的地b，即pi+1＞pi，或校正点ji+1的序大于校正点ji的序，可以理解为飞行器在向量方向上有所前进。因此，从当前校正点搜索下一校正点时，采用“禁止后退搜索”方法，即仅考虑“序”更大的点，防止搜索回溯，以避免飞行器在向量方向上的迂回，增加不必要的航迹长度。

(4)计算每一步搜索校正点的最大前进距离。

首先讨论不考虑校正点地理位置的理想状况。理想状况下，即飞行器在任意点都可以进行任意校正(水平或者垂直)，飞行器的定位校正是一个有规律的循环过程，该过程由α1，α2，β1，β2共同决定；而决定飞行器是否能够按照规划路径飞行的θ不小于上述4个阈值，在到达终点之前均无需考虑。利用给定参数说明“有规律的循环过程”。

41)若先进行水平校正，校正过程下表所示，一个循环包括两次校正：航行20000米后进行水平校正，再航行5000米后进行垂直校正。

42)若先进行垂直校正，校正过程如下表所示，一个循环也包括两次校正：航行15000米后进行垂直校正，再航行10000米后进行水平校正。

通过分析可以发现，理想状况下，飞行器在每两个相邻校正点之间可以飞行的最大距离有四种情况：5，15，20，10，而在每两个最近的同类校正点之间可以飞行的最大距离有一种情况：25。

理想状况下，由于飞行器可以在任意点处做任意校正，故直接可以得到最短航迹长是ab两点间的欧氏距离，为100464.76米；最小校正点数量也可以通过循环规律得到，为5个。这两个结果可以作为检验最终结果是否正确的依据。

以理想状况为基础，考虑实际状况。实际状况下，由于校正点位置的限制，飞行器不能在任意点处进行校正。也就是说，即使飞行器没有达到不校正就无法继续按规划航迹飞行的极限，但在达到极限处没有相应的校正点，飞行器就必须在达到极限之前的校正点提前校正。所以，在实际状况下，飞行器在每两个相邻校正点之间、每两个最近的同类校正点之间可以飞行的最大距离一定不超过理想状况，可以用理想情况下的最大可飞行距离作为从当前点向前搜索校正点的最大前进步长。

需要分为以下三类进行讨论：

第一类：当前点为出发点a：由于在a点飞行器的垂直和水平误差均为0，飞行器最大可飞行15000m至垂直校正点进行垂直校正，或者飞行20000m至水平校正点进行水平校正。因为目前不能确定下一点是何种校正，所以飞行器从a出发最大可飞行的距离为20000m。

第二类：当前点为垂直校正点：当前点仅能校正垂直定位误差，所以上一个水平校正点之后累积的水平误差err2都不能得到校正。但是，在到达下一个水平校正点之前，总的水平误差不能超过25个单位，否则将再也无法进行水平校正。所以，要满足水平校正的要求，最远可以航行的距离为(25-err2)/0.001m。

同时，若err2过小(极端情况为err2＝0，即退化为出发点a)，飞行器的最大可飞行的距离还要受到20000m上限的约束。

综合以上两种情况，取约束更紧的一种情况作为最大前进步长，即min{20,(25-err2)}/0.001m。

第三类：当前点为水平校正点：与垂直校正点同理，为防止再也无法进行垂直校正，最远可以航行的距离为(25-err1)/0.001m，同时也要受到20000m上限的约束。

综合以上两种情况，最大前进步长取值为min{20,(25-err1)}/0.001m。减小后的搜索空间见图3。

(5)筛选满足水平误差校正和垂直误差校正约束的校正点。计算当前校正点ji与下一校正点ji+1之间的欧氏距离di+1，计算飞行器航行至ji+1的累积垂直误差和累积水平误差，分别为：err1+di+1/δ，err2+di+1/δ。校正点ji+1为垂直校正点时，若err1+di+1/δ≤α1且err2+di+1/δ≤α2，则ji+1满足误差校正约束，予以保留；校正点ji+1为水平校正点时，若err1+di+1/δ≤β1且err2+di+1/δ≤β2，则ji+1满足误差校正约束，予以保留；否则在后续步骤中不进行考虑。

(6)选取“序”最大的k个点。由于飞行器航迹所经过的校正点数量与步骤(4)中给定的最大前进距离直接相关，所以要保证每次前进距离尽量大，即每两个相邻校正点间的距离尽量的大。在所有符合约束的校正点中，“序”最大的点即为与当前校正点距离最大的点(向量方向上)，也是与b的距离相对更近的点(向量方向上)。其中k的值选为30。

(7)选取使得飞行器航迹长度最短的校正点。在步骤(6)选取的k个校正点中，可能存在投影值很大，但严重偏离向量的点，若飞行器航行至该点校正，会造成航迹总长度s过长。因此，从当前校正点ji选取下一校正点ji+1时，还需保证两点间的欧氏距离di+1尽量小。因此，在k个投影最大的校正点中，选取di+1最小的校正点作为飞行器航迹中的校正点。

(8)判断最后一个校正点与终点的距离是否可保证飞船沿规划轨迹飞行而不产生偏差。

在航迹中已有校正点的数量达到5个后，计算最后一个校正点与目的地b之间的欧氏距离db，并与(30+max{err1,err2})/0.001比较。若db≤(30+max{err1,err2})/0.001，则飞行器在到达b点之前无需再进行校正；若db＞(30+max{err1,err2})/0.001，返回步骤(4)，继续计算最大前进距离并寻找每一步中的最优校正点。搜索结束后，连接每一步中选取的最优校正点，即可得到最优航迹，见图4。

上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。