轨迹规划方法与系统

摘要

本公开涉及一种轨迹规划方法与系统，通过自走装置及计算装置执行以下步骤：自走装置于路径上行走形成移动轨迹，而计算装置取得自走装置的感测模块在路径上产生的感测数据文件，将感测数据文件导入同步定位与地图构建演算法后，可建构对应路径的地图，若导入路径闭环演算法后，可取得每一单位时间的姿态信息，并形成最佳化轨迹，导入制图定位演算法时可构成参考轨迹，判断参考轨迹是否趋近于最佳化轨迹，根据趋近于最佳化轨迹的参考轨迹得到能够导入自走装置的参数，使自走装置于路径上行走的移动轨迹对应于前述参考轨迹。

权利要求

1.一种轨迹规划方法，适用于一自走装置，其特征在于，该自走装置于一路径上行走形成一移动轨迹，该轨迹规划方法包括：取得该自走装置的一感测模块在该路径上产生的一感测数据文件；

将该感测数据文件导入一同步定位与地图构建演算法，以建构对应该路径的一地图；

将该感测数据文件导入一路径闭环演算法，取得每一单位时间的姿态信息，并于该地图构成一最佳化轨迹；

将该感测数据文件导入一制图定位演算法，取得每一单位时间的姿态信息，并于该地图构成一参考轨迹，该参考轨迹对应于该自走装置于该路径的该移动轨迹；

比对该参考轨迹及该最佳化轨迹，以判断该参考轨迹是否趋近于该最佳化轨迹；以及如否，则调整该制图定位演算法中的参数；

如是，则将该制图定位演算法中的参数导入该自走装置。

2.如权利要求1所述的轨迹规划方法，其特征在于，该感测数据文件包括多个对应于时间、位置与方向性的感测数据。

3.如权利要求2所述的轨迹规划方法，其特征在于，所述多个感测数据是由该感测模块的一光学雷达、一惯性测量单元与一里程计的至少其中之一所产生。

4.如权利要求1所述的轨迹规划方法，其特征在于，以一差异分布方法比对该参考轨迹及该最佳化轨迹。

5.如权利要求4所述的轨迹规划方法，其特征在于，于该差异分布方法中，计算该参考轨迹相对于该最佳化轨迹在每个单位时间的一姿态信息误差值，以取得该姿态信息误差值的多个误差范围的分布比例。

6.如权利要求5所述的轨迹规划方法，其特征在于，取出该误差范围的一最小误差范围的分布比例与一预设分布比例比较，以判断该参考轨迹是否趋近于该最佳化轨迹。

7.如权利要求6所述的轨迹规划方法，其特征在于，若该最小误差范围的分布比例小于该预设分布比例，则调整该制图定位演算法中的参数，再将该感测数据文件导入已调整参数的该制图定位演算法，以重新取得每一单位时间的姿态信息，得出一新的参考轨迹，再次比对该新的参考轨迹及该最佳化轨迹后，取得一新的最小误差范围的分布比例；当该新的最小误差范围中的分布比例大于该预设分布比例，则将已调整参数的该制图定位演算法中的参数导入该自走装置。

8.如权利要求7所述的轨迹规划方法，其特征在于，以一低延迟的原则调整该制图定位演算法中的参数。

9.一种轨迹规划系统，其特征在于，包括：

一自走装置，在一路径上行走形成一移动轨迹，该自走装置设有一感测模块，该感测模块感测该自走装置在该路径上的地形地物，以产生一感测数据文件；以及一计算装置，取得该感测数据文件，执行以下步骤：将该感测数据文件导入一同步定位与地图构建演算法，以建构对应该路径的一地图；

将该感测数据文件导入一路径闭环演算法，取得每一单位时间的姿态信息，并于该地图构成一最佳化轨迹；

将该感测数据文件导入一制图定位演算法，取得每一单位时间的姿态信息，并于该地图构成一参考轨迹，该参考轨迹对应于该自走装置于该路径的该移动轨迹；

比对该参考轨迹及该最佳化轨迹，判断该参考轨迹是否趋近于该最佳化轨迹；以及如否，则调整该制图定位演算法中的参数；

