一种智能打磨机器人自动打磨决策规划装置及方法与流程

1.本发明涉及智能机器人技术领域，特别是关于一种智能打磨机器人自动打磨决策规划装置及方法。


背景技术：

2.打磨工艺是指为了满足产品的精度和外观需求而进行的打磨任务工作。随着航空、船舶、汽车、高铁和发电等制造业的飞速发展，对于产品的外观精度要求和柔性化生产要求不断提高，因此以工业机器人为载体的智能打磨机器人系统逐渐取代高度依靠人工的传统打磨工艺。
3.智能打磨机器人系统具有算法精确、传感器精度高和机器人灵活性高等优点，除了能够更高效完成打磨任务外，也能更好的适应工件小批量生产和个性化生产定做等生产需求。通过在智能制造生产线中使用智能打磨机器人系统，还能够促进各种工业机器人的相互配合，从而更好的协调生产资源，提高生产效率。以高铁车厢和风力发电机叶片等大型工件为例，在打磨过程中它们具有移动困难、作业安全风险高和待打磨曲面复杂多样等特点，使用智能打磨机器人系统能够减少人工投入、提高生产质量、从而降低安全风险并降低生产成本。因此在解决打磨大型工件的工程问题中，研究了一种智能打磨机器人自动打磨决策规划方法及系统。
4.目前，国内常用的智能打磨机器人系统按照移动方式进行区分。
5.例如，现有一种依靠预设轨道移动式智能打磨机器人系统，它是通过提前铺设机器人移动地轨，实现打磨机器人按指定路线进行移动。智能打磨机器人系统依靠机器人地轨，能够在阶段打磨任务完成后，根据预先规划的路径沿着地轨进移动到下一个打磨位置。通过使用地轨，扩大机器人作业半径，扩展机器人使用范围功能，进一步提高机器人使用效率，降低机器人使用成本，不仅能够解决空间复杂和工件大等打磨问题，而且实现了全面自动化生产。而在建设机器人地轨时，要根据具体的打磨工件进行建设安装，其不仅建设地轨成本高安装调试复杂，而且安装成功后不便于调整。此外依靠轨道移动的智能打磨机器人系统，不能很好地适应打磨不同的工件，打磨柔顺性程度低。
6.还例如，现有另一种是依靠前轴转向后轴驱动的底盘移动式智能打磨机器人系统，它是通过将打磨机械臂装载在移动地盘上，实现机器人的移动。它解决了铺设机器人移动地轨费用高，生产柔性差等问题，同时也能够很好解决移动空间复杂和工件大等打磨问题。在进行打磨任务时，不仅能实现更多的打磨停靠点，而且可以实现更多的打磨姿态，提高了打磨质量和打磨效率。此外在面对不同工件打磨时，通过调整智能打磨机器人系统的程序即可实现功能的调整，因此具有更高的打磨的柔顺性和更高的使用率。然而，在空间有限的工厂中，由于转向的限制不能在多障碍物狭窄空间内实现原地转向，因此打磨姿态的调整存在不足，打磨的质量和效率受到影响。前轴转向后轴驱动的底盘移动方式在有障碍物的极端场景下，容易困在障碍物周围。


技术实现要素：

