CN106527438A - 机器人导航控制方法和装置 - Google Patents

Abstract

本发明公开了一种机器人导航控制方法和装置。其中，该方法包括：在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差；根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差；根据机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。本发明解决了现有技术中机器人无法准确跟随期望路径的技术问题。

Description

机器人导航控制方法和装置

技术领域

本发明涉及机器人控制领域，具体而言，涉及一种机器人导航控制方法和装置。

背景技术

智能移动机器人是一类能够通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自主导航运动，从而完成预定任务的机器人系统。要实现机器人自主导航运动，必须要解决环境建模、实时定位、路径规划、运动控制等一系列问题。

其中，移动机器人的运动控制根据控制目标的不同，可以分为轨迹跟踪、道路跟随和点镇定三类问题。道路跟随问题是指在惯性坐标系中，机器人从给定的初始状态出发，到达并跟随指定的几何路径。

在已知环境导航任务中，通过全局路径规划可以确定机器人期望的移动路径；在未知环境中，移动机器人通过传感器获取环境信息建立环境模型，基于环境模型进行路径规划，确定期望的移动路径。基于路径规划得到的期望轨迹一般表示为全局坐标系中的一系列路标点。现有技术中的路径跟随控制算法不能确保机器人准确的跟随期望路径，机器人会和期望路径发生较大偏差。

针对现有技术中机器人无法准确跟随期望路径的问题，目前尚未提出有效的解决方案。

发明内容

本发明实施例提供了一种机器人导航控制方法和装置，以至少解决现有技术中机器人无法准确跟随期望路径的技术问题。

根据本发明实施例的一个方面，提供了一种机器人导航控制方法，包括：

在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差；

根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差；

根据机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。

进一步地，在机器人沿着预先写入的期望路径移动过程中，还包括：

确定机器人是否到达期望路径的终点；

在机器人到达期望路径的终点的情况下，生成停车指令，并按照停车指令停止移动。

进一步地，在机器人沿着预先写入的期望路径移动过程中，还包括：

确定机器人前方是否有障碍；

在机器人前方有障碍的情况下，生成停车指令，并按照停车指令停止移动。

进一步地，在机器人沿着预先写入的期望路径移动过程中，还包括：

确定机器人是否接收到外界的遥控指令；

在机器人接收到外界的遥控指令的情况下，按照遥控指令进行移动。

进一步地，确定机器人是否到达期望路径的终点的步骤包括：

根据机器人的当前位置确定机器人的目标路径节点，其中，期望路径包括多个路径节点，每一个路径节点都有一个序号，且相邻的路径节点的序号相差为一，目标路径节点为多个路径节点之一；

确定目标路径节点的序号是否为多个路径节点中最后一个路径节点的序号加一；若是，则确定机器人到达期望路径的终点，否则，确定机器人还未到达期望路径的终点。

进一步地，根据机器人的当前位置确定机器人的目标路径节点的步骤包括：

从多个路径节点的第一个路径节点开始，依次计算机器人当前位置与每一个路径节点的距离；

若存在与机器人当前位置的距离小于等于门限值的路径节点，则选取第一个与机器人当前位置的距离小于等于门限值的路径节点的下一个路径节点作为目标路径节点，并终止计算机器人当前位置与多个路径节点中剩余路径节点的距离；

若机器人当前位置与多个路径节点的距离均大于门限值，则选取与机器人当前位置距离为最小时对应的路径节点作为目标路径节点。

进一步地，根据机器人的当前位置确定机器人的目标路径节点的步骤之后包括：

确定机器人当前位置与目标路径节点的距离；

在机器人当前位置与目标路径节点的距离小于等于门限值的情况下，确认机器人到达目标路径节点，将目标路径节点的下一个路径节点作为新的目标路径节点。

进一步地，确定机器人当前位置与目标路径节点的距离的步骤包括：

按照预设位置判断周期确定机器人当前位置与目标路径节点的距离。

进一步地，在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差的步骤包括：

在机器人沿着预先写入的期望路径移动过程中，按照预设航向调整周期确定机器人与期望路径的侧向偏差。

进一步地，确定机器人与期望路径的侧向偏差的步骤包括：

其中，pl_dist表示机器人与期望路径的侧向偏差，x和y分别表示机器人当前位置的横坐标和纵坐标，X1和Y1分别表示机器人在当前位置时的目标路径节点的上一个路径节点的横坐标和纵坐标，X2和Y2分别表示机器人在当前位置时的目标路径节点的横坐标和纵坐标。

