CN105115498B - 一种机器人定位导航系统及其导航方法 - Google Patents

Abstract

一种机器人定位导航系统及其导航方法，所述机器人定位导航系统包括多个信标模块、至少一个机器人定位终端和电子导航平台；信标模块包括第一无线收发模块、超声波发射模块和第一智能处理模块；机器人定位终端包括第二无线收发模块、超声波接收阵列、阵列处理模块和中央处理器；电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的运动目标或路径以及信标模块的位置；调度机器人，规避障碍物；还用于协调信标模块和机器人定位终端的同步，接收从各个机器人定位终端上传的位置坐标数据。本发明还包括机器人定位导航系统的导航方法。本发明解决了多径效应和非视距传播问题，提高测量精度，实现自动定位。

Description

一种机器人定位导航系统及其导航方法

技术领域

本发明涉及定位导航技术领域，特别是一种机器人定位导航系统及其导航方法。

背景技术

移动机器人导航是实现在复杂环境下机器人系统依靠传感器技术和人工智能技术，从出发点运动到目的点的系统和方法。机器人是一种可以帮助人类完成服务性工作的半自主或全自主工作的机器人。根据数据资料显示，全球范围内的机器人有着广泛的市场。近年来，机器人市场发展保持了较快的增长速度。随着劳动力价格日趋上涨，从事保安、看护、清洁工作的简单劳动力越来越少，但全球人口老龄化的加剧，看护等工作的人员需求却越来越多。为了弥补这种供需的巨大差异，机器人市场由此产生。

路径规划技术是机器人研究领域的一个重要分支，是依据某个或某些优化准则(如工作代价最小、行走路线最短、行走时间最短等)，在其工作空间中找到一条从起始地点到目标地点的能避开障碍物的最优路径。

室内是人类活动最密集，与人类生活生产最相关的场合。室内定位可视为定位技术与人联系最紧密的一个组成部分，其存在的重要潜在应用，近年来吸引了大量的研究和关注。室内由于有大量墙体、屏风等障碍存在，多径效应严重，场强分布不均甚至有盲区存在。人员的频繁移动、电子设备和门窗的开关等也给信号传输带来大量随机，不可测的干扰和波动。因此针对复杂室内环境的定位技术一直是研究的难点，也是学术界一直关注的热点问题。

室内定位技术大多采用短距无线定位技术，包含WLAN、RFID、蓝牙、UWB等。但总体而言它们都存在精度差的问题而不能用于机器人定位。当前采用这些技术的定位系统，最高标称能达到10cm，该精度基本上也是使用无线定位技术所能达到的极限精度。现有的超声波定位技术主要存在以下缺陷：

(1)超声波传输过程衰减明显；

(2)超声波受多径效应和非视距传播影响。这些缺点导致现在的这些技术基本只能用于无人且环境较空的场所。

另外，现有的机器人定位导航也存在以下不足：

(1)机器人偏离运动轨迹后就会迷路，不能正常工作；

(2)当机器人在运动过程中遇到障碍物时，不能自动规划绕道行驶。

发明内容

本发明的目的是克服现有技术的上述不足而提供一种定位精度高，可靠性好，灵活性高的机器人定位导航系统及其导航方法。

本发明的技术方案是：

本发明之一种机器人定位导航系统，包括多个固定位置的信标模块、至少一个机器人定位终端和电子导航平台；

所述信标模块包括：

第一无线收发模块，用于以无线的方式与机器人定位终端和/或电子导航平台联络，收发指令和同步信号；

超声波发射模块，用于发出已编址的超声波信号；

第一智能处理模块，用于处理接收的指令和储存超声波编码，处理同步时序安排；

所述机器人定位终端包括：

第二无线收发模块，用于负责与信标模块和电子导航平台联络，收发无线同步信号，以及接收对待接收的超声波信号做出的时序安排；

超声波接收阵列，用于接收来自不同角度的超声波信号；

第二智能处理模块，包括阵列处理模块和中央处理器；

所述阵列处理模块用于将接收的超声波信号放大，根据其信号的强弱自动进行增益控制，使输出的超声波模拟信号强度保持相对稳定，并将接收到的模拟信号转换成数字信号；

