基于误差校正点的无人机室内航迹规划装置及方法与流程

本发明属于无人机导航与控制技术领域，特别涉及一种基于误差校正点的无人机室内航迹规划装置及方法。

背景技术：

在室外开阔地带，由于gnss的存在，无人机导航问题较容易解决。但当无人机在某些密闭场合完成任务时，会出现gnss信号断点甚至无法被捕捉等情况。复杂未知环境下航迹快速规划是智能飞行器控制领域的一个重要课题，由于gnss的缺失，飞行器无法依据自身定位系统进行精准定位，一旦定位误差累积到一定程度就可能导致任务失败。因此，在飞行过程中对定位误差进行及时校正是智能飞行器航迹规划中的一项重要任务。

基于上述问题，有必要设计一种封闭环境的航迹规划方法。

技术实现要素：

针对在封闭空间，飞行器无法进行精确定位的问题，本发明提供一种基于误差校正点的无人机室内航迹规划装置及方法，解决智能飞行器在系统定位精度限制下的航迹快速规划问题。

本发明是通过以下技术方案来实现：

一种基于误差校正点的无人机室内航迹规划方法，在封闭空间中随机布置水平校正点和垂直校正点，并设定航迹约束条件，然后根据约束条件建立航迹单步约束和航迹双步约束获得阈值，根据阈值设置dijstra算法中的带权邻接矩阵并进行求解，得到最优航迹。

该基于误差校正点的无人机室内航迹规划方法，包括以下步骤：

步骤1、建立航迹约束条件；

步骤2、基于航迹约束条件建立航迹单步约束，单步约束为飞行器在第k段产生的水平和垂直误差的最大值；

步骤3、基于航迹约束条件，设定航迹规划路径交替经过垂直误差校正点和水平误差校正点，根据飞行器交替经过相邻两个校正点的过程建立双步约束，双步约束为相邻两段航迹的总垂直误差最大值，以及相邻两段航迹的总水平误差的最大值；

步骤4、根据dijstra算法的带权邻接矩阵r，并结合水平误差值和垂直误差值设定单步约束的阈值t1；

步骤5、根据单步约束和双步约束调整阈值t1，得到调整后的阈值t2；

步骤6、根据调整后阈值t2设定dijstra算法中带权邻接矩阵r中元素的值并进行解算，得到最优航迹。

优选的，步骤1所述航迹约束条件如下：

一、设飞行器出发地点的垂直和水平误差均为0；

二、飞行器在垂直误差校正点或水平误差校正点进行校正，校正后的垂直误差或水平误差变为0，未校正的垂直误差或水平误差保持不变；

三、当飞行器的垂直误差不大于α1，水平误差不大于α2时，进行垂直误差校正；

四、当飞行器的垂直误差不大于β1个单位，水平误差不大于β2个单位时，进行水平误差校正。

优选的，步骤2所述航迹单步约束的表达式如下：

v(k)<min{α1,β1,α2,β2},

l(k)<min{α1,β1,α2,β2}

其中，v(k)表示第k段航迹产生的垂直误差；l(k)表示第k段航迹产生的水平误差。

优选的，步骤3所述航迹双步约束的表达式如下：

l(k-1)+l(k)<min{α1,β1,α2,β2}+max{α2,β2}-min{α1,β1,α2,β2}

＝max{α2,β2},k＝2，3，…，n

v(k-1)+v(k)<min{α1,β1,α2,β2}+max{α1,β1}-min{α1,β1,α2,β2}

＝max{α1,β1},k＝3，4，…，n

优选的，步骤4所述阈值t1的表达式如下：

t1＝max{α1,α2,β1,β2}

优选的，步骤5所述调整后的阈值t2的表达式如下：

t2＝min{θ/2，t1}

其中，θ为飞行器到达终点时垂直误差和水平误差的最大误差值。

优选的，步骤6中设定带权邻接矩阵r中元素的方法如下：

当两校正点间的距离大于阈值t2时，将带权邻接矩阵r的(i,j)元素设置为inf；

当两校正点的距离小于阈值t2时，将带权邻接矩阵r的(i,j)元素设置为两点间的距离；

然后采用dijstra算法进行解算，得到最优校正点集，依次连接得到最优航迹。

一种航迹规划装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述基于误差校正点的无人机室内航迹规划方法的步骤。

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

本发明提供的基于误差校正点的无人机室内航迹规划方法，只需在空间中通过激光束相交，随机布置水平校正点和垂直校正点，通过分析单步约束和双步约束条件所获得的阈值约束规则，修改dijstra算法中的带权邻接矩阵，选取最优校正点集，从而实现智能飞行器在传统导航方法失能等复杂环境下的最优飞行路径规划。该方法约束条件少，不需拟定复杂预设航线，无人机即可根据空间中的预设校正点实现从起点到终点的飞行任务，解决智能飞行器在系统定位精度限制下的航迹快速规划问题。

附图说明

图1为本发明方法的流程图；

图2为本发明在封闭区域中起点、终点和随机分布校正点示意图；

图3为本发明单步约束条件示意图；

图4为本发明双步约束条件前提示意图；

图5为本发明数据集下最优航迹规划示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

参阅图1-5，一种基于误差校正点的无人机室内航迹规划方法，包括以下步骤：

