[发明专利]一种非传统欧式距离场的四旋翼无人机轨迹规划方法

说明书

本发明公开了一种非传统欧式距离场的四旋翼无人机轨迹规划方法。包括以下步骤：首先忽略初始轨迹与障碍物的碰撞，建立仅满足端点约束的标准B样条曲线，同时对每一个在当前碰撞段中控制点赋予一个锚点，利用增强RRT*算法得到原本在障碍物里面的轨迹的无碰撞路径并计算罚函数的碰撞项罚函数，并用此来估计梯度信息；利用无碰撞路径和罚函数计算无碰撞轨迹；判断该轨迹是否满足动力学约束，若不满足则对初始轨迹进行时间重新分配操作；通过重新分配的时间和构造的曲线拟合项的作用，生成满足动力学约束且能够高度拟合原来轨迹的B样条曲线。本发明方法可以大大减少规划无人机轨迹时的计算量，并且提高了轨迹计算的准确性，减少无人机飞行时的碰撞情况。

技术领域

本发明涉及无人机轨迹规划领域，具体涉及一种非传统欧式距离场的四旋翼无人机轨迹规划方法。

背景技术

近年来，四旋翼无人机的实时规划算法极大的推动了航空器的发展，无人机已经从实验室飞向了无数的实际应用场景，其中有一部分方法利用了梯度信息，平滑轨迹的同时避开了路径上面的障碍物，这些方法性能出众而得到了越来越广泛的应用。

基于梯度的运动规划方法是无人机局部路径生成的主流方法。很多规划框架都利用了欧式距离场(ESDF)其充足的梯度信息来在构形空间中优化轨迹。尽管如此，离散时间的轨迹优化对动力学约束非常敏感，因此并不适合无人机。而后有人提出了一种连续时间的多项式轨迹优化算法。然而，此算法涉及到了对可能性函数进行积分，计算量非常大，成功率也不高，即使是在随机场景下也只有百分之七十左右。为了解决这些缺点，提出了一种利用B样条曲线来对轨迹进行参数化的方式，这种方式利用了B样条曲线的凸包特性。在上述方法中，ESDF梯度的大小和方向在距离估计中发挥了重要作用。

传统上，基于梯度的算法依靠一个事先计算好的欧式距离场地图来估计梯度的方向和大小，并通过数值优化的方式生成一个局部最优的轨迹。尽管优化算法收敛的很快，但是在这之前，建立ESDF地图是件很困难的事情。根据测量数据显示，在整个生成轨迹的过程中，建立ESDF耗费了65％以上的计算时间，而且在构建ESDF的过程中有大量的计算对路径规划问题并无帮助，所以说当前基于ESDF的算法并不能直截了当的帮助求解路径规划问题。

发明内容

针对现有技术存在的上述问题，本发明要解决的技术问题是：建立ESDF地图来反应梯度信息的做法，需要大量的数据建模与计算，这样过多冗余或无用的计算数据增加了算法的计算量，同时也降低了对轨迹规划时的准确性。

为解决上述技术问题，本发明采用如下技术方案：一种非传统欧式距离场的四旋翼无人机轨迹规划方法，包括如下步骤：

S100：选取一条标准B样条曲线Φ经过障碍物但忽略对障碍物的碰撞，该标准B样条曲线Φ满足端点约束，在该标准B样条曲线Φ上每隔相同时间Δt设置一个控制点Q，且每个控制点Q拥有独立的环境信息；

S200：将标准B样条曲线Φ上处于障碍物里面的控制点记为Q_i，且每个Q_i被赋予一个对应的锚点P_ij，每个锚点P_ij位于障碍物表面并对应一个斥力向量V_ij，即一个{P，V}对，每个{P，V}对只属于一个控制点；其中，i表示障碍物里面标准B样条曲线Φ上的第i个控制点，j表示第j个锚点；

S300：对于标准B样条曲线Φ处于障碍物里面的部分，使用增强RRT*算法计算起始控制点和终止控制点之间的曲线得到该段曲线对应的局部无碰撞路径，该起始控制点和终止控制点分别是指最靠近障碍物且未在障碍物内部的控制点，该无碰撞路径包含了每个控制点对应的{P，V}信息；

S400：计算罚函数，所述罚函数由碰撞项罚函数、光滑项罚函数和动力学约束项罚函数组成；

利用标准B样条曲线Φ和罚函数计算生成满足B样条曲线要求的无碰撞轨迹，该无碰撞轨迹的表达式如下：