2018 年度卒業論文 GPS を使わないBebop Droneの …lang.sist.chukyo-u.ac.jp/Classes/seminar/Papers/2018/T...要旨題名 GPS を使わない Bebop Drone の自動飛行

2018 年度

卒業論文

GPS を使わない Bebop Drone の自動飛行

指導教員白井英俊教授

中京大学工学部電気電子工学科

学籍番号 T215054

氏名寺西廉

(2019 年 1 月)

要旨

題名 GPSを使わない Bebop Droneの自動飛行

学籍番号 T215054 氏名寺西廉指導教員白井英俊

近年、スマートフォンによるドローンの操縦アプリが開発され、その多くは自動飛行機能が実装さ

れている。しかし、それらの自動飛行は GPSを用いたものであるため、屋内など GPSの弱いエリア

では正常に飛行しない可能性がある。そこで本研究では ROSと SLAM を用い、指定したルートに

沿って GPSを用いずにドローンを飛行させることを目的とした。

本研究では Parrot社の BebopDrone2を使用し、その制御に ROS-kineticを使用した。また自動

飛行には ROSパッケージの bebop_autonomyと visual slamの ORB-SLAM を使用した。

SLAM とは、ロボットによる自己位置推定と地図作成の同時並行処理のことである。ロボット

が移動しながらセンサで周囲を計測し、移動軌跡に沿って地図を作る。この移動軌跡は自己

位置推定によって得られるものであり、それまで作成していた地図を利用し推定精度を上げる。

これを繰り返すことで精度の高い地図作成と自己位置推定を行うことができる。

本実験では一度マニュアルで飛行させて SLAM を実行する。マップを作成し、ルート飛行

に必要な離陸地点と目的地、またその間にチェックポイントを設定する。離陸地点、目的地、

チェックポイントの位置データを YAML 形式ファイルに記述する。このファイルを読み込ませ、

自動飛行を開始する。離陸地点から最初のチェックポイントに向けて飛行する。チェックポイントに

到達したら次のチェックポイントへ飛行。順次これを繰り返すことで目的地まで飛行することができ

る。

本研究では基本となる「直進」ルート飛行と「方向転換」ルート飛行を 5 回行った。結果はルートを

大きく外れ緊急着陸することなく目的地まで飛行することが出来たが、目的地と実際の着陸地点に

少し誤差が出てしまった。これは各チェックポイントに設定した通過許容範囲の影響だと考えられる。

また方向転換の方が直進に比べ誤差が大きくなった。これは転換のための回転時に機体位置が

乱れることがあるためである。

展望としては、方向転換時の機体の乱れを小さくすること、また、衝突回避を実装することに取り

組みたい。

i

目次

1. 初めに ....................................................................................................................................... 1

2. 研究の開発環境・構成要素 ......................................................................................................... 2

2.1 開発環境 ................................................................................................................................... 2

2.2 ROS (Robot Operating System) ................................................................................................ 2

2.3 bebop_autonomy ....................................................................................................................... 2

2.4 SLAM (simultaneous localization and mapping) ...................................................................... 2

2.5 ループクロージング .................................................................................................................. 4

2.6 ORB-SLAM .............................................................................................................................. 5

2.6.1 ORB Oriented FAST and Rotated BRIEF ........................................................................ 5

2.6.2 ORB-SLAM アルゴリズム ................................................................................................ 5

3. 研究内容 ...................................................................................................................................... 7

3.1 SLAMの選定 ....................................................................................................................... 7

3.3 ポイント飛行プログラムの作製 .................................................................................................. 8

4. 飛行システム................................................................................................................................. 9

4.1 飛行手順 ................................................................................................................................... 9

4.2 ルート飛行................................................................................................................................. 9

5. 結果・考察 .................................................................................................................................. 10

6. 展望 ............................................................................................................................................ 12

参考文献 ............................................................................................................................................ 13

付録 .................................................................................................................................................... 14

1．Bebop Drone2との接続プログラム bebop_autonomy .............................................................. 14

2．ドローンとのカメラ接続プログラム .............................................................................................. 15

3．ORB-SLAM ros_mono.cc ........................................................................................................ 16

4．自動飛行プログラム ................................................................................................................... 18

1

1. 初めに

近年、スマートフォンによるドローンの操縦アプリが開発され、その多くは自動飛行機能が実装さ

れている。しかし、それらの自動飛行は GPSを用いたものであるため、屋内など GPSの弱いエリア

