CN114594788A - 一种未知环境下四旋翼无人飞行器航迹规划方法及*** - Google Patents

Abstract

本发明公开了一种未知环境下四旋翼无人飞行器航迹规划方法及***，利用基于最小化snap的多项式优化得到全局航迹；在飞行过程中，触发更新全局航迹，通过引入新的全局安全点实现全局航迹的重规划；以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；对需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。本发明有效地突破了视觉传感器固有的视角限制，在保证实时性的同时大幅提升了无人机在未知环境中飞行的安全性和成功率。

Description

一种未知环境下四旋翼无人飞行器航迹规划方法及***

技术领域

本发明属于四旋翼无人飞行器自主导航的轨迹规划技术领域，具体涉及一种未知环境下四旋翼无人飞行器航迹规划方法及***。

背景技术

近年来无人机研制、生产成本不断降低，应用范围日益广泛。其中四旋翼无人机因其构造简单，机动性能好，使用方便等优点而深受大众喜爱。人们逐渐开始利用四旋翼无人机搭载各种负载设备完成电力巡检、物资配送、协助灾害救援等任务，但飞行环境的复杂性对无人机***的航迹规划能力提出了更高要求。目前，基于已知环境的无人机航迹规划已有很多解决方法。但当无人机面对未知环境时，环境先验信息被极大削弱，加上无人机自身的续航能力和计算资源有限，轻量、安全、实时性好的航迹规划方法变得至关重要。

未知环境下的无人机航迹规划一般采用全局航迹规划和局部航迹重规划相结合的方式。全局航迹规划可以确定全局最优或者较优的路径轨迹，一定程度上为无人机到达终点提供方向指引；而局部规划依靠从传感器获得的局部环境信息生成实时局部可行的安全轨迹。将全局规划和局部规划相结合可以有效弥补环境信息的缺失。其中，视觉传感器具有重量轻、体积小、成本低以及隐蔽性好的优点，在四旋翼无人机上得以广泛使用。但相机的视角有限，无法获取全向环境信息，在有限视角情况下保证未知环境中的无人机飞行安全是一个难点问题。多路视觉传感器的叠加在一定程度上可以获得更多的环境信息，但同时也增加了功耗和数据处理难度，给无人机的续航和机载计算资源带来了很大的负担。

目前，全局规划和局部重规划相结合的方式在障碍稀疏的森林环境中得到了不错的应用效果。全局规划是根据起点和终点生成全局航迹，一般为简单的直线或曲线，而局部规划是在全局航迹上选取局部目标点，然后基于传感器信息规划出安全可行的实际航迹。但在大型复杂障碍环境，例如楼宇或长走廊的室内环境，上述方法几乎失效，成功率很低。原因是无人机的视野受限明显，初始生成的全局航迹并不能保证提供正确的方向引导信息。特别，当全局航迹给出不当指引时，无人机将以错误的局部目标点进行优化，即使不断进行局部重规划也难以求解出可行的航迹，无人机的航迹规划成功率和飞行安全性大大降低，甚至可能出现无人机撞墙、坠毁等严重事故。

综上所述，尽管全局规划和局部重规划的方式可以一定程度实现未知环境下的无人机导航，但适用性不够，且要求环境中的无人机视野足够开阔。因此，研究轻量、鲁棒性强、安全、实时性好的未知环境下四旋翼无人飞行器航迹规划方法是目前提升无人机自主能力，扩大无人机应用场景的关键。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种未知环境下四旋翼无人飞行器航迹规划方法及***，结合环境信息更新全局轨迹，及时为无人机提供正确的方向指引，利用局部重规划进行实时避障确保安全，该设计有效地突破了视觉传感器固有的视角限制，极大地提升了无人机在未知环境中飞行的安全性和成功率。

本发明采用以下技术方案：

一种未知环境下四旋翼无人飞行器航迹规划方法，包括以下步骤：

S1、将当前选取的全局安全点的状态信息作为输入，以最小化snap作为优化目标，利用基于最小化snap的多项式优化得到全局航迹，初始的全局安全点是起点和终点；

S2、在飞行过程中，触发更新步骤S1得到的全局航迹，通过引入新的全局安全点实现全局航迹的重规划；

S3、在步骤S2的全局航迹上，以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；

S4、对步骤S3中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。

具体的，步骤S1中，基于最小化snap的多项式表示四旋翼无人飞行器轨迹，无人机轨迹由分段多项式组成，第i个多项式段表示为：