所述中央处理器用于处理接收到的指令、模块各部分的逻辑关系、做出同步时序安排、识别编码、储存和分析接收到的超声波数据信息，并将超声波数据会同电子导航平台发送来的电子地图一起放入计算单元中进行计算处理，得到位置坐标数据；

所述电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的运动目标或路径以及信标模块的位置，使得机器人在允许的运动范围内都能够获得信标模块的交叉覆盖；并将电子地图以无线方式传给机器人定位终端进行机器人的调度；还用于协调信标模块和机器人定位终端的同步，接收从各个机器人定位终端上传的位置坐标数据并显示。

进一步，所述超声波接收阵列的上部搭载有障碍物扫描仪；所述障碍物扫描仪包括同步旋转系统和控制系统；

所述同步旋转系统包括设于超声波接收阵列上的支架，支架上安装伺服电机，伺服电机通过旋转搭载头连接复合传感器座，复合传感器座上安装有人体感应传感器和超声波收发传感器，伺服电机的下端连接编码器；

所述控制系统包括电机驱动模块和障碍物扫描主控模块，所述障碍物扫描主控模块的输出端连接电机驱动模块，障碍物扫描主控模块的输入端分别连接编码器、人体感应传感器和超声波收发传感器，电机驱动模块的输出端连接伺服电机；所述障碍物扫描主控模块通过CAN总线分别连接电子导航平台和机器人定位终端。

进一步，所述超声波收发传感器通过超声波信号放大器与所述障碍物扫描主控模块连接，所述超声波信号放大器用于超声波收发传感器接收和发送的超声波信号进行放大。

进一步，所述超声波收发传感器的前端安装有用于收窄超声波接收角度的吸音管。

进一步，所述阵列处理模块包括前置放大模块和A/D转换模块；

所述前置放大模块用于将接收的超声波信号放大，根据其信号的强弱自动进行增益控制，使输出的超声波模拟信号强度保持相对稳定；

所述A/D转换模块用于将检测时间段内的超声波模拟信号连续转换成数字

信号并存储；通过中央处理器扫描采集到的超声波信号幅度，处理该组超声

波数据；根据接收到的超声波脉冲群的平均幅值，来计算用于判断的阈值；达到判断阈值的点则认为当前测试的超声波信号到达起始点，否则认为是噪声信号。

进一步，所述电子导航平台包括：

搭载导航软件的计算机，用于存储全部机器人的编码，构建电子地图，根据需要调度机器人，控制多台机器人同时完成各自的工作；

第三无线收发模块，用于传送电子地图给机器人定位终端，并发送定位同步信号，协调信标模块和机器人定位终端的时序，接收各个机器人定位终端上传的位置坐标数据以及障碍物扫描仪上传的通知信息；

显示器，用于显示电子地图上的导航信息；

电子地图运算模块，用于规划并计算机器人的运动目标或路径以及信标模块的位置，生成导航信息。

进一步，所述电子地图包括与机器人运动区域和信标模块覆盖区域分别相对应的电子平面地图和三维电子地图。

进一步，所述超声波收发阵列为具有至少四个超声波感应元的集成阵列，或由至少四个独立的超声波传感器分散而成的阵列；所述前置放大模块和A/D转换模块的通道数量与超声波感应元的数量相对应。

本发明之一种根据上述机器人定位导航系统的导航方法，包括以下步骤：

(1)由机器人定位终端依据移动体行走的起始坐标数据，结合定位后沿预设的运动轨迹可能达到的坐标，计算规划出每个可能达到的坐标处所需的至少两个信标模块，依次序分别呼唤需要的信标模块，建立信标模块和机器人定位终端的同步测试关系；

(2)收到同步信号的机器人定位终端启动数据采集时间窗口T，接收信标模块按时序发来的已编码的超声波信号，分别标记接收到的时间为T₁,T₂,…,T_i，其中，i与所呼唤信标模块的数量相同；处理计算每个时间段接收到的超声波信号数据段，识别其编码，若接收到多组相同编码的超声波信号，只取最先到达的一组超声波编码信号，剔除其余杂波，再经计算找到每个时间段最先到达的此组超声波编码信号的起头时间T₁’,T₂’,…,T_i’；

(3)计算得到超声波接收阵列与每个信标模块之间的距离z_i为z_i＝T_i’×c，其中，c为超声波信号在常温下的传播速度，并修正超声波信号的传播速度c；

