CN110749325A - 航迹规划方法和装置 - Google Patents

Abstract

本公开提出一种航迹规划方法和装置，涉及航迹规划领域。在本公开中，利用不确定性因素变量来对不确定因素进行表示，基于该不确定性因素变量和航迹矩阵变量来构建航迹质量相关的代价函数，根据该代价函数构建航迹的目标函数，优化该目标函数，得到一组航迹矩阵变量值，将航迹矩阵变量值对应的航迹确定为规划的航迹。该方法用变量的形式表征不确定因素，对目标函数进行优化，使得规划的航迹更可靠、更安全，提高了航迹规划的准确性。

Description

航迹规划方法和装置

技术领域

本公开涉及航迹规划领域，特别涉及一种航迹规划方法和装置。

背景技术

无人机作为物流行业面向未来的重要技术储备，发挥着越来越重要的作用，无人机航迹规划环节负责为无人机生成航行的局部路径，局部路径对于航行的安全性起到关键作用。在航迹规划问题中，如何处理环境中的不确定性因素是一个重要挑战。

在一些相关技术中，将不确定因素看待为确定情况，例如进行滤波去噪等方法将不确定因素用确定值进行表征，往往将使得问题的定义出现一定的偏差，以此方法来规划航迹，由此将会导致规划的航迹与实际可行的航迹出现一定的偏差，航迹的安全性和准确性都得不到保障。

发明内容

发明人发现，在相关技术中，在航迹规划过程中，将不确定因素看待为确定情况，会导致规划的航迹与实际可行的航迹出现一定的偏差，航迹的安全性和准确性都得不到保障。

在本公开中，利用变量来对不确定因素进行表示，基于不确定性因素变量和航迹矩阵变量来构建航迹质量相关的代价函数，根据该代价函数构建航迹的目标函数，优化该目标函数，得到一组航迹矩阵变量值，将航迹矩阵变量值对应的航迹确定为规划的航迹。本公开用变量的形式表征不确定因素，对目标函数进行优化，使得规划的航迹更可靠、更安全，提高了航迹规划的准确性。

根据本公开的一些实施例，提供一种航迹规划方法，其特征在于，包括：

利用航迹矩阵变量和不确定性因素变量构建航迹质量相关的代价函数；

根据航迹质量相关的代价函数构建航迹的目标函数；

在满足预设约束条件的情况下，确定一组航迹矩阵和不确定性因素取值，使得所述目标函数的值最小，并将确定的航迹矩阵相应的航迹确定为规划的航迹。

在一些实施例中，所述利用航迹矩阵变量和不确定性因素变量构建航迹质量相关的代价函数包括：根据航迹矩阵变量构建确定性因素相关的函数；根据航迹矩阵变量和不确定性因素变量构建不确定性因素相关的函数；对确定性因素相关的函数和不确定性因素相关的函数进行加权求和得到航迹质量相关的代价函数。

在一些实施例中，所述根据航迹质量相关的代价函数构建航迹的目标函数包括：构建基于代价函数表示的航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数；基于航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数构建航迹的目标函数。

在一些实施例中，所述不确定性因素变量包括威胁因素变量。

在一些实施例中，所述确定性因素相关的函数包括航迹长度函数、航迹高度函数、航迹平滑性函数和碰撞函数中的一种或多种；所述不确定性因素相关的函数包括威胁函数。

在一些实施例中，所述航迹长度函数是根据航迹的起始位置和终止位置之间的线段长度和航迹的实际折线长度确定的；所述航迹高度函数是根据最大航迹高度、最小航迹高度和航迹的平均海拔高度确定的；所述航迹平滑性函数是根据航迹的实际折线中的相邻折线段之间的夹角确定的；所述碰撞函数是根据航迹的实际折线长度和航迹中位于地表以下的折线长度的总和确定的；所述威胁函数是根据航迹的实际折线长度和航迹中位于威胁区域内的折线长度的总和确定的。

在一些实施例中，所述航迹质量下界函数是根据航迹中的不确定性因素变量变化时代价函数取值为最小值确定的。

在一些实施例中，所述航迹质量上界函数是根据航迹中的不确定性因素变量变化时代价函数取值为最大值确定的。

在一些实施例中，所述航迹质量上界最小值函数是根据航迹中的不确定性因素变量变化时航迹质量上界函数取值为最小值确定的。

