移动自组网络的时间校准与三维空间定位(MATLAB仿真)

移动自组网络的时间校准与空间定位（MATLAB仿真）

0 问题

0.1 问题描述

对于空间分布的$N$个节点，且它们存在时间同步误差。现考虑从它们的相互测距数据中求得各节点的空间位置座标。

0.2 假设

① 所有节点均可相互通信；
② 在足够短的时间内，各节点的时间同步误差保持不变；

1 时间校准

1.1 数据组织格式（邻接矩阵）

在空间中各节点间的距离可由邻接矩阵表示为：
$\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_{ij}$ 表示节点 $i$ 与节点 $j$ 间的距离，且有 $R_{ij}=R_{ji}$ ，即矩阵 $\bf{R}$ 为对角线为 0 的对称矩阵。

1.2 时间同步误差求解与消除

对于空间中分布的节点 $i$ 与节点 $j$ ，它们的实际距离为 ${R_{ij}} = {R_{ji}}$，它们之间的时间同步误差为 $\Delta t$ ，则：

从节点 $i$ 测量得到的两点距离为 : ${\tilde R_{ij}} = {R_{ij}} + \Delta t \times c$
从节点 $j$ 测量得到的两点距离为 : ${\tilde R_{ji}} = {R_{ji}} - \Delta t \times c$

其中，$c$ 为波速。

（1）时间同步误差求解
只需利用节点间的相互测距数据，便可获得各节点间的时间同步误差为：
$\Delta t = \frac{{{{\tilde R}_{ij}} - {{\tilde R}_{ji}}}}{{2 \times c}}$
利用邻接矩阵，可得各节点间的时间同步误差矩阵为：
${\bf{\Delta T}} = \frac{1}{{2c}}\left( {{\bf{\tilde R}} - {{{\bf{\tilde R}}}^T}} \right)$
其中，$ {\bf{\Delta T}} $ 为时间同步误差矩阵

（2）时间同步误差消除
因此，在不考虑测量误差的情况下，即可由测量距离 $R_{ij}$ 与 $R_{ji}$ 求得真实距离为：
${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}$
则可得消除各节点间的时间同步误差后的邻接矩阵为：
${\bf{R}} = \frac{1}{2}\left( {{\bf{\tilde R}} + {{{\bf{\tilde R}}}^T}} \right)$

2 移动自组网络局部定位

2.1 座标原点设置

在所有节点中任取一点（不妨取第1个节点）作为参考节点（座标原点），其座标为 $P_1=(0,0,0)$ 。

2.2 x轴设置

在设定参考节点（座标原点）后，任取另一节点（不妨取第2个节点），令原点与该节点的连线为 x 轴，则该节点的座标为 $P_2=(x_{2},0,0)=(R_{12},0,0)$

2.3 y轴设置（平面的两点定位方法）

在其他剩余节点中选择一个与x轴不共线的节点，将与x轴垂直且指向该节点的方向定为y轴，并构成一个平面（不妨设为第3个节点），由 $P_1$与 $P_2$ 定位 $P_3$（$P_3$取 $y_3>0$ 的解）

可列方程组：
$\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}$
解得 $P_3$ 座标为：
$\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}$

2.4 z轴设置（空间的三点定位方法）

在其他剩余节点中选择一个与xoy不共面的节点，将与xoy平面垂直且指向该节点（不妨设为第4个节点）的方向定为z轴，由 $P_1$、 $P_2$ 与 $P_3$ 定位 $P_4$（$P_4$取 $z_4>0$ 的解）

可列方程组：
$\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}$
解得 $P_4$ 座标为：
$\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}$

2.5 线性化的空间定位算法（四点定位法）

在三维空间中，由三个节点的测距可实现对任意节点的空间定位。但是，当三个参考节点不包含原点与座标轴上的点时，将涉及到求解三元二次方程组，复杂度高。
因此，考虑三维空间的四点定位算法，它利用距离差实现定位，将三元二次方程组转化为线性方程组，复杂度低。

对于已知座标的四个节点 $P_1$~$P_4$，待定位节点为 $P_5$，可利用三点定位方法列方程组如下：
$\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}$
展开平方项可得：
$\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}$
再用第一式与各式差分有：
$\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}$
令
$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$
从而转化为线性运算：
$\bf{A} \bf{X} = \bf{b}$
$\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}$
对系数矩阵求逆即可得到待求解节点座标：
$\bf{X} = {\bf{A}^{ - 1}}\bf{b}$

因此，对于新的节点，只需将任意已知位置的四个节点作为参考节点进行一次测距，即可通过矩阵求逆获得一个新的节点的位置座标。

3 移动自组网络全局定位

3.1 座标变换方法

为了将网络内部的局部定位座标统一为一致的全局座标系，需要进行座标变换。
常用的方法包括：座标变换矩阵、欧拉角、四元数。

座标变换矩阵方法的基本思想是利用同一个节点在两个座标系中的不同座标,可以推导出这两个座标系之间的座标变换矩阵。特别的,若座标系是三维座标系,则需要4个节点求解该矩阵。

$\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}$
座标旋转矩阵为：
$\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}$
$\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}$
$\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}$
座标平移向量为：
$\mathbf{T} = \begin{bmatrix} T_x & T_y & T_z \end{bmatrix}$
其中，$\theta_x$，$\theta_y$，$\theta_z$ 分别为座标系 $x_{local}$ $y_{local}$ $ z_{local}$ 绕 $x_{global}$， $y_{global}$，$ z_{global}$ 轴旋转至与座标系 $x_{global}$ $y_{global}$$ z_{global}$ 方向一致的角度。$T_x$，$T_y$，$T_z$ 是座标系 $x_{local}$ $y_{local}$ $ z_{local}$ 相对座标系 $x_{global}$ $y_{global}$$ z_{global}$ 平移的距离。

3.2 局部座标系转化为全局座标系

在这里，我们并不是要求解具体的旋转角度与平移量（这一问题较为复杂），而只是要求将局部座标系下的节点位置转化到全局座标系下。
因此，可将全部的线性变换矩阵作为一个整体求解，从而可直接通过最小二乘法得到一般化的座标变换矩阵。具体做法如下：

由座标变换矩阵的表示方法可简化为一般形式的座标变换关系：
$\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}$

可合并为
$\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}$
即
$\bf{y} = \bf{X} \bf{C}$
由最小二乘法（LS）即可获得一般化的座标变换矩阵为：
$\bf{C_{LS}} = \bf{X}^{+} \bf{y} = \left(\bf{X}^T \bf{X}\right)^{-1} \bf{X}^T \bf{y}$

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 仿真结果

综上各步骤，最终可根据节点间的测距结果得到各节点的全局座标为：