2012/2/8

1

自律 バ グ 学自律型ホバリング･ヘリで学ぶ各種センサ説明

参考資料およびアドバイザ

参考資料

• Modelling, Identification and Control of a Quadrotor Helicopter

D t t f A t ti C t l L d U i itDepartment of Automatic Control Lund University

Tommaso Bresciani

• AN-1057 アプリケーション・ノート

アナログ・デバイセズ株式会社

Christopher J. Fisher

• 移動補助具の安定性を評価する機器の開発

埼玉県産業技術総合センター

新里浩司、田中智大

アドバイザ

防衛大学校ロボット工学研究室

滝田好宏教授

2012/2/8

2

2012/2/8

3

• 開発の背景

• システムの構成• システムの構成

• 動作原理の説明

• 姿勢制御

• 位置制御

• センサの説明

• 機体の説明機体の説明

• 今後の自律型ホバリング・ヘリの展望

開発の背景

震災で避難されている方々が、立ち入り禁止区域の自分の家を見られたらいいのでは？を見られたらいいのでは？

緯度経度を指定して撮影して回れる自律型のロボットがあればいいのでは？

GPSとカメラを搭載した空飛ぶロボットを作ろう！！

2012/2/8

4

システムの構成（1）

ホバリング・ヘリの機体の構成を説明します。

システムの構成（2）

ZigBeeモジュールCPUボード

＋センサボード

2012/2/8

5

システムの構成（3）

赤外線センサ超音波センサ Li‐Poバッテリー

システムの構成（4）

ドライバモータ

2012/2/8

6

システムの構成（5）CPUボード・センサ 3軸地磁気センサ

3軸加速度センサRX62T

USBシリアル変換

RX62T搭載CPUボード センサボード

3軸ジャイロセンサ

気圧センサmicroSD

USBシリアル変換

超音波センサ 赤外線センサ

システムの構成（6）モータ・ドライバ・ＺｉｇＢｅｅモジュール

モータ ドライバ

ZigBeeモジュール

2012/2/8

7

システムの構成（7）ブロック図

センサボードCPUボード

RX62T

3軸ジャイロセンサ

3軸加速度センサ

3軸地磁気センサ

気圧センサ

超音波センサ

赤外線センサ

12bit A/D

RIIC

超音波センサ※

超音波センサ※

超音波センサ※

超音波センサ※

超音波センサ※

ドライバ モータ

ドライバ モータ

ドライバ モータ

ドライバ モータ

GPT

SCI

GPSモジュール※

ZigBeeモジュール

※搭載予定

Li-Poバッテリ抵抗分圧

超音波センサ※

システムの構成（8）GUIソフト

機体の各情報を表示します。

各目標値を設定します。

動作開始・着陸・動作停止ボタン

各ゲインを設定します。

ホバリングの出力を設定します。

将来使用する予定のGPSの経路選

択機能です。

2012/2/8

8

システムの構成（9）寸法600mm

600m

m

203.2m

m（8inch）

ボード寸法75mm×60mm

2012/2/8

9

動作原理の説明（1）

自律型ホバリング・ヘリと一般的なヘリとの違いを説明しいていきます。きます。

ヘリは飛行制御として、高さ制御・ヨー制御・ピッチ制御・ロール制御を行っています。

• 高さ制御：機体の高度を制御します。

• ヨー制御：機体の縦軸を中心とした回転を制御します。

• ロール制御：機体の前後軸を中心とした回転を制御します。

• ピッチ制御：機体の左右軸を中心とした回転を制御します。

動作原理の説明（2）高さ制御（一般的なヘリ）

メインローターの揚力が重量を上回ることで浮上します。揚力と重量とが釣り合っている場合に空中静止（ホバリング）となり、重量とが釣り合っている場合に空中静止（ホバリング）となり、揚力が下回ると下降します。

揚力

メインローター

テールローター

2012/2/8

10

動作原理の説明（3）高さ制御（自律型ホバリング・ヘリ）

4枚のローターの揚力の合計が重量を上回ることで浮上します。

右前

後左

一般的なヘリは、メインローターを回転させるとメインローターの回転とは逆方向に機体を回転させるモーメントが発生し、機

動作原理の説明（4）ヨー制御（一般的なヘリ）

体が回転してしまいます。機体を回転させるモーメントと逆のモーメントをテールローターで作り、機体が回転することを抑えます。

モーメント

モーメントメインローターの回転方向

テールローター

2012/2/8

11

動作原理の説明（5）ヨー制御（タンデムローターヘリ）

前後にローターを持つヘリは、前後のローターを逆回転することで、反トルクが相殺されます。とで、反トルクが相殺されます。

回転方向 モーメント 回転方向モーメント

動作原理の説明（6）ヨー制御（自律型ホバリング・ヘリ）

直交するローターを逆回転させることで、互いに逆方向に回転させようとするモーメントを打ち消し合います。させようとする ントを打ち消し合 ます。

