【UWB】公式推导计算坐标值
假设基站 A0A0A0 的坐标为（x0，y1，z1x_0，y_1，z_1x0，y1，z1），基站 A1A1A1 的坐标为（x2，y2，z2x_2，y_2，z_2x2，y2，z2），基站 A2A2A2 的坐标为（x3，y3，z3x_3，y_3，z_3x3，y3，z3）,基站 A3A3A3 的坐标为（x4，y4，z4x_4，y_4，z_4x4，y4，z4）,需要求解的标签坐标为（x，y，zx，y，zx，y，z），则有：
(x−x0)2+(y−y0)2+(z−z0)2=R02(x−x1)2+(y−y1)2+(z−z1)2=R12(x−x2)2+(y−y2)2+(z−z2)2=R22(x−x3)2+(y−y3)2+(z−z3)2=R32\begin{aligned} (x - x_0)^2 + (y - y_0)^2 + (z - z_0)^2 = R_0^2 \\ (x - x_1)^2 + (y - y_1)^2 + (z - z_1)^2 = R_1^2 \\ (x - x_2)^2 + (y - y_2)^2 + (z - z_2)^2 = R_2^2 \\ (x - x_3)^2 + (y - y_3)^2 + (z - z_3)^2 = R_3^2 \\ \end{aligned}(x−x0)2+(y−y0)2+(z−z0)2=R02(x−x1)2+(y−y1)2+(z−z1)2=R12(x−x2)2+(y−y2)2+(z−z2)2=R22(x−x3)2+(y−y3)2+(z−z3)2=R32
其中，R1,R2,R3,R4R_1, R_2, R_3, R_4R1,R2,R3,R4 分别是基站测得的与标签的距离。
将这些式子展开得到：
x2−x02−2xx0+y2−y02−2yy0+z2−z02−2zz0=R02x2−x12−2xx1+y2−y12−2yy1+z2−z12−2zz1=R12x2−x22−2xx2+y2−y22−2yy2+z2−z22−2zz2=R22x2−x32−2xx3+y2−y32−2yy3+z2−z32−2zz3=R32\begin{aligned} x^2 - x_0^2 - 2 x x_0 + y^2 - y_0^2 - 2 y y_0 + z^2 - z_0^2 - 2 z z_0 = R_0^2 \\ x^2 - x_1^2 - 2 x x_1 + y^2 - y_1^2 - 2 y y_1 + z^2 - z_1^2 - 2 z z_1 = R_1^2 \\ x^2 - x_2^2 - 2 x x_2 + y^2 - y_2^2 - 2 y y_2 + z^2 - z_2^2 - 2 z z_2 = R_2^2 \\ x^2 - x_3^2 - 2 x x_3 + y^2 - y_3^2 - 2 y y_3 + z^2 - z_3^2 - 2 z z_3 = R_3^2 \\ \end{aligned}x2−x02−2xx0+y2−y02−2yy0+z2−z02−2zz0=R02x2−x12−2xx1+y2−y12−2yy1+z2−z12−2zz1=R12x2−x22−2xx2+y2−y22−2yy2+z2−z22−2zz2=R22x2−x32−2xx3+y2−y32−2yy3+z2−z32−2zz3=R32
第 2, 3, 4 行的式子各自减去第 1 行式子，得到：
2(x0−x1)x+2(y0−y1)y+2(z0−z1)z=λ12(x0−x2)x+2(y0−y2)y+2(z0−z2)z=λ22(x0−x3)x+2(y0−y3)y+2(z0−z3)z=λ3\begin{aligned} 2(x_0 - x_1) x + 2(y_0 - y_1) y + 2(z_0 - z_1) z = \lambda_1 \\ 2(x_0 - x_2) x + 2(y_0 - y_2) y + 2(z_0 - z_2) z = \lambda_2 \\ 2(x_0 - x_3) x + 2(y_0 - y_3) y + 2(z_0 - z_3) z = \lambda_3 \\ \end{aligned}2(x0−x1)x+2(y0−y1)y+2(z0−z1)z=λ12(x0−x2)x+2(y0−y2)y+2(z0−z2)z=λ22(x0−x3)x+2(y0−y3)y+2(z0−z3)z=λ3
其中，
λ1=R12−R02−x12+x02−y12+y02−z12+z02λ2=R22−R12−x22+x12−y22+y12−z22+z12λ3=R32−R22−x32+x22−y32+y22−z32+z22\begin{aligned} \lambda_1 = R_1^2 - R_0^2 - x_1^2 + x_0^2 - y_1^2 + y_0^2 - z_1^2 + z_0^2 \\ \lambda_2 = R_2^2 - R_1^2 - x_2^2 + x_1^2 - y_2^2 + y_1^2 - z_2^2 + z_1^2 \\ \lambda_3 = R_3^2 - R_2^2 - x_3^2 + x_2^2 - y_3^2 + y_2^2 - z_3^2 + z_2^2 \\ \end{aligned}λ1=R12−R02−x12+x02−y12+y02−z12+z02λ2=R22−R12−x22+x12−y22+y12−z22+z12λ3=R32−R22−x32+x22−y32+y22−z32+z22
把这些式子转换为矩阵相乘的形式（有关矩阵的知识可以百度一下，知识过多，这里不便详说）：
[2(x0−x1)2(y0−y1)2(z0−z1)2(x0−x2)2(y0−y2)2(z0−z2)2(x0−x3)2(y0−y3)2(z0−z3)][xyz]=[λ1λ2λ3]\begin{aligned} \left[\begin{matrix} 2(x_0 - x_1) & 2(y_0 - y_1) & 2(z_0 - z_1) \\ 2(x_0 - x_2) & 2(y_0 - y_2) & 2(z_0 - z_2) \\ 2(x_0 - x_3) & 2(y_0 - y_3) & 2(z_0 - z_3) \\ \end{matrix}\right] \left[\begin{matrix} x \\ y \\ z \\ \end{matrix}\right] = \left[\begin{matrix} \lambda_1 \\ \lambda_2 \\ \lambda_3 \\ \end{matrix}\right] \end{aligned}⎣⎡2(x0−x1)2(x0−x2)2(x0−x3)2(y0−y1)2(y0−y2)2(y0−y3)2(z0−z1)2(z0−z2)2(z0−z3)⎦⎤⎣⎡xyz⎦⎤=⎣⎡λ1λ2λ3⎦⎤
总结
以上是生活随笔为你收集整理的【UWB】公式推导计算坐标值的全部内容，希望文章能够帮你解决所遇到的问题。
- 上一篇： 【UGV】小车一些图片
- 下一篇： 【UWB】ELM，Extreme Lea