CN104181926B - 机器人的导航控制方法 - Google Patents

Abstract

本发明涉及控制系统，公开了一种机器人的导航控制方法。本发明中，通过周期性地获取位于机器人前方的路面图像，检测获取的路面图像中是否存在地面标志，并根据地面标志存在与否的检测结果分两种情况计算机器人的位置和方向，进而再根据计算的位置和方向，控制所述机器人的行走方向和速度。降低了机器人的制造成本，而且坐标系的位置检测以及航位推测法的位置检测都是相对红外检测更为精准的位置计算方式，从而保证了对机器人行进路线的精准控制。另外，由于针对是否能检测到地面标志，分两种情况进行处理，可使得获取路面图像的周期时长不受限制，从而保证了对机器人行进方向与位置的实时计算。

Description

机器人的导航控制方法

技术领域

本发明涉及控制系统，特别涉及机器人的行走控制系统。

背景技术

现有的群体机器人的控制系统如图1所示，在该框架中，一般是把任务调度和任务分配模块在服务器上实现，在机器人本身的计算机上实现路径规划、运动控制、障碍物避让等功能。比如，通过机器人本身的传感器探测地图的变化、障碍物的存在及运动，并使用机器人自身的计算机计算行进的路线。机器人之间通过无线网络互相通信，根据需要分享相互之间的位置、速度、姿态、及探测到的障碍物信息等。

为了保证机器人按照规划的路线行走，通常需要在机器人中设计一个用于测距的红外线检测模块，通过该模块对外界物体的测距，控制机器人的行走。

然而，在上述方案中，不但需要对机器人增设红外线检测模块，导致机器人的制造成本上升。而且，仅根据对外界物体的测距控制机器人的行走，也无法保证其行进路线的准确性。

发明内容

本发明的目的在于提供一种机器人的导航控制方法，不但可以降低机器人的制造成本，同时保证了对机器人行进路线的精准控制。

为解决上述技术问题，本发明的实施方式提供了一种机器人的导航控制方法，包含以下步骤：

机器人在根据制定的导航指令进行行走的过程中，周期性地获取位于机器人前方的路面图像；

在每一次获取到所述路面图像后，检测所述路面图像中是否存在地面标志；

如果存在所述地面标志，则根据以路面图像建立的坐标系，计算所述机器人的位置和方向；如果不存在所述地面标志，则根据航位推测法计算所述机器人的位置和方向；

所述机器人根据所述计算的位置和方向，控制所述机器人的行走方向和速度。

本发明实施方式相对于现有技术而言，通过周期性地获取位于机器人前方的路面图像，检测获取的路面图像中是否存在地面标志，并根据地面标志存在与否的检测结果分两种情况计算机器人的位置和方向，进而再根据计算的位置和方向，控制所述机器人的行走方向和速度。由于是利用了存在于地面上的地面标志，进行机器人当前位置和方向(姿态)的计算，因此无需对机器人增设红外线检测模块，降低了机器人的制造成本，而且坐标系的位置检测以及航位推测法的位置检测都是相对红外检测更为精准的位置计算方式，从而保证了对机器人行进路线的精准控制。另外，由于针对是否能检测到地面标志，分两种情况进行处理，可使得获取路面图像的周期时长不受限制，从而保证了对机器人行进方向与位置的实时计算。

优选地，在根据以路面图像建立的坐标系，计算所述机器人的位置和方向时，以所述路面图像的垂直方向为x轴方向，水平方向为y轴方向，建立坐标系；在建立的所述坐标系中，获取所述所述机器人的中心点K和所述地面标志的中心点T；判断所述K的y轴坐标与所述T的y轴坐标是否相同，如果相同，则判定所述机器人的行走方向未发生偏移；如果不相同，则判定所述机器人的行走方向发生偏移，计算所述机器人偏移的距离x和角度θ。在检测到地面标志的情况下，进一步根据路面图像的中轴线和地面标志的中轴线是否一致，判断机器人的行走方向是否发生偏移，计算简单，简化了机器人的运算负荷。

优选地，在根据航位推测法计算机器人的位置和方向时，根据以下公式计算机器人偏移的距离x和角度θ：

其中，所述v₀表示机器人的行进速度；所述r表示机器人左右侧的轮距；所述θ₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移角度；所述x₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移距离；所述Δv表示根据最近一次以路面图像建立的坐标系，计算到的机器人的偏移角度和偏移距离，在机器人的两个轮子上施加的纠正偏差的控制量，所述dt表示对时间的积分。进一步保证了在未检测到地面标志时，对机器人位置和方向的计算准确性。

