CN110162029A - 一种基于规划路径的运动控制方法及装置、机器人 - Google Patents

Abstract

本申请公开了一种基于规划路径的运动控制方法及装置、机器人，用以为运动对象提供规则的运动路径，减少对运动对象进行运动控制的计算量。本申请实施例提供的一种基于规划路径的运动控制方法，该方法包括：针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

Description

一种基于规划路径的运动控制方法及装置、机器人

技术领域

本申请涉及机器人技术领域，尤其涉及一种基于规划路径的运动控制方法及装置、机器人。

背景技术

机器人沿着规划路径运动的过程中，机器人的速度与前方路径的特点有着密切关系：在一条平直的路径上，机器人可以加速行驶；当前方有转弯时，需要提前减速通过，通过对前方路径特点进行检测并进行拟合，机器人可以提前预知前方道路情况，方便运动控制做出速度计算。

现有技术一般采用全局路径规划算法得到运动路径，而全局路径规划算法得到的路径都是点的集合，即现有技术根据全局规划路径的路径点的集合进行运动控制，通过对每一路径点不同的角速度和线速度的仿真结果进行比较，选择结果最好的作为当前机器人运动的角速度和线速度。

综上，采用现有技术基于全局路径规划路径的路径点的集合进行运动控制的方法，由于需要对每一路径点不同的角速度和线速度的仿真结果进行比较，计算量大、实时效果差。

发明内容

本申请实施例提供了一种基于规划路径的运动控制方法及装置、机器人，用以为运动对象提供规则的运动路径，减少对运动对象进行运动控制的计算量。

本申请实施例提供的一种基于规划路径的运动控制方法，该方法包括：

针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；

从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

本申请实施例提供的基于规划路径的运动控制方法，在为运动对象提供运动参数控制其在规划路径上运动之前，将预先规划的路径上的路径点进行线性拟合或圆弧拟合，得到拟合路径及其终点，由于拟合路径为线段或圆弧，也就是说运动对象可以按照规则的路径运动，方便控制运动对象运动；并且根据确定的运动参数控制运动对象从当前位置向终点运动，无需对拟合路径上的每一路径点进行运动参数的计算，从而可以减少对运动对象进行运动控制的计算量；此外，当运动对象是机器人的情况下，运动路径是线段或者圆弧符合机器人的运动特性，更有利于控制机器人的运动。

可选地，在预先规划的路径上确定所述当前位置的最近路径点，具体包括：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

可选地，从所述最近路径点开始，进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点具体包括：

从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

确定所述拟合线段的长度是否小于预设长度，如果是则将所述拟合线段作为拟合路径，所述拟合线段的终点作为拟合终点；

否则从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能进行圆弧拟合为止，得到拟合圆弧；

确定所述拟合圆弧的长度是否小于所述拟合线段的长度，如果是则将所述拟合圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点；

否则，将所述拟合线段作为拟合路径，所述拟合线段的终点作为拟合终点。

可选地，从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段具体包括：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径分割点，从所述最近路径点开始依次连接所述分割点，得到多段路径向量，计算每一段路径向量的倾斜角；

将所述最近路径点所在的路径向量的倾斜角作为线性拟合的当前拟合直线的倾斜角，确定下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差是否小于第一预设阈值，如果是则将该段路径向量与当前拟合直线进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角，以此类推，直到下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差不小于第一预设阈值，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；

其中，采用如下公式计算更新的当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

可选地，在所述最近路径点至所述路径的终点范围内，以所述最近路径点作为路径拟合的起点进行弧线拟合，得到拟合圆弧具体包括：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径点，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标和半径；

将所述最近路径点所在的圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标和半径，确定下一段圆弧的圆心坐标和当前拟合圆弧的圆心坐标之差是否小于第二预设阈值，且下一段圆弧的半径和当前拟合圆弧的半径之差是否小于第三预设阈值，如果是则将该段圆弧拟合到当前拟合圆弧上，得到更新后的拟合圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标和半径，以此类推，直到下一段圆弧不可与当前拟合圆弧进行圆弧拟合，将当前拟合圆弧作为最终拟合圆弧；

其中采用如下公式计算更新后的当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算更新后的当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

可选地，在得到所述预先规划的路径上的拟合终点之后，该方法还包括确定所述当前位置的运动参数，具体包括：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值；