f_i(t)＝p_i，nt^N+p_i，n-1t^N-1+…+p_i，0

其中，p_i,0，p_i,1，…，p_i,n是多项式系数，f_i(t)为时刻t时轨迹上的位置。

具体的，步骤S1中，优化目标Min S(T)为：

其中，S_i(T)为第j段多项式的总和的snap代价，P_i ^T为第i段多项式的系数矩阵，P_i为P_i ^T矩阵的转置，Q_i是积分所得的包含时间变量的矩阵。

进一步的，第j段多项式的总和的snap代价S_i(T)为：

其中，T_i和T_i-1为第i段多项式的时间区间点，f_i ⁴(t)为无人机第i段轨迹的snap，n、v为多项式的次数，p_i,n、p_i,v为第i段多项式的系数。

具体的，步骤S2中，全局航迹的重规划包括紧急全局航迹重规划和常规全局航迹重规划；

紧急全局航迹重规划具体为：

当有效视野率γ为0时，代表无人机视野内全是障碍物，触发紧急全局航迹重规划，用相邻障碍边界点之间的方向向量表示障碍物走向，此时无人机增加视野有效率，沿障碍向左或者向右，当无人机当前位置点P_o到无人机视野最左端分界线与障碍物的交点A_l的距离d_uo1大于无人机当前位置点P_o到无人机视野最右端分界线与障碍物的交点A_r的距离d_uo2时，选择障碍物的左方向作为安全方向，当d_uo1＜d_uo2时，选择障碍物的右方向作为安全方向；当||d_uo1-d_uo2||≤εRs时，如果cosα1＞cosα2，选择障碍物的左方向作为安全方向，反之，选择障碍物的右方向作为安全方向，ε是放缩系数，R_s为无人机的可见视野半径，α₁为无人机和目标点之间的方向向量与无人机视野左边界方向之间的夹角，α₂为无人机和目标点之间的方向向量与无人机视野右边界方向之间的夹角；选择安全方向后，在安全方向上按设定距离R_add选择路径点P_add作为新的全局安全点，和当前的无人机位置点P_o，目标点P_s，三个一起作为安全路径点，生成以最小化snap为优化目标的全局轨迹，实现无人机的方向扭转和局部目标的修正；

常规全局航迹重规划具体为：

当有效视野率γ不为0时，选择无人机走过的距离信息为触发条件，每当无人机执行路径L_a达到起点终点间总距离L_A的设定比例时，触发一次全局航迹规划，在进行常规全局航迹重规划时，选择无人机局部视野范围内的最远安全点作为新的全局路径点P_add进行添加。

进一步的，常规全局重规划情况新增的全局路径点P_add1满足条件如下：

其中，

为局部航迹曲线，R_s为无人机的可见视野半径。

进一步的，紧急全局重规划情况新增的全局路径点P_add2为：

其中，

为紧急全局航迹重规划的要求下确定的安全方向向量。

具体的，步骤S3中，依据无人机的规划距离R_p在全局航迹上选取局部目标点，基于约束条件进行无人机局部航迹的优化求解，以生成可行的局部轨迹，优化问题如下：

minJ＝λ_sJ_s+λ_cJ_c+λ_dJ_d

其中，J_s代表光滑惩罚，J_c代表碰撞惩罚，而J_d代表动力学可行项惩罚，λ_s，λ_c，λ_d为各惩罚项的权值。

具体的，步骤S3中，如果求解不成功，则继续进行求解，当需要进行重规划时进行局部航迹重规划，在抵达终点时停止执行，局部航迹重规划的条件如下：

无人机距离此次规划的起点距离大于设置的最小规划威胁距离r_plan或者在当前的局部航迹基础上无法成功避障；最小规划威胁距离r_plan意味着在一次局部规划后，无人机在开始执行轨迹的前一段时间不进行重规划检查，实时的局部轨迹在规划时直接执行，而超过设定距离后进入重新规划轨迹状态；重新规划轨迹状态能够在当前点和局部目标点之间重新求解安全轨迹，得到的轨迹可行进入执行状态，否则将持续进行求解。

本发明的另一个技术方案是，一种未知环境下四旋翼无人飞行器航迹规划***，其特征在于，包括：

初始模块，将当前选取的全局安全点的状态信息作为输入，以最小化snap作为优化目标，利用基于最小化snap的多项式优化得到全局航迹，初始的全局安全点是起点和终点；

