室内定位算法入门篇

室内定位算法入门篇

  • 前言
    • 注意
  • 你需要懂的基础知识
    • 平面坐标系
    • 概率
    • 欧氏距离
  • 算法
    • 惯性导航
    • 蓝牙定位
    • 指纹法
    • 定位融合
  • 总结

前言

貌似今年做室内定位的人多起来了,总有人问我一些非常基础问题,一方面有的人确实是零基础,另一方面我之前写的一些文章也确实没说的很详细,这里就弄个入门篇。

这里写的算法和代码,我会尽量使用用最最简单的,可能在实际运用中并不是那么实用,但是可以帮助零基础的朋友快速入门。

注意

俗话说的好,师父领进门,修行在个人。如果有朋友是想把代码抄过去直接拿来用,那么你可能会失望的。

你需要懂的基础知识

平面坐标系

坐标系咱们小学数学肯定都学过的,这个相信不用我多说了吧。
室内定位算法入门篇_第1张图片
我这里就用C语言写个类型Position,分别有两个参数x和y,代表平面坐标系的坐标,表示如下:

struct Position{
	int x;
	int y;
}

概率

这个应该是高中知识,这里给各位回顾一下。

一个袋子里有五个球,三个白球,两个红球。从中取出一个球,取到白球的概率是多少?取到红球的概率是多少?
答案:白球0.6,红球0.4

另外,所有事件的概率和等于1。大概只需要知道这么多就够了。

欧氏距离

听名字挺玄乎的,其实还是咱们小学知识。

欧式距离放在二维、三维坐标系里,就是两点之间的直线距离。举个例子,小明在 ( x 1 , y 1 , z 1 ) (x_1,y_1,z_1) (x1,y1,z1),小明他爸在 ( x 2 , y 2 , z 2 ) (x_2,y_2,z_2) (x2,y2,z2),小明和他爸的欧式距离就是:
d = ( x 1 − x 2 ) 2 + ( y 1 − y 2 ) 2 + ( z 1 − z 2 ) 2 d=\sqrt{(x_1-x_2)^2+(y_1-y_2)^2+(z_1-z_2)^2} d=(x1x2)2+(y1y2)2+(z1z2)2
所以欧氏距离的本质就是任意维度中两个点之间的距离。

算法

惯性导航

假设小明在一个平面坐标系的原点 ( 0 , 0 ) (0,0) (0,0)上,他向北( 90 ° 90\degree 90°)走了10米,又向东( 0 ° 0\degree 0°)走了10米,咱们都知道他现在应该在 ( 10 , 10 ) (10,10) (10,10)的位置上对吧。那么我们就先用代码来表示小明是怎么移动的:

//oldPosition表示移动前的坐标
//distace表示移动的距离
//angle表示移动的方向
Position getNewPosition(Position oldPosition,float distance,float angle){
	Position newPosition;
	
	newPosition.x = oldPosition.x + distance.x * cos(angle);
	newPosition.y = oldPosition.y + distance.y * sin(angle);
	
	return newPosition;
}

然后把小明的移动过程用代码描述一遍(下面的 P I = π PI=\pi PI=π,代码里面打不出来,这里说明下):

static float PI = 3.141592;

void XiaoMingMove(){
	//初始化坐标起点(0,0)
	Position position;
	position.x=0;
	position.y=0;
	
	//向北走10米,北的方向在坐标系里是90度,转换成弧度也就是PI/2
	position=getNewPosition(position,10,PI/2);
	
	//向东走10米,东的方向在坐标系里是0度,弧度就是0
	position=getNewPosition(position,10,0);
}

对,这玩意就可以称之为惯性导航了。简而言之,知道了小明前进的距离和方向,就能根据移动前的位置计算出小明移动后所在的位置。

蓝牙定位

从理论上来说,蓝牙定位是有唯一解的,但是实际运用中由于信号误差比较大,按照理论方法计算出来的结果可信度往往不高,所有现在一般的做法是将其转换为一个概率问题

