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

权利要求书

1.一种多段导航路径曲率连续拼接优化方法，其特征在于步骤包括：

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

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

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

2.根据权利要求1所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述第一法则包括：当筛查出存在三阶贝塞尔曲线子路径时，统计其相邻两端路径中一阶贝塞尔曲线子路径的数量，根据第三法则以进行分类优化。

3.根据权利要求1所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述第二法则包括：判断五阶贝塞尔曲线子路径的前后两段子路径是否为五阶贝塞尔曲线，若为是，则根据其前后两段路径类型，依据第一策略进行优化。

4.根据权利要求2所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述第三法则包括：当其相邻两端路径中一阶贝塞尔曲线子路径数量n1=1时，设置其控制点选取在与其相邻的一阶贝塞尔曲线子路径的切线方向上；

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

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

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

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

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

5.根据权利要求3所述的多段导航路径曲率连续拼接优化方法，其特征在于，其中所述第一策略包括：

C1若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；

C2若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；

C3若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；

C4若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；

C5若前段子路径为五阶贝塞尔曲线，后段子路径也为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；

C6若前段子路径为五阶贝塞尔曲线，后段子路径为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；

C7若前段子路径为三阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；

C8若前段子路径为三阶贝塞尔曲线，后段子路径也为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化。

6.根据权利要求1所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述平滑处理步骤包括：

D1根据前段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得前段子路径在连接点处的一阶导数和二阶导数；

D2根据后段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得后段子路径在连接点处的一阶导数和二阶导数；

D3 设置可优化子路径的控制点坐标，使其前后连接点处的贝塞尔曲线的一阶导数与二阶导数，分别与前/后段子路径连接点处的一阶导数与二阶导数相等。