适用于车型机器人的局部路径规划与追踪方法与流程

本发明涉及自动化清洁领域，具体涉及一种适用于车型机器人的局部路径规划与追踪方法。

背景技术：

在自动化清洁应用领域，室内无人驾驶扫地/洗地机因其高效的清洁效率，高自动化作业方式，开始受到众多大型室内场景环卫需求方的青睐。但是，目前的扫地/洗地机器人产品中，大多是面向小型化室内场景，终端用户以家庭为主，故其所选机器人型号多以圆盘型小型机器人为主。事实上，在更大型的场馆，如机场、高铁站、球馆等场景下，该类机器人存在清洁效率低下、清洁能力不足的问题，因此针对此类场景，需要改用清洁能力更强的车型机器人，以克服圆盘型机器人的不足。

对于清洁机器人而言，路径追踪与避障是指其在参考路径的指引下，追踪路径并实现障碍物躲避的技术。对于车型机器人而言，由于其运动灵活性不如圆盘型机器人，因此针对圆盘型机器人的局部路径规划与追踪无法直接适用于车型机器人，这就需要针对车型机器人重新设计一套局部路径规划与追踪方法。

技术实现要素：

本发明的目的是克服现有技术的不足，提供一种适用于车型机器人的局部路径规划与追踪方法。

为达到上述目的，本发明采用的技术方案是：

一种适用于车型机器人的局部路径规划与追踪方法，其包括如下步骤：

(1)在待清洁区域内选择待执行的参考路径，定位车型机器人并感知其周边障碍物，得到相应的参考信息；

(2)结合所述参考信息，在所述参考路径上选取局部路径，规划连接所述局部路径的起点和终点，得到避开障碍物的安全路径；

(3)对所述安全路径进行拟合，得到平滑的最终路径；

(4)沿着所述最终路径依次选取追踪点，并分别计算所述车型机器人对应于每个所述追踪点的转向角度，根据相应的转向角度控制所述车型机器人的行进方向，从而实现对所述的最终路径的追踪。

优选地，在所述步骤(1)中，利用cartographer算法实现所述车型机器人的定位。

优选地，在所述步骤(1)中，通过单线激光雷达对所述车型机器人的周边障碍物进行感知。

优选地，在所述步骤(2)中，所述局部路径的规划连接通过a*算法实现，所述a*算法包括如下步骤：

a1、建立所述待清洁区域的搜索图，将所述局部路径的起点放入开集作为初始节点；

a2、在所述开集中寻找所述初始节点的周围可达节点，并跳过存在于闭集中的节点，将所述初始节点标记为其可达节点的父节点；

a3、计算所述初始节点至其可达节点的损失值f，将f的最小值所对应的所述可达节点作为新初始节点，并将所述的新初始节点从开集删除后加入闭集；

a4、对所述的新初始节点进行判断，若所述的新初始节点与所述局部路径的终点不相重合，则重复步骤a2和步骤a3；若所述的新初始节点与所述局部路径的终点相重合，则回溯所有的父节点，并得到所述的安全路径。

进一步优选地，所述的损失值f的计算公式为：f＝h+c，其中h为启发式函数，c为累积路径损失值。

优选地，在所述步骤(3)中，所述的拟合包括如下步骤：

b1、在所述的安全路径内采样锚点，其中所述锚点的个数为n；

b2、在所述待清洁区域所在平面内建立坐标系，并分别利用拟合函数参数化拟合所述锚点的横坐标和纵坐标，得到采样参数；

b3、将所述采样参数代入拟合函数回算横坐标和纵坐标，随后根据多个所述的横坐标和纵坐标得到平滑曲线，所述的平滑曲线即为所述的最终路径。

进一步优选地，所述的参数化拟合包括如下步骤：

c1、将每个锚点的参数u分别设定为1/n，根据每个锚点对应的参数u分别拟合所述锚点的横坐标x和纵坐标y的值；

c2、对所述的横坐标x和所述的纵坐标y进行迭代拟合，并计算当前迭代的损失值；

c3、判断前后两次迭代的损失值之差，当所述损失值之差大于期望阈值时，重复所述步骤c2，继续进行迭代拟合；当所述损失值之差小于期望阈值时，结束迭代，将当前的参数u输出并作为所述锚点的采样参数；

其中，所述拟合函数的具体公式为：

x＝a*u+b*u*u+c*u*u*u；

y＝a*u+b*u*u+c*u*u*u。

更进一步优选地，所述的期望阈值为10^-3。

优选地，在所述步骤(4)中，所述追踪点的选取包括如下步骤：

d1、确定所述车型机器人的预瞄距离；

d2、在所述最终路径上选择与所述车型机器人的后轮轴中心点之间距离相对最小的点作为目标点；