如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

可选地，确定所述当前位置的运动参数还包括：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

本申请实施例提供的一种基于规划路径的运动控制装置，该装置包括：

运动路径拟合模块，具体包括：

最近路径点确定模块，用于针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；

路径点拟合模块，用于从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

所述装置还包括运动控制模块，用于控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

可选地，所述最近路径点确定模块具体用于：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

可选地，所述路径点拟合模块具体包括：

线性拟合模块，用于从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

拟合路径确定模块，用于确定所述线段的长度是否小于预设长度，如果是则将所述线段作为拟合路径，所述拟合线段的终点作为拟合终点；

圆弧拟合模块，用于当所述线段长度小于预设长度时，从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能拟合成圆弧为止，得到拟合圆弧；

所述拟合路径确定模块还用于当得到拟合圆弧之后确定所述圆弧的长度是否小于所述线段的长度，如果是则将所述圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点，否则，将所述拟合直线的线段作为拟合路径，所述拟合线段的终点作为拟合终点。

可选地，所述线性拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径分割点，从所述最近路径点开始依次连接所述分割点，得到多段路径向量，计算每一段路径向量的倾斜角；

将所述最近路径点所在的路径向量的倾斜角作为线性拟合的当前拟合直线的倾斜角，确定下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差是否小于第一预设阈值，如果是则将该段路径向量与当前拟合直线进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角，以此类推，直到下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差不小于第一预设阈值，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；

其中，采用如下公式计算更新的当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

可选地，所述圆弧拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径点，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标和半径；

将所述最近路径点所在的圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标和半径，确定下一段圆弧的圆心坐标和当前拟合圆弧的圆心坐标之差是否小于第二预设阈值，且下一段圆弧的半径和当前拟合圆弧的半径之差是否小于第三预设阈值，如果是则将该段圆弧拟合到当前拟合圆弧上，得到更新后的拟合圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标和半径，以此类推，直到下一段圆弧不可与当前拟合圆弧进行圆弧拟合，将当前拟合圆弧作为最终拟合圆弧；

其中采用如下公式计算当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

可选地，还包括运动参数确定模块，具体用于：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值；

如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

可选地，所述运动参数确定模块还用于：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

本申请实施例提供的一种机器人，包括本申请实施例提供的上述运动控制装置。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简要介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例提供的一种基于规划路径的运动控制方法流程示意图；

图2为本申请实施例提供的另一种基于规划路径的运动控制方法流程示意图；

图3为本申请实施例提供的确定当前位置最近路径点示意图；

图4为本申请实施例提供的按照预设步长查找路径点的示意图；

图5为本申请实施例提供的一种运动控制装置的结构示意图。

具体实施方式

本申请实施例提供了一种基于规划路径的运动控制方法及装置、机器人，用以为运动对象提供规则的运动路径，减少对运动对象进行运动控制的计算量。

本申请实施例提供的一种基于规划路径的运动控制方法，如图1所示，该方法包括：

S101、针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；

其中，所述预先规划的路径为路径点的集合，所述路径点的集合中包括所述预先规划的路径的起点和终点；

S102、从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

S103、控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

本申请实施例提供的基于规划路径的运动控制方法，在为运动对象提供运动参数控制其在规划路径上运动之前，将预先规划的路径上的路径点进行线性拟合或圆弧拟合，得到拟合路径及其终点，由于拟合路径为线段或圆弧，也就是说运动对象可以按照规则的路径运动，方便控制运动对象运动；并且根据确定的运动参数控制运动对象从当前位置向终点运动，无需对拟合路径上的每一路径点进行运动参数的计算，从而可以减少对运动对象进行运动控制的计算量；此外，当运动对象是机器人的情况下，运动路径是线段或者圆弧符合机器人的运动特性，更有利于控制机器人的运动。

可选地，步骤S101在预先规划的路径上确定所述当前位置的最近路径点，具体包括：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

如图2所示，P点为运动对象当前位置，A、B、C、D、E为所述预先规划的路径上的路径点，其中A为当前位置P的最近路径点。

可选地，如图3所示，步骤S102从所述最近路径点开始，进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点具体包括：

S1021、从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

S1022、确定所述线段的长度是否小于预设长度，如果是则执行步骤S1023，否则执行步骤S1024；

