2025StampFlyハンズオンITプラザ武蔵

Drone Control Workshop ２０２５ 〜マルチコプタを 触って学ぼう︕〜 StampFlyハンズオン 2025.6.20 ITビジネスプラザ武蔵 伊藤恒平 ©2025 Kouhei Ito, This work is openly licensed via CC BY 4.0.

• https://x.com/kouhei_kanazawa
• https://creativecommons.org/licenses/by/4.0/

Drone Control Workshop StampFlyについて Introduction マルチコプタの制御 2

Drone Control Workshop StampFly 300mAh1S電池 距離センサ 電圧・電流センサ M5StampS3 オプティカル フローセンサ RGBLED 拡張ポート 気圧センサ 距離センサ 加速度センサ ⾓速度センサ 地磁気センサ ブラシ付 DCモータ RGBLED 3

Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 ⽬標 マルチコプタの制御 4

Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 実装に直接関係するところ ⽬標 マルチコプタの制御 5

Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 制御対象の知識 ⽬標 マルチコプタの制御 6

Drone Control Workshop クアッドコプタの運動原理 X 前後 クアッドコプタの運動 ロール回転 1 上昇・下降 2 ロール回転 3ピッチ回転 4 ヨー回転 Y 左右 ピッチ回転 Z マルチコプタの制御 ヨー回転 上下 7

Drone Control Workshop クアッドコプタの運動原理 全てのプロペラを 同時に 同じだけ 強くしたり 弱くしたり すると 上昇・下降する X 上昇 下降 Y Z マルチコプタの制御 8

Drone Control Workshop クアッドコプタの運動原理 左のプロペラを 早く回転し 右のプロペラを 遅く回転すると 機体は ロール回転します 逆回転も同様 右に移動します X ロール回転 Y Z マルチコプタの制御 9

Drone Control Workshop クアッドコプタの運動原理 前のプロペラを 遅く回転し 後ろのプロペラを 早く回転すると 機体は ピッチ回転します X Y 前に移動 Z マルチコプタの制御 ピッチ回転 10

Drone Control Workshop クアッドコプタの運動原理 反時計回りしている プロペラを早くし 時計回りの プロペラを弱く すると 機体は時計回り にヨー回転する X Y Z ヨー回転 マルチコプタの制御 11

Drone Control Workshop クアッドコプタの運動原理 直接制御できる運動は ・上下運動 ・各軸周りの回転運動 これらの合計4つの運動のみ 前後運動と左右運動は直接制御できない マルチコプタの制御 12

Drone Control Workshop StampFlyのポート 内部I2C Motor Buzzer ボタン マルチコプタの制御 13

