三点定位法原理及实现—

三点定位法原理及实现——蓝牙定位

前言
原理
- 两圆相交
- 两圆不相交
代码实现

前言

最近在做蓝牙室内定位，蓝牙定位用到的一个非常经典的算法就是三点定位法。

原理

三点定位法，顾名思义首先有三个圆点，同时我们也知道这三个圆的半径，最终求得三圆的交点，达到定位效果。如图：

我们的目的是求得O点，利用毕达哥拉斯定理我们可以快速求得O点坐标。不过事与愿违，现实情况并不总是如人意，而是这样：

甚至有的情况其中两个圆或者三个圆完全就不相交，如下图：

由于这些误差原因，实际上想要准确的知道使用者当前位置是相当困难的，但是我们可以通过运算得到一个近似解。为了应付这些情况，我们需从两个圆入手，先找到两两圆之间的中心点，再求三圆的中心点。

除去圆中圆的情况（现实基本上不存在），两圆关系可分为两种，相交，不相交。

两圆相交

可知此时两圆交点A，B，我们的目标点是AB中点C。根据勾股定理我们可知
{PQ=PC+QCQA2=QC2+AC2PA2=PC2+AC2\begin{cases} \quad PQ=PC+QC\\ \quad QA^2=QC^2+AC^2\\ \quad PA^2=PC^2+AC^2 \end{cases} ⎩⎪⎨⎪⎧PQ=PC+QCQA2=QC2+AC2PA2=PC2+AC2
解得：
PC=PQ2+PA2−QA22PQPC=\frac{PQ}{2}+\frac{PA^2-QA^2}{2PQ}PC=2PQ+2PQPA2−QA2
根据比例关系求得坐标：
{x=xP+(xQ−xP)PCPQy=yP+(yQ−yP)PCPQ\begin{cases} \quad x=x_P+\frac{(x_Q-x_P)PC}{PQ}\\ \quad \\ \quad y=y_P+\frac{(y_Q-y_P)PC}{PQ} \end{cases}⎩⎪⎨⎪⎧x=xP+PQ(xQ−xP)PCy=yP+PQ(yQ−yP)PC

两圆不相交

如图，我们需要求O点，简单的做法就是直接根据半径比例计算
{sum=rP+rQx=xP+(xQ−xP)rPsumy=yP+(yQ−yP)rPsum\begin{cases} \quad sum=r_P+r_Q\\ \quad \\ \quad x=x_P+\frac{(x_Q-x_P)r_P}{sum}\\ \quad \\ \quad y=y_P+\frac{(y_Q-y_P)r_P}{sum} \end{cases}⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧sum=rP+rQx=xP+sum(xQ−xP)rPy=yP+sum(yQ−yP)rP

代码实现

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) {//圆心距离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 {//相交则套用公式（上面推导出的）float dr = p2p / 2 + (dis[i] * dis[i] - dis[j] * dis[j]) / (2 * p2p); p.x += ps[i].x + (ps[j].x - ps[i].x)*dr / p2p;p.y += ps[i].y + (ps[j].y - ps[i].y)*dr / p2p;}}}//三个圆两两求点，最终得到三个点，求其均值p.x /= 3;p.y /= 3;return p;
}