ドローンによる獣害対策に関する研究

平成

28

ドローンによる獣害対策に関する研究

-GNSS

指導教員 鬼頭 孝治准教授

王 秀崙教授

三重大学大学院 生物資源学研究科 共生環境学専攻 環境情報システム工学専攻 エネルギー利用工学教育研究分野 河野 純輝

目次

第

1

1.1

1.2

1.3

第

2

2.1

2.2

2.2.1

2.2.2

第

3

3.1

3.2

3.3

第

4

4.1

4.1.1

4.1.2 GNSS

4.2

4.3

4.4

4.4.1

4.4.2 GNSS

4.4.3

第

5

5.1

第

6

1

第1章 緒言

1.1

現在，中山間地域における野生動物による農作物に対する被害が深刻化している．農 林水産省によると被害額は年間およそ

200

7

また，農作物に対する被害だけでなく，獣害によって農家の営農意欲の減退⁴⁾も大きな 問題である．

1.2

獣害対策に向けた取り組みは大きく分けて個体数管理，被害防除，生育環境管理の

3

3

1

ICT

2

「追い上げ」を行ない，農作物への被害を防止することである．主に防護柵を用いた侵 入阻止，音や光などを用いた威嚇，人やモンキードックによる追い払いや追い上げが効 果を上げている．しかし音や光による威嚇では動物の慣れにより数か月で効果が薄くな り，防護柵や人による追い払いは多大な費用と労力を要する⁷⁾．

3

獣害対策は上記

3

1.3

本研究では上記の対策の中でも特に「被害防除」に注目している．本研究の目的は，

防護柵や人によって行なわれている野生動物の「追い払い（侵入防止）」と「追い上げ」

をドローンで代替させることである．これまでに，ドローンの動作が野生動物を威嚇す るために効果的であることはすでに報告⁷⁾したが，この威嚇を自動で行うためには野生 動物の検知からドローンの離陸・威嚇・着陸をすべて自動で行うシステムの構築が必須 となる．ここではそのプラットフォームとなるセンサやプログラムで自動飛行可能なド ローンを開発し，追い払いの基本となる機能を想定して，その動作確認を行った．

2

第2章 追い払いと追い上げの現状

2.1

「追い払い」とは，図

2-1

2-2

2-3

追い上げ時のチームワークが必要であり，高度な技術を要する．

図 2-1 追い払い

図 2-2 短距離の追い上げ 図 2-3 長距離の追い上げ

3 2.2

2.2.1

野生のシカやイノシシの追い払い（侵入防止）に防護柵が利用されている．図

2-4

2-5

2-6

しかし，柵の設置にはコストがかかる．三重県津市白山町上ノ村では補助金を使用し 集落の周囲約

8 km

2,000

図 2-4 折り返しのあるワイヤーメッシュ柵⁸⁾ 図 2-5 トタンで目隠しをした柵⁸⁾

図

2-6

4

また地理的要因により柵の設置が困難な場所には，光や音による威嚇でシカやイノシ シの追い払いが行われている．例えば三重県津市白山町上ノ村では，図

2-7

「超音波は自分たちに害はない」と学習し，効果は薄れた．図

2-8

またシカやイノシシに対しても，追い払いだけでなく追い上げを行なうべきであるが，

筆者の調べる限りシカやイノシシに対しての追い上げの事例が少ない．その理由として は，これらの動物は夜行性のため人間が深夜の追い上げが困難であることが考えられる．

図

2-7

図

2-8

5

2.2.2

シカやイノシシに対して有効な柵でも，サルにとっては容易に上ることが可能なので，

サル対策として図

2-9

サルに対して有効な追い払いをするためには，電気柵に加えて野生サルを見かけた場 合は，図

2-10

図

2-9

図

2-10

6

上記の追い払いは，野生サルの接近を目視で確認した後に追い払うものである．近年 は野生サルに

GPS