右前

回転方向

モーメント

後左

2012/2/8

12

メインローターの回転面を左右に傾けることで機体が左右に傾き移動します。

動作原理の説明（7）ロール制御（一般的なヘリ）

き移動します。

推力

揚力

推力

動作原理の説明（8）ロール制御（自律型ホバリング・ヘリ）

左右のプロペラの回転数を変えることで、機体が左右に傾き移動します。動します。

右前

後左

2012/2/8

13

動作原理の説明（9）ピッチ制御（一般的なヘリ）

メインローターの回転面を前後に傾けることで前後に移動します。す。

推力

揚力

推力

動作原理の説明（10）ピッチ制御（自律型ホバリング・ヘリ）

前後のプロペラの回転数を変えることで、機体を傾け前後に移動します。動します。

右前

後左

2012/2/8

14

動作原理の説明（11）なぜ4ローターヘリを採用したか

一般的なヘリやタンデムローターヘリの場合は、ローターの回転面を傾ける機構が複雑になります。転面を傾ける機構が複雑になります。

自律型ホバリング・ヘリの場合は、可動部減少による機構の単純化ができます。

2012/2/8

15

姿勢制御（1）

ヘリを飛行させるためには、姿勢制御をする必要があります。

ホバリングの場合には 地面と水平になる状態を保つように制ホバリングの場合には、地面と水平になる状態を保つように制御を行う必要があります。

• 各モーターの出力の決定

• 高さ制御

• ヨー制御

• ロール制御

• ピッチ制御

• 出力の補正

• ドライバへの指示値の決定

姿勢制御（2）各モーターの出力の決定

v1：右モータ出力

v2：左モータ出力

3 前モ タ出力

各モータ出力の計算式

1 1 2 5 6 7 8 1 f //右 v3：前モータ出力

v4：後モータ出力

v1ofs：右モータホバリング出力

v2ofs：左モータホバリング出力

v3ofs：前モータホバリング出力

v4ofs：後モータホバリング出力

a1：ロール角度による出力の増減値

a2：ロール角速度による出力の増減値

ピ 角度 よる出力 増減値

v1 =  a1 + a2 + a5 + a6 + a7 + a8 + v1ofs; // 右

v2 = ‐a1 ‐ a2 + a5 + a6 + a7 + a8 + v2ofs; // 左

v3 = a3 + a4 ‐ a5 ‐ a6 + a7 + a8 + v3ofs; // 前

v4 = ‐a3 ‐ a4 ‐ a5 ‐ a6 + a7 + a8 + v4ofs; // 後

センサの値により、a1～a8を計算する

必要があります。

a3：ピッチ角度による出力の増減値

a4：ピッチ角速度による出力の増減値

a5：ヨー角度による出力の増減値

a6：ヨー角速度による出力の増減値

a7：高さによる出力の増減値

a8：垂直速度による出力の増減値

2012/2/8

16

姿勢制御（3）高さ制御（ブロック図）

高さゲイン

目標の高さ

超音波センサで測定した高さ

目標の垂直速度

＋

－

＋

a7

超音波センサで測定した垂直速度

垂直速度

垂直速度ゲイン

＋

－ a8

姿勢制御（4）高さ制御（模式図）

右

後

前

左ホバリング出力

+a7+a8

ホバリング出力+a7+a8

ホバリング出力+a7+a8

ホバリング出力+a7+a8

高さによる出力の増減値 垂直速度による出力の増減値

2012/2/8

17

姿勢制御（5）高さ制御（プログラム）

a7 = (TargetHighDegree ‐ HighDegree) * GainHighDegree;

if(a7 < ‐20){

a7 = ‐20;

}

if(20 < a7){

a7 = 20;

}

a8 = (TargetVerticalSpeed ‐ VerticalSpeed) * GainVerticalSpeed;

if(a8 < ‐20){

a8 = ‐20;

}

if(20 < a8){

a8 = 20;

}

姿勢制御（6）ヨー制御（ブロック図）

ヨー角度ゲイン

目標のヨー角度

ジャイロセンサと地磁気センサで

測定したヨー角度

目標のヨー角速度

＋

－

＋

a5

ジャイロセンサで測定したヨー角速度

ヨ 角速度

ヨー角速度ゲイン

＋

－ a6

2012/2/8

18

姿勢制御（7）ヨー制御（模式図）

ホバリング出力‐a5‐a6

右

後

前

左

ホバリング出力‐a5‐a6

ホバリング出力+a5+a6

ホバリング出力+a5+a6

ヨー角度による出力の増減値 ヨー角速度による出力の増減値

姿勢制御（8）ヨー制御（プログラム）

a5 = (TargetYawAngle ‐ YawAngle) * GainYawAngle;

if(a5 < ‐20){

a5 = ‐20;

}

if(20 < a5){

a5 = 20;

}

a6 = (TargetYawAngleRate ‐ YawAngleRate) * GainYawAngleRate;

