2025StampFly勉強会

Drone Control Workshop ２０２５ StampFlyで学ぶ マルチコプタ制御 2025.3.24〜3.28 StampFly勉強会資料 伊藤恒平 ©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 スケジュール 1⽇⽬（24⽇） AM PM 2⽇⽬（25⽇） AM PM 3⽇⽬（26⽇） AM イントロダクション・クアッドコプタの運動原理 Lesson 0 環境構築 Lesson 1 モータを回す Lesson 2 送信機からの操縦指令の受信（⼿動操縦） 実習 Lesson 3 LED表⽰ Lesson 4 ⾓速度・加速度の取得 Lesson 5 ⾓速度フィードバック 実習 PM Lesson 6 PID制御 Lesson 7 テレメトリ Lesson 8ドローンのモデルとループ整形による制御系設計 実習 4⽇⽬（27⽇） AM PM Lesson 9 より⾼度な制御へ（姿勢推定について） 実習 5⽇⽬（28⽇） AM ドローンレース マルチコプタの制御 2

Drone Control Workshop 本テキストについて テキストはドクセルにアップロード されています。PDFをダウンロード もできますし、ブラウザ等で直接⾒ ることもできます。 講義の進捗に沿って次の講義の内容 を追加していきます。 https://www.docswell.com/s/Kouhei_Ito/K4VR7G-2025-03-23-104258 マルチコプタの制御 3

• https://www.docswell.com/s/Kouhei_Ito/K4VR7G-2025-03-23-104258

Drone Control Workshop Introduction マルチコプタの制御 4

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

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

Drone Control Workshop ロボットの動作概要 ドライバ 外乱 アク チュ エータ メカ 運動 環境 ノイズ センサ 制御 航法・誘導 位置・速度推定 ⽬標 マルチコプタの制御 7

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

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

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

Drone Control Workshop ロボットの動作概要 ドライバ 外乱 アク チュ エータ メカ 運動 環境 ノイズ センサ 制御 航法・誘導 ⽬標 位置・速度推定 ⾃分の興味のあるものはどこに当てはまりますか︖ マルチコプタの制御 11

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

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

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

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

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

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

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

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 GND4.2V@2.23A GND 1 2 3 マルチコプタの制御 4 19

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

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

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

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

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

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

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

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

Drone Control Workshop マルチコプタの制御 28

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

• 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/M5Fly-kanazawa/M5StampFly_skeleton マルチコプタの制御 30

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

Drone Control Workshop GitやGithubについて • バージョン管理のスタンダード • Githubでクラウド管理することで共 同開発を容易にする • 現在のソフト開発では必要不可⽋な 知識とスキル マルチコプタの制御 31

Drone Control Workshop Git の設定 Git Bash端末を検索して探し、起動してください マルチコプタの制御 32

Drone Control Workshop Git の設定 名前とメールアドレスの登録 端末で以下の様にして名前とメールアドレスを登録する 名前を登録します git config --global user.name ”kouhei" メールアドレスを登録します git config --global user.email ”kouhei@example.com" マルチコプタの制御 33

Drone Control Workshop Git の設定プロキシサーバの設定 学内WiFiに接続している場合プロキシの設定が必要です git config --global http.proxy "http://wwwproxy.kanazawa-it.ac.jp:8080" git config --global https.proxy "http://wwwproxy.kanazawa-it.ac.jp:8080" プロキシが不要になったら git config --global -–unset-all マルチコプタの制御 34

Drone Control Workshop Gitの設定の確認 git config --global -–list マルチコプタの制御 35

Drone Control Workshop サンプルコードの取得 • VSCodeのGitツールでサンプルコードをcloneする • Clone元 • https://github.com/M5Fly-kanazawa/M5StampFly.git マルチコプタの制御 36

• https://github.com/M5Fly-kanazawa/M5StampFly.git

Drone Control Workshop VSCodeでclone cloneとはリモート（Github）からローカル（⾃分のPCのフォルダ）に リポジトリをコピーしてくる事です。端末でもできますがVSCodeを使 う⼿順を説明します 👆 マルチコプタの制御 37

Drone Control Workshop VSCodeでM5StampFlyをclone マルチコプタの制御 38

Drone Control Workshop VSCodeでclone M5StampFlyと打ち込んで検索する M5Fly-kanazawaのM5StampFlyリ ポジトリを選択する マルチコプタの制御 39

Drone Control Workshop VSCodeでclone レポジトリを格納するフォルダを選ぶ マルチコプタの制御 40

Drone Control Workshop VSCodeでclone 開く マルチコプタの制御 41

Drone Control Workshop VSCodeの新しいウィンドウを開く ファイルメニューから「新しいウィンドウ」を選択 マルチコプタの制御 42

Drone Control Workshop フォルダを開く マルチコプタの制御 43

Drone Control Workshop ビルドと書き込み PC USB-Cケーブル StampFly PCとStampFlyを接続 ⾚丸のアイコンをクリック して書き込み 書き込みが失敗した場合はUSB-Cケーブルを抜いてStampのボタンを 押しながら再度ケーブルを接続して書き込み マルチコプタの制御 44

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

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

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

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

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

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

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

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

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

Drone Control Workshop Lesson0 Exercise 1. StampFly_skeltonレポジトリをclone 2. クローンしたプログラムをビルドしStampFly に書き込む 3. ペアリンクして右スティックのボタンを押し て右スティックを上に倒すとプロペラが回る ことを確認する マルチコプタの制御 53

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

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

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

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

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％で回す例 マルチコプタの制御 58

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

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 } マルチコプタの制御 60

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

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

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

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

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進 65

Drone Control Workshop Lesson1 Exercise 1. モータを2秒回転、2秒⽌まるを繰り返すプログラム を作成してください マルチコプタの制御 66

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

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

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

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

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

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の状態を交互に繰り返す） マルチコプタの制御 72

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

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

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

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); マルチコプタの制御 76

