[发明专利]一种基于弯曲柱坐标系的局部平滑轨迹规划方法

说明书

一种基于弯曲柱坐标系的局部平滑轨迹规划方法，无人载具轨迹规划技术领域。本发明解决了现有三维空间内无人载具局部路径规划方法轨迹连续性和平滑性差，且轨迹约束难的问题。本发明根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径；以全局路径为基线，创建弯曲柱坐标系；将无人载具的运动状态信息由全局坐标系转换到弯曲柱坐标系中；根据全局路径，利用代价函数和时间空间离散化对未来时间T内，无人载具的局部运动轨迹进行规划；删除与障碍物碰撞的和超出无人载具动力约束范围的局部运动轨迹，将代价函数值最低的局部运动轨迹作为局部规划的最优运动轨迹。本发明适用于无人载具的局部运动轨迹的规划。

技术领域

本发明属于无人载具轨迹规划技术领域。

背景技术

近年来，三维空间内无人载具，例如，小型无人潜艇、无人机等由于其模型简单、制作方便、响应迅速、机动性好、成本低廉等优点，吸引力一大批国内外研究者的关注。然而在复杂的三维空间中，实时规划出能成功避障、轨迹平滑且运动状态不突变的局部路径一直是实现无人载具高动态运行的主要研究课题之一。保证三维空间内既要快速平稳的通过事先未知障碍的长直区域，又要成功准确的避开障碍到达目标点仍然是一件十分具有挑战性的工作。本工作聚焦于在已知全局路径的情况下获得最优的局部路径的方法，本质为最优化问题。

三维空间内无人载具任务层面的规划与控制往往可以分成三个层次：全局路径规划、局部路径规划、局部路径跟随。三个层次由高到低、由抽象到具体：全局路径规划(Global path planning)在整个任务规划的最高层，需要在无人载具运行之前根据现有的任务以及地图等约束信息获取到一条由起点到终点的平滑的路径(path)；局部路径规划(Local trajectory planning)在整个任务规划的第二层，局部路径规划在已经获得的全局路径的基础上，再结合三维空间内无人载具上携带的传感器实时观测到的周围环境的信息，实时规划出一条条符合当前环境约束以及无人载具动力学约束的轨迹(trajectory)；局部路径跟随(Path tracking)在无人载具任务规划与控制的最下层，也是最具体的一层，无人载具要根据自己当前的状态以及上一层获取到的局部轨迹，使用基于模型或无模型的控制率，实现以较低的偏差跟随局部路径。在整个任务规划与控制的过程中，以上任何一个层次都是现今的研究重点，而局部路径规划位于其中承上启下的位置，更是其中的难点，既要保证规划出的局部轨迹满足某种程度的最优(速度、曲率、平滑等)，又要保证路径的可行性(避障、满足动力学等)。

目前局部路径规划的方法可分为基于自主学习的和非自主学习类，其中基于深度学习的端到端的规划与控制方法以及基于强化学习的算法虽然近年来在理论上得到了长足的发展，但无论是安全性、可靠性还是稳定性都有很大的问题，对于未知的环境更是根本无法使用；非自主学习类算法中，可分为基于搜索的规划算法和基于采样的规划算法。基于搜索的规划算法主要的代表者是A*算法和Dijkstra算法，这两种算法在很多场合都有很好的应用，尤其在游戏和汽车导航等领域，但是它们规划时间长、消耗算力大，并不能很好的应用在障碍位置未知的区域进行高动态运行的无人载具上；基于采样的规划算法主要的代表者是RRT算法和PRM算法，但是三维空间中采样的随机性更大，而且得到的路径不但不能保证最优，而且会出现很多毛刺和抖动，超出无人载具的模型约束和动力学范围。

为解决上述问题，出现了一类更加适合三维空间内无人载具路径规划的特殊曲线拼接的方法：基于平滑连接直线和圆弧的路径规划算法能根据无人载具的初始状态和目标配置轻易的得到一条平顺的路径，但是得到的路径灵活性差，直线和圆弧的连接处三维空间内无人载具的向心加速度有突变，对后面无人载具的路径跟随要求更高；基于Bezier曲线的路径规划算法得到的路径更加“智能化”，它的四个控制点均可以编辑，但是Bezier曲线拼接时，满足几何连续性条件是比较困难的，难以对其进行进一步的优化；基于B样条曲线的路径规划算法能得到平滑拼接的路径，但是B样条曲线的曲率表示困难，不容易保证、三维空间内无人载具的曲率约束条件。

发明内容