if(a6 < ‐20){

a6 = ‐20;

}

if(20 < a6){

a6 = 20;

}

2012/2/8

19

姿勢制御（9）ロール制御（ブロック図）

ロール角度ゲイン

目標のロール角度

ジャイロセンサと加速度センサで

測定したロール角度

目標のロール角速度

＋

－

＋

a1

ジャイロセンサで測定したロール角速度

ロ ル角速度

ロール角速度ゲイン

＋

－ a2

姿勢制御（10）ロール制御（模式図）

ホバリング出力+a1+a2

右

後

前

左ホバリング出力

‐a1‐a2

ホバリング出力

ホバリング出力

ロール角度による出力の増減値 ロール角速度による出力の増減値

2012/2/8

20

姿勢制御（11）ロール制御（プログラム）

a1 = (TargetRollAngle ‐ RollAngle) * GainRollAngle;

if(a1 < ‐20){

a1 = ‐20;

}

if(20 < a1){

a1 = 20;

}

a2 = (TargetRollAngleRate ‐ RollAngleRate) * GainRollAngleRate;

if(a2 < ‐20){

a2 = ‐20;

}

if(20 < a2){

a2 = 20;

}

姿勢制御（12）ピッチ制御（ブロック図）

ピッチ角度ゲイン

目標のピッチ角度

ジャイロセンサと加速度センサで

測定したピッチ角度

目標のピッチ角速度

＋

－

＋

a3

ジャイロセンサで測定したピッチ角速度

角速度

ピッチ角速度ゲイン

＋

－ a4

2012/2/8

21

姿勢制御（13）ピッチ制御（模式図）

ホバリング出力‐a3‐a4

右

後

前

左

ホバリング出力+a3+a4

ホバリング出力

ホバリング出力

ピッチ角度による出力の増減値 ピッチ角速度による出力の増減値

姿勢制御（14）ピッチ制御（プログラム）

a3 = (TargetPitchAngle ‐ PitchAngle) * GainPitchAngle;

if(a3 < ‐20){

a3 = ‐20;

}

if(20 < a3){

a3 = 20;

}

a4 = (TargetPitchAngleRate ‐ PitchAngleRate) * GainPitchAngleRate;

if(a4 < ‐20){

a4 = ‐20;

}

if(20 < a4){

a4 = 20;

}

2012/2/8

22

システムの簡略化のためモータの出力のフィードバックをしていません。

姿勢制御（15）出力の補正（1）

ません。

バッテリ電圧が変化するとモータの出力も下がってしまいます。

ドライバに加える電圧が変わっても一定の推力になるような補正をする必要があります。

はかりの上に機体を乗せて固定し、その時の重量をメモしておきます。

姿勢制御（16）出力の補正（2）

きます。

モータを回すと、はかりの値が減少するので、モータが停止している時との差分を推力として測定します。

2012/2/8

23

姿勢制御（17）出力の補正（3）

補正後出力=補正前出力‐補正前出力*(バッテリ電圧‐基準電圧)*補正係数

10V、出力35％時に推力210g

13V、出力30％時に推力210g

30=35‐35*(13‐10)*X

X =0.0476

v1 = v1 * (1 ‐ (BattVolt ‐ 10.0) * 0.0476);

v2 = v2 * (1 ‐ (BattVolt ‐ 10.0) * 0.0476);

v3 = v3 * (1 ‐ (BattVolt ‐ 10.0) * 0.0476);

v4 = v4 * (1 ‐ (BattVolt ‐ 10.0) * 0.0476);

姿勢制御（18）ドライバへの指示値の決定（1）

補正後の出力が0％より小さくなっていないか、また100％より大きくなっていないかを