Drone Control Workshop Lesson2 Exercise 1. スロットル、エルロン、エレベータ、ラダーの 値をシリアル通信で数値を⾒て確認し、スティ クの倒す⽅向と値の正負、⼤⼩を確認しメモし なさい 2. スロットルスティックの倒し量に応じて上下、 ロール、ピッチ、ヨーの運動が起こる様にしな さい マルチコプタの制御 77

Drone Control Workshop LED表⽰ Lesson 3 マルチコプタの制御 78

Drone Control Workshop フルカラーシリアルLEDについて 1本の信号線で繋がった全ての LEDを制御できる （C）SwitchScience ItoCopter やIkaringの状態表⽰⽤ に使⽤ StampFlyはWS2812（シリアル LEDの⼀つ）を使⽤ 綺麗で役⽴つ︕ FastLEDライブラリが便利 https://github.com/FastLED/FastLED /wiki/Basic-usage マルチコプタの制御 79

• https://github.com/FastLED/FastLED/wiki/Basic-usage

Drone Control Workshop 今回⽤意したLED⽤の関数 onboard_led1(YELLOW, 1); onboard_led2(YELLOW, 1); ⾊の指定部分 点灯する︓１ 消灯する︓０ ⾊の指定について ⾊は１６進数で指定できる 0x00ffff マルチコプタの制御 80

Drone Control Workshop C⾔語の関数 .cppファイルの内部 型名 関数名（引数の型 引数変数）{ 処理したいコード } .hppファイルの内部 型名 関数名（引数の型 引数変数); void led_Illumination(void){ if(Time_counter<400){ onboard_led1(YELLOW, 1); onboard_led2(YELLOW, 1); } else if(Time_counter<800){ onboard_led1(0, 1); onboard_led2(0, 1); } else Time_counter = 0; Time_counter = Time_counter + 1; } マルチコプタの制御 81

Drone Control Workshop Lesson3 Exercise parking_mode関数の中から呼ばれる、LEDをクリスマスイルミ ネーション⾵に次から次へと⾊が変わる関数を作成し、停⽌時に LEDの⾊がカラフルに変更される様にしなさい。 (ヒントモータを2秒回転2秒停⽌の応⽤) 関数名︓led_Illumination ※parking_mode関数は0.0025周期で動く必要があるためその中のプロ グラムは速やかに終わる必要があります マルチコプタの制御 82

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

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

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

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

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

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; 構造体の定義はヘッダーファイルにかくとよい マルチコプタの制御 88

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; マルチコプタの制御 89

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

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 マルチコプタの制御 91

Drone Control Workshop Lesson5 Exercise • シリアルモニタを使って加速度三軸分と⾓速度三軸分をコンマで区 切って表⽰しなさい • その際、⼀番最初の列に今の時刻を秒単位で合わせて表⽰しなさい • 各軸の⾓速度の正⽅向がドローンの回転の正⽅向と⼀致しているか 確認しなさい（⼀致していない場合は報告してください） • X軸周りの回転速度がある程度以上ならLEDを⾚く光らせる • Y軸周りの回転速度がある程度以上ならLEDを緑に光らせる • Z軸周りの回転速度がある程度以上ならLEDを⻘く光らせる マルチコプタの制御 92

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Drone Control Workshop Lesson6 Exercise StampFlyの３軸のジャイロセンサから の⾓速度をフィードバックして⾓速度 を制御し⾶⾏することを実現してくだ さい マルチコプタの制御 114

Drone Control Workshop PID制御 Lesson 6 マルチコプタの制御 115

Drone Control Workshop PID制御とは • P（⽐例）制御︓誤差に⽐例した制御⼊⼒ • I（積分）制御︓誤差を積分して制御⼊⼒ • D（微分）制御︓誤差を微分して制御⼊⼒ マルチコプタの制御 116

Drone Control Workshop PID制御の効果 Pゲインで ⽴ち上が りが早く なる 過ぎると、 振動的に なる Iゲインで定常 偏差改善、外 乱応答も改善 過ぎると遅れ が⼤きくなり 振動的に 元の応答 Dゲインで、振動 を抑制する 過ぎると、ノイズ 等を増⼤し振動的 に マルチコプタの制御 117

Drone Control Workshop PID制御の基本的な実装 誤差 𝑦!"# 𝑒/ = 𝑦'*3 / − 𝑦/ $ ⽬標値 𝑦$ センサ出⼒ 数値積分（積分器） 𝑠/ = 𝑠/01 + 𝑒/ 𝑇2 数値微分 𝑑𝑒/ = (𝑒/ − 𝑒/01 )⁄𝑇2 PID制御の操作量 𝑢/ = 𝐾! (𝑒/ + 𝑠/ ⁄𝑇+ + 𝑇" 𝑑𝑒/ ) マルチコプタの制御 𝐾' :⽐例ゲイン 𝑇1 :積分時間 𝑇( :微分時間 𝜂:微分係数（0.125） 𝑇5 :制御周期 118

Drone Control Workshop PID制御器の実⽤上の実装 𝐾' :⽐例ゲイン 𝑇1 :積分時間 𝑇( :微分時間 𝜂:微分係数（0.125） 𝑇5 :制御周期 積分（台形積分） 𝑡2 𝑠/ = 𝑠/01 + (𝑒/ + 𝑒/01 ) 2𝑇+ 微分（不完全微分・フィルタつき微分） 2𝜂𝑇" − 𝑡2 2𝑇" 𝑑/ = 𝑑/01 + (𝑒/ − 𝑒/01 ) 2𝜂𝑇" + 𝑡2 2𝜂𝑇" + 𝑡2 PID制御の操作量 ! 𝑢/ = 𝐾! (𝑒/ + 𝑠/ + 𝑑/ ) マルチコプタの制御 (rad/s)までは微分器として働き "! ! (rad/s)からは微分の効果はほぼない #"! 119