実際に人間によって追い払い・追い上げを行う際は，小規模なサルの群れに対しては 数人での追い上げが可能であるが，大規模な群れの場合は人間側も大人数で追い上げを 行う必要がある．図

2-11

5

1

5

またサルを急激に驚かすと，群れの移動方向のコントロールが困難になるので，追い 上げの際は群れをパニック状態にさせない程度の威嚇をする必要がある．

図

2-11

1班 追い上げ班

2班 追い上げ班 3班

コントロール班 4班

コントロール班

5班 指令班 指令

指令

7

第3章 ドローンを用いた追い払い・追い上げシステムの構想

3.1

本研究の最終目標は完全自動でドローンが野生動物の追い払いまたは追い上げを行 うことである．そのためには

3

第一ステップは野生動物の農地への侵入を検知し，さらにその野生動物の位置情報を リアルタイムで取得することである．現在用いられているセンサには

GPS

第二ステップは，センサから得られたリアルタイム位置情報を基にドローンへ自動で 指令を送ることである．外部に基地局(Ground station)を設置することで，威嚇対象の位 置情報とドローンの位置情報に基づいた指令をドローンに送ることができる．また，基 地局の機能をドローンに搭載することも可能であるが，外部に設置することで全体の状 況を外部から把握できるメリットがある．

第三ステップは，基地局からの指令に従ってドローンが威嚇を行うことである．プロ グラムに従って自動で飛行するドローンの製作が必要である．

これら

3

3

を含んだプラットフォームの構築が必要となる．

① 野生動物の検知とその位置情報を送信するセンサ部．

② 野生動物の位置情報を基にドローンに指令を送る基地局．

③ 実際に威嚇を行なうドローンである．

システムの全体像を図

3-1

図

3-1

GPS

基地局

防衛エリア 侵入検知

＋ 位置情報

侵入検知

＋ 位置情報

自動操縦

焦電 センサ

8

3.2

シカやイノシシはサルとは異なりフェンスを登ることができないので，効果的な防護 柵を設置することで被害を減少させることができるが，柵の設置には多くの費用を要す る．また地理的な要因で柵が困難な場所には音や光を利用した対策が行われているが，

慣れが生じるため効果が限定的である．

シカやイノシシは家族単位（1つの家族に

4

1

GPS

図

3-2

また森林の奥まで追い上げを行うためには，ドローンに搭載したカメラで常に対象を 捉えながら追い上げるプログラムの構築が必要である．

図

3-2

農地

焦電センサ 検知領域

9

3.3

現在，野生ザルの生態調査のために一部で

GPS

GPS

サルは

1

10

GPS

3-3

またドローンを用いるメリットとして，人間やモンキードッグによる追い上げでは，

サルが樹や崖地に移動した場合に対応できないが，三次元的に自由に飛行可能なドロー ンであれば対応できる．

図

3-3

追い上げ班

2班 追い上げ班 3班

コントロール班 4班

コントロール班

5班 指令 指令班

指令

基地局

10

第4章 実験装置と方法

4.1

4.1.1

今回使用した焦電センサは，

Senba

SB612A

4-1

4-1

4-2

1m の高さに固定し，検出対象

4-2

ZigBee

TWE-Lite DIP

Lipo

850mAh）で駆動し，農地でメンテナンスフリーで使用できるよう太陽光パネル（3 V，

140mAh）で充電可能な回路を製作した．センサユニットの概観とブロック図をそれぞ

4-3

4-4

このセンサの消費電力は

1mA

14mA

24

2

今回の実験ではこのセンサユニットを１つ用意した．焦電センサの検出保持時間は

2

TWE-Lite DIP

1

表

4-1

図

4-1

名称 焦電型赤外線センサーモジュール SB612A

電源電圧 3.3 ～ 12 V

検知出力保持時間 2秒 ～ 80分

その他 感度，動作照度の調整可能

11

図

4-2

図

4-3

図

4-4

5 m

3.8 m

Lipoバッテリー

焦電センサ 送信機

太陽電池

充電回路

太陽電池

昇圧回路

充放電回路

