技术摘要：
一种基于弯曲柱坐标系的局部平滑轨迹规划方法，无人载具轨迹规划技术领域。本发明解决了现有三维空间内无人载具局部路径规划方法轨迹连续性和平滑性差，且轨迹约束难的问题。本发明根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径；以全局路径为基线，创  全部
背景技术：
近年来，三维空间内无人载具，例如，小型无人潜艇、无人机等由于其模型简单、制 作方便、响应迅速、机动性好、成本低廉等优点，吸引力一大批国内外研究者的关注。然而在 复杂的三维空间中，实时规划出能成功避障、轨迹平滑且运动状态不突变的局部路径一直 是实现无人载具高动态运行的主要研究课题之一。保证三维空间内既要快速平稳的通过事 先未知障碍的长直区域，又要成功准确的避开障碍到达目标点仍然是一件十分具有挑战性 的工作。本工作聚焦于在已知全局路径的情况下获得最优的局部路径的方法，本质为最优 化问题。 三维空间内无人载具任务层面的规划与控制往往可以分成三个层次：全局路径规 划、局部路径规划、局部路径跟随。三个层次由高到低、由抽象到具体：全局路径规划 (Global  path  planning)在整个任务规划的最高层，需要在无人载具运行之前根据现有的 任务以及地图等约束信息获取到一条由起点到终点的平滑的路径(path)；局部路径规划 (Local  trajectory  planning)在整个任务规划的第二层，局部路径规划在已经获得的全 局路径的基础上，再结合三维空间内无人载具上携带的传感器实时观测到的周围环境的信 息，实时规划出一条条符合当前环境约束以及无人载具动力学约束的轨迹(trajectory)； 局部路径跟随(Path  tracking)在无人载具任务规划与控制的最下层，也是最具体的一层， 无人载具要根据自己当前的状态以及上一层获取到的局部轨迹，使用基于模型或无模型的 控制率，实现以较低的偏差跟随局部路径。在整个任务规划与控制的过程中，以上任何一个 层次都是现今的研究重点，而局部路径规划位于其中承上启下的位置，更是其中的难点，既 要保证规划出的局部轨迹满足某种程度的最优(速度、曲率、平滑等)，又要保证路径的可行 性(避障、满足动力学等)。 目前局部路径规划的方法可分为基于自主学习的和非自主学习类，其中基于深度 学习的端到端的规划与控制方法以及基于强化学习的算法虽然近年来在理论上得到了长 足的发展，但无论是安全性、可靠性还是稳定性都有很大的问题，对于未知的环境更是根本 无法使用；非自主学习类算法中，可分为基于搜索的规划算法和基于采样的规划算法。基于 搜索的规划算法主要的代表者是A*算法和Dijkstra算法，这两种算法在很多场合都有很好 的应用，尤其在游戏和汽车导航等领域，但是它们规划时间长、消耗算力大，并不能很好的 应用在障碍位置未知的区域进行高动态运行的无人载具上；基于采样的规划算法主要的代 表者是RRT算法和PRM算法，但是三维空间中采样的随机性更大，而且得到的路径不但不能 保证最优，而且会出现很多毛刺和抖动，超出无人载具的模型约束和动力学范围。 为解决上述问题，出现了一类更加适合三维空间内无人载具路径规划的特殊曲线 拼接的方法：基于平滑连接直线和圆弧的路径规划算法能根据无人载具的初始状态和目标 配置轻易的得到一条平顺的路径，但是得到的路径灵活性差，直线和圆弧的连接处三维空 5 CN 111552296 A 说　明　书 2/10 页 间内无人载具的向心加速度有突变，对后面无人载具的路径跟随要求更高；基于Bezier曲 线的路径规划算法得到的路径更加“智能化”，它的四个控制点均可以编辑，但是Bezier曲 线拼接时，满足几何连续性条件是比较困难的，难以对其进行进一步的优化；基于B样条曲 线的路径规划算法能得到平滑拼接的路径，但是B样条曲线的曲率表示困难，不容易保证、 三维空间内无人载具的曲率约束条件。
技术实现要素：
本发明是为了解决现有三维空间内无人载具局部路径规划方法轨迹连续性和平 滑性差，且轨迹约束难的问题，提出了一种基于弯曲柱坐标系的局部平滑轨迹规划方法。 本发明所述的一种基于弯曲柱坐标系的局部平滑轨迹规划方法，该方法的具体步 骤为： 步骤一：根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径； 步骤二：以全局路径为基线，创建弯曲柱坐标系； 步骤三：对无人载具可视空间进行环境构建，提取无人载具可视空间坐标系内的 障碍物信息； 步骤四：将无人载具的运动状态信息由全局坐标系转换到弯曲柱坐标系中；将无 人载具可视空间内的障碍物信息转换为全局坐标系障碍物信息； 步骤五、根据全局路径，利用代价函数和时间空间离散化算法对未来时间T内，无 人载具的局部运动轨迹进行规划；获取多条可能的局部运动轨迹； 步骤六、根据步骤四转换获得的全局坐标系障碍物信息，从多条可能的局部运动 轨迹中删除无效的局部运动轨迹，获取多条有效局部运动轨迹；步骤七、计算每条有效局部 运动轨迹的代价函数值，并将代价函数值最低的局部运动轨迹作为本次局部规划的最优运 动轨迹。 进一步地，本发明中，步骤二所述以全局路径为基线，创建弯曲柱坐标系的具体方 法为： 将全局路径作为z轴，将路径上每一点的法平面使用极坐标(r,θ)表示，其中，r是 坐标点距离全局路径的距离，θ即是坐标点绕全局路径的螺旋角，获得弯曲柱坐标系(r,θ, z)。 进一步地，本发明中，步骤六中所述的无效的局部运动轨迹包括：与障碍物碰撞的 局部运动轨迹和超出无人载具动力约束范围的局部运动轨迹。 进一步地，本发明中，步骤五中所述的代价函数为： C＝krCr kzCz kθCθ    (1) 其中，C为总代价函数，kr为径向代价函数的比例系数，Cr为径向代价函数，kz为航 向代价函数的比例系数，Cz为航向代价函数，kθ为螺旋代价函数的比例系数，Cθ为螺旋代价 函数。 径向代价函数Cr为： 其中，Ct(r(t))为径向加速度变化率的优化指标，r(T)是本次局部规划末端时刻 6 CN 111552296 A 说　明　书 3/10 页 位置距离全局路径的距离，r(t)是t时刻无人载具位置距离全局路径的距离， 是本次局部 规划时间指标的径向权重系数，kjr本次局部规划加速度变化率指标的径向权重系数， 是 本次局部规划末端状态指标的径向权重系数， 就是r(t)对时间的三阶导的平方。 航向代价函数为： 其中，Ct(z(t))为航向加速度的变化率的优化指标，z(t)是t时刻无人载具的航向 运动的路程， 是本次局部规划末端时刻z轴方向的代价函数；z(T)是本次 局部规划结束时刻z轴方向的值， 为z(T)的一阶导数， 为z(T)的二阶导数， 是本 次局部规划加速度变化率指标的航向权重系数， 是本次局部规划时间指标的航向权重系 数，螺旋代价函数为： 其中，Ct(θ(t))为螺旋加速度的变化率的优化指标，θ(0)为初始时刻的螺旋角，θ (t)为t时刻的螺旋角， 是本次局部规划加速度变化率指标的螺旋方向权重系数， 是本 次局部规划末端状态指标的螺旋方向权重系数， 是本次局部规划时间指标的螺旋方向权 重系数。 进一步地，步骤五所述的获取多条可能的局部运动轨迹的具体方法为： 利用时间空间离散化算法和径向加速度变化率的优化指标、航向加速度变化率的 优化指标和螺旋加速度变化率的优化指标求取t时刻无人载具位置距离全局路径的距离r (t)的表达式、t时刻无人载具的航向运动的位移z(t)的表达式和t时刻的螺旋角θ(t)的表 达式； 无人载具位置距离全局路径的距离r(t)的表达式求取的具体方法为： 令： 要求Ct(r(t))最小，需要r(t)满足 所以以加速度变化率的累积为指标 的变量r(t)一定满足五次多项式关系： r(t)＝a a t a t2 a t30 1 2 3 a4t4 a 55t    (6) a0、a1、a2、a3、a4、a5均为五次多项式的系数，将初值和终值带入得： r(0)＝a0    (7) r(T)＝a0 a T a T21 2 a T33 a4T4 a5T5    (10) 7 CN 111552296 A 说　明　书 4/10 页 获得a0、a1和a2的值；其中，r(0)是初始时刻无人载具距全局路径的距离， 是初 始时刻无人载具的径向速度， 是初始时刻无人载具的径向加速度，r(T)是本次局部规 划结束的时候无人载具的目标径向位置， 本次局部规划结束的时候无人载具的目标径 向速度， 是本地局部规划结束的时候无人载具的目标径向加速度； 将(7)(8)(9)带入(10)(11)(12)得： 求解出a3、a4和a5，得到r(t)的表达式；同理获得z(t)的表达式和θ(t)的表达式，进 而获得可能的局部运动轨迹为： 其中，b0、b1、b2、b3、b4、b5、c0、c1、c2、c3、c4、c5分别为表达式z(t)的系数和表达式θ (t)的系数。本发明采用代价函数和时间空间分解算法找到了求解路径最优性和问题可解 性之间的平衡，计算效率与代价函数复杂度之间的平衡，非常适用于三维空间内高动态无 人潜艇、无人机等无人载具的局部轨迹规划。有效的提高了局部运动轨迹的平滑性。且提出 一种新的坐标系，在此坐标系下，可以极大简化无人潜艇、无人机等无人载具任务规划问题 的描述与代价函数的设计，提高运动轨迹的连续性和平滑性，有效的实现对其运动轨迹的 约束。 附图说明 图1是本发明所述基于弯曲柱坐标系的局部平滑轨迹规划方法的流程图； 图2是弯曲柱坐标系的示意图； 图3是弯曲柱坐标系与全局坐标系关系示意图； 图4是实例中径向规划的结果； 图5是实例中螺旋方向规划的结果； 图6是实例中航向规划的结果； 图7是具体实施例中无人机的部分规划轨迹。