Drone Control Workshop StampFlyの回路 1 PIQ402 NLINT0SCL INT_SCL PIU106 6 SCL NLINT0SDA INT_SDA PIU107 7 COR8 R8 SI2302 PIR801 GND NLL0Up L-Up PIR502 SDA 5 A0 3 GND 17 PIU1017 GND 10KΩ PIU105 PIU103 GND I2C Addr 7-bit 40H PIR1302 PIR1201 COQ1 Q1NCE30P20Q VBAT PIQ109 PIQ108 PIQ107 PIQ106 PIQ105 PIC502 COC5 C5 PIC501 100nF NLIN1P IN1P PIQ103 PIQ102 PIQ101 COR16 R16 PIR1601 PIR2301 COJ3 J3 1 IIC_SCL 2 IIC_SDA PIJ302 3 VCC PIJ303 4 GND PIJ304 COR23 R23 4.7KΩ PIR2401 C COR18 R18 COR25 R25 PIR2502 PIR1802 +3.3V PIR10 2 PIR10 1 PIQ503 TH-1.25-2P COQ5 Q5 D PIC120 COC12 C12 PIC1201 100nF NLBEEP BEEP COR19 R19 NLL0Down L-Down 10Ω COLS1 LS1 PILS101 PIQ502 SI2302 GND PIR1101 PIQ203 COQ2 COC11 C11 COR11 R11 PIR1102 PIC1101 470Ω PIC1102 10uF PIQ201 PID20 COD2 D2 PIQ20 Q2 SS8050 Y1 1N4148WT PIR2102 100Ω PIQ501 PIR1901 PIR1902 PID201 PIU4014 PIU4013 4 9 PIU404 PIR2501 PIU409 10KΩ GND GND 8 PIU208 PIR901 PIU204 11 PIU201 +3.3V +3.3V E1 PIU30E1 8 VDD PIU408 5 VDDIO PIU405 12 CSB PIU4012 INT1 INT2 2 ASDX 3 ASCX PIU403 GNDIO GND 11 OSDO PIU4011 10 OCSB PIU4010 PIU30D2 PIR1502 PIU402 PIR1501 E3 PIC602 PIC702 COC6 COC7 C6 C7 PIC601 100nF PIC701 100nF NLCS CS COR15 R15 4.7KΩ GND PIU30E3 PID302 INT GND +3.3V PIP1012 PIM106 PIP107 PIM1011 PIM1016 5 SDO PIU505 1 GND PIU501 7 GND PIU506 PIU507 B2 COS1 USER_A S1 PIS104 PIU30C5 GND B4 INT_SDA SDI PIU30B4 PIC802 COC8 C8 PIC801 100nF A5 CSB PIU30A5 GND GND GND PIS102 PILED103 PIM1024 PIM1019 COJ2 J2 GROVE_I 1 I GROVE_OPIJ202 2 O 3 PIJ203 +5VOUT PIC1602 VCC 4 PIJ204 GND COC16 C16 100nF PIC1601 HY-2.0_IO PILED102 PILED203 PILED202 C GND GND VBAT_IN PIC1302 COC13 C13 PIC130 100nF PIP207 PIP202 B GND PIU30B2 SW-PB 16 G2 INT_SDA PIM103 3 15 PIP1015 G3 INT_SCL PIM104 4 GND 14 PIP1014 G4 COLED1 L-Up 5 LED1 PIM105 13 PIP1013NLINT0G1 G5 COP2 INT_G1 6 +3.3V P2 RGB 3 2 12 G6 DI GND NLINT0XSHUT INT_XSHUTPIM107 7 28 PIP2011 11 11 PIP1011NLEXT0G1 G7 3V3 PIM1028 EXT_G1 8 27 CSPIP2010 +3.3V 4 1 PIM1027 PILED104 10 PIP1010NLEXT0XSHUT PIM108 G8 G46 10 +3.3V VDD DO PILED101 2 0 1 C I P EXT_XSHUT 9 26 MISO PIM109 PIP209 9 PIP109 G9 G43/Tx PIM1026 9 COC10 C10 WS2812 NLR0Up PIC1902 L-Down 10 25 R-UpPIP208 PIM1010 PIM1025 8 PIP108NLGND G10 G42/MTMS 8 COC19 C19 PIC10 1 100nF GND 11 24 SCK 7 GND G44/Rx 7 NLR0Down INT1 12 23 R-Down PIC1901 100nF PIM1012 PIM1023 PIP206 6 6 PIP106NLM5V G11 G41/MTDI NLRST M5V 13 22 RST PIP205 PIM1013 5 PIP105 5V EN PIM1022 5 INT2 14 21 BEEP GND PIM1014 PIM1021 PIP204 4 PIP104 G12 G40/MTDO 4 NLUSER0A COLED2 SDA 15 20 USER_A GND LED2 PIM1015 PIP203 3 PIP103 G13 G0/Boot PIM1020 3 NLRGB MOSI 16 19 RGB 3 2 2 G14 G39/MTCK 2 DI GND SCL 17 18 PIP101 PIM1017 PIM1018 PIP201 1 G15 GND 1 4 1 +3.3V PIC1402 PILED204 VDD DO PILED201 Header 17 +5VOUT STAMP-S3-DIP-1.27 Header 11 COC14 C14 WS2812 M5V GND PIC1401 100nF PIM102 3 INT_SDA SDI PIU503 2 CSB 8 PIU508 Vdd 6 Vddio PIJ201 NLGROVE0I GROVE_I PIM101 1 17 PIP1017NLGROVE0O G1 GROVE_O 2 A Q3 SI2302 BMP280 +5VOUT COM1 M1 PIP1016 TH-1.25-2P GND PIU502 A1 PS PIU30A1 B5819W SL COP1 P1 PIQ30 COQ3 A3 INT_SCL SCK PIU30A3 PIC902 COC9 C9 PIC901 100nF GND BMM150 VDDIO GND D4 PIU30D4 DRDY +3.3V E5 PIU30E5 VDD GND COD3 D3 PID301 GND 10KΩ COU5 U5 INT_SCL 4 PIU504 SCK COU3 U3 PIJ402 PIQ302 COR9 R9 GND GND PIU30C1 PIR902 100Ω COJ4 J4 1 2 PIJ401 PIR601 PIQ301 2 AVSS 3 GND PIU203 4 GND2 6 GND3 PIU206 12 GND4 PIU2012 PIU2011 GND COR6 R6 PIR602 PIU202 AVDD PIC402 1 AVDD COC4 C4 PIC401 100nF VL53L3CXV0DH/1 GND SDO SDX SCX GND PIP102 COR21 R21 PIR210 NLINT1 INT1 4.7KΩ NLINT2 INT2 4.7KΩ 1 14 13 PIU401 COR10 R10 Buzzer VBAT_IN DNC 9 SDA 10 PIU2010 SCL +3.3V D2 VBAT_IN PID102 COD1 D1 PID10 R-Up PIU209 COU4 U4 PIR1801 GND PILS102 1 2 PIJ502 COU2 U2 5 PIU205 XSHUT 7 PIU207 GPIO1 BMI270 GND PIJ501 GND SDO=GND 0x68 SDO=VDDIO 0x69 6 7 NLSCL SCL NLSDA SDA PIR401 4.7KΩ +3.3V PIU407 4.7KΩ PIR301 4.7KΩ VBAT_IN PIC302 COC3 C3 PIC301 100nF COR4 R4 PIR1401 NLSCK SCK 1N4148WT COJ5 J5 PIR201 4.7KΩ VBAT_IN VBAT_IN GND PIU406 PIC1502 +5VOUT COC15 C15 PIC1501 100nF HY-2.0_IIC PIR10 4.7KΩ INT_SDA INT_SCL GND COR24 R24 PIJ301 PIR402 COR3 R3 GND COR14 R14 +3.3V PIR2402 +3.3V PIR302 COR2 R2 4.7KΩ 4.7KΩ 4.7KΩ PIR2302 +3.3V PIR20 COR1 R1 PIC20 2 PIC2102 PIC2 02 PIC2302 COC20 COC21 COC22 COC23 C20 C21 C22 C23 PIC20 1 22uF PIC210 22uF PIC2 01 22uF PIC2301 22uF NLMISO MISO NLMOSI MOSI +3.3V +3.3V PIR1402 10Ω PIR1701 GND +3.3V PIR102 INT_XSHUT INT_G1 IN1P CH2P PIR1602 PIQ104 COR17 R17 PIR1702 10Ω 0.01R COR13 R13 PIR1301 CH2N PIR702 COR12 R12 +3.3V INT_SCL INT_SDA EXT_G1 EXT_XSHUT VBAT_IN PIC1702 COC17 C17 PIC1701 100nF +3.3V INT_SCL PIR120 B 2 2 PIP302 4 4 PIP304 6 6 PIP306 8 8 PIP308 GND GND VBAT_IN NLIN1N COR7 IN1N PIR701 R7 PIP303 A2005WR-2x4P GND 16 VPU PIU1016 10 PV PIU1010 9 Critical PIU109 8 Warning PIU108 13 PIU1013 TC INA3221AIRGVR GND COP3 P3 1 1 3 3 5 PIP305 5 7 PIP307 7 PIP301 C1 COR5 R5 PIR802 100Ω PIQ401 PIR501 PIC1802 COC18 C18 PIC1801 100nF SDO NLCH2N CH2N NLCH2P CH2P VBAT PIC20 COC2 C2 PIC201 100nF PIU1012 INT_SDA PIQ403 TH-1.25-2P COQ4 Q4 +3.3V 4 VS PIU104 PIU1011 4 GND PIC102 COC1 C1 PIC10 100nF 1 2 PIJ102 PIJ101 3 C5 COU1 U1 11 IN-1 12 IN+1 14 PIU1014 IN-2 15 PIU1015 IN+2 1 PIU101 IN-3 2 PIU102 IN+3 VBAT_IN COJ1 J1 A 2 GND R-Down COR20 R20 PIR2002 PIR20 PIJ602 PIQ603 COQ6 TH-1.25-2P PIR2001 PIQ601 100Ω PIR201 GND COR22 R22 10KΩ GND COJ6 J6 1 2 PIJ601 PIQ602 Q6 SI2302 D [email protected] GND 1 2 3 マルチコプタの制御 4 14

