[发明专利]多段导航路径曲率连续拼接优化处理机、方法及存储介质

说明书

本发明提供了一种多段导航路径曲率连续拼接优化处理机、方法及存储介质，其中方法步骤包括：S1遍历多段预设子路径，筛查不可优化子路径；S2以不可优化子路径为起始子路径，依次向其两端遍历，以筛查各可优化子路径中是否存在一阶贝塞尔曲线子路径、三阶贝塞尔曲线子路径，如存在则根据第一法则对可优化子路径进行优化后，再进行平滑处理，如不存在则执行步骤S3；S3筛查各可优化子路径中是否存在五阶贝塞尔曲线子路径，如存在，则根据第二法则对剩余可优化子路径进行优化后，再进行平滑处理。籍此使得各子路径在拼接交点处切线方向连续并且曲率连续。

技术领域

本发明涉及导航路径拼接优化技术领域，尤其涉及一种多段导航路径曲率连续拼接优化处理机、方法及存储介质。

背景技术

现有技术领域中，大多数路径规划算法都可以规划出单段曲率连续路径，或者不超过三段相连曲率连续的路径，但是实际移动机器人的导航路径规划中，往往会遇到大于三段路径曲率连续拼接的问题。

而目前现有的路径规划技术，却无法较好的处理这种情况。从而使得移动机器人的各子路径段的拼接显得十分生硬，无法达到各子路径在拼接交点处切线方向连续并且曲率连续，从而使得现有移动机器人在拼接路径段行驶的不够平稳，由此容易造成移动机器人的角速度的变化率不能连续，从而影响移动机器人的移动轨迹跟踪控制的精度与效率。

发明内容

本发明的主要目的在于提供一种多段导航路径曲率连续拼接优化处理机、方法及存储介质，以使各子路径在拼接交点处切线方向连续并且曲率连续。

为了实现上述目的，根据本发明的第一个方面，提供了一种多段导航路径曲率连续拼接优化方法，步骤包括：

S1 遍历多段预设子路径，筛查不可优化子路径；

S2 以不可优化子路径为起始子路径，依次向其两端遍历，以筛查各可优化子路径中是否存在一阶贝塞尔曲线子路径、三阶贝塞尔曲线子路径，如存在则根据第一法则对可优化子路径进行优化后，再进行平滑处理，如不存在则执行步骤S3；

S3 筛查各可优化子路径中是否存在五阶贝塞尔曲线子路径，如存在，则根据第二法则对剩余可优化子路径进行优化后，再进行平滑处理。

在可能的优选实施方式中，所述不可优化子路径包括以下中的至少一种：一阶贝塞尔曲线子路径、预设被锁定的子路径。

在可能的优选实施方式中，所述第一法则包括：当筛查出存在三阶贝塞尔曲线子路径时，统计其相邻两端路径中一阶贝塞尔曲线子路径的数量，根据第三法则以进行分类优化。

在可能的优选实施方式中，所述第二法则包括：判断五阶贝塞尔曲线子路径的前后两段子路径是否为五阶贝塞尔曲线，若为是，则根据其前后两段路径类型，依据第一策略进行优化。

在可能的优选实施方式中，所述第三法则包括：当其相邻两端路径中一阶贝塞尔曲线子路径数量n1=1时，设置其控制点选取在与其相邻的一阶贝塞尔曲线子路径的切线方向上；

当n1=2时，设置与其相邻的一阶贝塞尔曲线子路径的控制点，选取在与其相邻的两条一阶贝塞尔曲线子路径的延长线交点处；

当n1=0时，统计其相邻两端子路径中三阶贝塞尔曲线的数量n2；

当n2=1时，判断前段子路径是否为五阶贝塞尔曲线且被锁定，若为否，则以三阶贝塞尔曲线作为参考路径进行平滑处理；若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理；

当n2=2时，以前段三阶贝塞尔曲线作为参考路径进行平滑处理；

当n2=0时，判断前段路径是否为五阶贝塞尔曲线子路径且被锁定，若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理。

在可能的优选实施方式中，其中所述第一策略包括：