確認する必要があります。if(100 < v1){if(100 < v1){

v1 = 100;

}

if(v1 < 0){

v1 = 0;

}

if(100 < v2){

v2 = 100;

}

if(v2 < 0){

v2 = 0;

}

if(100 < v3){

v3 = 100;

}

if(v3 < 0){

v3 = 0;

}

if(100 < v4){

v4 = 100;

}

if(v4 < 0){

v4 = 0;

}

2012/2/8

24

姿勢制御（19）ドライバへの指示値の決定（2）

0％ 100％

1ms

2ms

5ms

GPT0.GTCCRC = 12288 + v1 / 100 * 12288; // 右

GPT0.GTCCRE = 12288 + v2 / 100 * 12288; // 左

GPT1.GTCCRC = 12288 + v3 / 100 * 12288; // 前

GPT1.GTCCRE = 12288 + v4 / 100 * 12288; // 後

2012/2/8

25

位置制御（1）

姿勢制御だけでは、風などの外乱や重量バランスによる僅かな傾きにより、定点にとどまる事が難しくなります。傾きにより、定点にとどまる事が難しくなります。

位置制御は、姿勢制御の上位に位置し、位置のずれにより傾きを制御し、定点にとどまるようにします。

• 位置制御のしくみ

• X軸位置制御

• Y軸位置制御

位置制御（2）位置制御のしくみ（1）

X軸がずれると、左右に機体を傾けて、戻ろうとします。

Y

右

後

前

左

X機体の位置

目標位置

2012/2/8

26

位置制御（3）位置制御のしくみ（2）

Y軸がずれると、前後に機体を傾けて、戻ろうとします。

Y

右

後

前

左

X目標位置

機体の位置

位置制御（4）X軸位置制御（ブロック図）

‐a1‐a3を目標のロール角に設定します。

X軸位置ゲイン

目標のX軸位置

赤外線センサで測定したX軸位置

目標のX軸速度

＋

－

＋

a3測定した高さ

赤外線センサで測定したX軸速度

X軸速度

X軸速度ゲイン

＋

－ a1測定した高さ

2012/2/8

27

位置制御（5）X軸位置制御（模式図）

目標のロール角をに設定します

右

後

前

左

‐a1‐a3に設定します。

X軸位置による角度の増減値X軸速度による角度の増減値

位置制御（6）X軸位置制御（プログラム）

a1 = (TargetXSpeed ‐ XSpeed) * GainXSpeed * HighDegree;

if(a1 < ‐5){

a1 = ‐5;

}

if(5 < a1){

a1 = 5;

}

a3 = (TargetXPosition ‐ XPosition) * GainXPosition * HighDegree;

if(a3 < ‐5){

a3 = ‐5;

}

if(5 < a3){

a3 = 5;

}

TargetRollAngle = ‐a1 ‐ a3;

2012/2/8

28

位置制御（7）Y軸位置制御（ブロック図）

‐a2‐a4を目標のピッチ角に設定します。

Y軸位置ゲイン

目標のY軸位置

赤外線センサで測定したY軸位置

目標のY軸速度

＋

－

＋

a4測定した高さ

赤外線センサで測定したY軸速度

Y軸速度

Y軸速度ゲイン

＋

－ a2測定した高さ

位置制御（8）Y軸位置制御（模式図）

目標のピッチ角をに設定します

右

後

前

左

‐a2‐a4に設定します。

Y軸位置による角度の増減値Y軸速度による角度の増減値

2012/2/8

29

位置制御（9）Y軸位置制御（プログラム）

a2 = (TargetYSpeed ‐ YSpeed) * GainYSpeed * HighDegree;

if(a2 < ‐5){

a2 = ‐5;

}

if(5 < a2){

a2 = 5;

}

a4 = (TargetYPosition ‐ YPosition) * GainYPosition * HighDegree;

if(a4 < ‐5){

a4 = ‐5;

}

if(5 < a4){

a4 = 5;

}

TargetPitchAngle = ‐a2 ‐ a4;

2012/2/8

30

センサの説明（1）

機体の制御をするためには、機体のさまざまな情報を正しく把握する必要がありますので、各センサについて説明していきま握する必要がありますので、各センサについて説明していきます。

• ジャイロセンサ

• 加速度センサ

• 超音波センサ

• 赤外線センサ

• 地磁気センサ

• 気圧センサ

センサの説明（2）ジャイロセンサ

角速度の測定に使用します。（レートジャイロ）

安価で小型な振動式のセンサの中から選定することにしました。

測定方式 原理 精度 価格 サイズ

回転式 コマの原理を利用 高い 高い 大きい

振動式 振動する物体に生じるコリオリの力を利用

低い 安い 小さい

光学式 サニャック効果を利用 高い 高い 大きい

安価で小型な振動式のセンサの中から選定する とにしました。

2012/2/8

31

センサの説明（3）ジャイロセンサの種類

メーカー 型番 スペック

STMicroelectronics L3G4200D 測定軸：3軸測定範囲：±2000deg/sec測定範囲 g/感度：0.070deg/sec/digitサイズ：4mm×4mm

InvenSense ITG3200 測定軸：3軸測定範囲：±2000deg/sec感度：0.070deg/sec/digitサイズ：4mm×4mm

村田製作所 ENC‐03R 測定軸：1軸測定範囲：±300deg/sec

小型で3軸で入手性の良かったL3G4200Dを選択しました。

測定範囲：±300deg/sec感度：0.67mV/deg/secサイズ：4mm×8mm

矩形積分による積分

センサの説明（4）ジャイロセンサによる角度の求め方（1）

台形積分矩形積分

RollAngle += RollAngleRate * 0.01;

PitchAngle += PitchAngleRate * 0.01;

周期角速度角度

2012/2/8

32

机に固定した状態で、ジャイロセンサのみで角度を求めた場合、0度が次第に増加してしまいます。

センサの説明（5）ジャイロセンサによる角度の求め方（2）

0度が次第に増加してしまいます。

0

2

4

6

8

10

系列1

0度が増加

‐10

‐8

‐6

‐4

‐20 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

約45度に手で2回傾けた場合、角度は求められているようですが、やはり0度は増加してしまいます。

センサの説明（6）ジャイロセンサによる角度の求め方（3）

やはり0度は増加してしまいます。

0

20

40

60

系列1

0度が増加

‐60

‐40

‐20

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

2012/2/8

33

積分により時間とともに誤差が蓄積してしまい0度がずれてしまいます。

センサの説明（7）ジャイロセンサの欠点

います。

ジャイロセンサのみで角度を求めることは困難だという事が分かりました。

センサの説明（8）加速度センサ

重力方向の測定に使用します。

精度が高く、小型な静電容量式のセンサの中から選定することにしました。

測定方式 原理 精度 価格 サイズ

静電容量式 容量変化を検出 温度特性に優れる 安い 小さい

ピエゾ抵抗式 抵抗変化を検出 感度の温度特性が悪い 安い 小さい

ガス式 温度変化を検出 感度の温度特性が悪い 安い 小さい

2012/2/8

34

センサの説明（9）加速度センサの種類

メーカー 型番 スペック

カイオニクス KXP84‐2050 測定軸：3軸測定範囲：±2g感度：819count/gサイズ：5mm×5mm

BOSCH BMA180 測定軸：3軸測定範囲：±16g感度：512counts/gサイズ：3mm×3mm

AnalogDevices ADXL345 測定軸：3軸

小型で3軸で入手性がよく、I2Cバス通信のできるBMA180を選択しました。

AnalogDevices ADXL345 測定軸：3軸測定範囲：±16g感度：32counts/gサイズ：3mm×5mm

1軸で求める場合

センサの説明（10）加速度センサによる角度の求め方（1）

Ax（1g）

90°

Ax（0g）

( )x-1 Asin=θ

2012/2/8

35

2軸で求める場合

センサの説明（11）加速度センサによる角度の求め方（2）

⎟⎟⎠

⎞⎜⎜⎝

⎛= −

z

x1

AAtanθ

Ax

θ

θ

Az

θ

1g

3軸で求める場合のX軸の水平からの傾き角度

センサの説明（12）加速度センサによる角度の求め方（3）

z

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

+= −

22

1tanzy

x

AA

AθAy

Azθ

y

XAccelerationAngle = atan(XAcceleration ¥

/ sqrt(pow(YAcceleration, 2) + pow(ZAcceleration, 2))) * 360 / (2 * 3.14);

Axx

2012/2/8

36

3軸で求める場合のY軸の水平からの傾き角度

センサの説明（13）加速度センサによる角度の求め方（4）

z

⎟⎟

⎠

⎞

⎜⎜

⎝

⎛

+= −

22

1tanzx

y

AA

Aθ

AxAy

θy

YAccelerationAngle = atan(YAcceleration ¥

/ sqrt(pow(XAcceleration, 2) + pow(ZAcceleration, 2))) * 360 / (2 * 3.14);

Azx

3軸で求める場合のZ軸の垂直からの傾き角度

センサの説明（14）加速度センサによる角度の求め方（5）

z

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛ += −

z

yx

AAA 22

1tanθ

A

Ay

y

ZAccelerationAngle = atan(sqrt(pow(XAcceleration, 2) + pow(YAcceleration, 2)) ¥

/ ZAcceleration) * 360 / (2 * 3.14);

Ax Azθx

2012/2/8

37

机に固定した状態で、加速度センサのみで角度を求めた場合、0度は安定しています。

センサの説明（15）加速度センサによる角度の求め方（6）

0度は安定しています。

0

2

4

6

8

10

系列1

‐10

‐8

‐6

‐4

‐20 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

約45度に手で2回傾けた場合、角度以外の情報が出力されています。

センサの説明（16）加速度センサによる角度の求め方（7）

います。

0

20

40

60

系列1

机に接地したときの衝撃

‐60

‐40

‐20

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

2012/2/8

38

移動するときの加速度や衝撃など重力以外の成分も拾ってしまいます。

センサの説明（17）加速度センサの欠点

います。

加速度センサのみで角度を求めることは困難だという事が分かりました。

ジャイロセンサの交流成分と加速度センサの直流成分を合成して角度を出せば、両方のセンサがもつ良いところを引き出せます。

センサの説明（18）ジャイロセンサと加速度センサの

合成による角度の求め方（1）

角度を出せば、両方のセンサがもつ良いところを引き出せます。

2012/2/8

39

センサの説明（19）ジャイロセンサと加速度センサの

合成による角度の求め方（2）

θ

g

・

θ

f

Δtジャイロセンサの角速度

加速度センサの角度 カットオフ周波数

サンプリング周期

RollAngle += ‐ RollAngle * 0.628 * 0.01 ¥

+ XAccelerationAngle * 0 628 * 0 01 ¥

aθ

kθ

0f

02 πfω =

ΔtθΔtωθΔtωθθθ g

・

akkk ++−=+ 1

加速度センサの角度

前回の角度

角度

角周波数

カットオフ周波数

+ XAccelerationAngle * 0.628 * 0.01 ¥

+ RollAngleRate * 0.01;

PitchAngle += ‐ PitchAngle * 0.628 * 0.01 ¥

+ YAccelerationAngle * 0.628 * 0.01 ¥

+ PitchAngleRate * 0.01;

カットオフ周波数1Hzの場合、0度は安定していますが、加速度センサの振動の成分の影響が若干でています。

センサの説明（20）ジャイロセンサと加速度センサの

合成による角度の求め方（3）

センサの振動の成分の影響が若干でています。

0

20

40

60

系列1

机に接地したときの衝撃

‐60

‐40

‐20

00 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

系列1

2012/2/8

40

カットオフ周波数0.1Hzの場合、安定した出力が得られています。

センサの説明（21）ジャイロセンサと加速度センサの

合成による角度の求め方（4）

0

20

40

60

系列1

‐60

‐40

‐20

00 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

系列1

カットオフ周波数0.01Hzの場合、ジャイロセンサのみで角度を求めたときと同じように、0度が増加しています。

センサの説明（22）ジャイロセンサと加速度センサの

合成による角度の求め方（5）

求めたときと同じように、0度が増加しています。

0

20

40

60

系列1

0度が増加

‐60

‐40

‐20

00 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.5 10

系列

2012/2/8

41

卓上で正しく角度が求まるようになりました。

センサの説明（23）ジャイロセンサと加速度センサの

合成による角度の求め方（6）

卓上で正しく角度が求まるようになったので、ゲインを調整して飛ばしてみましたが安定しません。

センサの説明（24）センサの振動対策（1）

飛ばしてみましたが安定しません。

モータの振動がセンサに影響を与えるので、ハード側でセンサにクッションを入れたり、信号にローパスフィルタを入れるなどの対策が必要だということがわかりました。

2012/2/8

42

ローパスフィルタなし、モータ停止中の波形

センサの説明（25）センサの振動対策（2）

1

0

1

2

3

4

5

6

00.5 1

1.5 2

2.5 3

3.5 4

4.5 5

5.5 6

6.5 7

7.5 8

8.5 9

9.5 10

系列1

1

0

1

2

3

4

5

6

0 0.5 1 1.5 2

系列1

‐6

‐5

‐4

‐3

‐2

‐1

‐6

‐5

‐4

‐3

‐2

‐1

拡大

ローパスフィルタなし、モータ動作中の波形

センサの説明（26）センサの振動対策（3）

1

0

1

2

3

4

5

6

0 0.5 1 1.5 2

25%

50%

75%1

0

1

2

3

4

5

6

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.510

25%

50%

75%

75％ 50％ 25％

‐6

‐5

‐4

‐3

‐2

‐1 75%

‐6

‐5

‐4

‐3

‐2

‐1 75%

拡大

2012/2/8

43

// フィルタなし

XAcceleration = (float)((short)((unsigned short)buf[1] << 8) ¥

( d h )b f[ ]) / * l f

センサの説明（27）センサの振動対策（4）

+ (unsigned short)buf[0]) / 16384.0 * 1.0 ‐ XAccelerationOfs;