进一步地，根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差的步骤包括：

其中，Corner表示机器人的航向偏差，Head表示机器人的航向，Kd_P表示侧向偏差比例系数。

进一步地，根据机器人的航向偏差确定左右电机控制量的步骤包括：

根据机器人的航向偏差确定航向PI调整参数；

根据航向PI调整参数确定左右电机控制量。

根据本发明实施例的另一方面，还提供了一种机器人导航控制装置，包括：

第一确定模块，用于在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差；

第二控制模块，用于根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差；

第三控制模块，用于根据机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。

在本发明实施例中，采用双闭环道路跟随控制算法，通过先确定机器人与期望路径的侧向偏差，根据侧向偏差确定机器人的航向偏差，再根据机器人的航向偏差确定左右电机控制量，通过左右电机控制量生成的左右电机控制指令来控制左右电机，达到了确保机器人准确跟随期望路径的目的，从而实现了机器人平稳、连贯按照期望路径运动的技术效果，进而解决了现有技术中机器人无法准确跟随期望路径的技术问题。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例1的一种可选的机器人导航控制的方法示意图一；

图2是根据本发明实施例1的一种可选的机器人导航控制的方法示意图二；

图3是根据本发明实施例2的一种可选的机器人导航控制的装置结构图一；

图4是根据本发明实施例2的一种可选的机器人导航控制的装置结构图二；

图5是根据本发明实施例2的一种可选的机器人导航控制的装置结构图三；

图6是根据本发明实施例2的一种可选的机器人导航控制的装置结构图四；

图7是根据本发明实施例2的一种可选的机器人导航控制的装置结构图五；

图8是根据本发明实施例2的一种可选的机器人导航控制的装置结构图六；

图9是根据本发明实施例2的一种可选的机器人导航控制的装置结构图七；以及

图10是根据本发明实施例2的一种可选的机器人导航控制的装置结构图八。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

根据本发明实施例，提供了一种机器人导航控制的方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

图1是根据本发明实施例1的一种可选的机器人导航控制的方法示意图一，如图1所示，该方法包括如下步骤：

步骤S102，在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差。

具体的，在已知环境导航任务中，通过全局路径规划可以确定机器人期望的移动路径，也就是本发明中所说的期望路径；在未知环境中，机器人可以通过传感器来获取环境信息从而建立环境模型，基于环境模型进行路径规划，最终得到期望路径。通常情况下，期望路径一般表示为全局坐标系中的一系列路径节点。期望路径会被保存在机器人中的硬盘中，通常可以通过交互流程将期望路径写入硬盘中，因此，在启动机器人后，或者在向机器人发送按照期望路径移动的指令后，机器人需要首先从硬盘中读取有关期望路径的数据，然后才能沿着期望路径进行移动，并且为了确定机器人与期望路径的航向偏差，本发明采用通过机器人与期望路径的侧向偏差来确定航向偏差的方式，因此需要首先确定机器人与期望路径的侧向偏差。

步骤S104，根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差。

具体的，在确定了机器人与期望路径的侧向偏差后，可以根据机器人的航向、侧向偏差和侧向偏差比例系数来确定机器人的航向偏差。

需要说明的是，机器人的航向是实时测量的，侧向偏差比例系数是预先设置好的。

步骤S106，根据机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。

具体的，机器人中设置有左右电机，包括左电机和右电机，左右电机控制机器人的运动，包括机器人的运动速度、方向等，因此在机器人与期望路径发生偏差的时候，最终需要通过调节左右电机来使机器人重新回到期望路径上，因此在确定了机器人与期望路径的航向偏差后，需要根据航向偏差最终得到左右电机控制量，包括左电机控制量和右电机控制量，根据左右电机控制量生成具体的左右电机控制指令，包括左电机控制指令和右电机控制指令，并将左电机控制指令发送给左电机，将右电机控制指令发送给右电机，实现对左电机和右电机的分别控制，从而调整机器人的航向，使机器人准确跟随期望路径。

通过上述步骤S102-步骤S106，本发明采用双闭环道路跟随控制算法，通过先确定机器人与期望路径的侧向偏差，根据侧向偏差确定机器人的航向偏差，再根据机器人的航向偏差确定左右电机控制量，通过左右电机控制量生成的左右电机控制指令来控制左右电机，达到了确保机器人准确跟随期望路径的目的，从而实现了机器人平稳、连贯按照期望路径运动的技术效果，进而解决了现有技术中机器人无法准确跟随期望路径的技术问题。本发明的机器人导航控制方法和装置应用在机器人控制领域，尤其适用于地面移动机器人在室内环境中的导航控制。