では正常に飛行しない可能性がある。そこで本研究では ROSと SLAM を用い、指定したルートに

沿って GPSを用いずにドローンを飛行させることを目的とした。

本研究では Parrot社の BebopDrone2を使用し、その制御に ROS-kineticを使用した。また自動

飛行には ROSパッケージの bebop_autonomyと visual slamの ORB-SLAM を使用した。

SLAM とは、ロボットによる自己位置推定と地図作成の同時並行処理のことである。ロボット

が移動しながらセンサで周囲を計測し、移動軌跡に沿って地図を作る。この移動軌跡は自己

位置推定によって得られるものであり、それまで作成していた地図を利用し推定精度を上げる。

これを繰り返すことで精度の高い地図作成と自己位置推定を行うことができる。

本実験では一度マニュアルで飛行させて SLAM を実行する。マップを作成し、ルート飛行

に必要な離陸地点と目的地、またその間にチェックポイントを設定する。離陸地点、目的地、

チェックポイントの位置データを YAML 形式ファイルに記述する。このファイルを読み込ませ、

自動飛行を開始する。離陸地点から最初のチェックポイントに向けて飛行する。チェックポイントに

到達したら次のチェックポイントへ飛行。順次これを繰り返すことで目的地まで飛行することができ

る。

本研究では基本となる「直進」ルート飛行と「方向転換」ルート飛行を 5 回行った。結果はルートを

大きく外れ緊急着陸することなく目的地まで飛行することが出来たが、目的地と実際の着陸地点に

少し誤差が出てしまった。これは各チェックポイントに設定した通過許容範囲の影響だと考えられる。

また方向転換の方が直進に比べ誤差が大きくなった。これは転換のための回転時に機体位置が

乱れることがあるためである。

展望としては、方向転換時の機体の乱れを小さくすること、また、衝突回避を実装することに取り

組みたい。

本論文の構成は次のようである。2 章では開発環境と本研究で使用した ROSと SLAM について

説明する。第 3 章では飛行システムについて説明する。第 4 章では研究結果について考察する。

第 5 章では 4 章を受けて、これからの展望について述べる。

2

2. 研究の開発環境・構成要素

本章では、研究の開発環境と研究の構成要素について述べる。

2.1 開発環境

開発環境を表 2.1に示す。

表 2.1 開発環境

デバイス Parrot bebop drone2

OS Ubuntu16.04

デバイス制御フレームワーク ROS kinetic

デバイス制御パッケージ bebop_autonomy

SLAM ORB-SLAM

2.2 ROS (Robot Operating System)

ROS は、ロボットアプリケーションの作成を支援するライブラリとツールを提供しているロボット制

御フレームワークである(小倉 2015)。 ROS は主にハードウェア抽象化, デバイスドライバ、ライブ

ラリ、視覚化ツール、メッセージ通信、パッケージ管理の 6 機能に分けられる。

2.3 bebop_autonomy

bebop_autonomyは Parrotの公式 ARDroneSDK3に基づく Bebop Droneの ROSドライバーであ

る。本実験ではドローンに搭載されたカメラからのストリームおよび情報の取得、メッセージの送受

信として使用した。

2.4 SLAM (simultaneous localization and mapping)

SLAM とは自己位置推定と地図作成を同時並行処理のことである(友納 2018)。ロボットが移動

しながらセンサで周囲を計測し、移動軌跡に沿って地図を作る。この移動軌跡は自己位置推定に

よって得られるものであり、それまで作成していた地図を用いて推定精度を上げている。

図 2.1のようにロボットは移動しながらランドマークに対し、センサデータを取得する。ランドマーク

とは位置の目印になるものである（友納 2018）。マーク A に注目するとロボはデータ 1.2をとる。し

かしながらセンサデータをそのままつなげると図 2.2のようになってしまう。そこで自己位置推定を行

うことで図 2.3 のような地図を作ることができる。しかし自己位置推定の誤差により A、A’のようにマ

ーク位置にズレが出てしまうため、データ 1をとった時の周辺地図を用いてデータ 2をとった時の自

己位置とマーク位置を修正する。そうすると図 2.4のように正確な地図を作ることができる。これを全

てのランドマークに適用し、繰り返すことで精度の高い地図を作成することができる。

3

図 �� センサによるデータ取得

図 �� センサデータを繋げる

