Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

3D シミュレーションでの UAV ナビゲーション向けステレオ Visual SLAM

"Visual SLAM" とは、環境地図作成を行うのと同時に、周囲に対するカメラの位置と向きを計算する処理のことです。Visual SLAM アルゴリズムを開発し、さまざまな条件でそのパフォーマンスを評価することは、困難な課題です。最大の課題の 1 つは、特に屋外環境で、カメラ センサーのグラウンド トゥルースを生成することです。シミュレーションを使用することで、さまざまなシナリオやカメラ構成でテストを行って、正確なグラウンド トゥルースを得ることができます。

この例では、Unreal Engine® シミュレーションを使用して、ステレオ カメラを搭載した UAV の市街地シナリオにおける Visual SLAM アルゴリズムを開発する方法を示します。ステレオ カメラ [1] 向け Visual SLAM パイプラインの実装の詳細については、ステレオ Visual Simultaneous Localization and Mappingの例を参照してください。

シミュレーション環境の設定

まず、Visual SLAM アルゴリズムのテストに使用できるシミュレーション環境でシナリオを設定します。典型的な市街地を描いたシーンをテスト対象ビークルの UAV とともに使用します。

次に、UAV がシーン内でたどる軌道を選択します。Unreal Engine シミュレーションのウェイポイントの選択 (Automated Driving Toolbox)の例に従って、一連のウェイポイントを対話形式で選択した後、関数 helperSelectSceneWaypoints を使用して UAV の基準軌跡を生成できます。この例では、以下に示すように、記録済みの基準軌跡を使用します。

% Load reference path
data        = load("uavStereoSLAMData.mat");

pos         = data.pos;          % Position
orientEuler = data.orientEuler;  % Orientation

Simulation 3D Scene Configuration (UAV Toolbox)ブロックを使用して、米国の市街地シーンを組み込んだ UAVVisualSLAMIn3DSimulation Simulink® モデルが構成されます。モデルは、Simulation 3D UAV Vehicle (UAV Toolbox)ブロックを使用して UAV をシーンに配置します。2 つのSimulation 3D Camera (UAV Toolbox)ブロックで構成されるステレオ カメラが UAV に接続されます。Simulation 3D Camera (UAV Toolbox)ブロックのダイアログ ボックスで、[取り付け] タブを使用してカメラの位置を調整します。[パラメーター] タブを使用して、さまざまなカメラをシミュレートするようにカメラのプロパティを構成します。シミュレートするステレオ カメラの内部パラメーターを推定するには、ステレオ カメラ キャリブレーター アプリの使用アプリを使用します。

% Stereo camera parameters
focalLength    = [1109, 1109]; % In pixels
principalPoint = [640, 360];   % In pixels [x, y]
imageSize      = [720, 1280];  % In pixels [mrows, ncols]
baseline       = 0.5;          % In meters

% Open the model
modelName = 'UAVVisualSLAMIn3DSimulation';
open_system(modelName);

vSLAMModelUAV.PNG

ステレオ Visual SLAM アルゴリズムの実装

Helper Stereo Visual SLAM System ブロックは、次の手順で構成されるステレオ Visual SLAM パイプラインを実装するものです。

  • 地図の初期化: パイプラインはまず、視差マップを使用して、ステレオ カメラで生成されたイメージのペアから 3 次元点のマップを初期化します。左のイメージは最初のキー フレームとして保存されます。

  • トラッキング: マップが初期化された後は、新しいステレオ ペアごとに、左のイメージの特徴を最後のキー フレームの特徴とマッチングすることにより、カメラ姿勢が推定されます。推定されたカメラ姿勢は、局所地図のトラッキングにより調整されます。

  • 局所地図作成: 現在の左のイメージがキー フレームとして識別された場合、ステレオ ペアの視差から新しい 3 次元マップ点が計算されます。この段階で、カメラ姿勢と 3 次元点を調整することにより、バンドル調整を使用して再投影誤差を最小化します。

  • ループ閉じ込み: bag-of-features アプローチを使用して、キー フレームごとにそれまでのすべてのキー フレームと比較することで、ループを検出します。ループ閉じ込みが検出されると姿勢グラフが最適化され、すべてのキー フレームのカメラ姿勢を調整します。

アルゴリズムの実装の詳細については、ステレオ Visual Simultaneous Localization and Mappingの例を参照してください。

ステレオ Visual SLAM シミュレーションの実行

モデルをシミュレートし、結果を可視化します。"Video Viewer" ブロックがステレオ イメージ出力を表示します。"Point Cloud Player" が、推定されたカメラ軌跡で再構築された 3 次元マップを表示します。

if ~ispc
    error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Set the random seed to get consistent results
rng(0);

% Run simulation
sim(modelName);

Loop edge added between keyframe: 2 and 91

stereoImageUAV.png

モデルを閉じます。

close_system(modelName);

参考文献

[1] Mur-Artal, Raul, and Juan D. Tardós. "ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras." IEEE Transactions on Robotics 33, no. 5 (2017): 1255-1262.