CN111552296B - 一种基于弯曲柱坐标系的局部平滑轨迹规划方法 - Google Patents

Abstract

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

Description

一种基于弯曲柱坐标系的局部平滑轨迹规划方法

技术领域

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

背景技术

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

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

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

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

发明内容

本发明是为了解决现有三维空间内无人载具局部路径规划方法轨迹连续性和平滑性差，且轨迹约束难的问题，提出了一种基于弯曲柱坐标系的局部平滑轨迹规划方法。

本发明所述的一种基于弯曲柱坐标系的局部平滑轨迹规划方法，该方法的具体步骤为：

步骤一：根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径；

步骤二：以全局路径为基线，创建弯曲柱坐标系；

步骤三：对无人载具可视空间进行环境构建，提取无人载具可视空间坐标系内的障碍物信息；

步骤四：将无人载具的运动状态信息由全局坐标系转换到弯曲柱坐标系中；将无人载具可视空间内的障碍物信息转换为全局坐标系障碍物信息；

步骤五、根据全局路径，利用代价函数和时间空间离散化算法对未来时间T内，无人载具的局部运动轨迹进行规划；获取多条可能的局部运动轨迹；

步骤六、根据步骤四转换获得的全局坐标系障碍物信息，从多条可能的局部运动轨迹中删除无效的局部运动轨迹，获取多条有效局部运动轨迹；步骤七、计算每条有效局部运动轨迹的代价函数值，并将代价函数值最低的局部运动轨迹作为本次局部规划的最优运动轨迹。

进一步地，本发明中，步骤二所述以全局路径为基线，创建弯曲柱坐标系的具体方法为：

将全局路径作为z轴，将路径上每一点的法平面使用极坐标(r,θ)表示，其中，r是坐标点距离全局路径的距离，θ即是坐标点绕全局路径的螺旋角，获得弯曲柱坐标系(r,θ,z)。

进一步地，本发明中，步骤六中所述的无效的局部运动轨迹包括：与障碍物碰撞的局部运动轨迹和超出无人载具动力约束范围的局部运动轨迹。

进一步地，本发明中，步骤五中所述的代价函数为：

C＝k_rC_r+k_zC_z+k_θC_θ (1)

其中，C为总代价函数，k_r为径向代价函数的比例系数，C_r为径向代价函数，k_z为航向代价函数的比例系数，C_z为航向代价函数，k_θ为螺旋代价函数的比例系数，C_θ为螺旋代价函数。

径向代价函数C_r为：

其中，C_t(r(t))为径向加速度变化率的优化指标，r(T)是本次局部规划末端时刻位置距离全局路径的距离，r(t)是t时刻无人载具位置距离全局路径的距离，

是本次局部规划时间指标的径向权重系数，k_jr本次局部规划加速度变化率指标的径向权重系数，

是本次局部规划末端状态指标的径向权重系数，

就是r(t)对时间的三阶导的平方。

航向代价函数为：

其中，C_t(z(t))为航向加速度的变化率的优化指标，z(t)是t时刻无人载具的航向运动的路程，

是本次局部规划末端时刻z轴方向的代价函数；z(T)是本次局部规划结束时刻z轴方向的值，

为z(T)的一阶导数，

为z(T)的二阶导数，

是本次局部规划加速度变化率指标的航向权重系数，

是本次局部规划时间指标的航向权重系数，螺旋代价函数为：

其中，C_t(θ(t))为螺旋加速度的变化率的优化指标，θ(0)为初始时刻的螺旋角，θ(t)为t时刻的螺旋角，

是本次局部规划加速度变化率指标的螺旋方向权重系数，

是本次局部规划末端状态指标的螺旋方向权重系数，

是本次局部规划时间指标的螺旋方向权重系数。

进一步地，步骤五所述的获取多条可能的局部运动轨迹的具体方法为：

利用时间空间离散化算法和径向加速度变化率的优化指标、航向加速度变化率的优化指标和螺旋加速度变化率的优化指标求取t时刻无人载具位置距离全局路径的距离r(t)的表达式、t时刻无人载具的航向运动的位移z(t)的表达式和t时刻的螺旋角θ(t)的表达式；

无人载具位置距离全局路径的距离r(t)的表达式求取的具体方法为：

令：

要求C_t(r(t))最小，需要r(t)满足

所以以加速度变化率的累积为指标的变量r(t)一定满足五次多项式关系：

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

a₀、a₁、a₂、a₃、a₄、a₅均为五次多项式的系数，将初值和终值带入得：

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

