具体实施方式

下面对本发明的具体实施方式进行详细地说明。以下示例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

为了使本领域的技术人员更好的理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“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 设置可优化子路径的控制点坐标，使其前后连接点处的贝塞尔曲线的一阶导数与二阶导数，分别与前/后段子路径连接点处的一阶导数与二阶导数相等。

其中该设计原理为：考虑前后段子路径，根据前段子路径末端点处的一阶与二阶导数，与后段子路径前端点处的一阶与二阶导数，来选取评价子路径的三阶或者五阶贝塞尔曲线的控制点，使得其与前后段子路径连接处切线方向连续，曲率连续。

其中对于贝塞尔曲线的求导公式表达为：

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

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

其中表示其在起点处的一阶导数值，表示其在末端处的一阶导数值，同理可得贝塞尔曲线在起点与末端处的二阶导数值与：

。

通过上述公式，可方便的计算出前后段贝塞尔曲线子路径在拼接点处的一阶与二阶导数的数值，同样的中间的贝塞尔曲线拼接子路径在端点出的数值，也可以通过待确定的控制点表示出来。

路径拼接的要求，需要使得路径在连接点处切线方向连续与曲率连续，贝塞尔曲线可以由参数方程决定。由曲率的计算公式：

单位切线向量的计算公式：

由此可以看出，只需要使得两路径在连接点处一阶导数以及二阶导数连续即可以保证路径在拼接点处切线方向连续与曲率连续。

其中如图13所示，展示了步骤S2中进行了第二次循环后的平滑三阶贝塞尔曲线的多段曲线。

而步骤S3中进行的第三次循环时，会涉及到对于不同类型曲线路径的平滑连接问题。而步骤S2中的第二次循环对于单端路径的拼接问题可以较为简单的从对于双段路径的轨迹拼接问题中简化出来。

因此，在进一步优化过程中主要针对两条子路径拼接轨迹的平滑连接进行优化，如图14所示，展示了步骤S3中进行了第三次循环后的平滑五阶贝塞尔曲线的多段曲线。

另一方面，本发明研究了轨迹的光滑连接问题，其中光滑定义为切线方向连续并且曲率连续。对分段路径进行光滑的拼接不仅能够使机器人运行的更加平稳，也可以提高轨迹跟踪的精度。根据实际拼接时遇到的路径类型，分别考虑由一阶贝塞尔曲线(直线)，三阶贝塞尔曲线与五阶贝塞尔曲线组成的路径。针对实际需求，本方案只考虑8种情况下的路径光滑问题。并且，可以通过组合这8种情况，来解决更加复杂的线路平滑问题，也可以通过简化这8中情况得到在上述第二次循环中需要用到的单条轨迹的优化问题。

其中下述实验例中，使用表示第i条贝塞尔曲线的第j+1个控制点，且i = 1, 2,3, j=0,1,2,3,4,5。

实验例1

情况1： 考虑前后两段轨迹均为直线，使用5阶贝塞尔曲线进行连接时，如图4所示。

规则1：由于直线的切线是固定的并且其二阶导数恒等于0，这时的控制点选取规则可以被简单的分析出来而无需经过求导进行复杂运算。因此可以选取6个参考点：第一个选取前直线末尾，第二个选取在直线的延长线上使得与直线的切线方向一致。第三个点为了使其二阶导数为0，也选取在前直线的延长线方向上。

同理，为了保证路径与后段直线光滑连接也需要将第四个与第五个控制点选取在后直线的延长线上，第六个控制点选取在后直线的前端。注：第二个与第三个控制点均选取到直线的延长线方向上，是可以重合的，即。同理，第四个与第五个控制点也是可以重合的，即。

例1：前段路径为直线，后端路径为直线.根据情况1的控制点选取规则，选取 =[0,0]为直线末端点，==[10,10], =[40,10], =[50,10],=[60,10]为后段直线的前端点。生成的轨迹与控制点如图4所示。

实验例2

情况2： 考虑前后两段轨迹均为直线，使用3阶贝塞尔曲线进行连接时，如图5所示。

规则2：此情况使情况1的特殊形式，当控制点时，意味着四点是重合的，此时情况1中的5阶贝塞尔曲线将会退化为3阶贝塞尔曲线。这时需要选取4个参考点：第一个选取前直线末尾，第二个与第三个重合选取在前后直线的延长线交点处，第四个选取在后直线的前端。