図 �� 位置推定してデータを繋げる

図 �� 位置推定を修正してデータを繋げる

4

�� ループクロージング

�� 節で述べたように、ランドマークを測定して、その測定データを基に自己位置推定を行う。そ

の自己位置推定はそれまで作成していた地図を用いて修正する。修正された推定位置よりランド

マーク位置を修正して周辺の地図を作成する。これを繰り返すことで ��は誤差を減らし精度を

高めている。しかし、それだけでは誤差を完全に消すことはできない。ランドマーク計測に誤差があ

るためである。ランドマーク位置が外部より与えられたものであれば、その計測誤差を修正すること

ができるが、�� はそれまで作成した地図のランドマーク位置を使用して、測定したランドマーク

位置を修正しているので、誤差を消し切れず、累積してしまう。そのため地図が歪み図 �� のように

実際には元の位置に戻ってきているはずなのに、誤差の影響でループが閉じなくなってしまう。

ループクロージングとは、このループを閉じて地図を修正するものである。図 ��（�）はロボットが

�� から �� にむけて移動したときの �� 結果を示している。��でロボットは �� で計測したランドマ

ーク �� を計測する。しかし �� は累積誤差によりずれてしまっているので ��のようになる。こ

の �� と ��が同じ位置にくるように �� を修正する。こうして �� を修正すると今度は ��-� のずれ

が大きくなるので ��-� も修正する。こうしてすべての位置とその位置でのランドマーク位置を修正す

ることで地図全体の誤差を修正することができる（友納� ��）。

図 �� 誤差累積による位置推定のズレの例

図 �� ループクロージング解説図（友納正裕� ��）

5

�� -��

��-��とは単眼カメラで動作する ��の一種であり、��特徴量を用いる特徴ベー

スのＳＬＡＭである。

��

�� とは方向付き �� と回転 �� を合わせたものである。

�� とは、画像中のコーナーを高速に検出する方法である。図 �� のように画素 � の周辺の円

周上の ��画素を観測し、輝度が閾値以上のピクセルが連続して �個以上になる点を特徴点とする。

円周上の画素から観測する画素を選び、「決定木」を構成して、コーナーであることを学習させる。

（��）

図 ��

�� とは、マッチングを高速で行うことのできる特徴量記述子である。�� は平滑化された

画像のパッチを取り、� 個の��位置のペアを選ぶ。それらの位置のペアにおいて輝度値の比較

を行い、� 値コード列を返す。特徴点をマッチングする際、� 値コードの方が演算が高速になる。

つまり、�� とは特徴点を高速で検出し、前に検出した特徴点とのマッチングも高速で行えるも

のである。

�� -�� アルゴリズム

図 �� に ��-�� のアルゴリズムの構成を示す。��-�� はカメラからの画像を入力（フ

レーム）とする。フレームを受け取ったトラッキングではフレームから特徴点を検出し、前の画像とマ

ッチングを行うことで現在のカメラ姿勢を求める。またフレームをキーフレームとして次のローカルマ

ッピングに引数を渡す。キーフレームを引数として渡されたローカルマッピングでは地図を作成、修

正する。いままでのキーフレームに新たなキーフレームを追加し、そのキーフレームの特徴点をマ

ップに追加する。また追加されたキーフレームとその周辺のキーフレーム範囲でカメラ位置と特徴

点を修正、最適化する。その後ループクロージングで地図全体を最適化する。追加されたキーフレ

ームが既に検出されたものであるかどうか判断し、ループが検出されたらループが結ばれるように

最適化する。

6

図 �� -�� アルゴリズム（��-��）

7

3. 研究内容

本研究ではまず使用する SLAM の選定、コンパイルから行った。その結果本研究では

ORB-SLAM を採用した。次に（Mur-Artal & Tardos 2015）からクローンした ORB-SLAM をドローン

のカメラで起動させるためにコードの書き換えとキャリブレーションファイルの編集を行った。また、

ドローンからカメラ情報を得るプログラムを作製した（付録１）。次に ORB-SLAM からの位置情報

をポイントとして、そのポイントに向けて飛行するプログラムを作製した。最後にそのプログラムを元

に、事前に設定したルートに沿ってドローンが自動飛行するシステムを作成した

本章では本研究の研究内容を研究の流れに沿って説明する。

3.1 SLAM の選定

2.4.1節で述べたように SLAM ではロボットが移動しながらセンサで周囲を計測し、移動軌跡に