优选地，机器人利用自身携带的摄像头周期性地拍摄前方路面，得到所述路面图像。由于摄像头是机器人本身会配备的设备，因此，本发明实施方式的机器人的导航控制方法，无需对机器人增设额外的设备，易于实现。

附图说明

图1是根据现有技术中的群体机器人的控制系统结构示意图；

图2是本发明第一实施方式所涉及的机器人群体的控制系统结构示意图；

图3是根据本发明第一实施方式的机器人的导航控制方法流程图；

图4是根据本发明第一实施方式中的地图栅格化示意图；

图5是根据本发明第一实施方式中的行进路线示意图；

图6是根据本发明第一实施方式中的地面标志示意图；

图7是根据本发明第一实施方式中的通过二维码标签记录栅格编号信息的示意图；

图8是根据本发明第一实施方式中的机器人行驶在路径中心处且拍摄到地面标志的路面图像示意图；

图9是根据本发明第一实施方式中的机器人未行驶在路径中心处且拍摄到地面标志的路面图像示意图；

图10是根据本发明第一实施方式中的机器人未拍摄到地面标志的路面图像示意图；

图11是根据本发明第一实施方式中的纵向、横向相结合的运动控制示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请各权利要求所要求保护的技术方案。

本发明的第一实施方式涉及一种机器人的导航控制方法，本实施方式的导航控制方法应用在机器人群体的控制系统中。如图2所示，在该控制系统中，由信息管理子系统设置需要执行的任务，并将设置的任务发送给群体机器人管理服务器，供群体机器人管理服务器中的任务调度单元进行分配。具体地说，任务调度单元为各机器人分配任务，并将分配的任务发送给群体机器人管理服务器中的路径规划单元。路径保留单元用于标记禁止机器人行走的地理位置；路径规划单元用于根据分配给机器人的任务，结合路径保留单元标记的禁止行走的地理位置，为该机器人规划路径，制定该机器人到达目的地的行进路线。该控制系统中的机器人包含：导航单元与运动控制单元；其中，导航单元用于根据来自群体机器人管理服务器的任务指令，制定导航指令，任务指令包含行进路线；运动控制单元用于根据所述导航指令控制所述机器人的行走。

本实施方式的机器人的导航控制方法的流程图如图3所示，在步骤301中，机器人内的导航单元根据来自群体机器人管理服务器的任务指令，制定导航指令。

具体地说，预先将室内空间构建成栅格化地图，比如说，由路径规划单元中的地图构建子单元预先将室内的地图进行栅格化，每个栅格为正方形。在具体应用中，可根据实际需要确定每个栅格的大小，比如将每个栅格的实际面积设为1米左右。并且对每个栅格按行列进行编号，如图4所示，每个编号为二维数组，如(M,N),其中M代表行号，N代表列号。并且定义行号增加的方向为南方，列号增加的方向为东方(当然，也可以定义行号递减的方向为南方，列号递减的方向为东方)。机器人的导航单元从路径规划单元获取的行进路径是一个数组，这个数组的元素是从起点到终点的路径栅格的行列编号。通过对这个数组的行列编号进行处理，可以得到路径的长度、路径的方向、路径拐弯的栅格、路径拐弯的方向等一系列信息。使用这些信息可以制定一些导航指令如前进、转弯等。导航指令可设计为二元组：<方向，格数>、<拐弯，角度>。

比如说，导航指令中的方向是该路径下一次转弯前的方向，格数是从机器人所处的当前格到下一次转向前要走过的格数。这样指令<东，10>，<北，8>就分别代表往东走过10格，往北走过8格。当格数退到0时，表示要转弯或者已经抵达目的地，此时，算法要发出新的导航指令，若抵达目的地，则导航结束。

接着，在步骤302中，机器人对其当前位置进行初始化。

具体地说，实体机器人首次进入仓库时要打开相机，通过读取地面标志，对其当前位置进行初始化。同时，其摆放的方向要和程序中默认的初始方向一致。

