路径轨迹追踪系统

技术领域

本发明涉及一种系统，尤其涉及一种路径轨迹追踪系统。

背景技术

随着科技的发展，自动导引车、机器人等移动平台技术已经逐渐应用于各个领域。一般来说移动平台皆会遵循一规划路径抵达至一目的地。在移动平台脱离规划路径的时候，大部分会先旋转重新定位，并依照重新定位后的位置重新运算规划一个新的规划路径以抵达上述目的地。然而，重新运算规划路径会占用移动平台的运算资源，而且，移动平台在原本的规划路径上可能有其他待执行的指令动作。

发明内容

有鉴于在现有技术中，移动平台重新运算规划新的规划路径，会占用运算资源，而且可能会使得原来的规划路径上待执行的指令动作无法执行。本发明的一主要目的提供一种路径轨迹追踪系统，用以解决现有技术中的至少一个问题。

本发明为解决现有技术的问题，所采用的必要技术手段为提供一种路径轨迹追踪系统，设置于一移动平台，包含一定位模块、一路径分段模块、一位置向量运算模块、一路径向量运算模块、一路径重建模块、一轨迹追踪向量运算模块与一控制模块。

定位模块，用以定位移动平台而获得一目前坐标。路径分段模块，用以利用多个路径点将移动平台所应遵行的一规划路径划分成多个路段，且每一路径点具有一路径点坐标。位置向量运算模块，电性连接定位模块与路径分段模块，用以接收目前坐标与路径点的路径点坐标，并利用目前坐标分别与每一路径点的路径点坐标运算出一位置向量，藉以运算出多个上述的位置向量。路径向量运算模块，电性连接路径分段模块，用以利用路径点中的一者与次一相邻者定义出一与位置向量相对应的路径向量。

路径重建模块，电性连接位置向量运算模块与路径向量运算模块，藉以利用位置向量与路径向量中的彼此对应者计算出一向量夹角，将向量夹角大于270度与小于90度所对应的路径点删除，以将规划路径剩余的部分重建成一残余路径。轨迹追踪向量运算模块，电性连接路径重建模块与定位模块，用以运算出目前坐标与残余路径之间的最短距离，据以产生一轨迹追踪向量。控制模块，电性连接轨迹追踪向量运算模块，藉以依据轨迹追踪向量产生一控制命令，并利用控制命令控制移动平台沿轨迹追踪向量移动并追迹至残余路径。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统，还包含一路径产生模块，且路径产生模块电性连接路径分段模块，用以产生规划路径。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统中的位置向量运算模块，包含一位置向量运算单元，且位置向量运算单元将目前坐标分别减去每一路径点的路径点坐标以运算出位置向量。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统中的路径向量运算模块，包含一路径向量运算单元，且路径向量运算单元将上述路径点中的次一相邻者减去路径点中的该者以定义出路径向量。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统中的路径重建模块，包含一长度运算单元、一余弦运算单元与一判断单元。长度运算单元，用以运算出每一位置向量与每一路径向量的长度。余弦运算单元，电性连接长度运算单元，并利用位置向量与路径向量中的彼此对应者及其长度计算出向量夹角的一余弦值。判断单元，电性连接余弦运算单元，用以接收余弦值，并在确认余弦值大于0时，判断出向量夹角大于270度或小于90度。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统中的轨迹追踪向量运算模块，包含一向量运算单元与一距离运算单元。向量运算单元，利用被删除的路径点的最后一者所对应的位置向量与路径向量运算出一面积值。距离运算单元，电性连接向量运算单元，并利用面积值与上述路径向量的长度运算出最短距离。

在上述必要技术手段的基础下，本发明所衍生的一附属技术手段为使路径轨迹追踪系统中的轨迹追踪向量运算模块，还包含一方向判断单元与一向量产生单元。方向判断单元，判断出一自目标位置朝向残余路径的追迹方向。向量产生单元，利用最短距离与追迹方向产生轨迹追踪向量。

承上所述，本发明所提供的路径轨迹追踪系统，利用定位模块、路径分段模块、位置向量运算模块与路径向量运算模块运算出位置向量与路径向量，并利用路径重建模块删除部分路径点，并将规划路径剩余的部分重建成残余路径，再控制移动平台以最短距离追迹至残余路径。因此，本发明可以使移动平台不必重新定位并且重新运算新的规划路径，而是直接将旧有的规划路径中的部分路径点删除，并以最短距离追迹至残余路径，使移动平台不必重新定位重新运算新的规画路径并且也可以执行旧有的规划路径上的待执行的指令动作。

附图说明

图1显示本发明较佳实施例所提供的路径轨迹追踪系统的方块图；

图2显示本发明较佳实施例所提供的路径轨迹追踪系统设置于移动平台的立体示意图；

图3显示本发明较佳实施例所提供的路径分段模块划分规划路径的示意图；

图4显示本发明较佳实施例所提供的位置向量运算模块、路径向量运算模块与路径重建模块运算向量与向量夹角的示意图；

图5显示本发明较佳实施例所提供的路径重建模块重建残余路径的示意图；