例2：前段路径为直线，后端路径为直线.根据情况2的控制点选取规则，选取=[0,0]为直线末端点，==[10,10],=[60,10]为后段直线的前端点。生成的轨迹与控制点如图5所示。

实验例3

情况3：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线，使用5阶贝塞尔曲线进行拼接，如图6所示。

规则3：后段5阶贝塞尔曲线的控制点表示为(,中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，

选取在前直线的延长线方向上。

选取在后曲线的前端，使得=。

的选取：应使与关于中心对称，即-=-，得=-+。

的选取应满足：(-)-(-)=(-)-(-)，得=-2*+2*。

例3：前段路径为直线，后端路径为5阶贝塞尔曲线，且=[60,0],=[70,10],=[80,15] , =[110,20],=[120,30],=[140,50]。根据情况3的控制点选取规则，选取=[0,0]为直线末端点，==[10,10],=[40,-25],=[50,-10],=[60,0]。生成的轨迹与控制点如图6所示。

实验例4

情况4：考虑前段轨迹为直线，后段轨迹为5阶贝塞尔曲线时，使用3阶贝塞尔曲线进行拼接时，如图7所示。

规则4：后段3阶贝塞尔曲线的控制点表示为(，中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，

选取在前直线的延长线方向上。

选取在后曲线的前端，使得=。

的选取应满足：3*-=5*(-)，得=-3*(-)/5。

的选取应满足：3*2*[(-)-(-)] =5*4*[(-)-(-)]，得=3*(-2*+)/10-+2*。

例4：前段路径为直线，后端路径为3阶贝塞尔曲线，且=[60,0],=[70,10],=[80,15] , =[90,30]。根据情况4的控制点选取规则，选取=[0,0]为直线末端点，=[10,10],=[48,-13.5],=[54,-6],=[60,0]。 生成的轨迹与控制点如图7所示。

实验例5

情况5：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图8所示。

规则5：前段的5阶贝塞尔控制点表示为(，后段的5阶贝塞尔曲线的控制点表示为(，中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，使=，

的选取：应使与关于中心对称，即-=-，得=-+。

的选取应满足：(-)-(-)=(-)-(-)，得=-2*++2*-。

选取在后曲线的前端，使得=。

的选取：应使与关于中心对称，即-=-，得=-+。

的选取应满足：(-)-(-)=(-)-(-)，得=-2*+2*。

例5：考虑方案5（前段轨迹为5阶贝塞尔曲线，后段轨迹也为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接）

前段的5阶贝塞尔控制点为：

=[-10，-10],=[0,0],=[5,20],=[20,10],=[30,20],=[40,10].，后段的5阶贝塞尔曲线的控制点为=[140,50],=[150,60],=[160,65] , =[180,40],=[190,30] , =[200,50]，根据情况5的控制点选取规则，选取==[40,10]，=[50,0],=[60,-30],=[120,25],=[130,40],==[140,50]. 生成的轨迹与控制点如图8所示。

实验例6

情况6：考虑了前段轨迹为5阶贝塞尔曲线，后段轨迹为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图9所示。

规则6：前段的5阶贝塞尔控制点表示为(，后段的3阶贝塞尔曲线的控制点表示为(，中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，使=，

的选取：应使与关于中心对称，即-=-，得=-+。

的选取应满足：(-)-(-)=(-)-(-)，得=-2*++2*-。

选取在后曲线的前端，使得=。

的选取应满足：3*(-)=5*(-)，得=-3*(-)/5。

的选取应满足：3*2*[(-)-(-)] =5*4*[(-)-(-)]，得=3*(-2*+)/10-+2*。

例6：前段的5阶贝塞尔控制点为

=[-10，-10],=[0,0],=[5,20],=[20,10],=[30,20],=[40,10].，后段的5阶贝塞尔曲线的控制点为=[120,50],=[140,60],=[155,65] , =[160,40]，根据情况6的控制点选取规则，选取==[40,10]，=[50,0],=[60,-30],=[94.5,36.5],=[108,44],==[120,50]。生成的轨迹与控制点如图9所示。

实验例7

情况7：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹为5阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图10所示。

规则7：前段的3阶贝塞尔控制点表示为(，后段的5阶贝塞尔曲线的控制点表示为(，中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，使=，

的选取应满足：5*(-)=3*(-)，得=3*(-)/5+。

的选取应满足：5*4(-2*+)=3*2(-2*+)，得=3*(-2*+)/10+ 2*-。

选取在后曲线的前端，使得=。

的选取：应使与关于中心对称，即-=-，得=-+。

的选取应满足：(-)-(-)=(-)-(-)，得=-2*+2*。

例7：前段的3阶贝塞尔控制点为=[-10，-10], =[0,0],=[15,20],=[40,10]，后段的5阶贝塞尔曲线的控制点为=[130,50],=[140,60],=[150,65] ,=[170,40],=[175,30] , =[185,50]，根据情况7的控制点选取规则，选取=[40,10]，=[55,4],=[73,-11],=[110,25],=[120,40],=[130,50]。生成的轨迹与控制点如图10所示。

实验例8

情况8：考虑了前段轨迹为3阶贝塞尔曲线，后段轨迹也为3阶贝塞尔曲线时，使用5阶贝塞尔曲线进行拼接，如图11所示。

规则8：前段的3阶贝塞尔控制点表示为(，后段的3阶贝塞尔曲线的控制点表示为(，中间的5阶贝塞尔控制点表示为(。此时需要选取6个参考点，选择方式如下：

选取前直线末尾，使=，

的选取应满足：5*(-)=3*(-)，得=3*(-)/5+。

的选取应满足：5*4(-2*+)=3*2(-2*+)，得=3*(-2*+)/10+ 2*-。

选取在后曲线的前端，使得=。

的选取应满足：3*(-)=5*(-)，得=-3*(-)/5。

的选取应满足：3*2*[(-)-(-)] =5*4*[(-)-(-)]，得=3*(-2*+)/10-+2*。

例8：前段的3阶贝塞尔控制点为=[10,-10],=[23,20],=[30,30],=[35,10]，后段的3阶贝塞尔曲线的控制点为=[120,50],=[135,55],=[140,45] ,=[150,40]，根据情况8的控制点选取规则，选取=[35,10]，=[38,-2],=[40.4,-23],=[99,39.5],=[111,47],=[120,50]。 生成的轨迹与控制点如图11所示。

为验证算法的正确性，附图12中，该初始时路径地图使用了一个，含有多段不同阶(1阶、3阶、5阶)子路径的复杂路径，进行轨迹的平滑连接。由图12可以看出初始路径在连接点处很明显的存在切线方向上的不连续，曲率的连续更无从谈起。

在经过步骤S2中的第二次循环(对3阶贝塞尔曲线进行优化后)，生成的路径如附图13所示可以看出，第二次的循环对三阶贝塞尔曲线进行了优化，三阶贝塞尔曲线针对其相邻的不同曲线类型进行了不同的优化方式。

当临近子路径只有一条直线时，将控制点选取在了直线的延长线上，当临近路径有两条直线时，将控制点选取在了两直线的延长线上交点处。在步骤S3中的第三次循环后(对五阶贝塞尔曲线进行优化后)，生成的路径如附图14所示可以看出，策略对剩余的五阶贝塞尔曲线依次进行了平滑处理，路径的平滑度得到了极大的提升。

从而解决了多段路径曲率连续拼接问题。使得移动机器人的导航路径在交点处切线方向连续并且曲率连续，进而使得机器人角速度的变化率连续，有效提高了移动机器人轨迹跟踪控制的精度与效率。

（二）

本发明第二个方面，所提供的该移动机器人多段路径曲率连续拼接处理机，其在优选实施方式下，包括：导航路径规划模块，路径拼接处理模块，其中移动机器人经扫描模块获取导航数据，以传输至导航路径模块中，经处理后生成多段子路径数据，并向路径拼接处理模块传输。

其中所述路径拼接处理模块采用下述中的至少一种方式对多段子路径进行拼接：使用五阶贝塞尔曲线对子路径为一阶、三阶、五阶的贝塞尔曲线进行路径拼接，或使用三阶贝塞尔曲线对子路径为一阶、三阶的贝塞尔曲线进行拼接；而后，路径拼接处理模块依据实施例一中所述的多段导航路径曲率连续拼接优化方法，对用于拼接的贝塞尔曲线子路径进行优化后，再进行平滑处理。

（三）

本发明第三个方面，所提供的该可读存储介质，在优选实施方式中，其上存储有计算机程序，其中该计算机程序被处理器执行时，实现实施例一中该多段导航路径曲率连续拼接优化方法的步骤。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

同时实现上述实施例方法中的全部或部分步骤，是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。