Lipoバッテリ

焦電センサ TWE-Lite DIP

1～3 (V)

5 (V)

充電14 (mA) 放電1 (mA)

電力供給1(mA)

検出信号

アンテナ

12

4.1.2 GNSS

今回用いた野生動物に装着すると仮定した

GNSS

u-blox

M8N-NEO

4-2

4-5

2

GNSS

4-3

表

4-2

GNSS

表

4-3

GNSS

図

4-5 GNSS

2

名称 NEO-M8N

受信可能な衛星 GPS，GLONASS，BeiDou，Galileo，SBAS 水平方向の位置精度 2.5 m （GPS & GLONASS）

速度精度 0.05 m/s

最大出力周波数 5 Hz （GPS & GLONASS）

受信衛星 GPS，GLONASS，MSAS

プロトコル NMEA

出力周波数 5 Hz

ボーレート 38400 bps

13

4.2

基地局にもラズベリーパイ

2

3

LAN

GNSS

WiFi

USB

TOCOSTick

キーボードやディスプレイへの接続を不要にするために，ラズベリーパイ専用の

7

4-6

この基地局にはドローン制御用のオープンソフトである「Dronekit」をインストール している．

Dronekit

NAVIO2

ドローンを自由に操作することができる

CUI

プログラム言語はインタプリタ型スクリプト言語の

Python

C

Python

C

合も

Python

Python

グラム実行時にコンパイルが不要なのでパラメータを何度も変更して実験する際もス ムーズに実験を進めることが可能である．

Dronekit

10 m

1

vehicle.simple_takeoff(10)

よってドローンを自動飛行させるプログラムを作成する際は細かいことを考える必要 はなく，いかにドローンに応用的な動きをさせるか，高いレイヤーで考えることができ る．また，この

Dronekit

図

4-6

14

4.3

市販の

DC

FC (フライトコントローラ）には， NAVIO2

GNSS

u-blox

M8N-NEO

4-4

4-7

地局との通信はラズベリーパイ

3

WiFi

ドローンの姿勢制御用の

PID

GUI

Mission Planner

4-8

Mission Planner

Dronekit

GUI

Python

能な

Dronekit

表

4-4

図

4-7

620 mm （H） × 550 mm （W) 約 1.5 kg

備考

2214/920KV，無負荷電流(10V)：0.4A，最大定格電流：10.3A 定格：20A，瞬間30A(10s)，BEC無

10' × 4.5'

380 mm （H） × 310 mm （W）

JR製

ATmega328P，22mm（H）×19mm（W)×5.5(D)，1.45(g) オンボードWiFi

フライトコントローラ用ラズパイシールド，65 mm （H） × 55 mm （W）

MPU9250 9DOF IMU 加速度・ジャイロ・地磁気　9軸センサ LSM9DS1 9DOF IMU 加速度・ジャイロ・地磁気　9軸センサ

MS5611 Barometer 気圧計 U-blox M8N GNSSセンサ インターフェイス UART, I2C, ADC NAVIO2

構成部品

TAROTブラシレスDCモータ ブラシレスアンプXROTOR20A

プロペラ

クアッドコプターカーボンフレーム 大きさ（プロペラを含む）

重さ（バッテリーを含む）

RC受信機 PPMエンコーダ

RaspberryPi3

15

図

4-8

Mission Planner

16

4.4

4.4.1

本実験の目的は，ドローン自動飛行用のプラットフォームの動作確認である．確認方 法は，野生動物に見立てた人間をセンサが検知し，そのセンサの位置情報を基にドロー ンが全自動で離陸・追い払い・着陸を行うものである．本実験での追い払いとは，ドロ ーンが常に対象の正面に移動することを指す．なお実験は三重大学附属農場で行った．

17

4.4.2 GNSS

本実験では

Dronekit

4-9

4-10

4-11

ABCD

ABCD

点

ABCD

GNSS

緯度を表

4-1

図

4-9 GNSS

4-10

図

4-11

