[发明专利]对含有直线路径轨迹的简并拼接方法、装置及其存储介质

说明书

本发明提供了一种对含有直线路径轨迹的简并拼接方法、装置及其存储介质，其中方法步骤包括：S1遍历多段预设子路径，筛查可优化子路径；S2以可优化子路径为起点，判断前后相邻路径中，直线子路径的存在数量是否等于二，若为是，则根据第一法则，对可优化子路径进行优化；S3对步骤S2优化后的可优化子路径进行简并平滑处理。籍此以在保留路径调整自由度的同时，降低用户使用时的复杂度，并且使路径在连接点处切线连续与曲率连续。

技术领域

本发明涉及导航路径拼接优化技术领域，尤其涉及一种对含有直线路径轨迹的简并拼接方法、装置及其存储介质。

背景技术

由于贝塞尔曲线在路径规划中应用广泛，因此在目前大多数的多段路径拼接算法与策略中只考虑了由不同阶贝塞尔曲线构成的路径拼接问题，但是在实际移动机器人的路径拼接问题中，往往会碰到多条连续直线需要拼接的复杂路径，而直线路径的拼接处只能满足连接点的切向连续，无法保证曲率连续，从而影响机器人的轨迹跟随精度和移动的平稳性。

此时若采用3阶贝塞尔曲线进行拼接，当其与两条直线拼接时，中间的两个控制点只能位于两条直线延长线的交点处，缺少曲线的可调整性。而采用5阶贝塞尔曲线对两条直线进行拼接时，虽然能够同时满足与直线相切，并且保持曲线的可调整性。但是由于过于灵活（拥有过多的控制点），很难适用于通过手动拖动控制点，来调整线性，因此只能通过优化方法来自动调整线性，可见若采用手动方式调整，则增加了用户使用时的手调复杂性，而自动调整却降低用户使用的自由度，牺牲了一定的体验感。

为此，本领域亟待一种技术来解决上述问题，以在保留路径调整自由度的同时，降低用户使用时的复杂度，并且使路径在连接点处切线连续与曲率连续。

发明内容

本发明的主要目的在于提供一种对含有直线路径轨迹的简并拼接方法、装置及其存储介质，以解决背景技术中的问题。

为了实现上述目的，根据本发明的第一个方面，提供了一种对含有直线路径轨迹的简并拼接方法，其步骤包括：

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

S2 以可优化子路径为起点，判断前后相邻路径中，直线子路径的存在数量是否等于二，若为是，则根据第一法则，对可优化子路径进行优化；

S3 对步骤S2优化后的可优化子路径进行简并平滑处理。

在可能的优选实施方式下，所述第一法则包括：可优化子路径采用N阶贝塞尔曲线进行子路径轨迹拼接，其中N≥5。

在可能的优选实施方式下，所述简并平滑处理步骤包括：

D1 计算直线子路径的一阶及二阶导数数据；

D2使优化后的可优化子路径的贝塞尔曲线的一阶及二阶导数与相邻的直线子路径的一阶及二阶导数在连接点处相等；

D3根据第二法则对优化后的可优化子路径的控制点进行重合处理。

在可能的优选实施方式下，所述第二法则包括根据下述中的至少一种方式对控制点进行重合处理：重合所有控制点，生成绝对控制点坐标；或所有控制点分为两组，且组内控制点顺序连续，各组组内控制点重合，以生成第一控制点和第二控制点。

在可能的优选实施方式下，所述可优化子路径的贝塞尔曲线的一阶及二阶导数的计算步骤包括：

1.解算贝塞尔曲线的控制点求导公式

其中，为贝塞尔曲线的阶数，是贝塞尔曲线的第个控制点，且其中一阶导数可以表示为：

其中，因此贝塞尔曲线在端点处的一阶导数可以表示为：

其中，表示其在起点处的一阶导数值，表示其在末端出的一阶导数值，而贝塞尔曲线在起点与终点处的二阶导数值与为：

。