需要说明的是，在本实施方式中，为了保证机器人会沿着收到的路径行走(如图5所示，图5中的编号1为起点位置，编号2为终点位置)，地图中每个栅格的中心固定有地面标志(地面标志为实物，放置于室内的地面上)，该地面标志可以是位于正方形外框内的二维码标签，如图6所示，通过二维码标签记录栅格的编号信息，如图7所示。使用地面标志记录地图上每个栅格的行列编号，并把标志固定到栅格的中心。实体机器人首次进入仓库时要打开相机，通过读取地面标志，对其当前位置进行初始化。同时，其摆放的方向要和程序中默认的初始方向一致。此后，机器人在工作过程中通过读取地面标志来确定自己在仓库中的位置，并且每次转向后，要对新的方向加以记录。这样，机器人就可以实时的掌握自身的位置和方向信息(将在后续的步骤中进行详细介绍)。

另外，需要说明的是，在实际应用中，机器人首次进入仓库时的位置也可以任意摆放，但需要告知其初始的方向，比如通过一个旋钮开关，让机器人知道自己的初始方向。或者，也可以在地面标志中添加特定的符号供机器人识别，从而确定初始方向。再或者，也可以让机器人试走几步，通过对比行列号的变化确定机器人的初始方向。

接着，在步骤303中，机器人在根据制定的导航指令进行行走的过程中，周期性地获取位于机器人前方的路面图像。

具体地说，机器人可利用自身携带的摄像头周期性地拍摄前方路面，得到所述路面图像。比如说，机器人安装于正面头部或胸部位置的摄像头，每隔5s拍摄一次，得到当前的前方路面图像。由于摄像头是机器人本身会配备的设备，因此，无需对机器人增设额外的设备，易于实现。

接着，进入步骤304，机器人在每一次获取到路面图像后，检测拍摄到的路面图像中是否存在地面标志。

具体地说，若机器人严格的行驶在路径中心处，则其拍摄的图像如图8所示，图像的中轴线和地标的中轴线应该完全一致(由于拍摄时机的问题，地面标签不一定出现在图像的正中心，但中轴线应一致)。因此，在本实施方式中，机器人首先检测拍摄到的路面图像中是否存在地面标志，如果检测到该地面标志，则进入步骤305，如果未检测到地面标志，则进入步骤306。

在步骤305中，机器人根据以路面图像建立的坐标系，计算机器人的位置和方向。

具体地说，如图8所示，以路面图像的垂直方向为x轴方向，水平方向为y轴方向，建立坐标系xoy，左上、右上、左下、右下四个顶点分别为A(0,0),B(0,b),C(c,0),D(c,b)。然后，在建立的坐标系中，获取机器人的中心点K和地面标志的中心点T。比如说，地面标志的四个顶点分别为E,F,G,H,坐标分别为(x₁，y₁)，(x₂，y₂)，(x₃，y₃)，(x₄，y₄)，那么机器人的中心点K位于地面标志的中心点，也就是栅格的中心点，位于接着，判断K的y轴坐标与T的y轴坐标是否相同，如果相同，则判定机器人的行走方向未发生偏移，根据这两点间的坐标，从而可确定机器人的姿态；如果不相同，则判定机器人的行走方向发生偏移，如图9所示，计算机器人偏移的距离x和角度θ，用于后续的校正。

本步骤中，机器人偏移的角度θ根据以下公式获取：

并以I、J、K作为3个顶点构建三角形，其中，I、J的坐标分别为：

计算顶点K到边IJ的高，并将计算的高作为机器人偏移的距离x。

也就是说，当机器人驶过栅格中心时，可通过采用自身携带的传感器识别栅格中心的地标(二维码)，从而得知自身位置。同时通过图像处理的方法，获得机器人的姿态(方向)信息。在检测到地面标志的情况下，进一步根据路面图像的中轴线和地面标志的中轴线是否一致，判断机器人的行走方向是否发生偏移，计算简单，简化了机器人的运算负荷。

如果在步骤304中，未检测到地面标志，则进入步骤306，机器人根据航位推测法计算所述机器人的位置和方向。

具体地说，如图10所示，当机器人驶离一个地标，但尚未到达下一个地标时，可根据偏差状态空间模型计算机器人偏移的距离x和角度θ。其中，偏差状态空间模型里有两个参数，一个是速度v0，可以通过运动控制单元的反馈信息获得，另一个是机器人左右侧的轮距r，可以通过机器人的设计图纸获得。偏差状态空间模型如下：

通过偏差状态空间模型计算出来的数据即是偏差距离x和方向角度，在本步骤中估计位置和方向的方程为：

其中，所述v₀表示机器人的行进速度；所述r表示机器人左右侧的轮距；所述θ₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移角度；所述x₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移距离；所述Δv表示根据最近一次以路面图像建立的坐标系，计算到的机器人的偏移角度和偏移距离，在机器人的两个轮子上施加的纠正偏差的控制量(Δv的计算方式将在后文中进行描述)，所述dt表示对时间的积分。也就是说，在每一次根据拍摄到的地面标志计算出机器人偏移的距离和角度后，都需要将计算出的偏移的距离和角度记录下来。如果当前拍摄的图像中不存在地标，则可以将上一次记录的偏移距离和偏移角度，作为积分公式中的偏移距离的初始值x₀和偏移角度的初始值θ₀，从而通过积分实时估计当前偏差的角度和距离。

接着，在步骤307中，机器人根据计算的位置和方向，控制机器人的行走方向和速度。

具体地说，为了使机器人沿着规划的路径行走，本实施方式还提供了对机器人的运动控制技术，它包括对机器人的横向和纵向控制。横向控制的目的是使机器人始终保持在路径中心附近行使，不产生过大的侧向偏移。纵向控制的目的是使得机器人以设定的速度平稳、准确的行使到目标位置。本实施方式中，横向控制采用左右轮差分驱动的技术，纵向控制采用比例-积分-微分PID位置闭环控制，通过两个方向的控制量互相叠加，同时实现纵向和横向的运动控制。具体阐述如下：

首先，根据计算的机器人偏移的距离x和角度θ计算纠正偏差的控制量Δv。比如说，根据以下公式计算纠正偏差的控制量Δv：

Δv＝-k₁x-k₂θ

其中，k₁和k₂表示预先设置的正参数，具体取值可以根据经验值得到，或者通过实验数据获取。

然后，将所述Δv的一半，分别以加减的方式与机器人的行走速度相叠加，得到分别分配给机器人左右两个电机的控制量，完成机器人的行走方向和速度的同时控制。具体地说，在本实施方式中，机器人的纵向控制采用PID控制，根据目标位置和当前位置的距离(也就是导航指令中路径的栅格数)产生纵向的速度指令，该指令受到预定速度值的饱和约束。机器人纵向和横向运动控制相结合的控制框图如图11所示。由此可见，在本步骤中，通过把纵向控制的速度指令和横向控制的差速指令叠加的方法，达到纵横向同时控制的目的。

不难发现，在本实施方式中，通过周期性地获取位于机器人前方的路面图像，检测获取的路面图像中是否存在地面标志，并根据地面标志存在与否的检测结果分两种情况计算机器人的位置和方向，进而再根据计算的位置和方向，控制所述机器人的行走方向和速度。由于是利用了存在于地面上的地面标志，进行机器人当前位置和方向(姿态)的计算，因此无需对机器人增设红外线检测模块，降低了机器人的制造成本，而且坐标系的位置检测以及航位推测法的位置检测都是相对红外检测更为精准的位置计算方式，从而保证了对机器人行进路线的精准控制。另外，由于针对是否能检测到地面标志，分两种情况进行处理，可使得获取路面图像的周期时长不受限制，从而保证了对机器人行进方向与位置的实时计算。

本发明的第二实施方式涉及一种机器人的导航控制方法。第二实施方式在第一实施方式的基础上做了进一步改进，主要改进之处在于：在本发明第二实施方式中，机器人还包含用于探测障碍物的超声传感器，运动控制单元在超声传感器探测到障碍物后，控制机器人停留在原地，直至障碍物消失。也就是说，本实施方式中对于不可预测的运动障碍物，比如行人，采用超声传感器实时探测。探测到障碍物以后，停留在当前栅格等待障碍物离开。障碍物离开以后重新启动保留、导航、运动控制过程。在具体实现时，机器人会每隔一定角度安装一个超声传感器，可以形成360度的覆盖。传感器把探测到的物体的距离发送给机器人内部的MCU(微控制单元)，MCU会根据所有传感器返回的信息绘制周围的障碍物和通道地图。

值得一提的是，上述各实施方式中所涉及到的各模块均为逻辑模块，在实际应用中，一个逻辑单元可以是一个物理单元，也可以是一个物理单元的一部分，还可以以多个物理单元的组合实现。此外，为了突出本发明的创新部分，本实施方式中并没有将与解决本发明所提出的技术问题关系不太密切的单元引入，但这并不表明本实施方式中不存在其它的单元。

本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (9)