沿って地図を作る。基本的に SLAM を使用するロボットにはレーザーレンジファインダなどの距離

センサを搭載させるのだが、本研究で使用する bebop drone2には距離センサが搭載されておらず、

本研究ではカメラのみで動作する visual slamを使用することにした。ドローン本体に距離センサを

搭載して SLAM を行うことも考えたが、センサを搭載した際の機体重量の変化、重心の変化を

bebop_autonomyに反映させることは難しいと判断した。

本研究では visual slamとして ORB-SLAM, LSD-SLAM, Hector-SLAMの 3 つの中から選ぶこと

にした。Visual slam には他にも Gmappingや Google Cartographerなどが存在するが、3 次元空

間上で使用することが出来るものでライセンスなどを考慮し上記の 3 つとした。

LSD-SLAM は基本的に ROS indigoを推奨しており、本研究の開発環境である ROS kineticでは

かなりコードを書き換えないとコンパイルすることが出来なかった。また、コンパイルは出来たものの

何故か Open CVと競合し、Open CVをアンインストールした環境でなければ動作させることができ

なかった。本研究ではドローンのカメラストリームに Open CV を使用するため開発環境の面から

LSD-SLAM は不採用とした。

Hector-slamは ROS kineticでの動作が保障されているため簡単にコンパイルすることが出来た。

しかし ORB-SLAM と比べ、自己位置推定の精度が低くなった。調べると、Hector SLAMにはルー

プクロージングが無いことが分かった。そのため精度が低くなってしまっていると考えられる。この段

階ではデフォルトの設定で動作させていたためキャリブレーションなどで改善する可能性も考えら

れたが 3次元空間を飛行するためにはより高い精度が必要となると考えループクロージングが無い

Hector SLAMも不採用とした。

ORB-SLAMはコンパイル時にコードの書き換えが必要ではあるが ROS kineticで動作させること

ができ Hector SLAMよりも精度が良かったため本実験では ORB-SLAM を採用した。また先行研

究（酒井研斗堀浩一 2014）では地図作成と自己位置推定に PTAM を使用している。PTAM とは

2008年に発表された自己位置推定と地図作成を行う手法であり、ORB-SLAM は PTAM に次ぐ手

法として 2014 年に発表されたものである。そのため先行研究を活かしやすいと考えたことも

ORB-SLAM を採用した理由の 1 つである。

8

3.2 ORB-SLAM の書き換え

3.1 で触れたコンパイル時の ORB-SLAM のコード書き換えについては github の Issues 377

（Mur-Artal & Tardos 2015）を参照し行った。

ORB-SLAM をコンパイルし、ドローンのカメラで実行させたが、ORB-SLAM がカメラ情報を取得

できずフリーズしてしまった。そこで ORB-SLAM を USBカメラで実行させて ORB-SLAM が購読し

ているトピックを調べたところ/camera/image_rawトピックを購読し動作していることが分かった。ドロ

ーンからカメラ情報を取得している bebop_autonomyはカメラ情報を/bebop/camera_rawトピックとし

ているため ORB-SLAM がカメラ情報を見つけることが出来ずフリーズしてしまっていた。そこで付

録 3 の購読トピックを指定するコードの/camera/image_rawを/bebop/camera_rawに書き換えること

で ORB-SLAM が/bebop/camera_rawを購読するように変更した。

上記の変更によりドローンのカメラで ORB-SLAM を実行することが出来たが、ORB-SLAM に用

意されていた yaml ファイルのカメラパラメータで動作させたためドローンを少し早く動かしただけで

特徴点を検出することが出来なくなった。そこでカメラをキャリブレーションして yaml ファイルを

bebop drone2のカメラパラメータに書き換え精度を改善させた。またマップが疎であったため特徴

点検出量を 1000から 2000に変更した。

3.3 ポイント飛行プログラムの作製

ドローンに限らず、四輪ロボットなどのいろいろな GPS自動運転のプログラムをネットで調べてみると、

使用者がルートを設定して、そのルート上を自動で移動するタイプの自動運転プログラムの多くは、通

行ポイントとそのポイントの位置情報を設定して、ポイントからポイントに向けて前進、ロボットの位置情報

が設定したポイントの位置情報と重なれば到着したらと判断。必要に応じてポイント上で次のポイントの

方向へ方向転換しまた次のポイントへ向けて前進するという動作を繰り返すものであることが分かった。