获得a₀、a₁和a₂的值；其中，r(0)是初始时刻无人载具距全局路径的距离，

是初始时刻无人载具的径向速度，

是初始时刻无人载具的径向加速度，r(T)是本次局部规划结束的时候无人载具的目标径向位置，

本次局部规划结束的时候无人载具的目标径向速度，

是本地局部规划结束的时候无人载具的目标径向加速度；

将(7)(8)(9)带入(10)(11)(12)得：

求解出a₃、a₄和a₅，得到r(t)的表达式；同理获得z(t)的表达式和θ(t)的表达式，进而获得可能的局部运动轨迹为：

其中，b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅分别为表达式z(t)的系数和表达式θ(t)的系数。本发明采用代价函数和时间空间分解算法找到了求解路径最优性和问题可解性之间的平衡，计算效率与代价函数复杂度之间的平衡，非常适用于三维空间内高动态无人潜艇、无人机等无人载具的局部轨迹规划。有效的提高了局部运动轨迹的平滑性。且提出一种新的坐标系，在此坐标系下，可以极大简化无人潜艇、无人机等无人载具任务规划问题的描述与代价函数的设计，提高运动轨迹的连续性和平滑性，有效的实现对其运动轨迹的约束。

附图说明

图1是本发明所述基于弯曲柱坐标系的局部平滑轨迹规划方法的流程图；

图2是弯曲柱坐标系的示意图；

图3是弯曲柱坐标系与全局坐标系关系示意图；

图4是实例中径向规划的结果；

图5是实例中螺旋方向规划的结果；

图6是实例中航向规划的结果；

图7是具体实施例中无人机的部分规划轨迹。

具体实施方式

具体实施方式一、结合图1至图3说明本实施方式，本实施方式所述一种基于弯曲柱坐标系的局部平滑轨迹规划方法，该方法的具体步骤为：

步骤一：根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径；

步骤二：以全局路径为基线，创建弯曲柱坐标系；

步骤三：对无人载具可视空间进行环境构建，提取无人载具可视空间坐标系内的障碍物信息；

步骤四：将无人载具的运动状态信息由全局坐标系转换到弯曲柱坐标系中；将无人载具可视空间内的障碍物信息转换为全局坐标系障碍物信息；

步骤五、根据全局路径，利用代价函数和时间空间离散化算法对未来时间T内，无人载具的局部运动轨迹进行规划；获取多条可能的局部运动轨迹；

步骤六、根据步骤四转换获得的全局坐标系障碍物信息，从多条可能的局部运动轨迹中删除无效的局部运动轨迹，获取多条有效局部运动轨迹；

所述无效的局部运动轨迹包括：与障碍物碰撞的局部运动轨迹和超出无人载具动力约束范围的局部运动轨迹。

步骤七、计算每条有效局部运动轨迹的代价函数值，并将代价函数值最低的局部运动轨迹作为本次局部规划的最优运动轨迹。

本实施方式中，从局部规划算法的具体实现步骤可以看出来，避碰和无人机动力学约束是单独考量的，并没有设计在代价函数中，这是因为碰撞惩罚项的引入将带入大量需要人工调整的参数，使得代价函数的设计变得极其复杂，而且会极大的增大计算量以及轨迹优化的复杂程度。

这里的优化指标只考量无人载具运行的稳定程度以及轨迹的平滑程度，即为保证无人载具加速度的连续性，选用加加速度(即加速度的变化率)作为优化的指标之一；因为全局路径规划是与任务层和静态地图息息相关的，但未必会适应环境中些微的细节以及临时的动态变化，所以局部路径规划主要就是为了应对环境的临时状况而存在的，所以局部路径就应该是在全局路径的基础上进行些微调整的，相对于全局路径不应该偏差过大，所以选用局部路径与全局路径的差别作为优化指标其中的一项；局部路径规划也可以看成无人机对未来一段时间内自己轨迹的预测，因为随着无人载具的运动，机载传感器获取到的环境的信息越来越丰富，自己当前规划的未来T时间内的路径可能随着自己的运动会有更优的路径，甚至有时由于环境发生了新的变化原来的路径直接变的无效了，所以规划时间T越长，实际上规划出的路径的可靠性可能就越不好，但是T过短的话会对无人载具的动力要求更高，可能会导致大部分路径都是超出无人载具动力学约束的，所以规划时长T应该根据实际的任务在一个合理的范围内取值，选取规划时长T作为代价函数的其中一项。而上述的这些指标在“弯曲柱坐标系”下就会很容易设计。因此就沿着弯曲柱坐标系的坐标轴(r,θ,z)将代价函数分解为三项：径向代价C_r、航向代价C_z和螺旋代价C_θ。

