クワッドコプタ搭載 LRF と単眼カメラによる BaySAC を用いた
地表面計測
○小山 翔平 (九州大学), 岩下 友美 (九州大学), 倉爪 亮 (九州大学)

Measurement of Land Surface using BaySACby Camera and LRF on Quadcopter○ Shouhei Koyama (Kyushu University), Yumi Iwashita (Kyushu University),

and Ryo Kurazume (Kyushu University)
Abstract: This paper describes a 3D shape measurement system by a quadcopter carrying a camera and a LRF.
The LRF measures 3D shape of ground while the camera tracks natural feature points in scenes and known
feature points which are mobile robots on the ground. The 3D positions of the mobile robots are identified by
CPS (Cooperative Positioning System) precisely and used as high-reliable landmarks. Position of the quadcopter
is identified by estimating Essential Matrix using BaySAC.

1.

はじめに歴史的・文化的価値の高い建造物を，風化や火災による破損，消失から守るために，レーザスキャナやデジタルカメ
ラなどにより建造物の見えや構造を計測し，デジタルデータとして保存するデジタルアーカイブ化が注目されている．
我々もこれまでに，レーザスキャナを搭載したロボット群による 3 次元レーザ計測システム (CPS-VI) を開発し，太宰府天満宮などのデジタルアーカイブ化に成功している [1]．
本システムは，対象建造物周囲の複数地点から，ロボットに搭載したレーザスキャナにより建造物をレーザ計測し，全周 3D モデルを獲得する．この際，ロボットがレーザ計測を行う位置は，協調ポジショニングシステム（CooperativePositioning System, CPS）[4] により高精度に同定される．
CPS とは，3 体以上のロボットが互いに位置同定を行いながら移動することで，極めて高精度な位置同定を実現するシステムのことである（Fig.1）．
しかし，これまでに開発したシステムは，地上を走行するロボットからのレーザ計測であり，計測中に人や車と衝突する危険性や，屋根など地上からでは取得できない部分が存在する問題があった．
そこで本研究では，クアッドコプタにレーザレンジファインダを搭載し，上空から対象建造物のレーザ計測を行うシステムを開発する．本システムの実現には，クアッドコプタによる安定な飛行の実現や，クアッドコプタの正確な位置・姿勢の取得手法など多くの開発項目がある．特に後者はクアッドコプタの制御だけでなく，取得したデータを処理・統合する際にも重要となる．クアッドコプタの制御・位置同定は GPS の利用が一般的であるが，より安定性や精度を高めるために，搭載したカメラにより地上の自然特徴点を追跡する手法や，IMU(Inertial Measurement Unit) により姿勢を推定する手法が提案されている [2] [3]．しかし，自然特徴点は，安定な特徴点の抽出や，追跡失敗時の対応，移動体の除外など解決すべき課題がある．一方，上述した CPS-VI
では，全てのロボットの 3 次元位置は正確に同定されている．そこで，開発するシステムでは，地上からレーザ計測を行うロボット群を，3 次元座標が既知の地上特徴点として利用し，それらと自然特徴点を同時にクアッドコプタのカメラで撮影することで，より安定な位置同定を実現する．

Fig.1 Cooperative Positioning System (CPS) [4]2.

システム構成本システムは，レーザレンジファインダ (LRF) やカメラを搭載したクワッドコプタと，既知特徴点として使用する複数の地上走行ロボットからなる（Fig.2）．

LRF, カメラ3次元計測＆撮影

地上走行ロボット
Fig.2 System overview

開発したクワッドコプタを Fig.3 に，飛行制御やセンサ処理を行う処理ボードを Fig.4 に示す．この処理ボー

第15回システムインテグレーション部門講演会(SI2014)（2014年12月14日～17日・東京）- 0972 -SY0010/14/0000 - 0972 © 2014 SICE