ABCD（三重大学附属農場）

表

4-1

ABCD

ドローン

GNSS GNSS

対象

現在位置(ドローン) 基地局 現在位置(対象) 速度

B A

C D

地点 経度 緯度

A 136.457124 34.809242 B 136.457049 34.809290 C 136.456932 34.809137 D 136.457031 34.809093

18

4

① 対象がエリア内に侵入した場合に自動で離陸（図

4-12(a)）

② 対象の侵入を遮るトラッキング（図

4-12(b)）

③ 対象がエリア外に離脱した場合に自動で着陸（図

4-12(c)）．

本実験ではドローンの速度を1 m s

⁄ に設定し，ドローンがランダムに移動する対象に

GNSS

またドローンと対象の位置偏差は，図

4-13

error (m)，対象を点 P，

ドローンを点

Q

P

Q

𝒆𝒓𝒓𝒐𝒓 = 𝑨𝑷 ∙

− 𝑨𝑸 ∙

(m) (1)

図

4-12(a)

4-12(b)

図

4-12(c)

図

4-12 GNSS

指定エリア

A B C

D A

C B

D

指定エリア

A B C

D

指定エリア

19

図

4-13

A C B

D

error

指定エリア

Q

P

20

4.4.3

本実験では

Dronekit

4-14

4-15

置を

GNSS

立てた人間の検出時には自動で離陸を行い，指定座標への飛行を行ったのちに，自動で 帰還させた．ドローンが自動で目標座標まで飛行できたかの判断を，目視で行った．

図

4-14

図

4-15

ドローン

GNSS

センサ

基地局 検知&

現在位置(対象) 速度

現在位置(ドローン)

対象

21

第5章 実験結果および考察

5.1

焦電センサ，GNSSセンサを用いた各実験で対象の侵入と離脱を検知し，ドローンが 自動で離着陸することは確認できた．焦電センサを用いた実験では指定した座標へドロ ーンが飛行したことも確認した．図

5-1

GNSS

図

5-1

5-1

6~10 (s)

25~30 (s)

35~40 (s)

ドローンは大きな振動なく目標座標でホバリングしていることが分かる．また

GNSS

1 m

図

5-1 GPS

-2 0 2 4 6 8 10 12 14

0 5 10 15 20 25 30 35 40 45 50

基 準 点 A か ら の 距 離

時間

座標（ドローン）

座標（人間）

約

1.8

約

1.6

( )m

(s)

22

第6章 結言

本研究より，製作したドローン自動飛行用のプラットフォームが正常に動作すること が確認できた．今後は追い払い（侵入防止）だけでなく，ドローンに野生動物を威嚇す る動きをさせるプログラムの開発が重要である．さらに

GNSS

23

1

