目次
1. アルゴリズム操作レンダリングのプレビュー
2.アルゴリズム実行ソフトウェアバージョン
マットラボ2022a
3. いくつかのコアプログラム
..........................................................................
kkk = 0;
for EbN0 = EbN0_sub
kkk
kkk = kkk + 1;
for jj1 = 1:Tag_Num
jj1
rng(jj1);
for jj = 1:num_bits
%TAG to BS1
delay_1 = round(time_bs_tag(1,jj1)/ts);
xx1 = zeros(1,delay_1);
%传播时延
delay_1_1(jj,:) = [xx1 sig(1:end-length(xx1))];
%UWB
h_4 = uwb_channel(dist_bs_tag(1,jj1));
%信号经过信道
conv_data1 = conv(delay_1_1(jj,:),h_4);
UWB_chan1(jj,:) = conv_data1(1:length(sig));
%TAG to BS2
delay_2 = round(time_bs_tag(2,jj1)/ts);
xx2 = zeros(1,delay_2);
%传播时延
delay_2_1(jj,:) = [xx2 sig(1:end-length(xx2))];
h_2 = uwb_channel(dist_bs_tag(2,jj1));
conv_data2 = conv(delay_2_1(jj,:),h_2);
UWB_chan2(jj,:) = conv_data2(1:length(sig));
%TAG to BS3
delay_3 = round(time_bs_tag(3,jj1)/ts);
xx3 = zeros(1,delay_3);
%传播时延
delay_3_1(jj,:) = [xx3 sig(1:end-length(xx3))];
h_3 = uwb_channel(dist_bs_tag(3,jj1));
conv_data3 = conv(delay_3_1(jj,:),h_3);
UWB_chan3(jj,:) = conv_data3(1:length(sig));
%TAG to BS4
delay_4 = round(time_bs_tag(4,jj1)/ts);
xx4 = zeros(1,delay_4);
%传播时延
delay_4_1(jj,:) = [xx4 sig(1:end-length(xx4))];
h_4 = uwb_channel(dist_bs_tag(4,jj1));
conv_data4 = conv(delay_4_1(jj,:), h_4);
UWB_chan4(jj,:) = conv_data4(1:length(sig));
end
for jj = 1:num_bits
UWB_chan1n(jj,:) = awgn(UWB_chan1(jj,:)/max(UWB_chan1(jj,:)),EbN0,'measured');
UWB_chan2n(jj,:) = awgn(UWB_chan2(jj,:)/max(UWB_chan2(jj,:)),EbN0,'measured');
UWB_chan3n(jj,:) = awgn(UWB_chan3(jj,:)/max(UWB_chan3(jj,:)),EbN0,'measured');
UWB_chan4n(jj,:) = awgn(UWB_chan4(jj,:)/max(UWB_chan4(jj,:)),EbN0,'measured');
end
%自适应前沿检测
%自适应前沿检测
..........................................................
end
end
P_est0 = [x_est0',y_est0',z_est0'];
P_est1 = [x_est1',y_est1',z_est1'];
figure;
plot(toa_error0,'-r>',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.9,0.9,0.0]);
hold on
title('估计误差')
axis([0,Tag_Num,0,2]);
ylabel('cm');
figure
axis([0 10 0 10 0 10]);
for i=1:BS_Num
plot3(BS_pos(i,1),BS_pos(i,2),BS_pos(i,3),'ko','MarkerFace','y','MarkerSize',8);
hold on
end
hold on
for i=1:Tag_Num
plot3(Tag(i,1),Tag(i,2),Tag(i,3),'k^','MarkerFace','b','MarkerSize',6);
hold on
plot3(x_est1(i),y_est1(i),z_est1(i),'ks','MarkerFace','r','MarkerSize',6);
hold on
end
grid on
xlabel('cm');
ylabel('cm');
zlabel('cm');
save R.mat toa_error1
36_003m4. アルゴリズム理論の概要
UWBとIMUの融合に基づく3次元空間測位アルゴリズムは、無線パルス波(UWB)と慣性測定ユニット(IMU)のそれぞれの利点を組み合わせた測位方法です。UWB は信号の伝送時間を測定して距離を計算するため、高精度と強力な耐干渉性という利点がありますが、マルチパスの影響や環境ノイズの影響を受けやすくなります。IMU は、加速度および角速度を測定することによって姿勢および位置情報を計算します。これはリアルタイムで動的ですが、加速度測定誤差やドリフトによって制限されます。
2つの技術を融合することで、それぞれの利点を最大限に活かし、位置決め精度と安定性を向上させます。具体的には、UWBはターゲットの位置と姿勢を計算するための高精度の距離情報を提供でき、IMUはリアルタイムの加速度および角速度情報を提供して、UWBの測定誤差とドリフトを補正し、システムの応答速度と堅牢性を向上させることができます。
以下に、UWB と IMU の融合に基づく 3 次元空間測位アルゴリズムを紹介します。その原理と数式は次のとおりです。
- UWBポジショニング
UWBではデュアル基地局による測位方式が採用されており、2つの基地局の位置座標を(x1, y1, z1)、(x2, y2, z2)、物標の位置座標を(x, y, z2)とすると、 z)、次の式は、ターゲットと 2 つの基地局の間の距離の差を計算します。
Δd = (x2-x1)² + (y2-y1)² + (z2-z1)² - (x-x1)² - (y-y1)² - (z-z1)²
このうち、(x, y, z) はターゲットの位置座標、(x1, y1, z1) と (x2, y2, z2) は 2 つの基地局の位置座標です。距離の差と 2 つの基地局の座標に基づいて、2 つの方程式をリストして解くことで、ターゲットの位置座標 (x、y、z) を取得できます。
IMUアシスト
IMU は、リアルタイムの加速度および角速度情報を提供して、UWB 測定誤差とドリフトを修正できます。具体的には、IMU は加速度センサーとジャイロスコープを提供して、それぞれ加速度と角速度の情報を測定できます。これらの情報を統合・平滑化することで、対象物の姿勢・位置情報が得られます。
融合プロセスでは、IMU の加速度および角速度情報を UWB の補助データとして使用して、UWB の測定結果を補正することができます。具体的には、IMUの加速度情報を利用してターゲットの速度と加速度を計算してUWB距離測定結果を補正し、IMUの角速度情報を利用してUWB角度測定結果を補正することができる。これにより、システムをより正確かつ堅牢にすることができます。
融合アルゴリズム
UWB と IMU の融合に基づく 3 次元空間位置決めアルゴリズムには、主にデータ収集段階とデータ融合段階の 2 つの段階が含まれます。データ収集段階では、ターゲットの位置、速度、加速度、角速度などの情報がUWBとIMUを通じて収集され、データ融合段階では、収集されたデータが融合されてターゲットの最終的な位置、速度、加速度、角速度が得られます。およびその他の情報。
必要に応じて、最小二乗法、カルマン フィルター、その他の方法を使用して解決プロセスを最適化できます。たとえば、カルマン フィルター アルゴリズムを使用して UWB データと IMU データを融合し、より正確なターゲットの位置、速度、加速度、角速度、その他の情報を取得できます。具体的な実装プロセスは次のとおりです。
(1) 状態マトリックスと制御マトリックスを初期化する;
(2) UWB および IMU を通じてデータを収集する;
(3) 収集されたデータを使用して状態マトリックスと制御マトリックスを計算する;
(4) 状態マトリックスと制御マトリックスを規則に従って計算するカルマンフィルタ式 反復計算;
(5) 反復結果に基づいて、目標の最終的な位置、速度、加速度、角速度などの情報を計算します。
アルゴリズムの利点
UWB と IMU の融合に基づく 3D 空間測位アルゴリズムには、次の利点があります。
(1) 高精度: UWB と IMU の融合により、測位精度に対する環境ノイズの影響が軽減され、アルゴリズムのロバスト性が向上します。
(2) リアルタイム性能 強力: IMU の加速度および角速度情報は、リアルタイムの姿勢および位置情報を提供し、UWB の距離測定結果を補正し、システムの応答時間を短縮します。(3) 高い信頼性: データ融合テクノロジーによるマルチセンサー
データの処理により、単一センサーの故障によるシステム パフォーマンスへの影響を軽減できます
(4) 強力な拡張性: アルゴリズムは、ロボットの位置決め、無人運転など。
5. アルゴリズム完全プログラムエンジニアリング
おおおお
ああ
○