三點定位法原理及實現——藍牙定位

前言

最近在做藍牙室內定位，藍牙定位用到的一個非常經典的算法就是三點定位法。

原理

三點定位法，顧名思義首先有三個圓點，同時我們也知道這三個圓的半徑，最終求得三圓的交點，達到定位效果。如圖：

我們的目的是求得O點，利用畢達哥拉斯定理我們可以快速求得O點座標。不過事與願違，現實情況並不總是如人意，而是這樣：

甚至有的情況其中兩個圓或者三個圓完全就不相交，如下圖：

由於這些誤差原因，實際上想要準確的知道使用者當前位置是相當困難的，但是我們可以通過運算得到一個近似解。爲了應付這些情況，我們需從兩個圓入手，先找到兩兩圓之間的中心點，再求三圓的中心點。

除去圓中圓的情況（現實基本上不存在），兩圓關係可分爲兩種，相交，不相交。

兩圓相交

可知此時兩圓交點A，B，我們的目標點是AB中點C。根據勾股定理我們可知
$\begin{cases} \quad PQ=PC+QC\\ \quad QA^2=QC^2+AC^2\\ \quad PA^2=PC^2+AC^2 \end{cases}$
解得：
$PC=\frac{PQ}{2}+\frac{PA^2-QA^2}{2PQ}$
根據比例關係求得座標：
$\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}$

兩圓不相交

如圖，我們需要求O點，簡單的做法就是直接根據半徑比例計算
$\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}$

代碼實現

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