在一些实施例中，所述目标函数为：

其中，obj表示目标函数的目标，lb₀表示航迹质量下界函数，ub₀表示航迹质量上界函数，val_minmax表示航迹质量上界最小值函数，Φ_threshold和Φ_index是预设参数，L_large表示设置的一个大于预设值的正数；所述预设的约束条件为ub₀≥val_minmax，Φ_index∈((val_minmin，val_minmax))，以及其中，F_cost(X,Y)表示利用航迹矩阵变量X和不确定性因素变量Y构建的航迹质量相关的代价函数。

根据本公开的另一些实施例，提供一种航迹规划装置，其特征在于，包括：存储器；以及耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行任一实施例所述的航迹规划方法。

根据本公开的又一些实施例，提供一种非瞬时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现任一实施例所述的航迹规划方法。

附图说明

下面将对实施例或相关技术描述中所需要使用的附图作简单的介绍。根据下面参照附图的详细描述，可以更加清楚地理解本公开。

显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本公开航迹规划方法的一些实施例的流程示意图。

图2为本公开航迹规划建模方法的一些实施例的示意图。

图3为本公开航迹规划装置的一些实施例的示意图。

具体实施方式

下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述。

图1为本公开航迹规划方法的一些实施例的流程示意图。该方法例如可以由航迹规划装置执行。

如图1所示，该实施例的方法包括步骤101-103。

在步骤101，利用航迹矩阵变量和不确定性因素变量构建航迹质量相关的代价函数。

首先，根据航迹矩阵变量构建确定性因素相关的函数，根据航迹矩阵变量和不确定性因素变量构建不确定性因素相关的函数，然后，对确定性因素相关的函数和不确定性因素相关的函数进行加权求和得到航迹质量相关的代价函数。其中，不确定性因素变量例如可以是威胁因素变量；确定性因素相关的函数可以是航迹长度函数、航迹高度函数、航迹平滑性函数和碰撞函数中的一种或多种，不确定性因素相关的函数可以是威胁函数。

下面将对代价函数的构建过程进行具体描述。

首先，在X-Y-Z三维坐标系下建模，用3×D个元素的矩阵变量来表述三维空间中的一条航迹。假设起始航行点为S，终止位置为T。在矩阵X的每一列上的三个元素(即x_1i,x_2i以及x_3i,i＝1,2,...,D)表征航迹折线第i个节点的三维坐标。矩阵变量X共决定D个这样的空间点位，将这些空间点位分别命名为Pos(i)＝([x_1i,x_2i,x_3i])，将这样的D个点顺次连接，即可得到一条连接始末位置的空间三维的航迹折线，如图2所示的航迹规划建模方法的示意图，示意性地给出了当D＝3时的一条航迹折线。

基于上述航迹规划的建模方法，根据航迹矩阵变量X中的起始位置和终止位置之间的线段长度和航迹的实际折线长度定义航迹长度函数为：

其中，C_length表示航迹长度，ST₁表示航迹的起始位置和终止位置之间的线段长度，ST₂表示为航迹的实际折线长度。由于欧式空间中的两点之间线段长度最短，则显然有C_length∈[0,1]。

根据最大航迹高度、最小航迹高度和航迹的平均海拔高度定义航迹高度函数为：

其中，C_altitude表示航迹高度，h_max表示最大航迹高度，h_min表示最小航迹高度，例如可以将h_max、h_min设置为场景地形图的最大、最小海拔高度。A_path表示航迹的平均海拔高度。显然可确定C_altitude∈[0,1]。

根据航迹的实际折线中的相邻折线段之间的夹角定义航迹平滑性函数为：

其中，C_smooth表示航迹的平滑性程度，

表示航迹中第i段折线段与第(i+1)段折线之间的夹角，d表示航迹中折线段的数量。基于上述定义，不难理解C_smooth∈[0,1]。

根据航迹的实际折线长度和航迹中位于地表以下的折线长度的总和定义碰撞函数为：

其中，C_collision表示航迹碰撞程度，L_under代表航迹中位于地表以下的折线长度的总和，P_large表示设置的一个大于预设值的正数，即表示一个较大的正数，ST₂表示为航迹的实际折线长度。类似地，有C_collision∈{0}∪[P_large,P_large+1]

