三点定位法（原理及实现）

三点定位法（原理及实现）

背景

学习笔记
参考 https://blog.csdn.net/jjwwwww/article/details/87714318#_4
由于文中大量数据来自上述博客，因此记为转载

原理

三点定位，顾名思义，需要三个点，这三个点一般为基站，定谁的位置？一般为客户端或者说终端的位置。通过测量终端和基站的距离（这个测量的过程中可能会存在误差，先假设没有误差），将其作为这三个圆的半径，画图，最终求得三圆的交点，交点即为终端位置，达到定位效果。如图。图中O点即为所求。
　　　

接下来考虑有测量误差的情况。实际测量过程中很容易出现下面的这种情况。这种情况下相交的是一块区域。如图中三个圆的交集为BCE区域。

这种情况怎么解决？我们先考虑两个圆的情况。这里我们先求C点的坐标。

两个圆（圆心为基站位置，半径是我们的测量数据即终端到基站的距离）交于A,B两点，连接AB,PQ,AQ,AP,　AB和PQ交于点C，圆心的距离PQ在布置完基站之后便已知，圆心和半径已知，联立方程可以求得A,B点的坐标，从而根据欧氏距离求得PA,AQ。根据勾股定理：

解得

根据比例关系求得坐标

三个圆两两进行计算可以得到三个点。之后可以取这三个点的坐标的均值作为终端的坐标。

此外，还有可能会出现以下圆不相交的情况。

这种情况怎么解决？我们同样先考虑两个圆的情况。如图。此时我们求O点。

简单的方法是直接根据比例半径计算。

同样的，最终会得到三个点，取这三个点的坐标的均值作为终端的坐标。

代码

struct Point
{             
	int x;                //x坐标
	int y;                //y坐标
	Point() :x(0), y(0) {};
};

//三点定位法
//dis:半径
//points：圆心
Point threePoints(float *dis, Point *ps) 
{
	Point p;
	if (dis == NULL || ps== NULL)
		return p;

	for (int i = 0; i < 3; ++i)
     {
	    //检查距离是否有问题
		if (dis[i] < 0)
			return Point();

		for (int j = i + 1; j < 3; ++j) 
        {
		    //圆心距离PQ
			float p2p = (float)sqrt((ps[i].x - ps[j].x)*(ps[i].x - ps[j].x) +
				                    (ps[i].y - ps[j].y)*(ps[i].y - ps[j].y));
		    //判断两圆是否相交
			if (dis[i] + dis[j] <= p2p) 
            {
			    //不相交，按比例求
				p.x += ps[i].x + (ps[j].x - ps[i].x)*dis[i] / (dis[i] + dis[j]);
				p.y += ps[i].y + (ps[j].y - ps[i].y)*dis[i] / (dis[i] + dis[j]);
			}
			else
             {
			    //相交则套用公式
                //PC
				float dr = p2p / 2 + (dis[i] * dis[i] - dis[j] * dis[j]) / (2 * p2p); 
                //x = xp + (xq-xp) * PC / PQ
				p.x += ps[i].x + (ps[j].x - ps[i].x)*dr / p2p;
                //y = yp + (yq-yp) * PC / PQ
				p.y += ps[i].y + (ps[j].y - ps[i].y)*dr / p2p;
			}
		}
	}
	
    //三个圆两两求点，最终得到三个点，求其均值
	p.x /= 3;
	p.y /= 3;

	return p;
}