(4)计算得出超声波接收阵列理论中心与每个信标模块的距离L_i以及信标模块与超声波接收阵列理论中心的右旋角γ_i；

(5)计算机器人所在位置的二维坐标数据或三维坐标数据，生成机器人的导航信息；

(6)规避运动过程中的障碍物，选择自动规避或人工介入。

进一步，步骤(3)中，所述超声波信号的传播速度c的修正方法为：

A.选取所需的两个信标模块，根据它们的固定坐标计算出它们之间的绝对距离；

B.在超声波有效覆盖范围内的任意一个地方，在固定的默认声速条件下，计算出超声波接收阵列理论中心与两个信标模块的相对距离和角度，进而计算出两个信标模块之间的相对距离；

C.将两个信标模块之间的绝对距离与相对距离的误差百分率作为补偿，计算出实际声速。

本发明与现有技术相比具有如下特点：

(1)将接收到的超声波信号根据其信号的强弱进行自动增益控制，输出稳定的动态范围，并转换成数字信号，计算出判断阈值，根据判断阈值判断当前信号为超声波信号还是噪声信号，从而实现超声波检测，解决了超声波信号在传输过程中的衰减问题；

(2)利用超声波阵列中超声波感应元之间的相位差和信号强度，分辨不同途径的超声波，只取直线到达的超声波，这样可解决多径效应和非视距传播问题，滤除环境声波的干扰，缩短测试时间，大大提高测距精度和可靠性；

(3)使用编码超声波方式可便于确定接收的超声波是否是需要的信标模块发送的，同时通过与发射源的理论编码比对，判断该组超声波是不是需要的信标模块发送的那束超声波，可精确确定超声波的起始点和发射角度，提高测距的准确度；

(4)利用超声波接收阵列接收超声波信号，经数据处理，根据超声波接收阵列与信标模块之间的超声波信号的相位关系来判断机器人的位置和方向，并会同电子地图发送至电子定位平台，实现机器人的路径规划和定位；

(5)利用采集的2个以上信标模块的超声波信号，来计算信标模块之间的绝对距离与相对距离之间误差百分率作为补偿，进而计算出在当前环境条件下精确的声速，精确率高，成本低，计算简单；

(6)通过扫描和数据处理，能够计算出障碍物是否存在以及障碍物的大小。机器人能够更好地选择使用障碍物处理机制，既可根据障碍物大小选择是停止等待还是绕行，又可针对障碍物大小的不同，做出相应的处置，当遇到的障碍物较大，将会对测量结果产生影响时，能够及时地选择其它信标对机器人进行定位，不会出现由于超声波路径障碍问题产生的定位误差，当遇到的障碍物较小，不会对影响到测量精度时，可使用原定位规划中的信标模块进行定位，使机器人定位系统更加智能化；

(7)通过人体感应传感器对障碍物的感应以及后续的数据处理，判定障碍物是否为人。当障碍物为人时，提供信息给机器人，使机器人采用原地待命或是打招呼请其离开的方式进行规避；当障碍物非人时，提供信息给机器人，使机器人采用常用的规避措施规避该障碍物；机器人能够取得障碍物是人还是物的信息，以提供机器人进行相关处置时的依据，并实现机器人与人类的互动；

(8)当机器人偏离预设的运动轨迹时，仍可根据设于机器人附近的其它运动轨迹中所规划好的信标模块实现自动定位，从而避免机器人偏离运动轨迹就迷路，不能正常工作的问题。

附图说明

图1为本发明实施例障碍物扫描仪的结构示意简图；

图2为本发明实施例的整体结构示意图；

图3为本发明实施例机器人所在三维坐标的计算简图；

图4为本发明实施例超声波接收阵列理论中心与信标模块的距离计算简图；

图5为本发明实施例超声波接收阵列理论中心与信标模块的角度计算简图；

图6为本发明实施例声速修正计算简图。

具体实施方式

以下结合附图和具体实施方式对本发明的详细结构作进一步描述。