图6显示本发明较佳实施例所提供的轨迹追踪向量运算模块产生轨迹追踪向量的示意图；以及

图7显示本发明较佳实施例所提供的控制模块控制移动平台追迹的示意图。

附图标号说明：

具体实施方式

下面将结合示意图对本发明的具体实施方式进行更详细的描述。根据下列描述和权利要求，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

请一并参阅图1与图2，其中，图1显示本发明较佳实施例所提供的路径轨迹追踪系统的方块图；以及，图2显示本发明较佳实施例所提供的路径轨迹追踪系统设置于移动平台的立体示意图。如图所示，一种路径轨迹追踪系统1设置于一移动平台2。移动平台2则是位于一移动空间S。在本实施例中，移动平台2绘制一机器人示意，但不以此为限，也可为自动导引车、移动载具。

路径轨迹追踪系统1包含一定位模块11、一路径分段模块12、一位置向量运算模块13、一路径向量运算模块14、一路径重建模块15、一轨迹追踪向量运算模块16、一控制模块17与一路径产生模块18。其中，位置向量运算模块13包含一位置向量运算单元131；路径向量运算模块14包含一路径向量运算单元141；路径重建模块15包含一长度运算单元151、一余弦运算单元152与一判断单元153；轨迹追踪向量运算模块16则是包含一向量运算单元161、一距离运算单元162、一方向判断单元163与一向量产生单元164。

接着，请一并参阅图1至图7，其中，图3显示本发明较佳实施例所提供的路径分段模块划分规划路径的示意图；图4显示本发明较佳实施例所提供的位置向量运算模块、路径向量运算模块与路径重建模块运算向量与向量夹角的示意图；图5显示本发明较佳实施例所提供的路径重建模块重建残余路径的示意图；图6显示本发明较佳实施例所提供的轨迹追踪向量运算模块产生轨迹追踪向量的示意图；以及，图7显示本发明较佳实施例所提供的控制模块控制移动平台追迹的示意图。如图所示，移动平台2所在的移动空间S对应到一地图M。

移动平台2应沿着一规划路径P移动，但是目前移动平台2并不在规划路径P上。另外，需说明的是，将规划路径P绘制于实际的移动空间S上仅是为了明确说明本发明的技术内容。规划路径P并不会实际存在于实际的移动空间S上，而是存在于虚拟的地图M里。

定位模块11会定位移动空间S中的移动平台2，而在地图M上获得一相对应的目前坐标LC。定位模块11可为GPS定位装置、蓝牙定位装置等具有定位功能的装置或器件。

路径分段模块12利用多个路径点(在此绘制五个路径点P1、P2、P3、P4、P5示意)将规划路径P划分成多个路段(在此绘制四个路段PP1、PP2、PP3、PP4示意)，如图3所示。其中，路径点P1、P2、P3、P4、P5各具有一路径点坐标，且路径点P1、P2、P3、P4、P5为移动平台2应遵循移动的顺序，也可以将路径点P1视为起点，路径点P5视为终点。

在本实施例中，路径分段模块12是利用规划路径P的转折点与头尾设定路径点P1、P2、P3、P4、P5，但不以此为限。路径分段模块12也可以利用等距离设定路径点P1、P2、P3、P4、P5，也可以利用等时间间间距设定路径点P1、P2、P3、P4、P5，也可以利用规划路径P的曲率去设定路径点P1、P2、P3、P4、P5。实务上，路径点P1、P2、P3、P4、P5的坐标信息为笛卡尔坐标系。

位置向量运算模块13电性连接定位模块11与路径分段模块12，接收目前坐标LC与路径点P1、P2、P3、P4、P5的路径点坐标。位置向量运算单元131便将目前坐标LC分别减去路径点P1、P2、P3、P4、P5的路径点坐标，以分别形成五个位置向量VL1、VL2、VL3、VL4、VL5。

路径向量运算模块14电性连接路径分段模块12，接收路径点P1、P2、P3、P4、P5的路径点坐标。路径向量运算单元141利用路径点P1、P2、P3、P4、P5的路径点坐标中的一者与次一相邻者定义出一对应位置向量的路径向量。位置向量运算模块13与路径向量运算模块14可为处理器、运算器、运算芯片或是其他具有运算功能的装置。

更详细的说明，路径向量运算单元141将路径点P2的路径点坐标减去路径点P1的路径点坐标以形成路径向量VR1。路径点P1的路径点坐标为上述路径点坐标中的一者，路径点P2的路径点坐标则为上述路径点坐标中的次一相邻者。以此类推，路径向量运算单元141总共会运算出四个路径向量VR1、VR2、VR3、VR4。因为路径点P5后方并无其他路径点，故仅会运算出上述四个路径向量。

路径重建模块15电性连接位置向量运算模块13与路径向量运算模块14，并利用位置向量与路径向量中的彼此对应者计算出一向量夹角。举例来说，路径重建模块15会利用位置向量VL1与路径向量VR1，计算出位置向量VL1与路径向量VR1所形成的向量夹角A1。同理，路径重建模块15还会运算出向量夹角A2、A3、A4。上述位置向量运算模块13、路径向量运算模块14与路径重建模块15的作动行为，即为图4所示。

