区域快速优化下的无人机在线轨迹规划方法

唐嘉宁，和雪梅，陈云浩，彭志祥，周思达

（1.云南民族大学电气信息工程学院，昆明 650000；2.云南民族大学无人自主系统研究院，昆明 650000）

0 引言

近年来，随着无人机在地震废墟、火灾现场、崎岖山林等未知复杂环境下进行救援救灾等应用的需求急剧增大，对无人机的自主导航提出了更高的要求。为确保无人机在未知环境下高速飞行时快速应对不可预见的风险，自主导航中在线轨迹规划模块至关重要。

在线轨迹规划问题已经被广泛研究［1－3］，其中基于梯度的规划方法具有成功率高、规划速度快等突出优势，逐渐成为无人机在线轨迹规划的主流方法［4］。该方法通常依赖特定的环境预处理方法将空间中的障碍物信息引入非线性优化问题中，并在平滑度、安全性和动态可行性之间进行权衡［5］。Ratliff等［6］提出利用预先构建的ESDF［7］提供障碍物梯度信息，通过协变梯度下降算法来最小化初始轨迹的平滑度和碰撞成本，进而优化离散时间轨迹；Oleynikova等［8］将文献［6］的工作扩展到连续时间轨迹优化，以避免微分误差，并采用随机轨迹扰动和优化重启，以实现实时避碰和获得更高的成功率，然而，该方法带来了沉重的计算负担；Usenko等［9］提出了实时局部轨迹重规划的方法，在避开未建模障碍物的同时使重规划的局部轨迹接近全局轨迹，并将该轨迹参数化为B样条，充分利用其凸包性质提高了优化性能；Zhou等［10］进一步利用了B样条的凸包性质，使轨迹优化效率和鲁棒性得到提升，并通过提供一条无碰撞的高质量初始轨迹，显著提高了在线规划的成功率，同时引入几何引导路径缓解了局部极小值的问题［11］；Tang等［12］基于均匀B样条曲线和非均匀动力学搜索算法，能够在保证安全的前提下，实现高效、平滑和安全的轨迹规划，但该方法在实时生成轨迹时需要有强大的计算资源和性能作为支撑。

上述工作中，在线或离线生成的ESDF，为机器人的轨迹规划和避障提供精确的障碍物距离信息和梯度信息［13－16］，被广泛应用于大多基于梯度的在线轨迹规划方法中。然而，ESDF的非光滑特性及空间离散性，使得优化过程的收敛速率和迭代的计算速率明显下降，且所构建地图中仅有部分空间直接服务于轨迹优化，构建时间却占据整体规划时间的70%［17］。随着环境规模及复杂性的增长，ESDF计算障碍物梯度信息需要消耗大量的计算资源和存储空间［18］。对于机载资源受限的无人机来说，这会极大限制其规划效率，甚至无法满足实时轨迹规划的需求。Zhou等［17］首次提出了一种无需构建ESDF地图即可实现基于梯度的轨迹规划方法，然而该方法在优化时需要进行多次迭代才能找到一条安全、平滑、动态可行的轨迹，耗费了宝贵的规划时间。

针对这个问题，提出了区域快速优化下的无人机在线轨迹规划方法，通过碰撞控制点替换，快速优化初始轨迹中的碰撞区域轨迹以避免多次迭代优化，并设计了一种轻量的方法计算轨迹与必要障碍物的距离和梯度信息，避免与轨迹规划无关区域的计算。在此基础上，建立多目标优化函数，进一步优化轨迹以获得满足安全、平滑、动态可行的高质量轨迹。

1 系统概述

无人机在线轨迹规划一般建立在状态估计和体素建图模块上，更新的地图和无人机的位姿信息被馈送到轨迹生成模块以生成初始轨迹；之后进入轨迹优化模块生成最佳轨迹，并发送给轨迹服务器，由相应飞行控制器实现对无人机的控制。所提出的区域快速优化下的无人机在线轨迹规划方法，通过设计更高效的轨迹优化策略，快速将初始轨迹优化为满足安全性、平滑性、动态可行性的高质量轨迹，其框架如图1所示。

