前言
最近在做蓝牙室内定位,蓝牙定位用到的一个非常经典的算法就是三点定位法。
原理
三点定位法,顾名思义首先有三个圆点,同时我们也知道这三个圆的半径,最终求得三圆的交点,达到定位效果。如图:
我们的目的是求得O点,利用毕达哥拉斯定理我们可以快速求得O点坐标。不过事与愿违,现实情况并不总是如人意,而是这样:
甚至有的情况其中两个圆或者三个圆完全就不相交,如下图:
由于这些误差原因,实际上想要准确的知道使用者当前位置是相当困难的,但是我们可以通过运算得到一个近似解。为了应付这些情况,我们需从两个圆入手,先找到两两圆之间的中心点,再求三圆的中心点。
除去圆中圆的情况(现实基本上不存在),两圆关系可分为两种,相交,不相交。
两圆相交
可知此时两圆交点A,B,我们的目标点是AB中点C。根据勾股定理我们可知
{
P
Q
=
P
C
+
Q
C
Q
A
2
=
Q
C
2
+
A
C
2
P
A
2
=
P
C
2
+
A
C
2
\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
解得:
P
C
=
P
Q
2
+
P
A
2
−
Q
A
2
2
P
Q
PC=\frac{PQ}{2}+\frac{PA^2-QA^2}{2PQ}
PC=2PQ+2PQPA2−QA2
根据比例关系求得坐标:
{
x
=
x
P
+
(
x
Q
−
x
P
)
P
C
P
Q
y
=
y
P
+
(
y
Q
−
y
P
)
P
C
P
Q
\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点,简单的做法就是直接根据半径比例计算
{
s
u
m
=
r
P
+
r
Q
x
=
x
P
+
(
x
Q
−
x
P
)
r
P
s
u
m
y
=
y
P
+
(
y
Q
−
y
P
)
r
P
s
u
m
\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;
}