接着，路径重建模块15会判断向量夹角A1、A2、A3、A4，并将向量夹角A1、A2、A3、A4大于270度与小于90度所对应到的路径点删除，藉以将规划路径P剩余的部分重建成一残余路径PR。

在本实施例中，长度运算单元151会运算出每个向量的长度，包含位置向量VL1、VL2、VL3、VL4、VL5与路径向量VR1、VR2、VR3、VR4。余弦运算单元152，电性连接长度运算单元151，并利用位置向量VL1、VL2、VL3、VL4、VL5、路径向量VR1、VR2、VR3、VR4以及各自的长度，去计算出向量夹角A1、A2、A3、A4的余弦值。向量夹角的余弦值为形成向量夹角的两向量的内积去除以两向量长度的乘积。接着，判断单元153，电性连接余弦运算单元152，用以接收上述余弦值，并在确认余弦值大于0时，判断出向量夹角大于270度与小于90度。在本发明其他实施例中，也可以直接利用角度判断向量夹角是否大于270度与小于90度。

在本实施例中，大于270度与小于90度的有向量夹角A1、A2。因此，路径重建模块15会将向量夹角A1、A2所对应到的路径点P1、P2删除，并将规划路径P剩余的部分重建成残余路径PR。将路径点P1、P2删除的用意在于可以避免移动平台2往路径点P1、P2移动，也就是避免移动平台2浪费时间往起点的方向走。此时，可以将路径点P3暂时视为是残余路径PR的起始点。

轨迹追踪向量运算模块16电性连接路径重建模块15与定位模块11，并运算目前坐标LC与残余路径PR之间的最短距离Hm，藉以产生一轨迹追踪向量DT。也就是说，虽然可以将路径点P3暂时视为是残余路径PR的起始点，但是为了使移动平台2可以尽快追踪至残余路径PR，轨迹追踪向量运算模块16便会去计算目前坐标LC与残余路径PR之间的最短距离Hm，藉以使移动平台2以最短距离Hm，也就是最短时间追踪至残余路径PR。

在本实施例中，向量运算单元161会利用被删除的路径点的最后一者所对应的位置向量与路径向量进行运算，在此为路径点P2所对应的位置向量VL2与路径向量VR2。向量运算单元161会计算位置向量VL2与路径向量VR2的外积再加上绝对值以运算出一面积值。以几何学观点来看，即是位置向量VL2与路径向量VR2所围成的平行四边形的面积值。接着，距离运算单元162会利用面积值除以路径向量VR2的长度，以计算出最短距离Hm，也就是平行四边形以路径向量VR2为底的高。平行四边形的高与底互相垂直，故算出来的高即为最短距离Hm。

方向判断单元163则会去判断出一个自目前坐标LC朝向路径向量VR2的追迹方向D。最后，向量产生单元164会利用追迹方向D与最短距离Hm产生轨迹追踪向量DT，如图5与图6所示。

控制模块17电性连接轨迹追踪向量运算模块16，并依据轨迹追踪向量DT产生一控制命令。接着，再利用控制命令控制移动平台2沿轨迹追踪向量DT以最短距离Hm追迹至规划路径P的残余路径PR。在图7中有绘制虚线方框用以示意移动前的移动平台，并标记为移动平台2’。控制模块17可为控制器、微控制器等具有控制功能的装置或器件。

在此需说明的是，目前坐标LC与规划路径P之间具有另一最短距离Hm1，即目前坐标LC与路段PP1之间的距离。其中，最短距离Hm1的值会小于最短距离Hm。但是本发明并不是直接控制移动平台2沿最短距离Hm1移动。

本发明先删除不必要的路径点P1、P2，再运算出目前坐标LC与残余路径PR之间的最短距离Hm。一来可以避免移动平台2往回走，二来可以确定移动平台2沿最短距离Hm追迹至残余路径PR。从图5可以明显看出，若直接控制移动平台2沿最短距离Hm1移动，还需要多走部分的路段PP1与部分的路段PP2。

综上所述，本发明所提供的路径轨迹追踪系统，可以在移动平台脱离规划路径时，删除不必要的路径点以避免移动平台往起点方向走，且重建残余路径并使移动平台以最短距离追迹至残余路径。相较于现有技术，移动平台需要旋转重新定位，并重新规划新的规划路径，本发明不须重新定位，不须重新规划新的路径，而是删除规划路径中偏向起点的路径点，并以最短路径追迹至删除路径点后的残余路径，可以达到更快追迹至残余路径的功效。此外，若原来的规划路径上还有其他待执行的指令动作，在本发明控制移动平台追迹回残余路径后，移动平台也可继续执行上述待执行的指令动作。

通过以上较佳具体实施例的详述，希望能更加清楚描述本发明的特征与精神，而并非以上述所揭示的较佳具体实施例来对本发明的范畴加以限制。相反地，其目的是希望能涵盖各种改变及具相等性的安排于本发明所欲申请的权利要求的范畴内。