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

　

三点定位

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

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

前言

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

原理

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

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

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

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

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

两圆相交

可知此时两圆交点A，B，我们的目标点是AB中点C。根据勾股定理我们可知

$\{\begin{array}{c}PQ=PC+QC\\ Q{A}^2=Q{C}^2+A{C}^2\\ P{A}^2=P{C}^2+A{C}^2\end{array}$⎩⎪⎨⎪⎧​PQ=PC+QCQA2=QC2+AC2PA2=PC2+AC2​

解得：

$PC=\frac{PQ}{2}+\frac{P{A}^2-Q{A}^2}{2PQ}$PC=2PQ​+2PQPA2−QA2​

根据比例关系求得坐标：

$\{\begin{array}{c}x=x_P+\frac{(x_Q-x_P)PC}{PQ}\\ \\ y=y_P+\frac{(y_Q-y_P)PC}{PQ}\end{array}$⎩⎪⎨⎪⎧​x=xP​+PQ(xQ​−xP​)PC​y=yP​+PQ(yQ​−yP​)PC​​

两圆不相交

如图，我们需要求O点，简单的做法就是直接根据半径比例计算

$\{\begin{array}{c}sum=r_P+r_Q\\ \\ x=x_P+\frac{(x_Q-x_P)r_P}{sum}\\ \\ y=y_P+\frac{(y_Q-y_P)r_P}{sum}\end{array}$⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧​sum=rP​+rQ​x=xP​+sum(xQ​−xP​)rP​​y=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;
}

相关阅读

基于LocationManager的定位工具类

现在开发的项目中经常用到定位功能，如果每次都去新写一个监听，太过于繁琐和增加代码重复量，于是写一个定位工具类。首先，定位的基本流

使用百度地图api模拟实时定位页面 完整示例

使用百度地图api模拟实时定位页面 完整示例 效果：使用百度地图api在页面上显示车辆的实时位置，并有自动刷新和手动刷新两种方

重读特劳特之定位（一）：三步构建产品运营战略

进行战略定位，只有站在更高的层面先去理解这件事，才能更好的具备战略思维，将计划落地。最近在网上看到不少文章，从产品架构上对各大知

语音识别（二）——基本框架, Microphone Array, 声源定位

基本框架 语音识别系统主要有四部分组成：信号处理和特征提取、声学模型、语言模型（Language Model, LM）和解码器(Decoder)。 信号

时间同步：适用于国内的 NTP 服务器地址，可用于时间同步

转载自：http://blog.csdn.net/maxsky/article/details/53866475 NTP 是什么？   NTP 是网络时间协议（Network Time Protocol），它用来