Drone Control Workshop オープンループの制御 ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control StampFly ロール ピッチ ヨー ⾓速度 操縦できない Transmitter ATOMS3 ESP-NOW ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 15

Drone Control Workshop アクロモードの制御 ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control Gyro Transmitter ATOMS3 ESP-NOW StampFly ロール ピッチ ヨー ⾓速度 ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 16

Drone Control Workshop SAS(Stability Augmentation System) ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control Gyro Transmitter ATOMS3 ESP-NOW StampFly ロール ピッチ ヨー ⾓速度 ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 17

Drone Control Workshop スタビライズモードの制御 ⽬標⾓度 StampS3 ESP-NOW Transmitter ATOMS3 ESP-NOW StampS3 Flight Control Angle Estimater StampFly ロール ピッチ ヨー⾓ 速度 1 𝑠 ロール ピッチ ヨー⾓ Gyro Accel ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 18

Drone Control Workshop CAS(Control Augmentation System) ⽬標⾓度 StampS3 ESP-NOW Transmitter ATOMS3 ESP-NOW StampS3 Flight Control Angle Estimater StampFly ロール ピッチ ヨー⾓ 速度 1 𝑠 ロール ピッチ ヨー⾓ Gyro Accel ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 19

Drone Control Workshop 制御に必要な基本機能 ・モータの速度制御 ・LED表⽰やテレメトリ受信 ・送信機からの操縦指令の受信 ・加速度・⾓速度の取得 マルチコプタの制御 20