一种机器人定位导航系统，包括多个信标模块、至少一个机器人定位终端、障碍物扫描仪和电子导航平台。机器人定位终端安装在机器人上，机器人可根据需要搭载数量不等的机器人定位终端。信标模块的位置必须是固定的，从而获得固定位置坐标，其可以安装于墙上或者其它固定物体上(放置的位置没有具体要求)，具体数量根据待定位置的空间大小决定。在机器人的运动轨迹上设置多个位置坐标，计算规划出这些位置坐标处分别所需的至少两个信标模块。

具体地，信标模块包括第一智能处理模块以及分别与第一智能处理模块连接的第一无线收发模块和超声波发射模块。

第一无线收发模块用于以无线的方式与需要定位的机器人定位终端和/或电子导航平台联络，收发指令和同步信号；第一无线收发模块可以采用符合多种无线通信协议的网络模块，例如WIFI模块、ZigBee模块、蓝牙通信模块等。优选地，第一无线收发模块采用无线串口通信模块。

超声波发射模块，用于发出已编址的超声波信号；由于多个信标模块中每个信标模块都赋予唯一编码，以便于机器人定位终端通过识别编码来判断接收的超声波是否是所呼唤的信标模块发送的。

第一智能处理模块用于处理接收的指令和储存超声波编码，并处理同步时序安排。

具体地，机器人定位终端包括第二智能处理模块以及分别与第二智能处理模块连接的第二无线收发模块和超声波接收阵列。

第二无线收发模块，用于负责与信标模块和电子导航平台联络，收发无线同步信号和接收超声波信号的时序；优选地，第二无线收发模块采用无线串口通信模块。

超声波接收阵列，用于接收来自信标模块发出的不同角度的超声波信号；超声波收发阵列为具有多个超声波感应元的集成环阵列，全部集成于一块电路板上，且位置关系固定，便于计算，简化程序。该阵列不仅可测试超声波到达时间，还可测试超声波发射源的角度。另外还可通过与发射源的理论编码的比对，可确认该超声波编码是否为需要的信标模块发送，还可精确确定超声波的起始点，以可以解决环境声波的干扰问题。

第二智能处理模块，用于处理接收的指令、模块各部分的逻辑关系、识别编码、并做出同步时序安排、储存和分析接收到的超声波数据信息，并将超声波数据会同电子导航平台发送来的电子地图一起放入计算单元进行计算处理，得到位置坐标数据。第二智能处理模块包括超声波阵列处理模块和中央处理器，超声波阵列处理模块包括前置放大模块和A/D转换模块，超声波阵列连接前置放大模块的输入端，前置放大模块的输出端连接A/D转换模块的模拟输入端，A/D转换模块的数字输出端连接中央处理器；中央处理器还与第二无线收发模块连接。前置放大模块和A/D转换模块的通道数量与超声波感应元的数量相同，即每个超声波感应元接收到的超声波信号均通过各自的前置放大模块和A/D转换模块进行处理，最终输入至中央处理器进行集中处理计算。

本实施例的前置放大模块采用了带AGC功能的放大器，AGC为自动增益控制。当超声波接收阵列接收到的信号较弱时，AGC放大器会自动增加信号放大倍数，将弱信号幅度增强。当超声波接收阵列接收到的信号较强时，AGC放大器会自动降低信号放大倍数，以实现无论信号较强还是较弱，前置放大模块的输出信号都保持相对稳定。

A/D转换模块用于将检测时间段内的超声波模拟信号连续转换成数字信号(需要的超声波信号未到达时，超声波幅值接近零，到达时超声波幅值将在设计范围内)，并存入RAM，成为一数据组；通过中央处理器扫描采集到的超声波信号幅度，并处理该组超声波数据；根据接收到的超声波脉冲群的平均幅值，来计算用于判断的阈值。本实施例将转换后的超声波数字信号的多个最高峰值超声波信号，取其平均值，再以该平均值的合适比例的值做为判断阀值。

扫描采集到的超声波信号幅度，达到判断阈值的点则认为当前测试的超声波信号到达起始点，否则认为是噪声信号。从而实现超声波检测，解决了超声波信号在传输过程中的衰减问题。

具体地，电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的运动目标或路径以及信标模块的位置，使得机器人在允许的运动范围内都能够获得信标模块的交叉覆盖；并将电子地图以无线方式传给机器人定位终端进行机器人的调度；还用于协调信标模块和机器人定位终端的同步，接收从各个机器人定位终端上传的位置坐标数据并显示。电子导航平台具体包括：