步骤1、建立航迹约束条件，具体如下:

飞行器在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。飞行器每飞行1m，垂直误差和水平误差将各增加δ个专用单位,，以下简称单位。到达终点时垂直误差和水平误差均应小于θ个单位，假设当垂直误差和水平误差均小于θ个单位时，飞行器仍能够按照规划路径飞行。

飞行器在飞行过程中需要对定位误差进行校正。飞行区域中存在一些安全位置(称之为校正点)可用于误差校正，通过激光束相交，随机布置水平校正点和垂直校正点，当飞行器到达校正点即能够根据该位置的误差校正类型进行误差校正。若垂直误差、水平误差都能得到及时校正，则飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。

约束条件如下：

1、设飞行器下出发地a点的垂直和水平误差均为0。

2、飞行器在垂直误差校正点进行校正后，其垂直误差将变为0，水平误差保持不变。

3、飞行器在水平误差校正点进行校正后，其水平误差将变为0，垂直误差保持不变。

4当飞行器的垂直误差不大于α1个单位，水平误差不大于α2个单位时才能进行垂直误差校正。

5当飞行器的垂直误差不大于β1个单位，水平误差不大于β2个单位时才能进行水平误差校正。

6无人机类别不同，则α1α2β1β2具体数值不同，通过前期试验，α1α2β1β2经验值已知，空间中垂直校正点和水平校正点个数相等且已知，校正点分布方式为空间随机分布。

步骤2、参阅图3，基于上述约束条件建立航迹单步约束，单步约束即表示飞行器在第k段产生的水平和垂直误差都不能超过{α1,β1,α2,β2}的最小值，否则将会出现飞行器到达校正点前产生的误差已超出可校正范围。

单步约束表达式如下：

其中，v(k)表示第k段航迹产生的垂直误差；l(k)表示第k段航迹产生的水平误差。

步骤3、参阅图4，基于步骤1的约束条件，设定航迹规划路径交替经过垂直误差校正点和水平误差校正点，根据飞行器交替经过两个校正点的过程建立双步约束，双步约束表达式如下：

双步约束即表示相邻两段航迹的总垂直误差不能超过{α1,β1}的最大值，总水平误差不能超过{α2,β2}的最大值。

双步约束表达式可保证最终的航迹规划路径在顺序经过任意三个误差校正点时，所构成的两条路段距离之和小于一定量，从而确保智能飞行器能一直按照规划航迹飞行。

步骤4、根据dijstra算法的带权邻接矩阵r，并结合α和β设定单步约束的阈值t1；

t1＝max{α1,α2,β1,β2}(3)

其中，α为水平误差值，β为垂直误差值；

步骤5、根据单步约束和双步约束调整阈值t1，得到调整后的阈值t2。

双步约束的前提，即要求飞行器交替经过两种类型的误差校正点，可以通过将带权邻接矩阵r中的对应元素设为inf实现。

双步约束条件自身无法在算法中体现，由误差导向通过阈值调整而解决双步约束问题，阈值调整公式为：

t2＝min{θ/2，t1}(4)

步骤6、根据调整后阈值t2设定dijstra算法中带权邻接矩阵r中元素的值，并进行解算，得到最优航迹。

参阅图5，具体为：当(i,j)两校正点间的距离大于阈值t2时，将带权邻接矩阵r的(i,j)元素设置为inf，当(i,j)两校正点的距离小于阈值t2时，将带权邻接矩阵r的(i,j)元素设置为两点间的距离，之后采用dijstra算法进行解算，得到最优点集，依次连接即为最优航迹。

实施例1

如图2所示，{α1,α2,β1,β2}＝{25，15，20，25}，同时，其他参数为：空间中共计612个校正点，起点坐标(0，50000，5000)，终点坐标(100000，59652.34，5022)，θ＝30，δ＝0.001，以此参数集进行阐述：

智能飞行器在密闭空间中某起点处等待起飞，执行携带一定载荷降落至终点的任务。在三维空间中，起点终点间直线距离为1.0046e+05，但由于gnss信号丢失等条件限制，智能飞行器无法沿最短路线飞行。在飞行途中，会持续产生水平方向和垂直方向的误差，若不及时校正两个方向上的误差，可能会使飞机发生碰撞，具体碰撞标准如发明内容中航迹约束条件所述。

为解决此问题，在空间中随机布置306个垂直校正点，306个水平校正点，每个校正点都是由两条激光射线交汇形成，坐标已知。在飞行器飞行前，通过上述算法即可选取校正点集，在飞行器控制栈中预设校正点集坐标，当飞行器在飞行途中经过选取的水平校正点时，即可校正水平误差；经过选取的垂直校正点时，即可校正垂直误差，最终实现经过校正点数目最少，飞行距离最短的目标。

在示例性实施例中，还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述基于误差校正点的无人机室内航迹规划方法的步骤。其中，所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(mo)等)、光学存储器(例如cd、dvd、bd、hvd等)、以及半导体存储器(例如rom、eprom、eeprom、非易失性存储器(nandflash)、固态硬盘(ssd))等。

在示例性实施例中，还提供了一种航迹规划装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述基于误差校正点的无人机室内航迹规划方法的步骤。处理器可能是中央处理单元(centralprocessingunit，cpu)，还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现成可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。