7.本发明的目的在于提供一种能够提高打磨的柔顺性和质量的智能打磨机器人自动打磨决策规划装置及方法。
8.为实现上述目的，本发明提供一种智能打磨机器人自动打磨决策规划装置，其包括预先设置在四轮的机器人本体的地图模块、决策规划模块和控制模块：
9.地图模块用于在扫描路径过程中，扫描生成所需打磨区域的打磨路径，对机器人本体实时定位和检测其周围障碍物信息，在判定打磨工件处于正确姿态后向决策规划模块反馈开始扫描信号和扫描完成信号，以及在反馈扫描完成信号后向决策规划模块反馈打磨工件的轮廓信息；
10.决策规划模块用于根据机器人本体的状态以及与地图模块和调度系统的交互信号，通过判断当前任务和动作是否完成确认下一步的任务、动作和决策状态以及生成全局轨迹和局部轨迹；其中，决策状态在任务层决策状态和动作层决策状态之间转移；
11.控制模块用于根据局部轨迹，控制机器人本体进行局部轨迹跟踪。
12.进一步地，“决策规划模块用于根据机器人本体的状态以及与地图模块和调度系统的交互信号，通过判断当前任务和动作是否完成确认下一步的任务、动作以及生成全局轨迹和局部轨迹”的方法具体包括：
13.决策规划模块用于在接收到调度系统下发任务后生成前往打磨区扫描起点的全局轨迹，在判定机器人本体停在扫描起点时向地图模块发出扫描请求，在判定机器人本体停在扫描终点、且在规定时间内未收到扫描完成信号时生成扫描起点的路径，在收到地图模块反馈的扫描完成信号和打磨工件的轮廓信息时确定打磨策略、生成前往打磨起点的局部路径，在判定机器人本体停在打磨起点时向机械臂发出开始打磨请求信号，在判定机器人本体位于打磨路径的终点、且收到机械臂反馈的打磨完成信号时规划模块生成前往打磨区等待点的局部路径。
14.进一步地，打磨策略包括：
15.根据不可打磨区域的长度对打磨工件上的第一轮廓点进行重新分割，再对分割后的第二轮廓点按调度系统下发任务中的打磨安全距离进行平移膨胀，然后根据膨胀后的第三轮廓点，生成打磨路径，最后根据打磨安全间隔对打磨路径进行分段处理，若最后一段长度达不到打磨安全间隔，则将剩下的部分作为最后一段的打磨路径。
16.进一步地，任务层决策状态分为：等待任务状态、判断任务状态和任务中状态，动作层决策状态分为：任务刚开始的停车状态、循迹行驶状态、扫描状态、前往打磨起点状态、打磨状态和前往等待点状态；
17.机器人本体前往打磨区执行打磨任务时，决策规划模块通过在任务层决策状态和动作层决策状态之间转移确认下一步的任务、动作和决策状态的方法包括：
18.首先，任务层决策状态由等待任务状态进入判断任务状态；
19.然后，判断是否能生成到达扫描起点的全局轨迹，如是，则任务层决策状态转移为任务中状态，否则，返回等待任务状态；
20.再者，在任务层决策状态为任务中状态时，动作层决策状态从任务刚开始的停车状态转移为循迹行驶状态；
21.最后，当机器人本体到达扫描起点时，动作层决策状态转移到扫描状态，在扫描完
毕后转移到前往打磨起点状态，在机器人本体到达打磨起点后再转移到打磨状态，并在打磨完成后转移到前往等待点的状态，到达等待点后，重新进入等待任务状态，直至打磨任务完成。
22.进一步地，地图模块还用于在打磨任务开始前采集充电点至打磨区扫描起点的路径、确定扫描工件时的扫描路径和打磨完成后等待点返回至对应充电点的路径；
23.决策规划模块生成全局轨迹的方法具体包括：
24.生成地图文件：在打磨任务开始前，根据机器人本体的具体环境，选取地图上的n个关键点，对每一个关键点进行编号，并判断第i关键点与第j关键点之间是否有连接关系，依此生成n阶方阵，作为全局轨迹的地图文件；其中，判断第i关键点与第j关键点之间有连接关系，则在n阶方阵的第i行第j列处的元素设置第i关键点与第j关键点之间的路径长度，否则设置为负数；
25.生成全局轨迹：在接收到调度系统下发任务后，确定全局轨迹终点处的编号，根据机器人本体当前的定位信息，在地图文件中搜索距离机器人本体最近的路点，确定该路点所在的路径两端的编号作为两个起点编号；再分别根据这两个起点编号获取最短路径，得到两个由编号构成的路径序列点；然后选取包括机器人本体所在路径的路径序列点，将地图路径上的路点进行拼接；最后根据机器人本体的定位信息，舍去前端多余的路点，得到全局路径，再经过速度规划后生成全局轨迹。
26.进一步地，决策规划模块在机器人本体与全局路径的横向距离大于偏离纠正距离阈值的情形下，采用如下三次样条插值方法进行路径纠偏方法生成对应不同航向角的多条局部路径，再根据各路径点曲率之和路径总长度的评判标准，构建符合打磨场景的代价函数，评判出最优的局部路径：
27.先将机器人本体当前位置和车速为作为局部轨迹的起点，采样终点为，将坐标原点平移至起点处，获得第一新坐标系，再将第一新坐标系以新原点为中心旋转得到第二新坐标系，分别得到起点和终点在第二新坐标系中的坐标：新起点和新终点；
28.再通过间隔地从新起点到新终点选择不同的，得到相对应的在第二新坐标系下的值和值；
29.最后还原过程与得到第二新坐标系的方法类似，将第二新坐标系反向旋转为第一新坐标系，然后反向平移还原为原坐标系，获得可通过改变起点和终点的航向角的多条局部路径。
30.本发明还提供一种智能打磨机器人自动打磨决策规划方法，其包括：
31.s1，决策规划模块根据调度系统下发任务，生成前往打磨区扫描起点的全局轨迹，并在判定四轮的机器人本体停在扫描起点后，向地图模块发出扫描请求；
32.s2，地图模块在判定打磨工件处于正确的姿态后，向决策规划模块反馈开始扫描信号，控制模块根据打磨区扫描起点的编号选择的扫描路径控制机器人本体行驶至扫描终点；
33.s3，决策规划模块在判定机器人本体停在扫描终点，且在规定时间内未收到机器人本体反馈扫描完成信号时，向控制模块下发返回扫描起点的路径，并在机器人本体返回扫描起点后，重新执行s2；
34.s4，决策规划模块在收到地图模块反馈的扫描完成信号和打磨工件的轮廓信息时确定打磨策略、生成前往打磨起点的局部路径；
35.s5，决策规划模块在判定机器人本体停在打磨起点时，向机械臂发出开始打磨请求信号，并在判定机器人本体位于整个打磨路径的终点、且收到机械臂反馈的打磨完成信号后，规划模块生成前往打磨区等待点的局部路径；
36.s6，决策规划模块在判定机器人本体到达打磨区等待点后驻车制动，上报机器人本体实时状态，该次打磨任务全部结束。
37.进一步地，打磨策略包括：
38.根据不可打磨区域的长度对打磨工件上的第一轮廓点进行重新分割，再对分割后的第二轮廓点按调度系统下发任务中的打磨安全距离进行平移膨胀，然后根据膨胀后的第三轮廓点，生成打磨路径，最后根据打磨安全间隔对打磨路径进行分段处理，若最后一段长度达不到打磨安全间隔，则将剩下的部分作为最后一段的打磨路径。
39.进一步地，机器人本体前往打磨区执行打磨任务时，决策规划模块通过在任务层决策状态和动作层决策状态之间转移确认下一步的任务、动作和决策状态，其具体包括：
40.首先，任务层决策状态由等待任务状态进入判断任务状态；
41.然后，判断是否能生成到达扫描起点的全局轨迹，如是，则任务层决策状态转移为任务中状态，否则，返回等待任务状态；
42.再者，在任务层决策状态为任务中状态时，动作层决策状态从任务刚开始的停车状态转移为循迹行驶状态；
43.最后，当机器人本体到达扫描起点时，动作层决策状态转移到扫描状态，在扫描完毕后转移到前往打磨起点状态，在机器人本体到达打磨起点后再转移到打磨状态，并在打磨完成后转移到前往等待点的状态，到达等待点后，重新进入等待任务状态，直至打磨任务完成。
44.进一步地，决策规划模块生成全局轨迹的方法具体包括：
45.生成地图文件：在打磨任务开始前，根据机器人本体的具体环境，选取地图上的n个关键点，对每一个关键点进行编号，并判断第i关键点与第j关键点之间是否有连接关系，依此生成n阶方阵，作为全局轨迹的地图文件；其中，判断第i关键点与第j关键点之间有连接关系，则在n阶方阵的第i行第j列处的元素设置第i关键点与第j关键点之间的路径长度，否则设置为负数；
46.生成全局轨迹：在接收到调度系统下发任务后，确定全局轨迹终点处的编号，根据机器人本体当前的定位信息，在地图文件中搜索距离机器人本体最近的路点，确定该路点所在的路径两端的编号作为两个起点编号；再分别根据这两个起点编号获取最短路径，得到两个由编号构成的路径序列点；然后选取包括机器人本体所在路径的路径序列点，将地图路径上的路点进行拼接；最后根据机器人本体的定位信息，舍去前端多余的路点，得到全局路径，再经过速度规划后生成全局轨迹。
47.本发明由于采取以上技术方案，其具有以下优点：
48.1.由于本发明针对打磨机器人多任务的打磨需求，提出了双层有限状态机的决策方法，将任务层决策状态和动作层决策状态进行区分，通过不同任务状态的转变和不同动作层决策状态的转变，能够很好的完成打磨、充电和移动等多种任务，并且更加有利于功能的增添和修改。
49.2.由于本发明针对打磨机器人在地图上移动的场景，结合具体任务进行了关键点的选取，建立具有连接关系的地图，并将地图文件与dijkstra算法有效结合，生成路径序列点，提供了一种根据路径序列点截取所需的全局路径方法，实现打磨机器人任意起始位置向目标关键点的移动。
50.3.由于本发明针对局部路径生成的场景，采用“轨迹簇生成+最优轨迹选择”的构思，在满足曲率约束和避障要求的前提下，基于三次多项式生成了多条轨迹，根据各路径点曲率之和路径总长度的评判标准，构建了符合打磨场景的代价函数，评判出最优的局部路径，兼顾计算时间的情况下提高了局部轨迹生成的质量。
51.本发明适用的行驶道路场景为加工厂房内部的类结构化道路，而无地面引导线。
附图说明
52.图1为本发明实施例提供的智能打磨机器人自动打磨决策规划的工作流程图。
53.图2为本发明实施例提供的双层有限状态机状态转移示意图。
54.图3为本发明实施例提供的关键点的编号信息示意图。
55.图4为采用本发明和现有方法的实验效果对比示意图。
具体实施方式
56.下面结合附图和实施例对本发明进行详细的描述。
57.如图1所示，本发明实施例提供的智能打磨机器人1包括机器人本体和自动打磨决策规划装置，其中，机器人本体为机器硬件本身，作为智能打磨机器人1的执行单元。由于机器人本体的行驶道路场景为加工厂房内部的类结构化道路，为多障碍物狭窄场景，因此，机器人本体配备有四轮转向的移动底盘，具有四轮原地转向功能，既能通过四轮原地转向调整航向角，在多障碍物狭窄空间的通行，提升了避障能力，还能通过四轮原地自转调整打磨姿态，最大限度调整姿态，增加姿态调整的范围，提高打磨质量。
58.机器人本体中预先设置有自动打磨决策规划装置，自动打磨决策规划装置作为智能打磨机器人1的决策单元，作用类比于大脑，根据操作人员在后台的调度系统输入的打磨任务，控制机器人本体动作。
59.其中，打磨任务包括前往打磨区打磨、返回充电点充电、路网中移动、取消当前任务、一键停车和恢复行驶等。
60.本实施例提供的自动打磨决策规划装置包括地图模块、决策规划模块和控制模块，其中：
61.地图模块具有定位和感知功能，用于在扫描路径过程中，扫描生成所需打磨区域的打磨路径，对机器人本体实时定位和检测其周围障碍物信息，在判定打磨工件处于正确姿态后向决策规划模块反馈开始扫描信号和扫描完成信号，以及在反馈扫描完成信号后向决策规划模块反馈打磨工件的轮廓信息。
62.其中，地图模块判定打磨工件处于正确的姿态的方法包括：例如，判断当前打磨工件的摆放方向是否可以扫描到打磨工件，如果是，则判定为打磨工件处于正确姿态。地图模块向决策规划模块反馈打磨工件的轮廓信息的方法可以是：地图模块将打磨工件的轮廓点进行等间隔传递，例如直线形状的打磨工件，地图模块将该直线沿长度方向等间隔分为若干个点，从而形成多段轮廓，那么分段向决策规划模块反馈打磨工件的轮廓信息，不仅可以减小传输的数据量，但又不丧失工件轮廓信息。
63.决策规划模块用于实时检测机器人本体的状态，根据机器人本体的状态以及与地图模块和调度系统的交互信号，通过判断当前任务和动作是否完成确认下一步的任务、动作和决策状态以及生成全局轨迹和局部轨迹。其中，决策状态在任务层决策状态和动作层决策状态之间转移。决策规划模块用于实时检测机器人本体的状态主要依赖于预先布置在机器人本体上方和四周的激光雷达和避障雷达等感知模块，通过感知模块为决策规划模块提供各种环境中的已知信息，从而实时检测机器人本体的状态。需要说明的是，打磨任务执行开始的准备阶段中，决策规划模块收到其他各模块自检完成信号，机器人本体上电后处于静默状态。
64.控制模块用于根据局部轨迹，控制机器人本体的转角和转速，进行局部轨迹跟踪。
65.在一个实施例中，“决策规划模块用于根据机器人本体的状态以及与地图模块和调度系统的交互信号，通过判断当前任务和动作是否完成确认下一步的任务、动作以及生成全局轨迹和局部轨迹”的方法具体包括：
66.决策规划模块用于在接收到调度系统下发任务后生成前往打磨区扫描起点的全局轨迹，在判定机器人本体停在扫描起点时向地图模块发出扫描请求，在判定机器人本体停在扫描终点、且在规定时间内未收到扫描完成信号时生成扫描起点的路径，直到收到扫描完成信号，在收到地图模块反馈的扫描完成信号和打磨工件的轮廓信息时确定打磨策略、生成前往打磨起点的局部路径，在判定机器人本体停在打磨起点时向机械臂发出开始打磨请求信号，在判定机器人本体位于打磨路径的终点、且收到机械臂反馈的打磨完成信号时规划模块生成前往打磨区等待点的局部路径。扫描终点可以由地图模块根据工厂实际地图和工件位置，人为设计得到。
67.其中，调度系统下发任务的信息包括打磨安全间隔、打磨区扫描起点的编号、打磨工件的长度和不可打磨区域的长度。决策规划模块生成前往打磨区扫描起点的全局轨迹例如可以使用dijkstra算法，搜索并拼接前往打磨区扫描起点所对应的最短路径，从而生成前往打磨区扫描起点的全局轨迹。
68.在判定机器人本体停在打磨起点时向机械臂发出开始打磨请求信号的方法具体包括：
69.在判定机器人本体到达打磨起点后，但航向角与打磨起点期望航向角偏差过大（例如偏差为5度）时，控制机器人本体进入原地转向模式，调整机器人本体航向角。当航向角与打磨起点期望航向角偏差满足要求时，控制机器人本体驻车制动，待完全停下时，再发出机械臂开始打磨请求信号。
70.在一个实施例中，打磨策略可以采用现有打磨策略，也可以包括：
71.根据不可打磨区域的长度对打磨工件上的第一轮廓点进行重新分割，再对分割后的第二轮廓点按调度系统下发任务中的打磨安全距离进行平移膨胀，然后根据膨胀后的第
三轮廓点，生成供机器人本体行走的打磨路径，最后根据打磨安全间隔对打磨路径进行分段处理，若最后一段长度达不到打磨安全间隔，则将剩下的部分作为最后一段的打磨路径。
72.其中，以直线形状的打磨工件为例说明“平移膨胀”的方法，也就是说，将打磨工件轮廓点向远离自身的方向平移一段距离，这样机器人本体便可以沿着打磨工件边缘行驶，而不会撞到打磨工件，以保证机器人本体运行时的安全性。
73.在一个实施例中，根据机器人本体实际运行场景，本系统采用任务、动作分层的有限状态机方法。如图2所示，任务层决策状态分为：等待任务状态、判断任务状态、任务中状态、未就绪状态和停止当前任务状态。当任务层决策状态在任务中时，机器人本体开始进行动作层决策状态转移。动作层决策状态分为：任务刚开始的停车状态、循迹行驶状态、扫描状态、前往打磨起点状态、打磨状态、前往等待点状态、充电点充电状态、停车状态、紧急停车状态和重定位状态。
74.通过嵌套的双层有限状态机，机器人本体可以完成前往打磨区打磨、返回充电点充电、路网中移动、取消当前任务、一键停车和恢复行驶等任务。
75.机器人本体前往打磨区执行打磨任务时，决策规划模块通过在任务层决策状态和动作层决策状态之间转移确认下一步的任务、动作和决策状态的方法包括：
76.首先，收到调度系统打磨任务后，任务层决策状态由等待任务状态进入判断任务状态；
77.然后，判断是否能生成到达扫描起点的全局轨迹，如是，则任务层决策状态转移为任务中状态，否则，返回等待任务状态；
78.再者，在任务层决策状态为任务中状态时，动作层决策状态从任务刚开始的停车状态转移为循迹行驶状态；
79.最后，当机器人本体到达扫描起点时，动作层决策状态转移到扫描状态，在扫描完毕后转移到前往打磨起点状态，在机器人本体到达打磨起点后再转移到打磨状态，并在打磨完成后转移到前往等待点的状态，到达等待点后，重新进入等待任务状态，直至打磨任务完成。
80.除上述实施例外，机器人本体在执行任务时还会遇到如下几种特殊情况，也需要决策规划模块通过在任务层决策状态和动作层决策状态之间转移确认下一步的任务、动作和决策状态：
81.（一）出现故障的情形：例如地图模块等模块异常断开，表征为定位点浮动过大；还比如无法收到各个模块的消息，又或者是：前后两帧的位置变化过大，诸如此等等，均可以判定出现故障。此时，动作层决策状态则会进入停车状态，任务层进入未就绪状态，等待人工处理后返回原状态。其中的原状态可以理解为各个模块通讯正常，定位正常。如果上一状态还未完成打磨、充电等任务，原状态也包含继续执行该任务。
82.（二）在调度系统已经发送指令后想要停止当前任务的情形：动作层决策状态会进入到停车状态，任务君决策层状态进入未就绪状态，进而转移为等待任务状态。
83.（三）在机器人本体移动过程中收到调度系统下发暂停机器人行驶指令的情形：动作层决策状态便会转移为停车状态，等待调度系统发送恢复行驶时，重新转移为原状态。
84.（四）在机器人本体移动过程中遇到有障碍物阻挡其行驶轨迹的情形：通过决策规划模块判断障碍物的远近，决定状态转移为停车还是紧急停车，当障碍物消失后，机器人重
新返回原状态。
85.（五）在定位信息变动过大的情形：例如：定位前后两帧相差2cm，此时，动作层决策状态会进入重定位状态，机器人本体停在原地，待定位信息准确后，重新返回原状态。
86.在一个实施例中，如图3所示，地图模块还用于在打磨任务开始前采集充电点至打磨区扫描起点的路径、确定扫描工件时的扫描路径和打磨完成后等待点返回至对应充电点的路径。
87.决策规划模块生成全局轨迹的方法具体包括：
88.a.生成地图文件：在打磨任务开始前，根据机器人本体的具体环境，选取地图上的n个关键点，关键点例如是充电点、打磨起点、打磨终点和等待点等，从前到后对每一个关键点进行编号，并判断第i关键点与第j关键点之间是否有连接关系，依此生成n阶方阵，作为全局轨迹的地图文件，地图文件中的每段路径信息主要包括路径两段的关键点信息和路径上的路点信息。其中，判断第i关键点与第j关键点之间有连接关系，则在n阶方阵的第i行第j列处的元素设置第i关键点与第j关键点之间的路径长度，否则设置为负数。其中，“前”可以理解为距离起点更近，“后”可以理解为距离终点更近。
89.机器人本体可能停在关键点处，也可能因为任务取消停留在关键点之间。
90.b.生成全局轨迹：在接收到调度系统下发任务后，根据具体任务确定全局轨迹终点处的编号，根据机器人本体当前的定位信息，在地图文件中搜索距离机器人本体最近的路点，确定该路点所在的路径两端的编号作为两个起点编号。再分别根据这两个起点编号获取最短路径，得到两个由编号构成的路径序列点。然后选取包括机器人本体所在路径的路径序列点，将地图路径上的路点进行拼接；最后根据机器人本体的定位信息，舍去前端多余的路点，得到全局路径，再经过速度规划后生成全局轨迹。
91.在一个实施例中，上述各实施例中的“生成打磨路径”、“生成前往打磨起点或打磨区等待点的局部路径”或在上述情形（五）时，都需要决策规划模块进行路径纠偏，生成局部轨迹。
92.在一个实施例中，决策规划模块在机器人本体与全局路径的横向距离大于偏离纠正距离阈值（例如：0.5m）的情形下，采用如下三次样条插值方法进行路径纠偏方法生成对应不同航向角的多条局部路径，再根据各路径点曲率之和路径总长度的评判标准，构建符合打磨场景的代价函数，评判出最优的局部路径。通过将三次样条插值和带有自转功能的打磨机人结合，可以更大限度实现路径纠偏，更大限度实现狭窄空间的通过性。
93.本实施例采用三次样条插值方法进行路径纠偏的方法具体包括：
94.将机器人本体当前位置和车速为作为局部轨迹的起点，根据全局轨迹采样偏离纠正轨迹目标点，生成若干条采样路径，然后基于特定目标函数，选择最优的一条轨迹。
95.而对于每一对起点和采样终点，采用如下的三次样条曲线拟合方法，再结合全局路径，得到一条路径偏离纠正路径：
96.先设起点为，采样终点为，将坐标原点平移至起点处，获得第一新坐标系，此时采样终点在第一新坐标系中的坐标表示为下式（1）：
97.ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（1）
98.式（1）中，、和分别为原坐标系（地图模在建图过程中选取的实际点，以图3为例，选取充电点3作为坐标原点，向右为x正方向，向上为y的正方向。）中起点的横坐标、纵坐标和航向角，、和分别为原坐标系中终点的横坐标、纵坐标和航向角，和分别为第一新坐标系中终点的横坐标和纵坐标。
99.然后，将第一新坐标系以该坐标系坐标原点为中心旋转如下式（2）示出的角度，得到第二新坐标系，起点在第二新坐标系中的坐标表示为下式（3），终点在第二新坐标系中的坐标表示为下式（4），即为新起点，即起点在新坐标系下的坐标，为新终点：
100.ꢀꢀꢀꢀꢀ
（2）
101.ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（3）
102.ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（4）
103.再对新起点和新终点进行三次样条插值，这样即能简化计算量，又能保证插值曲线的平滑性，防止出现龙格现象。此时三次样条曲线的各项系数可通过以下各式求得：
104.（5）
105.式（5）中，、、和分别为三次多项式的0次项、1次项、2次项和3次项。
106.这样，通过间隔地（比如：等间距）的从起点位置到终点位置选择不同的，就可得到相对应的在第二新坐标系下的值和值，最后将坐标系还原，即可得到一条插值曲线，以此作为一条可选的偏离纠正路径。具体的选点和坐标还原相关公式为现有技术，在此不再赘述。
107.当对所有的采样终点都进行三次样条插值后，为了保证机器人本体尽可能快的回到全局路径上，本发明选择采样点离机器人本体自身最近的一条，并进行约束检测与碰撞
检测，其中约束检测可以理解为检查此路径的曲率是否会超出最大曲率限制。若检测不通过，则选择次近的一条，直到满足条件为止。
108.选择好最优纠正路径后，还需要给每个路点分配一个速度，分为两种情况：一种是起步时偏离，另一种是行驶过程中偏离。对于第一种情况，本发明采用梯形速度规划方法规划一条速度曲线，而对于第二种情况，本发明直接给所有路点附上当前的速度值，以保证偏离纠正过程的平顺性。
109.因为每次的打磨路径在扫描阶段才得到，所以扫描终点到打磨起点的路径和打磨终点到等待点的路径需要在机器人本体工作过程中生成，这里也采用了基于三次多项式的路径生成方法，具体包括如下步骤：
110.打磨起点与打磨终点满足三次多项式的函数关系式（6），：
111.（6）
112.将坐标系平移至打磨起点处，则四个常量参数的计算公式为下式（7）：
113.（7）
114.式中，为起点的航向角角度，为终点的航向角角度，为。
115.上述实施例中，由于起点、终点的坐标、航向角均为已知，因此，可以求出该两点之间的曲线方程，并用等间距插值，计算各个插值点的坐标与航向角，生成局部路径。同时，本发明可以改变起点、终点的航向角来改变曲线方程，从而得到不同的路径，进而在满足无碰撞的条件下，对路径的曲率之和与路径总长度取评价权重，得出最优局部路径。
116.上述各实施例中，双层有限状态机的决策方法可以使用多状态有限状态进行替代，以实现不断调整和增加机器人功能。也可以通过使用行为树和决策树进行替代，局部路径使用的三次样条曲线，可以使用更高次的样条曲线替代。
117.上述各实施例中，在构建打磨机器人路径地图的过程中，充电点和作业区的位置布置构思可以以图3示出的位置关系为例，其中包括充电点1、充电点2和充电点3以及作业区1、作业区2和作业区3。在每一个作业区设置有等待点，例如：作业区1等待点、作业区2等待点和作业区3等待点。各作业区内分别设置有扫描区和打磨区，扫描区又包含有扫描起点和扫描终点。为了方便路径的采集和编号，设置如下：
118.充电点1位置为序号，设计充电点2位置为序号，设计充电点3位置为序号，作业区3等待点位置为序号，作业区2等待点位置为序号，作业区1等待点位置
为序号，扫描区3扫描起点位置为序号，扫描区2扫描起点位置为序号，扫描区1扫描起点位置为序号。编号完成的地图中，各编号之间的路径便可以唯一标识和命名，路径的采集后，打磨机器人便可运行使用。打磨区和充电点的数量可以根据情况调整。
119.本发明实施例还提供一种智能打磨机器人自动打磨决策规划方法，其包括：
120.s1，决策规划模块根据调度系统下发任务，生成前往打磨区扫描起点的全局轨迹，并在判定机器人本体停在扫描起点后，向地图模块发出扫描请求；
121.s2，地图模块在判定打磨工件处于正确的姿态后，向决策规划模块反馈开始扫描信号，控制模块根据打磨区扫描起点的编号选择的扫描路径控制机器人本体行驶至扫描终点；
122.s3，决策规划模块在判定机器人本体停在扫描终点，且在规定时间内未收到机器人本体反馈扫描完成信号时，向控制模块下发返回扫描起点的路径，并在机器人本体返回扫描起点后，重新执行s2；
123.s4，决策规划模块在收到地图模块反馈的扫描完成信号和打磨工件的轮廓信息时确定打磨策略、生成前往打磨起点的局部路径；
124.s5，决策规划模块在判定机器人本体停在打磨起点时，向机械臂发出开始打磨请求信号，并在判定机器人本体位于整个打磨路径的终点、且收到机械臂反馈的打磨完成信号后，规划模块生成前往打磨区等待点的局部路径；
125.s6，决策规划模块在判定机器人本体到达打磨区等待点后驻车制动，上报机器人本体实时状态，该次打磨任务全部结束，机器人本体处于空闲状态。
126.结合图1，再次说明本发明的在打磨区实施打磨任务的工作过程，图中示出了远端的调度系统和工厂内部的充电区和打磨区，下面以机器人本体在打磨区1的工作为例进行说明：
127.机器人完成启动，自检；
128.调度系统进行任务下发；
129.机器人本体前往工作区，即打磨区；
130.机器人本体首先到达扫描区1的扫描起点1；
131.机器人在扫描区1沿扫描轨迹行驶，扫描工件；
132.机器人本体到达扫描区1的扫描终点1，此时完成对工件的扫描；
133.机器人本体继续前往扫描区1的打磨起点1；
134.接入电缆，启动机械手臂，即打开机械手臂的电源，启动后便可以开始打磨；
135.多阶段打磨工件：根据工件大小可以设置多个打磨点；
136.当机器人本体到达打磨结束点1，此时意味着打磨结束，前往等待点1；
137.（11）机器人本体再根据调度系统指令决定返回充电区或则继续下一区域的打磨
任务。
138.机器人本体在图1中示出的打磨区2以及该区域的等待点2、扫描起点2、扫描终点2、打磨起点2和打终点2的行驶路径与在打磨区1的完全相同，在此不再一一赘述。
139.如图4所示，图中的a采用双轮转向机器人不能原地自转，通过多项式求解生成的局部路径会与障碍物发生碰撞，因此不能通过特殊的狭窄工况。图中的b采用四轮转向的机器人，具备原地自转的可能，在m位置经过自转后，通过本发明方法生成的局部路径不会与障碍物发生碰撞，因此能更大限度通过特殊的狭窄工况。
140.最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。