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

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

Info

Publication number
CN113686345A
CN113686345A CN202111237946.6A CN202111237946A CN113686345A CN 113686345 A CN113686345 A CN 113686345A CN 202111237946 A CN202111237946 A CN 202111237946A CN 113686345 A CN113686345 A CN 113686345A
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.)
Granted
Application number
CN202111237946.6A
Other languages
English (en)
Other versions
CN113686345B (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 895184DEST_PATH_IMAGE001
其中，
Figure 408729DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure 922887DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure 911572DEST_PATH_IMAGE004
个控制点，且
Figure 194786DEST_PATH_IMAGE005
，
Figure 610723DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure 346598DEST_PATH_IMAGE007
其中
Figure 138974DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure 604590DEST_PATH_IMAGE009
其中
Figure 66795DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure 414600DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，同理可得贝塞尔曲线在起点与末端处的二阶导数值
Figure 620454DEST_PATH_IMAGE012
与
Figure 737314DEST_PATH_IMAGE013
：
Figure 636000DEST_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 471101DEST_PATH_IMAGE001
其中，
Figure 215066DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure 449083DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure 253091DEST_PATH_IMAGE004
个控制点，且
Figure 575487DEST_PATH_IMAGE005
，
Figure 123143DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure 214596DEST_PATH_IMAGE007
其中
Figure 455085DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure 999198DEST_PATH_IMAGE015
其中
Figure 350545DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure 562084DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，同理可得贝塞尔曲线在起点与末端处的二阶导数值
Figure 973474DEST_PATH_IMAGE012
与
Figure 4884DEST_PATH_IMAGE013
：
Figure 894342DEST_PATH_IMAGE014
。
通过上述公式，可方便的计算出前后段贝塞尔曲线子路径在拼接点处的一阶与二阶导数的数值，同样的中间的贝塞尔曲线拼接子路径在端点出的数值，也可以通过待确定的控制点表示出来。
路径拼接的要求，需要使得路径在连接点处切线方向连续与曲率连续，贝塞尔曲线可以由参数方程
Figure 960387DEST_PATH_IMAGE016
决定。由曲率的计算公式：
Figure 542678DEST_PATH_IMAGE017
单位切线向量的计算公式：
Figure 61384DEST_PATH_IMAGE018
由此可以看出，只需要使得两路径在连接点处一阶导数以及二阶导数连续即可以保证路径在拼接点处切线方向连续与曲率连续。
其中如图13所示，展示了步骤S2中进行了第二次循环后的平滑三阶贝塞尔曲线的多段曲线。
而步骤S3中进行的第三次循环时，会涉及到对于不同类型曲线路径的平滑连接问题。而步骤S2中的第二次循环对于单端路径的拼接问题可以较为简单的从对于双段路径的轨迹拼接问题中简化出来。
因此，在进一步优化过程中主要针对两条子路径拼接轨迹的平滑连接进行优化，如图14所示，展示了步骤S3中进行了第三次循环后的平滑五阶贝塞尔曲线的多段曲线。
另一方面，本发明研究了轨迹的光滑连接问题，其中光滑定义为切线方向连续并且曲率连续。对分段路径进行光滑的拼接不仅能够使机器人运行的更加平稳，也可以提高轨迹跟踪的精度。根据实际拼接时遇到的路径类型，分别考虑由一阶贝塞尔曲线(直线)，三阶贝塞尔曲线与五阶贝塞尔曲线组成的路径。针对实际需求，本方案只考虑8种情况下的路径光滑问题。并且，可以通过组合这8种情况，来解决更加复杂的线路平滑问题，也可以通过简化这8中情况得到在上述第二次循环中需要用到的单条轨迹的优化问题。
其中下述实验例中，使用
Figure 754534DEST_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 412436DEST_PATH_IMAGE020
。同理，第四个与第五个控制点也是可以重合的，即
Figure 493524DEST_PATH_IMAGE021
。
例1：前段路径为直线
Figure 374893DEST_PATH_IMAGE022
，后端路径为直线
Figure 996367DEST_PATH_IMAGE023
.根据情况1的控制点选取规则，选取
Figure 912370DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 898781DEST_PATH_IMAGE025
=
Figure 392079DEST_PATH_IMAGE026
=[10,10],
Figure 427031DEST_PATH_IMAGE027
=[40,10],
Figure 587754DEST_PATH_IMAGE028
=[50,10],
Figure 417170DEST_PATH_IMAGE029
=[60,10]为后段直线的前端点。生成的轨迹与控制点如图4所示。
实验例2
情况2： 考虑前后两段轨迹均为直线，使用3阶贝塞尔曲线进行连接时，如图5所示。
规则2：此情况使情况1的特殊形式，当控制点
Figure 397764DEST_PATH_IMAGE030
时，意味着
Figure 236407DEST_PATH_IMAGE031
四点是重合的，此时情况1中的5阶贝塞尔曲线将会退化为3阶贝塞尔曲线。这时需要选取4个参考点：第一个选取前直线末尾，第二个与第三个重合选取在前后直线的延长线交点处，第四个选取在后直线的前端。
例2：前段路径为直线
Figure 923740DEST_PATH_IMAGE032
，后端路径为直线
Figure 311341DEST_PATH_IMAGE033
.根据情况2的控制点选取规则，选取
Figure 654597DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 421565DEST_PATH_IMAGE025
=
Figure 901088DEST_PATH_IMAGE026
=[10,10],
Figure 462519DEST_PATH_IMAGE034
=[60,10]为后段直线的前端点。生成的轨迹与控制点如图5所示。
实验例3
情况3：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线，使用5阶贝塞尔曲线进行拼接，如图6所示。
规则3：后段5阶贝塞尔曲线的控制点表示为(
Figure 27493DEST_PATH_IMAGE035
,中间的5阶贝塞尔控制点表示为(
Figure 332572DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 666602DEST_PATH_IMAGE024
选取前直线末尾，
Figure 398934DEST_PATH_IMAGE020
选取在前直线的延长线方向上。
Figure 451204DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 294395DEST_PATH_IMAGE029
=
Figure 748510DEST_PATH_IMAGE037
。
Figure 651744DEST_PATH_IMAGE038
的选取：应使
Figure 519206DEST_PATH_IMAGE038
与
Figure 41454DEST_PATH_IMAGE039
关于
Figure 743218DEST_PATH_IMAGE037
中心对称，即
Figure 692720DEST_PATH_IMAGE039
-
Figure 578636DEST_PATH_IMAGE037
=
Figure 904575DEST_PATH_IMAGE029
-
Figure 192337DEST_PATH_IMAGE038
，得
Figure 640636DEST_PATH_IMAGE038
=
Figure 154794DEST_PATH_IMAGE029
-
Figure 143479DEST_PATH_IMAGE039
+
Figure 161113DEST_PATH_IMAGE037
。
Figure 842630DEST_PATH_IMAGE034
的选取应满足：(
Figure 578505DEST_PATH_IMAGE040
-
Figure 370881DEST_PATH_IMAGE039
)-(
Figure 243022DEST_PATH_IMAGE039
-
Figure 95440DEST_PATH_IMAGE037
)=(
Figure 318611DEST_PATH_IMAGE029
-
Figure 935185DEST_PATH_IMAGE038
)-(
Figure 927412DEST_PATH_IMAGE038
-
Figure 950732DEST_PATH_IMAGE034
)，得
Figure 395619DEST_PATH_IMAGE034
=
Figure 529798DEST_PATH_IMAGE040
-2*
Figure 642110DEST_PATH_IMAGE039
+2*
Figure 570752DEST_PATH_IMAGE038
。
例3：前段路径为直线
Figure 768515DEST_PATH_IMAGE022
，后端路径为5阶贝塞尔曲线，且
Figure 644067DEST_PATH_IMAGE037
=[60,0],
Figure 735520DEST_PATH_IMAGE039
=[70,10],
Figure 710429DEST_PATH_IMAGE040
=[80,15] ,
Figure 520122DEST_PATH_IMAGE041
=[110,20],
Figure 871469DEST_PATH_IMAGE042
=[120,30],
Figure 83008DEST_PATH_IMAGE043
=[140,50]。根据情况3的控制点选取规则，选取
Figure 494397DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 528737DEST_PATH_IMAGE025
=
Figure 418195DEST_PATH_IMAGE026
=[10,10],
Figure 484240DEST_PATH_IMAGE027
=[40,-25],
Figure 800952DEST_PATH_IMAGE028
=[50,-10],
Figure 319658DEST_PATH_IMAGE044
=[60,0]。生成的轨迹与控制点如图6所示。
实验例4
情况4：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线时，使用3阶贝塞尔曲线进行拼接时，如图7所示。
规则4：后段3阶贝塞尔曲线的控制点表示为(
Figure 340704DEST_PATH_IMAGE045
，中间的5阶贝塞尔控制点表示为(
Figure 136622DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 14448DEST_PATH_IMAGE024
选取前直线末尾，
Figure 895816DEST_PATH_IMAGE020
选取在前直线的延长线方向上。
Figure 251711DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 902135DEST_PATH_IMAGE029
=
Figure 950863DEST_PATH_IMAGE037
。
Figure 319527DEST_PATH_IMAGE038
的选取应满足：3*
Figure 479113DEST_PATH_IMAGE046
-
Figure 984044DEST_PATH_IMAGE047
=5*(
Figure 200743DEST_PATH_IMAGE029
-
Figure 56703DEST_PATH_IMAGE038
)，得
Figure 754401DEST_PATH_IMAGE038
=
Figure 379417DEST_PATH_IMAGE029
-3*(
Figure 769947DEST_PATH_IMAGE039
-
Figure 847625DEST_PATH_IMAGE037
)/5。
Figure 614593DEST_PATH_IMAGE034
的选取应满足：3*2*[(
Figure 94115DEST_PATH_IMAGE040
-
Figure 655547DEST_PATH_IMAGE039
)-(
Figure 220520DEST_PATH_IMAGE039
-
Figure 260020DEST_PATH_IMAGE037
)] =5*4*[(
Figure 859629DEST_PATH_IMAGE029
-
Figure 591962DEST_PATH_IMAGE038
)-(
Figure 644231DEST_PATH_IMAGE038
-
Figure 487422DEST_PATH_IMAGE034
)]，得
Figure 941538DEST_PATH_IMAGE034
=3*(
Figure 516875DEST_PATH_IMAGE040
-2*
Figure 387267DEST_PATH_IMAGE039
+
Figure 34149DEST_PATH_IMAGE037
)/10-
Figure 139508DEST_PATH_IMAGE029
+2*
Figure 89010DEST_PATH_IMAGE038
。
例4：前段路径为直线
Figure 974926DEST_PATH_IMAGE022
，后端路径为3阶贝塞尔曲线，且
Figure 628761DEST_PATH_IMAGE037
=[60,0],
Figure 588627DEST_PATH_IMAGE039
=[70,10],
Figure 709030DEST_PATH_IMAGE040
=[80,15] ,
Figure 347822DEST_PATH_IMAGE041
=[90,30]。根据情况4的控制点选取规则，选取
Figure 211872DEST_PATH_IMAGE024
=[0,0]为直线末端点，
Figure 354141DEST_PATH_IMAGE020
=[10,10],
Figure 911024DEST_PATH_IMAGE027
=[48,-13.5],
Figure 771533DEST_PATH_IMAGE028
=[54,-6],
Figure 173695DEST_PATH_IMAGE044
=[60,0]。 生成的轨迹与控制点如图7所示。
实验例5
情况5：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图8所示。
规则5：前段的5阶贝塞尔控制点表示为(
Figure 436049DEST_PATH_IMAGE048
，后段的5阶贝塞尔曲线的控制点表示为(
Figure 163834DEST_PATH_IMAGE035
，中间的5阶贝塞尔控制点表示为(
Figure 243130DEST_PATH_IMAGE036
。此时需要选取6个参考点，选择方式如下：
Figure 714562DEST_PATH_IMAGE024
选取前直线末尾，使
Figure 831423DEST_PATH_IMAGE024
=
Figure 464529DEST_PATH_IMAGE049
，
Figure 299630DEST_PATH_IMAGE025
的选取：应使
Figure 309175DEST_PATH_IMAGE025
与
Figure 280542DEST_PATH_IMAGE050
关于
Figure 350129DEST_PATH_IMAGE049
中心对称，即
Figure 406946DEST_PATH_IMAGE025
-
Figure 220182DEST_PATH_IMAGE024
=
Figure 311634DEST_PATH_IMAGE049
-
Figure 286544DEST_PATH_IMAGE050
，得
Figure 96237DEST_PATH_IMAGE025
=
Figure 447584DEST_PATH_IMAGE049
-
Figure 393543DEST_PATH_IMAGE050
+
Figure 539353DEST_PATH_IMAGE024
。
Figure 573693DEST_PATH_IMAGE026
的选取应满足：(
Figure 56627DEST_PATH_IMAGE026
-
Figure 529197DEST_PATH_IMAGE025
)-(
Figure 439384DEST_PATH_IMAGE051
-
Figure 833456DEST_PATH_IMAGE024
)=(
Figure 651239DEST_PATH_IMAGE052
-
Figure 447157DEST_PATH_IMAGE050
)-(
Figure 793825DEST_PATH_IMAGE053
-
Figure 799827DEST_PATH_IMAGE054
)，得
Figure 296667DEST_PATH_IMAGE026
=
Figure 71725DEST_PATH_IMAGE049
-2*
Figure 323715DEST_PATH_IMAGE050
+
Figure 692380DEST_PATH_IMAGE054
+2*
Figure 849036DEST_PATH_IMAGE025
-
Figure 353966DEST_PATH_IMAGE024
。
Figure 573595DEST_PATH_IMAGE029
选取在后曲线的前端，使得
Figure 163976DEST_PATH_IMAGE029
=
Figure 127253DEST_PATH_IMAGE037
。
Figure 752270DEST_PATH_IMAGE038
的选取：应使
Figure 408379DEST_PATH_IMAGE038
与
Figure 486056DEST_PATH_IMAGE039
关于
Figure 987445DEST_PATH_IMAGE037
中心对称，即
Figure 466968DEST_PATH_IMAGE039
-
Figure 28399DEST_PATH_IMAGE037
=
Figure 593373DEST_PATH_IMAGE029
-
Figure 898452DEST_PATH_IMAGE038
，得
Figure 498061DEST_PATH_IMAGE038
=
Figure 230393DEST_PATH_IMAGE029
-
Figure 17084DEST_PATH_IMAGE039
+
Figure 128784DEST_PATH_IMAGE037
。
Figure 582899DEST_PATH_IMAGE034
的选取应满足：(
Figure 486133DEST_PATH_IMAGE040
-
Figure 760119DEST_PATH_IMAGE039
)-(
Figure 407001DEST_PATH_IMAGE055
-
Figure 715623DEST_PATH_IMAGE037
)=(
Figure 789758DEST_PATH_IMAGE044
-
Figure 816620DEST_PATH_IMAGE038
)-(
Figure 1614DEST_PATH_IMAGE028
-
Figure 430321DEST_PATH_IMAGE034
)，得
Figure 675357DEST_PATH_IMAGE034
=
Figure 923936DEST_PATH_IMAGE040
-2*
Figure 912621DEST_PATH_IMAGE039
+2*
Figure 195835DEST_PATH_IMAGE038
。
例5：考虑方案5（前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接）
前段的5阶贝塞尔控制点为：
Figure 611772DEST_PATH_IMAGE056
=[-10，-10],
Figure 347647DEST_PATH_IMAGE057
=[0,0],
Figure 160531DEST_PATH_IMAGE058
=[5,20],
Figure 626147DEST_PATH_IMAGE059
=[20,10],
Figure 88352DEST_PATH_IMAGE053
=[30,20],
Figure 436157DEST_PATH_IMAGE052
=[40,10].，后段的5阶贝塞尔曲线的控制点为
Figure 907590DEST_PATH_IMAGE037
=[140,50],
Figure 24450DEST_PATH_IMAGE039
=[150,60],
Figure 657557DEST_PATH_IMAGE040
=[160,65] ,
Figure 492658DEST_PATH_IMAGE041
=[180,40],
Figure 502202DEST_PATH_IMAGE042
=[190,30] ,
Figure 473569DEST_PATH_IMAGE043
=[200,50]，根据情况5的控制点选取规则，选取
Figure 543156DEST_PATH_IMAGE024
=
Figure 599974DEST_PATH_IMAGE049
=[40,10]，
Figure 413209DEST_PATH_IMAGE060
=[50,0],
Figure 504662DEST_PATH_IMAGE061
=[60,-30],
Figure 745150DEST_PATH_IMAGE062
=[120,25],
Figure 292194DEST_PATH_IMAGE063
=[130,40],
Figure 643541DEST_PATH_IMAGE064
=
Figure 792762DEST_PATH_IMAGE065
=[140,50]. 生成的轨迹与控制点如图8所示。
实验例6
情况6：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图9所示。
规则6：前段的5阶贝塞尔控制点表示为(
Figure 594365DEST_PATH_IMAGE066
，后段的3阶贝塞尔曲线的控制点表示为(
Figure 829037DEST_PATH_IMAGE067
，中间的5阶贝塞尔控制点表示为(
Figure 249655DEST_PATH_IMAGE068
。此时需要选取6个参考点，选择方式如下：
Figure 50120DEST_PATH_IMAGE069
选取前直线末尾，使
Figure 632411DEST_PATH_IMAGE069
=
Figure 354380DEST_PATH_IMAGE070
，
Figure 172163DEST_PATH_IMAGE060
的选取：应使
Figure 968081DEST_PATH_IMAGE060
与
Figure 580328DEST_PATH_IMAGE071
关于
Figure 461696DEST_PATH_IMAGE070
中心对称，即
Figure 80240DEST_PATH_IMAGE060
-
Figure 730665DEST_PATH_IMAGE069
=
Figure 779392DEST_PATH_IMAGE070
-
Figure 882477DEST_PATH_IMAGE071
，得
Figure 42063DEST_PATH_IMAGE060
=
Figure 812573DEST_PATH_IMAGE070
-
Figure 32202DEST_PATH_IMAGE071
+
Figure 622583DEST_PATH_IMAGE069
。
Figure 585860DEST_PATH_IMAGE072
的选取应满足：(
Figure 210876DEST_PATH_IMAGE072
-
Figure 601406DEST_PATH_IMAGE060
)-(
Figure 679084DEST_PATH_IMAGE073
-
Figure 446052DEST_PATH_IMAGE069
)=(
Figure 925574DEST_PATH_IMAGE074
-
Figure 487006DEST_PATH_IMAGE071
)-(
Figure 786400DEST_PATH_IMAGE075
-
Figure 563251DEST_PATH_IMAGE076
)，得
Figure 21914DEST_PATH_IMAGE072
=
Figure 895192DEST_PATH_IMAGE070
-2*
Figure 72095DEST_PATH_IMAGE071
+
Figure 56232DEST_PATH_IMAGE076
+2*
Figure 369402DEST_PATH_IMAGE060
-
Figure 148002DEST_PATH_IMAGE069
。
Figure 812201DEST_PATH_IMAGE064
选取在后曲线的前端，使得
Figure 334450DEST_PATH_IMAGE064
=
Figure 970967DEST_PATH_IMAGE065
。
Figure 717206DEST_PATH_IMAGE063
的选取应满足：3*(
Figure 71964DEST_PATH_IMAGE077
-
Figure 256958DEST_PATH_IMAGE065
)=5*(
Figure 420086DEST_PATH_IMAGE064
-
Figure 927772DEST_PATH_IMAGE063
)，得
Figure 176351DEST_PATH_IMAGE063
=
Figure 165035DEST_PATH_IMAGE064
-3*(
Figure 448249DEST_PATH_IMAGE077
-
Figure 864187DEST_PATH_IMAGE065
)/5。
Figure 600062DEST_PATH_IMAGE062
的选取应满足：3*2*[(
Figure 392437DEST_PATH_IMAGE078
-
Figure 530158DEST_PATH_IMAGE077
)-(
Figure 116997DEST_PATH_IMAGE077
-
Figure 340168DEST_PATH_IMAGE065
)] =5*4*[(
Figure 670655DEST_PATH_IMAGE064
-
Figure 662882DEST_PATH_IMAGE063
)-(
Figure 686201DEST_PATH_IMAGE063
-
Figure 396668DEST_PATH_IMAGE062
)]，得
Figure 265267DEST_PATH_IMAGE062
=3*(
Figure 377580DEST_PATH_IMAGE078
-2*
Figure 309151DEST_PATH_IMAGE077
+
Figure 506914DEST_PATH_IMAGE065
)/10-
Figure 179204DEST_PATH_IMAGE064
+2*
Figure 146023DEST_PATH_IMAGE063
。
例6：前段的5阶贝塞尔控制点为
Figure 511145DEST_PATH_IMAGE079
=[-10，-10],
Figure 930625DEST_PATH_IMAGE080
=[0,0],
Figure 406606DEST_PATH_IMAGE081
=[5,20],
Figure 227932DEST_PATH_IMAGE082
=[20,10],
Figure 763955DEST_PATH_IMAGE075
=[30,20],
Figure 670731DEST_PATH_IMAGE074
=[40,10].，后段的5阶贝塞尔曲线的控制点为
Figure 684824DEST_PATH_IMAGE065
=[120,50],
Figure 891814DEST_PATH_IMAGE077
=[140,60],
Figure 333160DEST_PATH_IMAGE078
=[155,65] ,
Figure 727232DEST_PATH_IMAGE083
=[160,40]，根据情况6的控制点选取规则，选取
Figure 810595DEST_PATH_IMAGE069
=
Figure 75354DEST_PATH_IMAGE070
=[40,10]，
Figure 215829DEST_PATH_IMAGE060
=[50,0],
Figure 831619DEST_PATH_IMAGE061
=[60,-30],
Figure 453093DEST_PATH_IMAGE062
=[94.5,36.5],
Figure 103517DEST_PATH_IMAGE084
=[108,44],
Figure 886665DEST_PATH_IMAGE064
=
Figure 255330DEST_PATH_IMAGE065
=[120,50]。生成的轨迹与控制点如图9所示。
实验例7
情况7：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图10所示。
规则7：前段的3阶贝塞尔控制点表示为(
Figure 680495DEST_PATH_IMAGE085
，后段的5阶贝塞尔曲线的控制点表示为(
Figure 451005DEST_PATH_IMAGE086
，中间的5阶贝塞尔控制点表示为(
Figure 405054DEST_PATH_IMAGE068
。此时需要选取6个参考点，选择方式如下：
Figure 261015DEST_PATH_IMAGE069
选取前直线末尾，使
Figure 224292DEST_PATH_IMAGE069
=
Figure 583729DEST_PATH_IMAGE076
，
Figure 974259DEST_PATH_IMAGE060
的选取应满足：5*(
Figure 51936DEST_PATH_IMAGE060
-
Figure 818904DEST_PATH_IMAGE069
)=3*(
Figure 298427DEST_PATH_IMAGE076
-
Figure 862788DEST_PATH_IMAGE087
)，得
Figure 427761DEST_PATH_IMAGE060
=3*(
Figure 732841DEST_PATH_IMAGE076
-
Figure 66870DEST_PATH_IMAGE087
)/5+
Figure 799203DEST_PATH_IMAGE069
。
Figure 851472DEST_PATH_IMAGE072
的选取应满足：5*4(
Figure 694664DEST_PATH_IMAGE072
-2*
Figure 148779DEST_PATH_IMAGE060
+
Figure 52013DEST_PATH_IMAGE069
)=3*2(
Figure 591578DEST_PATH_IMAGE076
-2*
Figure 238460DEST_PATH_IMAGE087
+
Figure 547082DEST_PATH_IMAGE088
)，得
Figure 824479DEST_PATH_IMAGE072
=3*(
Figure 710396DEST_PATH_IMAGE076
-2*
Figure 36335DEST_PATH_IMAGE087
+
Figure 996201DEST_PATH_IMAGE088
)/10+ 2*
Figure 465007DEST_PATH_IMAGE060
-
Figure 103799DEST_PATH_IMAGE069
。
Figure 967850DEST_PATH_IMAGE064
选取在后曲线的前端，使得
Figure 110118DEST_PATH_IMAGE064
=
Figure 463739DEST_PATH_IMAGE065
。
Figure 527510DEST_PATH_IMAGE063
的选取：应使
Figure 195252DEST_PATH_IMAGE063
与
Figure 192027DEST_PATH_IMAGE077
关于
Figure 716549DEST_PATH_IMAGE065
中心对称，即
Figure 939720DEST_PATH_IMAGE077
-
Figure 473469DEST_PATH_IMAGE065
=
Figure 262434DEST_PATH_IMAGE064
-
Figure 285754DEST_PATH_IMAGE063
，得
Figure 730641DEST_PATH_IMAGE063
=
Figure 864820DEST_PATH_IMAGE064
-
Figure 977132DEST_PATH_IMAGE077
+
Figure 908703DEST_PATH_IMAGE065
。
Figure 106467DEST_PATH_IMAGE062
的选取应满足：(
Figure 778756DEST_PATH_IMAGE078
-
Figure 745575DEST_PATH_IMAGE077
)-(
Figure 845118DEST_PATH_IMAGE077
-
Figure 530178DEST_PATH_IMAGE065
)=(
Figure 6158DEST_PATH_IMAGE064
-
Figure 827484DEST_PATH_IMAGE063
)-(
Figure 363507DEST_PATH_IMAGE063
-
Figure 270284DEST_PATH_IMAGE062
)，得
Figure 284376DEST_PATH_IMAGE062
=
Figure 225787DEST_PATH_IMAGE078
-2*
Figure 667133DEST_PATH_IMAGE077
+2*
Figure 61205DEST_PATH_IMAGE063
。
例7：前段的3阶贝塞尔控制点为
Figure 878988DEST_PATH_IMAGE079
=[-10，-10],
Figure 674906DEST_PATH_IMAGE088
=[0,0],
Figure 549802DEST_PATH_IMAGE081
=[15,20],
Figure 431171DEST_PATH_IMAGE076
=[40,10]，后段的5阶贝塞尔曲线的控制点为
Figure 787066DEST_PATH_IMAGE065
=[130,50],
Figure 437490DEST_PATH_IMAGE077
=[140,60],
Figure 220638DEST_PATH_IMAGE078
=[150,65] ,
Figure 854882DEST_PATH_IMAGE083
=[170,40],
Figure 14468DEST_PATH_IMAGE089
=[175,30] ,
Figure 519398DEST_PATH_IMAGE090
=[185,50]，根据情况7的控制点选取规则，选取
Figure 942289DEST_PATH_IMAGE069
=[40,10]，
Figure 922884DEST_PATH_IMAGE060
=[55,4],
Figure 495948DEST_PATH_IMAGE061
=[73,-11],
Figure 245598DEST_PATH_IMAGE062
=[110,25],
Figure 511494DEST_PATH_IMAGE063
=[120,40],
Figure 713805DEST_PATH_IMAGE064
=[130,50]。生成的轨迹与控制点如图10所示。
实验例8
情况8：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹也为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图11所示。
规则8：前段的3阶贝塞尔控制点表示为(
Figure 152877DEST_PATH_IMAGE085
，后段的3阶贝塞尔曲线的控制点表示为(
Figure 632400DEST_PATH_IMAGE067
，中间的5阶贝塞尔控制点表示为(
Figure 400023DEST_PATH_IMAGE068
。此时需要选取6个参考点，选择方式如下：
Figure 89630DEST_PATH_IMAGE069
选取前直线末尾，使
Figure 801234DEST_PATH_IMAGE069
=
Figure 400843DEST_PATH_IMAGE076
，
Figure 133176DEST_PATH_IMAGE060
的选取应满足：5*(
Figure 185445DEST_PATH_IMAGE060
-
Figure 28636DEST_PATH_IMAGE069
)=3*(
Figure 482752DEST_PATH_IMAGE076
-
Figure 385985DEST_PATH_IMAGE087
)，得
Figure 925551DEST_PATH_IMAGE060
=3*(
Figure 572433DEST_PATH_IMAGE076
-
Figure 881055DEST_PATH_IMAGE087
)/5+
Figure 955190DEST_PATH_IMAGE069
。
Figure 716473DEST_PATH_IMAGE072
的选取应满足：5*4(
Figure 167046DEST_PATH_IMAGE072
-2*
Figure 330174DEST_PATH_IMAGE060
+
Figure 572281DEST_PATH_IMAGE069
)=3*2(
Figure 86438DEST_PATH_IMAGE076
-2*
Figure 75123DEST_PATH_IMAGE087
+
Figure 92758DEST_PATH_IMAGE088
)，得
Figure 774275DEST_PATH_IMAGE072
=3*(
Figure 510150DEST_PATH_IMAGE076
-2*
Figure 36946DEST_PATH_IMAGE087
+
Figure 174666DEST_PATH_IMAGE088
)/10+ 2*
Figure 27084DEST_PATH_IMAGE060
-
Figure 984676DEST_PATH_IMAGE069
。
Figure 580743DEST_PATH_IMAGE064
选取在后曲线的前端，使得
Figure 572969DEST_PATH_IMAGE064
=
Figure 330710DEST_PATH_IMAGE065
。
Figure 41177DEST_PATH_IMAGE063
的选取应满足：3*(
Figure 175355DEST_PATH_IMAGE077
-
Figure 756509DEST_PATH_IMAGE065
)=5*(
Figure 219239DEST_PATH_IMAGE064
-
Figure 151423DEST_PATH_IMAGE063
)，得
Figure 823713DEST_PATH_IMAGE063
=
Figure 790532DEST_PATH_IMAGE064
-3*(
Figure 890075DEST_PATH_IMAGE077
-
Figure 575134DEST_PATH_IMAGE065
)/5。
Figure 51115DEST_PATH_IMAGE062
的选取应满足：3*2*[(
Figure 872440DEST_PATH_IMAGE078
-
Figure 142884DEST_PATH_IMAGE077
)-(
Figure 49661DEST_PATH_IMAGE077
-
Figure 329332DEST_PATH_IMAGE065
)] =5*4*[(
Figure 5164DEST_PATH_IMAGE064
-
Figure 712089DEST_PATH_IMAGE063
)-(
Figure 106161DEST_PATH_IMAGE063
-
Figure 923944DEST_PATH_IMAGE062
)]，得
Figure 719862DEST_PATH_IMAGE062
=3*(
Figure 594759DEST_PATH_IMAGE078
-2*
Figure 210548DEST_PATH_IMAGE077
+
Figure 832022DEST_PATH_IMAGE065
)/10-
Figure 482446DEST_PATH_IMAGE064
+2*
Figure 531174DEST_PATH_IMAGE063
。
例8：前段的3阶贝塞尔控制点为
Figure 899838DEST_PATH_IMAGE079
=[10,-10],
Figure 59424DEST_PATH_IMAGE088
=[23,20],
Figure 564355DEST_PATH_IMAGE087
=[30,30],
Figure 783983DEST_PATH_IMAGE076
=[35,10]，后段的3阶贝塞尔曲线的控制点为
Figure 374365DEST_PATH_IMAGE065
=[120,50],
Figure 9745DEST_PATH_IMAGE077
=[135,55],
Figure 962658DEST_PATH_IMAGE078
=[140,45] ,
Figure 25292DEST_PATH_IMAGE083
=[150,40]，根据情况8的控制点选取规则，选取
Figure 227603DEST_PATH_IMAGE069
=[35,10]，
Figure 604358DEST_PATH_IMAGE060
=[38,-2],
Figure 208514DEST_PATH_IMAGE072
=[40.4,-23],
Figure 645312DEST_PATH_IMAGE062
=[99,39.5],
Figure 337849DEST_PATH_IMAGE063
=[111,47],
Figure 518295DEST_PATH_IMAGE064
=[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 414944DEST_PATH_IMAGE001
其中，
Figure 21506DEST_PATH_IMAGE002
为贝塞尔曲线的阶数，
Figure 218132DEST_PATH_IMAGE003
是贝塞尔曲线的第
Figure 910145DEST_PATH_IMAGE004
个控制点，且
Figure 847489DEST_PATH_IMAGE005
，
Figure 574137DEST_PATH_IMAGE006
，其中一阶导数可以表示为：
Figure DEST_PATH_IMAGE007
其中
Figure 144926DEST_PATH_IMAGE008
，因此贝塞尔曲线在端点处的一阶导数可以表示为：
Figure 589814DEST_PATH_IMAGE009
其中
Figure 68200DEST_PATH_IMAGE010
表示其在起点处的一阶导数值，
Figure 649354DEST_PATH_IMAGE011
表示其在末端处的一阶导数值，而贝塞尔曲线在起点与末端处的二阶导数值
Figure 922204DEST_PATH_IMAGE012
与
Figure 854388DEST_PATH_IMAGE013
：
Figure 870885DEST_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 true CN113686345A (zh) 2021-11-23
CN113686345B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114137991A (zh) * 2022-02-08 2022-03-04 河北工业大学 一种基于二阶贝塞尔曲线的机器人连续路径优化方法
CN114237244A (zh) * 2021-12-15 2022-03-25 东南大学 一种基于光线跟踪的移动机器人路径规划方法

Citations (6)

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

Patent Citations (7)

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

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114237244A (zh) * 2021-12-15 2022-03-25 东南大学 一种基于光线跟踪的移动机器人路径规划方法
CN114237244B (zh) * 2021-12-15 2023-11-03 东南大学 一种基于光线跟踪的移动机器人路径规划方法
CN114137991A (zh) * 2022-02-08 2022-03-04 河北工业大学 一种基于二阶贝塞尔曲线的机器人连续路径优化方法
CN114137991B (zh) * 2022-02-08 2022-04-26 河北工业大学 一种基于二阶贝塞尔曲线的机器人连续路径优化方法

Also Published As

Publication number Publication date
CN113686345B (zh) 2022-01-25

Similar Documents

Publication Publication Date Title
CN113686345B (zh) 多段导航路径曲率连续拼接优化处理机、方法及存储介质
CN105225510A (zh) 用于验证地图的路网的方法和系统
CN104252408A (zh) 提供测试用例的方法和装置
KR101911693B1 (ko) 치아 변연선 추출 방법, 장치 및 치아 변연선 추출 방법을 수행하는 프로그램이 기록된 기록매체
CN114724108B (zh) 车道线处理方法及装置
CN105335133A (zh) 用于生成业务规则模型的方法和装置
CN110458920B (zh) 一种笔迹擦除方法及装置
CN111765893B (zh) 电子地图道路挂接方法及设备
CN113865594A (zh) 对含有圆弧导航路径段的曲率连续拼接优化方法、装置及存储介质
CN106415517A (zh) Usb 3.1重定时器存在检测和索引的方法与装置
US8423948B2 (en) Wiring design support device and wiring design supporting method
US8595682B2 (en) Phase compensation in a differential pair of transmission lines
CN113275976B (zh) 计算机控制光学面形误差收敛加工方法、装置及介质
CN105912265A (zh) 一种Expander适配配置文件的方法及装置
CN113848922A (zh) 对含有直线路径轨迹的简并拼接方法、装置及其存储介质
CN107766865A (zh) 池化方法、物体检测方法、装置、系统及计算机可读介质
US8786599B2 (en) Segmentation of free-form shapes into chains of circular elements in computer graphics applications
CN107845036A (zh) 一种金融软件中的自动画线确认方法
CN111143485A (zh) 一种轨迹重合段融合方法、装置、系统及存储介质
CN111798015B (zh) 一种规划路线调整方法、装置、电子设备及存储介质
JPS6286412A (ja) 電子部品挿入順序決定方法
CN115489550A (zh) 车辆调头控制方法、装置、设备及计算机可读存储介质
CN105868553A (zh) 轨迹数据处理方法
CN106815014A (zh) 一种信息输入提示方法和装置
CN110398681A (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