举个例子,小明在一个三角形中,假设三角形的顶点分别是 A ( x A , y A ) A(x_A,y_A) A(xA,yA) B ( x B , y B ) B(x_B,y_B) B(xB,yB) C ( x C , y C ) C(x_C,y_C) C(xC,yC)

同时告诉你小明在A点的概率是0.6,在B点的概率是0.3,在C点的概率是0.1,要求计算出小明所在的位置。一般来说有两种解决方案:

  1. 在A点的概率最大,就直接选择A点
    { x = x A y = y A \begin{cases} x=x_A \\ y=y_A \end{cases} {x=xAy=yA
  2. 根据概率来计算坐标。这是啥意思呢,本质就是求个均值:
    { x = 0.6 x A + 0.3 x B + 0.1 x C y = 0.6 y A + 0.3 y B + 0.1 y C \begin{cases} x=0.6x_A+0.3x_B+0.1x_C \\ y=0.6y_A+0.3y_B+0.1y_C \end{cases} {x=0.6xA+0.3xB+0.1xCy=0.6yA+0.3yB+0.1yC

不用多说,肯定是方案二相对更准确些。

蓝牙定位的本质就是这么个概率问题,那么可能有人(谁!?)就要问了,这概率怎么求呢?现在市面上的蓝牙或者也叫ibeancon,在连接端(比如你的手机)是可以计算出蓝牙的信号强度(又叫RSSI)的,我们可以认为蓝牙信号强度越强,当前蓝牙的概率就越大,信号强度越弱,当前蓝牙的概率就越小

另外,通过RSSI在接收端可以近似计算出当前位置距离蓝牙的距离,这里我并不想提供相应的转换公式,因为这个介绍需要考虑到一些蓝牙实际运用中的参数,介于篇幅有限,各位自行了解吧(可以参考这篇文章)。

我这里重点介绍如何将得到的蓝牙距离如何转换为概率。还是用刚才三角形,已知小明根据蓝牙信号测出自己到三个蓝牙的距离分别为 s A s_A sA s B s_B sB s C s_C sC。距离越小,信号越强,概率就越大,说明距离和概率是反比关系。然而具体是个怎么样的反比关系,其实我也不清楚,得根据实践来摸索。反正既然是反比关系,这里简单点,就让其为倒数关系:
p = 1 s p=\frac{1}{s} p=s1
(当然也可以是 1 / s 2 1/s^2 1/s2 1 / s 1/\sqrt{s} 1/s 等)。另外,假设有
s u m = 1 1 s A + 1 s B + 1 s C sum = \frac{1}{\frac{1}{s_A}+\frac{1}{s_B}+\frac{1}{s_C}} sum=sA1+sB1+sC11

三个点的概率计算式为:
{ p A = 1 s A 1 s u m p B = 1 s B 1 s u m p C = 1 s C 1 s u m \begin{cases} p_A=\frac{1}{s_A} \frac{1}{sum} \\ p_B=\frac{1}{s_B} \frac{1}{sum} \\ p_C=\frac{1}{s_C} \frac{1}{sum} \end{cases} pA=sA1sum1pB=sB1sum1pC=sC1sum1
为什么要除上 s u m sum sum呢?因为概率之和需要等于1,因此这个处理过程又叫做归一化处理。

代码实现一哈。

//poses蓝牙坐标数组
//dises蓝牙距离数组
//bt_num蓝牙数量
Position getBTPosition(Position[] poses,float[] dises,const int bt_num){
	//初始化坐标(0,0)
	Position position;
	position.x=position.y=0;
	
	//计算sum
	float sum=0;
	for(int i=0;i<bt_num;++i){
		sum += 1/dises[i];
	}
	
	//计算每个蓝牙的概率
	float p[bt_num]={0};
	for(int i=0;i<bt_num;++i){
		p[i] = 1 / (dises[i] * sum);
	}
	
	//根据概率求得当前位置
	for(int i=0;i<bt_num;++i){
		position.x += poses[i].x * p[i];
		position.y += poses[i].y * p[i];
	}

	return position;
}

有的朋友跟我说没学过C语言,这里我就尽量使用通用的语法来描述,众口难调,如果还看不懂的话我也是没辙了。另外,蓝牙的位置怎么得到,由于每个蓝牙具有唯一标识号,可以通过唯一标识在数据库里进行查找坐标。这里吐槽一句,居然还有人问我数据库里的坐标哪来的,你问安装蓝牙的师傅啊,蓝牙安在哪就是哪了。

指纹法

指纹法是一类算法的统称,比如人脸识别就是一种指纹法,相机先拍了你的脸,然后把你的脸放到数据库里一一进行对比,最后找到了你,或者找错找到了别人。

这里讲一种最简单的指纹法,就是对比数据之间的欧式距离,找到最短欧氏距离所在的点,就找到了定位位置。

举个例子,还是在三角形内,

作为指纹法,肯定需要在定位之前先采集数据(就跟人脸识别一样,得先把脸记录下来),假设我们的设备在三角形的三个点采集的数据(这里的数据是个抽象值,可以是磁场或者其他数据)分别为: D A ( a 1 , a 2 , a 3 ) D_A(a_1,a_2,a_3) DA(a1,a2,a3) D B ( b 1 , b 2 , b 3 ) D_B(b_1,b_2,b_3) DB(b1,b2,b3) D C ( c 1 , c 2 , c 3 ) D_C(c_1,c_2,c_3) DC(c1,c2,c3),这个时候,小明在三角形的某处采集的信号为 D X M ( x 1 , x 2 , x 3 ) D_{XM}(x_1,x_2,x_3) DXM(x1,x2,x3)

由于没有具体得数值,我们就假设其中欧式距离最小的点为B,那么就可以认为小明此时在B点处。

代码实现一下

//poses信号采集点的坐标数组
//datas信号采集点的数据集合
//data当前位置采集的信号数据
//bt_num信号点的数量
Position getPosition(Position poses[],float datas[][],float data[],int bt_num){
	//保存最小欧式距离
	float min_d = -1;
	//保存最小欧式距离所在的点的下标
	int index = 0;
	
	//寻找最小欧氏距离
	for(int i=0; i < bt_num; ++i){
		//计算欧氏距离,根据上面得描述,每个点采集得数据维度是3,
		float d=sqrt(
			(datas[i][0]-data[i][0]) * (datas[i][0]-data[i][0]) +
			(datas[i][1]-data[i][1]) * (datas[i][1]-data[i][1]) +
			(datas[i][2]-data[i][2]) * (datas[i][2]-data[i][2])
		);
		
		//判断是否是小于当前记录最小的欧氏距离
		if(min_d < 0 || d < min_d){
			index = i;
			min_d = d;
		}
	}
	
	return poses[index];
}

这个同样是给大家做个示范,实际运用效果如何,就不好说了,一般来说,采集数据的维度越多,指纹法匹配的精度就越高。

定位融合

融合本质上是指将两种定位方式结合在一起,具体怎么结合,方法其实很多种。

举个例子,我现在得到了两种定位方法的坐标 P 1 ( x 1 , y 1 ) P_1(x_1,y_1) P1(x1,y1) P 2 ( x 2 , y 2 ) P_2(x_2,y_2) P2(x2,y2),我假设定位方法一的可信度为0.6,方法二的可信度为0.4,那么融合的坐标为:
{ x = 0.6 x 1 + 0.4 x 2 y = 0.6 y 1 + 0.4 y 2 \begin{cases} x=0.6x_1+0.4x_2 \\ y=0.6y_1+0.4y_2 \end{cases} {x=0.6x1+0.4x2y=0.6y1+0.4y2
so easy!当然这只是一种最简单的融合策略,根据每种定位方式的特性,我们可以采取不同的融合方式,这就需要各位自行摸索了。

总结

这篇文章基本上囊括了最基础的定位算法,由于室内定位这个行业没有太多的理论体系支撑,大部分的时候往往需要自己来摸索,而且每个人的做法不同,这篇文章就当是抛砖引玉了。写的时候比较匆忙,如果各位有什么疑虑或者文章中有什么不对的地方,欢迎留言。

你可能感兴趣的:(蓝牙定位)