S1023、将所述线段作为拟合路径，所述线段的终点作为拟合终点；

S1024、从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能拟合成圆弧为止，得到拟合圆弧；

S1025、确定所述拟合圆弧的长度是否小于所述拟合线段的长度，如果是则执行步骤S1023，否则，执行步骤S1026；

S1026、将所述拟合圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点。

其中，预设长度可根据需要进行设置。

可选地，步骤S1021从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段具体包括：

如图4所示，在所述预先规划的路径上，从所述最近路径点A开始至所述预先规划的路径上的终点F的方向，按照预设步长D_step依次查找L₁、L₂、L₃等路径分割点，从所述最近路径点A开始依次连接所述分割点，得到AL₁、L₁L₂、L₂L₃等多段路径向量，计算每一段路径向量的倾斜角；

将AL₁的倾斜角θ₁作为线性拟合的当前拟合直线的倾斜角θ，确定L₁L₂的倾斜角θ₂与AL₁的倾斜角θ₁之差是否小于第一预设阈值，如果是则认为L₁L₂与AL₁可以进行线性拟合，将L₁L₂与AL₁进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角θ，以此类推，直到下一段路径向量的倾斜角与当前拟合直线的倾斜角之差不小于第一预设阈值，认为下一段路径向量与当前拟合直线不可进行线性拟合，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；如果L₁L₂的倾斜角θ₂与AL₁的倾斜角θ₁之差不小于第一预设阈值，则AL₁即为拟合线段；

其中，采用如下公式计算更新当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

其中，预设步长D_step可以是1，即从最近路径点开始，计算任意相邻的两个路径点的倾斜角，当然，预设步长也可以大于1，可以根据对计算复杂度的需要来具体设置预设步长的值。当D_step是1时，图4中A与L₁、L₁与L₂、L₂与L₃为预先规划的路径上相邻的路径点，当D_step大于1时，A与L₁、L₁与L₂、L₂与L₃之间还有未示出的路径点。路径向量的倾斜角是指路径向量与预先规划的路径所在的直角坐标系X轴正向的夹角。

上述得到拟合线段的方法，是在判断是否可以进行线性拟合之前，在预先规划的路径上按照预设步长查找所有路径点得到路径向量，并计算每一路径向量的倾斜角，当然，也可以从最近路径点A开始，按照预设步长D_step查找L₁、L₂，得到路径向量AL₁、L₁L₂，计算AL₁、L₁L₂的倾斜角，将AL₁的倾斜角θ₁作为线性拟合的当前拟合直线的倾斜角θ，确定L₁L₂与AL₁可以进行线性拟合，得到更新后的当前拟合直线，并计算更新后的当前拟合直线的倾斜角θ之后，再预设步长D_step查找下一个路径点，得到下一段路径向量并计算路径向量的倾斜角，判断下一段路径向量与当前拟合直线是否可以进行线性拟合。

可选地，在所述最近路径点至所述路径的终点范围内，以所述最近路径点作为路径拟合的起点进行圆弧拟合，得到拟合圆弧具体包括：

如图4所示，从所述最近路径点A开始至所述预先规划的路径上的终点F的方向，在所述预先规划的路径上，按照预设步长D_step依次查找路径点L₁、L₂、L₃，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点A所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标O_i和半径R_i；

将AL₁之间的路径点拟合得到的圆弧作为当前拟合圆弧，将该圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标O和半径R，确定L₁L₂之间的路径点拟合得到的圆弧的圆心坐标O₂和当前拟合圆弧的圆心坐标O之差是否小于第二预设阈值，且L₁L₂之间的路径点拟合得到的圆弧的半径R₂和当前拟合圆弧的半径R之差是否小于第三预设阈值，如果是则认为L₁L₂之间的路径点拟合得到的圆弧与当前拟合圆弧可以进行圆弧拟合，将该段圆弧拟合到当前拟合圆弧上，得到更新后的拟合圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标O和半径R，以此类推，直到下一段圆弧不可与当前拟合圆弧进行圆弧拟合为止，将当前拟合圆弧作为最终拟合圆弧；如果L₁L₂之间的路径点拟合得到的圆弧的圆心坐标O₂和当前拟合圆弧的圆心坐标O之差不小于第二预设阈值，且L₁L₂之间的路径点拟合得到的圆弧的半径R₂和当前拟合圆弧的半径R之差不小于第三预设阈值，则认为L₁L₂之间的路径点拟合得到的圆弧与当前拟合圆弧不可以进行圆弧拟合，将AL₁之间的路径点拟合得到的圆弧作为最终拟合圆弧；