YAcceleration = (float)((short)((unsigned short)buf[3] << 8) ¥

+ (unsigned short)buf[2]) / 16384.0 * 1.0 ‐ YAccelerationOfs;

ZAcceleration = (float)((short)((unsigned short)buf[5] << 8) ¥

+ (unsigned short)buf[4]) / 16384.0 * 1.0 ‐ ZAccelerationOfs;

// フィルタあり（10Hz）f = (float)((short)((unsigned short)buf[1]<<8)+(unsigned short)buf[0])/16384.0*1.0‐XAccelerationOfs;

XAcceleration += ‐XAcceleration * 62.8 * 0.01 + f * 62.8 * 0.01;

f = (float)((short)((unsigned short)buf[3]<<8)+(unsigned short)buf[2])/16384.0*1.0‐YAccelerationOfs;

YAcceleration += ‐YAcceleration * 62.8 * 0.01 + f * 62.8 * 0.01;

f = (float)((short)((unsigned short)buf[5]<<8)+(unsigned short)buf[4])/16384.0*1.0‐ZAccelerationOfs;

ZAcceleration += ‐ZAcceleration * 62.8 * 0.01 + f * 62.8 * 0.01;

// 2000dps フィルタなし

RollAngleRate = (float)((short)((unsigned short)buf[1] << 8) ¥

( d h )b f[ ]) * ll l f

