trilateration三边测距算法及C语言实现（适用stm32）

我们经常会使用UWB进行室内的简单定位工作，其使用的算法多种多样，今天简单介绍一下trilateration三边测距算法，这个算法很好理解，同时代码实现起来也不是很困难。

算法解析

1、已知三点位置 (x1, y1), (x2, y2), (x3, y3)
2、已知未知点 (x0, y0) 到三点距离 d1, d2, d3

以 d1, d2, d3 为半径作三个圆，根据毕达哥拉斯定理，得出交点即未知点的位置计算公式：
( x1 - x0 )2 + ( y1 - y0 )2 = d12
( x2 - x0 )2 + ( y2 - y0 )2 = d22
( x3 - x0 )2 + ( y3 - y0 )2 = d32

设未知点位置为 (x, y)， 令其中的第一个球形 P1 的球心坐标为 (0, 0)，P2 处于相同纵坐标，球心坐标为 (d, 0)，P3 球心坐标为 (i, j)，三个球形半径分别为 r1, r2, r3，z为三球形相交点与水平面高度。则有：
r12 = x2 + y2 + z2
r22 = (x - d)2 + y2 + z2
r32 = (x - i)2 + (y - j)2 + z2

当 z = 0 时， 即为三个圆在水平面上相交为一点，首先解出 x:
x = (r12 - r22 + d2) / 2d

将公式二变形，将公式一的 z2 代入公式二，再代入公式三得到 y 的计算公式：
y = (r12 - r32 - x2 + (x - i)2 + j2) / 2j

具体解法示意图如下：

代码实现

该代码是需要输入四个基站坐标和四个基站到未知点的距离的， 因为一般实际工程中最少都是使用四个基站，如果需要改成三个直接引用 trilateration(AX, AY, d1, BX, BY, d2, CX, CY, d3); 即可。4个基站只是在三个基站的基础上来解析的，多了一步对4种不同的三个基站的定位位置数据平均化。

同时，此代码与上文的解算过程略有出入，在后面会进行解释。

.h文件代码：

#ifndef __MY_LOCATION_H
#define __MY_LOCATION_H
	static int AX,BX,CX,DX,AY,BY,CY,DY;
	void MY_location_Init(int X1, int Y1, int X2, int Y2, int X3, int Y3,int X4,int Y4);
	double* trilateration(double x1, double y1, double d1, double x2, double y2, double d2, double x3, double y3, double d3);
	double* Printf(double d1, double d2, double d3, double d4);
	int P(int x, int n)