根据航迹的实际折线长度和航迹中位于威胁区域内的折线长度的总和定义威胁函数为：

其中，C_threat表示航迹威胁程度，L_inside表示航迹中位于威胁区域内的折线长度的总和，P_large表示设置的一个大于预设值的正数，即表示一个较大的正数，ST₂表示为航迹的实际折线长度。从这一定义来看，有C_threat∈{0}∪[P_large,P_large+1]。

对于上述的A_path,L_inside以及L_under的求解,可以采用有限点元近似的方法进行简化计算。具体地，以L_inside的计算为例，首先将每一段折线段中间(不包括首尾端点)均匀地分布多达Nfe个点，这样在总共(D+1)(即d)段折线段中，也就是在整条航迹上，累计拥有(D+2+(D+1)·Nfe)个采样点。如果其中有n个采样点落入了威胁区域之内，就可以粗略地将L_inside估计为

尽管这样的估计并不精确(主要原因在于各段折线段的长度不一定相等)，但是在Nfe、D取值较大时，仍能够保证估计精度。至此，建立了一种普遍适用的三维空间航迹规划模型。

最后，对确定性因素相关的函数和不确定性因素相关的函数进行加权求和，也就是对航迹长度函数、航迹高度函数、航迹平滑性函数、碰撞函数以及威胁函数进行加权求和得到航迹质量相关的代价函数。

构建航迹质量相关的代价函数为：

F_cost＝w₁·C_length+w₂·C_altitude+w₃·C_threat+w₄·C_collision+w₅·C_smooth

其中，C_length惩罚较长的航迹，C_altitude惩罚较高的平均高度，C_threat惩罚航迹中存在飞入威胁区域(包括军事管理区域、禁飞区、危险化工品加工厂、医院、加油站、高考考点等等)内的情况，C_collision表征无人机与地面的碰撞，最后，C_smooth对平滑性提出了质量上的要求。此外，w₁～w₅为各单项指标的加权系数，可根据用户需求设定。这里，指定C_length,C_altitude以及C_smooth为航迹质量型指标，而将另外的C_threat、C_collision指标设置为可行性关键约束指标，换言之，如果无人机与地面发生碰撞或实际进入威胁区域，则认为该条航迹的质量较差，不予考虑。

为区别对待可行性指标与质量型指标，在C_threat、C_collision指标的取值上添加了较大的门限阈值。以C_collision为例，如果一条待选航迹并未碰撞地面，则该项取值为0；反之，该项指标取P_large与(P_large+1)之间的一个数值，其中P_large为较大的正数，惩罚性地对碰撞的情况予以直接地表征。将C_collision的碰撞情况映射到[P_large,P_large+1]这一区间上，是为了有区别地处理不同碰撞程度时的情况，这样做主要是方便优化算法逐步进化的寻优搜索过程。

在步骤102，根据航迹质量相关的代价函数构建航迹的目标函数。

具体地，构建基于代价函数表示的航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数；然后，基于航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数构建航迹的目标函数。下面将进行具体的描述。

首先，在航迹规划任务中，对不确定因素的不确定性表达恰恰是对既有环境威胁因素的准确表达，基于区间分析(内)、区间评价(外)的双层随机规划方法，旨在处理环境威胁态势不能准确表征状况下的航迹规划问题。

假设部分威胁区域的中心位置坐标或其辐射半径信息在某连续闭区间内变化，但不能准确刻画其变化的分布情况，这样需要对整个区间上的威胁存在可能进行一致性的考虑。将不确定信息当做自变量，寻求待选航迹基于威胁变化的航迹质量浮动区间，进而方便后续外层对每一条待选航迹的区间进行比较。然而，如果内层对每一条待选航迹的质量浮动区间进行精确的优化计算，将构成标准的嵌套优化结构，使得求解速度大幅度恶化。在此，本公开采用配点区间分析方法对较小区间上自变量变化导致的航迹质量因变量变化情况进行估计。这一方法的求解速度较快，有效地避免了嵌套优化情况的发生。下面将对该方法进行具体描述。

假设一条确定的待选航迹X₀对应的区间响应表示为[lb₀,ub₀]，具体而言，这一区间响应即为考虑不确定性情况后的，通过配点区间分析方法求解得到的一个对应的区间数，lb₀也称为航迹X₀对应的航迹质量下界函数，根据航迹中的不确定性因素变量变化时代价函数取值为最小值定义航迹质量下界函数为：