このシステムならば必要な動作は「前進」と「方向転換」だけであるため、比較的作成しやすいのではと

考え、本実験はルートを設定して、そのルート上を自動で飛行する自動飛行システムの作成を目指すこ

とにした。プログラムの作製には ardrone プログラム（銭飛 2016）や ROS のパッケージである

tum_ardroneの GPSパイロットプログラム、bebop_autonomyの GPSウェイポイントナビゲーションなどを

参考にして、ORB-SLAM の位置情報をポイントとする飛行プログラムを作製した。

また、このルート飛行システムは ORB-SLAM の位置情報を事前に設定する必要があるため、

ORB-SLAM の位置情報をトピックとして配信させるコードを ORB-SLAM に追加した。

ルート飛行システムについては次の 4 章で詳しく説明する。

9

4. 飛行システム

本章では飛行手順と作成したシステムのルート飛行について述べる。

�� 飛行手順

本実験で作成したシステムの飛行手順を以下に示す。

① ドローンと PCを ROSで無線接続する。

② マニュアルで飛行させる。この時 SLAM を実行し、マップを作製する。

③ ルートの設定：自動飛行のための離陸地点と目的地、その間のチェックポイントを設定。これを

YAML 形式ファイルに記述する

④ ドローンを離陸地点に運び、地図と③のファイルを読み込ませ、自動飛行開始

⑤ 最初のチェックポイントに向けて飛行。チェックポイントに到達したら次のチェックポイントへ飛

行。順次これを繰り返す。

⑥ 目的地に到着したら着陸

�� ルート飛行

離陸地点より離陸したドローンは最初のチェックポイントに向けて飛行する。図 �� のようにチェッ

クポイントには通過許容範囲（オレンジの範囲）を設定しており、ドローンがこの範囲に入ると次の

チェックポイントに飛行する。これを繰り返し、目的地まで向かい着陸する。

本研究ではルート飛行の基本となる「直進」と「方向転換」飛行を行った。

図 �� ルート飛行

10

5. 結果・考察

直進飛行と方向転換飛行を � 回ずつ行い、目的地と実際に着陸した地点の誤差を、図 �� の黒

い点を基準に測定した。そのときの結果を表 �� に示す。またこのときのルートを図 �� に示す。

表 �� 飛行誤差

直進方向転換

�� 秒 �� 後 �� 秒

飛行回数誤差� ��

� ��

� ��

� ��

� ��

� ��

図 �� ドローンのサイズと測定基準

11

図 �� 飛行したルートの略図

表 �� に示したように、直進では平均 ��、方向転換では平均 �� の誤差が出ている。ま

た標準偏差は直進が ��、方向転換が �� となっている。標準偏差の値から、誤差の値にばらつ

きが少ないことがわかる。そのため直進では誤差 �� ㎝前後、方向転換では �� ㎝前後の範囲内で

安定して飛行できているといえる。

また、この誤差は各チェックポイントに設定した通過許容範囲の影響だと考えられる。そのためポ

イントの範囲を調節することで小さくできる。が、�� での位置測定では全く同じ位置データが出

ることが少なく、許容範囲を設定せずに飛行することはできないため誤差を完全になくすことは難し

い。また方向転換時の誤差が直進時と比べ大きくなっているのは回転時に機体位置が乱れてしま

うためであるが、これについては機体回転スピードの調整や転換専用に設定したチェックポイントを

作成することで解決できると考える。

本研究に似た研究として福井��）がある。そこでは飛行させて地図を作製した後、その � 次元

地図内で着陸地点へ向かうルートを設定することができる。これと比べると、本研究は地図作成の

マニュアル飛行中にチェックポイントや着陸地点の位置情報を取得しなければならない点が短所と

なる。しかし、本研究では飛行中にチェックポイントの位置を取得する、つまりマップを作りながらル

ートを設定している。� 次元地図内でルートを設定する方法に比べ、飛行させながらの設定では、

ルートを修正するためは飛行させなければならない。

本研究で � 次元地図内でルートを作成することができない理由は、現実空間上の距離をとること

ができないからである。福井（��）では ��-� カメラを使用している。��-� カメラは深度を取る

ことができ、現実空間と同じ軸を持つ � 次元地図が作成できるため、地図内でルートが設定できる。

それに対し本研究の単眼カメラは距離を取ることができないため、作成された地図は現実空間とは