如是，则将该制图定位演算法中的参数导入该自走装置。

10.如权利要求9所述的轨迹规划系统，其特征在于，该感测数据文件包括多个对应于时间、位置与方向性的感测数据。

11.如权利要求10所述的轨迹规划系统，其特征在于，所述多个感测数据是由该感测模块的一光学雷达、一惯性测量单元与一里程计的至少其中之一所产生。

12.如权利要求9所述的轨迹规划系统，其特征在于，以一差异分布方法比对该参考轨迹及该最佳化轨迹。

13.如权利要求12所述的轨迹规划系统，其特征在于，于该差异分布方法中，计算该参考轨迹相对于该最佳化轨迹在每个单位时间的一姿态信息误差值，以取得该姿态信息误差值的多个误差范围的分布比例。

14.如权利要求13所述的轨迹规划系统，其特征在于，取出该误差范围的一最小误差范围的分布比例与一预设分布比例比较，以判断该参考轨迹是否趋近于该最佳化轨迹。

15.如权利要求14所述的轨迹规划系统，其特征在于，若该最小误差范围的分布比例小于该预设分布比例，则调整该制图定位演算法中的参数，再将该感测数据文件导入已调整参数的该制图定位演算法，以重新取得每一单位时间的姿态信息，得出一新的参考轨迹，再次比对该新的参考轨迹及该最佳化轨迹后，取得一新的最小误差范围的分布比例；当该新的最小误差范围中的分布比例大于该预设分布比例，则将已调整参数的该制图定位演算法中的参数导入该自走装置。

16.如权利要求15所述的轨迹规划系统，其特征在于，该计算装置以一低延迟的原则调整该制图定位演算法中的参数。

说明书

轨迹规划方法与系统

技术领域

[0001] 本公开涉及一种自走装置轨迹规划的技术，特别涉及一种采用经优化的轨迹为基础来修正制图定位演算法的参数的轨迹规划方法与系统。

背景技术

[0002] 近年来人工智能(AI)产业蓬勃发展，市面上陆陆续续有相关产品出现，其中之一为应用在服务型机器人的运行上，包括使用服务型机器人执行巡逻、导引、递送文件等工作，其中的核心技术之一为室内定位导航，在进行室内定位导航时，需要事先执行及时模拟定位建图，如一种同步定位与地图构建(Simultaneous localization and mapping，SLAM)技术，但是SLAM的效果好坏将影响机器人运行的能力。

[0003] 在现有技术中，在所述SLAM的机制中，主要包括先采用外部的高阶摄影机对机器人录影，并建立此机器人在特定场合中的室内定位导航图。

[0004] 传统执行实时模拟同步定位与地图构建制作室内定位导航图时，因为要取得高分辨率影像，需要用多部高分辨率摄影机录制真实的轨迹，如此，相关软硬件设备相当昂贵，并可能依场地大小需架设更多的摄影机。并且，若是想要更换场地，则又需要重新布设所有摄影机。

发明内容

[0005] 本申请公开一种轨迹规划方法与系统，根据技术目的，所述方法使用一路径闭环(loop closure)演算法演算得出一最佳化轨迹，以此比较通过制图定位(Cartographer Localization)演算法得到的参考轨迹，判断参考轨迹是否趋近于最佳化轨迹，根据趋近于最佳化轨迹的参考轨迹得到导入自走装置运行在所述路径上的参数。

[0006] 根据方法实施例，适用于一自走装置，自走装置于一路径上行走形成一移动轨迹，轨迹规划方法包括通过一计算装置取得一自走装置的感测模块在一路径上产生的感测数据文件，将感测数据文件导入同步定位与地图构建演算法，以建构对应所述路径的地图，再将感测数据文件导入一路径闭环演算法取得每一单位时间的姿态信息，以于所述地图构成一最佳化轨迹。