其中采用如下公式计算当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

当然，也可以从最近路径点A开始，按照预设步长D_step查找L₁、L₂，将AL₁之间的路径点进行圆弧拟合得到圆弧，将L₁L₂之间的路径点进行圆弧拟合得到圆弧，并计算圆弧的圆心位置坐标和半径，确定L₁L₂之间的路径点拟合得到的圆弧的与AL₁之间的路径点拟合得到的圆弧可以进行圆弧拟合，得到更新后的当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心和半径之后，再预设步长D_step查找下一个路径点L₃，将L₂L₃之间的路径点进行圆弧拟合得到圆弧，并计算圆弧的圆心位置坐标和半径，判断该段圆弧与当前拟合圆弧是否可以进行圆弧拟合。

需要说明的是，第一预设阈值、第二预设阈值以及第三预设阈值可根据计算精度的需要来进行具体设置。

可选地，在得到所述预先规划的路径上的拟合终点之后，该方法还包括确定所述当前位置的运动参数，具体包括：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值，如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

可选地，确定所述当前位置的运动参数还包括：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

第四预设阈值可根据需要设置，第五预设阈值可根据运动对象的不同来进行具体设置，本申请实施例提供的运动控制方法，确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值，即判断运动对象当前位置是否与当前规划的路径距离较远，如果是，则需要让机器人向最近路径点运动，之后再为运动对象提供在运动路径上的线速度和角速度。

需要说明的是，在确定所述运动对象的在所述当前位置的运动参数，根据所述运动参数控制所述运动对象从所述当前位置向所述终点运动之后，仍继续采用本申请实施例提供的运动控制方法，确定下一段拟合路径，并提供相应的运动参数，以此类推，直到运动对象运动到预先规划的运动路径的终点为止。

本申请实施例提供的一种基于规划路径的运动控制装置，如图5所示，该装置包括：

运动路径拟合模块1，具体包括：

最近路径点确定模块2，用于针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点，其中，所述预先规划的路径为路径点的集合，所述路径点的集合中包括所述预先规划的路径的起点和终点；

路径点拟合模块3，用于从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

所述装置还包括运动控制模块4，用于控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

可选地，所述最近路径点确定模块具体用于：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

可选地，所述路径点拟合模块具体包括：

线性拟合模块，用于从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

拟合路径确定模块，用于确定所述线段的长度是否小于预设长度，如果是则将所述线段作为拟合路径，所述拟合线段的终点作为拟合终点；

圆弧拟合模块，用于当所述线段长度小于预设长度时，从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能拟合成圆弧为止，得到拟合圆弧；

所述拟合路径确定模块还用于当得到拟合圆弧之后确定所述圆弧的长度是否小于所述线段的长度，如果是则将所述圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点，否则，将所述拟合直线的线段作为拟合路径，所述拟合线段的终点作为拟合终点。

可选地，所述线性拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径分割点，从所述最近路径点开始依次连接所述分割点，得到多段路径向量，计算每一段路径向量的倾斜角；

将所述最近路径点所在的路径向量的倾斜角作为线性拟合的当前拟合直线的倾斜角，确定下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差是否小于第一预设阈值，如果是则将该段路径向量与当前拟合直线进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角，以此类推，直到下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差不小于第一预设阈值，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；

其中，采用如下公式计算更新的当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

可选地，所述圆弧拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径点，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标和半径；

将所述最近路径点所在的圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标和半径，确定下一段圆弧的圆心坐标和当前拟合圆弧的圆心坐标之差是否小于第二预设阈值，且下一段圆弧的半径和当前拟合圆弧的半径之差是否小于第三预设阈值，如果是则将该段圆弧拟合到当前拟合圆弧上，得到更新后的拟合圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标和半径，以此类推，直到下一段圆弧不可拟合到当前拟合圆弧上，将当前拟合圆弧作为最终拟合圆弧；

其中采用如下公式计算当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

可选地，还包括运动参数确定模块，具体用于：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值；

如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

可选地，所述运动参数确定模块还用于：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

