【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?=R02?x2?x12??2xx1?+y2?y12??2yy1?+z2?z12??2zz1?=R12?x2?x22??2xx2?+y2?y22??2yy2?+z2?z22??2zz2?=R22?x2?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=λ1?2(x0??x2?)x+2(y0??y2?)y+2(z0??z2?)z=λ2?2(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