[0007] 接着，将所取得感测数据文件导入制图定位演算法，能取得每一单位时间的姿态信息，并于所述地图构成一参考轨迹，参考轨迹对应于自走装置于路径的一移动轨迹。之后比对参考轨迹及最佳化轨迹，由计算装置判断参考轨迹是否趋近于最佳化轨迹，若判定参考轨迹趋近于最佳化轨迹，则将制图定位演算法中的参数导入自走装置。若判定参考轨迹不趋近于最佳化轨迹时，则调整制图定位演算法中的参数，重新以制图定位演算法演算出新的参考轨迹，同样再与最佳化轨迹比对，再判断新的参考轨迹是否趋近最佳化轨迹，重复演算，直到得到趋近最佳化轨迹的参考轨迹，再将对应前述参考轨迹的制图定位演算法中的参数导入自走装置，让自走装置根据此参数运行在所述路径上。

[0008] 进一步地，所述感测数据文件包括多个对应于时间、位置与方向性的感测数据，而这些感测数据可由感测模块的光学雷达(LiDAR)、惯性测量单元(IMU)与一里程计(Odometer)的至少其中之一所产生。

[0009] 进一步地，所述比较参考轨迹与最佳化轨迹的方式是以一差异分布方法实现，在此差异分布方法中，会计算参考轨迹相对于最佳化轨迹在每个单位时间的姿态信息误差值，经得出在多个单位时间的多组姿态信息误差值，可以得到姿态信息误差值的多个误差范围的分布比例。

[0010] 经取出误差范围的最小误差范围的分布比例，可与一预设分布比例比较，以此判断参考轨迹是否趋近于最佳化轨迹。

[0011] 进一步地，于轨迹规划方法中，是以一低延迟的原则调整制图定位演算法中的参数。

[0012] 根据轨迹规划系统的实施例，轨迹规划系统包括自走装置，其中自走装置设有感测模块，用以感测自走装置在路径上的地形地物，产生感测数据文件；轨迹规划系统亦包括一计算装置，取得感测数据文件，便可依据上述实施例中的轨迹规划方法执行，以取得最趋近于最佳化轨迹的参考轨迹，并将对应前述参考轨迹的该制图定位演算法中的参数导入自走装置。

[0013] 本公开提出的轨迹规划方法与系统为采用同步定位与地图构建演算法，以其中路径闭环演算法将自走装置运行的轨迹做全局的调整后产生出最佳化轨迹，用以比对根据感测数据文件以一制图定位演算法得出的参考轨迹，能检测出以相关参数的轨迹定位效果的优劣，并能取代现有技术中需要架设高阶摄影机等设备所产生的成本。

[0014] 为了能更进一步了解本发明为实现既定目的所采取的技术、方法及技术效果，请参阅以下有关本发明的详细说明、附图，相信本发明的目的、特征与特点，当可由此得以深入且具体的了解，然而说明书附图仅提供参考与说明用，并非用来对本发明加以限制。

附图说明

[0015] 图1显示为轨迹规划方法中自走装置于一路径上运行的情境示意图；

[0016] 图2显示自走装置在一室内场景中运行的情境示意图；

[0017] 图3显示轨迹规划方法的流程实施例图；

[0018] 图4显示在轨迹规划方法中以最佳化轨迹与参考轨迹在每个单位时间的姿态信息误差值的实施例示意图；

[0019] 图5显示在轨迹规划方法中执行差异分布方法的流程实施例图。

[0020] 附图标记说明：

[0021] 自走装置      10            计算装置   12

[0022] 感测模块      11            光学雷达   101

[0023] 惯性测量单元  102           里程计     103

[0024] 控制单元      105           通信单元   107

[0025] 路径          100           起点       a

[0026] 终点          b

[0027] 佳化轨迹      40            参考轨迹   401

[0028] 时间          t0,t1,t2,tn   距离误差值 d0,d1,d2,dn

[0029] 最佳化位置    s0,s1,s2,sn

[0030] 参考位置      p0,p1,p2,pn

[0031] 步骤S301～S321 轨迹规划流程

[0032] 步骤S501～S509 差异分布方法的流程

具体实施方式