其中，lb₀表示航迹质量下界，X表示航迹矩阵变量，Y表示航迹威胁变量，F_cost(X₀,Y)表示根据确定的航迹矩阵X₀和不确定的Y所确定的与航迹质量相关的代价函数，

表示根据航迹威胁变量Y的变化使得代价函数F_cost(X₀,Y)取得最小值的代价值。

ub₀也称为航迹X₀对应的航迹质量上界函数，根据航迹中的不确定性因素变量变化时代价函数取值为最大值定义航迹质量上界函数为：

其中，ub₀表示航迹质量上界，X₀表示航迹矩阵变量，Y表示航迹威胁变量，F_cost(X₀,Y)表示根据确定的航迹矩阵X₀和不确定的Y所确定的与航迹质量相关的代价函数，

表示根据航迹威胁变量Y的变化使得代价函数F_cost(X₀,Y)取得最大值的代价值。

首先，将X航迹待选变量放开为自由变量，跟随Y一起进行优化，得到

然后，仅仅令X自由，去寻找使得F_cost(X,Y)最大化的Y，换言之，寻求

即根据航迹中的不确定性因素变量变化时航迹质量上界函数取值为最小值定义航迹质量上界最小值函数为：

其中，val_minmax表示航迹质量上界最小值，X表示航迹矩阵变量，Y表示航迹威胁变量，F_cost(X,Y)表示根据不确定的X和不确定的Y所确定的与航迹质量相关的代价函数，

表示根据航迹威胁变量Y的变化使得代价函数F_cost(X,Y)取得最大值的代价值，

表示根据航迹威胁变量X的变化使得航迹质量上界函数

取得最小值的代价值。

由于val_minmin代表了极限理想情况(不确定性与优质航迹“恰好”同时匹配)，因此可以理解真实情况下的航迹质量最优值不会超过这一理想情况，也即lb₀≥val_minmin；另一方面，寻优的过程中任意一条待选轨迹，其“最坏”可能性都会比

这一结果更差，因为这一表达式是针对所有上界值中的最好情况，因此有ub₀≥val_minmax。设计如下目标函数obj(·)：

其中，obj表示目标函数的目标，lb₀表示航迹质量下界，ub₀表示航迹质量上界，Φ_threshold和Φ_index是预设参数，可以由用户指定，L_large表示设置的一个大于预设值的正数，即一个较大的正数；预设的约束条件为ub₀≥val_minmax，Φ_index∈(val_minmin，val_minmax)，以及

下面对上述目标函数优化指标的合理性进行阐述。首先，航迹规划往往对航迹的保守性具有一定的考虑，因此用户容易地能对航迹区间响应人为地设置上界，代表用户对最差情况的接受上限，上述公式中隐含ub₀≤Φ_threshold的要求。第二点，前面已经提及ub₀-val_minmax≥0，但是(lb₀-Φ_index)的符号无法固定。经过简单分析，可以知道，当lb₀＞Φ_index时，优化搜索过程将倾向于寻找较小上界ub₀的待选响应区间，换言之，此时目标是上下边界的浮动范围尽可能地收缩；相反地，如果lb₀＜Φ_index，此时目标函数的符号为负，导致寻优过程倾向于下界lb₀更小、上界ub₀更大的解。由于上界还有Φ_threshold的限制，因此最理想的寻优过程将是lb₀趋于val_minmin。

用变量的形式表征不确定因素，对目标函数进行优化，使得规划的航迹更可靠、更安全。

在步骤103，在满足预设约束条件的情况下，确定一组航迹矩阵和威胁因素取值，使得所述目标函数的值最小，并将确定的航迹矩阵相应的航迹确定为规划的航迹。

基于定义的目标函数，在满足预设约束条件的情况下，通过对矩阵

中的每个元素进行优化，即可在考虑环境不确定性的条件下，并将环境的不确定性因素表示为威胁因素变量，生成更具可靠性和安全性的航迹，提高了航迹规划的准确性。

此外，使用一种高效的群智能优化算法，如人工蜂群算法，在外层对待选航迹进行筛选；每一条航迹对应的区间响应在内层通过本公开的配点区间分析方法获得；在人工蜂群算法中对响应区间进行质量评估，在外层对质量较好的响应区间进行搜索寻优。