图1 无人机在线轨迹规划系统框架图

轨迹优化分2个阶段：第1阶段为碰撞区域的轨迹快速优化，对输入的初始轨迹进行碰撞检测，生成碰撞轨迹区域对应的安全引导路径，设计碰撞控制点替换策略，快速优化该区域轨迹；第2阶段为轨迹多目标优化，通过构建局部障碍物梯度及计算碰撞成本为进一步轨迹优化做准备，之后以轨迹的平滑性、安全性和动态可行性为目标进行优化，得到最终轨迹。

2 碰撞区域的轨迹快速优化

2.1 轨迹表示与碰撞区域轨迹检测

在无人机轨迹规划中，需要预先规划从指定起点到目标点的初始轨迹。研究中采用直线化初始轨迹，确定起点和目标点的位置，通过计算它们之间的距离和方向来确定直线的方向和长度。用直线连接起点和目标点的过程可以被视为一种最简单的初始轨迹规划方法，具有计算简单、实现容易、快速响应等优点。

由于存在不可预见的障碍物或其他风险因素，在未知环境下需要保证无人机能够快速、高效地进行局部规划以应对风险。为将局部规划转换为初始轨迹局部快速优化问题，使用均匀B样条进一步参数化初始轨迹，根据当前无人机的运动状态和环境信息，对初始轨迹上的控制点进行快速优化，使无人机能够安全、高效地通过当前局部环境。因此，将初始轨迹参数化为1组均匀B样条曲线的控制点：

式中：Qi为轨迹优化的决策变量，并记录独自的环境信息；Nc为控制点的数目。

为保证无人机飞行的安全性，在初始轨迹上不断进行碰撞检测，如图2所示。使用1对控制点Qin和Qout来记录每段碰撞区域（灰色区域）轨迹的首末位置，找到由碰撞控制点组成的“碰撞集”Qcol，对应为图2中的黑点。之后使用A*路径搜索算法来搜索最优路径，即寻找1条从Qin到Qout的安全引导路径，对应为图2中的蓝色曲线，并得到路径点集A，对应为图2中的蓝点。

图2 碰撞区域轨迹检测

2.2 碰撞控制点替换策略

为将“碰撞集”Qcol中的每个碰撞控制点Qi以最快的速度、最短的距离推离当前障碍物，提出碰撞控制点替换策略，即寻找在路径点集A中每个碰撞控制点对应的路径点AQ，之后进行替换操作，并作为新的控制点Qinew，如图3所示。进行替换的过程中，只调整了初始轨迹上的碰撞控制点的位置，最大限度减少了对整个轨迹的影响，能够更加灵活地调整碰撞区域轨迹，实现快速轨迹优化。

图3 碰撞区域轨迹快速优化

图3中，灰色为障碍物，黑色曲线为均匀B样条表示的初始轨迹，黑色虚线为初始轨迹与障碍物形成的碰撞区域轨迹，黑点为“碰撞集”Qcol中的控制点，蓝点为碰撞控制点策略寻找到的新控制点Qinew，红色曲线为碰撞区域轨迹快速优化后的轨迹。

寻找碰撞控制点对应的路径点示意图如图4所示。其中，黑色曲线为初始轨迹，蓝色曲线为A*算法生成的安全引导路径，黑点和灰点为碰撞控制点，蓝点为安全引导路径的路径点，紫点为安全引导路径的路径点总数目一半的位置，红点为最终寻找到的碰撞控制点对应的路径点，Ri为碰撞控制点Qi的切向量灰色为垂直于切向量的平面。

图4 寻找碰撞控制点对应的路径点示意图

