声速条件下，可计算出超声波接 收阵列与两个信标模块的相对距离和角度；再计算出两个固定坐标的信标模块之间的相对 距离(默认声速条件下)，再通过运两个固定坐标的信标模块之间的相对距离与它们之间的 绝对距离进行比较，就可W计算出精确的当前声速。
[0049] 为便于理解，本实施例取=维坐标为（X，0, 0)的信标模块F与（0, 0, 0)的信标模块 D为例进行具体说明，参照图6所示： di为信标模块F与信标模块D之间的绝对距离（Wmm为单位)，默认声速C1，当前实际 声速为C2,Li、L2、右旋角丫 1和丫 2已在上述公式中计算得出，从而得到两个固定坐标的信 标模块之间的夹角y= 丫 2-丫 1，两个固定坐标的信标模块之间的相对距离d2为：
通过计算两个信标模块之间的绝对距离与相对距离之间的误差百分率作为补偿，进而 计算出在当前环境条件下精确的声速。
[0050] 上述计算过程由机器人定位终端2内的中央处理器实现，通过机器人1将接收到 的超声波信号会同电子地图一起放入中央处理器的运算模块中进行计算，通过比较接收到 的超声波信号的相位差，来判断机器人1在电子地图中的角度，最终将该位置处所得的= 维坐标数据无线传送给电子导航平台3,生成机器人1的导航信息并显示。
[0051] 在机器人1的行走过程中，只显示移动路径导航，不上传位置坐标数据，只有到达 预设的坐标处才向电子导航平台3上传位置坐标数据，如图1中，当机器人1行走到W信标 模块A、B、C为所需要呼唤的位置坐标处时，根据上述方法计算出机器人的=维坐标，上传 至电子导航平台3即可。当机器人1在行走过程中遇到障碍物时，如通过障碍物扫描仪4 检测出遇到的障碍物为物体时，机器人1将通知电子导航平台3,同时自动规划自己的运动 范围可不可W绕道，例如选择无障碍的信标模块D、F、G来进行定位数据计算，来规划绕道 行驶。
【主权项】
1. 一种机器人定位导航系统，其特征在于：包括多个固定位置的信标模块、至少一个 机器人定位终端和电子导航平台； 所述信标模块包括： 第一无线收发模块，用于以无线的方式与机器人定位终端和/或电子导航平台联络， 收发指令和同步信号； 超声波发射模块，用于发出已编址的超声波信号； 第一智能处理模块，用于处理接收的指令和储存超声波编码，处理同步时序安排； 所述机器人定位终端包括： 第二无线收发模块，用于负责与信标模块和电子导航平台联络，收发无线同步信号，以 及接收对待接收的超声波信号做出的时序安排； 超声波接收阵列，用于接收来自不同角度的超声波信号； 第二智能处理模块，包括阵列处理模块和中央处理器； 所述阵列处理模块用于将接收的超声波信号放大，根据其信号的强弱自动进行增益 控制，使输出的超声波模拟信号强度保持相对稳定，并将接收到的模拟信号转换成数字信 号； 所述中央处理器用于处理接收到的指令、模块各部分的逻辑关系、做出同步时序安排、 识别编码、储存和分析接收到的超声波数据信息，并将超声波数据会同电子导航平台发送 来的电子地图一起放入计算单元中进行计算处理，得到位置坐标数据； 所述电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的 运动目标或路径以及信标模块的位置；并将电子地图以无线方式传给机器人定位终端进行 机器人的调度，规避障碍物；还用于协调信标模块和机器人定位终端的同步，接收从各个机 器人定位终端上传的位置坐标数据。2. 根据权利要求1所述的机器人定位导航系统，其特征在于：所述超声波接收阵列的 上部搭载有障碍物扫描仪；所述障碍物扫描仪包括同步旋转系统和控制系统； 所述同步旋转系统包括设于超声波接收阵列上的支架，支架上安装伺服电机，伺服电 机通过旋转搭载头连接复合传感器座，复合传感器座上安装有人体感应传感器和超声波收 发传感器，伺服电机的下端连接编码器； 所述控制系统包括电机驱动模块和障碍物扫描主控模块，所述障碍物扫描主控模块的 输出端连接电机驱动模块，障碍物扫描主控模块的输入端分别连接编码器、人体感应传感 器和超声波收发传感器，电机驱动模块的输出端连接伺服电机；所述障碍物扫描主控模块 通过CAN总线分别连接电子导航平台和机器人定位终端。3. 根据权利要求2所述的机器人定位导航系统，其特征在于：所述超声波收发传感器 通过超声波信号放大器与所述障碍物扫描主控模块连接，所述超声波信号放大器用于超声 波收发传感器接收和发送的超声波信号进行放大。4. 根据权利要求2或3所述的机器人定位导航系统，其特征在于：所述超声波收发传 感器的前端安装有用于收窄超声波接收角度的吸音管。5. 根据权利要求1或2或3所述的机器人定位导航系统，其特征在于：所述阵列处理 模块包括前置放大模块和A/D转换模块； 所述前置放大模块用于将接收的超声波信号放大，根据其信号的强弱自动进行增益控 制，使输出的超声波模拟信号强度保持相对稳定； 所述A/D转换模块用于将检测时间段内的超声波模拟信号连续转换成数字信号并存 储；通过中央处理器扫描采集到的超声波信号幅度，处理该组超声波数据；根据接收到的 超 声波脉冲群的平均幅值，来计算用于判断的阈值；达到判断阈值的点则认为当前测试 的超声波信号到达起始点，否则认为是噪声信号。6. 根据权利要求2或3所述的机器人定位导航系统，其特征在于：所述电子导航平台 包括： 搭载导航软件的计算机，用于存储全部机器人的编码，构建电子地图，根据需要调度机 器人，控制多台机器人同时完成各自的工作； 第三无线收发模块，用于传送电子地图给机器人定位终端，并发送定位同步信号，协调 信标模块和机器人定位终端的时序，接收各个机器人定位终端上传的位置坐标数据以及障 碍物扫描仪上传的通知信息； 显示器，用于显示电子地图上的位置信息； 电子地图运算模块，用于规划并计算机器人的运动目标或路径以及信标模块的位置， 生成导航彳目息。7. 根据权利要求1或2或3所述的机器人定位导航系统，其特征在于：所述电子地图 包括与机器人运动区域和信标模块覆盖区域分别相对应的电子平面地图和三维电子地图。8. 根据权利要求1或2或3所述的机器人定位导航系统，其特征在于：所述超声波收发 阵列为具有至少四个超声波感应元的集成阵列，或由至少四个独立的超声波传感器分散而 成的阵列；所述前置放大模块和A/D转换模块的通道数量与超声波感应元的数量相对应。9. 根据权利要求1~8任一项所述机器人定位导航系统的导航方法，其特征在于：包括 以下步骤： (1) 由机器人定位终端依据移动体行走的起始坐标数据，结合定位后沿预设的运动轨 迹可能达到的坐标，计算规划出每个可能达到的坐标处所需的至少两个信标模块，依次序 分别呼唤需要的信标模块，建立信标模块和机器人定位终端的同步测试关系； (2) 收到同步信号的机器人定位终端启动数据采集时间窗口 T，接收信标模块按时序发 来的已编码的超声波信号，分别标记接收到的时间为TliT2…，T 1，其中，i与所呼唤信标模块 的数量相同；处理计算每个时间段接收到的超声波信号数据段，识别其编码，若接收到多组 相同编码的超声波信号，只取最先到达的一组超声波编码信号，剔除其余杂波，再经计算找 到每个时间段最先到达的此组超声波编码信号的起头时间T/，T 2'，…，IV ; (3) 计算得到超声波接收阵列与每个信标模块之间的距离Z1Sz1= IV Xc，其中，c为 超声波信号在常温下的传播速度，并修正超声波信号的传播速度c ; (4) 计算得出超声波接收阵列理论中心与每个信标模块的距离L1以及信标模块与超声 波接收阵列理论中心的右旋角Y 1; (5) 计算机器人所在位置的二维坐标数据或三维坐标数据，生成机器人的导航信息； (6 )规避运动过程中的障碍物，选择自动规避或人工介入。10. 根据权利要求9所述机器人定位导航系统的导航方法，其特征在于：步骤（3)中，所 述超声波信号的传播速度c的修正方法为： A. 选取所需的两个信标模块，根据它们的固定坐标计算出它们之间的绝对距离； B. 在超声波有效覆盖范围内的任意一个地方，在固定的默认声速条件下，计算出超声 波接收阵列理论中心与两个信标模块的相对距离和角度，进而计算出两个信标模块之间的 相对距离； C. 将两个信标模块之间的绝对距离与相对距离的误差百分率作为补偿，计算出实际声 速。
【专利摘要】一种机器人定位导航系统及其导航方法，所述机器人定位导航系统包括多个信标模块、至少一个机器人定位终端和电子导航平台；信标模块包括第一无线收发模块、超声波发射模块和第一智能处理模块；机器人定位终端包括第二无线收发模块、超声波接收阵列、阵列处理模块和中央处理器；电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的运动目标或路径以及信标模块的位置；调度机器人，规避障碍物；还用于协调信标模块和机器人定位终端的同步，接收从各个机器人定位终端上传的位置坐标数据。本发明还包括机器人定位导航系统的导航方法。本发明解决了多径效应和非视距传播问题，提高测量精度，实现自动定位。
【IPC分类】G01C21/00
【公开号】CN105115498
【申请号】CN201510637976
【发明人】戴晓洪, 戴苗苗
【申请人】长沙开山斧智能科技有限公司
【公开日】2015年12月2日
【申请日】2015年9月30日