Drone Control Workshop 制御に必要な基本機能 ・PWM信号の出⼒ ・ESP-NOWでの通信確⽴ ・IMUからのデータの取得 マルチコプタの制御 21

Drone Control Workshop 開発環境構築 Lesson 0 マルチコプタの制御 22

Drone Control Workshop 弊ブログ「理系的戯れ」の以下のページ の内容にそって，開発環境をインストール してください https://rikei-tawamure.com/entry/2023/11/19/101426 マルチコプタの制御 23

• https://rikei-tawamure.com/entry/2023/11/19/101426

Drone Control Workshop サンプルのコードはGitHubにあります M5Fly-kanazawaのGitHubエントリーページ https://github.com/M5Fly-kanazawa StampFlyのファームウエア https://github.com/M5Fly-kanazawa/M5StampFly 本勉強会で使う⾻組みプログラム https://github.com/kouhei1970/M5StampFly_skeleton マルチコプタの制御 24

• https://github.com/kouhei1970/M5StampFly_skeleton
• https://github.com/M5Fly-kanazawa
• https://github.com/M5Fly-kanazawa/M5StampFly

Drone Control Workshop ATOM Joystick⽤のコードを取得 Joystick⽤のレポジトリ https://github.com/M5Fly-kanazawa/Simple_StampFly_Joy 1. VSCodeでClone 2. VSCodeで開く 3. ビルドして書き込み マルチコプタの制御 25

• https://github.com/M5Fly-kanazawa/Simple_StampFly_Joy

Drone Control Workshop StampFlyの操縦 無線通信 ESP-NOW WiFi BlueTooth マルチコプタの制御 26

Drone Control Workshop 送信機のスティックの意味 左ボタン 割り当てなし モード３にす るために 左ボタンを押 しながら電源 スイッチを⼊ れてください L 右ボタン ⾶⾏モード変更（アクロ・スタビライズ） R ③ ② ① 左スティックのボタン 宙返り これは モード３の設定です ストップ ウォッチ スタート ストップ リセット ④ 一般的な機能 ①エルロン（ロール制御） ②エレベータ（ピッチ制御） ③スロットル （推力制御） ④ラダー（ヨー制御） 右スティックのボタン ARM マルチコプタの制御 27

Drone Control Workshop StampFlyとコントローラの関連付け L R ② ③ Peering ① 👆 ③ 機体の電源を⼊れるか リセットボタンを押す ④ ① パネルボタンを押したまま電源を⼊れる ② パネルボタンを離し、指⽰に従いもう⼀度押す マルチコプタの制御 28

Drone Control Workshop 電池の付け⽅ ①電池に アダプタを 装着する ②アダプタは しっかり根 元まで⼊れ る ③機体のヘッ ダピン8本 をきっちり 合わせる ④根元まで しっかり⼊ れる マルチコプタの制御 29

Drone Control Workshop 現場ネコ案件 ❌ この様に左右に1本ずらし て⼊れると電池が短絡状態 になり燃えます🔥 マルチコプタの制御 30

Drone Control Workshop 完全に刺す前に確認 マルチコプタの制御 31

Drone Control Workshop うまく⾶ばすコツ ① ③ StampFlyと⾃分の向きを合わせるように ヨーを常に微調整する 通常は前後左右のトリム（0点）は狂っている ので常に舵を前後，左右に傾けて動かないと ころを探し⼩刻みに微調整する ② ④ 思い切って⾼度を上げて⼩刻みにスロットル を調整し、⼀定の⾼さを保つ練習を繰り返す ホバリング（空中停⽌）ができるまで繰り 返し練習する。 よくなったら、移動してみる マルチコプタの制御 32

