移動自組網絡的時間校準與三維空間定位(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 仿真結果

綜上各步驟，最終可根據節點間的測距結果得到各節點的全局座標爲：