r4这五个超声波感应元接收，其中Z(j-z4S r〇-r4分别与超声波 发射源的距离，0为超声波接收阵列理论中心，R为圆周分布的16个超声波感应元与超声 波接收阵列理论中心的距离，1^为超声波发射源与超声波接收阵列理论中心的距离，角度b 和长度S为超声波接收阵列的固定尺寸。
[0089] 其中，在zQ_z4中，z 3最小，z 2次小，r3的右旋角为α，角度为直线z 2与长度s之 间的夹角，超声波接收阵列理论中心到超声波发射源的的距离Q与最近的超声波感应元r3的角度偏移为β，此时超声波接收阵列理论中心与超声波发射源的角度为，计算得出：
[0094] 根据上述公式可求得Q的值，同理可求得L 2和L 3。其中，Z(]-z4即超声波感应元与 超声波发射源的距离可由上述导航方法中的步骤（1)-步骤（3)获得。
[0095] 由于在外部一定条件下其声速是一定的，当外部条件发生改变，我们可以根据外 部条件的改变量来修正声速。使用经修正后的声速来进行计算，就可以在外在条件改变后 也能计算出精确的距离。
[0096] 由于各信标模块的坐标是固定的，则它们之间的距离(绝对距离）也是固定的。在 超声波有效覆盖范围内的任意一个地方，先在固定的默认声速条件下，可计算出超声波接 收阵列与两个信标模块的相对距离和角度；再计算出两个固定坐标的信标模块之间的相对 距离(默认声速条件下)，再通过这两个固定坐标的信标模块之间的相对距离与它们之间的 绝对距离进行比较，就可以计算出精确的当前声速。
[0097] 为便于理解，本实施例取三维坐标为（X，0, 0)的信标模块F与（0, 0, 0)的信标模块 D为例进行具体说明，参照图6所示：
[0098] 山为信标模块F与信标模块D之间的绝对距离（以mm为单位)，默认声速c i，当前 实际声速为c2, Li、L2、右旋角yJP γ 2已在上述公式中计算得出，从而得到两个固定坐标 的信标模块之间的夹角，两个固定坐标的信标模块之间的相对距离(12为：
[0100] 由于山=c2t，d2= Cit，因此实际声速公式为：
[0102] 通过计算两个信标模块之间的绝对距离与相对距离之间的误差百分率作为补偿， 进而计算出在当前环境条件下精确的声速。
[0103] 上述计算过程由机器人定位终端2内的中央处理器实现，通过机器人1将接收到 的超声波信号会同电子地图一起放入中央处理器的运算模块中进行计算，通过比较接收到 的超声波信号的相位差，来判断机器人1在电子地图中的角度，最终将该位置处所得的三 维坐标数据无线传送给电子导航平台3,生成机器人1的导航信息并显示。
[0104] 在机器人1的行走过程中，只显示移动路径导航，不上传位置坐标数据，只有到达 预设的坐标处才向电子导航平台3上传位置坐标数据，如图1中，当机器人1行走到以信标 模块A、B、C为所需要呼唤的位置坐标处时，根据上述方法计算出机器人的三维坐标，上传 至电子导航平台3即可。当机器人1在行走过程中遇到障碍物时，如通过障碍物扫描仪4 检测出遇到的障碍物为物体时，机器人1将通知电子导航平台3,同时自动规划自己的运动 范围可不可以绕道，例如选择无障碍的信标模块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. 根据权利要求5所述的机器人定位导航系统，其特征在于：所述超声波收发阵列为 具有至少四个超声波感应元的集成阵列，或由至少四个独立的超声波传感器分散而成的阵 列；所述前置放大模块和A/D转换模块的通道数量与超声波感应元的数量相对应。
【专利摘要】一种机器人定位导航系统，包括多个信标模块、至少一个机器人定位终端和电子导航平台；信标模块包括第一无线收发模块、超声波发射模块和第一智能处理模块；机器人定位终端包括第二无线收发模块、超声波接收阵列、阵列处理模块和中央处理器；电子导航平台用于存储全部机器人的编码，构建电子地图，规划并计算机器人的运动目标或路径以及信标模块的位置；调度机器人，规避障碍物；还用于协调信标模块和机器人定位终端的同步，接收从各个机器人定位终端上传的位置坐标数据。本实用新型解决了多径效应和非视距传播问题，提高测量精度，实现自动定位。
【IPC分类】G01C21/00
【公开号】CN205049151
【申请号】CN201520768513
【发明人】戴晓洪, 戴苗苗
【申请人】长沙开山斧智能科技有限公司
【公开日】2016年2月24日
【申请日】2015年9月30日