Drone Control Workshop PIDクラスの使い⽅ pid.cppとpid.hppの中でクラスとして定義されている StampFly構造体の中でStampFly.pidとして既に宣⾔済みで、特に 何もすることなく使⽤できる ゲインの設定（ロールの例） StampFly.pid.roll.set_parameter(kp, Ti, Td, eta, Ts) ⽐例、積分時間、微分時間、微分係数、制御周期 制御⼊⼒の算出（ロールの例） roll_u = StampFly.pid.roll.update(error, Ts) 誤差、制御周期 リセット（ロールの例）︓積分器・フィルタリセット StampFly.pid.roll.reset() マルチコプタの制御 120

Drone Control Workshop Lesson6 Exercise Lesson5で作った⽐例制御のプログラ ムをPIDクラスを使ったものに改造し なさい。積分と微分は働かせなくとも 良い。違う時間で調整時間を設けます 積分時間︓100000 微分時間︓0 マルチコプタの制御 121

Drone Control Workshop Telemetry Lesson 7 マルチコプタの制御 122

Drone Control Workshop StampFly構造体に制御⽬標を追加 Stampfly.hppに以下を追加 Stampfly.hppのstampfly_tを変更 typedef struct{ float roll_ref; float pitch_ref; float yaw_ref; float throttle_ref; }control_ref_t; typedef struct{ sensor_value_t sensor; flag_t flag; counter_t counter; pidstruct_t pid; control_ref_t ref; times_t times; }stampfly_t; telemetory.cppを変更 さらにtelemetry.cppを修正 実際に指⽰します マルチコプタの制御 123

Drone Control Workshop Simple_stampfly_joyを更新 状況を確認して指⽰します マルチコプタの制御 124

Drone Control Workshop Joystickをケーブルで接続して通電 • 電池を外す（念の為） • PCのUSBケーブルを指す • ケーブルを⽩いマイコンのUSB-Cポートに接続 • その際にMode3にしたい場合はケーブルを指す前にLボタンを押 しておく • シリアルモニタボタンを押す マルチコプタの制御 125

Drone Control Workshop データの可視化 データをエクセル等で開いて確認する オレンジ︓ヨー⽬標値 ⻘︓ヨー⾓速度 マルチコプタの制御 126

Drone Control Workshop モデルとループ整形 Lesson 8 マルチコプタの制御 127

Drone Control Workshop 機体に働く推⼒とモーメント Stamp Flyの概要とモデリング

Drone Control Workshop ロータの配置 4 𝑦' 1 𝑦% 𝑥' 𝑥% 𝑥( 𝑥& 𝑦( 𝑦& 2 3 Stamp Flyの概要とモデリング

Drone Control Workshop 𝑇ℎ𝑟𝑢𝑠𝑡 推⼒の配分 𝑇ℎ𝑟𝑢𝑠𝑡 = 𝑇! + 𝑇" + 𝑇# + 𝑇$ Stamp Flyの概要とモデリング

Drone Control Workshop Left Right 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 推⼒の配分 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 = −𝑥! 𝑇! − 𝑥" 𝑇" + 𝑥# 𝑇# + 𝑥$ 𝑇$ Stamp Flyの概要とモデリング

Drone Control Workshop トルクの発⽣の向き はプロペラの回転⽅ 向とは逆なので注意 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 トルクの配分 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑄! − 𝑄" + 𝑄# − 𝑄$ Stamp Flyの概要とモデリング

Drone Control Workshop 機体に働く推⼒とモーメント 推⼒ 𝑇 = 𝑇! + 𝑇" + 𝑇# + 𝑇$ ロールモーメント 𝐿 = −𝑥! 𝑇! − 𝑥" 𝑇" + 𝑥# 𝑇# + 𝑥$ 𝑇$ ピッチモーメント 𝑀= ヨーモーメント 𝑁 = 𝑄! − 𝑄" + 𝑄# − 𝑄$ 𝑦! 𝑇! − 𝑦" 𝑇" − 𝑦# 𝑇# + 𝑦$ 𝑇$ Stamp Flyの概要とモデリング

Drone Control Workshop ４つの制御量を独⽴して制御可能 X 前後 • • • • スラスト ロール回転 ピッチ回転 ヨー回転 ロール回転 Y 左右 ピッチ回転 Z Stamp Flyの概要とモデリング ヨー回転 上下

Drone Control Workshop モータ・プロペラ系の制御 Stamp Flyの概要とモデリング

Drone Control Workshop プロペラの発⽣する 推⼒とトルク 推力 𝑇 Thrust プロペラの角速度 𝜔 プロペラの回転方向と トルクの向きは逆です トルク Torque 𝑄 Stamp Flyの概要とモデリング

Drone Control Workshop 記号の説明 推⼒とトルクの算出式 𝑇 = 𝐶! 𝜔 𝐶# 推⼒係数 " 𝑑𝜔 " 𝑄 = 𝐶# 𝜔 + 𝐽 𝑑𝑡 𝐶$ トルク係数 𝐽 ロータ・プロペラの慣性モーメント Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒とトルク 推⼒とトルクの算出式 𝑇 = 𝐶! 𝜔 𝐶# 𝜅= 𝐶! " 𝑄 = 𝐶# 𝜔 " 𝑄 = 𝜅𝑇 Stamp Flyの概要とモデリング

Drone Control Workshop モータプロペラ系モデル 𝑅 𝐿 インダクタンスは無視 ⾓速度 𝜔 𝑒 𝐾𝜔 𝐽 慣性モーメント 𝐾𝜔 モータプロペラの運動⽅程式 𝐽𝜔̇ + 𝐷𝜔 + 𝐶4 𝜔5 + 𝑞3 = 𝐾𝑖 電気の⽅程式 𝐷 𝑅𝑖 + 𝐾𝜔 = 𝑒 粘性係数 𝐾 逆起電⼒係数 𝑞3 動摩擦トルク Stamp Flyの概要とモデリング

Drone Control Workshop モータプロペラ系モデル モータプロペラの⾮線形モデル % 𝐾 𝐽𝜔̇ + 𝐷 + 𝑅 𝐾 % 𝜔 + 𝐶$ 𝜔 + 𝑞& = 𝑒 𝑅 モータプロペラ定常回転時 𝐾& 𝐾 & 𝐷+ 𝜔 + 𝐶) 𝜔 + 𝑞# = 𝑒 𝑅 𝑅 Stamp Flyの概要とモデリング

Drone Control Workshop ⼊⼒電圧とプロペラ⾓速度（定常状態） 𝐶) 𝜔& + & 𝜔= 𝐾 − 𝐷+ 𝑅 𝐾& 𝐾 𝐷+ 𝜔 + 𝑞# − 𝑒 = 0 𝑅 𝑅 + 𝐷+ & & 𝐾 𝑅 − 4𝐶) 2𝐶) Stamp Flyの概要とモデリング 𝐾 𝑞# − 𝑒 𝑅