Drone Control Workshop モータを回す Lesson 1 マルチコプタの制御 33

Drone Control Workshop モータの速度制御 Dutyの⼤⼩で速度が制御できる Duty ON OFF 周期 Period PWM信号 マルチコプタの制御 34

Drone Control Workshop PWMの周期（周波数）について ON OFF 周期 同じDutyで周波数(周期) が異なるPWM波形 OFF期間が⻑いと電流が0になってしまう（電流不連続） 電流が0になる前にON（電流連続） モータの電気的時定数より⼗分⼩さな値の周期にPWM周期を設定する必要がある マルチコプタの制御 35

Drone Control Workshop モータ制御関連の機能の付与 これらのファイルに モータ制御の機能が 記述されています 送受信を⾏うコードを記述するファイルの冒頭に以下を記述して ヘッダファイル（.hppファイル）を取り込む #include “motor.hpp” マルチコプタの制御 36

Drone Control Workshop モータの制御 モータをDutyに応じて回転させる関数 void motor_set_duty_fr(float duty) void motor_set_duty_fl(float duty) void motor_set_duty_rr(float duty) void motor_set_duty_rl(float duty) 使⽤例 motor_set_duty_fr(0.15) 左前のモータをDuty 15％で回す例 マルチコプタの制御 37

Drone Control Workshop メインループ関数について 今回のWorkshopでのプログラミングは0からプログラムを⾏う事はしません。 サンプルのプログラムに追加する形で制御の勉強を進めていきます 400Hzのメインループ関数 main_loop.cppの中にある loop_400Hz()関数の中に周期的に実⾏するコードを記述 loop_400Hz()関数は必ず400Hz（0.0025秒周期）で実⾏されます マルチコプタの制御 38

Drone Control Workshop タスク（仕事）の分岐 // Main loop void loop_400Hz(void) { // 400Hzで以降のコードが実⾏ update_loop400Hz(); // Mode select if (StampFly.flag.mode == INIT_MODE) init_mode(); else if (StampFly.flag.mode == AVERAGE_MODE) average_mode(); else if (StampFly.flag.mode == FLIGHT_MODE) flight_mode(); else if (StampFly.flag.mode == PARKING_MODE) parking_mode(); //// Telemetry telemetry(); StampFly.flag.oldmode = StampFly.flag.mode; // Memory now mode // End of Loop_400Hz function } マルチコプタの制御 39

Drone Control Workshop 着陸時のプログラム マルチコプタの制御 40

Drone Control Workshop ⾶⾏時のプログラム マルチコプタの制御 41

Drone Control Workshop プロペラをDuty15%で回す すべてDuty15％で回す マルチコプタの制御 42

Drone Control Workshop シリアル通信による printf デバグ PC USB-Cケーブル StampFly PCとStampFlyを接続 VSCodeで⾚丸のアイコン をクリック コードの任意の場所で以下のように記述するとHello Worldと端末に表⽰されます USBSerial.printf(“Hello World”); 実際にコードが実⾏されているかや変数の中⾝を確認するなどデバグに使えます マルチコプタの制御 43

Drone Control Workshop USBSerial.printf について C⾔語のprintf関数の使い⽅とほぼ同じです。C⾔語の参考書 を参照してください USBSerial.printf(”%d %d¥n¥r”, x, y); 整数xと整数yの内容をスペースを開けて表⽰する USBSerial.printf(”%6.3f¥n¥r”, x); 実数xを書式設定に基づいて 書式設定 %6.3fの意味は ％は書式設定を表す始めの記号 -1.234の様にマイナス記号と⼩数点を⼊れ ると６⽂字で⼩数点以下3⽂字ということ fは実数を表⽰する事を表す マルチコプタの制御 d 整数 f 実数 e 指数表⽰ x,X 16進 44

Drone Control Workshop 送信機からの操縦指令の受信 Lesson 2 マルチコプタの制御 45

Drone Control Workshop StampFlyの操縦 無線通信 ESP-NOW WiFi BlueTooth マルチコプタの制御 46

Drone Control Workshop ESP-NOWについて ・ESP32の独⾃の通信規格 ・2.4GHz WiFiの電波を使⽤ ・M5Stack製品同⼠の通信に便利 ・14チャンネルまで使⽤可能 マルチコプタの制御 47

Drone Control Workshop 送信機のスティックの意味 左ボタン 割り当てなし L 右ボタン ⾶⾏モード変更（アクロ・スタビライズ） R ③ ② ① 左スティックのボタン 宙返り これは モード３の設定です ストップ ウォッチ スタート ストップ リセット ④ 一般的な機能 ①エルロン（ロール制御） ②エレベータ（ピッチ制御） ③スロットル （推力制御） ④ラダー（ヨー制御） 右スティックのボタン ARM マルチコプタの制御 48