在一种可选的实施例中，在机器人沿着预先写入的期望路径移动过程中，还包括步骤S200：确定机器人是否到达期望路径的终点；在机器人到达期望路径的终点的情况下，生成停车指令，并按照停车指令停止移动。

具体的，如果机器人到达期望路径的终点，则自动生成停车指令，并将停车指令发送给机器人中相应的控制部件，由相应的控制部件控制机器人停止移动，如果机器人没有到达期望路径的终点，则控制机器人继续沿着期望路径移动。

需要说明的是，本发明中确定机器人是否到达期望路径的终点时，并没有限定是确定机器人是否第一次到达期望路径的终点，本发明中对机器人的控制命令可以是机器人第一次到达期望路径的终点时，就生成停车指令，也可以在机器人第一次到达期望路径的终点后，沿着期望路径进行循环运动，直至某一次到达期望路径的终点时，再生成停车指令控制机器人停止运动，具体循环的次数，可以由机器人控制者自行设置。

在一种可选的实施例中，在机器人沿着预先写入的期望路径移动过程中，还包括步骤S300：确定机器人前方是否有障碍；在机器人前方有障碍的情况下，生成停车指令，并按照停车指令停止移动。

具体的，如果机器人在移动的过程中，前方出现障碍物，则自动生成停车指令，并将停车指令发送给机器人中相应的控制部件，由相应的控制部件控制机器人停止移动，其中，可以通过安装在机器人上的传感器或者摄像装置等来识别障碍物，或者通过其他现有技术来识别前方是否出现障碍物，本发明中对此不作赘述。

需要说明的是，在障碍物被移除前，机器人不能按照最先的控制指令继续沿着期望路径移动，但是可以重新接收其他的一些控制指令，例如绕开障碍物等，可以在绕开障碍物后，接收继续沿着期望路径进行移动的控制指令，继续沿着期望路径移动；同样的，在障碍物被移除后，也可以接收继续沿着期望路径进行移动的控制指令，继续沿着期望路径移动。

在一种可选的实施例中，在机器人沿着预先写入的期望路径移动过程中，还包括步骤S400：确定机器人是否接收到外界的遥控指令；在机器人接收到外界的遥控指令的情况下，按照遥控指令进行移动。

具体的，如果机器人在沿着期望路径移动的过程中，接收到了其他的遥控指令，来命令机器人执行其他动作，则机器人放弃当前的移动命令，优先执行新接收到的遥控指令。

需要说明的是，在执行完遥控指令后，可以提供给机器人控制者不再执行接收到遥控指令时正在执行的控制指令的选项和继续执行接收到遥控指令时正在执行的控制指令的选项。

可选的，发送给机器人的遥控指令可以是控制机器人的专用的遥控装置发出的，也可以是智能移动设备发出的，智能移动设备可以是手机、电脑或穿戴式设备如智能手表等。

在一种可选的实施例中，步骤S200包括：

步骤S202，根据机器人的当前位置确定机器人的目标路径节点，其中，期望路径包括多个路径节点，每一个路径节点都有一个序号，且相邻的路径节点的序号相差为一，目标路径节点为多个路径节点之一。

步骤S204，确定目标路径节点的序号是否为多个路径节点中最后一个路径节点的序号加一；若是，则确定机器人到达期望路径的终点，否则，确定机器人还未到达期望路径的终点。

具体的，由于期望路径一般表示为全局坐标系中的一系列路径节点，按照顺序给每一个路径节点进行编号，每一个路径节点都有一个序号，本发明首先确定机器人将要移向的路径节点，也就是目标路径节点，如果目标路径节点的序号是最后一个路径节点的序号加一，就说明机器人已经到达了最后一个路径节点，也就是机器人已经到达了期望路径的终点。

在一个可选的实施例中，例如假设期望路径上有PNum+1个路径节点，序号分别是0、1、2、…、PNum，用R_Num表示目标路径节点的序号，如果R_Num＝PNum+1，也就是说明机器人已经到达了期望路径的终点。

在一种可选的实施例中，步骤S202包括：