Drone Control Workshop 電圧とプロペラ⾓速度 𝐶" 𝜔# + 𝐾# 𝐾 𝐷+ 𝜔 + 𝑞$ − 𝑒 = 0 𝑅 𝑅 𝐶" 2𝜔𝜔̇ + 𝐾# 𝐾 𝐷+ 𝜔̇ − = 0 𝑅 𝑅 𝑒で微分 𝐾 𝜔̇ = 2𝐶" 𝑅𝜔 + 𝐷𝑅 + 𝐾 # Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒と電圧 𝑇 = 𝐶* 𝜔& 𝜔= 𝑇 𝐶* 𝑅 𝑇 𝑒= 𝐶) + 𝐾 𝐶* 𝐾& 𝐷+ 𝑅 Stamp Flyの概要とモデリング 𝑇 + 𝑞# 𝐶*

Drone Control Workshop モータ・プロペラ定常特性の推定 モータプロペラ定常回転時 𝐾& 𝐾 & 𝐷+ 𝜔 + 𝐶) 𝜔 + 𝑞# = 𝑒 𝑅 𝑅 𝐾& 𝐾 𝐾 & 𝐾 𝐷+ 7 𝜔 + 𝐶) 8 𝜔 + 𝑞# 8 = 𝑒 𝑅 𝑅 𝑅 𝑅 𝐴𝜔& + 𝐵𝜔 + 𝐶 = 𝑒 実験により𝑒と𝜔の対応を取得し最⼩⼆ 乗法によりパラメータ𝐴, 𝐵, 𝐶を求める Stamp Flyの概要とモデリング

Drone Control Workshop パラメータ推定のための最⼩⼆乗法 𝐴𝜔& + 𝐵𝜔 + 𝐶 = 𝑒 より % 𝐽 = ∑ 𝐴𝜔& + 𝐵𝜔 + 𝐶 − 𝑒 & & 正規⽅程式 +, ++, +. +, +/ = ∑ 𝐴𝜔& + 𝐵𝜔 + 𝐶 − 𝑒 𝜔& = 0 = ∑ 𝐴𝜔& + 𝐵𝜔 + 𝐶 − 𝑒 𝜔 = 0 = ∑ 𝐴𝜔& + 𝐵𝜔 + 𝐶 − 𝑒 = 0 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒から⼊⼒電圧を算出 𝜔= 6 7! 5 𝑒 = 𝐴𝜔 + 𝐵𝜔 + 𝐶 𝑒=𝐴 6 7! +𝐵 6 7! +𝐶 Stamp Flyの概要とモデリング