[0033] 本公开提出一种轨迹规划方法与系统，适用于通过内部感测器感测四周环境与运行路径的自走装置，可参考图1所示有一自走装置10在一路径100上行走的情境示意图，其中自走装置10在一路径100行走形成一移动轨迹(图未示出)，而自走装置10中设有感测模块11，用于感测自走装置10在路径100上的地形地物，产生一感测数据文件。其中感测数据文件包括多个对应于时间、位置与方向性的感测数据，根据一实施例，这些感测数据是由感测模块11中的光学雷达(Light Detection and Ranging，LiDAR)101、惯性测量单元(Inertial Measurement Unit，IMU)102与里程计(Odometer)103的至少其中之一所产生。

[0034] 其中，光学雷达101是以光探测达到测距目的感测器，利用脉冲光扫描自走装置10到四周环境地形地物的距离，根据接收到的光波数据可以建构四周环境的三维信息，可以实现定位自走装置10的目的。

[0035] 惯性测量单元102是可以感测到机器人姿态的惯性导航仪，可利用加速度计(accelerometer)和陀螺仪(gyroscope)等感测器测量自走装置10的姿态信息(pose)，其中记录的数据例如自走装置10在三个轴向的角速度与加速度。

[0036] 自走装置10另设有可以感测自走装置巡航里程的里程计103，若应用在以轮轴驱动的自走装置10上，里程计103即根据轮轴的圆周与转速等数据计算自走装置10的里程数，也可得到方向数据。

[0037] 自走装置10设有控制单元105及通信单元107，控制单元105电性连接感测模块11，其中可运行一作业系统，如一种机器人作业系统(Robot Operating System，ROS)，当控制单元105接收到感测模块11中各感测元件感测到的数据，形成感测数据文件，可通过通信单元107输出至计算装置12，由计算装置12执行部分轨迹规划方法。

[0038] 图2即接着显示自走装置在一室内场景中运行的情境示意图。图3显示轨迹规划方法的流程实施例图。请一并参考图1、图2及图3。在轨迹规划方法中，于室内场景中设定了起点a与终点b，让自走装置10在起点a与终点b之间的路径走动，于一实施方式，可以由人操作自走装置10于路径100上走动，自走装置10中的感测模块11同时运行并用以感测自走装置10于起点a与终点b之间的路径走动时的各种地形地物，产生一感测数据文件，并由控制单元105的机器人作业系统(Robot Operating System，ROS)记录上述由感测模块产生的感测数据文件，其中感测数据文件包含多个感测数据，所述感测数据对应于时间、位置(如X、Y、Z值)与方向性等数值。

[0039] 在轨迹规划系统中，还包含一具有数据处理能力的计算装置12，如步骤S301，计算装置12通过通信单元107取得自走装置10运行于一路径100上由其中感测模块11产生的感测数据文件。

[0040] 接着，如步骤S303，计算装置12取得感测数据文件后，将感测数据文件导入一同步定位与地图构建演算法(Simultaneous localization and mapping，SLAM)，通过自走装置10的感测模块11于此路径100所得到感测数据文件构建出对应上述路径100的地图。

[0041] 于建构出地图后，如步骤S311，将感测数据文件导入一路径闭环(Loop Closure)演算法，以取得每一单位时间的姿态信息，在此路径闭环演算法的主要目的是能够消除自走装置在运行一段路程后产生的累积误差。其中运用的方法之一是，当自走装置到某一位置时，判断有前一次到达相同位置的数据，可以前次误差小的位置为主，避免因为时间长而造成越来越大的偏移，借此消除累积误差。之后，在步骤S313中，能于地图上构成一经过上述优化过程产生的最佳化轨迹。

[0042] 另一方面，在步骤S307中，将感测数据文件导入一制图定位(Cartographer Localization)演算法，取得每一单位时间的姿态信息，姿态信息包括时间、位置与方向等信息，如步骤S309，并于地图上构成一参考轨迹，此参考轨迹对应于自走装置10于所述路径100的移动轨迹。

[0043] 在步骤S315中，比较最佳化轨迹与参考轨迹，目的是要得出两条轨迹的分布特性，接着如步骤S317，判断参考轨迹是否趋近于最佳化轨迹，其中采用的方法例如一差异分布方法(difference distribution)。