步骤S206，从多个路径节点的第一个路径节点开始，依次计算机器人当前位置与每一个路径节点的距离；若存在与机器人当前位置的距离小于等于门限值的路径节点，则选取第一个与机器人当前位置的距离小于等于门限值的路径节点的下一个路径节点作为目标路径节点，并终止计算机器人当前位置与多个路径节点中剩余路径节点的距离；若机器人当前位置与多个路径节点的距离均大于门限值，则选取与机器人当前位置距离为最小时对应的路径节点作为目标路径节点。

具体的，为了确定机器人在当前位置使的目标路径节点，首先需要从第一路径节点开始计算与机器人当前位置的距离，并设置了一个门限值，比较计算出来的距离与门限值的大小，如果出现距离小于等于门限值的路径节点，则将该路径节点的下一个路径节点作为目标路径节点，并且假设该路径节点不是最后一个路径节点，剩余的路径节点与机器人当前位置的距离就不再计算了；如果所有路径节点与机器人当前位置的距离都不能满足小于等于门限值，说明机器人当前位置距离所有的路径节点都较远，则选择一个相对较近的路径节点作为目标路径节点，也就是选择与机器人当前位置距离最小的路径节点作为目标路径节点。

在一个可选的实施例中，例如假设期望路径上有PNum+1个路径节点，序号分别是0、1、2、…、PNum，PNum+1个路径节点的XY坐标分别是(Route[0][0],Route[0][1])、(Route[1][0],Route[1][1])、…、(Route[PNum][0],Route[PNum][1])，其中(Route[i][0],Route[i][1])表示序号为i的路径节点也就是第i+1个路径节点的XY坐标，机器人当前位置的坐标为(x，y)，则机器人当前位置与序号为i的路径节点的距离的计算公式如下：

其中，distN[i]表示机器人当前位置与序号为i的路径节点的距离，若序号为l的路径节点为第一个与机器人当前位置的距离小于等于门限值的路径节点，也就是满足distN[l]≤P_Gate，其中P_Gate表示门限值，则将序号为l+1的路径节点也就是第l+2个路径节点作为机器人在当前位置的目标路径节点，即R_Num＝l+1，其中R_Num表示目标路径节点的序号。如果机器人当前位置与所有路径节点的距离均大于门限值，则选取与机器人当前位置距离为最小时对应的路径节点作为目标路径节点，用计算公式表示即：

其中，n_min表示与机器人当前位置距离为最小时对应的路径节点的序号，distN[n_min]表示与机器人当前位置的最小距离。此时，R_Num＝n_min。

需要说明的是，门限值的大小可以根据机器人的实际应用场景或者其他因素进行具体设置，本发明对门限值的大小并不做限定。

在一种可选的实施例中，在计算目标路径节点时，如果计算出的目标路径节点的序号与上一次计算目标路径算法时计算的结果相同，则说明该目标路径节点已经走过，因此将计算出的目标路径节点的下一个路径节点作为目标路径节点。

在一种可选的实施例中，步骤S202之后包括：

步骤S208：确定机器人当前位置与目标路径节点的距离；在机器人当前位置与目标路径节点的距离小于等于门限值的情况下，确认机器人到达目标路径节点，将目标路径节点的下一个路径节点作为新的目标路径节点。

在一种可选的实施例中，步骤S208具体为：按照预设位置判断周期确定机器人当前位置与目标路径节点的距离。

具体的，机器人并不是不间断的判断其与目标路径节点的距离，而是按照一定的位置判断周期进行判断的，预设的位置判断周期可以根据具体情况进行设置，本发明对预设位置判断周期的具体数值不做限定。

在一种可选的实施例中，步骤S102具体为：在机器人沿着预先写入的期望路径移动过程中，按照预设航向调整周期确定机器人与期望路径的侧向偏差。

具体的，机器人并不是不间断的确定其与期望路径的侧向偏差，而是按照一定的航向调整周期进行确定的，只有到达预设的航向调整周期，才开始进行机器人与期望路线的侧向偏差的确定；预设的航向调整周期可以根据具体情况进行设置，本发明对预设航向调整周期的具体数值不做限定。

在一种可选的实施例中，步骤S102中确定侧向偏差使用的公式如下所示：

其中，pl_dist表示机器人与期望路径的侧向偏差，x和y分别表示机器人当前位置的横坐标和纵坐标，X1和Y1分别表示机器人在当前位置时的目标路径节点的上一个路径节点的横坐标和纵坐标，X2和Y2分别表示机器人在当前位置时的目标路径节点的横坐标和纵坐标。