Drone Control Workshop 送受信関連の機能の付与 これらのファイルに 送受信の機能が記述 されています 送受信を⾏うコードを記述するファイルの冒頭に以下を記述して ヘッダファイル（.hppファイル）を取り込む #include “rc.hpp” マルチコプタの制御 49

Drone Control Workshop プログラムの中での取り扱い 姿勢制御関連の変数 Stick[RUDDER] ヨー制御 Stick[ELEVATOR] ピッチ制御 Stick[THROTTLE] 推⼒制御 Stick[AILERON] ロール制御 ボタン関連の変数 Stick[BUTTON_ARM] STK右 Stick[BUTTON_FLIP] STK左 Stick[CONTROLMODE] 右ボタン 姿勢制御関連の変数はスティックの倒れ量に⽐例した-1~1までの実数値を取る ただしStick[THROTTLE]の値は0〜1です ボタン関連の変数はボタンを押すと1 離すと0 ただし右ボタンはトグル出⼒（押すたびに0と1の状態を交互に繰り返す） マルチコプタの制御 50

Drone Control Workshop ⾶⾏時のプログラム（Stick読み込み） マルチコプタの制御 51

Drone Control Workshop Stickの状態を表⽰する マルチコプタの制御 52

Drone Control Workshop ロールの動きを起こす X ロール回転 左を強め Y Z 右を弱め マルチコプタの制御 53

Drone Control Workshop ロールの動きを起こす float throttle_delta = limit(Stick[THROTTLE], 0.0, 0.9); float roll_delta = limit(Stick[AILERON], -0.9, 0.9); float fl_duty = throttle_delta + roll_delta; float fr_duty = throttle_delta - roll_delta; float rl_duty = throttle_delta + roll_delta; float rr_duty = throttle_delta - roll_delta; 左を強め、右を同じだ け弱める motor_set_duty_fl(fl_duty); motor_set_duty_fr(fr_duty); motor_set_duty_rl(rl_duty); motor_set_duty_rr(rr_duty); マルチコプタの制御 54

Drone Control Workshop ⾓速度・加速度の取得 Lesson 4 マルチコプタの制御 55

Drone Control Workshop IMU=加速度計+⾓速度計 • 加速度計 • 三軸の加速度を計測 重⼒加速度を検知 • ⾓速度計（ジャイロ） • 三軸周りの⾓速度を検知 マルチコプタの制御 56

Drone Control Workshop 加速度センサの原理 おもり 正の⽅向に加速すると この曲がりを検知し て加速度とする 加速度 正⽅向 マルチコプタの制御 57

Drone Control Workshop 重⼒加速度の検知 静⽌ Z⽅向 正の⽅向の動的加速度 重⼒（静的加速度）は正の⽅向に働いてもマイナスの値として検出される マルチコプタの制御 58

Drone Control Workshop ⾓速度の検知 振動⽅向 コリオイ⼒ 圧電素⼦ 振動素⼦ 圧電振動ジャイロ マルチコプタの制御 59

Drone Control Workshop 構造体について PID制御器はゲインや積分値などの状態を保持しておく必要がありドローンだと制御する状 態が多くなるとたくさんの変数を⽤意する必要がある。 配列で解決するのも⼀つの⼿だがインデックスの数値が何を意味しているのか分かりづらく マクロを⽤意する必要が出てくる，そこまでするならば構造体を使うことを考えた⽅がよい． 構造体の定義 typedef struct { float kp; float ti; float td; float s; } pid_t; 使⽤する時 pid_t pitch_pid pitch_pid.kp = 2.0; pitch_pid.ti = 1000.0; pitch_pid.td = 0.05; pitch_pid_s = 0.0; 構造体の定義はヘッダーファイルにかくとよい マルチコプタの制御 60

Drone Control Workshop sensor_value_t構造体 typedef struct{ float accx; float accy; sensor_value_t構造体の定義 float accz; float roll_rate; sensor_value_t構造体の宣⾔ float pitch_rate; float yaw_rate; sensor_value_t sensor float roll_angel; sensor_value_t構造体のメンバー float pitch_angle; にアクセス float yaw_angle; float voltage; sensor.roll_rate uint16_t bottom_tof_range; }sensor_value_t; マルチコプタの制御 61