センサの説明（28）センサの振動対策（5）

+ (unsigned short)buf[0]) * 0.07 ‐ RollAngleRateOfs;

PitchAngleRate = (float)((short)((unsigned short)buf[3] << 8) ¥

+ (unsigned short)buf[2]) * 0.07 ‐ PitchAngleRateOfs;

YawAngleRate = (float)((short)((unsigned short)buf[5] << 8) ¥

+ (unsigned short)buf[4]) * 0.07 ‐ YawAngleRateOfs;

// 2000dps フィルタあり（10Hz）f = (float)((short)((unsigned short)buf[1] << 8) + (unsigned short)buf[0]) * 0.07 ‐ RollAngleRateOfs;

RollAngleRate += (‐RollAngleRate * 62.8 * 0.01 + f * 62.8 * 0.01);

f = (float)((short)((unsigned short)buf[3] << 8) + (unsigned short)buf[2]) * 0.07 ‐ PitchAngleRateOfs;

PitchAngleRate += (‐PitchAngleRate * 62.8 * 0.01 + f * 62.8 * 0.01);

f = (float)((short)((unsigned short)buf[5] << 8) + (unsigned short)buf[4]) * 0.07 ‐ YawAngleRateOfs;

YawAngleRate += (‐YawAngleRate * 62.8 * 0.01 + f * 62.8 * 0.01);