触发模块，在飞行过程中，触发更新初始模块得到的全局航迹，通过引入新的全局安全点实现全局航迹的重规划；

规划模块，在触发模块的全局航迹上，以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；

更新模块，对规划模块中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。

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

本发明一种未知环境下四旋翼无人飞行器航迹规划方法，通过基于最小化snap的多项式全局航迹和相应的重规划策略，突破了无人机感知视角的限制，为无人机在未知环境中抵达目标点提供正确且高效的方向引导；同时利用实时的局部重规划规避实际飞行中的障碍，确保无人机的顺利飞行，两者的有效结合大幅提升了无人机在未知环境中飞行的安全性和成功率。

进一步的，多项式曲线天然满足无人机对轨迹光滑的要求，利用分段多项式表示全局航迹，可以方便求解轨迹的积分和微分，同时基于最小化snap的多项式存在闭式解，求解快速。

进一步的，将各段航迹的snap总和作为优化目标Min S(T)，根据无人机动力学知识，航迹的snap对应无人机推力的导数，最小化snap意味着最大限度减少差动推力，节省能源。

进一步的，推导一段多项式航迹的snap代价，将求解代价问题建模为数学上的二次规划问题。

进一步的，初始的全局航迹只包含起点和终点信息，无法确保航迹指引的正确性，容易导致无人机陷入危险状态；在实际飞行中，根据实时环境信息引入了额外的安全路径点，通过紧急全局航迹重规划和常规全局航迹重规划修正全局航迹，提升无人机的飞行安全。

进一步的，在无人机视野被障碍完全遮挡的情况下，沿选定的安全空间大的“障碍边界”添加新的全局路径点P_add，并触发紧急全局航迹重规划，可以快速提升无人机的视野有效率，使局部优化求解的成功率提高，避免引发安全事故。

进一步的，在无人机正常飞行过程中，按照飞行距离触发常规全局航迹重规划，将可见范围边界的安全点作为新的全局路径点P_add，该点包含实时的环境信息并且是局部优化的结果，将其引入全局航迹可以提升全局航迹指引的可靠性。

进一步的，在局部轨迹优化目标的设定中分别引入了光滑惩罚项以确保规划的航迹能被无人机更好地跟随，并且节省能量；引入碰撞惩罚项使无人机远离障碍物，确保飞行安全；引入动力学可行惩罚项以保证规划的航迹满足无人机的最大速度，最大加速度等动力学限制，确保规划的航迹可以被无人机实际执行。

进一步的，设置局部重规划条件避免了频繁触发重规划，在保证无人机安全的情况下，避免了不必要的局部重规划，减少了计算时间，有利于无人机的快速飞行。

综上所述，本发明方法有效地突破了视觉传感器固有的视角限制，在保证实时性的同时大幅提升了无人机在未知环境中飞行的安全性和成功率。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明流程示意图；

图2为全局航迹重规划紧急触发示意图；

图3为方向选择考虑规划终点情况示意图；

图4为全局航迹重规划常规触发示意图；

图5为本发明自主决策过程展示图；

图6为未知环境下本发明和对比算法仿真结果对比图，其中，(a)为EGO-Planner项目的运行结果，(b)为本发明方法的运行结果。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系。

应当理解，尽管在本发明实施例中可能采用术语第一、第二、第三等来描述预设范围等，但这些预设范围不应限于这些术语。这些术语仅用来将预设范围彼此区分开。例如，在不脱离本发明实施例范围的情况下，第一预设范围也可以被称为第二预设范围，类似地，第二预设范围也可以被称为第一预设范围。

取决于语境，如在此所使用的词语“如果”可以被解释成为“在……时”或“当……时”或“响应于确定”或“响应于检测”。类似地，取决于语境，短语“如果确定”或“如果检测(陈述的条件或事件)”可以被解释成为“当确定时”或“响应于确定”或“当检测(陈述的条件或事件)时”或“响应于检测(陈述的条件或事件)”。

在附图中示出了根据本发明公开实施例的各种结构示意图。这些图并非是按比例绘制的，其中为了清楚表达的目的，放大了某些细节，并且可能省略了某些细节。图中所示出的各种区域、层的形状及它们之间的相对大小、位置关系仅是示例性的，实际中可能由于制造公差或技术限制而有所偏差，并且本领域技术人员根据实际所需可以另外设计具有不同形状、大小、相对位置的区域/层。