生成的安全引导路径的路径点集A中的路径点用Ai表示，QAi为Qi与Ai构成的向量，见图4中的紫线，即：

当QAi与Qi的切向量Ri的内积为0时即可得到Qi对应的路径点AQ，相当于寻找垂直于Ri方向上的路径点AQ，如图4中的红线所示。为了避免Qi在路径点集A中一一寻找AQ而消耗大量的算力，每个碰撞控制点Qi从路径点集A中路径点总数目一半的位置索引am开始进行寻找，即第1次寻找路径点AQ时，路径点Ai的位置索引ai＝am。

判断Ri·QAi的值后，根据式（3）来决定下一个寻找AQ路径点的位置索引ai＋1，当Ri·QAi的值大于0时，下一个寻找的路径点的方向如图4（a）所示；当Ri·QAi的值小于0时，下一个寻找的路径点的方向如图4（b）所示；当Ri·QAi的值等于0时，就可得到碰撞控制点Qi对应的路径点AQ。

3 轨迹多目标优化

3.1 局部障碍物梯度及碰撞成本

在未预先构建ESDF的情况下，障碍物由占用栅格地图上的体素表示，不能够直接获得轨迹优化的碰撞成本和梯度，因此利用碰撞区域快速优化后的轨迹上的控制点Qinew和其对应在障碍物表面的基点来估计轨迹与局部障碍物碰撞的成本和梯度。

采用A*算法生成安全引导路径具有贴近障碍物表面的特点，且研究只对相对短的局部障碍物进行梯度信息构建，因此能够快速完成所有Qinew对应在障碍物表面的基点的搜索。通过以Qinew为初始位置，朝原始位置Qi方向以占用栅格地图的分辨率为步长快速搜索到第1个处于占用状态的栅格，如图5（a）所示，即为轨迹上控制点Qinew对应在障碍物表面的基点pi，i为控制点的索引。以基点pi与碰撞区域优化后轨迹上的控制点Qinew间的排斥方向向量vi来构建梯度信息，该方向为梯度的方向，如图5（b）所示，pi与vi是一一对应的关系，简称｛p，v｝对。

图5 局部障碍物梯度信息的构建

轨迹与局部障碍物的碰撞成本为控制点Qinew的碰撞成本累加，即：

式中：ccollision（Qinew）为控制点Qinew对应的｛p，v｝对碰撞成本函数。

式中：dthr为无人机飞行的安全距离阈值，若距离小于该阈值则碰撞成本将开始快速升高；di为控制点Qinew到对应基点pi的距离。通过Ccollision对Qinew进行求导，即可获取梯度值［19］：

由于只在轨迹遇到新障碍时进行障碍物碰撞成本与梯度的估计，因此整个规划过程只提取了必要的障碍物信息，减少地图信息冗余的同时使规划更加精确、高效。

3.2 多目标优化函数设计

无人机最终的飞行轨迹必须同时满足安全性、平滑性、动态可行性，因此优化如下综合三者的目标优化函数：

式中：fsafe为安全目标函数对应为轨迹的碰撞成本Ccollision；fsmooth为轨迹平滑目标函数；fdynamic为动态可行性目标函数；αsafe、αsmooth、αdynamic分别为对应目标函数的权重系数。

平滑目标函数的设置是为了优化轨迹的平滑性以达到节省无人机飞行时能量的目的。根据无人机动力学的微分平坦性［20］，可通过对无人机的加速度加以约束来实现，轨迹平滑目标函数可构造成如下形式［10］：

其中无人机的加速度为轨迹上控制点的速度，可通过求B样条曲线的高阶导数获得，即：

动态可行性目标函数是为保证无人机的速度、加速度在可行的范围内。受益于B样条的凸包性质，如果控制点的速度、加速度在控制点处不超过极限值，那么该B样条曲线上的任意一点的速度、加速度将小于极限值，从而满足动态可行性。因此，在给定最大速度vmax、最大加速度amax的情况下，该函数可以定义为：