(平成 26

http://www.maff.go.jp/j/seisan/tyozyu/higai/h_zyokyo2/h26/pdf/160122-b.pdf

2

27

10

http://www.maff.go.jp/j/seisan/tyozyu/higai/pdf/h271001_meguji_jirei.pdf

3

26

3

http://www.maff.go.jp/j/seisan/tyozyu/higai/h_manual/h26_03/pdf/data0_6.pdf

4

5

6

14

GIS

ICT

http://www.pref.shimane.lg.jp/admin/region/kikan/chusankan/sympo/H27_gisshinpo.data/kityou hp.pdf

7

78，196-200

8

http://www.ffpri.affrc.go.jp/fsm/research/pubs/documents/saru-manual_200803.pdf

（2017-01-15参照）

24

9

http://www.ffpri.affrc.go.jp/fsm/research/pubs/documents/saru-jireishu_200803.pdf

（2017-01-15参照）

10

11

http://akizukidenshi.com/download/ds/senba/Pir-Am612.pdf

12

http://ardupilot.org/planner/#，（2017-01-19

13

5

https://www.pref.yamanashi.jp/s-rikouken/documents/houkokusyo2010.pdf，

25

本研究の遂行に当たり，ご指導を頂いた鬼頭孝治准教授，王秀崙教授には感謝の意を 表し，御礼申し上げます．

また，講義を通じて多くの知識や示唆を頂いた環境情報システム工学講座の先生方に 感謝の意を表します．

また，日常の議論を通じて多くのご助言を頂いたエネルギー利用工学研究室の皆様に 感謝します．

最後に本論文作成に多大なるご協力を頂いた鬼頭孝治准教授，王秀崙教授に重ねて厚 く御礼申し上げます．

平成

29

2

26

1

GNSS

from dronekit import connect from dronekit import VehicleMode

from dronekit import LocationGlobalRelative from pymavlink import mavutil

import time import socket import pyproj import numpy as np

#Area lon lat lona = 136.457124 lata = 34.809242 lonb = 136.457049 latb = 34.809290 lonc = 136.456932 latc = 34.809137 lond = 136.457031 latd = 34.809093

lonp = 0 #target latp = 0 #target lonq = 0 #drone latq = 0 #drone lont = 0 latt = 0

#Transform from lon/lat to xy

epsg2448 = pyproj.Proj(init="epsg:2448")#left-hand ay, ax = epsg2448(lona, lata)

by, bx = epsg2448(lonb, latb)

cy, cx = epsg2448(lonc, latc)

dy, dx = epsg2448(lond, latd)

27 py, px = epsg2448(lonp, latp)

qy, qx = epsg2448(lonq, latq) ty, tx = epsg2448(lont, latt)

#Coordinate

a = np.array([ax, ay]) b = np.array([bx, by]) c = np.array([cx, cy]) d = np.array([dx, dy]) p = np.array([px, py]) q = np.array([qx, qy]) t = np.array([tx, ty])

#Vector ab = b - a bc = c - b cd = d - c da = a - d ad = d - a ap = p - a bp = p - b cp = p - c dp = p - d aq = q - a

#Normalization

ab_len = np.linalg.norm(ab) e_ab = ab / ab_len

ad_len = np.linalg.norm(ad) e_ad = ad / ad_len

print e_ad

def make_vector(lonw, latw, a, b, c, d):

wy, wx = epsg2448(lonw, latw) w = np.array([wx, wy])

aw = w - a

28 bw = w - b

cw = w - c dw = w - d

return aw, bw, cw, dw

host = "192.168.0.1"

port = 5000 buffsize = 1024

#AF_INET:IPv4

#SOCK_DGRAM:UDP protocol(not tcp)

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) print "Socket created"

sock.bind((host, port))#name udp, configure address print "Socket bind complete"

#Connect to vehicle

vehicle = connect('192.168.0.1:14550' ,wait_ready=True, baud=115200, rate=10)

# set GPS stream rate to 10

print "SR0_POSITION: %s" % vehicle.parameters['SR0_POSITION']

vehicle.parameters['SR0_POSITION'] = 10

print "SR0_POSITION: %s" % vehicle.parameters['SR0_POSITION']

# Fail safe with RC ch3 = vehicle.channels['3']

ch5 = vehicle.channels['5']

while not ch3 > 1400 and ch3 < 1600:

print "not 1400 < ch3 <1600"

ch3 = vehicle.channels['3']

time.sleep(1)

while not ch5 > 1400 and ch5 < 1600:

print "not 1400 < ch5 <1600"

ch5 = vehicle.channels['5']

time.sleep(1)

29

#Inside outside determination while True:

print "Target searching"

data, addr = sock.recvfrom(buffsize) data = data.split(',')

latp = float(data[0]) #target lonp = float(data[1]) #target print latp, lonp

ap, bp, cp, dp = make_vector(lonp, latp, a, b, c, d) if np.cross(ab, ap) < 0¥

and np.cross(bc, bp) < 0¥

and np.cross(cd, cp) < 0¥

and np.cross(da, dp) < 0:

print "target detect"

break else:

print "Target not inside"

continue

#Takeoff

aTargetAltitude = 3

print "Basic pre-arm checks"