搭载导航软件的计算机，用于存储全部机器人的编码，构建电子地图，根据需要调度机器人，控制多台机器人同时完成各自的工作。每一台机器人都具有全球唯一编码，将用到的机器人的编码全部搭载导航软件的计算机内存储。再将规划好的电子地图传送给每一台机器人。

第三无线收发模块，用于传送电子地图给机器人定位终端，并发送定位同步信号，协调信标模块和机器人定位终端的时序，接收各个机器人定位终端上传的位置坐标数据以及障碍物扫描仪上传的通知信息。

显示器，用于显示电子地图上的导航信息。

电子地图运算模块，用于规划并计算机器人的运动目标或路径以及信标模块的位置，生成导航信息。

如图1所示：超声波接收阵列的上部搭载有障碍物扫描仪4；障碍物扫描仪4包括同步旋转系统和控制系统；同步旋转系统包括设于超声波接收阵列上的支架41，支架41上安装伺服电机42，伺服电机42通过旋转搭载头43连接复合传感器座44，复合传感器座44上安装有人体感应传感器441和超声波收发传感器442，伺服电机42的下端连接绝对编码器45；控制系统包括电机驱动模块46和障碍物扫描主控模块47，障碍物扫描主控模块47的输出端连接电机驱动模块46，障碍物扫描主控模块47的输入端分别连接绝对编码器45、人体感应传感器441和超声波收发传感器442，电机驱动模块46的输出端连接伺服电机42；超声波收发传感器442还通过超声波信号放大器与障碍物扫描主控模块47连接，超声波信号放大器用于超声波收发传感器442接收和发送的超声波信号进行放大。超声波收发传感器442的前端安装有吸音管443。吸音管443用于收窄超声波接收的角度；在使用超声波对障碍物进行探测与扫描时，超声波的能量是扩散的，吸音管443通过收窄超声波接收的角度，将超声波的能量集中，从而更好地提高检测的效果。障碍物扫描主控模块47通过CAN总线分别连接电子导航平台和机器人定位终端。

本实施例的导航方法，具体包括以下步骤：

(1)由机器人定位终端依据移动体行走的起始坐标数据，结合定位后沿预设的运动轨迹可能达到的坐标，计算规划出每个可能达到的坐标处所需的至少两个信标模块，依次序分别呼唤需要的信标模块，被呼唤到的信标模块发送无线信号回应；待呼唤到的信标模块全部回应后，机器人定位终端依次向信标模块发射无线申请定位信号，收到申请定位信号的信标模块，按照测试时序安排向机器人定位终端发射无线同步信号，使信标模块和机器人定位终端配合同步测试。

具体的，在预设的运动轨迹上标定好机器人可能达到的位置坐标点，当机器人沿运动轨迹到达某个坐标点时，依次序呼唤离该坐标点角度最佳的至少两个信标模块；由于每个信标模块都具有一定的覆盖范围，通过合理安排每个可能达到的坐标处的信标模块位置，使机器人在允许的全部运动范围内都能够有信标模块的交叉覆盖。其中，两个信标模块可确定机器人的二维坐标，三个信标模块可确定机器人的三维坐标，可根据电子地图的类型(平面电子地图和三维电子地图)来确定信标模块的数量。被呼唤的信标按照测试时序安排向机器人定位终端发射无线同步信号，以使得机器人定位终端在不同的时间段接收到不同的信标模块发射来的超声波信号。另外，当存在多个机器人时，如果出现请求信标模块冲突的问题，由电子导航平台查询正在工作的机器人所呼唤的信标模块是否被其它机器人占用，若已占用，则通知该机器人需间隔某一时间后再动作，间隔时间到后，继续完成后面的定位测试，从而达到多台机器人同时各自完成各自工作的目的。间隔时间通常设为以毫秒为单位。还有，当机器人偏离预设的运动轨迹时，仍可根据设于机器人附近的其它运动轨迹中所规划好的信标模块实现自动定位，从而避免机器人偏离运动轨迹就迷路，不能正常工作的问题。