当速度、加速度超过限制后，使用二范数和单位归一化因子λvel、λacc作为动态可行性惩罚。通过上述目标函数的优化，获得了1条更安全、光滑、满足动态可行性的更适合无人机飞行的轨迹。

4 实验与结果分析

快速区域优化下的无人机在线轨迹规划仿真实验在Ubuntu操作系统下进行，基于ROS（robot operating system）框架和C＋＋语言实现规划设计，并通过ROS的Rviz（the robot visualization tool）软件完成规划方法在三维环境下的可视化演示。实验平台的相关配置如下：处理器为Intel i9－12900KF 4.9 GHz，内存大小为32 GB，系统版本为Ubuntu18.04 LTS，ROS版本为Melodic。

仿真操作流程如图6所示，在生成的障碍物随机分布的三维栅格地图中确定飞行目标点后，通过在线轨迹规划算法生成最佳轨迹，并发送给轨迹服务器，由相应的飞行控制器实现对无人机的控制。

图6 仿真操作流程图

实验采用表1所示的基础参数配置，在简单场景下短距离规划和复杂场景下长距离规划2种情况下对所提方法进行测试。其测试结果的数据为30次测试的平均值，其中起点与目标点的距离为D，障碍物数量为No，总规划时间为Ta（包括了建图、轨迹初始化和优化的时间），规划初始化时间和轨迹优化的时间分别为Tinit和Topt，Topt用于衡量轨迹优化的收敛速度。

表1 基础参数配置

4.1 简单场景下短距离规划的性能测试

当设置的目标点在无人机的传感器感知范围内时，可以认为是在已知环境下的轨迹规划，无人机从起点到目标点一般只需进行1次规划。同时由于无人机规划距离较短，无人机一般进行定高飞行，可视为是二维平面上的简单规划。将所提方法与前沿的未构建ESDF地图进行基于梯度的规划方法EGO＿planner（an ESDF-free gradientbased local planner for quadrotors）［17］进行比较，该方法提出近似梯度场计算方法，将初始轨迹进行迭代优化推离障碍物。2种方法设置相同的仿真参数，分别在无人机起点距离目标点4、6、8 m的位置进行3组规划实验，地图的障碍物数目为300。

所提方法在无人机起点距离目标点8 m位置的规划过程如图7所示。其中，蓝色柱状为生成的三维栅格地图中随机分布的障碍物。图7（a）为初始化过程，通过无人机起点与目标点直线化来生成初始轨径（深绿色直线），通过将初始轨迹参数化为B样条的一系列控制点后，进行碰撞区域轨迹的检测，之后在碰撞区域轨迹处由A*算法生成安全引导路径（淡绿色曲线）；图7（b）为所提方法的轨迹优化过程，红色曲线为初始轨迹中碰撞区域快速优化后的轨迹，避免多次迭代情况下通过碰撞控制点替换直接将碰撞区域轨迹快速优化至安全区域，蓝色曲线为考虑无人机飞行时的安全性、平滑性及动态可行性，设计的多目标函数进一步优化后得到的轨迹。最终生成的无人机飞行轨迹如图7（c）粉色曲线所示，该轨迹足够安全、平滑。

图7 所提方法规划过程

表2记录了2种方法在规划时间上的比较结果，包括总规划时间、轨迹初始化时间和轨迹优化时间。

表2 规划时间测试结果

由表2 可知，相比于前沿方法 EGO＿planner［17］，所提方法在轨迹规划时间上有较大提升，具体如下：在规划距离为4 m时，总规划时间、轨迹初始化时间、轨迹优化时间分别缩短了38.3%、31.7%、39.1%；在规划距离为6 m时，总规划时间、轨迹初始化时间、轨迹优化时间分别缩短了36.1%、30.7%、38.5%；在规划距离为8 m时，总规划时间、轨迹初始化时间、轨迹优化时间分别缩短了34%、36.9%、35.5%。所提方法提出的碰撞控制点替换策略能够实现碰撞区域轨迹的快速优化，避免多次迭代，同时设计的目标函数加速了轨迹优化收敛的过程，因此所提方法的规划时间小于EGO＿planner方法，规划效率得到了较大提升。