本发明提供了一种未知环境下四旋翼无人飞行器航迹规划方法，生成全局航迹，重规划全局航迹，生成局部航迹，并对局部航迹进行重新规划，得到安全可行的无人机航迹，本发明结合环境信息更新全局轨迹，及时为无人机提供正确的方向指引，利用局部重规划进行实时避障确保安全，该设计有效地突破了视觉传感器固有的视角限制，极大地提升了无人机在未知环境中飞行的安全性和成功率。

请参阅图1，本发明一种未知环境下四旋翼无人飞行器航迹规划方法，包括以下步骤：

S1、全局航迹的生成；

将当前选取的全局安全点的状态信息(位置、速度、加速度)作为输入，利用基于最小化snap的多项式优化得到全局航迹。初始的全局安全点是起点和终点。

在进行全局航迹规划时是利用多项式表征航迹，并以最小化snap作为优化目标。snap是加速度的二阶导数，根据无人机动力学知识，最小化snap意味着最大限度减少差动推力，节省能量。

基于分段多项式来表示四旋翼无人飞行器轨迹，第i个多项式段可以表示为：

其中，p_i,0，p_i,1，…，p_i,n是多项式系数，f_i(t)为时刻t时轨迹上的位置，可以对位置进行四次求导计算出轨迹上某一时刻的加加加速度snap(t)。

用S_i(T)代表第i段多项式的总和的snap代价，则有：

第i段多项式的总和的snap代价S_i(T)为：

其中，T_i和T_i-1为第i段多项式的时间区间点，f_i ⁴(t)为无人机第i段轨迹的snap，n、v为多项式的次数，p_i,n、p_i,v为第i段多项式的系数，P_i ^T为第i段多项式的系数矩阵，P_i ^T＝[p_i,0，p_i,1，…，p_i,n]，P_i为P_i ^T矩阵的转置，Q_i是积分所得的包含时间变量的矩阵。

设定的优化目标为：

最小化snap轨迹优化问题建模为二次规划问题，又因为Q矩阵是半正定的，因此该QP问题构成典型的凸优化问题，存在闭式解，可以直接求解。

S2、全局航迹重规划；

除了初始的全局安全点，飞行过程会触发全局航迹的更新，通过引入新的全局安全点，实现全局航迹的重规划，包括紧急全局重规划和常规全局重规划。

在初始状态时，无人机处于未知环境，没有其它的环境信息，此时可以确定的安全路径点只有起点和终点，在实际飞行中，根据环境信息引入了额外的安全路径点进行全局航迹重规划。

无人机的可见视野半径为R_s，规划半径R_p，水平视野角为θ₁，垂直视野角为θ₂。视野范围中的空闲区域越多，意味着供无人机路径搜索的范围和可能性更多，这有利于得到安全可行的路径。选取无人机飞行高度所在的平面，针对无人机可见的扇形区域，即无人机为圆心的前方θ₁范围内，从k个方向以均匀的分散式角度

进行障碍查询，通过无人机可见范围内各个方位的障碍情况，可以推测该时刻无人机的视野可视程度。

定义有效视野率γ如下：

其中，n_obs代表存在障碍物遮挡的角度数量，若有障碍，则将该方向上的障碍物边界点计入对应的pt_k。γ越大，越有利于无人机寻找到可通行路径，反之，则路径搜索范围缩小；当γ为0时，代表无人机视野内全是障碍物，没有突破点，此时无人机会陷入不停搜索但无法有效逃离的状态。此时将触发全局航迹的紧急更新。

1)紧急全局航迹重规划触发

请参阅图2，黑色标记代表局部的目标点。无人机在以该目标点为局部规划终点的过程中，不断接近目标点，但同时视野有效率γ不断减小，在③时，无人机的视野有效率γ为0。为使无人机跳出视野无效的危险状态，这里通过触发全局航迹，改变全局轨迹来矫正局部目标点，从根本上解决无人机无法找到安全出口的情况，为无人机安全到达终点提供正确指引。

用相邻障碍边界点之间的方向向量表示障碍物走向，即

记录障碍物的“左方向”，

记录障碍物的“右方向”。则有：

此时无人机有两个选择，沿着障碍向左或者向右，此时通过障碍查询，分别记录下无人机视野最左端和最右端的分界线与障碍物的交点A_l和A_r，同时记下无人机当前位置点P_o到两个交点之间的距离d_uo分别为d_uo1和d_uo2。

d_uo1＝||P_o-pt₁||，d_uo2＝||P_o-pt_k|| (6)