(2)再由信标模块按时序分别向机器人发射已编码的超声波信号，同时收到同步信号的机器人定位终端启动数据采集时间窗口T，实时采集数据并依次存储，根据接收到的超声波信号的时序，分别标记接收到的时间为T₁,T₂,…,T_i，其中，i与所呼唤信标模块的数量相同；然后处理计算每个时间段接收到的超声波信号数据段，识别其编码，若接收到多组相同编码的超声波信号，只取最先到达的一组超声波编码信号，剔除其余杂波，再经计算找到每个时间段最先到达的此组超声波编码信号的起头时间T₁’,T₂’,…,T_i’。

具体的，启动数据采集时间窗口T后，开始连续采集数据，开启数据采集窗口的那一刻标记为采集时间零点，在时间窗口内采集的数据全部存入RAM(存储时按照时序依次存储)，并根据接收到的超声波信号的时序，分别标记接收到的时间为T₁,T₂,…,T_i；在时间窗口关闭后，机器人定位终端将立即处理RAM中的数据。其中，根据接收到的超声波信号的时序，分别标记接收到的时间为T₁,T₂,…,T_i可以理解为：假设T＝0.15s，根据接收到的超声波信号的时序，接收到第一个信标模块发射的一组超声波信号的时间为0.05s，接收到第二个信标模块发射的一组超声波信号的时间为0.1s，则0～0.05s的时间段标记为T₁，0.05s～0.1s的时间段标记为T₂；每个时间段只获取最先到达的一组超声波编码信号，其中，获取最先到达的一组超声波编码信号是指最先到达某个超声波感应元或某几个超声波感应元上的超声波信号，且一定是直线到达；其余杂波包括多径效应、非视距传播和超声波反射产生的信号，剔除这些杂波可解决环境声波的干扰问题。

(3)计算得到超声波接收阵列与每个信标模块之间的距离z_i为z_i＝T_i’×c，其中，c为超声波信号在常温下的传播速度，并需要对超声波信号的传播速度c进行修正；

(4)计算得出超声波接收阵列理论中心与每个信标模块的距离L_i以及信标模块与超声波接收阵列理论中心的右旋角γ_i；其中，超声波接收阵列理论中心是指超声波接收阵列的几何中心点；右旋角是指该信标模块与超声波接收阵列理论中心之间的距离直线沿超声波接收阵列理论中心的中轴线向右旋转所呈的夹角；

(5)由机器人定位终端甄选出两路或三路信标模块直线到达超声波接收阵列的超声波信号，并结合内置的电子地图进行计算处理，即分别收发根据步骤(4)计算得到的超声波接收阵列理论中心与这两路或三路信标模块之间的距离，并结合这两路或三路信标模块的固定坐标计算得出机器人的所在位置的二维坐标数据或三维坐标数据；

(6)将所得的二维坐标数据或三维坐标数据无线传送给电子导航平台，生成机器人的导航信息并显示。

(7)当机器人运动到半途遇有障碍时，将停止运动，视情况处理。若遇到的障碍是人，将语音提醒人们让路，如超时不让，则通知电子导航平台，由人工介入处理；若遇到的是物体，机器人将无线通知电子导航平台，同时自己规划在自己的运动范围内可不可以绕道，若能够绕过则规划绕道行驶，如不能绕过则通知导航平台，由人工介入处理。

具体的，当机器人运动到半途遇有障碍时，发送指令给CAN总线，CAN总线在接收到机器人发送的指令后，会把指令传给障碍物扫描主控模块47；障碍物扫描主控模块47在接收到指令后会判定该指令是否有效：如无效，则直接丢弃；如有效，将对指令进行分解，从中获取到扫描角度信息；根据指令需要测试的角度(如45°)以及需要扫描的角度范围(如40°～50°)，障碍物扫描主控模块47控制电机驱动模块46，带动伺服电机42在指令的角度范围内(如40°～50°)摆动，与伺服电机46同轴的绝对编码器45同步反馈摆动到的角度，在此过程中，障碍物扫描主控模块通过指令将需要的角度范围分解为在这个角度前后一定角度范围内的N(如50)个点，伺服电机分别在这些点停顿一定时间(如0.1秒)；超声波收发传感器和人体感应传感器将在这些停顿点进行测试，获取到这些点中的数据(超声波返回时间及强度)，这50个测试点的数据与绝对编码器中的数据依次串起，即获得扫描数据；障碍物扫描主控模块对扫描数据进行处理，确定在指定的角度上有无障碍物、障碍物大小及障碍物是否是人。

