CN113686345B - 多段导航路径曲率连续拼接优化处理机、方法及存储介质 - Google Patents

多段导航路径曲率连续拼接优化处理机、方法及存储介质 Download PDF

Info

Publication number
CN113686345B
CN113686345B CN202111237946.6A CN202111237946A CN113686345B CN 113686345 B CN113686345 B CN 113686345B CN 202111237946 A CN202111237946 A CN 202111237946A CN 113686345 B CN113686345 B CN 113686345B
Authority
CN
China
Prior art keywords
sub
path
bezier curve
paths
order
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111237946.6A
Other languages
English (en)
Other versions
CN113686345A (zh
Inventor
王为科
赵越
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202111237946.6A priority Critical patent/CN113686345B/zh
Publication of CN113686345A publication Critical patent/CN113686345A/zh
Application granted granted Critical
Publication of CN113686345B publication Critical patent/CN113686345B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Generation (AREA)

Abstract

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

Description

多段导航路径曲率连续拼接优化处理机、方法及存储介质
技术领域
本发明涉及导航路径拼接优化技术领域，尤其涉及一种多段导航路径曲率连续拼接优化处理机、方法及存储介质。
背景技术
现有技术领域中，大多数路径规划算法都可以规划出单段曲率连续路径，或者不超过三段相连曲率连续的路径，但是实际移动机器人的导航路径规划中，往往会遇到大于三段路径曲率连续拼接的问题。
而目前现有的路径规划技术，却无法较好的处理这种情况。从而使得移动机器人的各子路径段的拼接显得十分生硬，无法达到各子路径在拼接交点处切线方向连续并且曲率连续，从而使得现有移动机器人在拼接路径段行驶的不够平稳，由此容易造成移动机器人的角速度的变化率不能连续，从而影响移动机器人的移动轨迹跟踪控制的精度与效率。
发明内容
本发明的主要目的在于提供一种多段导航路径曲率连续拼接优化处理机、方法及存储介质，以使各子路径在拼接交点处切线方向连续并且曲率连续。
为了实现上述目的，根据本发明的第一个方面，提供了一种多段导航路径曲率连续拼接优化方法，步骤包括：
S1 遍历多段预设子路径，筛查不可优化子路径；
S2 以不可优化子路径为起始子路径，依次向其两端遍历，以筛查各可优化子路径中是否存在一阶贝塞尔曲线子路径、三阶贝塞尔曲线子路径，如存在则根据第一法则对可优化子路径进行优化后，再进行平滑处理，如不存在则执行步骤S3；
S3 筛查各可优化子路径中是否存在五阶贝塞尔曲线子路径，如存在，则根据第二法则对剩余可优化子路径进行优化后，再进行平滑处理。
在可能的优选实施方式中，所述不可优化子路径包括以下中的至少一种：一阶贝塞尔曲线子路径、预设被锁定的子路径。
在可能的优选实施方式中，所述第一法则包括：当筛查出存在三阶贝塞尔曲线子路径时，统计其相邻两端路径中一阶贝塞尔曲线子路径的数量，根据第三法则以进行分类优化。
在可能的优选实施方式中，所述第二法则包括：判断五阶贝塞尔曲线子路径的前后两段子路径是否为五阶贝塞尔曲线，若为是，则根据其前后两段路径类型，依据第一策略进行优化。
在可能的优选实施方式中，所述第三法则包括：当其相邻两端路径中一阶贝塞尔曲线子路径数量n1=1时，设置其控制点选取在与其相邻的一阶贝塞尔曲线子路径的切线方向上；
当n1=2时，设置与其相邻的一阶贝塞尔曲线子路径的控制点，选取在与其相邻的两条一阶贝塞尔曲线子路径的延长线交点处；
当n1=0时，统计其相邻两端子路径中三阶贝塞尔曲线的数量n2；
当n2=1时，判断前段子路径是否为五阶贝塞尔曲线且被锁定，若为否，则以三阶贝塞尔曲线作为参考路径进行平滑处理；若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理；
当n2=2时，以前段三阶贝塞尔曲线作为参考路径进行平滑处理；
当n2=0时，判断前段路径是否为五阶贝塞尔曲线子路径且被锁定，若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理。
在可能的优选实施方式中，其中所述第一策略包括：
C1若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C2若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；
C3若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C4若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；
C5若前段子路径为五阶贝塞尔曲线，后段子路径也为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C6若前段子路径为五阶贝塞尔曲线，后段子路径为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C7若前段子路径为三阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C8若前段子路径为三阶贝塞尔曲线，后段子路径也为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化。
在可能的优选实施方式中，所述平滑处理步骤包括：
D1根据前段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得前段子路径在连接点处的一阶导数和二阶导数；
D2根据后段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得后段子路径在连接点处的一阶导数和二阶导数；
D3 设置可优化子路径的控制点坐标，使其前后连接点处的贝塞尔曲线的一阶导数与二阶导数，分别与前/后段子路径连接点处的一阶导数与二阶导数相等。
在可能的优选实施方式中，所述贝塞尔曲线的求导公式表达为：
Figure 100002_DEST_PATH_IMAGE001
其中，
Figure 100002_DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure 100002_DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure 100002_DEST_PATH_IMAGE004
个控制点，且
Figure 100002_DEST_PATH_IMAGE005
，
Figure 100002_DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure 100002_DEST_PATH_IMAGE007
其中
Figure 100002_DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure 100002_DEST_PATH_IMAGE009
其中
Figure 100002_DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure 100002_DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，同理可得贝塞尔曲线在起点与末端处的二阶导数值
Figure 100002_DEST_PATH_IMAGE012
与
Figure 100002_DEST_PATH_IMAGE013
：
Figure 100002_DEST_PATH_IMAGE014
。
为了实现上述目的，根据本发明的第二个方面，还提供了一种多段导航路径曲率连续拼接优化处理机，其包括：导航路径规划模块，路径拼接处理模块，其中移动机器人经扫描模块获取导航数据，以传输至导航路径模块中，经处理后生成多段子路径数据，并向路径拼接处理模块传输，所述路径拼接处理模块采用下述中的至少一种方式对多段子路径进行拼接：使用五阶贝塞尔曲线对子路径为一阶、三阶、五阶的贝塞尔曲线进行路径拼接，或使用三阶贝塞尔曲线对子路径为一阶、三阶的贝塞尔曲线进行拼接；而后，路径拼接处理模块依据如上述本发明第一或第二个方面任一项所述的多段导航路径曲率连续拼接优化方法，对用于拼接的贝塞尔曲线子路径进行优化后，再进行平滑处理。
为了实现上述目的，根据本发明的第三个方面，还提供了一种可读存储介质，其上存储有计算机程序，其中所述计算机程序被处理器执行时，实现如上述本发明第一或第二个方面任一项所述的多段导航路径曲率连续拼接优化方法的步骤。
通过本发明提供的多段导航路径曲率连续拼接优化处理机、方法及存储介质，其有益效果包括：
1、针对多段贝塞尔路径下如何选择轨迹平滑顺序的问题，给出了使用三次循环依次优化的方案，该方案全面的考虑了优化中可能出现的各种情况，解决了多段路径曲率连续拼接问题。
2、根据前后段路径信息来选取贝塞尔曲线控制点，可以有效的使路径在交点处切线方向连续并且曲率连续，从而使移动机器人运行的更加平稳。
3、路径的切线方向连续，意味着机器人的角度变化连续，路径的曲率连续意味着机器人角速度的变化率连续，有利于提高机器人轨迹跟踪控制的精度与效率。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明第一实施例的步骤S1的流程示意图；
图2为本发明第一实施例的步骤S2的流程示意图；
图3为本发明第一实施例的步骤S3的流程示意图；
图4为本发明第一实施例中实验例1的拼接优化示意图；
图5为本发明第一实施例中实验例2的拼接优化示意图；
图6为本发明第一实施例中实验例3的拼接优化示意图；
图7为本发明第一实施例中实验例4的拼接优化示意图；
图8为本发明第一实施例中实验例5的拼接优化示意图；
图9为本发明第一实施例中实验例6的拼接优化示意图；
图10为本发明第一实施例中实验例7的拼接优化示意图；
图11为本发明第一实施例中实验例8的拼接优化示意图；
图12为本发明第一实施例中整体路径初始化示意图；
图13为本发明第一实施例中整体路径经过步骤S2优化后的示意图；
图14为本发明第一实施例中整体路径经过步骤S3优化后的示意图。
具体实施方式
下面对本发明的具体实施方式进行详细地说明。以下示例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
为了使本领域的技术人员更好的理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“S1”、“S2”、“C1”、“D1”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。此外除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本发明中的具体含义。
其中本案发明提供的该多段导航路径曲率连续拼接优化方法，在下述实施例中，主要针对具有不同贝塞尔曲线阶数(主要为一阶、三阶与五阶)的多段复杂路径，进行光滑连接处理。为此本发明提供的拼接优化方案中，假设存在一条由若干段具有不同贝塞尔阶数的子路径连接起来的一条待优化路径。
针对此路径，策略上设计了对路径的三次遍历，来解决多段子路径优化时的优化顺序问题。在实施拼接策略时，需要根据前段的子路径或者前后两段的子路径，分别对轨迹进行优化及平滑处理，从而达到平滑拼接的效果。
为此本发明构思上，主要通过使用五阶贝塞尔曲线对一，三，五阶贝塞尔曲线进行路径拼接，或者使用三阶贝塞尔曲线对一、三阶贝塞尔曲线进行拼接。
同时在进行路径拼接时，根据其前、后段贝塞尔曲线子路径的控制点坐标，由贝塞尔曲线的求导公式，可以得到其在连接点处的一阶导数和二阶导数信息，由此便可合理安排该被优化的贝塞尔曲线的控制点坐标，使其贝塞尔曲线的一阶二阶导数与前、后段曲线的一阶二阶导数在连接点处相等，从而便可实现移动机器人在路径连接点处切线连续并且曲率连续。
（一）
具体来说，如图1至图14所示，本发明第一方面提供的该多段导航路径曲率连续拼接优化方法，具体步骤包括：
S1 如图1所示，遍历多段预设子路径，筛查不可优化子路径；其中该不可优化子路径包括以下中的至少一种子路径：一阶贝塞尔曲线子路径、预设被锁定的子路径。
S2 如图2所示，以不可优化子路径为起始子路径，依次向其两端遍历，以筛查各可优化子路径中是否存在一阶贝塞尔曲线子路径、三阶贝塞尔曲线子路径，如存在则根据第一法则对可优化子路径进行优化后，再进行平滑处理，如不存在则执行步骤S3。
其中所述第一法则包括：当筛查出存在三阶贝塞尔曲线子路径时，统计其相邻两端路径中一阶贝塞尔曲线子路径的数量，根据第三法则以进行分类优化。
其中所述第三法则包括：当其相邻两端路径中一阶贝塞尔曲线子路径数量n1=1时，设置其控制点选取在与其相邻的一阶贝塞尔曲线子路径的切线方向上；
当n1=2时，设置与其相邻的一阶贝塞尔曲线子路径的控制点，选取在与其相邻的两条一阶贝塞尔曲线子路径的延长线交点处；
当n1=0时，统计其相邻两端子路径中三阶贝塞尔曲线的数量n2；
当n2=1时，判断前段子路径是否为五阶贝塞尔曲线且被锁定，若为否，则以三阶贝塞尔曲线作为参考路径进行平滑处理；若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理；
当n2=2时，以前段三阶贝塞尔曲线作为参考路径进行平滑处理；
当n2=0时，判断前段路径是否为五阶贝塞尔曲线子路径且被锁定，若为是，则以前段被锁定的五阶贝塞尔曲线作为参考路径进行平滑处理。
S3 如图3所示，筛查各可优化子路径中是否存在五阶贝塞尔曲线子路径，如存在，则根据第二法则对剩余可优化子路径进行优化后，再进行平滑处理；其中所述第二法则包括：判断五阶贝塞尔曲线子路径的前后两段子路径是否为五阶贝塞尔曲线，若为是，则根据其前后两段路径类型，依据第一策略进行优化。
其中考虑到使用贝塞尔曲线与两条相邻贝塞尔曲线平滑连接的问题，假设已知前后两段子路径控制点的信息，通过已知信息设计出中间拼接子路径，使之连接前后两段子路径，并在接点处达到切线方向连续与曲率连续的效果。
为此本发明所设计的平滑子路径优选为三阶或者五阶贝塞尔曲线，为满足切线方向与曲率连续的条件，根据前后段子路径的信息，本实施例中给出了贝塞尔曲线控制点的选取方法。其中本发明中的光滑路径定义为，切线方向连续并且曲率连续的路径。
具体来说：其中该第一策略包括：
C1若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C2若前后两段子路径均为一阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；
C3若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C4若前段子路径为一阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用三阶贝塞尔曲线优化；
C5若前段子路径为五阶贝塞尔曲线，后段子路径也为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C6若前段子路径为五阶贝塞尔曲线，后段子路径为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C7若前段子路径为三阶贝塞尔曲线，后段子路径为五阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化；
C8若前段子路径为三阶贝塞尔曲线，后段子路径也为三阶贝塞尔曲线时，拼接子路径采用五阶贝塞尔曲线优化。
其中该平滑处理步骤包括：
D1根据前段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得前段子路径在连接点处的一阶导数和二阶导数；
D2根据后段子路径的贝塞尔曲线的控制点坐标，由贝塞尔曲线的求导公式，解得后段子路径在连接点处的一阶导数和二阶导数；
D3 设置可优化子路径的控制点坐标，使其前后连接点处的贝塞尔曲线的一阶导数与二阶导数，分别与前/后段子路径连接点处的一阶导数与二阶导数相等。
其中该设计原理为：考虑前后段子路径，根据前段子路径末端点处的一阶与二阶导数，与后段子路径前端点处的一阶与二阶导数，来选取评价子路径的三阶或者五阶贝塞尔曲线的控制点，使得其与前后段子路径连接处切线方向连续，曲率连续。
其中对于贝塞尔曲线的求导公式表达为：
Figure 6314DEST_PATH_IMAGE001
其中，
Figure 392295DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure 640874DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure 504925DEST_PATH_IMAGE004
个控制点，且
Figure 788139DEST_PATH_IMAGE005
，
Figure 79443DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure 313853DEST_PATH_IMAGE007
其中
Figure 981595DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure DEST_PATH_IMAGE015
其中
Figure 588156DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure 50362DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，同理可得贝塞尔曲线在起点与末端处的二阶导数值
Figure 539112DEST_PATH_IMAGE012
与
Figure 744965DEST_PATH_IMAGE013
：
Figure 737192DEST_PATH_IMAGE014
。
通过上述公式，可方便的计算出前后段贝塞尔曲线子路径在拼接点处的一阶与二阶导数的数值，同样的中间的贝塞尔曲线拼接子路径在端点出的数值，也可以通过待确定的控制点表示出来。
路径拼接的要求，需要使得路径在连接点处切线方向连续与曲率连续，贝塞尔曲线可以由参数方程
Figure DEST_PATH_IMAGE016
决定。由曲率的计算公式：
Figure DEST_PATH_IMAGE017
单位切线向量的计算公式：
Figure DEST_PATH_IMAGE018
由此可以看出，只需要使得两路径在连接点处一阶导数以及二阶导数连续即可以保证路径在拼接点处切线方向连续与曲率连续。
其中如图13所示，展示了步骤S2中进行了第二次循环后的平滑三阶贝塞尔曲线的多段曲线。
而步骤S3中进行的第三次循环时，会涉及到对于不同类型曲线路径的平滑连接问题。而步骤S2中的第二次循环对于单端路径的拼接问题可以较为简单的从对于双段路径的轨迹拼接问题中简化出来。
因此，在进一步优化过程中主要针对两条子路径拼接轨迹的平滑连接进行优化，如图14所示，展示了步骤S3中进行了第三次循环后的平滑五阶贝塞尔曲线的多段曲线。
另一方面，本发明研究了轨迹的光滑连接问题，其中光滑定义为切线方向连续并且曲率连续。对分段路径进行光滑的拼接不仅能够使机器人运行的更加平稳，也可以提高轨迹跟踪的精度。根据实际拼接时遇到的路径类型，分别考虑由一阶贝塞尔曲线(直线)，三阶贝塞尔曲线与五阶贝塞尔曲线组成的路径。针对实际需求，本方案只考虑8种情况下的路径光滑问题。并且，可以通过组合这8种情况，来解决更加复杂的线路平滑问题，也可以通过简化这8中情况得到在上述第二次循环中需要用到的单条轨迹的优化问题。
其中下述实验例中，使用
Figure DEST_PATH_IMAGE019
表示第i条贝塞尔曲线的第j+1个控制点，且i = 1, 2,3, j=0,1,2,3,4,5。
实验例1
情况1： 考虑前后两段轨迹均为直线，使用5阶贝塞尔曲线进行连接时，如图4所示。
规则1：由于直线的切线是固定的并且其二阶导数恒等于0，这时的控制点选取规则可以被简单的分析出来而无需经过求导进行复杂运算。因此可以选取6个参考点：第一个选取前直线末尾，第二个选取在直线的延长线上使得与直线的切线方向一致。第三个点为了使其二阶导数为0，也选取在前直线的延长线方向上。
同理，为了保证路径与后段直线光滑连接也需要将第四个与第五个控制点选取在后直线的延长线上，第六个控制点选取在后直线的前端。注：第二个与第三个控制点均选取到直线的延长线方向上，是可以重合的，即
Figure DEST_PATH_IMAGE020
。同理，第四个与第五个控制点也是可以重合的，即
Figure DEST_PATH_IMAGE021
。
例1：前段路径为直线
Figure DEST_PATH_IMAGE022
，后端路径为直线
Figure DEST_PATH_IMAGE023
.根据情况1的控制点选取规则，选取
Figure DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure DEST_PATH_IMAGE025
=
Figure DEST_PATH_IMAGE026
=[10,10],
Figure DEST_PATH_IMAGE027
=[40,10],
Figure DEST_PATH_IMAGE028
=[50,10],
Figure DEST_PATH_IMAGE029
=[60,10]为后段直线的前端点。生成的轨迹与控制点如图4所示。
实验例2
情况2： 考虑前后两段轨迹均为直线，使用3阶贝塞尔曲线进行连接时，如图5所示。
规则2：此情况使情况1的特殊形式，当控制点
Figure DEST_PATH_IMAGE030
时，意味着
Figure DEST_PATH_IMAGE031
四点是重合的，此时情况1中的5阶贝塞尔曲线将会退化为3阶贝塞尔曲线。这时需要选取4个参考点：第一个选取前直线末尾，第二个与第三个重合选取在前后直线的延长线交点处，第四个选取在后直线的前端。
例2：前段路径为直线
Figure DEST_PATH_IMAGE032
，后端路径为直线
Figure DEST_PATH_IMAGE033
.根据情况2的控制点选取规则，选取
Figure 57051DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 767518DEST_PATH_IMAGE025
=
Figure 777062DEST_PATH_IMAGE026
=[10,10],
Figure DEST_PATH_IMAGE034
=[60,10]为后段直线的前端点。生成的轨迹与控制点如图5所示。
实验例3
情况3：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线，使用5阶贝塞尔曲线进行拼接，如图6所示。
规则3：后段5阶贝塞尔曲线的控制点表示为(
Figure DEST_PATH_IMAGE035
,中间的5阶贝塞尔控制点表示为(
Figure DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 794435DEST_PATH_IMAGE024
选取前直线末尾，
Figure 598443DEST_PATH_IMAGE020
选取在前直线的延长线方向上。
Figure 796206DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 343862DEST_PATH_IMAGE029
=
Figure DEST_PATH_IMAGE037
。
Figure DEST_PATH_IMAGE038
的选取：应使
Figure 513943DEST_PATH_IMAGE038
与
Figure DEST_PATH_IMAGE039
关于
Figure 223273DEST_PATH_IMAGE037
中心对称，即
Figure 144218DEST_PATH_IMAGE039
-
Figure 495565DEST_PATH_IMAGE037
=
Figure 316890DEST_PATH_IMAGE029
-
Figure 728280DEST_PATH_IMAGE038
，得
Figure 635056DEST_PATH_IMAGE038
=
Figure 524515DEST_PATH_IMAGE029
-
Figure 465926DEST_PATH_IMAGE039
+
Figure 48217DEST_PATH_IMAGE037
。
Figure 940825DEST_PATH_IMAGE034
的选取应满足：(
Figure DEST_PATH_IMAGE040
-
Figure 837236DEST_PATH_IMAGE039
)-(
Figure 367575DEST_PATH_IMAGE039
-
Figure 386346DEST_PATH_IMAGE037
)=(
Figure 267715DEST_PATH_IMAGE029
-
Figure 764555DEST_PATH_IMAGE038
)-(
Figure 414979DEST_PATH_IMAGE038
-
Figure 574959DEST_PATH_IMAGE034
)，得
Figure 943623DEST_PATH_IMAGE034
=
Figure 978575DEST_PATH_IMAGE040
-2*
Figure 14664DEST_PATH_IMAGE039
+2*
Figure 844080DEST_PATH_IMAGE038
。
例3：前段路径为直线
Figure 700041DEST_PATH_IMAGE022
，后端路径为5阶贝塞尔曲线，且
Figure 538684DEST_PATH_IMAGE037
=[60,0],
Figure 898121DEST_PATH_IMAGE039
=[70,10],
Figure 662552DEST_PATH_IMAGE040
=[80,15] ,
Figure DEST_PATH_IMAGE041
=[110,20],
Figure DEST_PATH_IMAGE042
=[120,30],
Figure DEST_PATH_IMAGE043
=[140,50]。根据情况3的控制点选取规则，选取
Figure 881175DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 789088DEST_PATH_IMAGE025
=
Figure 268611DEST_PATH_IMAGE026
=[10,10],
Figure 705409DEST_PATH_IMAGE027
=[40,-25],
Figure 771847DEST_PATH_IMAGE028
=[50,-10],
Figure DEST_PATH_IMAGE044
=[60,0]。生成的轨迹与控制点如图6所示。
实验例4
情况4：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线时，使用3阶贝塞尔曲线进行拼接时，如图7所示。
规则4：后段3阶贝塞尔曲线的控制点表示为(
Figure DEST_PATH_IMAGE045
，中间的5阶贝塞尔控制点表示为(
Figure 624397DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 958426DEST_PATH_IMAGE024
选取前直线末尾，
Figure 566125DEST_PATH_IMAGE020
选取在前直线的延长线方向上。
Figure 618394DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 336952DEST_PATH_IMAGE029
=
Figure 555181DEST_PATH_IMAGE037
。
Figure 333781DEST_PATH_IMAGE038
的选取应满足：3*
Figure DEST_PATH_IMAGE047
-
Figure DEST_PATH_IMAGE048
=5*(
Figure 545451DEST_PATH_IMAGE029
-
Figure 67699DEST_PATH_IMAGE038
)，得
Figure 641900DEST_PATH_IMAGE038
=
Figure 591401DEST_PATH_IMAGE029
-3*(
Figure 842430DEST_PATH_IMAGE039
-
Figure 168369DEST_PATH_IMAGE037
)/5。
Figure 331497DEST_PATH_IMAGE034
的选取应满足：3*2*[(
Figure 451900DEST_PATH_IMAGE040
-
Figure 966058DEST_PATH_IMAGE039
)-(
Figure 830109DEST_PATH_IMAGE039
-
Figure 847743DEST_PATH_IMAGE037
)] =5*4*[(
Figure 404627DEST_PATH_IMAGE029
-
Figure 639037DEST_PATH_IMAGE038
)-(
Figure 572358DEST_PATH_IMAGE038
-
Figure 444499DEST_PATH_IMAGE034
)]，得
Figure 172283DEST_PATH_IMAGE034
=3*(
Figure 395454DEST_PATH_IMAGE040
-2*
Figure 601308DEST_PATH_IMAGE039
+
Figure 593534DEST_PATH_IMAGE037
)/10-
Figure 492220DEST_PATH_IMAGE029
+2*
Figure 202687DEST_PATH_IMAGE038
。
例4：前段路径为直线
Figure 713696DEST_PATH_IMAGE022
，后端路径为3阶贝塞尔曲线，且
Figure 826009DEST_PATH_IMAGE037
=[60,0],
Figure 630017DEST_PATH_IMAGE039
=[70,10],
Figure 827780DEST_PATH_IMAGE040
=[80,15] ,
Figure 375436DEST_PATH_IMAGE041
=[90,30]。根据情况4的控制点选取规则，选取
Figure 342255DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 317164DEST_PATH_IMAGE020
=[10,10],
Figure 2224DEST_PATH_IMAGE027
=[48,-13.5],
Figure 852106DEST_PATH_IMAGE028
=[54,-6],
Figure 673431DEST_PATH_IMAGE044
=[60,0]。 生成的轨迹与控制点如图7所示。
实验例5
情况5：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图8所示。
规则5：前段的5阶贝塞尔控制点表示为(
Figure DEST_PATH_IMAGE049
，后段的5阶贝塞尔曲线的控制点表示为(
Figure 553662DEST_PATH_IMAGE035
，中间的5阶贝塞尔控制点表示为(
Figure 726018DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 615476DEST_PATH_IMAGE024
选取前直线末尾，使
Figure 556888DEST_PATH_IMAGE024
=
Figure DEST_PATH_IMAGE050
，
Figure 342441DEST_PATH_IMAGE025
的选取：应使
Figure 237978DEST_PATH_IMAGE025
与
Figure DEST_PATH_IMAGE051
关于
Figure 399969DEST_PATH_IMAGE050
中心对称，即
Figure 195887DEST_PATH_IMAGE025
-
Figure 949079DEST_PATH_IMAGE024
=
Figure 830447DEST_PATH_IMAGE050
-
Figure 61709DEST_PATH_IMAGE051
，得
Figure 977712DEST_PATH_IMAGE025
=
Figure 400341DEST_PATH_IMAGE050
-
Figure 769005DEST_PATH_IMAGE051
+
Figure 803957DEST_PATH_IMAGE024
。
Figure 308888DEST_PATH_IMAGE026
的选取应满足：(
Figure 669462DEST_PATH_IMAGE026
-
Figure 259844DEST_PATH_IMAGE025
)-(
Figure DEST_PATH_IMAGE052
-
Figure 567328DEST_PATH_IMAGE024
)=(
Figure DEST_PATH_IMAGE053
-
Figure 897072DEST_PATH_IMAGE051
)-(
Figure DEST_PATH_IMAGE054
-
Figure DEST_PATH_IMAGE055
)，得
Figure 100651DEST_PATH_IMAGE026
=
Figure 178329DEST_PATH_IMAGE050
-2*
Figure 820662DEST_PATH_IMAGE051
+
Figure 300185DEST_PATH_IMAGE055
+2*
Figure 2562DEST_PATH_IMAGE025
-
Figure 66071DEST_PATH_IMAGE024
。
Figure 980937DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 580546DEST_PATH_IMAGE029
=
Figure 188245DEST_PATH_IMAGE037
。
Figure 240514DEST_PATH_IMAGE038
的选取：应使
Figure 959072DEST_PATH_IMAGE038
与
Figure 678766DEST_PATH_IMAGE039
关于
Figure 457366DEST_PATH_IMAGE037
中心对称，即
Figure 510115DEST_PATH_IMAGE039
-
Figure 32364DEST_PATH_IMAGE037
=
Figure 340985DEST_PATH_IMAGE029
-
Figure 290487DEST_PATH_IMAGE038
，得
Figure 51769DEST_PATH_IMAGE038
=
Figure 377708DEST_PATH_IMAGE029
-
Figure 540837DEST_PATH_IMAGE039
+
Figure 661239DEST_PATH_IMAGE037
。
Figure 440976DEST_PATH_IMAGE034
的选取应满足：(
Figure 803562DEST_PATH_IMAGE040
-
Figure 821197DEST_PATH_IMAGE039
)-(
Figure DEST_PATH_IMAGE056
-
Figure 846922DEST_PATH_IMAGE037
)=(
Figure 582797DEST_PATH_IMAGE044
-
Figure 984959DEST_PATH_IMAGE038
)-(
Figure 122679DEST_PATH_IMAGE028
-
Figure 116043DEST_PATH_IMAGE034
)，得
Figure 575100DEST_PATH_IMAGE034
=
Figure 46532DEST_PATH_IMAGE040
-2*
Figure 38759DEST_PATH_IMAGE039
+2*
Figure 671866DEST_PATH_IMAGE038
。
例5：考虑方案5（前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接）
前段的5阶贝塞尔控制点为：
Figure DEST_PATH_IMAGE057
=[-10，-10],
Figure DEST_PATH_IMAGE058
=[0,0],
Figure DEST_PATH_IMAGE059
=[5,20],
Figure DEST_PATH_IMAGE060
=[20,10],
Figure 756234DEST_PATH_IMAGE054
=[30,20],
Figure 31358DEST_PATH_IMAGE053
=[40,10].，后段的5阶贝塞尔曲线的控制点为
Figure 878091DEST_PATH_IMAGE037
=[140,50],
Figure 682099DEST_PATH_IMAGE039
=[150,60],
Figure 879862DEST_PATH_IMAGE040
=[160,65] ,
Figure 693097DEST_PATH_IMAGE041
=[180,40],
Figure 659916DEST_PATH_IMAGE042
=[190,30] ,
Figure 634826DEST_PATH_IMAGE043
=[200,50]，根据情况5的控制点选取规则，选取
Figure 319885DEST_PATH_IMAGE024
=
Figure 172697DEST_PATH_IMAGE050
=[40,10]，
Figure DEST_PATH_IMAGE061
=[50,0],
Figure DEST_PATH_IMAGE062
=[60,-30],
Figure DEST_PATH_IMAGE063
=[120,25],
Figure DEST_PATH_IMAGE064
=[130,40],
Figure DEST_PATH_IMAGE065
=
Figure DEST_PATH_IMAGE066
=[140,50]. 生成的轨迹与控制点如图8所示。
实验例6
情况6：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图9所示。
规则6：前段的5阶贝塞尔控制点表示为(
Figure DEST_PATH_IMAGE067
，后段的3阶贝塞尔曲线的控制点表示为(
Figure DEST_PATH_IMAGE068
，中间的5阶贝塞尔控制点表示为(
Figure DEST_PATH_IMAGE069
。此时需要选取6个参考点，选择方式如下：
Figure DEST_PATH_IMAGE070
选取前直线末尾，使
Figure 180973DEST_PATH_IMAGE070
=
Figure DEST_PATH_IMAGE071
，
Figure 795625DEST_PATH_IMAGE061
的选取：应使
Figure 702401DEST_PATH_IMAGE061
与
Figure DEST_PATH_IMAGE072
关于
Figure 827745DEST_PATH_IMAGE071
中心对称，即
Figure 503577DEST_PATH_IMAGE061
-
Figure 85868DEST_PATH_IMAGE070
=
Figure 745520DEST_PATH_IMAGE071
-
Figure 438669DEST_PATH_IMAGE072
，得
Figure 234587DEST_PATH_IMAGE061
=
Figure 987779DEST_PATH_IMAGE071
-
Figure 603568DEST_PATH_IMAGE072
+
Figure 598944DEST_PATH_IMAGE070
。
Figure DEST_PATH_IMAGE073
的选取应满足：(
Figure 718210DEST_PATH_IMAGE073
-
Figure 642303DEST_PATH_IMAGE061
)-(
Figure DEST_PATH_IMAGE074
-
Figure 479809DEST_PATH_IMAGE070
)=(
Figure DEST_PATH_IMAGE075
-
Figure 983603DEST_PATH_IMAGE072
)-(
Figure DEST_PATH_IMAGE076
-
Figure DEST_PATH_IMAGE077
)，得
Figure 915963DEST_PATH_IMAGE073
=
Figure 10958DEST_PATH_IMAGE071
-2*
Figure 866918DEST_PATH_IMAGE072
+
Figure 705561DEST_PATH_IMAGE077
+2*
Figure 330578DEST_PATH_IMAGE061
-
Figure 596474DEST_PATH_IMAGE070
。
Figure 674151DEST_PATH_IMAGE065
选取在后曲线的前端，使得
Figure 549441DEST_PATH_IMAGE065
=
Figure 294543DEST_PATH_IMAGE066
。
Figure 731341DEST_PATH_IMAGE064
的选取应满足：3*(
Figure DEST_PATH_IMAGE078
-
Figure 499577DEST_PATH_IMAGE066
)=5*(
Figure 945602DEST_PATH_IMAGE065
-
Figure 545210DEST_PATH_IMAGE064
)，得
Figure 152909DEST_PATH_IMAGE064
=
Figure 441064DEST_PATH_IMAGE065
-3*(
Figure 425201DEST_PATH_IMAGE078
-
Figure 879316DEST_PATH_IMAGE066
)/5。
Figure 657916DEST_PATH_IMAGE063
的选取应满足：3*2*[(
Figure DEST_PATH_IMAGE079
-
Figure 666323DEST_PATH_IMAGE078
)-(
Figure 922992DEST_PATH_IMAGE078
-
Figure 497193DEST_PATH_IMAGE066
)] =5*4*[(
Figure 446695DEST_PATH_IMAGE065
-
Figure 972092DEST_PATH_IMAGE064
)-(
Figure 32452DEST_PATH_IMAGE064
-
Figure 461159DEST_PATH_IMAGE063
)]，得
Figure 581562DEST_PATH_IMAGE063
=3*(
Figure 830140DEST_PATH_IMAGE079
-2*
Figure 959770DEST_PATH_IMAGE078
+
Figure 242984DEST_PATH_IMAGE066
)/10-
Figure 534288DEST_PATH_IMAGE065
+2*
Figure 771628DEST_PATH_IMAGE064
。
例6：前段的5阶贝塞尔控制点为
Figure DEST_PATH_IMAGE080
=[-10，-10],
Figure DEST_PATH_IMAGE081
=[0,0],
Figure DEST_PATH_IMAGE082
=[5,20],
Figure DEST_PATH_IMAGE083
=[20,10],
Figure 49157DEST_PATH_IMAGE076
=[30,20],
Figure 186877DEST_PATH_IMAGE075
=[40,10].，后段的5阶贝塞尔曲线的控制点为
Figure 649082DEST_PATH_IMAGE066
=[120,50],
Figure 370788DEST_PATH_IMAGE078
=[140,60],
Figure 842221DEST_PATH_IMAGE079
=[155,65] ,
Figure DEST_PATH_IMAGE084
=[160,40]，根据情况6的控制点选取规则，选取
Figure 303289DEST_PATH_IMAGE070
=
Figure 936396DEST_PATH_IMAGE071
=[40,10]，
Figure 912442DEST_PATH_IMAGE061
=[50,0],
Figure 921987DEST_PATH_IMAGE062
=[60,-30],
Figure 768720DEST_PATH_IMAGE063
=[94.5,36.5],
Figure DEST_PATH_IMAGE085
=[108,44],
Figure 808613DEST_PATH_IMAGE065
=
Figure 740797DEST_PATH_IMAGE066
=[120,50]。生成的轨迹与控制点如图9所示。
实验例7
情况7：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图10所示。
规则7：前段的3阶贝塞尔控制点表示为(
Figure DEST_PATH_IMAGE086
，后段的5阶贝塞尔曲线的控制点表示为(
Figure DEST_PATH_IMAGE087
，中间的5阶贝塞尔控制点表示为(
Figure 757295DEST_PATH_IMAGE069
。此时需要选取6个参考点，选择方式如下：
Figure 192955DEST_PATH_IMAGE070
选取前直线末尾，使
Figure 699023DEST_PATH_IMAGE070
=
Figure 118503DEST_PATH_IMAGE077
，
Figure 968385DEST_PATH_IMAGE061
的选取应满足：5*(
Figure 789711DEST_PATH_IMAGE061
-
Figure 466680DEST_PATH_IMAGE070
)=3*(
Figure 107877DEST_PATH_IMAGE077
-
Figure DEST_PATH_IMAGE088
)，得
Figure 731756DEST_PATH_IMAGE061
=3*(
Figure 407588DEST_PATH_IMAGE077
-
Figure 989879DEST_PATH_IMAGE088
)/5+
Figure 897135DEST_PATH_IMAGE070
。
Figure 855864DEST_PATH_IMAGE073
的选取应满足：5*4(
Figure 651781DEST_PATH_IMAGE073
-2*
Figure 139394DEST_PATH_IMAGE061
+
Figure 20763DEST_PATH_IMAGE070
)=3*2(
Figure 517603DEST_PATH_IMAGE077
-2*
Figure 168027DEST_PATH_IMAGE088
+
Figure DEST_PATH_IMAGE089
)，得
Figure 59498DEST_PATH_IMAGE073
=3*(
Figure 428162DEST_PATH_IMAGE077
-2*
Figure 463114DEST_PATH_IMAGE088
+
Figure 233624DEST_PATH_IMAGE089
)/10+ 2*
Figure 328619DEST_PATH_IMAGE061
-
Figure 919000DEST_PATH_IMAGE070
。
Figure 757643DEST_PATH_IMAGE065
选取在后曲线的前端，使得
Figure 382660DEST_PATH_IMAGE065
=
Figure 914135DEST_PATH_IMAGE066
。
Figure 227698DEST_PATH_IMAGE064
的选取：应使
Figure 870032DEST_PATH_IMAGE064
与
Figure 349555DEST_PATH_IMAGE078
关于
Figure 786353DEST_PATH_IMAGE066
中心对称，即
Figure 351326DEST_PATH_IMAGE078
-
Figure 531772DEST_PATH_IMAGE066
=
Figure 865801DEST_PATH_IMAGE065
-
Figure 473500DEST_PATH_IMAGE064
，得
Figure 24305DEST_PATH_IMAGE064
=
Figure 8441DEST_PATH_IMAGE065
-
Figure 462557DEST_PATH_IMAGE078
+
Figure 241157DEST_PATH_IMAGE066
。
Figure 780722DEST_PATH_IMAGE063
的选取应满足：(
Figure 302971DEST_PATH_IMAGE079
-
Figure 611592DEST_PATH_IMAGE078
)-(
Figure 561094DEST_PATH_IMAGE078
-
Figure 587956DEST_PATH_IMAGE066
)=(
Figure 149780DEST_PATH_IMAGE065
-
Figure 578488DEST_PATH_IMAGE064
)-(
Figure 964470DEST_PATH_IMAGE064
-
Figure 213048DEST_PATH_IMAGE063
)，得
Figure 77099DEST_PATH_IMAGE063
=
Figure 360313DEST_PATH_IMAGE079
-2*
Figure 651617DEST_PATH_IMAGE078
+2*
Figure 387492DEST_PATH_IMAGE064
。
例7：前段的3阶贝塞尔控制点为
Figure 553769DEST_PATH_IMAGE080
=[-10，-10],
Figure 691489DEST_PATH_IMAGE089
=[0,0],
Figure 419273DEST_PATH_IMAGE082
=[15,20],
Figure 642444DEST_PATH_IMAGE077
=[40,10]，后段的5阶贝塞尔曲线的控制点为
Figure 848298DEST_PATH_IMAGE066
=[130,50],
Figure 840525DEST_PATH_IMAGE078
=[140,60],
Figure 739210DEST_PATH_IMAGE079
=[150,65] ,
Figure 449678DEST_PATH_IMAGE084
=[170,40],
Figure DEST_PATH_IMAGE090
=[175,30] ,
Figure DEST_PATH_IMAGE091
=[185,50]，根据情况7的控制点选取规则，选取
Figure 632791DEST_PATH_IMAGE070
=[40,10]，
Figure 745103DEST_PATH_IMAGE061
=[55,4],
Figure 549111DEST_PATH_IMAGE062
=[73,-11],
Figure 746874DEST_PATH_IMAGE063
=[110,25],
Figure 294530DEST_PATH_IMAGE064
=[120,40],
Figure 261349DEST_PATH_IMAGE065
=[130,50]。生成的轨迹与控制点如图10所示。
实验例8
情况8：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹也为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图11所示。
规则8：前段的3阶贝塞尔控制点表示为(
Figure 501838DEST_PATH_IMAGE086
，后段的3阶贝塞尔曲线的控制点表示为(
Figure 419853DEST_PATH_IMAGE068
，中间的5阶贝塞尔控制点表示为(
Figure 36779DEST_PATH_IMAGE069
。此时需要选取6个参考点，选择方式如下：
Figure 858104DEST_PATH_IMAGE070
选取前直线末尾，使
Figure 269494DEST_PATH_IMAGE070
=
Figure 176270DEST_PATH_IMAGE077
，
Figure 65729DEST_PATH_IMAGE061
的选取应满足：5*(
Figure 7140DEST_PATH_IMAGE061
-
Figure 589431DEST_PATH_IMAGE070
)=3*(
Figure 249083DEST_PATH_IMAGE077
-
Figure 166399DEST_PATH_IMAGE088
)，得
Figure 962317DEST_PATH_IMAGE061
=3*(
Figure 715509DEST_PATH_IMAGE077
-
Figure 596877DEST_PATH_IMAGE088
)/5+
Figure 93718DEST_PATH_IMAGE070
。
Figure 744142DEST_PATH_IMAGE073
的选取应满足：5*4(
Figure 402656DEST_PATH_IMAGE073
-2*
Figure 771321DEST_PATH_IMAGE061
+
Figure 304808DEST_PATH_IMAGE070
)=3*2(
Figure 340897DEST_PATH_IMAGE077
-2*
Figure 170313DEST_PATH_IMAGE088
+
Figure 26274DEST_PATH_IMAGE089
)，得
Figure 864917DEST_PATH_IMAGE073
=3*(
Figure 224354DEST_PATH_IMAGE077
-2*
Figure 490250DEST_PATH_IMAGE088
+
Figure 567927DEST_PATH_IMAGE089
)/10+ 2*
Figure 475841DEST_PATH_IMAGE061
-
Figure 456828DEST_PATH_IMAGE070
。
Figure 893626DEST_PATH_IMAGE065
选取在后曲线的前端，使得
Figure 458599DEST_PATH_IMAGE065
=
Figure 639045DEST_PATH_IMAGE066
。
Figure 973074DEST_PATH_IMAGE064
的选取应满足：3*(
Figure 580773DEST_PATH_IMAGE078
-
Figure 633043DEST_PATH_IMAGE066
)=5*(
Figure 351600DEST_PATH_IMAGE065
-
Figure 304250DEST_PATH_IMAGE064
)，得
Figure 348430DEST_PATH_IMAGE064
=
Figure 887996DEST_PATH_IMAGE065
-3*(
Figure 410244DEST_PATH_IMAGE078
-
Figure 718865DEST_PATH_IMAGE066
)/5。
Figure 668367DEST_PATH_IMAGE063
的选取应满足：3*2*[(
Figure 429649DEST_PATH_IMAGE079
-
Figure 21168DEST_PATH_IMAGE078
)-(
Figure 420181DEST_PATH_IMAGE078
-
Figure 806163DEST_PATH_IMAGE066
)] =5*4*[(
Figure 320321DEST_PATH_IMAGE065
-
Figure 184372DEST_PATH_IMAGE064
)-(
Figure 202007DEST_PATH_IMAGE064
-
Figure 758890DEST_PATH_IMAGE063
)]，得
Figure 494765DEST_PATH_IMAGE063
=3*(
Figure 162507DEST_PATH_IMAGE079
-2*
Figure 300227DEST_PATH_IMAGE078
+
Figure 526547DEST_PATH_IMAGE066
)/10-
Figure 749718DEST_PATH_IMAGE065
+2*
Figure 955571DEST_PATH_IMAGE064
。
例8：前段的3阶贝塞尔控制点为
Figure 947798DEST_PATH_IMAGE080
=[10,-10],
Figure 846484DEST_PATH_IMAGE089
=[23,20],
Figure 291371DEST_PATH_IMAGE088
=[30,30],
Figure 300916DEST_PATH_IMAGE077
=[35,10]，后段的3阶贝塞尔曲线的控制点为
Figure 678807DEST_PATH_IMAGE066
=[120,50],
Figure 482815DEST_PATH_IMAGE078
=[135,55],
Figure 182043DEST_PATH_IMAGE079
=[140,45] ,
Figure 729699DEST_PATH_IMAGE084
=[150,40]，根据情况8的控制点选取规则，选取
Figure 696518DEST_PATH_IMAGE070
=[35,10]，
Figure 671428DEST_PATH_IMAGE061
=[38,-2],
Figure 356487DEST_PATH_IMAGE073
=[40.4,-23],
Figure 707834DEST_PATH_IMAGE063
=[99,39.5],
Figure 529159DEST_PATH_IMAGE064
=[111,47],
Figure 940549DEST_PATH_IMAGE065
=[120,50]。 生成的轨迹与控制点如图11所示。
为验证算法的正确性，附图12中，该初始时路径地图使用了一个，含有多段不同阶(1阶、3阶、5阶)子路径的复杂路径，进行轨迹的平滑连接。由图12可以看出初始路径在连接点处很明显的存在切线方向上的不连续，曲率的连续更无从谈起。
在经过步骤S2中的第二次循环(对3阶贝塞尔曲线进行优化后)，生成的路径如附图13所示可以看出，第二次的循环对三阶贝塞尔曲线进行了优化，三阶贝塞尔曲线针对其相邻的不同曲线类型进行了不同的优化方式。
当临近子路径只有一条直线时，将控制点选取在了直线的延长线上，当临近路径有两条直线时，将控制点选取在了两直线的延长线上交点处。在步骤S3中的第三次循环后(对五阶贝塞尔曲线进行优化后)，生成的路径如附图14所示可以看出，策略对剩余的五阶贝塞尔曲线依次进行了平滑处理，路径的平滑度得到了极大的提升。
从而解决了多段路径曲率连续拼接问题。使得移动机器人的导航路径在交点处切线方向连续并且曲率连续，进而使得机器人角速度的变化率连续，有效提高了移动机器人轨迹跟踪控制的精度与效率。
（二）
本发明第二个方面，所提供的该移动机器人多段路径曲率连续拼接处理机，其在优选实施方式下，包括：导航路径规划模块，路径拼接处理模块，其中移动机器人经扫描模块获取导航数据，以传输至导航路径模块中，经处理后生成多段子路径数据，并向路径拼接处理模块传输。
其中所述路径拼接处理模块采用下述中的至少一种方式对多段子路径进行拼接：使用五阶贝塞尔曲线对子路径为一阶、三阶、五阶的贝塞尔曲线进行路径拼接，或使用三阶贝塞尔曲线对子路径为一阶、三阶的贝塞尔曲线进行拼接；而后，路径拼接处理模块依据实施例一中所述的多段导航路径曲率连续拼接优化方法，对用于拼接的贝塞尔曲线子路径进行优化后，再进行平滑处理。
（三）
本发明第三个方面，所提供的该可读存储介质，在优选实施方式中，其上存储有计算机程序，其中该计算机程序被处理器执行时，实现实施例一中该多段导航路径曲率连续拼接优化方法的步骤。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
同时实现上述实施例方法中的全部或部分步骤，是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (10)

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 设置可优化子路径的控制点坐标，使其前后连接点处的贝塞尔曲线的一阶导数与二阶导数，分别与前/后段子路径连接点处的一阶导数与二阶导数相等。
7.根据权利要求6所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述贝塞尔曲线的求导公式表达为：
Figure DEST_PATH_IMAGE001
其中，
Figure DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure DEST_PATH_IMAGE004
个控制点，且
Figure DEST_PATH_IMAGE005
，
Figure DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure DEST_PATH_IMAGE007
其中
Figure DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure DEST_PATH_IMAGE009
其中
Figure DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，而贝塞尔曲线在起点与末端处的二阶导数值
Figure DEST_PATH_IMAGE012
与
Figure DEST_PATH_IMAGE013
：
Figure DEST_PATH_IMAGE014
。
8.根据权利要求1所述的多段导航路径曲率连续拼接优化方法，其特征在于，所述不可优化子路径包括以下中的至少一种：一阶贝塞尔曲线子路径、预设被锁定的子路径。
9.一种多段导航路径曲率连续拼接优化处理机，其特征在于，包括：导航路径规划模块，路径拼接处理模块，其中移动机器人经扫描模块获取导航数据，以传输至导航路径模块中，经处理后生成多段子路径数据，并向路径拼接处理模块传输，所述路径拼接处理模块采用下述中的至少一种方式对多段子路径进行拼接：使用五阶贝塞尔曲线对子路径为一阶、三阶、五阶的贝塞尔曲线进行路径拼接，或使用三阶贝塞尔曲线对子路径为一阶、三阶的贝塞尔曲线进行拼接；而后，路径拼接处理模块依据如权利要求1至8中任一项所述的多段导航路径曲率连续拼接优化方法，对用于拼接的贝塞尔曲线子路径进行优化后，再进行平滑处理。
10.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-8中任一项所述的多段导航路径曲率连续拼接优化方法的步骤。
CN202111237946.6A 2021-10-25 2021-10-25 多段导航路径曲率连续拼接优化处理机、方法及存储介质 Active CN113686345B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111237946.6A CN113686345B (zh) 2021-10-25 2021-10-25 多段导航路径曲率连续拼接优化处理机、方法及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111237946.6A CN113686345B (zh) 2021-10-25 2021-10-25 多段导航路径曲率连续拼接优化处理机、方法及存储介质

Publications (2)

Publication Number Publication Date
CN113686345A CN113686345A (zh) 2021-11-23
CN113686345B true CN113686345B (zh) 2022-01-25

Family

ID=78587797

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111237946.6A Active CN113686345B (zh) 2021-10-25 2021-10-25 多段导航路径曲率连续拼接优化处理机、方法及存储介质

Country Status (1)

Country Link
CN (1) CN113686345B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114237244B (zh) * 2021-12-15 2023-11-03 东南大学 一种基于光线跟踪的移动机器人路径规划方法
CN114237436A (zh) * 2021-12-22 2022-03-25 杭州群核信息技术有限公司 相机路径绘制方法和基于相机路径绘制的渲染交互系统
CN114137991B (zh) * 2022-02-08 2022-04-26 河北工业大学 一种基于二阶贝塞尔曲线的机器人连续路径优化方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102010012750A1 (de) * 2010-03-25 2011-09-29 Kuka Laboratories Gmbh Verfahren zum Betreiben eines holonomen/omnidirektionalen Flurförderfahrzeugs
CN102998684B (zh) * 2012-11-21 2016-12-21 厦门雅迅网络股份有限公司 一种基于贝塞尔曲线的终端定位轨迹拟合方法
WO2018119745A1 (zh) * 2016-12-28 2018-07-05 深圳配天智能技术研究院有限公司 机器人运动控制方法及相关装置
CN110782504A (zh) * 2019-09-16 2020-02-11 腾讯科技（深圳）有限公司 曲线确定方法、装置、计算机可读存储介质及设备
CN112269965B (zh) * 2020-08-10 2024-04-05 中国北方车辆研究所 一种非完整约束条件下的连续曲率路径优化方法
CN113031592A (zh) * 2021-02-25 2021-06-25 杭州国辰机器人科技有限公司 一种基于五阶贝塞尔曲线的机器人路径平滑方法及系统

Also Published As

Publication number Publication date
CN113686345A (zh) 2021-11-23

Similar Documents

Publication Publication Date Title
CN113686345B (zh) 多段导航路径曲率连续拼接优化处理机、方法及存储介质
CN105225510A (zh) 用于验证地图的路网的方法和系统
CN104252408A (zh) 提供测试用例的方法和装置
CN114724108B (zh) 车道线处理方法及装置
CN105335133A (zh) 用于生成业务规则模型的方法和装置
CN113865594A (zh) 对含有圆弧导航路径段的曲率连续拼接优化方法、装置及存储介质
CN111433780A (zh) 车道线检测方法、设备、计算机可读存储介质
US8423948B2 (en) Wiring design support device and wiring design supporting method
CN113275976B (zh) 计算机控制光学面形误差收敛加工方法、装置及介质
US20190197767A1 (en) Surface extrction method, apparatus, and non-transitory computer readable storage medium thereof
CN110851896A (zh) 基于局部邻域信息的cad外墙体识别方法及装置
CN105912265A (zh) 一种Expander适配配置文件的方法及装置
CN103853526A (zh) 可重构处理器及可重构处理器的条件执行方法
US20070070064A1 (en) Program storage medium storing CAD program for controlling projection and apparatus thereof
US8786599B2 (en) Segmentation of free-form shapes into chains of circular elements in computer graphics applications
CN105892995A (zh) 查找负数的方法、装置及处理器
CN107845036A (zh) 一种金融软件中的自动画线确认方法
CN111143485A (zh) 一种轨迹重合段融合方法、装置、系统及存储介质
CN105868553A (zh) 轨迹数据处理方法
JP2022058659A (ja) 信号灯状態データの検出方法及び装置
CN114611289A (zh) 自动驾驶中参考线的确定方法、装置及电子设备
CN110398681B (zh) 一种双带线检查方法及相关装置
CN106815014A (zh) 一种信息输入提示方法和装置
US20120233581A1 (en) Design support apparatus for semiconductor device, design support program, and layout information generating method
CN107543556A (zh) 闭环路线规划方法及装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant