CN112880683A - 基于参考直线距离的机器人定位控制方法、系统及芯片 - Google Patents

Abstract

本发明公开基于参考直线距离的机器人定位控制方法、系统及芯片，所述机器人定位控制方法包括：通过控制移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离的方式来获取移动机器人在遍历前后与一个固定的定位基站之间的距离，而不是计算获取同一位置的机器人与不同的基站之间的距离，不需同时收发处理两个基站的通信指令，减少数据运算处理的难度，简化利用距离信息计算移动机器人的实时位置坐标的方法，定位精度的可控性增强，克服里程计实时反馈的移动机器人的行走距离所带来的漂移误差的影响。

Description

基于参考直线距离的机器人定位控制方法、系统及芯片

技术领域

本发明涉及移动机器人导航定位的技术领域，特别是基于参考直线距离的机器人定位控制方法、系统及芯片。

背景技术

具有自主导航功能的移动机器人，这几年发展迅速，例如常见的家居清洁类扫地机。目前常见的slam技术有视觉导航、激光导航、惯性导航等。其中，惯性导航由于其低成本，在一些低端产品上获得广泛应用，但是它存在全局坐标定位不准确的问题。

在现有技术中，惯性传感器在机器人轮组打滑或者机器人轮组在地毯移动过程中易于随时间累积非系统性误差。比如，惯性传感器所包括的里程计在相对短的距离上精确的导航位位置的推算可能易于随时间而累积出漂移误差，导致定位精度不可控；甚至在一些人为推动机器人机体的情况下，利用惯性传感器可以计算机器人转动的角度，但是不能计算出实时位置或计算结果的偏差很大。

中国专利CN111381586A为了克服上述技术缺陷，通过同时与至少两个UWB基站通信的方式来计算机器人相对于各个UWB基站之间的距离，结合至少两个UWB基站的位置计算得到机器人坐标位置，但是至少需要在同一室内区域中布置两个基站，计算过程复杂，加重移动机器人的运行负载。

发明内容

为此，本发明技术方案只使用一个用于无线测距的基站辅助机器人沿着坐标轴方向行走定位，克服里程计的定位信息随导航行走的时间推移而积累漂移误差的问题，并简化坐标数据运算方法。具体技术方案如下：

基于参考直线距离的机器人定位控制方法，包括：步骤1、在移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离的过程中，通过移动机器人上设置的第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走遍历参考直线距离之前与定位基站之间的距离、移动机器人行走遍历参考直线距离之后与同一定位基站之间的距离；其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于这个定位基站的位置在全局地图上建立起全局坐标系；移动机器人行走过参考直线距离是由移动机器人的里程计的实时反馈获得的；参考直线距离是预先设定的大于0的距离；步骤2、基于这个定位基站的位置、参考直线距离、移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离前后与定位基站之间的距离，计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标；步骤3、将步骤2计算出的位置坐标、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标。

与现有技术相比，本技术方案通过控制移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离的方式来获取移动机器人在遍历前后与一个固定的定位基站之间的距离，而不是计算获取同一位置的机器人与不同的基站之间的距离，不需同时收发处理两个基站的通信指令，减少数据运算处理的难度，简化利用距离信息计算移动机器人的实时位置坐标的方法，定位精度的可控性增强，克服里程计实时反馈的移动机器人的行走距离所带来的漂移误差的影响。

进一步地，所述步骤2的方法步骤具体包括：首先，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，其中，预设参考路径的路径长度是参考直线距离；所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，其中，所述全局坐标系是以所述定位基站的位置为原点建立的；根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标；其中，预设参考路径是平行于所述第一预设坐标轴方向。

与现有技术相比，本技术方案基于预设参考路径的起点位置、预设参考路径的终点位置与所述定位基站三个坐标位置确定的三角形，在一个全局坐标系内利用无线测距传感器测得的距离信息构建余弦定理关系式以计算出预设参考路径的终点位置的坐标作为移动机器人的实时位置坐标，使得定位精度可控，克服惯性传感器在全局坐标系内定位误差较大的问题。

进一步地，所述步骤3包括：将三角几何关系式计算出的预设参考路径的终点位置的坐标、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合，以滤除所述第一无线测距传感器与所述第二无线测距传感器的通信测距中出现的噪声，实现对三角几何关系式计算出的预设参考路径的终点位置的坐标进行滤波；其中，融合方式为：根据移动机器人的里程计测得的距离信息和移动机器人的陀螺仪测得的角度信息，利用三角几何关系计算出移动机器人在惯性导航过程中的惯性坐标，用于参与所述预设参考路径的终点位置的坐标的滤波运算。提高机器人的定位和移动精度。

进一步地，当第一无线测距传感器是UWB标签时，第二无线测距传感器是UWB基站。相较于GPS、Zigbee等无线定位方式，精度更高，成本更低；相对于超声波传感器，信号探测角度更大。

进一步地，在移动机器人上设置的第一无线测距传感器与所述定位基站内设置的第二无线测距传感器进行通信测距过程中，若计算获得移动机器人在行走过程中与同一个定位基站之间的距离保持不变，判定移动机器人被卡住，然后控制移动机器人的里程计停止记录行走距离数据。该技术方案在异常处理的过程中能够减小数据处理量。

进一步地，所述定位基站还集成充电座；当控制所述移动机器人沿着第二预设坐标轴方向离开充电座后，控制移动机器人转动以使得移动机器人的行走方向与第一预设坐标轴方向平行；其中，第一预设坐标轴与第二预设坐标轴垂直；其中，移动机器人的行走方向仅限于平行于坐标轴。该技术方案让充电结束的机器人沿着坐标轴方向顺利退座，便于进入定位导航模式。

进一步地，当第一预设坐标轴为X轴时，第二预设坐标轴为Y轴，其中，第一预设坐标轴方向包括X轴正方向或X轴负方向，第二预设坐标轴方向包括Y轴正方向或Y轴负方向；当第一预设坐标轴为Y轴时，第二预设坐标轴为X轴，其中，第一预设坐标轴方向包括Y轴正方向或Y轴负方向，第二预设坐标轴方向包括X轴正方向或X轴负方向；其中，全局坐标系包括第一预设坐标轴和第二预设坐标轴。从而拓展前述技术方案的定位方法的运用场景，且降低坐标的计算复杂度。

一种移动机器人的定位控制系统，所述定位控制系统包括一个移动机器人和一个定位基站，移动机器人上设置第一无线测距传感器和里程计，定位基站上集成充电座和第二无线测距传感器；移动机器人内部还包括：距离计算单元，用于在移动机器人沿着第一预设坐标轴方向行走参考直线距离的过程中，通过第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走参考直线距离之前与定位基站之间的距离、移动机器人行走参考直线距离之后与同一定位基站之间的距离；坐标位置计算单元，用于基于这个定位基站的位置、参考直线距离、距离计算单元传输的距离信息，计算移动机器人沿着第一预设坐标轴方向行走参考直线距离后所处的位置坐标；坐标融合单元，用于将坐标位置计算单元计算出的位置坐标信息、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标；其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于这个定位基站的位置在全局地图上建立起全局坐标系；移动机器人行走过参考直线距离是由移动机器人的里程计的实时反馈获得的。

与现有技术相比，本技术方案在常规的惯导系统或回充系统中增加一对无线测距传感器，解决惯性导航定位精度不可控问题、以及无线基站布置过多的问题；本技术方案将运算功能移植入移动机器人内，使得所需处理的通信距离数据较少且运算方法简单。

进一步地，所述移动机器人是视觉机器人或激光机器人，其内部的坐标位置计算单元用于根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标；在所述坐标位置计算单元内，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，预设参考路径在第一预设坐标轴方向上的路径长度是参考直线距离，预设参考路径平行于所述全局坐标系的同一轴线上；其中，所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，所述全局坐标系是以所述定位基站的位置为原点建立的。

与现有技术相比，本技术方案使用平行于第一预设坐标轴方向的预设参考路径的起止遍历点与所述定位基站确定的三角形，在一个全局坐标系内利用无线测距传感器测得的距离信息构建余弦定理关系式以计算出预设参考路径的终点位置的坐标作为移动机器人的实时位置坐标，相对于里程计记录的距离信息，定位运算难度降低，且克服惯性传感器在定位数据漂移误差较大的问题。

一种芯片，该芯片用于存储计算机程序代码，所述计算机程序代码被执行时实现所述基于无线测距传感器的机器人定位控制方法的步骤。使得移动机器人的定位导航精度可控，坐标位置的计算复杂度降低。

附图说明

图1是本发明实施例公开的基于参考直线距离的机器人定位控制方法的流程图。

图2是一实施例公开移动机器人沿着X轴方向移动的示意图。

图3是另一实施例公开移动机器人沿着Y轴方向移动的示意图。

图4是本发明实施例公开的一种移动机器人的定位控制系统框架示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行详细描述。

现有技术中的惯性导航扫地机器人，常使里程计中的码盘对驱动轮进行计数，若驱动轮打滑、地面介质存在颠簸，则随着时间的积累，码盘计数值相对于机器人实际行走过的距离出现较大误差，导致计算出的机器人的位姿等出现了偏差，直接表现在地图上是机器人实时构建的地图变倾斜，无法与原图匹配；即使使用激光雷达或视觉摄像头，在实时采样扫描定位中也会由于车轮自传、打滑等原因，扫描所定结果位置会出现较大偏差，严重时会使机器人无法重定位自身位置，导致机器人停止不动。为了克服这一缺陷，中国专利CN111381586A通过设置至少两个UWB基站来计算机器人相对于所述UWB基站之间的距离，结合至少两个UWB基站的位置计算得到机器人坐标位置，但是需要在有限的室内环境下至少布置两个基站，这对无线通信的收发条件提高要求，增加通信难度，特别是避开更多位置处的障碍物对无线通信信号的影响；同时，多个基站的使用也意味着参数的使用量增加，计算过程复杂，加重移动机器人的运行负载。

因此，本发明实施例在惯性导航（里程计记录的移动机器人的行走距离信息、陀螺仪所检测记录的移动机器人发生的转角）的基础上，只增加一个用于无线测距的基站，从而移动机器人上安装的测距传感器只匹配使用一个用于无线测距的基站，以辅助机器人沿着坐标轴方向行走定位，弥补惯性导航定位精度不高的问题，减少数据运算处理的难度，简化利用距离信息计算移动机器人的实时位置坐标的方法。具体实施算法是如图1所示的基于参考直线距离的机器人定位控制方法，包括：

步骤S101、在移动机器人沿着第一预设坐标轴方向行走参考直线距离的过程中，通过移动机器人上设置的第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走遍历参考直线距离之前与定位基站之间的距离、移动机器人行走遍历参考直线距离之后与同一定位基站之间的距离；其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于这个定位基站的位置在全局地图上建立起全局坐标系；然后进入步骤S102。其中，所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，所述定位基站的位置是预先设定的，一般将所述定位基站设置在平行墙位置、长廊区域等比较空旷的区域；本实施例中，所述全局坐标系是以所述定位基站的位置为原点建立的。

在执行步骤S101的过程中，移动机器人运动于定位基站的有效探测范围内，且第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器可以设置为主从通信设备，它们之间通信使用的脉冲信号包括但不限于超声波、红外调制光信号、激光调制信号、UWB发射的超宽带信号，通过它们的收发时间来计算出移动机器人的当前位置与所述定位基站的距离。同时，所述定位基站配置有识别信息的功能，使得移动机器人在进入所述定位基站的识别区域时，通过传感器（包括前述的第一无线测距传感器）获取识别信息来定位所述定位基站的位置，当定位基站作为充电座使用时，这个定位基站的位置配置为移动机器人的回充位置。所述定位基站配置的所能识别信息可以包含多种识别信息，具体的类型可以根据机器人上安装的单线测距传感器类型确定，比如，若移动机器人上安装有激光雷达，则定位基站可以识别雷达识别码；若移动机器人上安装有UWB标签，则定位基站可以识别UWB超宽带信号。

优选地，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，其中，预设参考路径在第一预设坐标轴方向上的投影长度是参考直线距离；所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，其中，所述全局坐标系是以所述定位基站的位置为原点建立的。其中，预设参考直线距离是预先设定，是与里程计的记录数据积累漂移误差的程度相关，可能存在：移动机器人行走过预设参考直线距离后，里程计记录的距离数据相对于实际遍历的路程距离出现明显的漂移误差，足以让移动机器人的位姿计算出现误差。

在执行步骤S101的过程中，每当移动机器人沿着第一预设坐标轴方向行走参考直线距离，则控制移动机器人上设置的第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器保持通信测距，分别计算获得移动机器人遍历当前这一段参考直线距离前后两个位置与定位基站之间的距离，并结合里程计测得的行走距离和陀螺仪测得的机体转动角度融合计算出移动机器人的实时位置坐标（也是全局地图上的坐标位置）；然后再沿着第一预设坐标轴方向继续行走下一段参考直线距离以通过同一个定位基站内设置的第二无线测距传感器通信测距获取新的位置相对于同一个定位基站的距离。如此迭代，通过遍历过的路径的起点和终点相对于定位基站的位置的距离信息来计算移动机器人的当前位置坐标。

步骤S102、基于这个定位基站的位置、参考直线距离、移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离前后与定位基站之间的距离，计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标。

需要说明的是，在常规的惯性导航方法中，使用里程计测量的移动机器人行走距离结合陀螺仪测量的移动机器人的转动角度就可以计算出移动机器人在全局地图中的坐标位置，但是随着时间的积累，码盘计数值相对于机器人实际行走过的距离出现较大误差，导致计算出的机器人的位姿等出现了偏差，所以不单纯使用里程计和陀螺仪的数据进行定位计算，转而通过执行步骤S101采样移动的机体与定位基站的距离信息参与步骤S102的定位计算。

在步骤S102中，设置在移动机器人的全局坐标系内，这个定位基站的位置视为原点坐标，本实施例为了简化坐标计算，将参考直线距离设置为一段平行第一预设坐标轴（X轴或Y轴）的线段，再结合这个线段的两个端点与原点的连线距离，计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标；具体是根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标，构建方式包括：预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、参考直线距离所在的与第一预设坐标轴平行的直线、第二预设坐标轴可以构建一个三角几何关系式；预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、参考直线距离所在的与第一预设坐标轴平行的直线、第二预设坐标轴可以构建另一个三角几何关系式；然后结合这两个三角几何关系式，等效变换为余弦定理关系，以实现通过移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离前后与定位基站之间的距离，计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标。

值得注意的是，在本实施例中，为了省去矩阵换算步骤、降低坐标运算难度，将前述的预设参考路径设置为平行于所述第一预设坐标轴方向，使得在本实施例中，移动机器人实际遍历过的预设参考路径的起点和终点的连线设置为平行于第一预设坐标轴方向，且起点和终点的连线长度配置为所述参考直线距离。

步骤S102基于预设参考路径的起点位置、预设参考路径的终点位置与所述定位基站三个坐标位置，在一个全局坐标系内利用无线测距传感器测得的距离信息构建余弦定理关系式以计算出预设参考路径的终点位置的坐标作为移动机器人的实时位置坐标，使得定位精度可控，克服惯性传感器在全局坐标系内定位误差较大的问题，等效于校正惯性数据推算的定位坐标。

其中，移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离前后与定位基站之间的距离是移动机器人移动这个参考直线距离前后与定位基站之间的水平距离，因为第一无线测距传感器接收到的来自第二无线测距传感器的测距信息可能会受到定位基站的高度的约束，所以需要利用直角三角形的三角几何将测距信息换算为移动机器人与定位基站的位置之间的距离，这个定位基站的位置是所述定位基站在移动机器人的行走平面上的投影。值得注意的是，移动机器人与定位基站的距离超过合法的探测距离后将无法正常通信，移动机器人不在定位基站的合法探测视角范围内也无法正常通信，导致无法计算出位置坐标。

在本实施例中，所述移动机器人沿着第一预设坐标轴方向是否行走过参考直线距离的这一导航行为，可以由移动机器人的驱动轮内安装的里程计实时记录的距离信息反馈获得的。参考直线距离是预先设定的大于0的距离，优选为移动机器人的一个机身直径长度、或预设倍数的机身直径长度，以表现出移动机器人在发生明显移动的状态下再通过执行步骤S102计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标，其中，预设倍数的机身直径长度对应的路径不能超出所述定位基站的有效探测范围。

步骤S103、将前述步骤S102计算出的位置坐标（基于行走过程中的移动机器人与定位基站的距离信息和前述的三角几何关系计算出的坐标信息）、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标。之所以执行该步骤，原因在于：所述第一无线测距传感器与所述第二无线测距传感器在通信过程中携带一定的噪声，导致测距数据产生漂移，所以实际应用时，会同时进行里程计加陀螺仪的融合计算。

具体融合计算方法包括：将三角几何关系式计算出的预设参考路径的终点位置的坐标、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合，实现对三角几何关系式计算出的预设参考路径的终点位置的坐标进行滤波；具体的融合方法是：根据移动机器人的里程计测得的距离信息与移动机器人的陀螺仪测得的角度信息，利用三角几何关系计算出移动机器人的惯性坐标，再将移动机器人的惯性坐标与三角几何关系式计算出的预设参考路径的终点位置的坐标输入滤波模型中，参与所述预设参考路径的终点位置的坐标的滤波运算，能够根据这两种坐标的差值波动情况调整所述预设参考路径的终点位置的坐标，以滤除所述第一无线测距传感器与所述第二无线测距传感器的通信测距中出现的噪声。

需要说明的是，由于存在里程计的累计误差不能直接用作移动机器人在惯性导航过程中的实时位置，但可以作为参考估计值去计算估计误差，用于融合修正坐标计算值以完成滤波运算。即在本实施例中，通过惯性传感器确定的坐标进一步修正无线测距传感器的测距信息所确定的坐标，从而得到更高精度的移动机器人在全局地图中的实时坐标位置。滤波模型包括卡尔曼滤波模型、低通滤波模型、粒子滤波模型等其它导航数据滤波模型。

与现有技术相比，前述步骤通过控制移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离的方式来获取移动机器人在遍历前后与一个固定的定位基站之间的距离，而不是计算获取同一位置的机器人与不同的基站之间的距离，不需同时收发处理两个基站的通信指令，减少数据运算处理的难度，简化利用距离信息计算移动机器人的实时位置坐标的方法，定位精度的可控性增强，克服里程计实时反馈的移动机器人的行走距离所带来的漂移误差的影响。同时，通过融合处理步骤S102计算的坐标信息和惯性传感器采集的位姿数据来过滤步骤S102计算的坐标信息可能携带的噪声信息，提高机器人的定位和移动精度。

优选地，当第一无线测距传感器是UWB标签时，第二无线测距传感器是UWB基站，UWB是超宽频测距传感器。相较于GPS、Zigbee等无线定位方式，精度更高，成本更低；相对于超声波传感器，信号探测角度更大。在一些实施场景中，T1时间从定位基站内的UWB基站（从设备）向移动机器人上的UWB标签（主设备）会发起测距请求脉冲，T2时刻测距请求脉冲到达移动机器人上的UWB标签完成一次测距，脉冲在UWB基站与UWB标签之间的飞行时间就是T2减去T1的所得的结果，已知脉冲运动速度近似为光速C，从而得到移动机器人当前位置与所述定位基站的位置之间的距离D=C*(T2-T1)。因此，移动机器人在移动过程中，可以不断的获取到机身内部陀螺仪计算出来的角度，还有里程计反馈的行走距离信息；同时移动机器人持续跟所述定位基站进行通信，用于计算两者之间的距离信息。因此，移动机器人行走过程中，可以获得传感器的信息包括：转动角度、行走路程、机体与所述定位基站的位置之间的距离。

在前述实施例中，当第一预设坐标轴为X轴时，第二预设坐标轴为Y轴，其中，第一预设坐标轴方向包括X轴正方向或X轴负方向，第二预设坐标轴方向包括Y轴正方向或Y轴负方向；当第一预设坐标轴为Y轴时，第二预设坐标轴为X轴，其中，第一预设坐标轴方向包括Y轴正方向或Y轴负方向，第二预设坐标轴方向包括X轴正方向或X轴负方向。拓展前述实施例的定位控制方法的运用场景，且降低坐标的计算复杂度。

作为一种实施例，如图2所示，通过以所述定位基站的位置为原点O建立全局坐标系，当移动机器人沿着X轴的负方向行走，并使用原点处的一个定位基站进行定位时，位置B（x-L，y）是移动机器人行走遍历过的一个位置，位置A（x，y）是移动机器人从位置B开始沿着X轴的负方向行走直线距离L所到达的位置，首先，位置B处的移动机器人通过与定位基站的位置O进行无线测距传感器的通信以获取位置B与定位基站的位置O之间的距离为D1，然后，沿着X轴负方向移动，当里程计反馈结果表示移动机器人已经沿着X轴负方向移动参考直线距离L时，移动机器人当前移动至位置A处，此时，移动机器人通过与定位基站进行无线测距传感器的通信以获取位置A与定位基站的位置O之间的距离为D2，其中，参考直线距离L对应的移动机器人的行走路径是所述预设参考路径，所述预设参考路径的起点位置B与所述预设参考路径的终点位置A的连线是平行于X轴方向。为了获取位置A的坐标（机器人的当前位置坐标），本实施例在不结合里程计的距离信息和陀螺仪的角度信息的三角运算结果的基础上，在全局坐标系上构建直角三角形的三角几何进行坐标运算；具体包括：

所述预设参考路径的起点位置B与所述定位基站的位置O之间的距离对应的线段BO、所述投影距离L所对应的与第一预设坐标轴X轴平行的直线、以及第二预设坐标轴Y可以构建一个三角几何关系式：

；

同时，所述预设参考路径的终点位置A与所述定位基站的位置O之间的距离对应的线段AO、所述参考直线距离L所对应的与第一预设坐标轴X轴平行的直线、以及第二预设坐标轴Y轴构建另一个三角几何关系式:

；

然后结合这两个三角几何关系式，等效变换为余弦定理关系式，计算出移动机器人遍历一次所述参考直线距离后，当前所处的位置坐标,即位置A的坐标为：

；这个关系式是基于三角形OAB进行余弦定理变换获得的；

;

显然比中国专利CN111381586A计算机器人的当前位置坐标的运算公式简单，且运算参数的数量也减小，且不用对移动机器人的部分遍历路径段进行矩阵变换，以投影到坐标轴方向上。

作为一种实施例，所述定位基站还集成充电座；当控制所述移动机器人沿着第二预设坐标轴方向离开充电座，再控制移动机器人转动以使其行走方向与第一预设坐标轴方向平行；其中，第一预设坐标轴与第二预设坐标轴垂直，如图2所示，第二预设坐标轴方向是Y轴的正方向，第一预设坐标轴方向是X轴负方向。在图2的实施例中，移动机器人返回所述定位基站的位置时，可以通过红外对准的方式准确对接充电座以实现回充。移动机器人在充电座上时，可以重置自身测得的角度，使得移动机器人退座并朝外前进时，保持固定的位姿，在本实施例中一般设置退座后朝外的前进角度为90度，具体是相对于图2的全局坐标系的X轴正方向偏转90度，即沿着Y轴的正方向，移动机器人的运动行为是可以是偏离全局坐标系的X轴正方向0度或者90度方向进行直线移动，从而让充电结束的机器人顺利退座，便于进入定位导航模式。然后在需要执行定位算法的时刻就开始逆时针转动90度进入开始定位计算距离和坐标信息。由于移动机器人是沿着X轴负方向行走一段参考直线距离L去计算当前位置坐标，所以简化移动机器人实际位置的计算。需要说明的是，所述定位基站的位置可以作为移动机器人执行定位算法的预先设定的初始位置，也可以作为移动机器人的回充位置。

优选地，所述充电座携带的红外对准信息包括识别码、红外引导信号的频段信息、红外线窄角或红外线近卫信号中的至少一个。所述充电座上还携带有识别信息，以供使移动机器人在进入识别区域时，通过传感器（包括前述的第一无线测距传感器）获取识别信息来定位充电座。

作为另一种实施例，如图3所示，通过以所述定位基站的位置为原点O建立全局坐标系，当移动机器人沿着Y轴的正方向行走，并使用原点O处的一个定位基站进行定位时，位置D（x1，y1-L1）是移动机器人行走遍历过的一个位置，位置C（x1，y1）是移动机器人从位置D开始沿着Y轴的正方向行走直线距离L1所到达的一个目标位置，首先，位置D处的移动机器人通过与定位基站的位置O进行无线测距传感器的通信以获取位置D与定位基站的位置O之间的距离为D3，然后，沿着Y轴正方向移动，当里程计反馈结果表示移动机器人已经沿着Y轴正方向移动参考直线距离L1时，移动机器人当前移动至位置C处，此时移动机器人通过与定位基站进行无线测距传感器的通信，以获取位置C与定位基站的位置O之间的距离为D4，其中，参考直线距离L1对应的移动机器人的行走路径是所述预设参考路径，所述预先参考路径的起点位置D与所述预先参考路径的终点位置C的连线是平行于Y轴方向。本实施例根据起点位置D与所述定位基站的位置O之间的距离对应的线段DO、终点位置C与所述定位基站的位置O之间的距离对应的线段CO、所述参考直线距离L1对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算位置C，具体的构建方式和计算公式可参照图2所述的实施例，本实施例的计算公式相对于图2的实施例只是X轴坐标与Y轴坐标的运算位置对调且调整相应的正负号，在此不再赘述。

作为一种异常处理实施例，机器人很容易被卡住，具体是：在移动机器人上设置的第一无线测距传感器与所述定位基站内设置的第二无线测距传感器进行通信测距过程中，若计算获得移动机器人在行走过程中（比如行走遍历所述参考直线距离的过程中）与同一个定位基站之间的距离保持不变，则判定移动机器人被卡住，没有发生位移；然后控制移动机器人的里程计停止计数，使得里程计的距离信息不会进行累计，并且不进行前述实施例的距离计算和坐标位置运算。从而在移动机器人卡住状态下减小坐标数据的处理量。

应理解的是，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

图4为本发明公开的一种移动机器人的定位控制系统的框架示意图，具体实施方式如下：

所述移动机器人的定位控制系统，包括一个移动机器人和一个定位基站，移动机器人上设置第一无线测距传感器和里程计，定位基站上集成充电座和第二无线测距传感器，以使得定位基站兼备充电和无线通信功能。

移动机器人内部还包括：距离计算单元，用于在移动机器人沿着第一预设坐标轴方向行走所述参考直线距离的过程中，通过第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走参考直线距离之前与定位基站之间的距离、移动机器人行走参考直线距离之后与同一定位基站之间的距离；其中，第一无线测距传感器将接收到来自第二无线测距传感器的脉冲信号，通过解析送入距离计算单元进行距离计算。其中，移动机器人从里程计反馈的数据中获取到其实际行走过参考直线距离，并将参考直线距离传输给所述距离计算单元。

坐标位置计算单元，用于基于这个定位基站的位置、参考直线距离、距离计算单元传输的距离信息，计算移动机器人沿着第一预设坐标轴方向行走参考直线距离后所处的位置坐标，其中，移动机器人沿着第一预设坐标轴方向行走参考直线距离所遍历的位置都是在所述定位基站的有效探测范围内；其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于预先设定的定位基站的位置在全局地图上建立起全局坐标系，同时，移动机器人的里程计实时反馈移动机器人的实际行走距离，并在里程计每次检测记录到移动机器人已经沿着第一预设坐标轴方向行走参考直线距离时，控制距离计算单元和坐标位置计算单元运行坐标计算。

坐标融合单元，用于将坐标位置计算单元计算出的位置坐标信息、移动机器人的里程计实时测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标；所述坐标融合单元：将坐标位置计算单元基于三角几何关系式计算出的预设参考路径的终点位置的坐标、以及移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合，实现对三角几何公式计算出的预设参考路径的终点位置的坐标进行滤波，获得融合处理后的移动机器人的位置坐标，作为实时位置坐标，用于导航定位，提高定位的精度。

与现有技术相比，所述定位控制系统在常规的惯导系统或回充系统中增加一对无线测距传感器，解决惯性导航定位精度不可控问题、以及无线基站布置过多的问题；所述定位控制系统将运算功能移植入移动机器人内，使得所需处理的通信距离数据较少且运算方法简单。提高定位基站和移动机器人的定位精度。

需要说明的是，本实施例使用的无线测距传感器包括但不限于UWB（超宽频测距传感器）、超声波传感器、激光测距传感器、红外测距传感器等单线测距传感器。

优选地，所述移动机器人是视觉机器人或激光机器人，其内部设置的坐标位置计算单元，用于根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标；在所述坐标位置计算单元内，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，预设参考路径在第一预设坐标轴方向上的路径长度是参考直线距离，预设参考路径平行于所述全局坐标系的同一轴线上；其中，所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，所述全局坐标系是以所述定位基站的位置为原点建立的。

与现有技术相比，坐标位置计算单元，基于前述的距离计算单元运算使用到的平行于第一预设坐标轴方向的预设参考路径的起止遍历点、与所述定位基站确定的三角形，在一个全局坐标系内利用无线测距传感器测得的距离信息构建余弦定理关系式以计算出预设参考路径的终点位置的坐标作为移动机器人的实时位置坐标，相对于里程计记录的距离信息，定位运算难度降低，且克服惯性传感器在定位数据漂移误差较大的问题，再交由坐标融合单元过滤部分噪声数据的干扰。

需要说明的是，在机器人移动过程中，还可以通过移动机器人上安装的深度相机、雷达或超声传感器中的一种或者多种，实时采集场景中的障碍物信息，并根据采集的障碍物信息，在朝目标位置前进时，自动避开障碍物，提高移动机器人在特定场景中通信测距同一定位基站的方式更加灵活。

图4所述移动机器人的定位控制系统，与图1所述的基于参考直线距离的机器人定位控制方法执行的各个步骤功能相对应。

本发明还公开一种芯片，该芯片用于存储计算机程序代码，并可以设置在前述的移动机器人内，所述计算机程序代码被执行时实现前述基于参考直线距离的机器人定位控制方法的步骤。或者，所述芯片执行所述计算机程序代码时实现上述定位控制系统实施例中各个单元的功能。示例性的，所述计算机程序代码可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述芯片中，并由所述芯片执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序代码在所述移动机器人中的执行过程。例如，所述计算机程序代码可以被分割成:前述定位控制系统实施例内的距离计算单元、坐标位置计算单元和坐标融合单元。使得移动机器人的定位导航精度可控，坐标位置的计算复杂度降低。

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.基于参考直线距离的机器人定位控制方法，其特征在于，包括：

步骤1、在移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离的过程中，通过移动机器人上设置的第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走遍历参考直线距离之前与定位基站之间的距离、移动机器人行走遍历参考直线距离之后与同一定位基站之间的距离；其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于这个定位基站的位置在全局地图上建立起全局坐标系；移动机器人是否行走过参考直线距离是由移动机器人的里程计的实时反馈的；参考直线距离是预先设定的大于0的距离，但不超出所述定位基站的有效探测范围；

步骤2、基于这个定位基站的位置、参考直线距离、移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离前后与定位基站之间的距离，计算移动机器人沿着第一预设坐标轴方向行走遍历参考直线距离后所处的位置坐标；

步骤3、将步骤2计算出的位置坐标、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标。

2.根据权利要求1所述机器人定位控制方法，其特征在于，所述步骤2的方法步骤具体包括：

首先，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，其中，预设参考路径的路径长度是参考直线距离；所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，其中，所述全局坐标系是以所述定位基站的位置为原点建立的；

根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标；

其中，预设参考路径是平行于所述第一预设坐标轴方向。

3.根据权利要求2所述机器人定位控制方法，其特征在于，所述步骤3包括：

将三角几何关系式计算出的预设参考路径的终点位置的坐标、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合，以滤除所述第一无线测距传感器与所述第二无线测距传感器的通信测距中出现的噪声，实现对三角几何关系式计算出的预设参考路径的终点位置的坐标进行滤波；

其中，融合方式为：根据移动机器人的里程计测得的距离信息和移动机器人的陀螺仪测得的角度信息，利用三角几何关系计算出移动机器人在惯性导航过程中的惯性坐标，用于参与所述预设参考路径的终点位置的坐标的滤波运算。

4.根据权利要求3所述机器人定位控制方法，其特征在于，当第一无线测距传感器是UWB标签时，第二无线测距传感器是UWB基站。

5.根据权利要求3所述机器人定位控制方法，其特征在于，在移动机器人上设置的第一无线测距传感器与所述定位基站内设置的第二无线测距传感器进行通信测距过程中，若计算获得移动机器人在行走过程中与同一个定位基站之间的距离保持不变，判定移动机器人被卡住，然后控制移动机器人的里程计停止记录行走距离数据。

6.根据权利要求5所述机器人定位控制方法，其特征在于，所述定位基站还集成充电座；

当控制所述移动机器人沿着第二预设坐标轴方向离开充电座后，控制移动机器人转动以使得移动机器人的行走方向与第一预设坐标轴方向平行；

其中，第一预设坐标轴与第二预设坐标轴垂直；

其中，移动机器人的行走方向仅限于平行于坐标轴。

7.根据权利要求1至6任一项所述机器人定位控制方法，其特征在于，当第一预设坐标轴为X轴时，第二预设坐标轴为Y轴，其中，第一预设坐标轴方向包括X轴正方向或X轴负方向，第二预设坐标轴方向包括Y轴正方向或Y轴负方向；

当第一预设坐标轴为Y轴时，第二预设坐标轴为X轴，其中，第一预设坐标轴方向包括Y轴正方向或Y轴负方向，第二预设坐标轴方向包括X轴正方向或X轴负方向；

其中，全局坐标系包括第一预设坐标轴和第二预设坐标轴。

8.一种移动机器人的定位控制系统，其特征在于，所述定位控制系统包括一个移动机器人和一个定位基站，移动机器人上设置第一无线测距传感器和里程计，定位基站上集成充电座和第二无线测距传感器；

移动机器人内部还包括：

距离计算单元，用于在移动机器人沿着第一预设坐标轴方向行走参考直线距离的过程中，通过第一无线测距传感器与同一个定位基站内设置的第二无线测距传感器的通信测距，分别计算获得移动机器人行走参考直线距离之前与定位基站之间的距离、移动机器人行走参考直线距离之后与同一定位基站之间的距离；

坐标位置计算单元，用于基于这个定位基站的位置、参考直线距离、距离计算单元传输的距离信息，计算移动机器人沿着第一预设坐标轴方向行走参考直线距离后所处的位置坐标；

坐标融合单元，用于将坐标位置计算单元计算出的位置坐标信息、移动机器人的里程计测得的距离信息、移动机器人的陀螺仪测得的角度信息融合处理，得到移动机器人的实时位置坐标；

其中，移动机器人在行走过程中，移动机器人内部即时构建全局地图，并基于这个定位基站的位置在全局地图上建立起全局坐标系；移动机器人行走过参考直线距离是由移动机器人的里程计的实时反馈获得的。

9.根据权利要求8所述定位控制系统，其特征在于，所述移动机器人是视觉机器人或激光机器人，其内部的坐标位置计算单元用于根据预设参考路径的起点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的终点位置与所述定位基站的位置之间的距离对应的线段、预设参考路径的起点位置与预设参考路径的终点位置之间的距离对应的线段在全局坐标系中的分布位置，构建三角几何关系式以计算出预设参考路径的终点位置的坐标；

在所述坐标位置计算单元内，移动机器人沿着第一预设坐标轴方向行走参考直线距离表示为移动机器人沿着预设参考路径行走，预设参考路径在第一预设坐标轴方向上的路径长度是参考直线距离，预设参考路径平行于所述全局坐标系的同一轴线上；

其中，所述定位基站在移动机器人的行走平面上的投影是所述定位基站的位置，所述全局坐标系是以所述定位基站的位置为原点建立的。

10.一种芯片，该芯片用于存储计算机程序代码，其特征在于，所述计算机程序代码被执行时实现权利要求1至7任一项所述基于参考直线距离的机器人定位控制方法的步骤。

Images