多约束的无人机动态路径规划算法研究_路径规划算法 约束

多约束的无人机动态路径规划算法研究

无人机的自主飞行是无人机相关研究的重点方向，如何在复杂环境中快速分析环境，并规划一条安全可行的路径，是该方向的研究目标。

针对传统路径搜索算法存在的路径不平滑问题，采用三阶B样条曲线进行预规划航迹。在欧式有符号距离函数（ESDF）地图提供的梯度信息的基础上，分别在平滑、碰撞和可行性上设计约束方程，实现轨迹动态重规划。

针对路径动态更新中，时间间隔变化产生的控制点不再符合约束的问题，采用各向异性曲线拟合方法，实现时间再分配，保障在动态更新路径的过程中，新产生的路径与原路径相似且具有同样的可行性。

实验证明，该算法实现了无人机的自主路径规划与优化，能够进行动态避障，面对复杂环境具有鲁棒性。

附赠自动驾驶最全的学习资料和量产经验：链接

00  引言

多旋翼无人机具有体积小、机动性高、速度快的特点，在航拍、搜救、巡检工作中具有广泛应用。

得益于智能移动设备的发展，斯坦福大学通过在无人机上搭载微机电系统（MEMS）传感器加嵌入式机载电脑的方式[1]，首次实现了无人机的自主飞行，此后，无人机的自主飞行研究迅速发展。无人机在自主飞行过程中，最重要的功能是路径规划与避障，即实现导航。与机器人相似，导航的目的在于找到从起点到终点具有避障能力的最优或次优路径[2]。

现实中的环境是复杂多变的，无人机在飞行的过程中除了要面对静态的障碍物如树木、灯柱等，还需要面对动态的障碍物如飞鸟和其他无人机等，如何使无人机在动态环境中实现灵活的自主避障，是研究的重要方向。

本文对基于轨迹优化的动态重规划方法进行研究，采用利于重规划的高阶B样条曲线，结合提出的三项飞行约束方程，动态重规划路径。并采用基于时间再分配策略，实现无人机动态避障，解决动态重规划路径算法的实时性。

01  算法流程

无人机要实现自主飞行，首先需要通过感知系统估计出无人机的状态信息，以及周围的环境地图[3]。传统的做法是将障碍物的点云图收集并转换为在三维地图中的占用栅格地图（Occupied Grid Map），然后通过路径搜索算法计算无人机的可通行路径[4]。但这类地图在路径规划算法的应用过程中效率较低，为了更高效获取环境信息，采用了包含了梯度信息的ESDF地图。基于梯度的运动规划是当前主流的无人机局部路径规划方法，该方法将问题表述为无约束线性规划。N.Ratliff等人率先将ESDF地图引入机器人的路径规划[5]，实现了ESDF地图使用上的突破。沿着这一思路，后续研究中，许多框架直接利用梯度信息进行优化配置空间中的轨迹[6]，有非常好的效果。

在路径搜索这一问题上，A*算法是一种经典的路径搜索算法，在使用A*算法进行路径规划时，虽然能找到最短路径，但A*算法不考虑无人机的动力学可行性，在实际飞行过程中，无人机可能会面临危险[7]，且在面对动态环境时，A*算法并不适用[8]。参考文献[9]，采用高阶B样条曲线进行路径规划。该方式的优点在于预规划的路径更为平滑，且有助于动态重规划。为了实现动态重规划，参考文献[5-6]，就平滑、碰撞、可行性创建惩罚项，以此设计约束方程，对轨迹进行动态规划。

然而，部分动态规划算法都会将时间这一维度抽离[10]，用离散的时间进行轨迹优化。但这样的方式并不适用于无人机，因为它对动态约束更加敏感。为此，H.Oleynikova等人提出了一种用于无人机规划的连续时间多项式轨迹优化方法[11]。文章借鉴这一思想，设计时间再分配策略。

文中的路径规划算法采用B样条曲线进行预规划路径，根据飞行约束方程，结合ESDF地图提供的梯度信息，实现路径搜索。在飞行的过程中，结合时间再分配策略，进行无人机飞行轨迹的动态规划，实现动态避障。算法的流程图如Fig.1所示。

02  算法实现

2.1 ESDF地图

自21世纪以来，ESDF已被用于从传感器中过滤嘈杂的数据，并构建对象[12]，而Ratliff等人带来了新的思路，将ESDF用于机器人的运动规划[5]。ESDF在运用初期，不适用于增量构建，而在四旋翼无人机飞行过程中，需要不断对场进行动态更新。为此，Han[13]等人提出的增量ESDF生成方式，实现并验证了ESDF的动态更新。

ESDF源于TSDF(Truncated Signed Distance Functions)，TSDF对距离信息进行了截断，即远离障碍物曲面一定距离后，视为无限大。这样的处理方式导致路径信息在穿过无限大的区域时，无法获取梯度信息。当控制点在障碍物里面时，无法被排斥出来。ESDF不对距离信息进行截断，可以很方便地对当前位置到障碍物表面的距离和梯度信息进行查询。采用港科大的开源工具FIESTA的原理[13]，可以快速生成并动态更新ESDF地图。其工作流程如Fig.2所示。

第一步，由传感器获取周围环境的位置信息和深度信息。

第二步，使用光线追踪法将点云叠加到占有栅格地图中，然后将所有占用状态发生改变的体素分别添加到insert Queue和delete Queue两个队列中。

第三步，初始化ESDF，将前两个队列合并到update Queue队列中，并使用基于广度优先搜索算法的ESDF更新算法更新所有可能更改的体素。

2.2 B样条曲线

传统寻路算法如A*算法等，在生成路径时，部分路径段存在尖锐拐点。为了保障无人机飞行的安全性，采用B样条曲线进行路径优化可获得平滑路径[14]。B样条曲线的核心在于节点的划分，在无人机的路径规划中，B样条曲线的节点就是一个个的控制点。控制点作为决策变量，影响一段曲线的形状。B样条曲线的数学表达式如公式（1）。

其中是次B样条基函数，又称调和函数，是一个递归函数，其递归表达式如公式（2）。

式中，