1.一种机器人的导航控制方法，其特征在于，包含以下步骤：

机器人在根据制定的导航指令进行行走的过程中，周期性地获取位于机器人前方的路面图像；

在每一次获取到所述路面图像后，检测所述路面图像中是否存在地面标志；

如果存在所述地面标志，则根据以路面图像建立的坐标系，计算所述机器人的位置和方向；如果不存在所述地面标志，则根据航位推测法计算所述机器人的位置和方向；

所述机器人根据所述计算的位置和方向，控制所述机器人的行走方向和速度；

所述根据航位推测法计算机器人的位置和方向的步骤中，根据以下公式计算机器人偏移的距离x和角度θ：

$\left\{\begin{array}{c}\mathrm{\&theta;}={\mathrm{\&theta;}}_0+\frac{1}{2r}\&Integral;\mathrm{\&Delta;}vdt\\ x=x_0+\&Integral;v_0\mathrm{\&theta;}dt\end{array}\right.$

其中，所述v₀表示机器人的行进速度；所述r表示机器人左右侧的轮距；所述θ₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移角度；所述x₀表示最近一次根据以路面图像建立的坐标系，计算到的机器人的偏移距离；所述Δv表示根据最近一次以路面图像建立的坐标系，计算到的机器人的偏移角度和偏移距离，在机器人的两个轮子上施加的纠正偏差的控制量，所述dt表示对时间的积分。

2.根据权利要求1所述的机器人的导航控制方法，其特征在于，所述根据以路面图像建立的坐标系，计算所述机器人的位置和方向的步骤中，包含以下子步骤：

以所述路面图像的垂直方向为x轴方向，水平方向为y轴方向，建立坐标系；

在建立的所述坐标系中，获取所述机器人的中心点K和所述地面标志的中心点T；

判断所述K的y轴坐标与所述T的y轴坐标是否相同，如果相同，则判定所述机器人的行走方向未发生偏移；如果不相同，则判定所述机器人的行走方向发生偏移，计算机器人偏移的距离x和角度θ。

3.根据权利要求2所述的机器人的导航控制方法，其特征在于，所述计算机器人偏移的距离和角度的步骤中，包含以下子步骤：

将所述地面标志左上、右上、左下、右下的四个顶点分别设为E(x₁，y₁)，F(x₂，y₂)，G(x₃，y₃)，H(x₄，y₄)；

所述机器人偏移的角度θ根据以下公式获取：

$\mathrm{\&theta;}=arctan\left(\frac{y_2-y_4}{x_4-x_2}\right)$

以I、J、K作为3个顶点构建三角形，其中，所述I、J的坐标分别为：

$I(\frac{x_1+x_2}{2},\frac{y_1+y_2}{2}),J(\frac{x_3+x_4}{2},\frac{y_3+y_4}{2})$

计算所述顶点K到边IJ的高，并将计算的高作为机器人偏移的距离x。

4.根据权利要求2所述的机器人的导航控制方法，其特征在于，所述控制所述机器人的行走方向和速度的步骤中，包含以下子步骤：

根据计算的机器人偏移的距离x和角度θ计算纠正偏差的控制量Δv；

将所述Δv的一半，分别以加减的方式与机器人的行走速度相叠加，得到分别分配给所述机器人左右两个电机的控制量，完成所述机器人的行走方向和速度的同时控制。

5.根据权利要求4所述的机器人的导航控制方法，其特征在于，所述机器人的行走速度采用比例-积分-微分PID的控制算法获取。

6.根据权利要求4所述的机器人的导航控制方法，其特征在于，根据以下公式计算所述纠正偏差的控制量Δv：

Δv＝-k₁x-k₂θ

其中，所述k₁和k₂表示预先设置的正参数。

7.根据权利要求1至6中任一项所述的机器人的导航控制方法，其特征在于，所述地面标志为位于正方形外框内的二维码标签，所述二维码标签记录该地面标志所在栅格的编号信息；其中，所述栅格为对室内进行栅格化后得到的各栅格，每个栅格按行列进行编号，得到所述编号信息，所述编号信息为二维数组。

8.根据权利要求7所述的机器人的导航控制方法，其特征在于，所述地面标志固定在所在栅格的中心。

9.根据权利要求1至6中任一项所述的机器人的导航控制方法，其特征在于，所述周期性地获取位于机器人前方的路面图像的步骤中，所述机器人利用自身携带的摄像头周期性地拍摄前方路面，得到所述路面图像。