違う独自の空間軸を持つ。そのため地図内で地図を作ることができず、現実空間上を実際に飛行

させながら設定しなければならない。

12

6. 展望

本研究の目的である GPS を用いずにドローンを自動飛行させることについて、目的地と着陸地

点に誤差はあるものの、安定してルートに沿って GPS を用いずに自動飛行させることができた。し

かし、衝突回避を実装することができなかった。

自動飛行において障害物、特に壁面との衝突は大きな問題である。本研究ではルート飛行のた

め壁面の存在しないルートを選択すればいいのだが、万が一ルートを外れた場合や、狭い空間を

飛行する場合には壁面衝突のリスクが伴う。そこで展望では壁面衝突回避システムについて述べ

る。

Bebop drone2に搭載された単眼カメラでは壁面と本体の距離を計測することができないため、カ

メラのみでは距離による壁面衝突回避は実装できない。しかし、本研究ではドローン単体で自動飛

行させているため壁面回避もドローン単体で行いたい。そこで距離センサを搭載する方法ではなく、

ORB-SLAM のトラッキングミスを利用した壁面衝突回避システムを考える。ORB-SLAM はカメラが

壁面に近づくと、特徴量を検出できず、トラッキングに失敗することが多い。この特徴を利用し、壁

面に近づきトラッキングに失敗したら後退するプログラムを組み込めば、壁面との正面衝突を回避

することができるのではないかと考える。しかし、トラッキングは機体の乱れなど別の要因でも失敗

することがあるため、壁面に近づいていなくても後退してしまう可能性がある。そのためこのシステム

では不十分であると考えられる。

壁面回避システムを開発するうえで、利用できる可能性が高い研究（尾山 2017）が行われてい

る。この研究は深層学習を用いて RGB 単眼画像から距離を推定し深度画像を生成しそれを基に

SLAM を行う研究である。単眼カメラから推定距離を得ることができるため、本研究で使用している

ドローンの単眼カメラでも推定距離を獲得することができる。よって壁との居 0 里がとることができる

ため衝突を回避することが可能になると考えられる。また物体認識を導入すれば、壁以外にも障害

物を認識し、回避させることが可能になると考えられる。

また、単眼カメラで距離が取れるならば、考察で述べた本研究の短所も、現実空間と同じ空間軸

で地図が作れるようになるため解消できると考えられる。

13

参考文献

小倉崇(2015) ROSではじめるロボットプログラミング. 工学社

尾山匡浩(2017) 深層学習による空撮画像からの SLAM に関する研究人工知能研究振興財団

助成事業発表会

酒井研斗堀浩一(2014) 小型UAV による自律型SLAM システムの実証研究東京大学大学院工

学系研究科航空宇宙工学専攻

銭飛 (2016) ROSプログラミング. 森北出版

友納正裕(2018) SLAM入門. オーム社

福井友季也(2017) マルチコプターの屋内利用に向けた飛行制御プラットフォームの開発奈良先

端科学大学院大学情報科学研究科情報科学専攻

Mur-Artal R. & Tardos J. (2015) ORB=SLAM: A versatile and accurate monocular SLAM system.

IEEE Trans. On Robotics, 31(5). 1147-1163. https://github.com/raulmur/ORB_SLAM2

Rosten, E. & Drummond T. (2006) achine Learning for High-Speed Corner Detection. In: Leonardis

A., Bischof H., Pinz A. (eds) Computer Vision – ECCV 2006. ECCV 2006. Lecture Notes in

Computer Science, vol 3951. Springer, Berlin, Heidelberg

14

付録

1．Bebop Drone2との接続プログラム bebop_autonomy

copyright Copyright (c) 2015, Autonomy Lab (Simon Fraser University)

#include <string>

#include <vector>

#include <algorithm>

#include <ros/ros.h>

#include <nodelet/loader.h>

int main(int argc, char* argv[])

{

��

��

nodelet::M_string remap(ros::names::getRe��

��

��

��

��

if (std::find(loaded_nodelets.begin(),

loaded_nodelets.end(),

nl_name) == loaded_nodelets.end())

{

// Nodelet OnInit() failed

��

��

}

// It reaches here when OnInit() succeeds

��

��

��

}

15

2．ドローンとのカメラ接続プログラム

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import Image

import cv2, cv_bridge

class Follower:

def __init__(self):

self.bridge = cv_bridge.CvBridge()

self.image_sub = rospy.Subscriber('bebop/image_raw', Image, self.image_callback,

queue_size=1)

self.fdet = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

def image_callback(self, msg):

image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

cv2.imshow("window", image)

cv2.waitKey(3)

rospy.init_node('follower')

follower = Follower()

rospy.spin()

16

3．ORB-SLAM ros_mono.cc

Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)