图3为本公开的航迹规划装置的一些实施例的示意图。

如图3所示，该实施例的航迹规划装置300包括存储器301以及耦接至该存储器301的处理器302，处理器302被配置为基于存储在存储器301中的指令，执行本公开中任意一些实施例中的航迹规划方法。

其中，存储器301例如可以包括系统存储器、固定非易失性存储介质等。系统存储器例如存储有操作系统、应用程序、引导装载程序(Boot Loader)、数据库以及其他程序等。

本领域内的技术人员应当明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解为可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

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

Claims (12)

1.一种航迹规划方法，其特征在于，包括：

利用航迹矩阵变量和不确定性因素变量构建航迹质量相关的代价函数；

根据航迹质量相关的代价函数构建航迹的目标函数；

在满足预设约束条件的情况下，确定一组航迹矩阵和不确定性因素取值，使得所述目标函数的值最小，并将确定的航迹矩阵相应的航迹确定为规划的航迹。

2.根据权利要求1所述的航迹规划方法，其特征在于，所述利用航迹矩阵变量和不确定性因素变量构建航迹质量相关的代价函数包括：

根据航迹矩阵变量构建确定性因素相关的函数；

根据航迹矩阵变量和不确定性因素变量构建不确定性因素相关的函数；

对确定性因素相关的函数和不确定性因素相关的函数进行加权求和得到航迹质量相关的代价函数。

3.根据权利要求1所述的航迹规划方法，其特征在于，所述根据航迹质量相关的代价函数构建航迹的目标函数包括：

构建基于代价函数表示的航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数；

基于航迹质量下界函数、航迹质量上界函数和航迹质量上界最小值函数构建航迹的目标函数。

4.根据权利要求1所述的航迹规划方法，其特征在于，所述不确定性因素变量包括威胁因素变量。

5.根据权利要求2所述的航迹规划方法，其特征在于，

所述确定性因素相关的函数包括航迹长度函数、航迹高度函数、航迹平滑性函数和碰撞函数中的一种或多种；

所述不确定性因素相关的函数包括威胁函数。

6.根据权利要求5所述的航迹规划方法，其特征在于，

所述航迹长度函数是根据航迹的起始位置和终止位置之间的线段长度和航迹的实际折线长度确定的；

所述航迹高度函数是根据最大航迹高度、最小航迹高度和航迹的平均海拔高度确定的；

所述航迹平滑性函数是根据航迹的实际折线中的相邻折线段之间的夹角确定的；

所述碰撞函数是根据航迹的实际折线长度和航迹中位于地表以下的折线长度的总和确定的；

所述威胁函数是根据航迹的实际折线长度和航迹中位于威胁区域内的折线长度的总和确定的。

7.根据权利要求3所述的航迹规划方法，其特征在于，所述航迹质量下界函数是根据航迹中的不确定性因素变量变化时代价函数取值为最小值确定的。

8.根据权利要求3所述的航迹规划方法，其特征在于，所述航迹质量上界函数是根据航迹中的不确定性因素变量变化时代价函数取值为最大值确定的。

9.根据权利要求3所述的航迹规划方法，其特征在于，所述航迹质量上界最小值函数是根据航迹中的不确定性因素变量变化时航迹质量上界函数取值为最小值确定的。

10.根据权利要求3所述的航迹规划方法，其特征在于，所述目标函数为：

其中，obj表示目标函数的目标，lb₀表示航迹质量下界函数，ub₀表示航迹质量上界函数，val_minmax表示航迹质量上界最小值函数，Φ_threshold和Φ_index是预设参数，L_large表示设置的一个大于预设值的正数；

所述预设的约束条件为ub₀≥val_minmax，Φ_index∈((val_minmin，val_minmax))，以及

其中，

F_cost(X，Y)表示利用航迹矩阵变量X和不确定性因素变量Y构建的航迹质量相关的代价函数。

11.一种航迹规划装置，其特征在于，包括：

存储器；以及

耦接至所述存储器的处理器，所述处理器被配置为基于存储在所述存储器中的指令，执行权利要求1-10中任一项所述的航迹规划方法。

12.一种非瞬时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现权利要求1-10中任一项所述的航迹规划方法。

Images