while not vehicle.is_armable:

print "Wating for vehicle to initialise..."

time.sleep(1)

print "GPS: %s" % vehicle.gps_0

print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame lat = vehicle.location.global_relative_frame.lat

lon = vehicle.location.global_relative_frame.lon print "latitude:{0}, longitude:{1}".format(lat, lon)

print"Arming motors"

vehicle.mode = VehicleMode("GUIDED")

30 vehicle.armed = True # 6sat 2.5HDOP

while not vehicle.armed:

print "Waiting for arming..."

time.sleep(1)

print "Taking off!"

vehicle.simple_takeoff(aTargetAltitude)

while True:

print " Altiude: ", vehicle.location.global_relative_frame.alt

if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:

print "Reach target altitude"

break time.sleep(1)

#---tracking--- kv = 1 # m/s

kp = 0.5 # gain for brake kd <= 2 kz = 0.5 # gain for maintaining altittude no_control_area_xy = 0.5 # m

#no_control_area_xy = 0.5 # m

#brake_area_xy = 1.5 # m

brake_area_xy = (1 * kv * t) + no_control_area_xy # m control_area_z = 0.5 # m

drone_dis_ad = 0 target_dis_ad = 0 dis_ad = 0 t = 0.2 # for time velocity_x = 0 velocity_y = 0 velocity_z = 0

velocity_xad = -0.891

velocity_yad = -0.454

31 limiter_velocity_x = velocity_xad * kv * 2 limiter_velocity_y = velocity_yad * kv * 2 limiter_velocity_z = 0.5

#velocity_xab = 0.609

#velocity_yab = -0.793

def msg_ned_velocity(velocity_x, velocity_y, velocity_z):

msg = vehicle.message_factory.set_position_target_local_ned_encode(

0, # time_boot_ms

0, 0, # target system, target component

mavutil.mavlink.MAV_FRAME_LOCAL_NED, #frame 0b0000111111000111, #type_mask

0, 0, 0, # x, y, z position

velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 0, 0, 0, # x, y, z acceleration

0, 0) # yaw, yaw_rate

return msg

def func_velocity_z(dis_alt, control_area_z, kz, limiter_velocity_z):

if dis_alt > control_area_z:

velocity_z = (dis_alt - control_area_z) * kz if velocity_z > limiter_velocity_z:

velocity_z = limiter_velocity_z elif dis_alt < -control_area_z:

velocity_z = (dis_alt + control_area_z) * kz if velocity_z < -limiter_velocity_z:

velocity_z = -limiter_velocity_z else:

velocity_z = 0 return velocity_z

while True:

32 start = time.time()

print t, dis_ad, drone_dis_ad, target_dis_ad, velocity_x, velocity_y, vehicle.velocity[0], vehicle.velocity[1], vehicle.velocity[2]

#GNSS check for failsafe if vehicle.gps_0 < 10:

print "The number of satelites is not enough"

break

#calc coordinate of drone

latq = vehicle.location.global_relative_frame.lat #drone lonq = vehicle.location.global_relative_frame.lon #drone aq, bq, cq, dq = make_vector(lonq, latq, a, b, c, d)

#calc coordinate of target

data, addr = sock.recvfrom(buffsize) data = data.split(',')

latp = float(data[0]) # target lonp = float(data[1]) # target

ap, bp, cp, dp = make_vector(lonp, latp, a, b, c, d)

if np.cross(ab, ap) < 0¥

and np.cross(bc, bp) < 0¥

and np.cross(cd, cp) < 0¥

and np.cross(da, dp) < 0:

#print "target inside"

else:

print "Target not inside"

velocity_x = 0 velocity_y = 0 velocity_z = 0

msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z)

vehicle.send_mavlink(msg)

33 time.sleep(5)

#Check target coordonate----

data, addr = sock.recvfrom(buffsize) data = data.split(',')

latp = float(data[0]) # target lonp = float(data[1]) # target

ap, bp, cp, dp = make_vector(lonp, latp, a, b, c, d)

