一种精确的超声波定位导航系统

定位部分

硬件组成

装置的核心部件是一套由单片机组成的系统，包括超声波发射器，超声波接收器，接收中控模块，发射中控模块构成，系统和计算机共同组成了以计算机为核心的定位系统。系统结构图如图1所示。

图1

系统工作方式

构思的系统工作方式为，以计算机控制定位过程并依次实现：首先计算机发送启动测量命令给发射端，延时等待；再一步为发射端发送超声波，并通知接收端启动计时；计算机延时结束后，从接收端读取距离数据，根据接收点的布置计算坐标，并根据需要发布坐标和显示。

该系统主要由两种核心程序来实现。一是单片机控制程序，二是坐标计算程序。单片机得到各个超声波接收器与超声波发射器的距离后，可将该信息传到计算机中，通过上述所介绍的数学模型和算法，即可计算出物体的相对位置坐标。

温度改变会引起声速的变化，由于本实验装置的空间尺寸较小，所以需要考虑到温度对测量精度的影响。该装置中有声速修正程序，通过单片机上的温度传感器，得到当前环境的实时温度，再通过声速修正公式计算出当前温度下的声速，以达到修正声速的目的。最后是对坐标的实时显示（可视化程序），利用计算机语言对所得到的位置坐标进行实时显示，并构建可视化显示方式，令物体的坐标位置具有很高的可读性。

系统界面组成

操作界面

此界面由四个部分组成，首先是位于右上方的XY相对坐标实时显示界面，位于右下方的XZ相对坐标实时显示界面，再者是位于左下方的YZ相对坐标实时显示界面，最后是位于右侧的坐标值显示区域。

系统的实验装置由空间超声波定位系统装置和实验支架组成，如图3所示。支架顶端四个角上分别装有四个超声波接收器，超声波发射器（被定位物体）被安装于可在平面自由移动的滑杆上，架子还可以在竖直方向上移动，从而能够实现被定位物体在空间上的自由移动。

以底端一个角作为原点建立了空间直角坐标系，并粘贴了刻度尺。在测量时，移动平面滑杆和竖直移动装置，将超声波发射器（被定位物体）置于空间中的某一位置，刻度尺上所显示的X、Y、Z轴坐标值即为被定位物体的实际值。程序所给出的X、Y、Z轴坐标值为被定位物体的测量值，将其与实际值相比较，即可得知空间超声波定位系统装置的定位精度。

导航部分

此部分用超声波定位来实现，这属于“绝对定位”范畴，即便出现一点较大的误差，也不会影响下一点的定位精度。不过超声波定位也有其局限性：主要是定位速度稍慢，每秒数据刷新率在3–10次，10次已经接近极限了，比较可靠的速率应该是每秒3-5次。若走的太快，则会出现较大误差。想要提高精度，可能需要用航位推测法弥补两次定位数据之间的空白，超声波定位信息正好作为航位推测的相对位置修正。

当前和目标位置信息派发设计

基于Processing 测试程序实现，在定位测试页面实现当前位置和目标位置的发送当前位置信息派发比较简单，每次定位完成后将坐标发送给小车即可，只需要设计一个数据帧，将定位的坐标信息和采集时间打包。目标位置的产生通过鼠标在定位区域点击获得。为了能实现“多目标位置信息”派发，设计一个按钮：清零，目的是将前面输入的目标位置清除，从头开始。正常操作，没点击一次鼠标，生成一个目标位置，设计一个独立帧，在发送当前位置信息后发送目标位置。

目标位置帧包含两个信息：第一：是否为第一点。第二：目标位置X、Y坐标。这样在接收时就可以知道该如何处理，如果是第一点，则将前面存放的目标位置清除，指向目标位置数组的第一单元，如果是后续目标，则将目标位置添加在数组的最后。

相应行为设计

有了上述控制端的设计，行为设计如下：

首先：在无线接收任务中增加相关的命令解析，得到小车的位置和目标位置。第二：增加一个任务，完成向目标位置行进。

向目标位置行进的方式初步设计为由两个行为组成：行为一：转向，将行走方向指向目标位置。行为二：直线行走，走向目标。当目标位置和小车的位置相距小于50mm后视为到达目标。

控制逻辑为：收到第一个目标位置后，立刻计算和我的距离，大于50mm则启动。如果我没有得到当前的方向，则第一个行为是直线行走，否则根据目标方向和我的方向差，转向。每收到一个新的位置信息，即判断和前面计算方向时保存的位置相距是否超过50mm，超过则计算一次方向，判断是否需要转向，根据结果执行转向或继续直行。位置信息和目标信息均通过邮箱传递，在行走任务中不断查询是否有新的坐标信息。