本申请实施例提供的一种机器人，包括本申请实施例提供的上述运动控制装置。本申请实施例提供的机器人例如可以是二轮差速驱动机器人。

综上所述，本申请实施例提供了一种基于规划路径的运动控制方法及装置、机器人，在为运动对象提供运动参数控制其在规划路径上运动之前，将预先规划的路径上的路径点进行线性拟合或圆弧拟合，得到拟合路径及其终点，由于拟合路径为线段或圆弧，也就是说运动对象可以按照规则的路径运动，方便控制运动对象运动；并且根据确定的运动参数控制运动对象从当前位置向终点运动，无需对拟合路径上的每一路径点进行运动参数的计算，从而可以减少对运动对象进行运动控制的计算量。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (15)

1.一种基于规划路径的运动控制方法，其特征在于，该方法包括：

针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；

从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

2.根据权利要求1所述的方法，其特征在于，在预先规划的路径上确定所述当前位置的最近路径点，具体包括：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

3.根据权利要求1所述的方法，其特征在于，从所述最近路径点开始，进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点具体包括：

从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

确定所述拟合线段的长度是否小于预设长度，如果是则将所述拟合线段作为拟合路径，所述拟合线段的终点作为拟合终点；

否则从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能进行圆弧拟合为止，得到拟合圆弧；

确定所述拟合圆弧的长度是否小于所述拟合线段的长度，如果是则将所述拟合圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点；

否则，将所述拟合线段作为拟合路径，所述拟合线段的终点作为拟合终点。

4.根据权利要求3所述的方法，其特征在于，从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段具体包括：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径分割点，从所述最近路径点开始依次连接所述分割点，得到多段路径向量，计算每一段路径向量的倾斜角；

将所述最近路径点所在的路径向量的倾斜角作为线性拟合的当前拟合直线的倾斜角，确定下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差是否小于第一预设阈值，如果是则将该段路径向量与当前拟合直线进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角，以此类推，直到下一段路径向量的倾斜角与当前拟合直线的倾斜角之差不小于第一预设阈值，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；

其中，采用如下公式计算更新的当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

5.根据权利要求3所述的方法，其特征在于，在所述最近路径点至所述路径的终点范围内，以所述最近路径点作为路径拟合的起点进行圆弧拟合，得到拟合圆弧具体包括：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径点，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标和半径；

将所述最近路径点所在的圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标和半径，确定下一段圆弧的圆心坐标和当前拟合圆弧的圆心坐标之差是否小于第二预设阈值，且下一段圆弧的半径和当前拟合圆弧的半径之差是否小于第三预设阈值，如果是则将该段圆弧与当前拟合圆弧进行圆弧拟合，得到更新后的圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标和半径，以此类推，直到下一段圆弧与当前拟合圆弧不可进行圆弧拟合，将当前拟合圆弧作为最终拟合圆弧；

其中采用如下公式计算更新后的当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算更新后的当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

6.根据权利要求1所述的方法，其特征在于，在得到所述预先规划的路径上的拟合终点之后，该方法还包括确定所述当前位置的运动参数，具体包括：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值；

如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

7.根据权利要求6所述的方法，其特征在于，确定所述当前位置的运动参数还包括：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

8.一种基于规划路径的运动控制装置，其特征在于，该装置包括：

运动路径拟合模块，具体包括：

最近路径点确定模块，用于针对运动对象当前位置，在预先规划的路径上确定所述当前位置的最近路径点；

路径点拟合模块，用于从所述最近路径点开始，在所述预先规划的路径上进行线性拟合或圆弧拟合，得到所述预先规划的路径上的拟合终点；

所述装置还包括运动控制模块，用于控制所述运动对象沿预先规划的路径从当前位置到所述拟合终点之间使用在所述当前位置确定的运动参数进行移动。

9.根据权利要求8所述的装置，其特征在于，所述最近路径点确定模块具体用于：

将所述当前位置与所述预先规划的路径上的每一路径点构成多个路径线段；

确定在代价地图上没有穿过障碍物区域、且长度最小的路径线段对应的路径点，作为所述当前位置的最近路径点。

10.根据权利要求8所述的装置，其特征在于，所述路径点拟合模块具体包括：