2012/2/8

44

ローパスフィルタ10Hz、モータ動作中の波形

センサの説明（29）センサの振動対策（6）

1

0

1

2

3

4

5

6

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.510

25%

50%

75% 1

0

1

2

3

4

5

6

0 0.5 1 1.5 2

25%

50%

75%

75％ 50％ 25％

‐6

‐5

‐4

‐3

‐2

‐1 75%

‐6

‐5

‐4

‐3

‐2

‐1 75%

拡大

ローパスフィルタ5Hz、モータ動作中の波形

センサの説明（30）センサの振動対策（7）

1

0

1

2

3

4

5

6

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8 8.5 9 9.510

25%

50%

75% 1

0

1

2

3

4

5

6

0 0.5 1 1.5 2

25%

50%

75%

75％ 50％ 25％

‐6

‐5

‐4

‐3

‐2

‐1 75%

‐6

‐5

‐4

‐3

‐2

‐1 75%

拡大

2012/2/8

45

2012/2/8

46

5Hzのほうがよりきれいな波形が出力されていますが、位相遅れが大きくなるので、機体が安定しません。

センサの説明（31）センサの振動対策（8）

れが大きくなるので、機体が安定しません。

以上から、10Hzのフィルタを採用することにしました。

センサの説明（32）超音波センサ（1）

超音波の反射時間を測定して機体の高度の測定に使用します。

小型で検出距離が長く低消費電力な超音波センサを選択しました。

メーカー 型番 スペック

MaxBotix LV‐MaxSonar‐EZ0 検出距離：0～6.45m動作電圧：2.5V～5V

2012/2/8

47

センサの説明（33）超音波センサ（2）

f = (float)S12AD0.ADDR0A * 5 / 4096 / 0.384;

SonarH += ‐SonarH * 6.28 * 0.01 + f * 6.28 * 0.01;

センサの説明（34）赤外線センサ（1）

機体の位置の測定に使用します。

赤外線セ サ

赤外線LED

赤外線センサ Y

X

2012/2/8

48

センサの説明（35）赤外線センサ（2）

XPosition = ((unsigned short)(buf[3] & 0x30) << 4) + buf[1] ‐ 512;

YPosition = ((unsigned short)(buf[3] & 0xc0) << 2) + buf[2] ‐ 512;(( g )( [ ] ) ) [ ]

XSpeed = (XPosition ‐ XPositionOld) / 0.01;

YSpeed = (YPosition ‐ YPositionOld) / 0.01;

XPositionOld = XPosition;

YPositionOld = YPosition;

センサの説明（36）地磁気センサ

機体の方位角の測定に使用します（予定） 。

小型で3軸で入手性がよく、I2Cバス通信のできるHMC5883Lを選択しました。

メーカー 型番 スペック

HoneyWell HMC5883L 測定軸：3軸測定範囲：±8gauss分解能：5mgaussサイズ：3mm×3mm

2012/2/8

49

センサの説明（37）地磁気センサのキャリブレーション（1）

各軸のオフセットとゲインをそろえるため、8の字を描くように回転させてキャリブレーションをする必要があります。転させてキャリブレ ションをする必要があります。

Vmax

Vmin

offset0V

gain

2minmax VVoffset +

=

2minmax VVgain −

= gainoffsetVinX −

=

センサの説明（38）地磁気センサのキャリブレーション（2）

if(xmax < XMagnetism){

xmax = XMagnetism;

}

XMagnetismOfs = (xmax + xmin) / 2;

YMagnetismOfs = (ymax + ymin) / 2;

ZMagnetismOfs = (zmax + zmin) / 2;}

if(XMagnetism < xmin){

xmin = XMagnetism;

}

if(ymax < YMagnetism){

ymax = YMagnetism;

}

if(YMagnetism < ymin){

ymin = YMagnetism;

}

ZMagnetismOfs = (zmax + zmin) / 2;

XMagnetismGain = (xmax ‐ xmin) / 2;

YMagnetismGain = (ymax ‐ ymin) / 2;

ZMagnetismGain = (zmax ‐ zmin) / 2;

}