Drone Control Workshop パラメータ推定のための最⼩⼆乗法 正規⽅程式の⾏列表現 ∑ 𝜔' ∑ 𝜔( ∑ 𝜔& ∑ 𝜔( ∑ 𝜔& ∑𝜔 ∑ 𝜔& ∑𝜔 𝑁 ' ∑𝜔 𝐴 ( ∑ = 𝜔 𝐵 𝐶 ∑ 𝜔& & ∑ 𝑒𝜔 𝐴 𝐵 = ∑ 𝑒𝜔 𝐶 ∑𝑒 ( ∑𝜔 ∑ 𝜔& ∑𝜔 & 0% ∑𝜔 ∑𝜔 𝑁 Stamp Flyの概要とモデリング ∑ 𝑒𝜔& ∑ 𝑒𝜔 ∑𝑒

Drone Control Workshop 推⼒係数・トルク係数の測定 Stamp Flyの概要とモデリング

Drone Control Workshop 実験結果 電圧[V] ⾓速度[rad/s] 1.04 1450 1.57 2083 2.04 2592 2.52 3152 3.01 3621 3.52 4097 パラメーター 値 𝐴 5.39e-8 𝐵 6.33e-4 𝐶 1.53e-2 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒係数の測定 ⾓速度[rad/s] 推⼒[N] 818.9 8.13e-3 1488 2.09e-2 2024 3.93e-2 2566 6.89e-2 3332 1.09e-1 3876 1.52e-1 パラメーター 値 𝐶" 1.00e-8 切⽚ -8.00e-5 Stamp Flyの概要とモデリング

Drone Control Workshop トルク係数の測定 ⾓速度[rad/s] トルク[Nm] 2083 2.94e-4 2592 5.42e-4 3152 7.77e-4 3621 1.18e-3 4097 1.50e-3 パラメーター 値 𝐶$ 9.75e-11 切⽚ -1.34e-4 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒と⼊⼒電圧の線形化 𝑑𝑒(𝑇% ) 𝑒 = 𝑒% + ∆𝑇 𝑑𝑇 𝐴 𝐵 𝑒 = 𝑒% + + ∆𝑇 𝐶& 2 𝐶& 𝑇% 𝑒% ∆𝑇 𝑒 = 𝑒% + 𝐶' ∆𝑇 𝑇% Stamp Flyの概要とモデリング 𝑑𝑒 ∆𝑇 𝑑𝑇

Drone Control Workshop 推⼒と⼊⼒電圧の線形化 𝑑𝑒(𝑇% ) 𝑒 = 𝑒% + ∆𝑇 𝑑𝑇 𝐴 𝐵 𝑒 = 𝑒% + + ∆𝑇 𝐶& 2 𝐶& 𝑇% 𝑒 = 𝑒% + 𝐶' ∆𝑇 𝑒% ∆𝑇 𝑒% − 𝐶& 𝑇% 𝑇% 𝑒 = 𝐶' 𝑇 + 𝑒% − 𝐶' 𝑇% Stamp Flyの概要とモデリング 𝑑𝑒 ∆𝑇 𝑑𝑇

Drone Control Workshop モータ・プロペラ系の フィードフォワードコントローラが完成 ⾮線形制御モデル ⽬標推⼒ 𝑇 = 𝐴 +𝐵 >! = +𝐶 >! 電圧 モータ ⾓速度 推⼒ プロペラ 𝑇 フィードフォワードコントローラ モータ・プロペラ系の逆システム 線形制御モデル ⽬標推⼒ 𝑇 𝐶0 𝑇 + 𝑒? − 𝐶0 𝑇? 電圧 フィードフォワードコントローラ モータ・プロペラ系の逆システム モータ ⾓速度 推⼒ プロペラ 𝑇 ※トリム点（釣り合い点）の周辺で有効 Stamp Flyの概要とモデリング

Drone Control Workshop 姿勢制御のためのモデリング Stamp Flyの概要とモデリング

Drone Control Workshop ニュートン・オイラー運動⽅程式 並進運動 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋1 + 𝑋2 𝑚 𝑣̇ + 𝑟𝑢 − 𝑝𝑤 = 𝑌1 + 𝑌2 𝑚 𝑤̇ + 𝑝𝑣 − 𝑞𝑢 = 𝑍1 + 𝑍2 + 𝑇 回転運動 𝐼3 𝑝̇ + 𝐼4 − 𝐼5 𝑞𝑟 = 𝐿 + 𝐿2 𝐼5 𝑞̇ + 𝐼3 − 𝐼4 𝑟𝑝 = 𝑀 + 𝑀2 𝐼4 𝑟̇ + 𝐼5 − 𝐼3 𝑝𝑞 = 𝑁 + 𝑁2 Stamp Flyの概要とモデリング

Drone Control Workshop オイラー⾓ベースの座標変換 機体座標⾓速度ー＞オイラー⾓ 𝜙̇ = 𝑝 + 𝑞 sin 𝜙 tan 𝜃 + 𝑟 cos 𝜙 tan 𝜃 𝜃̇ = 𝑞 cos 𝜙 − 𝑟 sin 𝜙 𝜓̇ = (𝑞 sin 𝜙 + 𝑟 cos 𝜙)/ cos 𝜃 ※ジンバルロックが起こる 機体座標速度ー＞地球座標速度 𝑋̇ ! = 𝑢 cos 𝜃 cos 𝜓 + 𝑣 sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 + 𝑤(cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓) 𝑌!̇ = 𝑢 cos 𝜃 sin 𝜓 + 𝑣 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 + 𝑤(cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓) 𝑍̇! = −𝑢 sin 𝜃 + 𝑣 sin 𝜙 cos 𝜃 + 𝑤 cos 𝜙 cos 𝜃 Stamp Flyの概要とモデリング

Drone Control Workshop クォータニオンベースの座標変換 機体座標⾓速度ー＞クォータニオン 𝑞"̇ = 0.5(−𝑝𝑞# − 𝑞𝑞$ − 𝑟𝑞% ) 𝑞#̇ = 0.5(𝑝𝑞" + 𝑟𝑞$ − 𝑞𝑞% ) 𝑞$̇ = 0.5(𝑞𝑞" − 𝑟𝑞# + 𝑝𝑞% ) 𝑞%̇ = 0.5(𝑟𝑞" + 𝑞𝑞# − 𝑝𝑞$ ) ※ジンバルロックがない 機体座標速度ー＞地球座標速度 𝑋̇ ! = 𝑞"$ + 𝑞#$ −𝑞$$ −𝑞&$ 𝑢 + 2 𝑞# 𝑞$ − 𝑞" 𝑞% 𝑣 + 2 𝑞# 𝑞% + 𝑞" 𝑞$ 𝑤 𝑌!̇ = 2 𝑞# 𝑞$ + 𝑞" 𝑞% 𝑢 + 𝑞"$ − 𝑞#$ + 𝑞$$ − 𝑞%$ 𝑣 + 2 𝑞$ 𝑞% − 𝑞" 𝑞# 𝑤 𝑍̇! = 2 𝑞# 𝑞% − 𝑞" 𝑞$ 𝑢 + 2 𝑞$ 𝑞% + 𝑞" 𝑞# 𝑣 + 𝑞"$ − 𝑞#$ − 𝑞$$ + 𝑞%$ 𝑤 Stamp Flyの概要とモデリング

Drone Control Workshop オイラー⾓とクォータニオン 𝐸## 𝐸$# 𝐸%# 𝐸#$ 𝐸$$ 𝐸%$ cos 𝜃 cos 𝜓 𝐸#% 𝐸$% = sin 𝜙 sin 𝜃 cos 𝜓 − cos 𝜙 sin 𝜓 𝐸%% cos 𝜙 sin 𝜃 cos 𝜓 + sin 𝜙 sin 𝜓 cos 𝜃 sin 𝜓 sin 𝜙 sin 𝜃 sin 𝜓 + cos 𝜙 cos 𝜓 cos 𝜙 sin 𝜃 sin 𝜓 − sin 𝜙 cos 𝜓 この⾏列を⽅向余弦⾏列（回転⾏列）とよぶ 𝜙 = tan@A 𝐸BC⁄𝐸CC B B 𝜃 = tan@A − 𝐸AC? 𝐸BC + 𝐸CC 𝜓 = tan@A 𝐸AB⁄𝐸AA Stamp Flyの概要とモデリング − sin 𝜃 sin 𝜙 cos 𝜃 cos 𝜙 cos 𝜃

Drone Control Workshop 機体に働く重⼒ 𝑋1 = −𝑚𝑔 sin 𝜃 𝑌1 = 𝑚𝑔 cos 𝜃 sin 𝜙 𝑍1 = 𝑚𝑔 cos 𝜃 cos 𝜙 Stamp Flyの概要とモデリング

Drone Control Workshop 機体に働く空気⼒ 𝑌G 𝜔 Y 𝑣 𝑁G 𝑢 𝑋G X 𝑉% 𝑤 𝛼 𝛽 Z 𝑍G Stamp Flyの概要とモデリング

Drone Control Workshop 機体に働く空気⼒ 𝑋2 = −𝐶6 𝑢 𝑌2 = −𝐶7 𝑣 𝑍2 = −𝐶8 𝑤 𝑁2 = −𝐶9 𝑟 ホバリングや定常⾶⾏を考える際は ロールやピッチの運動に関しては微⼩ だと考えて空⼒を無視する Stamp Flyの概要とモデリング

Drone Control Workshop 微⼩擾乱法による 運動⽅程式の線形化 Stamp Flyの概要とモデリング

Drone Control Workshop 微⼩擾乱法 𝑢 = 𝑢: + ∆𝑢 𝑣 = 𝑣: + ∆𝑣 𝑤 = 𝑤: + ∆𝑤 𝑟 = 𝑟: + ∆𝑟 𝑞 = 𝑞: + ∆𝑞 𝑋1 = 𝑋1: + ∆𝑋1 𝑋2 = 𝑋2: + ∆𝑋2 つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋1: + 𝑋2: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋1 + 𝑋2 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる Stamp Flyの概要とモデリング

Drone Control Workshop 微⼩擾乱法 𝑢 = 𝑢: + ∆𝑢 𝑣 = 𝑣: + ∆𝑣 𝑤 = 𝑤: + ∆𝑤 𝑟 = 𝑟: + ∆𝑟 𝑞 = 𝑞: + ∆𝑞 𝑋1 = 𝑋1: + ∆𝑋1 𝑋2 = 𝑋2: + ∆𝑋2 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋1: + 𝑋2: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋1 + 𝑋2 話を簡単にするためにホバリングの制御に限定 Stamp Flyの概要とモデリング

Drone Control Workshop 微⼩擾乱法 𝑢 = ∆𝑢 𝑣 = ∆𝑣 𝑤 = ∆𝑤 𝑟 = ∆𝑟 𝑞 = ∆𝑞 𝑋1 = ∆𝑋1 𝑋2 = ∆𝑋2 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋1: + 𝑋2: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋1 + 𝑋2 話を簡単にするためにホバリングの制御に限定 Stamp Flyの概要とモデリング

Drone Control Workshop 微⼩擾乱法 𝑢 = ∆𝑢 𝑣 = ∆𝑣 𝑤 = ∆𝑤 𝑟 = ∆𝑟 𝑞 = ∆𝑞 𝑋1 = ∆𝑋1 𝑋2 = ∆𝑋2 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋1: + 𝑋2: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋1 + 𝑋2 𝑚∆𝑢̇ = −𝑚𝑔∆𝜃 − 𝐶6 ∆𝑢 微⼩量同志の積は０とする Stamp Flyの概要とモデリング

Drone Control Workshop 線形化された運動⽅程式 並進運動 𝑚∆𝑢̇ = −𝑚𝑔∆𝜃 − 𝐶6 ∆𝑢 𝑚∆𝑣̇ = 𝑚𝑔∆𝜙 − 𝐶7 ∆𝑣 𝑚∆𝑤̇ = −𝐶8 ∆𝑤 + ∆𝑇 回転運動 𝐼3 ∆𝑝̇ = ∆𝐿 𝐼5 ∆𝑞̇ = ∆𝑀 𝐼4 ∆𝑟̇ = ∆𝑁 + 𝐶9 ∆𝑟 Stamp Flyの概要とモデリング

Drone Control Workshop プロペラ推⼒の線形化 𝑇 = 𝐶! 𝜔 " 線形化された推⼒ ∆𝑇 = 2𝐶! 𝜔$ ∆𝜔 線形化されたトルク ∆𝑄 = 2𝐶! 𝜔$ 𝜅∆𝜔 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒・モーメントの線形化 ∆𝑇 = 2𝐶& 𝜔' ∆𝜔! + ∆𝜔" + ∆𝜔# + ∆𝜔$ ∆𝐿 = 2𝐶& 𝜔' −𝑥! ∆𝜔! − 𝑥" ∆𝜔" + 𝑥# ∆𝜔# + 𝑥$ ∆𝜔$ ∆𝑀 = 2𝐶& 𝜔' 𝑦! ∆𝜔! − 𝑦" ∆𝜔" − 𝑦# ∆𝜔# + 𝑦$ ∆𝜔$ ∆𝑁 = 2𝐶& 𝜔' 𝜅∆𝜔! − 𝜅∆𝜔" + 𝜅∆𝜔# − 𝜅∆𝜔$ Stamp Flyの概要とモデリング

Drone Control Workshop モータ・プロペラ系の線形化 8" 8 5 𝐽𝜔̇ + 𝐷 + 9 𝜔 + 𝐶4 𝜔 + 𝑞3 = 9 𝑒 ̇ + 𝐷+ 𝐽∆𝜔 ̇ + 𝐽⁄𝐽 ∆𝜔 8" 8" 9 8 + 2𝐶4 𝜔: ∆𝜔 = ∆𝑒 9 8 8 𝐷 + 9 69 + 2𝐶4 𝜔: 69 ∆𝜔 = ∆𝑒 Stamp Flyの概要とモデリング

Drone Control Workshop モータ・プロペラ系の線形化 ̇ + 𝐷+ 𝐽∆𝜔 8 ̇ + 𝐽69 ∆𝜔 8" 9 8" 8 + 2𝐶4 𝜔: ∆𝜔 = ∆𝑒 9 8 8 𝐷 + 9 69 + 2𝐶4 𝜔: 69 ∆𝜔 = ∆𝑒 8 ̇ + 𝐵 + 2𝜔: 𝐴 ∆𝜔 = ∆𝑒 𝐽6 ∆𝜔 9 Stamp Flyの概要とモデリング

Drone Control Workshop モータ・プロペラ系の線形化 8 ̇ + 𝐵 + 2𝜔: 𝐴 ∆𝜔 = ∆𝑒 𝐽69 ∆𝜔 ̇ + 𝐶4 𝐵⁄𝐴 + 2𝐶4 𝜔: ∆𝜔 = 𝐽∆𝜔 𝐶D 𝐵 𝐾B =𝐷+ 𝐴 𝑅 𝐶D 𝐾 = 𝐴 𝑅 Stamp Flyの概要とモデリング 7# ; ∆𝑒

Drone Control Workshop モータの内部構造 Stamp Flyの概要とモデリング

Drone Control Workshop モータコイルの概形 重量 抵抗 12.4 0.58g 0.65ohm 6.5 21 慣性モーメント3.30e-8 kgm2 Stamp Flyの概要とモデリング 軸径 0.8 コイル外径 6 コイル厚 0.65 単位 mm

Drone Control Workshop プロペラ 全重量 ハブ ⽻4枚 7.4 30.6 Stamp Flyの概要とモデリング 0.36g 0.18g 0.18g

Drone Control Workshop プロペラ 全重量 ハブ ⽻4枚 7.4 30.6 Stamp Flyの概要とモデリング 0.36g 0.18g 0.18g 慣性モーメント2.01e-8 kgm2

Drone Control Workshop プロペラ 全重量 ハブ ⽻4枚 7.4 30.6 Stamp Flyの概要とモデリング 0.36g 0.18g 0.18g 慣性モーメント2.01e-8 kgm2

Drone Control Workshop 伝達関数の導出 Stamp Flyの概要とモデリング

Drone Control Workshop 回転運動の伝達関数 𝐼3 ∆𝑝̇ = ∆𝐿 ラプラス変換 𝐼3 𝑠∆𝑝 = ∆𝐿 𝐼5 ∆𝑞̇ = ∆𝑀 𝐼5 𝑠∆𝑞 = ∆𝑀 𝐼4 ∆𝑟̇ = ∆𝑁 + 𝐶9 ∆𝑟 𝐼4 𝑠 + 𝐶9 ∆𝑟 = ∆𝑁 Stamp Flyの概要とモデリング 1 ∆𝑝 = ∆𝐿 𝐼3 𝑠 1 ∆𝑞 = ∆𝑀 𝐼5 𝑠 1 ∆𝑟 = ∆𝑁 𝐼4 𝑠 + 𝐶9

Drone Control Workshop モータ・プロペラ系の伝達関数 ̇ + 𝐶4 𝐵⁄𝐴 + 2𝐶4 𝜔: ∆𝜔 = 𝐽∆𝜔 7# ; ∆𝑒 ラプラス変換 7# 𝐽𝑠∆𝜔 + 𝐶4 𝐵⁄𝐴 + 2𝐶4 𝜔: ∆𝜔 = ; ∆𝑒 7# ⁄; ∆𝜔 = =2>(7 @⁄;>57 A ) ∆𝑒 # # $ Stamp Flyの概要とモデリング ∆𝜔 = 8% C%2>1 ∆𝑒

Drone Control Workshop 上下速度の伝達関数 𝑚∆𝑤̇ = −𝐶8 ∆𝑤 + ∆𝑇 ラプラス変換 𝑚𝑠∆𝑤 = −𝐶8 ∆𝑤 + ∆𝑇 ∆𝑤 = % ;<=/! ∆8 ∆𝑇 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒の伝達関数 ∆𝑇 = 2𝐶* 𝜔: ∆𝜔% + ∆𝜔& + ∆𝜔( + ∆𝜔' H! ∆𝜔 = I JKL ∆𝑒 なので ! 2𝐶* 𝜔: 𝐾; ∆𝑇 = ∆𝑒% + ∆𝑒& + ∆𝑒( + ∆𝑒' 𝜏; 𝑠 + 1 Stamp Flyの概要とモデリング

Drone Control Workshop ロールモーメントの伝達関数 ∆𝐿 = 2𝐶* 𝜔: −𝑥% ∆𝜔% − 𝑥& ∆𝜔& + 𝑥( ∆𝜔( + 𝑥' ∆𝜔' 2𝐶* 𝜔: 𝐾; ∆𝐿 = −𝑥% ∆𝑒% − 𝑥& ∆𝑒& + 𝑥( ∆𝑒( + 𝑥' ∆𝑒' 𝜏; 𝑠 + 1 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒・モーメントの伝達関数まとめ 2𝐶* 𝜔: 𝐾; ∆𝑇 = ∆𝑒% + ∆𝑒& + ∆𝑒( + ∆𝑒' 𝜏; 𝑠 + 1 2𝐶* 𝜔: 𝐾; ∆𝐿 = −𝑥% ∆𝑒% − 𝑥& ∆𝑒& + 𝑥( ∆𝑒( + 𝑥' ∆𝑒' 𝜏; 𝑠 + 1 2𝐶* 𝜔: 𝐾; ∆𝑀 = 𝑦% ∆𝑒% − 𝑦& ∆𝑒& − 𝑦( ∆𝑒( + 𝑦' ∆𝑒' 𝜏; 𝑠 + 1 2𝜅𝐶* 𝜔: 𝐾; ∆𝑁 = ∆𝑒% − ∆𝑒& + ∆𝑒( − ∆𝑒' 𝜏; 𝑠 + 1 Stamp Flyの概要とモデリング

Drone Control Workshop 推⼒・モーメントの伝達関数まとめ 1 ∆𝑇 = 𝛿* 𝜏; 𝑠 + 1 1 ∆𝐿 = 𝛿@ 𝜏; 𝑠 + 1 1 ∆𝑀 = 𝛿? 𝜏; 𝑠 + 1 制御⼊⼒部分を１変数にまとめる 1 ∆𝑁 = 𝛿9 𝜏; 𝑠 + 1 Stamp Flyの概要とモデリング

Drone Control Workshop 制御⼊⼒とモータ⼊⼒電圧 制御⼊⼒とモータの⼊⼒電圧との関係を表す 𝛿* = 2𝐶* 𝜔: 𝐾; ∆𝑒% + ∆𝑒& + ∆𝑒( + ∆𝑒' 𝛿@ = 2𝐶* 𝜔: 𝐾; −𝑥% ∆𝑒% − 𝑥& ∆𝑒& + 𝑥( ∆𝑒( + 𝑥' ∆𝑒' 𝛿? = 2𝐶* 𝜔: 𝐾; 𝑦% ∆𝑒% − 𝑦& ∆𝑒& − 𝑦( ∆𝑒( + 𝑦' ∆𝑒' 𝛿? = 2𝜅𝐶* 𝜔: 𝐾; ∆𝑒% − ∆𝑒& + ∆𝑒( − ∆𝑒' Stamp Flyの概要とモデリング

Drone Control Workshop ミキシング 制御⼊⼒を各モータの⼊⼒電圧に振り分ける 𝛿= 1 𝛿E 0 = 2𝐶= 𝜔?𝐾G 𝛿F 0 𝛿F 0 0 1 0 0 0 0 0 0 1 0 0 𝜅 1 −𝑥A 𝑦A 1 1 −𝑥B −𝑦B −1 ∆𝑒A 1 1 ∆𝑒B −𝑥A = ∆𝑒C 2𝐶= 𝜔?𝐾G 𝑦A ∆𝑒H 1 1 −𝑥B −𝑦B −1 1 𝑥C −𝑦C 1 1 @A 1 𝑥H 0 𝑦H 0 0 −1 Stamp Flyの概要とモデリング 1 𝑥C −𝑦C 1 0 1 0 0 1 𝑥H 𝑦H −1 ∆𝑒A ∆𝑒B ∆𝑒C ∆𝑒H 0 0 @A 𝛿= 𝛿E 0 0 𝛿F 1 0 𝛿F 0 𝜅

Drone Control Workshop 各軸の独⽴制御 ミキシングの式を⽤いることにより 各軸を独⽴して制御することが可能となる Stamp Flyの概要とモデリング

Drone Control Workshop 実際の制御⼊⼒ 𝑒 = 𝑒! + ∆𝑒 𝑒:実際に加える⼊⼒電圧 𝑒: :ホバリングのための⼊⼒電圧 ∆𝑒:制御器が算出した制御⼊⼒ Stamp Flyの概要とモデリング

Drone Control Workshop 速度・⾓速度系の伝達関数まとめ 上下速度 ロール⾓速度 ピッチ⾓速度 ヨー⾓速度 ∆𝑤 = ∆𝑝 = ∆𝑞 = ∆𝑟 = % (;<=/! ∆8)(D$ <=%) % B& <(D$ <=%) % B% <(D$ <=%) 𝛿@ 𝛿? % (B" <=/# )(D$ <=%) 𝛿9 Stamp Flyの概要とモデリング 𝛿*

Drone Control Workshop PID制御について Stamp Flyの概要とモデリング

Drone Control Workshop ゲインチューニングの勘所 目標 − 𝛿# 𝐾!" 𝐾! 𝑠(𝜏𝑠 + 1) 比例制御器 𝑞 機体 𝛿# 比例制御器 𝐾! 𝑠(𝜏𝑠 + 1) ０dB低周波のゲインを上げる 𝑞 機体 -180deg これが一巡伝達関数（開ループ伝達関数） 𝐾'( 𝐾' 𝐿 𝑠 = 𝑠(𝜏𝑠 + 1) gc -20dB/dcより傾ける 定常偏差が減少 ゲイン交差周波数gcを大きくする 速応性向上（バンド幅大になる） フィードバックを切る 𝐾!" gc付近の傾きを緩く -20dB/dc 安定性向上 位相余裕を大きく 安定度増大 望ましいボード線図の形 安定性︓位相余裕を⼤きくする 速応性︓ゲイン交差周波数を⼤きくする 定常性︓低周波のゲインを上げる ロバスト性・対ノイズ性︓⾼周波のゲインを下げる Stamp Flyの概要とモデリング