在一种可选的实施例中，步骤S104中确定侧向偏差使用的公式如下所示：

其中，Corner表示机器人的航向偏差，Head表示机器人的航向，Kd_P表示侧向偏差比例系数。

具体的，机器人的航向可以通过罗盘测得，通过罗盘测得的航向可以调整到超宽带定位系统坐标系下，需要说明的是，计算出的航向偏差需要调整到0度至360度范围内。

在一种可选的实施例中，步骤S106包括：

步骤S502：根据机器人的航向偏差确定航向PI调整参数；

步骤S503：根据航向PI调整参数确定左右电机控制量。

在一种可选的实施例中，根据机器人的航向偏差确定航向PI调整参数可以用Xh表示，航向PI调整参数的计算公式如下所示：

Xh＝Xh_I+Xh_P

其中，其中航向PI调整参数的积分项为Xh_I，航向PI调整参数的比例项为Xh_P，Xh_I＝∑Kh_I·Corner，Xh_P＝Kh_P·Corner，Kh_I和Kh_P分别表示积分系数和比例系数。

在一种可选的实施例中，根据航向PI调整参数确定左右电机控制量具体包括：根据航向PI调整参数分别确定左电机控制量和右电机控制量，用TL和TR分别表示左电机控制量和右电机控制量，左电机控制量TL为在原先的基础上减少Xh/2，右电机控制量TR为在原先的基础上增加Xh/2。

在一种可选的实施例中，需要对左电机控制量和右电机控制量进行限幅，之后发送给驱动器，根据左右电机控制量进一步生成左右电机控制指令，发送给左右电机，左右电机根据左右电机控制指令来对机器人的航向进行调整。

在一种优选的实施例中，如图2所示，在机器人启动或者接收到沿着期望路径进行移动的控制命令后，首先读取期望路径，然后根据机器人的当前位置确定目标路径节点R_Num，通过R_Num＝PNum+1来判断机器人是否到达期望路径的终点，其中PNum表示期望路径上最后一个路径节点的序号，如果机器人到达期望路径的终点，则生成停车指令，并结束当前跟随期望路径的流程，即路径跟随流程结束，如果机器人没有到达期望路径的终点，则判断机器人的前方是否有障碍，如果有，则生成停车指令，并结束当前跟随期望路径的流程，即路径跟随流程结束，如果机器人的前方没有障碍，则判断机器人是否接收到遥控指令，如果接收到，则结束当前跟随期望路径的流程，即路径跟随流程结束，如果没有，则判断是否到达位置判断周期，如果到达位置判断周期，则计算机器人当前位置与目标路径节点的距离Dist，并判断Dist与门限值P_Gate的大小，如果Dist小于等于门限值P_Gate，则确定机器人到达目标路径节点，其中图2中R_Num++表示将当前位置的目标路径节点的下一个路径节点的序号赋予目标路径节点，也就是将当前目标路径节点的下一个路径节点作为新的目标路径节点；如果没有到达位置判断周期，以及如果Dist大于门限值P_Gate，以及在确定了新的目标路径节点后，则进入判断是否到达航向调整周期的判断，如果没有到达航向调整周期，则返回判断机器人是否到达期望路径的终点的步骤，如果到达航向调整周期，则首先计算侧向偏差pl_dist，然后根据侧向偏差pl_dist计算航向偏差Corner，接着根据航向偏差Corner计算PI调整参数Xh，根据PI调整参数Xh计算左右电机控制量TR和TL，接着发送左右电机控制指令，之后返回判断机器人是否到达期望路径的终点的步骤。

实施例2

根据本发明实施例，提供了一种机器人导航控制的产品实施例，图3是根据本发明实施例的机器人导航控制装置，如图3所示，包括第一确定模块101、第二确定模块103和第三确定模块105。

其中，第一确定模块101，用于在机器人沿着预先写入的期望路径移动过程中，确定机器人与期望路径的侧向偏差。

第二控制模块103，用于根据机器人的航向、侧向偏差以及侧向偏差比例系数，确定机器人的航向偏差。

第三控制模块105，用于根据机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。