本发明所述方法所述的无人载具不仅包括无人机，还适用于无人潜艇、三维空间内运动的机器人，例如磁力机器人、医疗纳米机器人等。

进一步地，本实施方式中，步骤二所述以全局路径为基线，创建弯曲柱坐标系的具体方法为：

将全局路径作为z轴，将路径上每一点的法平面使用极坐标(r,θ)表示，其中，r是坐标点距离全局路径的距离，θ即是坐标点绕全局路径的螺旋角，获得弯曲柱坐标系(r,θ,z)。

本实施方式中，弯曲柱坐标系是以全局路径为基线建立的，对于一条平滑的全局路径，将这条路径作为z轴，将路径上每一点的法平面使用极坐标(r,θ)来描述(如图2所示)。假设全局路径为一条直线，那么这个坐标系就退化成为柱坐标系，其中r即是坐标点距离全局路径的距离，θ即是坐标点绕全局路径的螺旋角。将柱坐标系推广到任意平滑的全局路径的情况下，就得到了所谓的“弯曲柱坐标系”(r,θ,z)。利用牛顿——莱布尼兹微元的思想，弯曲柱坐标系中每一小段均可近似看作柱坐标系，所以也可以认为弯曲柱坐标系是无数的柱坐标系拼接而成的。对于无人载具局部轨迹规划问题，在每一个瞬态，z就是无人载具沿全局路径方向飞行的距离，r即为当前法平面中无人载具距全局路径之间的距离，r＝0代表无人载具就在全局路径上，θ＝0的取向相对于全局路径的方向时刻水平向右。因为弯曲柱坐标系是在全局路径的基础上建立的，基于此坐标系可以很方便的对代价函数的实际物理意义进行表达。

本实施方式中，步骤五中所述的代价函数为：

C＝k_rC_r+k_zC_z+k_θC_θ (1)

其中，C为总代价函数，k_r为径向代价函数的比例系数，C_r为径向代价函数，k_z为航向代价函数的比例系数，C_z为航向代价函数，k_θ为螺旋代价函数的比例系数，C_θ为螺旋代价函数。

k_r、k_z、k_θ为三个方向代价的比例系数，可以理解为三个方向的代价加权平均得到的总代价。在不同的实际规划任务中，可能侧重于不同的代价对路径的影响程度不同，比如某任务中，通常希望路径尽量不螺旋，而距离全局路径远近的变化没那么重要，那就可以把k_θ调整的大一些，当要求局部路径与全局路径的距离比较重要时，就把k_r调大等。这三个参数是使用者根据实际问题的侧重不同，来进行调整的。k_r、k_z、k_θ这三个参数还有另外一个作用，就是由于每一个代价函数的设计并不相同，代价函数的设计中也有各自的比例参数，最后三个方向上的代价值并不一定会在同一数量级上，需要通过这三个参数来调整数量级。

径向代价函数C_r为：

其中，C_t(r(t))为径向加速度变化率的优化指标，r(T)是本次局部规划末端时刻位置距离全局路径的距离，r(t)是t时刻无人载具位置距离全局路径的距离，

是本次局部规划时间指标的径向权重系数，

本次局部规划加速度变化率指标的径向权重系数，

是本次局部规划末端状态指标的径向权重系数，

就是r(t)对时间的三阶导的平方。

根据本发明解决的实际问题，选用

作为加加速度(Jerk)的优化指标；局部路径与全局路径的偏差可选用本次局部规划最末时刻的径向距离r(T)来表示。

航向代价函数为：

其中，C_t(z(t))为航向加速度的变化率的优化指标，z(t)是t时刻无人载具的航向运动的路程，

是本次局部规划末端时刻z轴方向的代价函数；z(T)是本次局部规划结束时刻z轴方向的值，

为z(T)的一阶导数，

为z(T)的二阶导数，

是本次局部规划加速度变化率指标的航向权重系数，

是本次局部规划时间指标的航向权重系数，

是本次局部规划末端状态指标的航向权重系数。

