移动自组网络的时间校准与空间定位（MATLAB仿真）
文章目录
0 问题
0.1 问题描述
对于空间分布的 N N N个节点，且它们存在时间同步误差。现考虑从它们的相互测距数据中求得各节点的空间位置坐标。
0.2 假设
① 所有节点均可相互通信；
② 在足够短的时间内，各节点的时间同步误差保持不变；
1 时间校准
1.1 数据组织格式（邻接矩阵）
在空间中各节点间的距离可由邻接矩阵表示为：
R
=
[
R
11
R
12
R
13
.
.
.
R
1
N
R
21
R
22
R
23
.
.
.
R
2
N
R
31
R
32
R
33
.
.
.
R
3
N
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
R
N
1
R
N
2
R
N
3
.
.
.
R
N
N
]
\mathbf{R} = \begin{bmatrix} R_{11}&R_{12}&R_{13}&...&R_{1N}\\ R_{21}&R_{22}&R_{23}&...&R_{2N}\\ R_{31}&R_{32}&R_{33}&...&R_{3N}\\ ...&...&...&...&...\\ R_{N1}&R_{N2}&R_{N3}&...&R_{NN}\\ \end{bmatrix}
R=⎣⎢⎢⎢⎢⎡R11R21R31...RN1R12R22R32...RN2R13R23R33...RN3...............R1NR2NR3N...RNN⎦⎥⎥⎥⎥⎤
其中，
R
i
j
R_{ij}
Rij 表示节点
i
i
i 与节点
j
j
j 间的距离，且有
R
i
j
=
R
j
i
R_{ij}=R_{ji}
Rij=Rji ，即矩阵
R
\bf{R}
R 为对角线为 0 的对称矩阵。
1.2 时间同步误差求解与消除
对于空间中分布的节点 i i i 与节点 j j j ，它们的实际距离为 R i j = R j i {R_{ij}} = {R_{ji}} Rij=Rji，它们之间的时间同步误差为 Δ t \Delta t Δt ，则：
从节点
i
i
i 测量得到的两点距离为 :
R
~
i
j
=
R
i
j
+
Δ
t
×
c
{\tilde R_{ij}} = {R_{ij}} + \Delta t \times c
R~ij=Rij+Δt×c
从节点
j
j
j 测量得到的两点距离为 :
R
~
j
i
=
R
j
i
−
Δ
t
×
c
{\tilde R_{ji}} = {R_{ji}} - \Delta t \times c
R~ji=Rji−Δt×c
其中， c c c 为波速。
（1）时间同步误差求解
只需利用节点间的相互测距数据，便可获得各节点间的时间同步误差为：
Δ
t
=
R
~
i
j
−
R
~
j
i
2
×
c
\Delta t = \frac{{{{\tilde R}_{ij}} - {{\tilde R}_{ji}}}}{{2 \times c}}
Δt=2×cR~ij−R~ji
利用邻接矩阵，可得各节点间的时间同步误差矩阵为：
Δ
T
=
1
2
c
(
R
~
−
R
~
T
)
{\bf{\Delta T}} = \frac{1}{{2c}}\left( {{\bf{\tilde R}} - {{{\bf{\tilde R}}}^T}} \right)
ΔT=2c1(R~−R~T)
其中，$ {\bf{\Delta T}} $ 为时间同步误差矩阵
（2）时间同步误差消除
因此，在不考虑测量误差的情况下，即可由测量距离
R
i
j
R_{ij}
Rij 与
R
j
i
R_{ji}
Rji 求得真实距离为：
R
i
j
=
R
~
i
j
−
Δ
t
×
c
=
R
~
i
j
−
R
~
i
j
−
R
~
j
i
2
=
R
~
i
j
+
R
~
j
i
2
{R_{ij}} = {\tilde R_{ij}} - \Delta t \times c = {\tilde R_{ij}} - \frac{{{{\tilde R}_{ij}} - {{\tilde R}_{ji}}}}{2} = \frac{{{{\tilde R}_{ij}} + {{\tilde R}_{ji}}}}{2}
Rij=R~ij−Δt×c=R~ij−2R~ij−R~ji=2R~ij+R~ji
则可得消除各节点间的时间同步误差后的邻接矩阵为：
R
=
1
2
(
R
~
+
R
~
T
)
{\bf{R}} = \frac{1}{2}\left( {{\bf{\tilde R}} + {{{\bf{\tilde R}}}^T}} \right)
R=21(R~+R~T)
2 移动自组网络局部定位
2.1 坐标原点设置
在所有节点中任取一点（不妨取第1个节点）作为参考节点（坐标原点），其坐标为 P 1 = ( 0 , 0 , 0 ) P_1=(0,0,0) P1=(0,0,0) 。
2.2 x轴设置
在设定参考节点（坐标原点）后，任取另一节点（不妨取第2个节点），令原点与该节点的连线为 x 轴，则该节点的坐标为 P 2 = ( x 2 , 0 , 0 ) = ( R 12 , 0 , 0 ) P_2=(x_{2},0,0)=(R_{12},0,0) P2=(x2,0,0)=(R12,0,0)
2.3 y轴设置（平面的两点定位方法）
在其他剩余节点中选择一个与x轴不共线的节点，将与x轴垂直且指向该节点的方向定为y轴，并构成一个平面（不妨设为第3个节点），由 P 1 P_1 P1与 P 2 P_2 P2 定位 P 3 P_3 P3（ P 3 P_3 P3取 y 3 > 0 y_3>0 y3>0 的解）
可列方程组：
x
3
2
+
y
3
2
=
R
13
2
(
x
3
−
x
2
)
2
+
y
3
2
=
R
23
2
\begin{aligned} &x_3^2 + y_3^2 = R_{13}^2 \\ &{\left( {{x_3} - {x_2}} \right)^2} + y_3^2 = R_{23}^2 \end{aligned}
x32+y32=R132(x3−x2)2+y32=R232
解得
P
3
P_3
P3 坐标为：
x
3
=
R
13
2
−
R
23
2
+
x
2
2
2
x
2
y
3
=
R
13
2
−
x
3
2
\begin{aligned} {x_3} &= \frac{{R_{13}^2 - R_{23}^2 + x_2^2}}{{2{x_2}}} \\ {y_3} &= \sqrt {R_{13}^2 - x_3^2} \end{aligned}
x3y3=2x2R132−R232+x22=R132−x32
2.4 z轴设置（空间的三点定位方法）
在其他剩余节点中选择一个与xoy不共面的节点，将与xoy平面垂直且指向该节点（不妨设为第4个节点）的方向定为z轴，由 P 1 P_1 P1、 P 2 P_2 P2 与 P 3 P_3 P3 定位 P 4 P_4 P4（ P 4 P_4 P4取 z 4 > 0 z_4>0 z4>0 的解）
可列方程组：
x
4
2
+
y
4
2
+
z
4
2
=
R
14
2
(
x
4
−
x
2
)
2
+
y
4
2
+
z
4
2
=
R
24
2
(
x
4
−
x
3
)
2
+
(
y
4
−
y
3
)
2
+
z
2
=
R
34
2
\begin{aligned} &x_4^2 + y_4^2 + z_4^2 = R_{14}^2 \\ &{\left( {{x_4} - {x_2}} \right)^2} + y_4^2 + z_4^2 = R_{24}^2 \\ & {\left( {{x_4} - {x_3}} \right)^2} + {\left( {{y_4} - {y_3}} \right)^2} + {z^2} = R_{34}^2 \end{aligned}
x42+y42+z42=R142(x4−x2)2+y42+z42=R242(x4−x3)2+(y4−y3)2+z2=R342
解得
P
4
P_4
P4 坐标为：
x
4
=
R
14
2
−
R
24
2
+
x
2
2
2
x
2
y
4
=
(
R
14
2
−
x
4
2
)
−
(
R
34
2
−
(
x
4
−
x
3
)
2
)
+
y
3
2
2
y
3
z
4
=
R
14
2
−
x
4
2
−
y
4
2
\begin{aligned} &{x_4} = \frac{{R_{14}^2 - R_{24}^2 + x_2^2}}{{2{x_2}}} \\ &{y_4} = \frac{{\left( {R_{14}^2 - x_4^2} \right) - \left( {R_{34}^2 - {{\left( {{x_4} - {x_3}} \right)}^2}} \right) + y_3^2}}{{2{y_3}}} \\ &z_4 = \sqrt {R_{14}^2 - x_4^2 - y_4^2} \end{aligned}
x4=2x2R142−R242+x22y4=2y3(R142−x42)−(R342−(x4−x3)2)+y32z4=R142−x42−y42
2.5 线性化的空间定位算法（四点定位法）
在三维空间中，由三个节点的测距可实现对任意节点的空间定位。但是，当三个参考节点不包含原点与坐标轴上的点时，将涉及到求解三元二次方程组，复杂度高。
因此，考虑三维空间的四点定位算法，它利用距离差实现定位，将三元二次方程组转化为线性方程组，复杂度低。
对于已知坐标的四个节点
P
1
P_1
P1~
P
4
P_4
P4，待定位节点为
P
5
P_5
P5，可利用三点定位方法列方程组如下：
(
x
5
−
x
1
)
2
+
(
y
5
−
y
1
)
2
+
(
z
5
−
z
1
)
2
=
R
15
2
(
x
5
−
x
2
)
2
+
(
y
5
−
y
2
)
2
+
(
z
5
−
z
2
)
2
=
R
25
2
(
x
5
−
x
3
)
2
+
(
y
5
−
y
3
)
2
+
(
z
5
−
z
3
)
2
=
R
35
2
(
x
5
−
x
4
)
2
+
(
y
5
−
y
4
)
2
+
(
z
5
−
z
4
)
2
=
R
45
2
\begin{aligned} {\left( {{x_5} - {x_1}} \right)^2} + {\left( {{y_5} - {y_1}} \right)^2} + {\left( {{z_5} - {z_1}} \right)^2} = R_{15}^2 \\ {\left( {{x_5} - {x_2}} \right)^2} + {\left( {{y_5} - {y_2}} \right)^2} + {\left( {{z_5} - {z_2}} \right)^2} = R_{25}^2 \\ {\left( {{x_5} - {x_3}} \right)^2} + {\left( {{y_5} - {y_3}} \right)^2} + {\left( {{z_5} - {z_3}} \right)^2} = R_{35}^2 \\ {\left( {{x_5} - {x_4}} \right)^2} + {\left( {{y_5} - {y_4}} \right)^2} + {\left( {{z_5} - {z_4}} \right)^2} = R_{45}^2 \end{aligned}
(x5−x1)2+(y5−y1)2+(z5−z1)2=R152(x5−x2)2+(y5−y2)2+(z5−z2)2=R252(x5−x3)2+(y5−y3)2+(z5−z3)2=R352(x5−x4)2+(y5−y4)2+(z5−z4)2=R452
展开平方项可得：
x
5
2
+
x
1
2
−
2
x
5
x
1
+
y
5
2
+
y
1
2
−
2
y
5
y
1
+
z
5
2
+
z
1
2
−
2
z
5
z
1
=
R
15
2
x
5
2
+
x
2
2
−
2
x
5
x
2
+
y
5
2
+
y
2
2
−
2
y
5
y
2
+
z
5
2
+
z
2
2
−
2
z
5
z
2
=
R
25
2
x
5
2
+
x
3
2
−
2
x
5
x
3
+
y
5
2
+
y
3
2
−
2
y
5
y
3
+
z
5
2
+
z
3
2
−
2
z
5
z
3
=
R
35
2
x
5
2
+
x
4
2
−
2
x
5
x
4
+
y
5
2
+
y
4
2
−
2
y
5
y
4
+
z
5
2
+
z
4
2
−
2
z
5
z
4
=
R
45
2
\begin{aligned} x_5^2 + x_1^2 - 2{x_5}{x_1} + y_5^2 + y_1^2 - 2{y_5}{y_1} + z_5^2 + z_1^2 - 2{z_5}{z_1} = R_{15}^2 \\ x_5^2 + x_2^2 - 2{x_5}{x_2} + y_5^2 + y_2^2 - 2{y_5}{y_2} + z_5^2 + z_2^2 - 2{z_5}{z_2} = R_{25}^2 \\ x_5^2 + x_3^2 - 2{x_5}{x_3} + y_5^2 + y_3^2 - 2{y_5}{y_3} + z_5^2 + z_3^2 - 2{z_5}{z_3} = R_{35}^2 \\ x_5^2 + x_4^2 - 2{x_5}{x_4} + y_5^2 + y_4^2 - 2{y_5}{y_4} + z_5^2 + z_4^2 - 2{z_5}{z_4} = R_{45}^2 \\ \end{aligned}
x52+x12−2x5x1+y52+y12−2y5y1+z52+z12−2z5z1=R152x52+x22−2x5x2+y52+y22−2y5y2+z52+z22−2z5z2=R252x52+x32−2x5x3+y52+y32−2y5y3+z52+z32−2z5z3=R352x52+x42−2x5x4+y52+y42−2y5y4+z52+z42−2z5z4=R452
再用第一式与各式差分有：
2
(
x
1
−
x
2
)
x
5
+
2
(
y
1
−
y
2
)
y
5
+
2
(
z
1
−
z
2
)
z
5
=
R
25
2
−
R
15
2
−
x
2
2
+
x
1
2
−
y
2
2
+
y
1
2
−
z
2
2
+
z
1
2
2
(
x
1
−
x
3
)
x
5
+
2
(
y
1
−
y
3
)
y
5
+
2
(
z
1
−
z
3
)
z
5
=
R
35
2
−
R
15
2
−
x
3
2
+
x
1
2
−
y
3
2
+
y
1
2
−
z
3
2
+
z
1
2
2
(
x
1
−
x
4
)
x
5
+
2
(
y
1
−
y
4
)
y
5
+
2
(
z
1
−
z
4
)
z
5
=
R
45
2
−
R
15
2
−
x
4
2
+
x
1
2
−
y
4
2
+
y
1
2
−
z
4
2
+
z
1
2
\begin{aligned} 2\left( {{x_1} - {x_2}} \right){x_5} + 2\left( {{y_1} - {y_2}} \right){y_5} + 2\left( {{z_1} - {z_2}} \right){z_5} = R_{25}^2 - R_{15}^2 - x_2^2 + x_1^2 - y_2^2 + y_1^2 - z_2^2 + z_1^2 \\ 2\left( {{x_1} - {x_3}} \right){x_5} + 2\left( {{y_1} - {y_3}} \right){y_5} + 2\left( {{z_1} - {z_3}} \right){z_5} = R_{35}^2 - R_{15}^2 - x_3^2 + x_1^2 - y_3^2 + y_1^2 - z_3^2 + z_1^2 \\ 2\left( {{x_1} - {x_4}} \right){x_5} + 2\left( {{y_1} - {y_4}} \right){y_5} + 2\left( {{z_1} - {z_4}} \right){z_5} = R_{45}^2 - R_{15}^2 - x_4^2 + x_1^2 - y_4^2 + y_1^2 - z_4^2 + z_1^2 \end{aligned}
2(x1−x2)x5+2(y1−y2)y5+2(z1−z2)z5=R252−R152−x22+x12−y22+y12−z22+z122(x1−x3)x5+2(y1−y3)y5+2(z1−z3)z5=R352−R152−x32+x12−y32+y12−z32+z122(x1−x4)x5+2(y1−y4)y5+2(z1−z4)z5=R452−R152−x42+x12−y42+y12−z42+z12
令
b
1
=
R
25
2
−
R
15
2
−
x
2
2
+
x
1
2
−
y
2
2
+
y
1
2
−
z
2
2
+
z
1
2
b
2
=
R
35
2
−
R
15
2
−
x
3
2
+
x
1
2
−
y
3
2
+
y
1
2
−
z
3
2
+
z
1
2
b
3
=
R
45
2
−
R
15
2
−
x
4
2
+
x
1
2
−
y
4
2
+
y
1
2
−
z
4
2
+
z
1
2
b_1= R_{25}^2 - R_{15}^2 - x_2^2 + x_1^2 - y_2^2 + y_1^2 - z_2^2 + z_1^2 \\ b_2= R_{35}^2 - R_{15}^2 - x_3^2 + x_1^2 - y_3^2 + y_1^2 - z_3^2 + z_1^2 \\ b_3 = R_{45}^2 - R_{15}^2 - x_4^2 + x_1^2 - y_4^2 + y_1^2 - z_4^2 + z_1^2
b1=R252−R152−x22+x12−y22+y12−z22+z12b2=R352−R152−x32+x12−y32+y12−z32+z12b3=R452−R152−x42+x12−y42+y12−z42+z12
从而转化为线性运算：
A
X
=
b
\bf{A} \bf{X} = \bf{b}
AX=b
[
2
(
x
1
−
x
2
)
2
(
y
1
−
y
2
)
2
(
z
1
−
z
2
)
2
(
x
1
−
x
3
)
2
(
y
1
−
y
3
)
2
(
z
1
−
z
3
)
2
(
x
1
−
x
4
)
2
(
y
1
−
y
4
)
2
(
z
1
−
z
4
)
]
[
x
5
y
5
z
5
]
=
[
b
1
b
2
b
3
]
\begin{bmatrix} 2\left(x_1-x_2\right)&2\left(y_1-y_2\right)&2\left(z_1-z_2\right)\\ 2\left(x_1-x_3\right)&2\left(y_1-y_3\right)&2\left(z_1-z_3\right)\\ 2\left(x_1-x_4\right)&2\left(y_1-y_4\right)&2\left(z_1-z_4\right) \end{bmatrix} \begin{bmatrix} x_5 \\ y_5 \\ z_5 \end{bmatrix} = \begin{bmatrix} b_1 \\ b_2 \\ b_3 \end{bmatrix}
⎣⎡2(x1−x2)2(x1−x3)2(x1−x4)2(y1−y2)2(y1−y3)2(y1−y4)2(z1−z2)2(z1−z3)2(z1−z4)⎦⎤⎣⎡x5y5z5⎦⎤=⎣⎡b1b2b3⎦⎤
对系数矩阵求逆即可得到待求解节点坐标：
X
=
A
−
1
b
\bf{X} = {\bf{A}^{ - 1}}\bf{b}
X=A−1b
因此，对于新的节点，只需将任意已知位置的四个节点作为参考节点进行一次测距，即可通过矩阵求逆获得一个新的节点的位置坐标。
3 移动自组网络全局定位
3.1 坐标变换方法
为了将网络内部的局部定位坐标统一为一致的全局坐标系，需要进行坐标变换。
常用的方法包括：坐标变换矩阵、欧拉角、四元数。
坐标变换矩阵方法的基本思想是利用同一个节点在两个坐标系中的不同坐标,可以推导出这两个坐标系之间的坐标变换矩阵。特别的,若坐标系是三维坐标系,则需要4个节点求解该矩阵。
[
x
g
l
o
b
a
l
y
g
l
o
b
a
l
z
g
l
o
b
a
l
]
=
[
x
l
o
c
a
l
y
l
o
c
a
l
z
l
o
c
a
l
]
R
x
R
y
R
z
+
T
\begin{bmatrix} x_{global} & y_{global} & z_{global} \end{bmatrix} = \begin{bmatrix} x_{local} & y_{local} & z_{local} \end{bmatrix} \bf{R_x} \bf{R_y} \bf{R_z} + \bf{T}
[xglobalyglobalzglobal]=[xlocalylocalzlocal]RxRyRz+T
坐标旋转矩阵为：
R
x
=
[
1
0
0
0
cos
(
θ
x
)
−
sin
(
θ
x
)
0
sin
(
θ
x
)
cos
(
θ
x
)
]
\mathbf{R}_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\left(\theta_x\right) & -\sin\left(\theta_x\right) \\ 0 & \sin\left(\theta_x\right) & \cos\left(\theta_x\right) \end{bmatrix}
Rx=⎣⎡1000cos(θx)sin(θx)0−sin(θx)cos(θx)⎦⎤
R
y
=
[
cos
(
θ
y
)
0
sin
(
θ
y
)
0
1
0
−
sin
(
θ
y
)
0
cos
(
θ
y
)
]
\mathbf{R}_y = \begin{bmatrix} \cos\left(\theta_y\right) & 0 & \sin\left(\theta_y\right) \\ 0 & 1 & 0 \\ -\sin\left(\theta_y\right) & 0 & \cos\left(\theta_y\right) \end{bmatrix}
Ry=⎣⎡cos(θy)0−sin(θy)010sin(θy)0cos(θy)⎦⎤
R
z
=
[
cos
(
θ
z
)
−
sin
(
θ
z
)
0
sin
(
θ
z
)
cos
(
θ
z
)
0
0
0
1
]
\mathbf{R}_z = \begin{bmatrix} \cos\left(\theta_z\right) & -\sin\left(\theta_z\right) & 0 \\ \sin\left(\theta_z\right) & \cos\left(\theta_z\right) & 0 \\ 0 & 0 & 1 \end{bmatrix}
Rz=⎣⎡cos(θz)sin(θz)0−sin(θz)cos(θz)0001⎦⎤
坐标平移向量为：
T
=
[
T
x
T
y
T
z
]
\mathbf{T} = \begin{bmatrix} T_x & T_y & T_z \end{bmatrix}
T=[TxTyTz]
其中，
θ
x
\theta_x
θx，
θ
y
\theta_y
θy，
θ
z
\theta_z
θz 分别为坐标系
x
l
o
c
a
l
x_{local}
xlocal
y
l
o
c
a
l
y_{local}
ylocal $ z_{local}$ 绕
x
g
l
o
b
a
l
x_{global}
xglobal，
y
g
l
o
b
a
l
y_{global}
yglobal，$ z_{global}$ 轴旋转至与坐标系
x
g
l
o
b
a
l
x_{global}
xglobal
y
g
l
o
b
a
l
y_{global}
yglobal$ z_{global}$ 方向一致的角度。
T
x
T_x
Tx，
T
y
T_y
Ty，
T
z
T_z
Tz 是坐标系
x
l
o
c
a
l
x_{local}
xlocal
y
l
o
c
a
l
y_{local}
ylocal $ z_{local}$ 相对坐标系
x
g
l
o
b
a
l
x_{global}
xglobal
y
g
l
o
b
a
l
y_{global}
yglobal$ z_{global}$ 平移的距离。
3.2 局部坐标系转化为全局坐标系
在这里，我们并不是要求解具体的旋转角度与平移量（这一问题较为复杂），而只是要求将局部坐标系下的节点位置转化到全局坐标系下。
因此，可将全部的线性变换矩阵作为一个整体求解，从而可直接通过最小二乘法得到一般化的坐标变换矩阵。具体做法如下：
由坐标变换矩阵的表示方法可简化为一般形式的坐标变换关系：
[
x
g
l
o
b
a
l
y
g
l
o
b
a
l
z
g
l
o
b
a
l
]
=
[
x
l
o
c
a
l
y
l
o
c
a
l
z
l
o
c
a
l
]
[
a
11
a
12
a
13
a
21
a
22
a
23
a
31
a
32
a
33
]
+
[
b
1
b
2
b
3
]
\begin{bmatrix} x_{global} & y_{global} & z_{global} \end{bmatrix} = \begin{bmatrix} x_{local} & y_{local} & z_{local} \end{bmatrix} \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{bmatrix} + \begin{bmatrix} b_{1} & b_{2} & b_{3} \end{bmatrix}
[xglobalyglobalzglobal]=[xlocalylocalzlocal]⎣⎡a11a21a31a12a22a32a13a23a33⎦⎤+[b1b2b3]
可合并为
[
x
g
l
o
b
a
l
y
g
l
o
b
a
l
z
g
l
o
b
a
l
]
=
[
x
l
o
c
a
l
y
l
o
c
a
l
z
l
o
c
a
l
1
]
[
a
11
a
12
a
13
a
21
a
22
a
23
a
31
a
32
a
33
b
1
b
2
b
3
]
\begin{bmatrix} x_{global} & y_{global} & z_{global} \end{bmatrix} = \begin{bmatrix} x_{local} & y_{local} & z_{local} & 1 \end{bmatrix} \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \\ b_{1} & b_{2} & b_{3} \end{bmatrix}
[xglobalyglobalzglobal]=[xlocalylocalzlocal1]⎣⎢⎢⎡a11a21a31b1a12a22a32b2a13a23a33b3⎦⎥⎥⎤
即
y
=
X
C
\bf{y} = \bf{X} \bf{C}
y=XC
由最小二乘法（LS）即可获得一般化的坐标变换矩阵为：
C
L
S
=
X
+
y
=
(
X
T
X
)
−
1
X
T
y
\bf{C_{LS}} = \bf{X}^{+} \bf{y} = \left(\bf{X}^T \bf{X}\right)^{-1} \bf{X}^T \bf{y}
CLS=X+y=(XTX)−1XTy
4 MATLAB代码
% 时空同步联立求解
%{
@ TSENG ChihYuan 2020-06-14
%}
clear,clc,close all
set(0,'defaultfigurecolor','w')
%% 节点设置
c = 3e8; %波速
% 节点位置
pos=[ -1.0000 0 5.0073
-0.4600 0.7200 4.9792
-0.4600 -0.7200 5.0010
0.3900 1.0000 4.9631
0.3900 -1.0000 4.9634
1.0000 0.4800 4.9676
1.0000 -0.4800 5.0373
-0.0200 0 5.0392
1.1000 1.4000 5.0175
1.1000 -1.4000 5.0138 ];
N = size(pos,1);%节点数量
% 时间同步误差
delay = ones(N,1) .* normrnd( 0 , 1e-9 , N , 1 );%时间误差（每行一个节点）
delay(1)=0; %第一个节点为参考节点，时间误差为0
%% 真实临接矩阵
R = zeros(N,N);
for ii = 1 : N
for jj = 1 : N
R(ii,jj) = norm( pos(ii,:) - pos(jj,:) );%ii节点与jj节点的距离
end
end
%% 测量临接矩阵（真实距离+时间同步造成的测距误差）
Rm = zeros(N,N);
for ii = 1 : N
for jj = 1 : N
Rm(ii,jj) = R(ii,jj) + ( delay(ii)-delay(jj) ) * c; % 测量距离 = 真实距离+时间同步造成的测距误差
end
end
%% 求解时间误差
delay1 = zeros(N,1);%初始化
for ii = 1 : N
delay1(ii,1) = (Rm(ii,1) - Rm(1,ii) ) / (2*c) ; %理由和第1个参考节点的距离，求出各节点对于第1个节点的时间差
end
%% 去除测量临接矩阵的时间误差
Rm1 = zeros(N,N);
for ii = 1 : N
for jj = 1 : N
Rm1(ii,jj) = Rm(ii,jj) - ( delay1(ii)-delay1(jj) ) * c; %去除时间同步误差，恢复成真实测距
end
end
%% 求解空间定位
%初始化
pos1 = zeros(size(pos));
%第1个点设为原点
x1=0;
y1=0;
z1=0;
pos1(1,:)=[x1,y1,z1];
%第2个点设为x轴上点
x2 = Rm1(1,2);
y2 =0;
z2 =0;
pos1(2,:) = [x2,y2,z2];%选取1-2节点连线作为x轴
%第3个点设为xoy平面内点
x3 = ( Rm1(1,3)^2 - Rm1(2,3)^2 + x2^2 ) / ( 2 * x2 );
y3 = sqrt( Rm1(1,3)^2 - x3^2 ); %取根号取正值，即设定y方向
z3 = 0;
pos1(3,:) = [ x3,y3,z3 ];
%第4个点由1~3个点确定位置
x4 = ( Rm1(1,4)^2 - Rm1(2,4)^2 + x2^2 ) / (2 * x2);
y4 = ( ( Rm1(1,4)^2- x4^2 )-( Rm1(3,4)^2 - (x4-x3)^2 ) + y3^2 ) / ( 2 * y3 );
z4 = sqrt( Rm1(1,4)^2 - x4^2 - y4^2 ); %取根号取正值，即设定z方向
pos1(4,:) = [ x4,y4,z4 ];
% 其他点都由 1~4 节点空间定位
% 系数矩阵 A
A = [ 2*(x1-x2) 2*(y1-y2) 2*(z1-z2)
2*(x1-x3) 2*(y1-y3) 2*(z1-z3)
2*(x1-x4) 2*(y1-y4) 2*(z1-z4) ];
% 求解其他节点的定位
for ii = 5 : N
%值域向量
b1 = Rm1(2,ii)^2 - Rm1(1,ii)^2 - x2^2 + x1^2 - y2^2 + y1^2 - z2^2 + z1^2;
b2 = Rm1(3,ii)^2 - Rm1(1,ii)^2 - x3^2 + x1^2 - y3^2 + y1^2 - z3^2 + z1^2;
b3 = Rm1(4,ii)^2 - Rm1(1,ii)^2 - x4^2 + x1^2 - y4^2 + y1^2 - z4^2 + z1^2;
b = [b1;b2;b3];
% 求解第ii个节点的位置
pos1(ii,:) = inv(A)*b;
end
%% 最小二乘求解坐标变换矩阵
point = [1 4 6 7]; %已知位置的点索引（至少4点）
Y = pos(point,:);
X = [ pos1(point,:), ones(length(point),1) ];
w = pinv(X)*Y; %最小二乘求解坐标变换矩阵
pos_LS = [pos1,ones(size(pos1,1),1)] * w;
%% 绘图
figure,plot3(pos(:,1),pos(:,2),pos(:,3),'o'),grid on,title('节点真实空间位置（全局坐标系）')
figure,plot3(pos1(:,1),pos1(:,2),pos1(:,3),'o'),grid on,title('节点相对空间位置（局部坐标系）')
figure,
plot3(pos(:,1),pos(:,2),pos(:,3),'*','markersize',10),hold on
plot3(pos_LS(:,1),pos_LS(:,2),pos_LS(:,3),'o','markersize',10),
legend('真实空间位置','LS解算的空间位置')
grid on,title('节点空间位置（全局坐标系）')
5 仿真结果
综上各步骤，最终可根据节点间的测距结果得到各节点的全局坐标为：
885
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