选取方向原则1：

当d_uo1>d_uo2时，选择障碍物的“左方向”

作为安全方向，当d_uo1<d_uo2时，选择障碍物的“右方向”

作为安全方向。

d_uo越大代表无人机距离该障碍物越远，安全性更高，更有利于下一步进行路径优化的搜索。按照规则1，在图2所示的③状态下，无人机选择障碍物的“左方向”更为安全。

选取方向原则2：

当d_uo1和d_uo2两者相差较小时，即‖d_uo1-d_uo2‖≤εR_s，则比较如下：

如果cosα₁>cosα₂，则选择障碍物的“左方向”

作为安全方向，反之选择障碍物的“右方向”

作为安全方向。

其中，ε∈(0,0.1)，ε是放缩系数，ε越大则代表无人机对该环境中安全程度的要求越宽松，相比于安全保守，更倾向于快速搜索并到达全局目标。

请参阅图3，

为无人机和目标点之间的方向向量，这里通过比较方向向量和无人机视野边界方向之间的夹角的大小关系，反映出当时状态下无人机选择哪一方向将更有可能安全接近目标。在图3所示的①状态下，按照规则2，无人机左右两边视野边缘和障碍物的距离相近，因此参考最终的目标点进行方向抉择，最终选择障碍物的“右方向”作为安全方向。

选择安全方向后，在安全方向上按照一定距离R_add选择路径点P_add作为新的全局安全点，和当前的无人机位置点P_o，目标点P_s，三个一起作为安全路径点，生成以最小化snap为优化目标的全局轨迹，由此实现无人机的方向扭转和局部目标的修正。其中，紧急全局重规划情况新增的全局路径点P_add2为：

其中，

为

或

具体为根据规则1和规则2的要求确定的安全方向向量。

2)常规全局航迹重规划触发

当γ不为0时，为避免只选取起点和终点生成的全局航迹无法满足需求，同样引入对全局航迹的更新。这里选择无人机走过的距离信息为触发，即

L_a＝μL_A (9)

每当无人机执行路径L_a达到起点终点间总距离L_A的一定比例时，就触发一次全局航迹规划。触发的路径占比μ根据场景的大小和复杂度等进行调整。

在进行全局航迹重规划时，为了使全局航迹尽可能地包含环境障碍信息，本发明选择无人机局部视野范围内的最远安全点作为新的全局路径点P_add进行添加。

常规全局重规划情况新增的全局路径点P_add1满足以下条件：

其中，

是局部航迹曲线。

请参阅图4，将图中的深色小圆点作为新的全局安全点，重新进行全局航迹规划，增加了全局路径的引导正确性和可靠性。实线部分曲线是更新后的全局航迹，虚线部分是初始的全局规划路线。初始的全局规划路线有相当一部分是处于障碍物中，属于不可达状态，这将会给无人机以错误的引导信息。而更新后全局航迹结合了最新的环境信息，将整体规划路线拉向更安全的区域，增加了全局航迹的指引正确性，减少了无人机因为局部目标点错误而导致优化求解失败的概率。

S3、局部航迹的生成；

在全局航迹上以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功则执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，否则就继续求解。当需要进行重规划时进入步骤S4，在抵达终点时停止执行。

首先依据无人机的规划距离R_p在全局航迹上选取局部目标点，然后基于均匀B样条进行无人机局部航迹的优化求解。

B样条曲线由阶数m+1，n个控制点和一个节点向量{t₁,t₂,…t_a}决定，其中有a＝m+n-1，2≤m≤n。均匀B样条即每个节点有相同的时间间隔，m次B样条由以下公式表示：

其中，Q_i∈R³是t_i时刻B样条的控制点，B_i,m(t)是B样条的基函数。

根据无人机的微分平坦特性，规划的变量被简化为控制点的位置信息Q_i∈R³。

优化问题定义如下：

minJ＝λ_sJ_s+λ_cJ_c+λ_dJ_d (13)

其中，J_s代表光滑惩罚，J_c代表碰撞惩罚，而J_d代表动力学可行项惩罚，λ_s，λ_c，λ_d为各惩罚项的权值。最小化高阶导数能够使得整段轨迹光滑。这里光滑项由加速度A_i和加加速度J_i的平方和构成。