航向代价函数关于加速度的变化率C_t(z(t))和规划时长T部分的设计与径向代价函数的同理，只是在航向方向上不存在局部路径和全局路径的偏差，所以没有明确的关于末端位置的代价项，相应的可以根据实际任务情况替换成其他的关于末端状态的表达式，例如要求定速(巡航或竞速等任务)就可以设置关于末端速度状态的代价

要求定点(跟随或穿框等任务)就可以设置关于末端位置状态的代价(z-z_d)²等。综上，关于末端状态的代价设计就统一用h(z(T),

来表示。

螺旋代价函数为：

其中，C_t(θ(t))为螺旋加速度的变化率的优化指标，θ(0)为初始时刻的螺旋角，θ(t)为t时刻的螺旋角，

是本次局部规划加速度变化率指标的螺旋方向权重系数，

是本次局部规划末端状态指标的螺旋方向权重系数，

是本次局部规划时间指标的螺旋方向权重系数。

进一步地，本实施方式中，步骤五所述获取多条可能的局部运动轨迹的具体方法为：

利用时间空间离散化算法和径向加速度变化率的优化指标、航向加速度变化率的优化指标和螺旋加速度变化率的优化指标求取t时刻无人载具位置距离全局路径的距离r(t)的表达式、t时刻无人载具的航向运动的位移z(t)的表达式和t时刻的螺旋角θ(t)的表达式；

无人载具位置距离全局路径的距离r(t)的表达式求取的具体方法为：

令：

要求C_t(r(t))最小，需要r(t)满足

所以以加速度变化率的累积为指标的变量r(t)一定满足五次多项式关系：

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

a₀、a₁、a₂、a₃、a₄、a₅均为五次多项式的系数，将初值和终值带入得：

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

获得a₀、a₁和a₂的值；其中，r(0)是初始时刻无人载具距全局路径的距离，

是初始时刻无人载具的径向速度，

是初始时刻无人载具的径向加速度，r(T)是本次局部规划结束的时候无人载具的目标径向位置，

本次局部规划结束的时候无人载具的目标径向速度，

是本地局部规划结束的时候无人载具的目标径向加速度；

将(7)(8)(9)带入(10)(11)(12)得：

求解出a₃、a₄和a₅，得到r(t)的表达式；同理获得z(t)的表达式和θ(t)的表达式，进而获得可能的局部运动轨迹为：

其中，b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅分别为表达式z(t)的系数和表达式θ(t)的系数。

本实施方式所述的多条可能的局部运动轨迹为均匀分布在全局路径附近的有限条局部路径。

本实施方式中，通常情况下我们可以令

和

为零，也就是本次局部规划末端径向速度和径向加速度为0，对r(T)进行合理的离散化，在(r_min,r_max)之间，以△r为间隔采样得到有限个末端状态，T也同理，在(T_min,T_max)之间，以间隔△T采样，带入上面的线性方程组中得到一系列的a₀、a₁、a₂、a₃、a₃、a₄、a₅，就得到了一系列的r(t)。另外两个方向也是一样的，得到z(t)和θ(t)，这样就相当于曲线的参数方程得到了，就得到了一系列路径了。

本实施方式所述的时间空间离散化算法就是对目标状态的离散化。具体过程为，在r方向上就是通过设置(r_min,r_max)和(T_min,T_max)来限制无人机偏离全局路径的距离以及规划的时长，通过设置△r和△T得到一组径向的目标状态。同理，在θ方向上就是设置(θ_min,θ_max)限制螺旋角范围，设置△θ得到一组螺旋方向的目标状态，z方向上就是设置(z_min,z_max)以及△z得到一组航向的目标状态。△r、△θ、△z、△T就是离散化的体现。

具体实施例：

结合图4至图7说明本实施例，所实施例所述的是基于弯曲柱坐标系的无人载具局部轨迹规划方法。

具体为：在一个宽高均3m的长走廊，令无人机以5m/s定速飞行，走廊里偶尔会有行人经过，偶尔会有门打开等(各种未建模障碍)。假设无人载具是轴距200的四旋翼飞行器，速度上限10m/s，加速度上限5m/s²，曲率不设约束，定位***靠UWB定位。假设四旋翼飞行器初始位置位于全局坐标(1,0,0)的位置，初始速度和初始加速度均为0。

步骤一：根据地图信息规划出一条平滑的全局路径，在本实例中，沿着走廊的中心线就可以设置成全局路径，别忘记在走廊转角处做平滑处理即可，这里就假设使用的三次样条插值平滑。在获取到全局路径之后，全局路径在全局坐标系下的坐标、每点的切线方向向量以及从起点沿全局路径到路径上每一点的距离(即弯曲柱坐标系中的z坐标值)就都已知了。

步骤二：以全局路径为基线，创建“弯曲柱坐标系”。

步骤三：通过四旋翼飞行器上的TOF相机等机载传感器对走廊空间进行构建，提取出可视空间内所有障碍物的信息。

步骤四：将四旋翼飞行器的状态信息由全局坐标系转换到“弯曲柱坐标系”。全局坐标系与弯曲柱坐标系原点对齐，初始z轴指向相同。众所周知，柱坐标系和直角坐标系的转化如下：

但是弯曲柱坐标系的z轴是弯曲的，所以这里有一个小trick用来得到四旋翼飞行器在弯曲柱坐标系下的z值：选择全局路径中距离四旋翼飞行器当前位置的最近的点在弯曲柱坐标系下的z坐标值作为四旋翼飞行器当前在弯曲柱坐标系下的z坐标值，如图3所示，无人载具位于A点，全局路径上B点剧无人载具最近，因为B点位于全局路径上，B点在弯曲柱坐标系下的z坐标值是已知的，我们可以认为A在弯曲柱坐标系下的z坐标值和B是相同的。之后只需要在法平面内使用直角坐标系和极坐标系的转换即可得到四旋翼飞行器在“弯曲柱坐标系”下的坐标值。

步骤五：设计代价函数；

径向代价函数：

其中

航向代价函数：

其中巡航速度

螺旋代价函数：

总代价为C＝k_rC_r+k_zC_z+k_θC_θ，根据参数的物理含义，分别进行参数调节；

步骤六：目标配置的离散化，即时间空间的离散化；根据实际情况，走廊宽高均3m，全局路径在走廊中间，可以设置d_min＝-1.2m，d_max＝1.2m，△d＝0.2m空间采样间隔选取和轴距相同；圆柱被均等分成8份θ_min＝0，θ_max＝π，

规划时间T_min＝4s，T_max＝5s，△T＝0.2s；由于是定速问题，对于目标速度也要离散化v_min＝3m/s，v_max＝7m/s，△v＝1m/s。

利用理论依据部分的理论求解出在上述目标配置集合下的所有路径航点的坐标(r,θ,z)与时间t的关系，如图4、图5和图6所示，分别为本实施例中径向位置和时间的关系图、螺旋方向位置和时间的关系图与航向位置与时间的关系图。由于得到的不是路径的明确解析表达式而是参数方程形式，所以以waypoint列表的形式保存起来即可。如图7所示就是某些条waypoint绘制出的三维局部轨迹。

步骤七：结合步骤五中所述的代价函数，遍历所有的路径，积分就用累加求和代替，求解出集合中所有路径的代价函数值。

步骤八：通过判断路径点距障碍物的距离，设置一个阈值d＝0.2m，当路径点中的任何一点与障碍物之间的距离小于阈值d时，就认为此局部路径有一定危险性，这样的路径就直接从waypoint列表中删掉。计算出各路径点的速度、加速度、瞬时曲率，超出各自设置的上限的路径也删除掉。选出剩余路径中代价最小的路径，就是本次局部规划的路径。

本发明得到的无人机的局部轨迹路径平滑，加速度、速度均没有突变，加速度不突变电机就不会猛转猛停，飞机的寿命以及电池的能量效率等都会有所提高且提出的坐标系能给局部规划的过程更清晰的物理描述，方便设计各种代价函数，能极大的简化无人机局部路径规划问题的描述。通过在新坐标系中合理的离散，巧妙的解决了最优化问题的次优解，找到了可解性和最优性之间的平衡，更适用于真正的解决实际问题。

本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (3)

1.一种基于弯曲柱坐标系的局部平滑轨迹规划方法，其特征在于，该方法的具体步骤为：

步骤一：根据地图信息和无人载具目标任务要求，规划一条平滑的全局路径；

步骤二：以全局路径为基线，创建弯曲柱坐标系；

步骤三：对无人载具可视空间进行环境构建，提取无人载具可视空间坐标系内的障碍物信息；

步骤四：将无人载具的运动状态信息由全局坐标系转换到弯曲柱坐标系中；将无人载具可视空间内的障碍物信息转换为全局坐标系障碍物信息；

步骤五、根据全局路径，利用代价函数和时间空间离散化算法对未来时间T内，无人载具的局部运动轨迹进行规划；获取多条可能的局部运动轨迹；

其中，代价函数为：

C＝k_rC_r+k_zC_z+k_θC_θ (1)

其中，C为总代价函数，k_r为径向代价函数的比例系数，C_r为径向代价函数，k_z为航向代价函数的比例系数，C_z为航向代价函数，k_θ为螺旋代价函数的比例系数，C_θ为螺旋代价函数；

径向代价函数C_r为：

其中，C_t(r(t))为径向加速度变化率的优化指标，r(T)是本次局部规划末端时刻位置距离全局路径的距离，r(t)是t时刻无人载具位置距离全局路径的距离，

是本次局部规划时间指标的径向权重系数，

本次局部规划加速度变化率指标的径向权重系数，

是本次局部规划末端状态指标的径向权重系数，

就是r(t)对时间的三阶导的平方；

航向代价函数为：

其中，C_t(z(t))为航向加速度的变化率的优化指标，z(t)是t时刻无人载具的航向运动的路程，h(z(T),

是本次局部规划末端时刻z轴方向的代价函数；z(T)是本次局部规划结束时刻z轴方向的值，

为z(T)的一阶导数，

为z(T)的二阶导数，

是本次局部规划加速度变化率指标的航向权重系数，

是本次局部规划时间指标的航向权重系数，

是本次局部规划末端状态指标的航向权重系数；

螺旋代价函数为：

其中，C_t(θ(t))为螺旋加速度的变化率的优化指标，θ(0)为初始时刻的螺旋角，θ(t)为t时刻的螺旋角，

是本次局部规划加速度变化率指标的螺旋方向权重系数，

是本次局部规划末端状态指标的螺旋方向权重系数，

是本次局部规划时间指标的螺旋方向权重系数；

获取多条可能的局部运动轨迹的具体方法为：

利用时间空间离散化算法和径向加速度变化率的优化指标、航向加速度变化率的优化指标和螺旋加速度变化率的优化指标求取t时刻无人载具位置距离全局路径的距离r(t)的表达式、t时刻无人载具的航向运动的位移z(t)的表达式和t时刻的螺旋角θ(t)的表达式；

无人载具位置距离全局路径的距离r(t)的表达式求取的具体方法为：

令：

要求C_t(r(t))最小，需要r(t)满足

所以以加速度变化率的累积为指标的变量r(t)一定满足五次多项式关系：

r(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵ (6)

a₀、a₁、a₂、a₃、a₄、a₅均为五次多项式的系数，将初值和终值带入得：

r(0)＝a₀ (7)

r(T)＝a₀+a₁T+a₂T²+a₃T³+a₄T⁴+a₅T⁵ (10)

获得a₀、a₁和a₂的值；其中，r(0)是初始时刻无人载具距全局路径的距离，

是初始时刻无人载具的径向速度，

是初始时刻无人载具的径向加速度，r(T)是本次局部规划末端时刻无人载具的目标径向位置，

本次局部规划末端时刻无人载具的目标径向速度，

是本次局部规划末端时刻无人载具的目标径向加速度；

将(7)(8)(9)带入(10)(11)(12)得：

求解出a₃、a₄和a₅，得到r(t)的表达式；同理获得z(t)的表达式和θ(t)的表达式，进而获得可能的局部运动轨迹为：

其中，b₀、b₁、b₂、b₃、b₄、b₅、c₀、c₁、c₂、c₃、c₄、c₅分别为表达式z(t)的系数和表达式θ(t)的系数；

步骤六、根据步骤四转换获得的全局坐标系障碍物信息，从多条可能的局部运动轨迹中删除无效的局部运动轨迹，获取多条有效局部运动轨迹；

步骤七、计算每条有效局部运动轨迹的代价函数值，并将代价函数值最低的局部运动轨迹作为本次局部规划的最优运动轨迹。

2.根据权利要求1所述的一种基于弯曲柱坐标系的局部平滑轨迹规划方法，其特征在于，步骤二所述以全局路径为基线，创建弯曲柱坐标系的具体方法为：

将全局路径作为z轴，将路径上每一点的法平面使用极坐标(r,θ)表示，其中，r是坐标点距离全局路径的距离，θ即是坐标点绕全局路径的螺旋角，获得弯曲柱坐标系(r,θ,z)。

3.根据权利要求1所述的一种基于弯曲柱坐标系的局部平滑轨迹规划方法，其特征在于，步骤六中所述的无效的局部运动轨迹包括：与障碍物碰撞的局部运动轨迹和超出无人载具动力约束范围的局部运动轨迹。

Images