d3、对所述目标点进行判断，若所述车型机器人的后轮轴中心点至所述目标点的距离不小于所述的预瞄距离，则选定所述目标点为所述的追踪点；若所述车型机器人的后轮轴中心点至所述目标点的距离小于所述的预瞄距离，则重复所述步骤d2，直至得到所述的追踪点。

优选地，在所述步骤(4)中，所述转向角度的计算公式如下：

其中，α为所述的转向角度，d为所述车型机器人的前轮轴中心点与后轮轴中心点之间的距离，l为所述车型机器人的预瞄距离，β为所述车型机器人的后轮轴中心点与所述追踪点之间的连线以及所述车型机器人的前轮轴中心点与后轮轴中心点之间的连线这两者所形成的夹角。

由于上述技术方案的运用，本发明与现有技术相比具有下列优点：

(1)能够充分考虑车型机器人的车身运动约束及障碍物约束，对参考路径进行局部的规划与追踪，使得车型机器人能够在行进过程中有效的躲避障碍物并追踪安全路径。

(2)基于几何关系的路径追踪不仅能够分别实现车型机器人的前进与后退，同时能够实现狭窄空间的掉头，提高了车型机器人清洁作业的灵活性。

附图说明

附图1为本发明的具体实施例中的局部路径规划与追踪方法的流程示意图；

附图2为本发明的具体实施例中的a*算法的流程示意图；

附图3为本发明的具体实施例中的拟合流程示意图；

附图4为本发明的具体实施例中的参数化拟合的流程示意图；

附图5为本发明的具体实施例中的追踪点的选取流程示意图；

附图6为本发明的具体实施例中的计算转向角度的几何原理示意图。

具体实施方式

下面结合附图来对本发明的技术方案作进一步的阐述。

本发明涉及对清洁机器人的局部路径规划与追踪方法的改进，改进后的局部路径规划与追踪方法，能够很好的适用于车型机器人，以克服圆盘型机器人在大场景内清洁能力不足、清洁效率低下的问题。

具体地，如图1所示，本发明公开的该种适用于车型机器人的局部路径规划与追踪方法，其包括如下步骤：

(1)在待清洁区域内选择待执行的参考路径，定位车型机器人并感知其周边障碍物，得到相应的参考信息。

在本实施例中，参考路径为根据现有规划算法得出的待清洁区域的全局覆盖路径，只要能够得出区域全局覆盖路径的规划算法均可应用于本发明中。同时利用cartographer算法实现车型机器人的定位，并通过单线激光雷达对车型机器人的周边障碍物进行感知。这里的单线激光雷达以及cartographer算法均为现有技术，具体结构及原理不再赘述。

(2)结合参考信息，在参考路径上选取局部路径，规划连接局部路径的起点和终点，得到避开障碍物的安全路径。在这里，参考信息为车型机器人的位置信息、周边障碍物信息以及选取的参考路径。

在本实施例中，局部路径的规划连接通过a*算法实现，如图2所示，a*算法包括如下步骤：

a1、建立待清洁区域的搜索图，将局部路径的起点放入开集作为初始节点。在这里，搜索图为根据区域地图及周边障碍物建立的区域节点图，能够有效的将区域中的各种因素考虑在内，以供a*算法得出一条最佳路径。

a2、在开集中寻找初始节点的周围可达节点，并跳过存在于闭集中的节点，将初始节点标记为其可达节点的父节点。

a3、计算初始节点至其可达节点的损失值f，将f的最小值所对应的可达节点作为新初始节点，并将新初始节点从开集删除后加入闭集。这里的损失值f的计算公式为：f＝h+c，其中h为启发式函数，c为累积路径损失值。

a4、对新初始节点进行判断，若新初始节点与局部路径的终点不相重合，则重复步骤a2和步骤a3；若新初始节点与局部路径的终点相重合，则回溯所有的父节点，并得到安全路径。这里得到的安全路径即为根据a*算法计算出的起点到终点的最短避障路径。

(3)对安全路径进行拟合，得到平滑的最终路径。

在本实施例中，如图3所示，拟合包括如下步骤：

b1、在安全路径内采样锚点，其中锚点的个数为n。

b2、在待清洁区域所在平面内建立坐标系，并分别利用拟合函数参数化拟合锚点的横坐标和纵坐标，得到采样参数。

其中，拟合函数的具体公式为：

x＝a*u+b*u*u+c*u*u*u；

y＝a*u+b*u*u+c*u*u*u。

如图4所示，具体的参数化拟合包括如下步骤：

c1、将每个锚点的参数u分别设定为1/n，根据每个锚点对应的参数u分别拟合锚点的横坐标x和纵坐标y的值。

c2、对横坐标x和纵坐标y进行迭代拟合，并计算当前迭代的损失值。

c3、判断前后两次迭代的损失值之差，当损失值之差大于期望阈值时，重复步骤c2，继续进行迭代拟合；当损失值之差小于期望阈值时，结束迭代，将当前的参数u输出并作为锚点的采样参数。这里的期望阈值为10^-3。