碰撞惩罚用于将控制点推离障碍物。这是通过采用安全间隙s_f和使用d_ij<s_f惩罚控制点来实现的。即设障碍物与控制点之间距离的阈值为s_f，当障碍物距离小于安全间隙时，意味着有发生碰撞的可能，d_ij是由A*搜素得到的紧贴障碍边缘的j个安全点和B样条控制点Q_i之间的引导向量，这里利用引导项构建碰撞代价，省去了欧式符号距离场ESDF的存储和使用，加速了优化求解；其中，c_ij＝s_f-d_ij代表安全距离余量。

第i个控制点的碰撞代价是所有碰撞牵引点的代价之和j_c(Q_i)，对所有控制点的碰撞代价求和得到总的碰撞项：

动力学可行性惩罚实际上通过限制轨迹在三个轴上的各阶导数来实现。得益于B样条曲线的凸包性质，只要对控制点进行限制即可满足动力学可行性。

其中，ω_v，ω_a，ω_j是每一项权值，F(·)为控制点高阶导数的二次连续可微函数。

F(C)＝∑_{r＝x，y，z}f(c_r) (17)

其中，c_r∈C∈{V_i，A_i，J_i}，a₁，b₁，c₁，a₂，b₂，c₂是用来满足函数二阶连续的常量，c_m是导数限制，c_j为二次和三次函数的交界，λ为弹性系数，使得最终的结果满足约束。

在进行初步的路径点优化后，对于动力学可行性将采取细化调整的方式，必要时将对时间进行再分配，以保证航迹的可执行。可执行的航迹将发给控制器执行。

S4、局部航迹重规划。

对步骤S3中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则转入步骤S3的执行航迹。

符合重规划的条件如下：

无人机距离此次规划的起点的距离大于设置的最小规划威胁距离r_plan或者在当前的局部航迹基础上无法成功避障。最小规划威胁距离r_plan意味着在一次局部规划后，无人机在开始执行轨迹的前一段时间是不需要进行重规划检查的，实时的局部轨迹在规划时已经确保了安全，可以直接执行，而超过这一距离，则进入无法保障安全，需要进入重新规划轨迹状态。重新规划轨迹状态会在当前点和局部目标点之间重新求解安全轨迹。得到的轨迹可行将进入执行状态，否则将持续进行求解。

本发明再一个实施例中，提供一种未知环境下四旋翼无人飞行器航迹规划***，该***能够用于实现上述未知环境下四旋翼无人飞行器航迹规划方法，具体的，该未知环境下四旋翼无人飞行器航迹规划***包括初始模块、触发模块以及规划模块。

其中，初始模块，将当前选取的全局安全点的状态信息作为输入，以最小化snap作为优化目标，利用基于最小化snap的多项式优化得到全局航迹，初始的全局安全点是起点和终点；

触发模块，在飞行过程中，触发更新初始模块得到的全局航迹，通过引入新的全局安全点实现全局航迹的重规划；

规划模块，在触发模块的全局航迹上，以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；

更新模块，对规划模块中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

以下介绍利用本项目提出全局和局部双层重规划方法在仿真室内环境下的应用实例。在该仿真实验中，无人机仅能获取视野范围内的障碍物信息，对于全局的环境是未知的，以此模拟无人机在未知环境中的避障过程。

参考英特尔RealSense深度相机D435相机的性能参数，在仿真中将无人机的水平视角设为85°，垂直视角60°，可见距离R_s＝5m，规划距离R_p＝7.5m，最大速度为3m/s，最大加速度为2m/s²。局部优化各个惩罚项的权重设置为：λ_s＝1.5，λ_c＝0.2，λ_d＝0.2，最小规划威胁距离r_plan＝1.0。全局更新利用基于最小化snap的五次多项式进行优化求解，在局部优化时采用三次均匀B样条。

仿真中选取17个方向均匀地进行障碍检查，放缩系数ε＝0.1，触发比例μ＝0.1，即无人机每飞行过全局总路径长度的十分之一就会触发常规全局更新。浅色的扇状点用于可视化无人机的视野边界，深色伴有箭头标识的曲线代表规划的局部航迹，浅色线段代表全局路径，大圆点和小圆点分别代表紧急全局更新和常规全局更新策略下加入的新的安全路径点。无人机从坐标轴起点出发安全抵达所选定的目标点，请参阅图5，具体的自主飞行决策展示如下：

决策1).初始生成全局路径；

决策2).利用局部规划避障；

决策3).视野全是障碍，触发紧急全局更新；

决策4).根据更新后的全局路径飞行；

