基于超声波的定位与跟踪

1. 三个超声波发射器的摆放位置决定了其本地坐标系，如下图所示（其中黄色圆圈代表发射器）：           
2. 发射端发射声波信号，并同时通过射频信号通知接收端开始计时，当接收器接收到声波时结束计时，则此发射器和接收器之间的距离为声速*t。依次计算三个发射器和接收器各自的距离；
3. 设此时接收器坐标为(x,y,z)，且通过结构定义已知三个发射器的坐标以及各自和接收器的距离关系，就可以根据两点间距离公式

                         (x-x’)2+(y-y’)2+(z-z’)2=d

获得三个一次方程，联立方程组即可求解坐标值；