通过上述模块，本发明采用双闭环道路跟随控制算法，通过第一确定模块101先确定机器人与期望路径的侧向偏差，通过第二控制模块103根据侧向偏差确定机器人的航向偏差，再通过第三控制模块105根据机器人的航向偏差确定左右电机控制量，通过左右电机控制量生成的左右电机控制指令来控制左右电机，达到了确保机器人准确跟随期望路径的目的，从而实现了机器人平稳、连贯按照期望路径运动的技术效果，进而解决了现有技术中机器人无法准确跟随期望路径的技术问题。

在一种可选的实施例中，如图4所示，本发明的装置还包括终点确定模块201，用于确定机器人是否到达期望路径的终点；在机器人到达期望路径的终点的情况下，生成停车指令，并按照停车指令停止移动。

在一种可选的实施例中，如图5所示，本发明的装置还包括障碍确定模块301，用于确定机器人前方是否有障碍；在机器人前方有障碍的情况下，生成停车指令，并按照停车指令停止移动。

在一种可选的实施例中，如图6所示，本发明的装置还包括遥控指令确定模块401，用于确定机器人是否接收到外界的遥控指令；在机器人接收到外界的遥控指令的情况下，按照遥控指令进行移动。

在一种可选的实施例中，如图7所示，终点确定模块201包括目标路径节点确定模块203和终点判断模块205。

其中，目标路径节点确定模块203用于根据机器人的当前位置确定机器人的目标路径节点，其中，期望路径包括多个路径节点，每一个路径节点都有一个序号，且相邻的路径节点的序号相差为一，目标路径节点为多个路径节点之一。

终点判断模块205用于确定目标路径节点的序号是否为多个路径节点中最后一个路径节点的序号加一；若是，则确定机器人到达期望路径的终点，否则，确定机器人还未到达期望路径的终点。

在一种可选的实施例中，如图8所示，目标路径节点确定模块203包括距离计算模块207，用于从多个路径节点的第一个路径节点开始，依次计算机器人当前位置与每一个路径节点的距离；若存在与机器人当前位置的距离小于等于门限值的路径节点，则选取第一个与机器人当前位置的距离小于等于门限值的路径节点的下一个路径节点作为目标路径节点，并终止计算机器人当前位置与多个路径节点中剩余路径节点的距离；若机器人当前位置与多个路径节点的距离均大于门限值，则选取与机器人当前位置距离为最小时对应的路径节点作为目标路径节点。

在一种可选的实施例中，如图9所示，终点确定模块201还包括距离确定模块209，距离确定模块209分别与目标路径节点确定模块203和终点判断模块205连接，用于确定机器人当前位置与目标路径节点的距离；在机器人当前位置与目标路径节点的距离小于等于门限值的情况下，确认机器人到达目标路径节点，将目标路径节点的下一个路径节点作为新的目标路径节点。

在一种可选的实施例中，距离确定模块209的具体实施包括：按照预设位置判断周期确定机器人当前位置与目标路径节点的距离。

在一种可选的实施例中，第一确定模块101的具体实施包括：在机器人沿着预先写入的期望路径移动过程中，按照预设航向调整周期确定机器人与期望路径的侧向偏差。

在一种可选的实施例中，如图10所示，第三确定模块105包括参数确定模块501和控制量确定模块503。

其中，参数确定模块501用于根据机器人的航向偏差确定航向PI调整参数。

控制量确定模块503用于根据航向PI调整参数确定左右电机控制量。

其中侧向偏差、航向偏差、航向PI调整参数以及左右电机控制量的计算公式在方法部分已经进行了详细的论述，此处不再赘述。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (13)

1.一种机器人导航控制方法，其特征在于，包括：

在所述机器人沿着预先写入的期望路径移动过程中，确定所述机器人与所述期望路径的侧向偏差；

根据所述机器人的航向、所述侧向偏差以及侧向偏差比例系数，确定所述机器人的航向偏差；

根据所述机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。

2.根据权利要求1所述的方法，其特征在于，在所述机器人沿着预先写入的期望路径移动过程中，还包括：

确定所述机器人是否到达所述期望路径的终点；

在所述机器人到达所述期望路径的终点的情况下，生成停车指令，并按照所述停车指令停止移动。

3.根据权利要求1或2所述的方法，其特征在于，在所述机器人沿着预先写入的期望路径移动过程中，还包括：

确定所述机器人前方是否有障碍；

在所述机器人前方有障碍的情况下，生成停车指令，并按照所述停车指令停止移动。

4.根据权利要求1或2所述的方法，其特征在于，在所述机器人沿着预先写入的期望路径移动过程中，还包括：