另外，针对上述的若遇到的是物体，机器人能够规划在自己的运动范围内可不可以绕道，具体是：机器人通过选择附近的其它运动轨迹中所规划好的无障碍的信标模块进行定位数据计算。

为方便描述，本实施例以机器人在室内移动为例进行具体说明。如图2所示：机器人定位终端2固定安装在机器人1的顶部，它搭载的超声波接收阵列21斜向上安装在机器人1的顶部中心位置，其倾斜角度以能够获得室内最大接收范围为准，超声波接收阵列21包括若干个圆周分布的超声波感应元。超声波接收阵列21的上部搭载有障碍物扫描仪4；还设有一套电子导航平台3。

在墙的两面安装有信标模块A～H，每个信标模块A～H发射的超声波信号都将覆盖自己的一片区域，当有3路及以上的信号时，则共同覆盖着一片区域，且该覆盖区域形成立体交叉式，机器人1在室内移动时，都可在形成立体交叉的超声波覆盖区域内行走。当机器人1需要定位信号时，由机器人定位终端2依据机器人1行走的起始坐标数据，结合定位后沿预设的运动轨迹可能达到的坐标，计算规划出每个可能达到的坐标处所需的最佳位置的三个信标模块，按照上述定位方法使被呼唤的信标模块和机器人1配合同步测试，本实施例中，机器人1从起始坐标沿运动轨迹向前移动，当移动到某处预设的位置坐标上时，所需的信标模块为D、E、F，信标模块D、E、F的三维坐标分别为D(0,0,0)、E(0,Y,0)和F(X,0,0)。

按D、E、F的时序发射无线同步信号后的信标模块，依次按此时序向机器人定位终端2发射带有各自编码的超声波信号，同时收到同步信号的机器人1启动数据采集，机器人1透过所搭载的超声波阵列21将接收到3路以上的信号。如图3所示：根据可靠性需要，甄选出3路用于计算的超声波信号数据(只采用直线到达的超声波数据，剔除由于多径效应和非视距传播造成的干扰信号)，计算出机器人所在的三维坐标(x,y,z)：

其中，L₁、L₂和L₃分别为超声波接收阵列理论中心与三个信标模块D、F、E的距离。为便于理解L₁、L₂和L₃的计算过程，本实施例还以三维坐标为(0,0,0)的信标模块D发射的超声波信号为例对计算L₁的过程进行具体说明。参照图4和图5所示：超声波接收阵列包括16个超声波感应元r0-r15,当(0,0,0)的信标模块D的超声波发射源发射超声波时，被超声波接收阵列中的r0-r4这五个超声波感应元接收，其中z₀-z₄为r0-r4分别与超声波发射源的距离，O为超声波接收阵列理论中心，R为圆周分布的16个超声波感应元与超声波接收阵列理论中心的距离，L₁为超声波发射源与超声波接收阵列理论中心的距离，角度b和长度s为超声波接收阵列的固定尺寸。

其中，在z₀-z₄中，z₃最小，z₂次小，r3的右旋角为α，角度a为直线z₂与长度s之间的夹角，超声波接收阵列理论中心到超声波发射源的的距离L₁与最近的超声波感应元r₃的角度偏移为β，此时超声波接收阵列理论中心与超声波发射源的角度为γ₁，计算得出：

根据上述公式可求得L₁的值，同理可求得L₂和L₃。其中，z₀-z₄即超声波感应元与超声波发射源的距离可由上述导航方法中的步骤(1)-步骤(3)获得。

由于在外部一定条件下其声速是一定的，当外部条件发生改变，我们可以根据外部条件的改变量来修正声速。使用经修正后的声速来进行计算，就可以在外在条件改变后也能计算出精确的距离。

由于各信标模块的坐标是固定的，则它们之间的距离(绝对距离)也是固定的。在超声波有效覆盖范围内的任意一个地方，先在固定的默认声速条件下，可计算出超声波接收阵列与两个信标模块的相对距离和角度；再计算出两个固定坐标的信标模块之间的相对距离(默认声速条件下)，再通过这两个固定坐标的信标模块之间的相对距离与它们之间的绝对距离进行比较，就可以计算出精确的当前声速。

为便于理解，本实施例取三维坐标为(X,0,0)的信标模块F与(0,0,0)的信标模块D为例进行具体说明，参照图6所示：

