蓝牙多点测距小记

最近在学习蓝牙多点测距的内容，故此记录一下。

基于 RSSI 的距离计算方法

在蓝牙定位技术中，最常用的方法是基于信号强度 RSSI 值，测距方法的本质是通过计算当前环境下信号强度与距离的关系，从信号强度的 RSSI 值计算出两个设备之间的距离。

首先 RSSI 信号的衰减和距离的对数成正比，在最简单的情况下，RSSI 的计算方式可以表示为：

RSSI=TxPower−10∗n∗log(d)RSSI = TxPower - 10 * n * log(d)

转换为距离公式为：

d=10(TxPower−RSSI10∗n)d = 10 ^{(\frac{TxPower - RSSI}{10 * n})}

其中，

  • TxPower - 发射和接收相隔 1 米时的信号强度
  • d - 距离
  • n - 环境衰减因子
  • RSSI - 接收信号强度

使用 C 代码实现：

1
2
3
4
#define ALG_LOG_10_VALUE 2.3026
#define ALG_TXPOWER_1M 50
#define ALG_DECREASE_FACTOR 2
double distance = exp((abs(rssi) - ALG_TXPOWER_1M) / (ALG_DECREASE_FACTOR * 10) * ALG_LOG_10_VALUE);

Or

1
2
3
#define ALG_TXPOWER_1M 50
#define ALG_DECREASE_FACTOR 2
double distance = pow(10, (abs(RSSI) - ALG_TXPOWER_1M) / (ALG_DECREASE_FACTOR * 10));

其中 ALG_TXPOWER_1M 和 ALG_DECREASE_FACTOR 要根据实际情况，调节对应的宏定义参数。

环境衰减因子 n 的经典值：

Environment n
Outdoor Open Space 2
Covered Space 2.7-5
Indoor Open Space 1.6-1.8
Covered Space 4-6

数据来自：https://iopscience.iop.org/article/10.1088/1742-6596/1631/1/012162/pdf

三点定位（三边测量、三角测量）算法

三点定位，是通过已知的三个圆的坐标和半径，求得三个圆的交点，从而达到定位的效果。

在上图中，每个圆代表手机在基站给定距离（半径）处的所有可能位置。三边测量算法的目的是计算三个圆的交点的 (x, y) 坐标。

三个圆的方程为：

(x−x1)2+(y−y1)2=r12(x - x_1)^2 + (y - y_1)^2 = r_1^2

(x−x2)2+(y−y2)2=r22(x - x_2)^2 + (y - y_2)^2 = r_2^2

(x−x3)2+(y−y3)2=r32(x - x_3)^2 + (y - y_3)^2 = r_3^2

将三个方程展开：

  1. x2−2xx1+x12+y2−2yy1+y12=r12x^2 - 2xx_1 + x_1^2 + y^2 - 2yy_1 + y_1^2 = r_1^2

  2. x2−2xx2+x22+y2−2yy2+y22=r22x^2 - 2xx_2 + x_2^2 + y^2 - 2yy_2 + y_2^2 = r_2^2

  3. x2−2xx3+x32+y2−2yy3+y32=r32x^2 - 2xx_3 + x_3^2 + y^2 - 2yy_3 + y_3^2 = r_3^2

方程 1 减去方程 2，方程 2 减去方程 3 后可得：

(−2x1+2x2)x+(−2y1+2y2)y=r12−r22−x12+x22−y12+y22(-2x_1 + 2x_2)x+(-2y_1+2y_2)y=r_1^2-r_2^2-x_1^2+x_2^2-y_1^2+y_2^2

(−2x2+2x3)x+(−2y2+2y3)y=r22−r32−x22+x32−y22+y32(-2x_2 + 2x_3)x+(-2y_2+2y_3)y=r_2^2-r_3^2-x_2^2+x_3^2-y_2^2+y_3^2

由于除了 x 和 y，其他的变量都已知，所以可以将方程简化为：

Ax+By=CAx+By=C

Dx+Ey=FDx+Ey=F

{A=(−2x1+2x2)B=(−2y1+2y2)C=r12−r22−x12+x22−y12+y22D=(−2x2+2x3)E=(−2y2+2y3)F=r22−r32−x22+x32−y22+y32\begin{cases} A = (-2x_1 + 2x_2) \\ B = (-2y_1 + 2y_2) \\ C = r_1^2 - r_2^2 - x_1^2 + x_2^2 - y_1^2 + y_2^2 \\ D = (-2x_2 + 2x_3) \\ E = (-2y_2 + 2y_3) \\ F = r_2^2 - r_3^2 - x_2^2 + x_3^2 - y_2^2 + y_3^2 \\ \end{cases}

最后，由已知的条件可以得出：

x=CE−FBEA−BDx = \frac{CE-FB}{EA-BD}

y=CD−AFBD−AEy = \frac{CD-AF}{BD-AE }

由上面的方程总结完可以得到对应的计算接口：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
typedef struct
{
    int x;
    int y;
} POINT_T;

typedef struct
{
    POINT_T center;
    int r;
} CIRCLE_T;

POINT_T calculate_circle_intersection(CIRCLE_T c1, CIRCLE_T c2, CIRCLE_T c3)
{
    POINT_T point;
    int A, B, C, D, E, F;
    A = 2 * c2.center.x - 2 * c1.center.x;
    B = 2 * c2.center.y - 2 * c1.center.y;
    C = c1.r * c1.r - c2.r * c2.r - c1.center.x * c1.center.x + c2.center.x * c2.center.x - c1.center.y * c1.center.y + c2.center.y * c2.center.y;
    D = 2 * c3.center.x - 2 * c2.center.x;
    E = 2 * c3.center.y - 2 * c2.center.y;
    F = c2.r * c2.r - c3.r * c3.r - c2.center.x * c2.center.x + c3.center.x * c3.center.x - c2.center.y * c2.center.y + c3.center.y * c3.center.y;

    point.x = (C * E - F * B) / (E * A - B * D);
    point.y = (C * D - A * F) / (B * D - A * E);

    return point;
}