if(zmax < ZMagnetism){

zmax = ZMagnetism;

}

if(ZMagnetism < zmin){

zmin = ZMagnetism;

}

2012/2/8

50

センサの説明（39）方位角の求め方

// ガウスに変換

XMagnetism = (float)((short)((unsigned short)buf[0] << 8) + (unsigned short)buf[1]) * 8.1 / 2048;

YMagnetism = (float)((short)((unsigned short)buf[2] << 8) + (unsigned short)buf[3]) * 8 1 / 2048;YMagnetism = (float)((short)((unsigned short)buf[2] << 8) + (unsigned short)buf[3])   8.1 / 2048;

ZMagnetism = (float)((short)((unsigned short)buf[4] << 8) + (unsigned short)buf[5]) * 8.1 / 2048;

// ±1に正規化

XMagnetism = (XMagnetism ‐ XMagnetismOfs) / XMagnetismGain;

YMagnetism = (YMagnetism ‐ YMagnetismOfs) / YMagnetismGain;

ZMagnetism = (ZMagnetism ‐ ZMagnetismOfs) / ZMagnetismGain;

MagnetismYawAngle = atan( ¥

(XMagnetism * cos(RollAngle * (2 * 3.14) / 360) + ZMagnetism * sin(RollAngle * (2 * 3.14) / 360)) ¥

/ (YMagnetism * cos(PitchAngle * (2 * 3 14) / 360) + ZMagnetism * sin(PitchAngle * (2 * 3 14) / 360)) ¥/ (YMagnetism   cos(PitchAngle   (2   3.14) / 360) + ZMagnetism   sin(PitchAngle   (2   3.14) / 360)) ¥

) * 360 / (2 * 3.14);

if(YMagnetism<0){

if(XMagnetism<0){

MagnetismYawAngle = MagnetismYawAngle ‐ 180;

}else{

MagnetismYawAngle = MagnetismYawAngle + 180;

}

}

センサの説明（40）気圧センサ

機体の高度の測定に使用します（予定）。

超音波センサだけでは 短距離しか測定できないので 気圧セ超音波センサだけでは、短距離しか測定できないので、気圧センサで高度を測定します。

小型で入手性が良くかつI2Cバス通信できるBMP085を選択しま

メーカー 型番 スペック

BOSCH BMP085 300～1100hPa0.01hPa5mm×5mm

した。

超音波センサで測定出来るのは6.45mまで

6.45mより上は気圧センサを使用します

2012/2/8

51

センサの説明（41）気圧センサ

PressureHighDegree = 44330.8 * (1 ‐ pow((Pressure / 1013.25),0.190263));

2012/2/8

52

機体の説明（1）

モータの選定と、フレームの材質、強度を出すための工夫について説明します工夫について説明します。

現在の4ローターの機体を製作する以前にチルトロータの機体を製作してましたが、出力コントロールが難しく、推力によりRC

機体の説明（2）モータの選定（1）

を製作してましたが、出力コントロ ルが難しく、推力によりRCサーボの軸が動くことがあり、ホバリングするまでに至りませんでした。

2012/2/8

53

機体の説明（3）モータの選定（2）

メーカー 型番 重量［g］

KV値［rpm/V］

電力［W］

最大電流［A］

推力［g］

画像

GAUI GM410 41 1050 130 12A 410

TAHMAZO IR‐283020DF 164 2000 1000 45A 2000

機体の説明（4）フレームの材質

フレームの材質には1mm厚のプリント基板を使用しています。

プリント基板を使用することによって センサなどの配線をフレープリント基板を使用することによって、センサなどの配線をフレーム上に形成できるので、配線を固定する部品や配線自体の重量を減らし、見た目にもすっきりさせることができます。

2012/2/8

54

機体の説明（5）強度を出すための工夫（1）

最初にフレームをはしご状にして製作しました。

フレームがたわみ、モーターの軸がぶれてしまいました。

機体の説明（6）強度を出すための工夫（2）

補強を入れ強度を上げました。

2012/2/8

55

機体の説明（7）強度を出すための工夫（3）

モータの軸の方向が変わってしまうと、制御がうまくできないし、強度不足が振動を発生させる原因になってしまいます。強度不足が振動を発生させる原因になってしまいます。

ソフト以前に、機体の設計が大切です。

今後の自律型ホバリング・ヘリの展望

• フレームの軽量化

軽量化によって モータ出力を下げられ 動作時間を長くで軽量化によって、モ タ出力を下げられ、動作時間を長くできるため。

• プロペラのガードの発泡ポリプロピレン化

プロペラのガードが基板だと、当たったものが傷つくおそれがあるのでその対策と、軽量化のため。

• GPS搭載

目的地への移動のため。

• 側面に超音波センサ搭載

障害物の回避が行えるようにするため。

• ワイヤレスビデオカメラ搭載

撮影のため。

2012/2/8

56

ご清聴ありがとうございました。