d₁为信标模块F与信标模块D之间的绝对距离(以mm为单位)，默认声速c₁，当前实际声速为c₂，L₁、L₂、右旋角γ₁和γ₂已在上述公式中计算得出，从而得到两个固定坐标的信标模块之间的夹角μ＝γ₂-γ₁，两个固定坐标的信标模块之间的相对距离d₂为：

由于d₁＝c₂t，d₂＝c₁t，因此实际声速公式为：

通过计算两个信标模块之间的绝对距离与相对距离之间的误差百分率作为补偿，进而计算出在当前环境条件下精确的声速。

上述计算过程由机器人定位终端2内的中央处理器实现，通过机器人1将接收到的超声波信号会同电子地图一起放入中央处理器的运算模块中进行计算，通过比较接收到的超声波信号的相位差，来判断机器人1在电子地图中的角度，最终将该位置处所得的三维坐标数据无线传送给电子导航平台3，生成机器人1的导航信息并显示。

在机器人1的行走过程中，只显示移动路径导航，不上传位置坐标数据，只有到达预设的坐标处才向电子导航平台3上传位置坐标数据，如图2中，当机器人1行走到以信标模块A、B、C为所需要呼唤的位置坐标处时，根据上述方法计算出机器人的三维坐标，上传至电子导航平台3即可。当机器人1在行走过程中遇到障碍物时，如通过障碍物扫描仪4检测出遇到的障碍物为物体时，机器人1将通知电子导航平台3，同时自动规划自己的运动范围可不可以绕道，例如选择无障碍的信标模块D、F、G来进行定位数据计算，来规划绕道行驶。

Claims (2)

1.一种机器人定位导航系统的导航方法，其特征在于：包括以下步骤：

（1）由机器人定位终端依据移动体行走的起始坐标数据，结合定位后沿预设的运动轨迹可能达到的坐标，计算规划出每个可能达到的坐标处所需的至少两个信标模块，依次序分别呼唤需要的信标模块，建立信标模块和机器人定位终端的同步测试关系；

（2）收到同步信号的机器人定位终端启动数据采集时间窗口T，接收信标模块按时序发来的已编码的超声波信号，分别标记接收到的时间为T₁,T₂,…,T_i，其中，i与所呼唤信标模块的数量相同；处理计算每个时间段接收到的超声波信号数据段，识别其编码，若接收到多组相同编码的超声波信号，只取最先到达的一组超声波编码信号，剔除其余杂波，再经计算找到每个时间段最先到达的此组超声波编码信号的起头时间T₁’,T₂’,…,T_i’；

（3）计算得到超声波接收阵列与每个信标模块之间的距离z_i为z_i= T_i’×c，其中，c为超声波信号在常温下的传播速度，并修正超声波信号的传播速度c；

（4）计算得出超声波接收阵列理论中心与每个信标模块的距离L_i以及信标模块与超声波接收阵列理论中心的右旋角γ_i；其中，超声波接收阵列理论中心是指超声波接收阵列的几何中心点；右旋角是指该信标模块与超声波接收阵列理论中心之间的距离直线沿超声波接收阵列理论中心的中轴线向右旋转所呈的夹角；

（5）甄选出两路或三路信标模块直线到达超声波接收阵列的超声波信号，并结合内置的电子地图进行计算处理，分别收发根据步骤（4）计算得到的超声波接收阵列理论中心与这两路或三路信标模块之间的距离，并结合这两路或三路信标模块的固定坐标计算得出机器人的所在位置的二维坐标数据或三维坐标数据，生成机器人的导航信息；

（6）规避运动过程中的障碍物，选择自动规避或人工介入。

2.根据权利要求1所述机器人定位导航系统的导航方法，其特征在于：步骤（3）中，所述超声波信号的传播速度c的修正方法为：

A．选取所需的两个信标模块，根据它们的固定坐标计算出它们之间的绝对距离；

B．在超声波有效覆盖范围内的任意一个地方，在固定的默认声速条件下，计算出超声波接收阵列理论中心与两个信标模块的相对距离和角度，进而计算出两个信标模块之间的相对距离；

C．将两个信标模块之间的绝对距离与相对距离的误差百分率作为补偿，计算出实际声速。