ドはセンサ処理用 SH7144f（RENESAS）及び飛行制御用
Raspberry Pi（Raspberry Pi Foundation) の 2 つの処理系から構成され，地上 PC との通信には XBee Pro を用いる．
また GPS(GT-723F），高度計（MS5611-01BA01），3 軸加速度センサ（LIS3DH），ジャイロセンサ (L3GD20) を搭載しており，SH7144f を介してデータが処理される．姿勢安定化など飛行のための基本処理は制御ユニット（WookongM，DJI）により行われる．

ฎ⌮䝪䞊䝗
Wookong
Wookong
M M
Fig.5 Laser scanning by quadcopter䜹䝯䝷
する場合が考えられる．すなわち，既知特徴点は信頼度が高く，未知特徴点は信頼度が低いといえる．そこで提案システムでは，信頼度の異なる多数の特徴点から基本行列を安定に推定するために，Botterill らにより提案された BaySAC [6]を用いる．

LRF

Fig.3 Quadcopter for laser measurement

3.1

Raspberry Pi

BaySAC [6] とは基本行列の推定に一般に用いられる
RANSAC 法に確率を導入したものである．本手法では，まGPS᥋⥆➃Ꮚ

ず画像間の各特徴点対応に対し，それぞれの信頼度に応じた初期確率を与える．次に，確率の高い特徴点対応から選択し，基本行列を計算する．また各対応の確率は，反復計算の途中で以下の式により更新される．

㧗ᗘィ䝆䝱䜲䝻䝉䞁䝃

{Pt (i ∈ I) =3㍈ຍ㏿ᗘ䝉䞁䝃

Pt−1 (i∈I)−P (Ht ⊆I)
1−P (Ht ⊆I)

Pt−1 (i ∈ I)i ∈ Hti ̸∈ Ht(1)Ht : 計算に用いた対応集合
I : インライア集合

Fig.4 Processing board

クワッドコプタ本体には LRF と小型カメラを搭載し，LRF を地面鉛直下向きに設置し，計測対象の上空を飛行することで，スリット状の 3 次元距離データを得る (Fig.5)．
また計測と同時にカメラにより地表面の撮影を行い，後述する手法によりクアッドコプタの位置姿勢を推定する．得られた距離データを推定された位置姿勢を用いて座標変換することで，対象の 3 次元形状を復元する．

3.

BaySAC による基本行列の推定

既知および自然特徴点を用いた位置姿勢推定

レーザ計測時のクアッドコプタの位置姿勢は，連続する 2枚の地表面のカメラ画像間で既知，および自然特徴点を追跡し，基本行列（Essential Matrix, E 行列）を求めることで推定する．前述のように，既知特徴点には 3 次元位置が正確に同定された地上走行型の移動ロボットを用いる．一方，自然特徴点には画像から抽出される SURF 特徴点を用いる．
既知特徴点となる地上走行型ロボットには，追跡や画像間の対応付けが失敗しないように既知のマーカを取り付け，出来る限り安定した追跡を実現する．一方，画像から抽出される自然特徴点は，見えの変化により追跡や対応付けが失敗

ここで，Pt (i ∈ I) は i 番目の対応がインライアである確率，P (Ht ⊆ I) は選択した対応集合が全てインライアである確率を示している．また P (Ht ⊆ I) は以下の式で求められる．

P (Ht ⊆ I) =∏Pt−1 (j ∈ I)

(2)

j∈Ht

本システムでは，既知特徴点（地上走行ロボット）の初期確率を高く，自然特徴点の初期確率を低く設定する．

3.2

N-M Corespondences

N-M Corespondences [6] とは，BaySAC において，2 つの画像の特徴点間で複数の対応が考えられる際に用いられる手法である．例えば，Fig.6 では，照明や柱に対して複数の対応が考えられる．ここで仮にある組み合わせが真であるとすると，それ以外の組み合わせは全て偽となる．逆にある組み合わせが偽であるならば，他の組み合わせが真である確率は増加する．よって，N-M Corespondences では以下の式に従い，BaySAC による確率の更新時に，他の特徴点- 0973 -対応の確率を同時に更新する．

4.2
Pt ((i, k) ∈ I|(i, j) ∈ Ht ) =Pt−1 ((i, k) ∈ I)
(1 − Pt ((i, j) ∈ I))(1 − Pt−1 ((i, j) ∈ I))(3)

Pt ((i, k) ∈ I)) は特徴点 i, k の対応がインライアである確率である．式 (3) は Pt−1 ((i, j) ∈ I)) が変更された場合のPt−1 ((i, k) ∈ I)) を表しており，Pt ((i, j) ∈ I)) の変化 (他の特徴点対応の確率変化) に反比例して確率が変更される．

ペナルティの追加

提案する手法では，カメラはほぼ直進すると仮定していることから，進行方向が大きく変化する解は推定に失敗している可能性が高い．
また 4.1 項で示した手法では，移動開始・終了時の画像を用いて，全時刻の位置が推定される．そこで，4.1 項で示した手法で推定された位置（予測値）と，時刻順に逐次的に求められた位置（推定値）を組み合わせて，より正しい位置を推定する．具体的には，まず予測値と推定値の差を以下の式で計算する．