if np.cross(ab, ap) < 0¥

and np.cross(bc, bp) < 0¥

and np.cross(cd, cp) < 0¥

and np.cross(da, dp) < 0:

print "target inside"

continue else:

print "Target not inside"

print "Go home"

break

#---

if np.cross(ab, aq) < 0¥

and np.cross(bc, bq) < 0¥

and np.cross(cd, cq) < 0¥

and np.cross(da, dq) < 0:

#print "drone inside"

else:

print "drone not inside"

velocity_x = 0 velocity_y = 0 velocity_z = 0

msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z) vehicle.send_mavlink(msg)

time.sleep(2)

34 #Check target coordonate---

data, addr = sock.recvfrom(buffsize) data = data.split(',')

latp = float(data[0]) # target lonp = float(data[1]) # target

ap, bp, cp, dp = make_vector(lonp, latp, a, b, c, d)

if np.cross(ab, ap) < 0¥

and np.cross(bc, bp) < 0¥

and np.cross(cd, cp) < 0¥

and np.cross(da, dp) < 0:

print "target inside"

continue else:

print "Target not inside"

print "Go home"

break

#---

#calc distance between drone and target drone_dis_ad = np.inner(e_ad, aq) target_dis_ad = np.inner(e_ad, ap) dis_ad = target_dis_ad - drone_dis_ad

#dis_ad = np.inner(e_ad, ap) - np.inner(e_ad, aq) brake_area_xy = (1 * kv * t) + no_control_area_xy # m #print dis_ad

#calc distance of altitude

dis_alt = vehicle.location.global_relative_frame.alt - aTargetAltitude

#calc velocity_xyz

if dis_ad > brake_area_xy:

velocity_x = velocity_xad * kv

velocity_y = velocity_yad * kv

35

velocity_z = func_velocity_z(dis_alt, control_area_z, kz, limiter_velocity_z) msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z)

vehicle.send_mavlink(msg) t = time.time() - s

continue

if dis_ad < -brake_area_xy:

velocity_x = -velocity_xad * kv velocity_y = -velocity_yad * kv

velocity_z = func_velocity_z(dis_alt, control_area_z, kz, limiter_velocity_z) msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z)

vehicle.send_mavlink(msg) t = time.time() - s

continue

if dis_ad < brake_area_xy and dis_ad > -brake_area_xy:

#velocity_x = velocity_xad * (kv * kd) * (dis_ad - pre_dis_ad) #velocity_y = velocity_yad * (kv * kd) * (dis_ad - pre_dis_ad) velocity_x = -vehicle.velocity[0] * kp

velocity_y = -vehicle.velocity[1] * kp

velocity_z = func_velocity_z(dis_alt, control_area_z, kz, limiter_velocity_z) if dis_ad < no_control_area_xy and dis_ad > -no_control_area_xy:

velocity_x = 0 velocity_y = 0

#velocity_z = func_velocity_z(dis_alt, control_area_z, kz, limiter_velocity_z) """

velocity_z = 0

msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z) vehicle.send_mavlink(msg)

time.sleep(1) t = time.time() - s continue