4.2 复杂场景下长距离规划的性能测试

当设置的目标点较远且处于未知环境时，无人机生成的轨迹遇见新障碍时将不断进行重新规划以确保轨迹安全，因此从起点位置到目标点需要进行多次规划。同时障碍物数量是模拟真实场景复杂性的关键参数，通常障碍物数目越大，环境越复杂，对在线轨迹规划算法的性能要求越高。为此，在随机森林三维栅格地图中，给定相同的起点（－15.5，0，1）和目标点（15.5，0，1），规划距离为31 m，其他参数设置相同的情况下，依次设置障碍物数目为180、270、360，其中随机生成圆环和柱状障碍物的数量分别占据地图中障碍物总数的1／3和2／3。由于无人机规划距离较长，在三维空间内将进行较复杂的规划，因此该部分的性能测试可以用于验证所提方法在三维环境中的有效性。

将所提方法与前沿方法EGO＿planner和经典的基于梯度的规划方法Fast＿planner［10］进行规划比较，结果见表3，轨迹效果如图8所示。

表3 算法对比结果

图8 轨迹规划效果图

由表3可知，3种方法的规划时间随着地图中障碍物数目的增大而增大。Fast＿planner利用ESDF来获取地图中的障碍物距离信息和梯度信息，而所提方法和EGO＿planner不进行ESDF的构建，只计算当前轨迹面临的新障碍物的距离和梯度信息，因此相比Fast＿planner，所提方法极大减少了轨迹规划的时间。在总规划时间上，相比Fast＿planner，障碍物数目为180时，所提方法耗时缩短了87.64%；障碍物数目为270时，耗时缩短了86.63%；在障碍物数目为360时，耗时缩短了85.41%。总之，所提方法的规划时间远远小于Fast＿planner方法，规划效率得到了极大提升。

障碍物数目越大，所提方法较EGO＿planner的总规划时间缩短越多，优势越大。这是因为EGO＿planner在轨迹优化期间需要不断迭代来获得最终可执行的轨迹，但随着地图中障碍物数目的增加，环境规模及复杂性增长，迭代优化轨迹次数增加，影响了整体规划效率；而所提方法能够实现碰撞区域轨迹的快速优化，避免多次迭代，加速了轨迹优化的效率。

上述实验结果表明，所提方法在不同复杂场景中都显示了在线规划的有效性，且相比于前沿方法EGO＿planner和经典的基于梯度的规划方法Fast＿planner，在规划时间上存在明显优势。随着地图中障碍物数目的增大，所提方法能够快速高效实现碰撞区域轨迹的优化，避免在障碍物中进行多次迭代优化，极大降低了规划的计算时间。

5 结论

提出了区域快速优化下的无人机在线轨迹规划方法，在不构建ESDF情况下，通过快速优化碰撞区域的轨迹、有效构建局部障碍物梯度信息和设计多目标优化函数来高效优化轨迹，极大降低轨迹规划时间。

实验结果表明，该方法相比于现有算法在规划时间上有明显提升：在简单场景下短距离规划中，与前沿方法相比，总规划时间、轨迹初始化时间、轨迹优化时间分别缩短了36.1%、33.1%、37.7%；在复杂场景下长距离规划中，较经典的基于梯度的规划方法，轨迹规划时间平均缩短了86.56%，规划效率极大提升，较前沿的基于梯度的规划方法，随着环境的复杂度变高，轨迹优化的效率进一步提高。同时，所提方法在不同复杂环境中能进行有效的在线规划，具备较强的鲁棒性和扩展性。