线性拟合模块，用于从所述最近路径点开始至所述预先规划的路径上的终点，在所述预先规划的路径上进行线性拟合，直到不能进行线性拟合为止，得到拟合线段；

拟合路径确定模块，用于确定所述线段的长度是否小于预设长度，如果是则将所述线段作为拟合路径，所述拟合线段的终点作为拟合终点；

圆弧拟合模块，用于当所述线段长度小于预设长度时，从所述最近路径点开始在所述预先规划的路径上进行圆弧拟合，直到不能进行圆弧拟合为止，得到拟合圆弧；

所述拟合路径确定模块还用于当得到拟合圆弧之后确定所述圆弧的长度是否小于所述线段的长度，如果是则将所述圆弧作为拟合路径，所述拟合圆弧的终点作为拟合终点，否则，将所述拟合直线的线段作为拟合路径，所述拟合线段的终点作为拟合终点。

11.根据权利要求10所述的装置，其特征在于，所述线性拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径分割点，从所述最近路径点开始依次连接所述分割点，得到多段路径向量，计算每一段路径向量的倾斜角；

将所述最近路径点所在的路径向量的倾斜角作为线性拟合的当前拟合直线的倾斜角，确定下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差是否小于第一预设阈值，如果是则将该段路径向量与当前拟合直线进行线性拟合，得到更新后的拟合直线作为当前拟合直线，并计算更新后的当前拟合直线的倾斜角，以此类推，直到下一段路径向量的倾斜角与所述当前拟合直线的倾斜角之差不小于第一预设阈值，将当前拟合直线作为线性拟合的最终拟合直线，得到拟合线段；

其中，采用如下公式计算更新的当前拟合直线的倾斜角：

θ＝(θ×(i-1)+θ_i)/i；

θ代表当前拟合直线的倾斜角，i代表第i段路径向量，θ_i代表第i段路径向量的倾斜角。

12.根据权利要求10所述的装置，其特征在于，所述圆弧拟合模块具体用于：

在所述预先规划的路径上，从所述最近路径点开始至所述预先规划的路径上的终点，按照预设步长依次查找路径点，将所述预先规划的路径划分成多段路径点的集合；

从所述最近路近点所在的路径点集合开始，对每一段路径点的集合采用最小二乘法进行圆弧拟合，并计算每一段圆弧的圆心位置坐标和半径；

将所述最近路径点所在的圆弧的圆心坐标和半径作为当前拟合圆弧的圆心坐标和半径，确定下一段圆弧的圆心坐标和当前拟合圆弧的圆心坐标之差是否小于第二预设阈值，且下一段圆弧的半径和当前拟合圆弧的半径之差是否小于第三预设阈值，如果是则将该段圆弧与当前拟合圆弧进行圆弧拟合，得到更新当后的拟合圆弧作为当前拟合圆弧，并计算更新后的当前拟合圆弧的圆心坐标和半径，以此类推，直到下一段圆弧不可与当前拟合圆弧进行圆弧拟合，将当前拟合圆弧作为最终拟合圆弧；

其中采用如下公式计算当前拟合圆弧的圆心坐标：

O＝(O×(i-1)+O_i)/i；

采用如下公式计算当前拟合圆弧的半径：

R＝(R×(i-1)+R_i)/i；

其中，O代表当前拟合圆弧的圆心坐标，R代表当前拟合圆弧的半径，i代表第i段圆弧，O_i代表第i段圆弧的圆心坐标，R_i代表第i段圆弧的半径。

13.根据权利要求8所述的装置，其特征在于，还包括运动参数确定模块，具体用于：

确定所述运动对象当前位置点与所述最近路径点之间的距离是否超过第四预设阈值；

如果是，则采用局部目标点法计算所述运动对象的角速度和线速度。

14.根据权利要求13所述的装置，其特征在于，所述运动参数确定模块还用于：

当所述运动对象当前位置点与所述最近路径点之间的距离未超过第四预设阈值时，确定所述运动对象当前航向角与所述线段的倾斜角或所述圆弧起点切向量的倾斜角之差是否超过第五预设阈值；

如果是则增大所述运动对象的角速度并减小所述运动对象的线速度；

否则，根据所述运动路径起点到终点的倾斜角度差和路径长度来计算角速度和线速度。

15.一种机器人，其特征在于，包括权利要求8～14任一项所述的运动控制装置。