确定所述机器人是否接收到外界的遥控指令；

在所述机器人接收到所述外界的遥控指令的情况下，按照所述遥控指令进行移动。

5.根据权利要求2所述的方法，其特征在于，所述确定所述机器人是否到达所述期望路径的终点的步骤包括：

根据所述机器人的当前位置确定所述机器人的目标路径节点，其中，所述期望路径包括多个路径节点，每一个所述路径节点都有一个序号，且相邻的所述路径节点的序号相差为一，所述目标路径节点为所述多个路径节点之一；

确定所述目标路径节点的序号是否为所述多个路径节点中最后一个路径节点的序号加一；若是，则确定所述机器人到达所述期望路径的终点，否则，确定所述机器人还未到达所述期望路径的终点。

6.根据权利要求5所述的方法，其特征在于，所述根据所述机器人的当前位置确定所述机器人的目标路径节点的步骤包括：

从所述多个路径节点的第一个路径节点开始，依次计算所述机器人当前位置与每一个所述路径节点的距离；

若存在与所述机器人当前位置的距离小于等于门限值的路径节点，则选取第一个与所述机器人当前位置的距离小于等于所述门限值的路径节点的下一个路径节点作为所述目标路径节点，并终止计算所述机器人当前位置与所述多个路径节点中剩余路径节点的距离；

若所述机器人当前位置与所述多个路径节点的距离均大于所述门限值，则选取与所述机器人当前位置距离为最小时对应的路径节点作为所述目标路径节点。

7.根据权利要求6所述的方法，其特征在于，所述根据所述机器人的当前位置确定所述机器人的目标路径节点的步骤之后包括：

确定所述机器人当前位置与所述目标路径节点的距离；

在所述机器人当前位置与所述目标路径节点的距离小于等于所述门限值的情况下，确认所述机器人到达所述目标路径节点，将所述目标路径节点的下一个路径节点作为新的所述目标路径节点。

8.根据权利要求7所述的方法，其特征在于，所述确定所述机器人当前位置与所述目标路径节点的距离的步骤包括：

按照预设位置判断周期确定所述机器人当前位置与所述目标路径节点的距离。

9.根据权利要求1所述的方法，其特征在于，所述在所述机器人沿着预先写入的期望路径移动过程中，确定所述机器人与所述期望路径的侧向偏差的步骤包括：

在所述机器人沿着预先写入的期望路径移动过程中，按照预设航向调整周期确定所述机器人与所述期望路径的侧向偏差。

10.根据权利要求5-8任一项所述的方法，其特征在于，所述确定所述机器人与所述期望路径的侧向偏差的步骤包括：

$pl\_dist=\frac{x\·(Y2-Y1)+y\·(X1-X2)+Y1\·X2-X1\·Y2}{\sqrt{{(X1-X2)}^2+{(Y1-Y2)}^2}}$

其中，pl_dist表示所述机器人与所述期望路径的侧向偏差，x和y分别表示所述机器人当前位置的横坐标和纵坐标，X1和Y1分别表示所述机器人在所述当前位置时的所述目标路径节点的上一个路径节点的横坐标和纵坐标，X2和Y2分别表示所述机器人在所述当前位置时的所述目标路径节点的横坐标和纵坐标。

11.根据权利要求10所述的方法，其特征在于，所述根据所述机器人的航向、所述侧向偏差以及侧向偏差比例系数，确定所述机器人的航向偏差的步骤包括：

$Corner=arctg\frac{Y2-y}{X2-x}-Head+Kd\_P\·pl\_dist$

其中，Corner表示所述机器人的航向偏差，Head表示所述机器人的航向，Kd_P表示所述侧向偏差比例系数。

12.根据权利要求1或11所述的方法，其特征在于，所述根据所述机器人的航向偏差确定左右电机控制量的步骤包括：

根据所述机器人的航向偏差确定航向PI调整参数；

根据所述航向PI调整参数确定所述左右电机控制量。

13.一种机器人导航控制装置，其特征在于，包括：

第一确定模块，用于在所述机器人沿着预先写入的期望路径移动过程中，确定所述机器人与所述期望路径的侧向偏差；

第二控制模块，用于根据所述机器人的航向、所述侧向偏差以及侧向偏差比例系数，确定所述机器人的航向偏差；

第三控制模块，用于根据所述机器人的航向偏差确定左右电机控制量，并生成左右电机控制指令发送给左右电机。