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

大家好，欢迎来到IT知识分享网。

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

背景

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

原理

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

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

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

代码

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; }