[0044] 此时，系统可设一比对门限，根据参考轨迹与最佳化轨迹之间的误差值判断参考轨迹是否趋近最佳化轨迹，而在上述差异分布的方法中，则是设有预设分布比例，作为检测参考轨迹是否趋近最佳化轨迹的依据。

[0045] 若判断参考轨迹并未达到趋近最佳化轨迹的比对门限，即参考轨迹并未趋近于最佳化轨迹，如步骤S319，则调整在步骤S307中制图定位演算法中的参数，步骤回到S307，导入前述感测数据文件并以调整参数后的制图定位演算法演算得出的新的参考轨迹(步骤S309)，再次比对新的参考轨迹及最佳化轨迹，直到参考轨迹达到趋近最佳化轨迹的比对门限，即参考轨迹是趋近于最佳化轨迹。值得一提的是，在调整制图定位演算法的参数时，可以以一低延迟的原则调整制图定位演算法中的参数。于本实施例中，制图定位演算法的参数举例来说可以是指光学雷达101、惯性测量单元102或里程计103所输出的感测数据的权重、自走装置10的摩擦系数、或环境中不同的噪声干扰其对应的权重等，但不以上述情形为限。

[0046] 若判断参考轨迹已经趋近于最佳化轨迹，如步骤S321，则得出适用于自走装置10的制图定位演算法中的参数，并可将上述参数导入自走装置10，使自走装置10能够依据所接收的参数，在实际于路径100走动时得到对应于前述趋近于最佳化轨迹的参考轨迹的移动轨迹。

[0047] 因此通过本实施例所公开的轨迹规划方法，可先于计算装置12反复调整制图定位演算法中的参数，判断参考轨迹是否趋近最佳化轨迹，又因自走装置10的参数关联于制图定位演算法中的参数，故，再将最接近最佳化轨迹的参考轨迹其对应的参数导入自走装置10，如此自走装置10能够依据所接收的参数，在实际于路径100走动时得到对应于前述趋近于最佳化轨迹的参考轨迹的移动轨迹。

[0048] 此时可将前述得到的最佳化轨迹与多个参考轨迹绘制在同一平面坐标，可参考图4所示在所述起点a到终点b之间的最佳化轨迹40以及参考轨迹401，轨迹规划方法即从中得出趋近最佳化轨迹40至一定程度的参考轨迹及其参数。

[0049] 在图3所述流程实施例中，可以采用差异分布方法(difference distribution)作为判断参考轨迹是否趋近最佳化轨迹的比对方法，比对的方式可参考图4所示，图4所示为在轨迹规划方法中以最佳化轨迹与参考轨迹在每个单位时间的姿态信息误差值的实施例示意图，以及图5所示在轨迹规划方法中执行差异分布方法的流程实施例图。

[0050] 图4显示的两条轨迹中，有根据定位信息绘制的一最佳化轨迹40与一参考轨迹401，在此差异分布方法中，会计算参考轨迹相对于最佳化轨迹在每个单位时间的姿态信息误差值，此例示意显示出参考轨迹401在每个时间t0,t1,t2,……tn的参考位置p0,p1,p2,……pn，对比到最佳化轨迹40上相同时间的最佳化位置s0,s1,s2,……sn，可以得出各个时间的姿态信息误差值(于本图以距离误差d0,d1,d2,……dn为例，但不以此为限)，根据图5步骤S501，同理可以计算出参考轨迹相对最佳化轨迹在每单位时间的姿态信息误差值，也就是在每个单位时间的位置与方向差异，再如步骤S503，当系统将各姿态信息误差值区分为多个误差范围，即可以得出每个姿态信息误差值所在的误差范围，据此，如步骤S505，可以取得姿态信息误差值的多个误差范围的分布比例，这就是这两条轨迹(参考轨迹与最佳化轨迹)的差异分布。