"""

msg = msg_ned_velocity(velocity_x, velocity_y, velocity_z)

vehicle.send_mavlink(msg)

36 t = time.time() - start

continue

ch5 = vehicle.channels['5']

if ch5 < 1300:

print "Manual control"

else:

print "Returning to Launch" #where it was arm vehicle.mode = VehicleMode("RTL")

print "Close vehicle object"

vehicle.close()

37

2

焦電センサを用いた追い払いシステム

from dronekit import connect from dronekit import VehicleMode

from dronekit import LocationGlobalRelative import serial

import time import pyproj import numpy as np

#TOCOStick

s = serial.Serial("/dev/ttyUSB0", 115200)

#Connect to vehicle

vehicle = connect('192.168.0.1:14550' ,wait_ready=True, baud=115200)

# set GPS stream rate to 5

print "SR0_POSITION: %s" % vehicle.parameters['SR0_POSITION']

vehicle.parameters['SR0_POSITION'] = 10

print "SR0_POSITION: %s" % vehicle.parameters['SR0_POSITION']

# Fail safe with RC ch3 = vehicle.channels['3']

ch5 = vehicle.channels['5']

while not ch3 > 1400 and ch3 < 1600:

print "not 1400 < ch3 <1600"

ch3 = vehicle.channels['3']

time.sleep(1)

while not ch5 > 1400 and ch5 < 1600:

print "not 1400 < ch5 <1600"

ch5 = vehicle.channels['5']

time.sleep(1)

#Target searching while True:

print "Target searching"

38 line = s.readline()

digital_1 = line[33:35]

analog_4 = line[41:43].lower() print line

print digital_1 print analog_4 if digital_1 == '00':

print "detect"

s.close() break else:

print "searching"

time.sleep(0.5)

#Takeoff

aTargetAltitude = 2

print "Basic pre-arm checks"

while not vehicle.is_armable:

print "Wating for vehicle to initialise..."

time.sleep(1)

print"Arming motors"

vehicle.mode = VehicleMode("GUIDED") vehicle.armed = True # 6sat 2.5HDOP

while not vehicle.armed:

print "Waiting for arming..."

time.sleep(1)

print "Taking off!"

vehicle.simple_takeoff(aTargetAltitude)

while True:

39

print " Altiude: ", vehicle.location.global_relative_frame.alt

if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95:

print "Reach target altitude"

break time.sleep(1)

vehicle.groundspeed = 2

#point1 = LocationGlobalRelative(lat, lon, aTargetAltitude) # T point

point1 = LocationGlobalRelative(34.809250, 136.457126, aTargetAltitude) # T point vehicle.simple_goto(point1)

time.sleep(12)

print "Returning to Launch" #where it was arm vehicle.mode = VehicleMode("RTL")

print "Close vehicle object"

vehicle.close()

40

3

GNSS

t (s) t_sum (s) drone_dis_from_a (m) target_dis_from_a (m)

0.25 0.25 0 0

0.105897903 0.355897903 11.9533615 9.600674509 0.122106075 0.478003979 11.97308683 9.2724072 0.117377996 0.595381975 11.98862446 8.976970261 0.114402056 0.709784031 11.99848712 8.669589712 0.117339134 0.827123165 11.99429943 8.465384945 0.127761126 0.954884291 11.92404765 8.361776183 0.125417948 1.080302238 11.85230859 8.303015288 0.136157036 1.216459274 11.76651917 8.300529136 0.12876296 1.345222235 11.52320129 8.326449799 0.142860174 1.488082409 11.36567281 8.441517448 0.119291067 1.607373476 11.21381931 8.570520272 0.107881069 1.715254545 11.05210315 8.685587921 0.121768951 1.837023497 10.7301581 8.831031798 0.115615129 1.952638626 10.58397956 8.953092903 0.115181923 2.067820549 10.44198873 8.97897169 0.126946926 2.194767475 10.30567286 8.995412744 0.123832941 2.318600416 10.16935699 8.927214116 0.131557941 2.450158358 9.929013665 8.865957209 0.120997906 2.571156263 9.929013665 8.811661743 0.12994194 2.701098204 9.702720692 8.633335757 0.129662991 2.830761194 9.480615415 8.384767842 0.116602898 2.947364092 9.480615415 8.189958929 0.133572817 3.080936909 9.272560495 7.962329511 0.120692015 3.201628923 9.162858066 7.718249178 0.121911049 3.323539972 8.963178533 7.483626303 0.114474058 3.43801403 8.963178533 7.239504092 0.116083145 3.554097176 8.859151074 7.077619166 0.127103806 3.681200981 8.64960888 6.915734239 0.120012045 3.801213026 8.549769115 6.697542559 0.128673077 3.929886103 8.440066687 6.505239527 0.131804943 4.061691046 8.234712187 6.305975057