⃗⃗
X́ ) 11( X·+2 ∥X∥ ∥X́∥2(4)⃗⃗ が推定値であり，X́Xが予測値である．次に，BaySAC
は RANSAC と同様に，反復計算でインライア数が最大となる解を計算することに着目し，反復計算中に式 (4) で得られた値をインライア数に掛けることで，予測値から大きく外れた推定値を除外する．なお，予測値は式 (4) の計算にのみ使用し，最終的な計算結果には推定値をそのまま用いる．

Fig.6 N-M Corespondences [6]

3.3

4.3

スケールの計算BaySAC により求められた基本行列を特異値分解し，画像間の並進・回転行列を推定する [5]．この際，計測には単眼カメラを用いていることから，スケールは不定である．一方，地上走行ロボットは，CPS により 3 次元位置が同定されている．そこで本システムでは，地上走行ロボット間の距離を用いてスケールを推定する．

提案した位置同定法では，時間的に隣り合う 2 枚の画像を用いて位置姿勢の変化を推定し，それを繰り返して複数画像間の位置姿勢推定を計算する．しかし本手法では，推定を繰り返すうちに推定誤差が蓄積することは避けられない．
そこで提案システムでは，カメラ（クワッドコプタ）がほぼ直進する場合を想定し，より一層の位置推定精度の向上のための再帰的位置推定法を開発した．以下に提案手法の詳細を示す．

4.1

再帰的位置推定長時間の移動による誤差の累積を低減するため，Fig.7 に示すように，まず最初と最後の画像間で変位を計算し，次に中央の画像の変位を計算する．以降，再帰的に計算を行い，各画像の変位を推定する．

1ᯛ┠n/2ᯛ┠Fig.7 Calculation ordernᯛ┠Fig.7 において，(R1 , t1 , S1 ), (R2 , t2 , S2 ) を合成した際に(R, t, S) と一致するとは限らない．そのため，スケールの再計算を行い，誤差を最小化する．具体的には，以下の式を最小化する．

(St − S1′ R2 t1 − S2′ t2 )2 + α{(S1 − S1′ )2 + (S2 − S2′ )2 } (5)ここで，α は平滑化項に対する重みであり，ここでは 0.5としている． S1′ , S2′ は再計算後のスケールである．

再帰的計算による位置推定精度の向上

4.

スケールの調整

5.

計算機シミュレーション

計算機シミュレーションにより，提案した既知，自然特徴点を用いた位置同定手法の有効性を確認する．ただし自然特徴点は 500 点，既知特徴点は 3 点（すなわち地上移動型ロボット 3 台）とし，それぞれの特徴点には正規乱数により画像上でノイズを加える．また，自然特徴点では 30% の確率で対応付けの失敗が発生し，ノイズは既知特徴点の 3 倍とする．
この条件で以下の 2 つの場合に対して計算機シミュレーションを行った．

1. カメラの移動距離，すなわち特徴点の画像上での変化量を変更した場合

2. 既知，自然特徴点の初期確率を変更した場合

5.1

移動距離に対する位置推定成功率の変化

2 枚の画像に対し，カメラが十分に長い距離を移動して撮影した場合，カメラの進行方向は比較的正確に推定できる．
逆に，カメラの移動距離が非常に小さい場合，カメラの進行方向を正しく計算することは困難である．そこで，画像上の特徴点の平均移動量 (ピクセル) に対し，正しい位置同定が行えるかどうか，シミュレーションを行った．ただし，自然特徴点の最大初期確率は 0.6，既知特徴点の初期確率は- 0974 -0.7 とした．また，シミュレーションは 1000 回繰り返して行い，カメラは Y 軸 (光軸に対し垂直) に沿って移動する．
成功判定には真値と推定値 (両者とも長さ 1 に正規化) の内積を用い，値が 0.9 以上の場合に位置推定に成功したと判定した．
シミュレーション結果を Fig.8 に示す．

LED䝬䞊䜹䞊YSuccess rate [%]1008060X4020

Fig.9 Experimental models

0050100150200250

Distance [pixel]

Fig.8 Success rate for various distances䐠Z

これより，画像間のカメラの移動距離が長くなる (特徴点の変位が大きくなる) ことで，位置同定の性能が向上することがわかった．

5.2

䐟

Y

初期確率に対する位置推定の成功率

自然特徴点，既知特徴点の初期確率を変更した場合の位置推定の成功率を調べた．成功判定には 5.1 項と同じものを用いた．1000 回のシミュレーションを繰り返した時の位置推定の成功率を Table 1 に示す．なお，この時の特徴点の平均移動距離は 106 ピクセルであった．これより，既知特徴点の初期確率を高くすることで，位置同定の性能が向上することが確認できた．

X
Fig.10 Measurement paths

Table 1 Success rate for various initial values of probability

6.1

5.2 項の結果が，実機実験においても成立するのか確認を行った．結果を Table 2 に示す．なお，未知特徴点の初期確率は 0.6 としている．これより，既知特徴点の初期確率を1.0 とした場合，逆に精度が低下していることが分かった．
3.1 項より，初期確率を 1.0 とした場合，その特徴点対応は常に計算に用いられる．実機実験ではシミュレーションより既知特徴点に対するノイズが大きくなり，常に計算に用いることで逆に精度が低下したと考えられる．以降，既知特徴点の確率が 0.9 での結果を示す．これは，誤差の平均値が最も小さいためである．

既知特徴点の初期確率0(既知0.71.0点無し)

6.

未知特徴点0.5

83.1%

84.6%

84.7%の初期確率

0.6

80.7%

85.3%

88.4%

0.783.6%

85.2%

86.6%

初期確率に対する位置推定誤差

実機実験

Table 2 Range error for various initial values of probability

提案手法の有効性を確認するために，クワッドコプタは飛行させずに，空中に固定した棒の上を手動で移動させ，床面に設置された住宅模型 (Fig.9) を計測する実験を行った．床面には自然特徴点を抽出するために航空画像が敷かれており，また既知特徴点としてロボットの代わりに LED マーカを配置した．真値の計測には光学式モーションキャプチャシステム（Bonita B10，VICON）を用いた．
移動経路を (Fig.10) に示す．実験では，鉛直上向きを Z軸とする世界座標系に対し，X 軸，Y 軸方向にクアッドコプ
タを移動させて，2 回計測を行った．

既知特徴点の初期確率

0.7

0.8

0.9

1.034.7mm

40.8mm

33.5mm

90.7mm

O 18.6mm 31.1mm 28.3mm 85.7mm
差 経路O 50.9mm 50.4mm 38.6mm 95.7mm
誤- 0975 -経路 12平均値

6.2

位置推定精度O

Y 軸方向 (Fig.10- 1 ) に移動した場合の Y 軸方向の位置推定結果を (Fig.11) に示す．赤線が真値，緑線が画像間の変位をそれぞれ独立に計算し，それらを時系列で積算した結果，青線が提案手法の結果である．

ཎⅬ䛛䜙䛾㊥㞳 [mm]

2800
2600

᫬⣔ิ㡰
2400

ᥦ᱌ᡭἲ

2200
2000

┿್

Fig.13 3-D point cloud data

1800
010203040ィ ᅇᩘ50た状態で計測を行い，提案システムの有効性を確認する予定である．

OFig.11 Experimental result when the quadcopter
moves the path 1文献O同様に，X 軸方向 (Fig.10- 2 ) に移動した場合の X 軸方向の位置推定結果を (Fig.12) に示す．

ཎⅬ䛛䜙䛾㊥㞳 [mm]

6800
6600

ᥦ᱌ᡭἲ
6400

┿್6200

᫬⣔ิ㡰

6000
5800
01020304050ィ ᅇᩘO

Fig.12 Experimental result when the quadcopter
moves the path 2

これらより，両方向ともに提案手法より位置推定精度が向上していることがわかる．

6.3

３次元計測結果

計測の結果，得られた３次元点群を (Fig.13) に示す．視点は (Fig.9) とほぼ同じである．これより，住宅模型の 3 次元形状が計測できていることが確認できる．

7.

まとめ

本稿では，地上走行型ロボットとクアッドコプタを用いた上空からのレーザ計測システムを提案した．本システムは，レーザ計測時に撮影された地表面画像から，地上移動型ロボットである既知特徴点，および画像内の未知特徴点を抽出し，BaySAC により基本行列を推定することで，クワッドコプタの位置姿勢を推定するものである．また，コンピュータシミュレーションおよび実機実験により，提案手法の基本性能を確認した．今後は実際にクワッドコプタを飛行させ- 0976 -[1] 野田裕介, 倉爪亮, 岩下友美, 長谷川勉, レーザ・カメラ搭載群ロボットによる太宰府天満宮の幾何光学計測, 画像の認識
理解シンポジウム (MIRU2009), IS3-34, 2009[2] Wei Li, Tianguang Zhang, Kolja Kühnlenz,
A Vision-Guided Autonomous Quadrotor in An AirGround Multi-Robot System , IEEE International Conference on Robotics and Automation, pp.2980-2985,
2011.
[3] Volker Grabe, Heinrich H. Bulthoﬀ, and Paolo RobuﬀoGiordano,On-board Velocity Estimation and Closed-loop Controlof a Quadrotor UAV based on Optical Flow , IEEE
International Conference on Robotics and Automation,
pp.491-487, 2012.
[4] Ryo Kurazume, Yusuke Noda, Yukihiro Tobata, Kai
Lingemann, Yumi Iwashita, Tsutomu Hasegawa,
Laser-based Geometric Modeling using Cooperative
Multiple Mobile Robots, in Proc. , IEEE International
Conference on Robotics and Automation, pp.3200-3205,
2009.
[5] David Nistér,
An Eﬃcient Solution to the Five-Point Relative
Pose Problem IEEE TRANSACTIONS ON PATTERNANALYSIS AND MACHINE INTELLIGENCE, pp.756770, 2004.
[6] Tom Botterill, Steven Mills, Richard Green,New Conditional Sampling Strategies for Speeded-Up
RANSAC British Machine Vision Conference, pp.1-11,
2009.