#include<iostream>

#include<algorithm>

#include<fstream>

#include<chrono>

#include<ros/ros.h>

#include <cv_bridge/cv_bridge.h>

#include<opencv2/core/core.hpp>

#include"../../../include/System.h"

��

class ImageGrabber

{

public:

ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}

��

��

��

int main(int argc, char **argv)

{

��

��

if(argc != 3)

{

cerr << endl << "Usage: rosrun ORB_SLAM2 Mono path_to_vocabulary

��

��

��

}

// Create SLAM system. It initializes all system threads and gets ready to process frames.

17

��

��

��

ros::Subscriber sub = nodeHandler.subscribe("/bebop/image_raw", 1,

��

��

// Stop all threads

��

// Save camera trajectory

��

��

��

}

void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)

{

// Copy the ros image message to cv::Mat.

��

try

{

��

}

catch (cv_bridge::Exception& e)

{

��

��

}

mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr-��

}

18

4．自動飛行プログラム

#include "ros/ros.h"

#include "geometry_msgs/Twist.h"

#include "std_msgs/Empty.h"

#include "std_msgs/Bool.h"

#include <iostream>

#include <fstream>

��

class pid_control

{

public:

pid_control(){

��

��

��

��

��

��

��

��

��

��

��

��

publand_ = n_.advertise<std_msgs::Empty>("bebop��

sub_ = n_.subscribe("/pose", 1, &pid_con��

��

��

��

��

��

n_.getParam("roll_Kp", rol��

19

��

��

��

��

��

��

��

��

��

��

��

��

��

��

��

��

}

��

��

��

��

��

��

��

��

float pit_dist, roll_��

��

��

��

��

��

��

��

��

��

��

20

��

��

��

��

��

��

��

��

��

private :

��

��

��

��

��

void pid_control::callback(const geometry_msgs::Twist &msg){

��

��

��

if(rStep > 0)

{

��

��

��

��

��

rStep--�

��

��

��

msg��

��

myfile << i << '¥t' << 0 << '¥��

myfile <<'¥t' << 0 << '¥��

myfile <<'¥t' << 0 << '¥��

myfile <<'¥t' << 0 << '¥��

21

��

��

}

pit_dist = fabs(pit_ref[i] - pit_ref[i-��

pit_error = pit_ref[i] - ��

pit_error_dot = pit_error - ��

yaw_error = yaw_ref - ��

roll_error= roll_ref - ��

roll_error_dot = roll_error - roll_lastE��

��

��

��

if(msgOut.linear.y < -0.5)

msgOut.linear.y = -��

if(msgOut.linear.y > 0.5)

��

��

��

��

��

msgOut.angular.z = (-��

if(msg.linear.x > (pit_ref[i]-threshold) && msg.linear.x < (pit_ref[i]+threshold)){

��

if(i==1){

��

��

}

if(i==2){

��

��

}

if(i==3){

��

��

}

i++�

22

��

}

��

��

��

ROS_INFO_STREAM("--------------------------------"��

��

��

ROS_INFO_STREAM("--------------------------------��

��

ROS_INFO_STREAM("OUT_yaw ="<<��

ROS_INFO_STREAM("--------------------------------��

��

��

��

��

��

ROS_��

��

��

myfile << i << '¥t' << msg.linear.x << '¥t' << msg.linear.y << '¥t' <<msg.linear.z <<'¥t' <<

��

myfile << '¥t' << msgOut.linear.x << '¥t' << msgOut.linear.y << '¥t' << msgOut.angular.z

<<'¥��

��

��

}

int main(int argc, char **argv){

��

��

��

}

Documents

2018 年度 卒業論文 GPS を使わないBebop Droneの …lang.sist.chukyo-u.ac.jp/Classes/seminar/Papers/2018/T...要旨 題名 GPS を使わない Bebop Drone の自動飛行

2018 年度卒業論文 GPS を使わないBebop Droneの …lang.sist.chukyo-u.ac.jp/Classes/seminar/Papers/2018/T...要旨題名 GPS を使わない Bebop Drone の自動飛行