系统工作过程是：

   1、发射点发送命令将接收器置为“定位模式”
   2、根据测量方式发送启动计时命令给接收模块，测量方式借用原测距的模式。
   3、启动计时命令发完后，发射10个脉冲
   4、等待70ms,读最近一次数据，计算

   计算原理是：
   接收点布置 4个，构成正方形，边长1m, 定义为 S，顺时针编号。
   0号为坐标原点（0,0)，1号为（0,1），2号为（1,1），3号为（1,0)

   高度布置后测量确定。计算时先根据直角三角形算出投射到移动物体平面的距离，斜边为测距值，高为一个直角边。

   计算出的移动物体距接收点的平面距离定义为 D0 、D1、D2、D3，移动节点的坐标为（x，y）。
   经推导得到：
     Y1 =  S/2 + (D0^2 - D1^2)/2S      （1）
     X1 =  S/2 + (D1^2 - D2^2)/2S      （2）
     Y2 =  S/2 + (D3^2 - D2^2)/2S      （3）
     X2  = S/2 + (D0^2 - D3^2)/2S      （4）

   其中 S 为边长，D0、D1、D2、D3 为4个接收节点测得的距离值。

   根据计算投射距离的公式：（设4个空间距离分别为 L0 L1 L2 L3, 高度为 H)
     D0^2 = L0^2 - H^2
     D1^2 = L1^2 - H^2
     D2^2 = L2^2 - H^2
     D3^2 = L3^2 - H^2

   则上述坐标公式可以转换为：
     Y1 =  S/2 + (L0^2 - L1^2)/2S      （1）
     X1 =  S/2 + (L1^2 - L2^2)/2S      （2）
     Y2 =  S/2 + (L3^2 - L2^2)/2S      （3）
     X2  = S/2 + (L0^2 - L3^2)/2S      （4）

   可以忽略高度参数！不用计算投射到平面的距离，直接使用测量结果。

   通过上述 4 个测量值可以得到 2 组坐标。将2组平均可以减小误差，或者作为相互间的合理性判断。

   系统构建好之后，简单测试后就没有深入，因为缺少好的演示对象，用手摆放定位点太傻。同时 PC 上的程序缺少图形显示，用数字坐标极不直观。

   最近因为 PID 调试需求，请朋友帮忙做了一个图形显示，顺带兼顾了定位的需求。同时四轮全向小车平台完成，正好作为定位对象。

   这样完成了定位系统的测试，效果如下：

   从效果看还算满意，唯一的不足是超声波测距的采样速率不高，和图像识别定位不能比，图像至少能达到25Hz，超声波最多10Hz。但超声波的好处是范围大，且环境光线要求不高。

   如果作为小车定位可以在2个定位点之间用航位推测方式插值，这样既弥补了航位推测的累积误差，又可忽略超声波采样速率的不足。

   定位测试系统的构成部件如下：
 

   用无线同步还有一个附带的好处：就是可以用PC 机监听，甚至插入控制，这样大大方便调试，使用以下无线接口即可：
 

   上述测试只是个示例，具体的应用还需要客户自行优化，我提供源程序作为参考。

   实际上不一定是定位，还可以作为跟踪、三维鼠标等应用。

   以下就是广州一个同学所做的“购物车自动跟随系统”：

——————————————
南京嵌入之梦工作室
2011年5月21日星期六

最近，基于新做的 Arduino控制四代小车底盘，以及 Processing 语言，做了一个小车定位的演示，比前一个效果略好，在此与大家分享：