[0051] 为了得到趋近最佳化轨迹的参考轨迹，系统可以设有一预设分布比例，所述差异分布方法即将每次得到的参考轨迹与最佳化轨迹之间形成的误差范围的分布比例比对至此预设分布比例，如步骤S507，作为检测参考轨迹是否已经趋近最佳化轨迹的依据。

[0052] 再如步骤S509，计算装置12中运行的轨迹规划方法将判断前述误差范围中的最小误差范围的分布比例是否大于预设分布比例，若否，即目前最小误差范围的分布比例小于预设分布比例，也就是表示目前参考轨迹仍未趋近最佳化轨迹，即回到图3的步骤，如S319，调整制图定位演算法中的参数，再如步骤S307，导入前述感测数据文件并以调整参数后的制图定位演算法演算，以重新取得每一单位时间的姿态信息，得出一新的参考轨迹(步骤S309)，再重新执行图5所述的差异分布方法流程中的步骤，包括计算出新的参考轨迹相对于最佳化轨迹在每单位时间的姿态信息误差值(步骤S501)、得出每个姿态信息误差值所在的误差范围(步骤S503)、得出姿态信息误差值的多个误差范围的分布比例(步骤S505)，以及取出误差范围的分布比例的至少其中之一比对至此预设分布比例(步骤S507)判断新的参考轨迹是否趋近最佳化轨迹(步骤S509)。

[0053] 若在步骤S509的判断中，当最小误差范围的分布比例大于预设分布比例，表示参考轨迹已经趋近最佳化轨迹，即如图3步骤S321，得出适用于自走装置10的制图定位演算法中的参数，并可将上述参数导入自走装置10，使自走装置10能够依据所接收的参数，于路径100走动时得到对应于前述趋近于最佳化轨迹的参考轨迹的移动轨迹。

[0054] 举例来说，得出每个单位时间的姿态信息误差值可以一距离为例，即计算两条轨迹在同一单位时间的两个点坐标位置之间的误差距离，可以形成多个距离误差值，如d1,d2,d3,……dn。系统可以设定多个误差范围，如D0(1米)、D1(0.1米)、D2(0.05米)以及D3(0.01米)，如此计算两条曲线在每个时间点的位置误差在0.01米(D3)内有多少比例、误差在0.05米(D2)内有多少比例、误差在0.1米(D1)内有多少比例，以及误差在1米(D0)内有多少比例，这些比例值即表现出对应的参考轨迹的曲线特性，从中可以评估出最趋近于最佳化轨迹的参考轨迹。

[0055] 在一实施例中，系统可以仅考量最小误差范围(如此例的D3)，以此比对预设分布比例，作为判断参考轨迹是否趋近最佳化轨迹的依据。若最小误差范围的分布比例小于预设分布比例，表示需要继续调整制图定位演算法中的参数，再将感测数据文件导入已调整参数的制图定位演算法，以重新演算取得每一单位时间的姿态信息，得出新的参考轨迹，并重新判断新的参考轨迹是否趋近最佳化轨迹；若新的最小误差范围中的分布比例大于预设分布比例，表示新的参考轨迹已经趋近最佳化轨迹，则将已调整参数的制图定位演算法中的参数导入自走装置10。

[0056] 如此反复以上流程，轨迹规划方法可以有效降低参考轨迹与最佳化轨迹之间的误差，有助于改善自走装置的的定位导航。

[0057] 综上所述，以上所公开的轨迹规划方法与系统的实施例主要为，先通过自走装置的感测模块在路径上走动以产生感测数据文件，并将感测数据文件导入同步定位与地图构建演算法建构地图，再将感测数据文件导入路径闭环演算法将自走装置行走的轨迹做全局的调整后产生最佳化轨迹，以及将感测数据文件导入制图定位演算法产生参考轨迹，再判断参考轨迹是否趋近于最佳化轨迹，用以检测制图定位演算法的参数优劣，使得自走装置10能够通过在计算装置12中先行模拟，以便更快速取得参数调整方向，并能取代现有技术中需要架设高阶摄影机等设备所产生的成本。

[0058] 以上所述仅为本发明的优选可行实施例，凡依本发明保护范围所做的均等变化与修饰，皆应属本发明的涵盖范围。