b3、将采样参数代入拟合函数回算横坐标和纵坐标，随后根据多个横坐标和纵坐标得到平滑曲线，该平滑曲线即为最终路径。这里得到的最终路径便于车型机器人追踪，符合车辆的运动规律，提高了车型机器人的适应性。

(4)沿着最终路径依次选取追踪点，并分别计算车型机器人对应于每个追踪点的转向角度，根据相应的转向角度控制车型机器人的行进方向，从而实现对最终路径的追踪。这里的转向角度即为车型机器人的方向盘转过角度，能够体现车型机器人的行进方向。

在本实施例中，如图5所示，追踪点的选取包括如下步骤：

d1、确定车型机器人的预瞄距离。在这里，预瞄距离为结合车型机器人的自身车辆参数以及运动约束条件得出的能够实现车辆追踪的最短距离，本实施例中的预瞄距离为0.5～1m。

d2、在最终路径上选择与车型机器人的后轮轴中心点之间距离相对最小的点作为目标点。在这里，车型机器人前轮转向，后轮驱动，因此通过后轮轴中心点作为车型机器人的定位点。其中前轮轴中心点表示两前轮的对称中心，后轮轴中心点表示两后轮的对称中心。

d3、对该目标点进行判断，若车型机器人的后轮轴中心点至该目标点的距离不小于预瞄距离，则选定该目标点为追踪点；若车型机器人的后轮轴中心点至该目标点的距离小于预瞄距离，则重复步骤d2，直至得到追踪点。如车型机器人需要向前行进，则向前选取目标点；如需向后行进，则向后选取目标点。

在这里，若车型机器人的后轮轴中心点至该目标点的距离不小于预瞄距离，则表示考虑到车型机器人的运动约束，该目标点能够作为车型机器人的追踪点来行进转向；若车型机器人的后轮轴中心点至该目标点的距离小于预瞄距离，则表示考虑到车型机器人的运动约束，该目标点不能作为车型机器人的追踪点来行进转向，需要重新寻找其他目标点。当得到追踪点后，车型机器人能够以该追踪点为目标行进转向，当行进至该追踪点与后轮轴中心点之间的距离小于预瞄距离的位置后，此时考虑到车型机器人的运动约束，该追踪点无法继续追踪，需要依照前述方法重新选择新的追踪点，并以该新追踪点为目标继续行进，以实现对局部路径的完整追踪。

本实施例中的具体的转向角度的计算公式如下：

其中，α为转向角度，d为车型机器人的前轮轴中心点与后轮轴中心点之间的距离，l为车型机器人的预瞄距离，β为车型机器人的后轮轴中心点与追踪点之间的连线以及车型机器人的前轮轴中心点与后轮轴中心点之间的连线这两者所形成的夹角。

在本实施例中，如图6所示，上述的转向角度的计算公式为根据车型机器人与追踪点之间的几何关系得出，其包括前进转向角度以及后退转向角度：

其中，①为车型机器人前进时的转向角度计算公式，②为车型机器人后退时的转向角度计算公式。

具体的，如图6所示，其中a为车型机器人的前轮；b为车型机器人的后轮；g_fg_b曲线为通过前述a*算法得到的最终路径曲线；o点为车型机器人的后轮轴中心点；l_f和l_b分别为车型机器人相对于前进目标点g_f和后退目标点g_b的预瞄距离；β_f为在前进方向上，后轮轴中心点与追踪点之间的连线以及车型机器人的前轮轴中心点与后轮轴中心点之间的连线这两者所形成的夹角；β_b为在后退方向上，后轮轴中心点与追踪点之间的连线以及车型机器人的前轮轴中心点与后轮轴中心点之间的连线这两者所形成的夹角；曲线s1和曲线s2分别为根据前进转向角度和后退转向角度得到的车型机器人的前进曲线及后退曲线；o_f和o_b分别为前进和后退的车体旋转中心点。

具体的前进的转向角度的推导步骤如下：

根据图6中的几何关系可得：

将上述等式逐级向下推导：

由上述等式即可求得：这里的k为前进曲线的曲率。

而根据图6中的几何关系可得，车型机器人的转向角度α_f＝tan^-1(k*d)；由此可进一步推知，

在上述的推导过程中，r_f为车型机器人前进过程中的运动半径，用以辅助推导转向角度。

同样的，后退的转向角度

上述的基于几何关系的路径追踪不仅能够分别实现车型机器人的前进与后退，同时能够通过对车型机器人的前进与后退的控制实现狭窄空间的掉头。

根据计算出的上述的转向角度，实现车型机器人的路径追踪，从而车型机器人能够根据追踪的路径行进。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并加以实施，并不能以此限制本发明的保护范围，凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围内。