决策5).距上次重规划飞过1/10全局路径，触发常规全局更新；

决策6).触发常规全局更新；

决策7).局部重规划同时触发常规全局更新；

决策8).触发常规全局更新；

决策9).局部重规划同时触发常规全局更新；

决策10).触发常规全局更新；

决策11).局部规划避障；

决策12).局部规划避障；

决策13).局部规划避障；

决策14).触发常规全局更新；

决策15).局部规划避障；

决策16).抵达终点。

在搭建的仿真地图中，对本发明提出的航迹规划算法进行测试对比。

(1)室内环境往返飞行效果对比：

请参阅图6，在如图所示的仿真环境中，让无人机从起点S开始分别在目标点P₁、目标点P₂之间做往返运动，即路线为S-P₁-S-P₂-S得到的无人机轨迹被记录如下。左图为EGO-Planner项目的运行结果，右图为本发明提出的方法的运行结果。

从图6左图可以看到无人机在反复折返，直观原因是无人机的视野有限，在局部无法获取足够的信息，但根本原因是全局路径不能起到有效引导作用，导致无人机局部规划求解不成功，最终造成了碰撞墙壁的事故发生。

仿真对比显示，采用本发明提出的航迹规划策略提升了无人机飞行的安全性和成功率，极大改善了无人机因全局路径的不当指引而局部重规划失效的情况。

(2)图6所示搭建的60m×60m×2m室内仿真环境中，无人机参数设置、起始点等均相同的情况下，在环境中随机选取二十组需要跨越障碍的起终点，每组使用两种方法分别运行10次，无人机安全抵达终点记为成功，同时记录单次局部优化平均耗时。本发明提出的航迹规划方法和EGO-Planner项目对比如下：

表1本发明与对比算法的性能对比

综上所述，本发明一种未知环境下四旋翼无人飞行器航迹规划方法及***，针对复杂未知环境中无人机过度依赖初始全局轨迹，导致局部求解航迹失效，进而引发安全事故的问题，提出了全局和局部双层重规划的方法，引入创新策略保证无人机自主决策正确性，在保证实时性的同时轻量地提升了无人机的安全飞行能力。具有以下特点：

1)通过引入无人机有效视野率γ来控制紧急状态下全局规划的触发。当无人机视野被障碍完全遮挡时，通过触发全局轨迹，控制无人机沿选定的安全空间大的“障碍边界”飞行，有效避免了无人机陷入无限重规划的危险状态。

2)结合局部环境信息更新全局轨迹。在无人机飞行过程中，将可见范围内的安全点作为全局路径点之一进行全局规划。该点是局部优化求得的航迹点，包含实时的环境信息，将其引入全局航迹提升了全局指引路径的可靠性。在长走廊等大型障碍环境中，和浙大的EGO-Planner相比，将无人机的决策成功率从20％提升到95％。

3)全局规划采用的最小化snap求解方式存在闭式解，不影响整体耗时，使用全局重规划在保证实时性的同时提升了无人机在未知环境中的飞行安全性和成功率。

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

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

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

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

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

Claims (10)

1.一种未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，包括以下步骤：

S1、将当前选取的全局安全点的状态信息作为输入，以最小化snap作为优化目标，利用基于最小化snap的多项式优化得到全局航迹，初始的全局安全点是起点和终点；

S2、在飞行过程中，触发更新步骤S1得到的全局航迹，通过引入新的全局安全点实现全局航迹的重规划；

S3、在步骤S2的全局航迹上，以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；

S4、对步骤S3中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。

2.根据权利要求1所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，步骤S1中，基于最小化snap的多项式表示四旋翼无人飞行器轨迹，无人机轨迹由分段多项式组成，第i个多项式段表示为：

f_i(t)＝p_i，nt^N+p_i，n-1t^N-1+…+p_i，0

其中，p_i，0，p_i，1，…，p_i，n是多项式系数，f_i(t)为时刻t时轨迹上的位置。

3.根据权利要求1所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，步骤S1中，优化目标Min S(T)为：

其中，S_i(T)为第j段多项式的总和的snap代价，

为第i段多项式的系数矩阵，P_i为

矩阵的转置，Q_i是积分所得的包含时间变量的矩阵。

4.根据权利要求3所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，第j段多项式的总和的snap代价S_i(T)为：

其中，T_i和T_i-1为第i段多项式的时间区间点，f_i ⁴(t)为无人机第i段轨迹的snap，n、v为多项式的次数，p_i，n、p_i，v为第i段多项式的系数。