Drone Control Workshop StampFly構造体 typedef struct{ sensor_value_t sensor; flag_t flag; counter_t counter; pidstruct_t pid; times_t times; }stampfly_t; マルチコプタの制御 62

Drone Control Workshop IMU関係の変数（⾃動更新） 加速度 StampFly.sensor.accx StampFly.sensor.accy StampFly.sensor.accz ⾓速度 StampFly.sensor.roll_rate StampFly.sensor.pitch_rate StampFly.sensor.yaw_rate マルチコプタの制御 63

Drone Control Workshop Teleplotの紹介 • Teleplotと⾔うアドインをVSCodeに⼊れるとシリア ル通信データでグラフを描けるそうなので調べて⼊ れてジャイロや加速度計の出⼒をグラフ化して可視 化してみてください マルチコプタの制御 64

Drone Control Workshop ⾓速度フィードバック Lesson 5 マルチコプタの制御 65

Drone Control Workshop フィードバック制御によるSAS ⽬標⾓速度 StampS3 ESP-NOW + - KP StampFly ロール ピッチ ヨー ⾓速度 コントローラ Transmitter ATOMS3 Gyro ESP-NOW マルチコプタの制御 66

Drone Control Workshop ブロック線図の読み⽅ ⿊丸 信号s ⼊⼒u G(s) 信号s 分岐 出⼒y 信号s 伝達要素 ブロック y=G(s)u r ⽩丸 + 加え合わせ e - y マルチコプタの制御 e = r-y 67

Drone Control Workshop ⽐例制御 ⽬標⾓速度 + 誤差 - 制御⼊⼒ KP ⽐例ゲイン G(s) ロール StampFly ⽐例コントローラ H(s) Gyro ⽬標⾓速度とロール⾓速度の誤差を定数倍して制御⼊⼒として加える マルチコプタの制御 68

Drone Control Workshop ⽐例制御（信号名⼊り） ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro マルチコプタの制御 69

Drone Control Workshop 各信号の算出 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) StampFly ⽐例コントローラ H(s) 𝐻 𝑠 𝑝 ロール 𝑝 𝑝 Gyro 𝑒 =𝑟−𝐻 𝑠 𝑝 𝑢 = 𝐾! 𝑒 𝑝 = 𝐺(𝑠)𝑢 誤差の算出 制御則（⽐例制御） 制御対象の⼊出⼒ マルチコプタの制御 70

Drone Control Workshop 閉ループの⼊出⼒関係 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) StampFly ⽐例コントローラ 𝐻 𝑠 𝑝 ロール 𝑝 H(s) 𝑝 Gyro 𝐺(𝑠)𝐾' 𝑝= 𝑟 1 + 𝐺 𝑠 𝐾' 𝐻(𝑠) マルチコプタの制御 71

Drone Control Workshop 制御の効果 制御なしの⼊出⼒関係 𝑝 = 𝐺(𝑠)𝑢 制御ありの⼊出⼒関係 𝐺(𝑠)𝐾! 𝑝= 𝑟 1 + 𝐺 𝑠 𝐾! 𝐻(𝑠) 𝐾! は設計者が変えられるため特性を改善できる可能性がある マルチコプタの制御 72

Drone Control Workshop 例題︓単純な不安定システムの改善例 制御対象の伝達関数 1 𝐺(𝑠) = 𝑠−1 伝達関数の分⺟＝０にしたときSが プラスの場合不安定なシステム ※ドローンは不安定システムですがこれほど簡単ではありません センサの伝達関数 𝐻(𝑠) = 1 ⽬標rから出⼒pまでの伝達関数はどうなりますか︖ マルチコプタの制御 73

Drone Control Workshop ⽐例制御による改善 例題の場合の ⽬標値応答 𝐾! 𝑝= 𝑟 𝑠 + 𝐾! − 1 𝐾' が１を超えると分⺟＝０の解がマイナス になりシステム全体は安定となる システム全体の特性が改善された マルチコプタの制御 74

Drone Control Workshop ドローンに実装するにはどうする︖ ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) ロール 𝑝 StampFly ⽐例コントローラ H(s) 実際に存在 する部分 Gyro マルチコプタの制御 75

Drone Control Workshop 制御則 𝑢 = 𝐾! 𝑒 制御⼊⼒を誤差の定数倍で作り出す マルチコプタの制御 76

Drone Control Workshop ミキシング ドローンの基本運動 • 上下運動 • ロール運動 • ピッチ運動 • ヨー運動 ４つモータの回転 マルチコプタの制御 77