5.根据权利要求1所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，步骤S2中，全局航迹的重规划包括紧急全局航迹重规划和常规全局航迹重规划；

紧急全局航迹重规划具体为：

当有效视野率γ为0时，代表无人机视野内全是障碍物，触发紧急全局航迹重规划，用相邻障碍边界点之间的方向向量表示障碍物走向，此时无人机增加视野有效率，沿障碍向左或者向右，当无人机当前位置点P_o到无人机视野最左端分界线与障碍物的交点A_l的距离d_uo1大于无人机当前位置点P_o到无人机视野最右端分界线与障碍物的交点A_r的距离d_uo2时，选择障碍物的左方向作为安全方向，当d_uo1＜d_uo2时，选择障碍物的右方向作为安全方向；当||d_uo1-d_uo2||≤εRs时，如果cosα1＞cosα2，选择障碍物的左方向作为安全方向，反之，选择障碍物的右方向作为安全方向，ε是放缩系数，R_s为无人机的可见视野半径，α₁为无人机和目标点之间的方向向量与无人机视野左边界方向之间的夹角，α₂为无人机和目标点之间的方向向量与无人机视野右边界方向之间的夹角；选择安全方向后，在安全方向上按设定距离R_add选择路径点P_sdd作为新的全局安全点，和当前的无人机位置点P_o，目标点P_s，三个一起作为安全路径点，生成以最小化snap为优化目标的全局轨迹，实现无人机的方向扭转和局部目标的修正；

常规全局航迹重规划具体为：

当有效视野率γ不为0时，选择无人机走过的距离信息为触发条件，每当无人机执行路径L_a达到起点终点间总距离L_A的设定比例时，触发一次全局航迹规划，在进行常规全局航迹重规划时，选择无人机局部视野范围内的最远安全点作为新的全局路径点P_add进行添加。

6.根据权利要求5所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，常规全局重规划情况新增的全局路径点P_add1满足条件如下：

其中，

为局部航迹曲线，R_s为无人机的可见视野半径。

7.根据权利要求5所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，紧急全局重规划情况新增的全局路径点P_add2为：

其中，

为紧急全局航迹重规划的要求下确定的安全方向向量。

8.根据权利要求1所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，步骤S3中，依据无人机的规划距离R_p在全局航迹上选取局部目标点，基于约束条件进行无人机局部航迹的优化求解，以生成可行的局部轨迹，优化问题如下：

minJ＝λ_sJ_s+λ_cJ_c+λ_dJ_d

其中，J_s代表光滑惩罚，J_c代表碰撞惩罚，而J_d代表动力学可行项惩罚，λ_s，λ_c，λ_d为各惩罚项的权值。

9.根据权利要求1所述的未知环境下四旋翼无人飞行器航迹规划方法，其特征在于，步骤S3中，如果求解不成功，则继续进行求解，当需要进行重规划时进行局部航迹重规划，在抵达终点时停止执行，局部航迹重规划的条件如下：

无人机距离此次规划的起点距离大于设置的最小规划威胁距离r_plan或者在当前的局部航迹基础上无法成功避障；最小规划威胁距离r_plan意味着在一次局部规划后，无人机在开始执行轨迹的前一段时间不进行重规划检查，实时的局部轨迹在规划时直接执行，而超过设定距离后进入重新规划轨迹状态；重新规划轨迹状态能够在当前点和局部目标点之间重新求解安全轨迹，得到的轨迹可行进入执行状态，否则将持续进行求解。

10.一种未知环境下四旋翼无人飞行器航迹规划***，其特征在于，包括：

初始模块，将当前选取的全局安全点的状态信息作为输入，以最小化snap作为优化目标，利用基于最小化snap的多项式优化得到全局航迹，初始的全局安全点是起点和终点；

触发模块，在飞行过程中，触发更新初始模块得到的全局航迹，通过引入新的全局安全点实现全局航迹的重规划；

规划模块，在触发模块的全局航迹上，以规划距离R_p选取点作为局部规划的目标点，基于约束条件进行局部航迹的优化求解，以生成可行的局部轨迹，若求解成功，执行规划得到的无人机安全飞行航迹，将生成的轨迹点位置和偏航角信息发给控制器执行，完成航迹规划；

更新模块，对规划模块中需要重规划的局部轨迹进行更新求解，得到安全可行的无人机航迹则执行所得航迹。

Images