Drone Control Workshop ミキシング ドローンの基本運動 • 上下運動 • ロール運動 • ピッチ運動 • ヨー運動 ４つモータの回転 𝑓𝑙"#$% = 𝛿$&'($$)* + 𝛿'()) + 𝛿!+$,& + 𝛿%-. 𝑓𝑟"#$% = 𝛿$&'($$)* − 𝛿'()) + 𝛿!+$,& − 𝛿%-. 𝑟𝑙"#$% = 𝛿$&'($$)* + 𝛿'()) − 𝛿!+$,& − 𝛿%-. 𝑟𝑟"#$% = 𝛿$&'($$)* − 𝛿'()) − 𝛿!+$,& + 𝛿%-. マルチコプタの制御 78

Drone Control Workshop コントローラーからの⼊⼒ 運動 舵の名称 上下 スロットル THROTTLE 前︓＋ 後︓ー ロール エルロン AILERON 右︓＋ 左︓ー ピッチ エレベータ ELEVATOR 前︓ー 後︓＋ ヨー ラダー RUDDER 右︓＋ 左︓ー マルチコプタの制御 極性 79

Drone Control Workshop ドローン制御の実装 ⼿動制御 スティックの倒し量 ⽐例制御 制御則から作り出した量 マルチコプタの制御 ミキシングの式の 各デルタ ミキシングの式の 各デルタ 80

Drone Control Workshop ドローン制御の⽐例制御の実装 ⽐例制御 ⽬標⾓速度＝スティックの倒し量 誤差＝⽬標⾓速度ージャイロから得られた実際の⾓速度 制御⼊⼒＝⽐例ゲイン×誤差 マルチコプタの制御 81

Drone Control Workshop ⽐例制御実装のブロック線図表現 ⽬標⾓速度 𝑟 + 誤差 𝑒 - KP 制御⼊⼒ 𝑢 ⽐例コントローラー G(s) ロール 𝑝 StampFly H(s) Gyro ブロック線図は冗⻑な⾔葉での説明を図で表したもの マルチコプタの制御 82

Drone Control Workshop ４つの運動は独⽴に制御できる 𝑓𝑙()*+ = 𝛿*,-.**/0 + 𝛿-.// + 𝛿'1*2, + 𝛿+34 𝑓𝑟()*+ = 𝛿*,-.**/0 − 𝛿-.// + 𝛿'1*2, − 𝛿+34 𝑟𝑙()*+ = 𝛿*,-.**/0 + 𝛿-.// − 𝛿'1*2, − 𝛿+34 各デルタが 制御⼊⼒ 𝑟𝑟()*+ = 𝛿*,-.**/0 − 𝛿-.// − 𝛿'1*2, + 𝛿+34 ミキシングの式によりあらゆるデルタの組み合わ せに対して１対１で各モータの値が求められる ４つの運動を別々に制御則にあてはめ計算された制御⼊⼒をを ミキシングの式で統合して各モータのDutyを計算すれば良い マルチコプタの制御 83

Drone Control Workshop 実装時の注意 • デューティは１を超えてはいけない • モータはフル回転を⻑時間させると痛みやすいのでデューティ は0.95を上限にする • ⽐例ゲインは調整部分なので0.05程度から始めて、⼤⼩させて、 感覚で良いところを⾒つける • アーミングしたら、アーミング状態が⼀⽬でわかる様にLEDを ⻩⾊に点灯し、浮かび上がらない⼩さな回転数（アイドル回転 数）でプロペラを回す マルチコプタの制御 84

Drone Control Workshop 姿勢制御について ロール⾓⽬標 ⾓度PID ロール⾓ 姿勢推定 + 誤差 - ⾓速度PID u StampFly ロール運動 StampFly ロール⾓速度 Gyro ロールとピッチはこの様に⾓度制御する ヨーは⾓度推定ができないため⾓度制御をしない（⾓速度制御のみ） マルチコプタの制御 85

Drone Control Workshop PID制御やモデルとループ整形など この先や今回の話の詳細が気になる⽅は… StampFlyで学ぶマルチコプタ制御 https://www.docswell.com/s/Kouhei_Ito/K4VR7G-2025-0323-104258 「マルチコプタの運動と制御」基礎のきそ https://www.docswell.com/s/Kouhei_Ito/KDVNVK-202206-15-193343 マルチコプタの制御 86

• https://www.docswell.com/s/Kouhei_Ito/K4VR7G-2025-03-23-104258
• https://www.docswell.com/s/Kouhei_Ito/KDVNVK-2022-06-15-193343

Drone Control Workshop おしまい 今は、これが精⼀杯 マルチコプタの制御 87