M5Stamp Flyで学ぶ マルチコプタ制御入門
40K Views
August 28, 25
スライド概要
8月30日（土）・31日（日）で行う、ZEPエンジニアリング主催のStampFly講習会のテキストです。
■ドローンやロボットを自作することを通じて制御や関連技術の生涯勉強情報を提供■工学博士■防大航空宇宙→筑波大博士■陸自→対戦車誘導弾等の装備品開発→高専教員→大学教員■ロボットランサー優勝→マイクロマウスニューテクノロジー賞受賞■指導者としてつくばチャレンジバンナム賞→飛行ロボコンマルチコプタ部門１位等々■北海道函館出身
関連スライド
各ページのテキスト
Drone Control Workshop ２０２５ “M5Stamp Fly”で学ぶ マ ルチコプタ制御⼊⾨ ジャイロ・センサ活⽤からループ整形によるPID設計・チューニング，安定⾶⾏まで 伊藤恒平 ©2025 Kouhei Ito, This work is openly licensed via CC BY 4.0.
Drone Control Workshop 1⽇⽬スケジュール 10:00-10:30 オープニング・機材確認・安全説明 10:30-11:15 講義①︓クアッドコプタの運動原理（ロール・ピッチ・ヨー） 11:15-12:00 実習①︓スティック⼊⼒によるモータ制御（PWM動作確認） 13:00-13:45 実習②︓⾓速度センサの値を取得・表⽰する 13:45-14:30 実習③︓スティック⼊⼒→⾓速度⽬標→制御出⼒の流れを構成 14:30-15:15 講義②︓⾓速度制御とP制御の設計原理（簡易ループ設計） 15:15-17:00 実習④︓3軸（ピッチ・ロール・ヨー）のP制御実装と動作確認 マルチコプタの制御 2
Drone Control Workshop 2⽇⽬スケジュール 10:00-11:00 講義③︓ループ整形による制御設計（P/I/Dを統⼀的に解説） 11:00-12:00 実習⑤︓周波数応答に基づくPゲインの設計と実装 13:00-14:30 実習⑥︓設計ゲインでの制御性能評価（ログ取得・挙動⽐較） 14:30-15:30 実習⑦︓I項・D項の導⼊によるPID制御の⽐較評価 15:30-16:30 実習⑧︓⾶⾏デモと経験共有（短時間⾶⾏＋苦労話のシェア） 16:30-17:00 クロージング（全体まとめ，Q&Aほか） マルチコプタの制御 3
Drone Control Workshop 本テキストについて テキストはドクセルにアップロードされています。PDF をダウンロードもできますし、ブラウザ等で直接⾒るこ ともできます。 講義の進捗に沿って次の講義の内容を追加していきます。 テキストへのリンク https://www.docswell.com/s/Kouhei_Ito/5WM84J-stampfly_lec_zep マルチコプタの制御 4
Drone Control Workshop StampFlyについて Introduction マルチコプタの制御 5
Drone Control Workshop StampFly 300mAh1S電池 距離センサ 電圧・電流センサ M5StampS3 オプティカル フローセンサ RGBLED 拡張ポート 気圧センサ 距離センサ 加速度センサ ⾓速度センサ 地磁気センサ ブラシ付 DCモータ RGBLED 6
Drone Control Workshop StampFlyのプロペラの回転⽅向 前 機体を上から ⾒た場合です 反時計回り CCW 時計回り CW 時計回り CW 反時計回り CCW 超重要︕ マルチコプタの制御 7
Drone Control Workshop StampFlyのポート 内部I2C Motor Buzzer ボタン マルチコプタの制御 8
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 9
Drone Control Workshop 安全な⾶⾏試験のために Safty マルチコプタの制御 10
Drone Control Workshop ⾶⾏試験場所の約束 • 所定の場所で⾶⾏試験を⾏なってください マルチコプタの制御 11
Drone Control Workshop 回っているプロペラを上から覗かない ゆるいプロペラが時々あり ます。プロペラが真上に抜 けてきます • 回っている最中に上から 覗かない • 顔の真下や、顔に向いて いる状態でStampFlyのプ ロペラを回さない マルチコプタの制御 12
Drone Control Workshop 混信について 4chに設定されている StampFly B 3chに設定されている StampFly A Aとペアリングして いる送信機 マルチコプタの制御 本来Aを操縦しようと思っていてもチャ ンネルが隣り合っているBにも影響を与 えてしますBの操縦種が意図しない動き をする場合がある 13
Drone Control Workshop 混信について 1. ⾶⾏試験する際はホワイトボードに⾃分のチャンネル番号を書いておく 2. 同時に⾶ばす⼈はチャンネル番号を確認して譲り合う 3. ⾶⾏試験時「チャンネル３番で⾶ばします︕」宣⾔をする 4. 机で作業している⼈もチャンネル番号を意識した作業 5. ⾶⾏試験が終わったら書いたチャンネル番号を消す マルチコプタの制御 14
Drone Control Workshop ESP-NOWの混信について 混信が起きる状況（推定） 固有のMACアド レスがあります StampFlyのMAC アドレスを指定 して送信 MACアドレスを FF:FF:FF:FF:FF:FFにして ブロードキャスト • 基本的にはMACアドレスを指定することで ドローンと送信機は１対１の通信をするこ とができる • この際、同じチャンネルを使っていても混 信はほぼ起こらない • ただしブロードキャストは受信してしまう 対策案 送信データに受信側のMACアドレスを含め 受信側で⾃分に送られてきたデータか識別 し、ブロードキャストの影響を低減するこ ESP-NOWを使⽤している他の製品 とを試みる
Drone Control Workshop ペアリング時の注意 電源投⼊時に常に MACアドレスを FF:FF:FF:FF:FF:FFにして ブロードキャスト 機体のMACアドレスや チャンネル情報を送る 同時にペアリングをすると混信してうまくできない︕ • • ペアリングする際はそれを宣⾔「ペアリングします︕」 他の⼈がStampFlyの電源を投⼊した直後でないことを確認 マルチコプタの制御 16
Drone Control Workshop チャンネル番号の設定はソースコー ドを書き換えることで⾏います 後ほどチャンネル番号の設定の⽅法 はお話しします マルチコプタの制御 17
Drone Control Workshop 不具合の⼤きな原因 • モータの良し悪しに左右される • モータの経年劣化 • モータの当たり外れ マルチコプタの制御 18
Drone Control Workshop ⾶⾏ロボットの 制御ループ Control Loop マルチコプタの制御 19
Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 ⽬標 マルチコプタの制御 20
Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 実装に直接関係するところ ⽬標 マルチコプタの制御 21
Drone Control Workshop StampFlyの動作概要 トランジ スタ スイッチ ミキシング PID制御 外乱 ブラシ モータ プロペラ フレーム スタビライズ アクロ モード 上昇 下降 回転 運動 環境 ノイズ ジャイロ 加速度計 距離計 ⾓度推定 ⾼さ推定 位置推定 制御対象の知識 ⽬標 マルチコプタの制御 22
Drone Control Workshop クアッドコプタの⾶⾏原理 Flight Law of Quadcopter マルチコプタの制御 23
Drone Control Workshop クアッドコプタの運動 X 前後 1 上昇・下降 2 ロール回転 3ピッチ回転 4 ヨー回転 ロール回転 Y 左右 ピッチ回転 ヨー回転 Z マルチコプタの制御 上下 24
Drone Control Workshop 上下運動 X 全てのプロペラを 同時に 同じだけ 強くしたり 弱くしたり すると 上昇・下降する 上昇 下降 Y Z マルチコプタの制御 25
Drone Control Workshop ロール運動 左のプロペラを 早く回転し 右のプロペラを 遅く回転すると 機体は ロール回転します 逆回転も同様 右に移動します X 後ろから⾒て時計回りが正 ロール回転 Y Z マルチコプタの制御 26
Drone Control Workshop ピッチ運動 前のプロペラを 遅く回転し 後ろのプロペラを 早く回転すると 機体は ピッチ回転します X Y ピッチ回転 前に移動 Z マルチコプタの制御 前（頭）が上がる向きが正 27
Drone Control Workshop しっかり理解 ヨー運動 X 反時計回りしている プロペラを早くし 時計回りの プロペラを遅くすると 機体は時計回り にヨー回転する Y 上から⾒て時計回りが正 ヨー回転 Z マルチコプタの制御 28
Drone Control Workshop クアッドコプタの運動原理 直接制御できる運動は ・上下運動 ・各軸周りの回転運動 これらの合計4つの運動のみ 前後運動と左右運動は直接制御できない マルチコプタの制御 29
Drone Control Workshop オープンループの制御 ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control StampFly ロール ピッチ ヨー ⾓速度 操縦できない Transmitter ATOMS3 ESP-NOW ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 30
Drone Control Workshop アクロモードの制御 ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control Gyro Transmitter ATOMS3 ESP-NOW StampFly ロール ピッチ ヨー ⾓速度 ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 31
Drone Control Workshop SAS(Stability Augmentation System) ⽬標⾓速度 StampS3 ESP-NOW StampS3 Flight Control Gyro Transmitter ATOMS3 ESP-NOW StampFly ロール ピッチ ヨー ⾓速度 ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 32
Drone Control Workshop スタビライズモードの制御 ⽬標⾓度 StampS3 ESP-NOW Transmitter ATOMS3 ESP-NOW StampS3 Flight Control Angle Estimater StampFly ロール ピッチ ヨー⾓ 速度 1 𝑠 ロール ピッチ ヨー⾓ Gyro Accel ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 33
Drone Control Workshop CAS(Control Augmentation System) ⽬標⾓度 StampS3 ESP-NOW Transmitter ATOMS3 ESP-NOW StampS3 Flight Control Angle Estimater StampFly ロール ピッチ ヨー⾓ 速度 1 𝑠 ロール ピッチ ヨー⾓ Gyro Accel ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 34
Drone Control Workshop 制御に必要な基本機能 ・モータの速度制御 ・LED表⽰やテレメトリ受信 ・送信機からの操縦指令の受信 ・加速度・⾓速度の取得 マルチコプタの制御 35
Drone Control Workshop 制御に必要な基本機能 ・PWM信号の出⼒ ・LEDチカチカ ・ESP-NOWでの通信確⽴ ・IMUからのデータの取得 マルチコプタの制御 36
Drone Control Workshop マルチコプタの制御 37
Drone Control Workshop 開発環境の構築 Lesson0 マルチコプタの制御 38
Drone Control Workshop 弊ブログ「理系的戯れ」の以下のページ の内容にそって，開発環境をインストール してください https://rikei-tawamure.com/entry/2023/11/19/101426 マルチコプタの制御 39
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 本講習で使う送信機のプログラム https://github.com/M5Fly-kanazawa/Simple_StampFly_Joy マルチコプタの制御 40
Drone Control Workshop GitやGithubについて • バージョン管理のスタンダード • Githubでクラウド管理することで共 同開発を容易にする • 現在のソフト開発では必要不可⽋な 知識とスキル マルチコプタの制御 41
Drone Control Workshop Git の設定 Git Bash端末を検索して探し、起動してください マルチコプタの制御 42
Drone Control Workshop Git の設定 名前とメールアドレスの登録 端末で以下の様にして名前とメールアドレスを登録する 名前を登録します git config --global user.name ”kouhei" メールアドレスを登録します git config --global user.email ”[email protected]" マルチコプタの制御 43
Drone Control Workshop Git の設定プロキシサーバの設定 プロキシが必要な場合は設定が必要です 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 http.proxy git config --global –unset https.proxy マルチコプタの制御 44
Drone Control Workshop Gitの設定の確認 git config --global -–list マルチコプタの制御 45
Drone Control Workshop プログラムの取得 ビルドと書き込み マルチコプタの制御 46
Drone Control Workshop サンプルコードの取得 • VSCodeのGitツールでサンプルコードをcloneする • Clone元 • https://github.com/M5Fly-kanazawa/M5StampFly_skeleton マルチコプタの制御 47
Drone Control Workshop VSCodeでclone cloneとはリモート（Github）からローカル（⾃分のPCのフォルダ）に リポジトリをコピーしてくる事です。端末でもできますがVSCodeを使 う⼿順を説明します 👆 マルチコプタの制御 48
Drone Control Workshop VSCodeでM5StampFlyをclone マルチコプタの制御 49
Drone Control Workshop clone元のGItHubのURLを指定 M5Fly-kanazawaの M5StampFly_skeletonのURLをその まま⼊⼒ マルチコプタの制御 50
Drone Control Workshop cloneする先のフォルダを指定 レポジトリを格納するフォルダを選ぶ マルチコプタの制御 51
Drone Control Workshop cloneできたら開く 開く マルチコプタの制御 52
Drone Control Workshop ⼀度閉じたら VSCodeの新しいウィンドウを開く ファイルメニューから「新しいウィンドウ」を選択 マルチコプタの制御 53
Drone Control Workshop フォルダを開く マルチコプタの制御 54
Drone Control Workshop 電池を抜き、USBケーブル接続 ここに ボタン あります 電池を抜く USBケーブルを接続 書き込めなかったらボタンを押しながら接続 マルチコプタの制御 55
Drone Control Workshop ビルドと書き込み PC USB-Cケーブル StampFly PCとStampFlyを接続 ⾚丸のアイコンをクリック して書き込み 書き込みが失敗した場合はUSB-Cケーブルを抜いてStampのボタンを 押しながら再度ケーブルを接続して書き込み マルチコプタの制御 56
Drone Control Workshop ATOM Joystick⽤のコードを取得 Joystick⽤のレポジトリ https://github.com/M5Fly-kanazawa/Simple_StampFly_Joy 1. VSCodeでClone 2. VSCodeで開く 3. ビルドして書き込み マルチコプタの制御 57
Drone Control Workshop StampFlyとコントローラのペアリング L R ② ③ Peering ① 👆 ③ 機体の電源を⼊れるか リセットボタンを押す ④ ① パネルボタンを押したまま電源を⼊れる ② パネルボタンを離し、指⽰に従いもう⼀度押す マルチコプタの制御 58
Drone Control Workshop チャンネル番号の設定 👈 マルチコプタの制御 59
Drone Control Workshop チャンネル番号の設定 👈 マルチコプタの制御 60
Drone Control Workshop チャンネル番号の設定 👈 この３を⾃分の チャンネル番号 に変える マルチコプタの制御 61
Drone Control Workshop 当初のチャンネル番号について • 座席に置いてある付箋紙の番号に設定してください マルチコプタの制御 62
Drone Control Workshop StampFly_skeletonのLEDの⾊の意味 LED⽩点灯 LED紫点灯 LED緑点灯 起動中 センサ校正中 ⾶⾏準備完了 校正中は ⽔平⾯に置いて 触らない︕ 安全状態 マルチコプタの制御 63
Drone Control Workshop 電池切れサイン 電池切れサインです。 これ以上使うと、マイコンにリセット がかかり墜落します。 また、過放電になり電池が使えなくな ります。 LEDが時々⽔⾊になったら電池を交換 し、外した電池は充電してください LED⽔⾊点灯 マルチコプタの制御 64
Drone Control Workshop 送信機のスティックの意味 左ボタン 割り当てなし モード３にす るために 左ボタンを押 しながら電源 スイッチを⼊ れてください L 右ボタン ⾶⾏モード変更（アクロ・スタビライズ） R ③ ② ① 左スティックのボタン 宙返り これは モード３の設定です ストップ ウォッチ スタート ストップ リセット ④ 一般的な機能 ①エルロン（ロール制御） ②エレベータ（ピッチ制御） ③スロットル （推力制御） ④ラダー（ヨー制御） 右スティックのボタン ARM マルチコプタの制御 65
Drone Control Workshop ブランチの変更 各Lessonの解答 例をブランチを 切り替えること によって⾒るこ とができます。 👇 マルチコプタの制御 66
Drone Control Workshop ブランチの変更 ⾒たいLesson番号 のブランチを選ぶ 👉 マルチコプタの制御 67
Drone Control Workshop Lesson0 Exercise 1. StampFly_skeltonレポジトリをclone 2. クローンしたプログラムをビルドしStampFly に書き込む 3. ペアリンクして右スティックのボタンを押し て右スティックを上に倒すとプロペラが回る ことを確認する マルチコプタの制御 68
Drone Control Workshop モータを回す Lesson 1 マルチコプタの制御 69
Drone Control Workshop モータの速度制御 Dutyの⼤⼩で速度が制御できる Duty ON OFF 周期 Period PWM信号 マルチコプタの制御 70
Drone Control Workshop PWMの周期（周波数）について ON OFF 周期 同じDutyで周波数(周期) が異なるPWM波形 OFF期間が⻑いと電流が0になってしまう（電流不連続） 電流が0になる前にON（電流連続） モータの電気的時定数より⼗分⼩さな値の周期にPWM周期を設定する必要がある マルチコプタの制御 71
Drone Control Workshop モータ制御関連の機能の付与 これらのファイルに モータ制御の機能が 記述されています 送受信を⾏うコードを記述するファイルの冒頭に以下を記述して ヘッダファイル（.hppファイル）を取り込む #include “motor.hpp” マルチコプタの制御 72
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％で回す例 マルチコプタの制御 73
Drone Control Workshop メインループ関数について 今回のWorkshopでのプログラミングは0からプログラムを⾏う事はしません。 サンプルのプログラムに追加する形で制御の勉強を進めていきます 400Hzのメインループ関数 main_loop.cppの中にある loop_400Hz()関数の中に周期的に実⾏するコードを記述 loop_400Hz()関数は必ず400Hz（0.0025秒周期）で実⾏されます マルチコプタの制御 74
Drone Control Workshop タスク（仕事）の分岐 マルチコプタの制御 75
Drone Control Workshop 着陸時のプログラム マルチコプタの制御 76
Drone Control Workshop ⾶⾏時のプログラム マルチコプタの制御 77
Drone Control Workshop プロペラをDuty15%で回す すべてDuty15％で回す マルチコプタの制御 78
Drone Control Workshop シリアル通信による printf デバグ PC USB-Cケーブル StampFly PCとStampFlyを接続 VSCodeで⾚丸のアイコン をクリック コードの任意の場所で以下のように記述するとHello Worldと端末に表⽰されます USBSerial.printf(“Hello World”); 実際にコードが実⾏されているかや変数の中⾝を確認するなどデバグに使えます マルチコプタの制御 79
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進 80
Drone Control Workshop Lesson1 Exercise 1. モータを2秒回転、2秒⽌まるを繰り返すプログラム を作成してください マルチコプタの制御 81
Drone Control Workshop 送信機からの操縦指令の受信 Lesson 2 マルチコプタの制御 82
Drone Control Workshop StampFlyの操縦 無線通信 ESP-NOW WiFi BlueTooth マルチコプタの制御 83
Drone Control Workshop ESP-NOWについて ・ESP32の独⾃の通信規格 ・2.4GHz WiFiの電波を使⽤ ・M5Stack製品同⼠の通信に便利 ・14チャンネルまで使⽤可能 マルチコプタの制御 84
Drone Control Workshop 送信機のスティックの意味 左ボタン 割り当てなし L 右ボタン ⾶⾏モード変更（アクロ・スタビライズ） R ③ ② ① 左スティックのボタン 宙返り これは モード３の設定です ストップ ウォッチ スタート ストップ リセット ④ 一般的な機能 ①エルロン（ロール制御） ②エレベータ（ピッチ制御） ③スロットル （推力制御） ④ラダー（ヨー制御） 右スティックのボタン ARM マルチコプタの制御 85
Drone Control Workshop 送受信関連の機能の付与 これらのファイルに 送受信の機能が記述 されています 送受信を⾏うコードを記述するファイルの冒頭に以下を記述して ヘッダファイル（.hppファイル）を取り込む #include “rc.hpp” マルチコプタの制御 86
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の状態を交互に繰り返す） マルチコプタの制御 87
Drone Control Workshop ⾶⾏時のプログラム（Stick読み込み） マルチコプタの制御 88
Drone Control Workshop Stickの状態を表⽰する マルチコプタの制御 89
Drone Control Workshop Lesson2 Exercise スロットル、エルロン、エレベータ、ラダーの値 をシリアル通信で数値を⾒て確認し、スティクを 倒す⽅向と値の正負、⼤⼩を確認しメモしなさい マルチコプタの制御 90
Drone Control Workshop でんぐり返し ロール回転でひっくり返す Lesson3 マルチコプタの制御 91
Drone Control Workshop ロールの動きを起こす X ロール回転 左を強め Y Z 右を弱め マルチコプタの制御 92
Drone Control Workshop ロールの動きを起こす 左を強め、右を 同じだけ弱める 超重要︕ マルチコプタの制御 93
Drone Control Workshop Lesson3 Exercise スロットルスティックの倒し量に応じて上下、 ロール、ピッチ、ヨーの運動が起こる様にしなさ い（前転、後転、⽅向変換を確認する） （⼿動でドローンをうまくどばせますか︕︖） 超重要︕ マルチコプタの制御 94
Drone Control Workshop LED表⽰ Lesson 4 マルチコプタの制御 95
Drone Control Workshop フルカラーシリアルLEDについて 1本の信号線で繋がった全ての LEDを制御できる （C）SwitchScience StampFlyはWS2812（シリアル LEDの⼀つ）を使⽤ FastLEDライブラリが便利 綺麗で役⽴つ︕ https://github.com/FastLED/FastLED /wiki/Basic-usage マルチコプタの制御 96
Drone Control Workshop 今回⽤意したLED⽤の関数 onboard_led1(YELLOW, 1); onboard_led2(YELLOW, 1); ⾊の指定部分 点灯する︓１ 消灯する︓０ ⾊の指定について ⾊は１６進数で指定できる 0x00ffff マルチコプタの制御 97
Drone Control Workshop C⾔語の関数 .cppファイルの内部 関数の定義 型名 } 関数名（引数の型 引数変数）{ 処理したいコード 関数の宣⾔ 型名 関数名（引数の型 引数変数); ※新たに関数を作った場合は関数宣⾔を忘れずに 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; } 他のファイルからもつかえるように .hppファイルの内部 型名 関数名（引数の型 引数変数); マルチコプタの制御 98
Drone Control Workshop Lesson4 Exercise（オプション） parking_mode関数の中から呼ばれる、LEDをクリスマスイルミ ネーション⾵に次から次へと⾊が変わる関数を作成し、停⽌時に LEDの⾊がカラフルに変更される様にしなさい。 (ヒントモータを2秒回転2秒停⽌の応⽤) 関数名︓led_Illumination ※parking_mode関数は0.0025周期で動く必要があるためその中のプロ グラムは速やかに終わる必要があります この課題は⾶⾏に本質的に関係ないので後で余 裕がある時にやってください マルチコプタの制御 99
Drone Control Workshop ⾓速度・加速度の取得 Lesson 5 マルチコプタの制御 100
Drone Control Workshop IMU=加速度計+⾓速度計 • 加速度計 • 三軸の加速度を計測 重⼒加速度を検知 • ⾓速度計（ジャイロ） • 三軸周りの⾓速度を検知 マルチコプタの制御 101
Drone Control Workshop 加速度センサの原理 おもり 正の⽅向に加速すると この曲がりを検知し て加速度とする 加速度 正⽅向 マルチコプタの制御 102
Drone Control Workshop 重⼒加速度の検知 静⽌ Z⽅向 正の⽅向の動的加速度 重⼒（静的加速度）は正の⽅向に働いてもマイナスの値として検出される マルチコプタの制御 103
Drone Control Workshop ⾓速度の検知 振動⽅向 コリオイ⼒ 圧電素⼦ 振動素⼦ 圧電振動ジャイロ マルチコプタの制御 104
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; マルチコプタの制御 105
Drone Control Workshop StampFly構造体 typedef struct{ sensor_value_t sensor; flag_t flag; counter_t counter; pidstruct_t pid; controlref_t ref; times_t times; }stampfly_t; マルチコプタの制御 106
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; 構造体の定義はヘッダーファイルにかくとよい マルチコプタの制御 107
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 マルチコプタの制御 108
Drone Control Workshop Lesson5 Exercise • シリアルモニタを使って加速度三軸分と⾓速度三軸分をコンマで区切っ て表⽰しなさい • その際、⼀番最初の列に今の時刻を秒単位で合わせて表⽰しなさい • 各軸の⾓速度の正⽅向がドローンの回転の正⽅向と⼀致しているか確認 しなさい（⼀致していない場合は報告してください） • 以下はオプション余裕があればやる（あとの課題優先で） • X軸周りの回転速度がある程度以上ならLEDを⾚く光らせる • Y軸周りの回転速度がある程度以上ならLEDを緑に光らせる • Z軸周りの回転速度がある程度以上ならLEDを⻘く光らせる マルチコプタの制御 109
Drone Control Workshop Teleplotの紹介 • Teleplotと⾔うアドインをVSCodeに⼊れるとシリアル通信データでグ ラフを描けるそうなので調べて⼊れてジャイロや加速度計の出⼒を グラフ化して可視化してみてください マルチコプタの制御 110
Drone Control Workshop ⾓速度フィードバック Lesson 6 マルチコプタの制御 111
Drone Control Workshop フィードバック制御によるSAS ⽬標⾓速度 StampS3 ESP-NOW + - KP StampFly ロール ピッチ ヨー ⾓速度 コントローラ Transmitter ATOMS3 Gyro ESP-NOW マルチコプタの制御 112
Drone Control Workshop ブロック線図の読み⽅ ⿊丸 信号s ⼊⼒u G(s) 信号s 分岐 出⼒y 信号s 伝達要素 ブロック y=G(s)u r ⽩丸 + 加え合わせ e - y マルチコプタの制御 e = r-y 113
Drone Control Workshop ⽐例制御 ⽬標⾓速度 + 誤差 - 制御⼊⼒ KP ⽐例ゲイン G(s) ロール StampFly ⽐例コントローラ H(s) Gyro ⽬標⾓速度とロール⾓速度の誤差を定数倍して制御⼊⼒として加える マルチコプタの制御 114
Drone Control Workshop ⽐例制御（信号名⼊り） ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro マルチコプタの制御 115
Drone Control Workshop 各信号の算出 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) StampFly ⽐例コントローラ H(s) 𝐻 𝑠 𝑝 ロール 𝑝 𝑝 Gyro 𝑒 =𝑟−𝐻 𝑠 𝑝 𝑢 = 𝐾! 𝑒 𝑝 = 𝐺(𝑠)𝑢 誤差の算出 制御則（⽐例制御） 制御対象の⼊出⼒ マルチコプタの制御 116
Drone Control Workshop ドローンに実装するにはどうする︖ ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP G(s) ⽐例ゲイン ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro 機械として 存在する部分 マルチコプタの制御 117
Drone Control Workshop ドローンに実装するにはどうする︖ ⽬標⾓速度 𝑟 プログラムと して実装すべ き制御則 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP G(s) ⽐例ゲイン ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro マルチコプタの制御 118
Drone Control Workshop ドローンに実装するにはどうする︖ ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP G(s) ⽐例ゲイン ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro 𝑢 = 𝐾! 𝑒 制御⼊⼒を誤差の定数倍で作り出す マルチコプタの制御 119
Drone Control Workshop 制御則 𝑢 = 𝐾! 𝑒 制御⼊⼒を誤差の定数倍で作り出す 実際のプログラ ムではuは何か、 どうやってモー タの回転に反映 させるの︖ Kpはどう決 めるの 実際のプログラ ムでは誤差eは どうやって計算 するんだっけ マルチコプタの制御 120
Drone Control Workshop 制御⼊⼒の各モータへの配分 ミキシング マルチコプタの制御 121
Drone Control Workshop ミキシング ドローンの基本運動 • 上下運動 • ロール運動 • ピッチ運動 • ヨー運動 ４つモータの回転 マルチコプタの制御 122
Drone Control Workshop ミキシング ドローンの基本運動 • 上下運動 • ロール運動 • ピッチ運動 • ヨー運動 ４つモータの回転 2025/8/31 ヨーの符号が反対だったものをを修正 𝑓𝑙!"#$ = 𝛿#%&'##() + 𝛿&'(( + 𝛿*+#,% − 𝛿$-. 𝑓𝑟!"#$ = 𝛿#%&'##() − 𝛿&'(( + 𝛿*+#,% + 𝛿$-. 𝑟𝑙!"#$ = 𝛿#%&'##() + 𝛿&'(( − 𝛿*+#,% + 𝛿$-. 𝑟𝑟!"#$ = 𝛿#%&'##() − 𝛿&'(( − 𝛿*+#,% − 𝛿$-. マルチコプタの制御 123
Drone Control Workshop コントローラーからの⼊⼒ 運動 舵の名称 上下 スロットル THROTTLE 前︓＋ 後︓ー ロール エルロン AILERON 右︓＋ 左︓ー ピッチ エレベータ ELEVATOR 前︓ー 後︓＋ ヨー ラダー RUDDER 右︓＋ 左︓ー マルチコプタの制御 スティック極性 124
Drone Control Workshop ドローン制御の実装 ⼿動制御 ミキシングの式の 各デルタ スティックの倒し量 ⽐例制御 ミキシングの式の 各デルタ ⽐例制御則から 作り出した量 マルチコプタの制御 125
Drone Control Workshop StampFlyへの⽐例制御の実装 ⽐例制御 ⽬標⾓速度＝スティックの倒し量 誤差＝⽬標⾓速度ージャイロから得られた実際の⾓速度 制御⼊⼒＝⽐例ゲイン×誤差 マルチコプタの制御 126
Drone Control Workshop ⽐例制御実装のブロック線図表現 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP G(s) ロール 𝑝 StampFly ⽐例コントローラ H(s) Gyro ブロック線図は冗⻑な⾔葉での説明を図で表したもの マルチコプタの制御 127
Drone Control Workshop ４つの運動は独⽴に制御できる 2025/8/31 ヨーの符号が反対だったものをを修正 𝑓𝑙'()* = 𝛿)+,-))./ + 𝛿,-.. + 𝛿01)2+ − 𝛿*34 𝑓𝑟'()* = 𝛿)+,-))./ − 𝛿,-.. + 𝛿01)2+ + 𝛿*34 𝑟𝑙'()* = 𝛿)+,-))./ + 𝛿,-.. − 𝛿01)2+ + 𝛿*34 各デルタが 制御⼊⼒ 𝑟𝑟'()* = 𝛿)+,-))./ − 𝛿,-.. − 𝛿01)2+ − 𝛿*34 ミキシングの式によりあらゆるデルタの組み合わ せに対して１対１で各モータの値が求められる ４つの運動を別々に制御則にあてはめ計算された制御⼊⼒をを ミキシングの式で統合して各モータのDutyを計算すれば良い マルチコプタの制御 128
Drone Control Workshop 実装時の注意 • デューティは１を超えてはいけない • アーミングしたら、アーミング状態が⼀⽬でわかる様にLEDを⻩⾊に点灯する • モータはフル回転を⻑時間させると痛みやすいのでデューティは0.95を上限にする • Stickの中⽴は完全に０にならないので不感帯を±3％ほど持たせる • ⽐例ゲインは調整部分なので0.1程度から始めて、⼤⼩させて、感覚で良いところを⾒つける • Stickからの値を数倍して操縦の感度を上げるのも有効（特にヨーは５倍ぐらいしてちょうど良い） • 機体の持っている固有のアンバランスは今回の制御⽅法では消せないのでδRoll等に⼩さな数値を⾜ したり引いたりして調整すると良い(これをトリム調整と呼びます) マルチコプタの制御 129
Drone Control Workshop Lesson6 Exercise StampFlyの３軸のジャイロセンサから の⾓速度をフィードバックして⾓速度 を制御し⾶⾏することを実現してくだ さい マルチコプタの制御 130
Drone Control Workshop StampFlyの操縦 無線通信 ESP-NOW WiFi BlueTooth マルチコプタの制御 131
Drone Control Workshop 送信機のスティックの意味 左ボタン 割り当てなし モード３にす るために 左ボタンを押 しながら電源 スイッチを⼊ れてください L 右ボタン ⾶⾏モード変更（アクロ・スタビライズ） R ③ ② ① 左スティックのボタン 宙返り これは モード３の設定です ストップ ウォッチ スタート ストップ リセット ④ 一般的な機能 ①エルロン（ロール制御） ②エレベータ（ピッチ制御） ③スロットル （推力制御） ④ラダー（ヨー制御） 右スティックのボタン ARM マルチコプタの制御 132
Drone Control Workshop うまく⾶ばすコツ ① ③ StampFlyと⾃分の向きを合わせるように ヨーを常に微調整する 通常は前後左右のトリム（0点）は狂っている ので常に舵を前後，左右に傾けて動かないと ころを探し⼩刻みに微調整する ② ④ 思い切って⾼度を上げて⼩刻みにスロットル を調整し、⼀定の⾼さを保つ練習を繰り返す ホバリング（空中停⽌）ができるまで繰り 返し練習する。 よくなったら、移動してみる マルチコプタの制御 133
Drone Control Workshop ⾶⾏時の注意 • 周囲の安全を確認（後ろも注意） • 落ちたら即アーミング解除 • 落ちた後の機体チェック • プロペラが異物を巻き込んでいないか • 上から覗かない • 以上な挙動が⾒られたら即停⽌ マルチコプタの制御 134
Drone Control Workshop ご⾃分の⾶⾏を評価しましょう • ⾶⾏評価A︓安定化ができているか • ⾶⾏評価B︓１０秒ホバリング試験できるか マルチコプタの制御 135
Drone Control Workshop ⾶⾏評価A︓安定化ができているか 観点 設定根拠 A1 ロール/ピッチ/ヨー⾓速度の安定 離陸時に機体が転倒や異常回転せず浮き上がれば安定性が フィードバック制御により確保されていると判断できる. A2 微⼩振動の減衰 各ゲインを調整して微⼩振動が⾒られないようになれば良好に 減衰していると判断できる A3 速応性の確保 操縦（⽬標値）に対する速応性が確保されていないと操縦が極 めて難しくなり速応性の確保は重要 A4 定常偏差の軽減 ⾓速度制御で定常偏差が残りスティックを戻しても回転し続け る場合は定常偏差を0にできれば極めて良好な操縦特性が得られ るため重要 マルチコプタの制御 136
Drone Control Workshop ⾶⾏評価B︓10秒ホバリング試験の達成 観点 設定根拠 B1 ⼿順遵守 安全と⼲渉回避の前提を担保することが実験を成功させ⼀定な実験結果を導 くために重要である B2 ⾶⾏試験区域内の維持 安全な実験を達成するために極めて重要である B3 安定保持10 秒 環境と操縦者の適応し良好なホバリング特性が得られていることを判断する のに⼗分な時間である10 秒間の安定ホバリングできていれば操縦者のホバ リング技量と⾓速度SASが達成したことが証明でき重要である B4 安全動作 実験は不成功に終わる場合が多く, 安全区域内の逸脱などが起こる. 安全⾏ 動が取れたかは実験を継続するために極めて重要である マルチコプタの制御 137
Drone Control Workshop ⾶⾏評価A/Bチェックリスト 評価項⽬ 観点 判定基準 A1 ロール/ピッチ/ヨー⾓速度の安定 離陸しても転倒や異常回転がなく, スティックを離すと回転は停⽌傾 向にある A2 微⼩振動の減衰 ホバリング中に⾼頻度の⼩振幅振動が持続しない. ⼩振幅で⼀定に保 たれる程度であれば可（ブルブルしていない） A3 速応性の確保 ⼩さなステップ⼊⼒または往復操作に対し, ⽬視で遅れが⼤きすぎず, 操縦による補正が可能 A4 定常偏差の軽減 スティックセンターで残留回転が続かない. トリム/オフセット調整に より⽬視上の偏りが⼩さい B1 ⼿順遵守 ⾶⾏前確認とチャンネル番号の宣⾔を実施 B2 安全範囲内の維持 離陸から着陸まで安全範囲から逸脱しない B3 安定保持10 秒 操縦者の意図した定点から⽬視上のズレが⼩さい状態を10 秒継続 B4 安全動作 ⾼頻度の異常振動や危険動作がなく即時停⽌基準の発動を要しない Yes/No https://docs.google.com/forms/d/e/1FAIpQLSeAOPuAvkXuxt7TrhdqPXDdSiClB66mOQPEVor40BqTOJz7Q/viewform?usp=dialog マルチコプタの制御 138
Drone Control Workshop Lessonまとめ • Lesson0 開発環境インストール • Lesson1 モータの2秒回転2秒停⽌ • Lesson2 Stickの値を取得しシリアルモニタに表⽰ • Lesson3 でんぐり返し • Lesson4 LED点滅（省略可） • Lesson5 シリアルモニタにセンサの値を表⽰ • Lesson6 ⾓速度フィードバック実装＆フライト試験 マルチコプタの制御 139
Drone Control Workshop 1⽇⽬はこれにて終了 マルチコプタの制御 140
Drone Control Workshop 制御⼯学における応答の観察 マルチコプタの制御 141
Drone Control Workshop 過渡応答 オーバシュート （最⼤⾏き過ぎ量） 定常偏差 真値 𝑇, ⽴ち上がり時間 10％から90％になる時間 ステップ応答 𝑇5 整定時間 ±X％に収束するまでの時間 142
Drone Control Workshop 周波数応答 出力 入力 過渡応答 定常応答 143
Drone Control Workshop 周波数応答 位相差 𝐴' 𝐴+ !! !!" :ゲイン 144
Drone Control Workshop 周波数応答の図⽰︓ボード線図 縦軸は ゲイン （ｄＢ） 縦軸は 位相差 （deg） ゲイン線図 ゲイン曲線 位相線図 位相曲線 横軸は⾓周波数 （rad/s） 145
Drone Control Workshop フィードバック制御 マルチコプタの制御 146
Drone Control Workshop ⾞両のライン追随 進⾏⽅向にたいして 右が正 左が負 負 正 進⾏⽅向 マルチコプタの制御 147
Drone Control Workshop ⾞両のライン追随 進⾏⽅向にたいして 右が正 左が負 進⾏⽅向 タイヤのきれ⾓は ⾞体を正の⽅向に 移動させる⾓度が 正の⽅向 マルチコプタの制御 負 正 148
Drone Control Workshop ⾞両のライン追随 進⾏⽅向 ライン中⼼からのズレ＝誤差 誤差 ＝ ０ ー 左右の位置 この場合負の誤差 マルチコプタの制御 149
Drone Control Workshop ⾞両のライン追随 進⾏⽅向 ライン中⼼からのズレ＝誤差 タイヤのきれ⾓ ＝ ⽐例ゲイン✖誤差 この場合負のきれ⾓になりライン に戻ろうとする マルチコプタの制御 150
Drone Control Workshop カーブをする場合 タイヤのきれ⾓＝あらかじめカーブするためのきれ⾓＋微妙なずれに対する修正量 数式で表すと 𝑢 = 𝑢" + ∆𝑢 制御⼯学では制御量（制御⼊⼒）を𝑢で書くことが多い マルチコプタの制御 151
Drone Control Workshop ドローンのホバリング 𝑢"#$$ = 𝑢% + ∆𝑢"#$$ ホバリングするための制御⼊⼒𝑢! ⽬標のロールレートにする制御⼊⼒∆𝑢"#$$ ∆𝑢$%&& の部分を⽐例制御やPID制御でつくる マルチコプタの制御 152
Drone Control Workshop フィードバック制御 ⽬標⾓速度 𝑟 + 誤差 𝑒 - C(s) 制御⼊⼒ 𝑢 G(s) ロール 𝑝 出⼒ StampFly コントローラ H(s) Gyro 検出器 教科書的なフィードバック制御では𝑢6は暗黙の存在でここでは描かれない しかし、実装では𝑢6が必要 マルチコプタの制御 153
Drone Control Workshop ⽐例制御 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) ロール 𝑝 StampFly ⽐例コントローラ 1 Gyro マルチコプタの制御 154
Drone Control Workshop 各信号の算出 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) StampFly ⽐例コントローラ 1 𝐻 𝑠 𝑝 ロール 𝑝 𝑝 Gyro 𝑒 =𝑟−𝑝 誤差の算出 𝑢 = 𝐾! 𝑒 𝑝 = 𝐺(𝑠)𝑢 制御則（⽐例制御） 制御対象の⼊出⼒ マルチコプタの制御 155
Drone Control Workshop 閉ループの⼊出⼒関係 ⽬標⾓速度 𝑟 + 誤差 𝑒 - 制御⼊⼒ 𝑢 KP ⽐例ゲイン G(s) StampFly ⽐例コントローラ 𝐻 𝑠 𝑝 ロール 𝑝 1 𝑝 Gyro 𝐺(𝑠)𝐾0 𝑝= 𝑟 1 + 𝐺 𝑠 𝐾0 マルチコプタの制御 156
Drone Control Workshop 制御の効果 制御なしの⼊出⼒関係 𝑝 = 𝐺(𝑠)𝑢 制御ありの⼊出⼒関係 𝐺(𝑠)𝐾* 𝑝= 𝑟 1 + 𝐺 𝑠 𝐾* 𝐾! は設計者が変えられるため特性を改善できる可能性がある マルチコプタの制御 157
Drone Control Workshop 例題︓単純な不安定システムの改善例 制御対象の伝達関数 1 𝐺(𝑠) = 𝑠−1 伝達関数の分⺟＝０にしたときSが プラスの場合不安定なシステム センサの伝達関数 𝐻(𝑠) = 1 ⽬標rから出⼒pまでの伝達関数はどうなりますか︖ マルチコプタの制御 158
Drone Control Workshop ⽐例制御による改善 例題の場合の ⽬標値応答 𝐾* 𝑝= 𝑟 𝑠 + 𝐾* − 1 𝐾0 が１を超えると分⺟＝０の解がマイナス になりシステム全体は安定となる システム全体の特性が改善された マルチコプタの制御 159
Drone Control Workshop PID制御 Lesson 7 マルチコプタの制御 160
Drone Control Workshop PID制御とは • P（⽐例）制御︓誤差に⽐例した制御⼊⼒ • I（積分）制御︓誤差を積分して制御⼊⼒ • D（微分）制御︓誤差を微分して制御⼊⼒ マルチコプタの制御 161
Drone Control Workshop PID制御の効果 Iゲインで定常 偏差改善、外 乱応答も改善 過ぎると遅れ が⼤きくなり 振動的に Pゲインで ⽴ち上が りが早く なる 過ぎると、 振動的に なる 元の応答 Dゲインで、振動を抑 制する 過ぎると、ノイズ等 を増⼤し振動的に マルチコプタの制御 162
Drone Control Workshop PID制御について 𝐶 𝑠 = 𝐾! (1 + 𝐾0 :⽐例ゲイン 𝑇1 :積分時間 𝑇' :微分時間 1 + 𝑇# ) 𝑇" 𝑠 Stamp Flyの概要とモデリング
Drone Control Workshop 不完全微分つきPID制御 𝐾! :⽐例ゲイン 𝑇" :積分時間 𝑇# :微分時間 𝜂:微分係数（0.125） 𝑇! 𝑠 𝜂𝑇! 𝑠 + 1 Stamp Flyの概要とモデリング
Drone Control Workshop PID制御の基本的な実装 誤差 𝑦!"# 𝑒/ = 𝑦&)3 / − 𝑦/ $ ⽬標値 𝑦$ センサ出⼒ 数値積分（積分器） 𝑠/ = 𝑠/01 + 𝑒/ 𝑇2 数値微分 𝑑𝑒/ = (𝑒/ − 𝑒/01 )⁄𝑇2 PID制御の操作量 𝑢/ = 𝐾* (𝑒/ + 𝑠/ ⁄𝑇+ + 𝑇! 𝑑𝑒/ ) マルチコプタの制御 𝐾0 :⽐例ゲイン 𝑇1 :積分時間 𝑇' :微分時間 𝜂:微分係数（0.125） 𝑇5 :制御周期 165
Drone Control Workshop PID制御器の実⽤上の実装 𝐾0 :⽐例ゲイン 𝑇1 :積分時間 𝑇' :微分時間 𝜂:微分係数（0.125） 𝑇5 :制御周期 積分（台形積分） 𝑡2 𝑠/ = 𝑠/01 + (𝑒/ + 𝑒/01 ) 2𝑇+ 微分（不完全微分・フィルタつき微分） 2𝜂𝑇! − 𝑡2 2𝑇! 𝑑/ = 𝑑/01 + (𝑒/ − 𝑒/01 ) 2𝜂𝑇! + 𝑡2 2𝜂𝑇! + 𝑡2 PID制御の操作量 $ 𝑢/ = 𝐾* (𝑒/ + 𝑠/ + 𝑑/ ) マルチコプタの制御 (rad/s)までは微分器として働き %" $ (rad/s)からは微分の効果はほぼない &%" 166
Drone Control Workshop 補⾜︓PID補償器の双1次変換による離散化 不完全微分付きのPID補償器 1 𝑇# 𝑠 𝑐 𝑠 = 𝐾! 1 + + 𝑇" 𝑠 𝜂𝑇# 𝑠 + 1 双1次変換で離散化 双1次変換とは次の変換です 𝑠= 2(𝑧 − 1) 𝑡' (𝑧 + 1) PID補償器を⼀変に離散化するのは ⾻が折れるので積分と微分の部分 にわけてやっていきます。 積分補償器の離散化 𝑡' (𝑧 + 1) 𝑦( = 𝑒 2𝑇" (𝑧 − 1) ( 2𝑇" (𝑧 − 1)𝑦( = 𝑡' (𝑧 + 1)𝑒( 2𝑇" (𝑦()$ − 𝑦( ) = 𝑡' (𝑒()$ + 𝑒( ) 𝑦()$ = 𝑦( + 𝑡' (𝑒 + 𝑒( ) 2𝑇" ()$ ※これは台形積分と同じです 不完全微分補償器の離散化 2(𝑧 − 1) 𝑡' (𝑧 + 1) 𝑦( = 𝑒( 2(𝑧 − 1) 𝜂𝑇# +1 𝑡' (𝑧 + 1) 2𝑇# (𝑧 − 1) 𝑦( = 𝑒 2𝜂𝑇# (𝑧 − 1) + 𝑡' (𝑧 + 1) ( 2𝑇# (𝑧 − 1) 𝑦( = 𝑒 2𝜂𝑇# + 𝑡' 𝑧 − (2𝜂𝑇# − 𝑡' ) ( 𝑇# ⻘︓微分器 緑︓ローパス 1 1 𝑇# 𝜂𝑇# 不完全微分の周波数特性（オレンジ） 2𝜂𝑇# + 𝑡' 𝑧 − (2𝜂𝑇# − 𝑡' ) 𝑦( = 2𝑇# (𝑧 − 1)𝑒( 2𝜂𝑇# + 𝑡' 𝑦()$ − (2𝜂𝑇# − 𝑡' )𝑦( = 2𝑇# (𝑒()$ − 𝑒( ) 2𝜂𝑇# − 𝑡' 2𝑇# 𝑦()$ = 𝑦 + (𝑒 − 𝑒( ) 2𝜂𝑇# + 𝑡' ( 2𝜂𝑇# + 𝑡' ()$ //積分 integral = integral + m_h * (err + m_err)/2/m_ti; //不完全微分 differential = (2*m_eta*m_td – m_h)*differential/(2*m_eta*m_td + m_h) + 2*m_td*(err – m_err)/(2*m_eta*m_td + m_h)
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) 誤差、制御周期 リセット（ロールの例）︓積分器・フィルタリセット Parkingモードでリセット StampFly.pid.roll.reset() マルチコプタの制御 168
Drone Control Workshop 補⾜︓StampFly構造体 StampFly.hppの中で以下の構造体が定義されています。そして stampfly_t StampFlyとして宣⾔されて使えるようになっています。 リアルタイムのセン サ値が⾃動更新され て⼊っています （skeltonでは⾼度セ ンサは読み取ってい ません） 経過時間 ⼀つ前の 時間経過時間 周期 初期化が終 わった時刻 マルチコプタの制御 169
Drone Control Workshop Lesson7 Exercise Lesson6で作った⽐例制御のプログラ ムをPIDクラスを使ったものに改造し なさい。積分と微分は働かせなくとも 良い。違う時間で調整時間を設けます 積分時間Ti︓1000000 積分制御と微分制御が働かない設定 微分時間Td︓0 マルチコプタの制御 170
Drone Control Workshop Telemetry Lesson 8 マルチコプタの制御 171
Drone Control Workshop Joystickをケーブルで接続して通電 • Joystickの電池を外す（念の為） • PCとJopyStickをUSBケーブルで接続 • （⽩いマイコンのUSB-Cポートに接続） • その際にMode3にしたい場合はケーブルを指す前にLボタンを押 しておく • シリアルモニタボタンを押す • StampFlyに電源を接続 • シリアルモニタにテレメトリ情報が表⽰される • CTRLキーを押しながらCを押すとシリアルモニタは終了 マルチコプタの制御 172
Drone Control Workshop シリアルモニタでTelemetry シリアルモニタにこのように数値が並びます ヨー⾓（deg） ヨー⾓速度（deg/s） ピッチ⾓（deg） ピッチ⾓速度（deg/s） ロール⾓（deg） ロール⾓速度（deg/s） 周期（s） 時間（s） 以後のデータ数 Z軸加速度（G） ヨー⽬標値 Y軸加速度（G） ピッチ⽬標値 X軸加速度（G） マルチコプタの制御 ロール⽬標値 173
Drone Control Workshop Telemetryをファイルに残す 👇 このアイコンを押してターミナルを開く マルチコプタの制御 174
Drone Control Workshop Windowsの場合 開いたターミナルがcmd.exeの場合 pio device monitor > log.txt 画⾯には何も出ない、終わる時はCTRL+C これでlog.txtというファイルにログが保存される。 開いたターミナルがPowershellの場合 pio device monitor | Tee-Object -FilePath log.txt 画⾯にも表⽰される。終わる時はCTRL+C これでlog.txtというファイルにログが保存される。 マルチコプタの制御 175
Drone Control Workshop MacまたはLinuxの場合 ⽅法１ pio device monitor | tee log.txt 画⾯にも表⽰される、終わる時はCTRL+C これでlog.txtというファイルにログが保存される。 ⽅法２ pio device monitor > log.txt 画⾯には何も出ない、終わる時はCTRL+C これでlog.txtというファイルにログが保存される。 マルチコプタの制御 176
Drone Control Workshop データの可視化 データをエクセル等で開いて確認する オレンジ︓ヨー⽬標値 ⻘︓ヨー⾓速度 マルチコプタの制御 177
Drone Control Workshop gnuplot グラフ作成のおすすめ • • • • • グラフ作成ソフトの古典であり最⾼峰 軽い ⼤データも取り扱える どんなOSでも使える http://www.gnuplot.info/ 保存したTelemetryデータからグラフを作成 plot “log.txt” u 2:7 w l, “” u 2:($13*180/3.14) w l マルチコプタの制御 178
Drone Control Workshop StampFlyの 伝達関数と制御⼊⼒ Lesson 8 マルチコプタの制御 179
Drone Control Workshop StampFlyの⾓速度系の伝達関数 ロール⾓速度 ∆𝑝 = %1'! (" )# *& +(-# +./) ∆𝑒3 𝜏= 𝐽𝑅 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( 𝐾) = ピッチ⾓速度 ヨー⾓速度 ∆𝑞 = ∆𝑟 = %1'! (" )# *% +(-# +./) %&'! (" )# *$ +(-# +./) 𝐾 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( ∆𝑒2 ∆𝑒! Stamp Flyの概要とモデリング それぞれの記号の意味は付録を 参照してください
Drone Control Workshop StampFlyの⾓速度系の伝達関数 ロール⾓速度 ∆𝑝 = %1'! (" )# /*& +(-# +./) ∆𝑒3 𝜏= 𝐽𝑅 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( 𝐾) = ピッチ⾓速度 ヨー⾓速度 ∆𝑞 = ∆𝑟 = %1'! (" )# /*% +(-# +./) %&'! (" )# /*$ +(-# +./) 𝐾 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( ∆𝑒2 ∆𝑒! Stamp Flyの概要とモデリング それぞれの記号の意味は付録を 参照してください
Drone Control Workshop StampFlyの⾓速度系の伝達関数 ロール⾓速度 ピッチ⾓速度 ∆𝑝 = ∆𝑞 = )) +(-# +./) )( +(-# +./) ∆𝑒3 𝜏= 𝐽𝑅 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( 𝐾) = ∆𝑒2 𝐾 𝐷𝑅 + 𝐾 & + 2𝑅𝐶' 𝜔( 𝐾* = 2𝑙𝐶+ 𝜔(𝐾) /𝐼, 𝐾- = 2𝑙𝐶+ 𝜔(𝐾) /𝐼. 𝐾/ = 2𝜅𝐶+ 𝜔(𝐾) /𝐼0 ヨー⾓速度 ∆𝑟 = )' +(-# +./) ∆𝑒! Stamp Flyの概要とモデリング それぞれの記号の意味は付録を 参照してください
Drone Control Workshop 制御⼊⼒とモータ⼊⼒電圧 制御⼊⼒とモータの⼊⼒電圧との関係を表す ∆𝑒7 = ∆𝑒/ + ∆𝑒% + ∆𝑒5 + ∆𝑒6 ∆𝑒3 = −∆𝑒/ − ∆𝑒% + ∆𝑒5 + ∆𝑒6 ∆𝑒2 = ∆𝑒/ − ∆𝑒% − ∆𝑒5 + ∆𝑒6 ∆𝑒! = ∆𝑒/ − ∆𝑒% + ∆𝑒5 − ∆𝑒6 Stamp Flyの概要とモデリング 前 時計回り CW 反時計回り CCW 4 1 反時計回り CCW 3 2 時計回り CW
Drone Control Workshop ミキシング 制御⼊⼒を各モータの⼊⼒電圧に振り分ける ∆𝑒) 1 1 1 1 ∆𝑒0 −1 −1 1 1 = ∆𝑒F 1 −1 −1 1 1 −1 1 −1 ∆𝑒, ∆𝑒G ∆𝑒H ∆𝑒I ∆𝑒J ∆𝑒G 0.25 −0.25 0.25 0.25 ∆𝑒H 0.25 −0.25 −0.25 −0.25 = ∆𝑒I 0.25 0.25 −0.25 0.25 ∆𝑒J 0.25 0.25 0.25 −0.25 Stamp Flyの概要とモデリング ∆𝑒K ∆𝑒L ∆𝑒M ∆𝑒M
Drone Control Workshop 実装上の制御⼊⼒計算式(ミキシング) ※並びに注意skeltonと違います 𝑒89 = 𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 + 0.25∆𝑒2 + 0.25∆𝑒! 𝑒99 = 𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 − 0.25∆𝑒2 − 0.25∆𝑒! 𝑒9; = 𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 − 0.25∆𝑒2 + 0.25∆𝑒! 𝑒8; = 𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 + 0.25∆𝑒2 − 0.25∆𝑒! ホバリン グに必要 な電圧 ホバリン グ微調整 電圧こん かいは０ ロール制 御電圧 マルチコプタの制御 ピッチ制 御電圧 ピッチ制 御電圧 185
Drone Control Workshop
PWM制御のDUTY計算式
Duty89 = (𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 + 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty99 = (𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 − 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
Duty9; = (𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 − 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty8; = (𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 + 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
電池電圧
の公称値
マルチコプタの制御
186
Drone Control Workshop
PWM制御のDUTY計算式
Duty89 = (𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 + 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty99 = (𝑒: + 0.25∆𝑒7 − 0.25∆𝑒3 − 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
Duty9; = (𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 − 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty8; = (𝑒: + 0.25∆𝑒7 + 0.25∆𝑒3 + 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
マルチコプタの制御
187
Drone Control Workshop
PWM制御のDUTY計算式
Duty89 = (𝑒: − 0.25∆𝑒3 + 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty99 = (𝑒: − 0.25∆𝑒3 − 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
Duty9; = (𝑒: + 0.25∆𝑒3 − 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty8; = (𝑒: + 0.25∆𝑒3 + 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
マルチコプタの制御
188
Drone Control Workshop
PWM制御のDUTY計算式
Duty89 = 𝑒: /𝑒<=77"!> + (−0.25∆𝑒3 + 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty99 = 𝑒: /𝑒<=77"!> + (−0.25∆𝑒3 − 0.25∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
Duty9; = 𝑒: /𝑒<=77"!> + (0.25∆𝑒3 − 0.25∆𝑒2 + 0.25∆𝑒! )/𝑒<=77"!>
Duty8; = 𝑒: /𝑒<=77"!> + (0.25∆𝑒3 +0.25 ∆𝑒2 − 0.25∆𝑒! )/𝑒<=77"!>
すでに正規化されて
いるスロットルSrck
の値で置き換える
0.25で括り出す
マルチコプタの制御
189
Drone Control Workshop PWM制御のDUTY計算式 Duty89 = 𝑆𝑡𝑖𝑐𝑘 ? + 0.25(−∆𝑒3 + ∆𝑒2 + ∆𝑒! )/𝑒<=77"!> Duty99 = 𝑆𝑡𝑖𝑐𝑘 ? + 0.25(−∆𝑒3 − ∆𝑒2 − ∆𝑒! )/𝑒<=77"!> Duty9; = 𝑆𝑡𝑖𝑐𝑘 ? + 0.25(∆𝑒3 − ∆𝑒2 + ∆𝑒! )/𝑒<=77"!> Duty8; = 𝑆𝑡𝑖𝑐𝑘 ? + 0.25(∆𝑒3 + ∆𝑒2 − ∆𝑒! )/𝑒<=77"!> マルチコプタの制御 190
Drone Control Workshop Lesson8 Exercise PW制御のDUTY計算式に基づいてLEsson7で作ったPID制御のミキシングの式を⾒直して修正 しなさい。 マルチコプタの制御 191
Drone Control Workshop ループ成形 Lesson9 マルチコプタの制御 192
Drone Control Workshop 数値例（StampFlyの伝達関数） ロール⾓速度 ピッチ⾓速度 ヨー⾓速度 ∆𝑝 = ∆𝑞 = ∆𝑟 = %/@.6 +(:.://B5+./) /D/./ +(:.://B5+./) %@.66 +(:.://B5+./) 𝛿; 𝛿E 𝛿C マルチコプタの制御 193
Drone Control Workshop StampFlyの⾓速度系のボード線図 ロールの 位相余裕︓34.5 deg ゲイン交差周波数︓124 rad/s (20Hz) ゲイン曲線 位相曲線 ピッチの 位相余裕︓40.8 deg ゲイン交差周波数︓98.8 rad/s (16Hz) ヨーの 位相余裕︓71.8 deg ゲイン交差周波数︓28.0 rad/s (4.5Hz) ヨーの 位相余裕 ピッチの 位相余裕 ゲイン交差周波数 マルチコプタの制御 194
Drone Control Workshop 開ループ伝達関数とループ成形 目標 − 𝛿# 𝐾!" 𝐾! 𝑠(𝜏! 𝑠 + 1) 比例制御器 𝑞 機体 𝛿# 比例制御器 𝐾! 𝑠(𝜏𝑠 + 1) ０dB低周波のゲインを上げる 𝑞 機体 -180deg 𝐾*! 𝐾* 𝑠(𝜏* 𝑠 + 1) 位相余裕を大きく 安定度増大 望ましいボード線図の形 これが一巡伝達関数（開ループ伝達関数） 𝐿 𝑠 = gc -20dB/dcより傾ける 定常偏差が減少 ゲイン交差周波数gcを大きくする 速応性向上（バンド幅大になる） フィードバックを切る 𝐾!" gc付近の傾きを緩く -20dB/dc 安定性向上 安定性︓位相余裕を⼤きくする 速応性︓ゲイン交差周波数を⼤きくする 定常性︓低周波のゲインを上げる ロバスト性・対ノイズ性︓⾼周波のゲインを下げる Stamp Flyの概要とモデリング
Drone Control Workshop ボード線図で設計するとは 𝑒𝑟𝑟% 𝑃𝐼𝐷(𝑠) PID制御器 ∆𝑒& 𝐾% 𝑠(𝜏𝑠 + 1) 𝑝 機体 + PID制御器のボード線図 機体のボード線図 開ループのボード線図 伝達関数の直列接続はボード線図の⾜し合わせになるー＞ビュジュアル的に設計を進められる マルチコプタの制御 196
Drone Control Workshop ⽐例（P）制御 開ループのゲイン曲線 ⽐例制御器のゲイン曲線 制御対象のゲイン曲線 ⽐例制御は⽐例ゲインKpだとすると 20 log$+ 𝐾! 分、ゲイン曲線全体を上げる （下げる） ⽐例曲線では位相曲線は変えられない この例のように１以上のゲインをかける とゲイン曲線全体が上に上がりゲイン交 差周波数は右にずれ帯域が広がり、速応 性が向上するが、位相余裕が⼩さくなり 安定性やロバスト性は下がる マルチコプタの制御 197
Drone Control Workshop （不完全）微分（D）制御 微分制御器のゲイン曲線 Td（微分時間）とηを⽤いて微分が聞く 範囲や微分が効かなくなる範囲を指定 できる 微分は位相が９０度進むが不完全微分 は帯域で位相が変化する 微分が効いている周波数帯域 微分が効かない周波数帯域 微分制御器の位相曲線 低周波では微分効果が働き、ノイズが 問題になる⾼周波では微分が効かず、 微分制御器があるために制御性能が下 がることを防⽌する 微分制御により急激な出⼒の変化は抑 制されオーバーシュートが⼩さくなり、 振動が減少する マルチコプタの制御 198
Drone Control Workshop 積分（I）制御 積分は位相が９０度遅れる 積分器はゲインが-20dB/decで下がり続 ける 低周波ではゲイン曲線の傾きが⼤きく なり、定常偏差が⼩さくなり追随性が あがる マルチコプタの制御 199
Drone Control Workshop PID制御 DとIで制御器の形を整えてPで全 体を上下させて調整するイメージ マルチコプタの制御 200
Drone Control Workshop Lesson9 Exercise ループ成形の考え⽅にもとづいて⽐例制御の⽐例ゲインを求めて実装して⾶ばしなさい。 その際、位相余裕が４０度程度になるように調整しなさい。 余裕があれば、積分や微分もいれてゲインや積分時間、微分時間の変化で、開ループ伝達関数 のボード線図の形や、実機の⾶⾏応答がどのようになるか⽐較しなさい。 マルチコプタの制御 201
Drone Control Workshop 設計ツール Python Control マルチコプタの制御 202
Drone Control Workshop 伝達関数オブジェクト作成 作りたい伝達関数 110 𝑠(0.01𝑠 + 1) import control as ctrl sys=ctrl.tf( [110], [0.01, 1 , 0] ) 分⼦多項式の 係数リスト 110 0.01𝑠 % + 𝑠 分⺟多項式の 係数リスト print(sys) マルチコプタの制御 203
Drone Control Workshop システムの安定余裕を求める import control as ctrl sys=ctrl.Z( [110], [0.01, 1 , 0] ) gm, pm, gcw, pcw = ctrl.margin(sys) gm:ゲイン余裕 pm:位相余裕 gcw:ゲイン交差周波数 pcm︓位相交差周波数 マルチコプタの制御 204
Drone Control Workshop ボード線図を描く import control as ctrl sys=ctrl.tf( [110], [0.01, 1 , 0] ) omega = np.logspace(-1, 3, 1000) ctrl.bode(sys, omega=omega) マルチコプタの制御 205
Drone Control Workshop ステップ応答を⾒る import control as ctrl import matplotlib.pyplot t, y = ctrl.step_response(sys) plt.figure() plt.plot(t, y) plt.title("Step Response") plt.xlabel("Time [s]") plt.ylabel("Amplitude") plt.grid() plt.show() マルチコプタの制御 206
Drone Control Workshop システムの接続 SYS1 SYS2 newsys= sys1*sys2 SYS1 + SYS2 + newsys= sys1+sys2 マルチコプタの制御 207
Drone Control Workshop colabで体験 以下のGitHubを開いてください https://github.com/M5Fly-kanazawa/Controller_desigen_exercise 👈 このアイコンをクリックするとcolabが開きます colabの変更はGoogleドライブに保存できます マルチコプタの制御 208
Drone Control Workshop Pythonで制御を学びたい⽅ Pythonによる制御⼯学⼊⾨ 南裕樹先⽣の御本 制御を広く知りたい⼊⾨者におすすめ 制御解析・設計にPythonを利⽤しているので設計ツール にかかる費⽤が０円です アマゾンの説明 Pythonを使って制御⼯学を学ぶための⼊⾨書 本書は，各所でわかりやすく，かつ，実⽤的と好評をいただいているPythonを 使って制御⼯学を学ぶための⼊⾨書の改訂版です． 「使ってみる，やってみる」を通して，制御⼯学を体感することができます． よりわかりやすさを追求して構成を⾒直し・追記したほか，ライブラリの最新 版に合わせてPythonコードを修正しています． これから制御⼯学に携わる⽅々にとって必読の書です． https://amzn.to/46eG5FR 伊藤のアフェリエイトに⾶ぶのでご注意ください マルチコプタの制御 209
Drone Control Workshop フォローお願いします 私OOOです。 とカミングアウトしていただけるとフォロー いたします。DMもできます。 マルチコプタの制御 210
Drone Control Workshop 以上で講義は終了です。 モデルの導出など付録につけました。 ご興味があればご覧ください。 最後にこのQRコードより⾶⾏評価チェックリス トの提出お願いします。（スマホでできます） マルチコプタの制御 211
Drone Control Workshop おしまい 今は、これが精⼀杯 マルチコプタの制御 212
Drone Control Workshop 付録 マルチコプタの制御 213
Drone Control Workshop 機体に働く推⼒とモーメント Stamp Flyの概要とモデリング
Drone Control Workshop ロータの配置 4 𝑦6 1 𝑦/ 𝑥6 𝑥/ 𝑥5 𝑥% 𝑦5 𝑦% 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 モータプロペラ系モデル モータプロペラの⾮線形モデル @ 𝐾 𝐽𝜔̇ + 𝐷 + 𝑅 𝐾 @ 𝜔 + 𝐶? 𝜔 + 𝑞A = 𝑒 𝑅 モータプロペラ定常回転時 𝐾% 𝐾 % 𝐷+ 𝜔 + 𝐶F 𝜔 + 𝑞# = 𝑒 𝑅 𝑅 Stamp Flyの概要とモデリング
Drone Control Workshop ⼊⼒電圧とプロペラ⾓速度（定常状態） 𝐶F 𝜔% + % 𝜔= 𝐾 − 𝐷+ 𝑅 𝐾% 𝐾 𝐷+ 𝜔 + 𝑞# − 𝑒 = 0 𝑅 𝑅 + 𝐷+ % % 𝐾 𝑅 − 4𝐶F 2𝐶F Stamp Flyの概要とモデリング 𝐾 𝑞# − 𝑒 𝑅
Drone Control Workshop 電圧とプロペラ⾓速度 𝐶4 𝜔5 + 𝐾5 𝐾 𝐷+ 𝜔 + 𝑞6 − 𝑒 = 0 𝑅 𝑅 𝐶4 2𝜔𝜔̇ + 𝐾5 𝐾 𝐷+ 𝜔̇ − = 0 𝑅 𝑅 𝑒で微分 𝐾 𝜔̇ = 2𝐶4 𝑅𝜔 + 𝐷𝑅 + 𝐾 5 Stamp Flyの概要とモデリング
Drone Control Workshop 推⼒と電圧 𝑇 = 𝐶? 𝜔% 𝜔= 𝑇 𝐶? 𝑅 𝑇 𝑒= 𝐶F + 𝐾 𝐶? 𝐾% 𝐷+ 𝑅 Stamp Flyの概要とモデリング 𝑇 + 𝑞# 𝐶?
Drone Control Workshop モータ・プロペラ定常特性の推定 モータプロペラ定常回転時 𝐾% 𝐾 % 𝐷+ 𝜔 + 𝐶F 𝜔 + 𝑞# = 𝑒 𝑅 𝑅 𝐾% 𝐾 𝐾 % 𝐾 𝐷+ K 𝜔 + 𝐶F L 𝜔 + 𝑞# L = 𝑒 𝑅 𝑅 𝑅 𝑅 𝐴𝜔% + 𝐵𝜔 + 𝐶 = 𝑒 実験により𝑒と𝜔の対応を取得し最⼩⼆ 乗法によりパラメータ𝐴, 𝐵, 𝐶を求める Stamp Flyの概要とモデリング
Drone Control Workshop パラメータ推定のための最⼩⼆乗法 𝐴𝜔% + 𝐵𝜔 + 𝐶 = 𝑒 より / 𝐽 = ∑ 𝐴𝜔% + 𝐵𝜔 + 𝐶 − 𝑒 % % 正規⽅程式 GH GI GH GJ GH G' = ∑ 𝐴𝜔% + 𝐵𝜔 + 𝐶 − 𝑒 𝜔% = 0 = ∑ 𝐴𝜔% + 𝐵𝜔 + 𝐶 − 𝑒 𝜔 = 0 = ∑ 𝐴𝜔% + 𝐵𝜔 + 𝐶 − 𝑒 = 0 Stamp Flyの概要とモデリング
Drone Control Workshop 推⼒から⼊⼒電圧を算出 𝜔= 6 7! 5 𝑒 = 𝐴𝜔 + 𝐵𝜔 + 𝐶 𝑒=𝐴 6 7! +𝐵 6 7! +𝐶 Stamp Flyの概要とモデリング
Drone Control Workshop パラメータ推定のための最⼩⼆乗法 正規⽅程式の⾏列表現 ∑ 𝜔6 ∑ 𝜔5 ∑ 𝜔% ∑ 𝜔5 ∑ 𝜔% ∑𝜔 ∑ 𝜔% ∑𝜔 𝑁 6 ∑𝜔 𝐴 5 ∑ = 𝜔 𝐵 𝐶 ∑ 𝜔% % ∑ 𝑒𝜔 𝐴 𝐵 = ∑ 𝑒𝜔 𝐶 ∑𝑒 5 ∑𝜔 ∑ 𝜔% ∑𝜔 % K/ ∑𝜔 ∑𝜔 𝑁 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 推⼒と⼊⼒電圧の線形化 𝑑𝑒(𝑇7 ) 𝑒 = 𝑒7 + ∆𝑇 𝑑𝑇 𝐴 𝐵 𝑒 = 𝑒7 + + ∆𝑇 𝐶8 2 𝐶8 𝑇7 𝑒+ ∆𝑇 𝑒 = 𝑒7 + 𝐶9 ∆𝑇 𝑇+ Stamp Flyの概要とモデリング 𝑑𝑒 ∆𝑇 𝑑𝑇
Drone Control Workshop 推⼒と⼊⼒電圧の線形化 𝑑𝑒(𝑇7 ) 𝑒 = 𝑒7 + ∆𝑇 𝑑𝑇 𝐴 𝐵 𝑒 = 𝑒7 + + ∆𝑇 𝐶8 2 𝐶8 𝑇7 𝑒 = 𝑒7 + 𝐶9 ∆𝑇 𝑒+ ∆𝑇 𝑒+ − 𝐶- 𝑇+ 𝑇+ 𝑒 = 𝐶9 𝑇 + 𝑒7 − 𝐶9 𝑇7 Stamp Flyの概要とモデリング 𝑑𝑒 ∆𝑇 𝑑𝑇
Drone Control Workshop モータ・プロペラ系の フィードフォワードコントローラが完成 ⾮線形制御モデル ⽬標推⼒ 𝑇 K 𝐴 +𝐵 X* K +𝐶 X* 電圧 モータ ⾓速度 推⼒ プロペラ 𝑇 フィードフォワードコントローラ モータ・プロペラ系の逆システム 線形制御モデル ⽬標推⼒ 𝑇 𝐶/ 𝑇 + 𝑒6 − 𝐶/ 𝑇6 電圧 フィードフォワードコントローラ モータ・プロペラ系の逆システム モータ ⾓速度 推⼒ プロペラ 𝑇 ※トリム点（釣り合い点）の周辺で有効 Stamp Flyの概要とモデリング
Drone Control Workshop 姿勢制御のためのモデリング Stamp Flyの概要とモデリング
Drone Control Workshop ニュートン・オイラー運動⽅程式 並進運動 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋L + 𝑋= 𝑚 𝑣̇ + 𝑟𝑢 − 𝑝𝑤 = 𝑌L + 𝑌= 𝑚 𝑤̇ + 𝑝𝑣 − 𝑞𝑢 = 𝑍L + 𝑍= + 𝑇 回転運動 𝐼M 𝑝̇ + 𝐼N − 𝐼> 𝑞𝑟 = 𝐿 + 𝐿= 𝐼> 𝑞̇ + 𝐼M − 𝐼N 𝑟𝑝 = 𝑀 + 𝑀= 𝐼N 𝑟̇ + 𝐼> − 𝐼M 𝑝𝑞 = 𝑁 + 𝑁= 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 𝜓 この⾏列を⽅向余弦⾏列（回転⾏列）とよぶ 𝜙 = tanYG 𝐸HI⁄𝐸II H H 𝜃 = tanYG − 𝐸GID 𝐸HI + 𝐸II 𝜓 = tanYG 𝐸GH⁄𝐸GG Stamp Flyの概要とモデリング − sin 𝜃 sin 𝜙 cos 𝜃 cos 𝜙 cos 𝜃
Drone Control Workshop 機体に働く重⼒ 𝑋L = −𝑚𝑔 sin 𝜃 𝑌L = 𝑚𝑔 cos 𝜃 sin 𝜙 𝑍L = 𝑚𝑔 cos 𝜃 cos 𝜙 Stamp Flyの概要とモデリング
Drone Control Workshop 機体に働く空気⼒ 𝑌S 𝜔 Y 𝑣 𝑁S 𝑢 𝑋S X 𝑉' 𝑤 𝛼 𝛽 Z 𝑍S Stamp Flyの概要とモデリング
Drone Control Workshop 機体に働く空気⼒ 𝑋= = −𝐶O 𝑢 𝑌= = −𝐶P 𝑣 𝑍= = −𝐶Q 𝑤 𝑁= = −𝐶C 𝑟 ホバリングや定常⾶⾏を考える際は微⼩だと考えて空⼒を無視する Stamp Flyの概要とモデリング
Drone Control Workshop 微⼩擾乱法による 運動⽅程式の線形化 Stamp Flyの概要とモデリング
Drone Control Workshop 線形化とは 微⼩範囲なら出⼒と⼊ ⼒の関係が線形だとみ なせる 𝑒+ ∆𝑇 𝑑𝑒 ∆𝑇 𝑑𝑇 ⾮線形なモデルを微⼩ 範囲でだけ通⽤するこ とを前提で線形モデル を⾒出すこと 𝑇+ マルチコプタの制御 250
Drone Control Workshop 微⼩擾乱法 𝑢 = 𝑢: + ∆𝑢 𝑣 = 𝑣: + ∆𝑣 𝑤 = 𝑤: + ∆𝑤 𝑟 = 𝑟: + ∆𝑟 𝑞 = 𝑞: + ∆𝑞 𝑋L = 𝑋L: + ∆𝑋L 𝑋= = 𝑋=: + ∆𝑋= つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋L: + 𝑋=: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋L + 𝑋= 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる Stamp Flyの概要とモデリング
Drone Control Workshop 微⼩擾乱法 𝑢 = 𝑢: + ∆𝑢 𝑣 = 𝑣: + ∆𝑣 𝑤 = 𝑤: + ∆𝑤 𝑟 = 𝑟: + ∆𝑟 𝑞 = 𝑞: + ∆𝑞 𝑋L = 𝑋L: + ∆𝑋L 𝑋= = 𝑋=: + ∆𝑋= 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋L: + 𝑋=: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋L + 𝑋= 話を簡単にするためにホバリングの制御に限定 Stamp Flyの概要とモデリング
Drone Control Workshop 微⼩擾乱法 𝑢 = ∆𝑢 𝑣 = ∆𝑣 𝑤 = ∆𝑤 𝑟 = ∆𝑟 𝑞 = ∆𝑞 𝑋L = ∆𝑋L 𝑋= = ∆𝑋= 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋L: + 𝑋=: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋L + 𝑋= 話を簡単にするためにホバリングの制御に限定 Stamp Flyの概要とモデリング
Drone Control Workshop 微⼩擾乱法 𝑢 = ∆𝑢 𝑣 = ∆𝑣 𝑤 = ∆𝑤 𝑟 = ∆𝑟 𝑞 = ∆𝑞 𝑋L = ∆𝑋L 𝑋= = ∆𝑋= 各変数を添え字０がつい てるトリム点からの微⼩ な変動Δが増減したもの とみる つり合いの式 𝑚 𝑞: 𝑤: − 𝑟: 𝑣: = 𝑋L: + 𝑋=: 運動⽅程式 𝑚 𝑢̇ + 𝑞𝑤 − 𝑟𝑣 = 𝑋L + 𝑋= 𝑚∆𝑢̇ = −𝑚𝑔∆𝜃 − 𝐶O ∆𝑢 微⼩量同志の積は０とする Stamp Flyの概要とモデリング
Drone Control Workshop 線形化された運動⽅程式 並進運動 𝑚∆𝑢̇ = −𝑚𝑔∆𝜃 − 𝐶O ∆𝑢 𝑚∆𝑣̇ = 𝑚𝑔∆𝜙 − 𝐶P ∆𝑣 𝑚∆𝑤̇ = −𝐶Q ∆𝑤 + ∆𝑇 回転運動 𝐼M ∆𝑝̇ = ∆𝐿 𝐼> ∆𝑞̇ = ∆𝑀 𝐼N ∆𝑟̇ = ∆𝑁 Stamp Flyの概要とモデリング
Drone Control Workshop プロペラ推⼒の線形化 𝑇 = 𝐶# 𝜔 $ 線形化された推⼒ ∆𝑇 = 2𝐶# 𝜔" ∆𝜔 線形化されたトルク ∆𝑄 = 2𝐶# 𝜔" 𝜅∆𝜔 Stamp Flyの概要とモデリング
Drone Control Workshop 線形化された推⼒・モーメント ∆𝑇 = 2𝐶( 𝜔) ∆𝜔# + ∆𝜔$ + ∆𝜔% + ∆𝜔& ∆𝐿 = 2𝐶( 𝜔) −𝑥# ∆𝜔# − 𝑥$ ∆𝜔$ + 𝑥% ∆𝜔% + 𝑥& ∆𝜔& ∆𝑀 = 2𝐶( 𝜔) 𝑦# ∆𝜔# − 𝑦$ ∆𝜔$ − 𝑦% ∆𝜔% + 𝑦& ∆𝜔& ∆𝑁 = 2𝐶( 𝜔) 𝜅∆𝜔# − 𝜅∆𝜔$ + 𝜅∆𝜔% − 𝜅∆𝜔& StampFlyの場合はプロペラが正⽅形の頂点に配置されているので 𝑙 = 𝑥* = 𝑦* 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 G9 + 2𝐶4 𝜔: G9 ∆𝜔 = ∆𝑒 Stamp Flyの概要とモデリング
Drone Control Workshop モータ・プロペラ系の線形化 ̇ + 𝐷+ 𝐽∆𝜔 8 ̇ + 𝐽G9 ∆𝜔 8" 9 8" 8 + 2𝐶4 𝜔: ∆𝜔 = ∆𝑒 9 8 8 𝐷 + 9 G9 + 2𝐶4 𝜔: G9 ∆𝜔 = ∆𝑒 8 ̇ + 𝐵 + 2𝜔: 𝐴 ∆𝜔 = ∆𝑒 𝐽G ∆𝜔 9 Stamp Flyの概要とモデリング
Drone Control Workshop モータ・プロペラ系の線形化 8 ̇ + 𝐵 + 2𝜔: 𝐴 ∆𝜔 = ∆𝑒 𝐽G9 ∆𝜔 ̇ + 𝐶4 𝐵⁄𝐴 + 2𝐶4 𝜔: ∆𝜔 = 𝐽∆𝜔 𝐶Z 𝐵 𝐾H =𝐷+ 𝐴 𝑅 𝐶Z 𝐾 = 𝐴 𝑅 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 回転運動の伝達関数 𝐼M ∆𝑝̇ = ∆𝐿 ラプラス変換 𝐼M 𝑠∆𝑝 = ∆𝐿 𝐼> ∆𝑞̇ = ∆𝑀 𝐼> 𝑠∆𝑞 = ∆𝑀 𝐼N ∆𝑟̇ = ∆𝑁 𝐼N 𝑠 ∆𝑟 = ∆𝑁 Stamp Flyの概要とモデリング 1 ∆𝑝 = ∆𝐿 𝐼M 𝑠 1 ∆𝑞 = ∆𝑀 𝐼> 𝑠 1 ∆𝑟 = ∆𝑁 𝐼N 𝑠
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 推⼒の伝達関数 ∆𝑇 = 2𝐶? 𝜔: ∆𝜔/ + ∆𝜔% + ∆𝜔5 + ∆𝜔6 T! ∆𝜔 = U VWX ∆𝑒 なので ! 2𝐶? 𝜔: 𝐾R ∆𝑇 = ∆𝑒/ + ∆𝑒% + ∆𝑒5 + ∆𝑒6 𝜏R 𝑠 + 1 Stamp Flyの概要とモデリング
Drone Control Workshop ロールモーメントの伝達関数 ∆𝐿 = 2𝑙𝐶? 𝜔: −∆𝜔/ − ∆𝜔% + ∆𝜔5 + ∆𝜔6 2𝑙𝐶? 𝜔: 𝐾R ∆𝐿 = −∆𝑒/ − ∆𝑒% + ∆𝑒5 + ∆𝑒6 𝜏R 𝑠 + 1 Stamp Flyの概要とモデリング
Drone Control Workshop 推⼒・モーメントの伝達関数まとめ 2𝐶? 𝜔: 𝐾R ∆𝑇 = ∆𝑒/ + ∆𝑒% + ∆𝑒5 + ∆𝑒6 𝜏R 𝑠 + 1 2𝑙𝐶? 𝜔: 𝐾R ∆𝐿 = −∆𝑒/ − ∆𝑒% + ∆𝑒5 + ∆𝑒6 𝜏R 𝑠 + 1 2𝑙𝐶? 𝜔: 𝐾R ∆𝑀 = ∆𝑒/ − ∆𝑒% − ∆𝑒5 + ∆𝑒6 𝜏R 𝑠 + 1 2𝜅𝐶? 𝜔: 𝐾R ∆𝑁 = ∆𝑒/ − ∆𝑒% + ∆𝑒5 − ∆𝑒6 𝜏R 𝑠 + 1 Stamp Flyの概要とモデリング
Drone Control Workshop 推⼒・モーメントの伝達関数まとめ 2𝐶? 𝜔: 𝐾R ∆𝑇 = ∆𝑒? 𝜏R 𝑠 + 1 2𝑙𝐶? 𝜔: 𝐾R ∆𝐿 = ∆𝑒; 𝜏R 𝑠 + 1 2𝑙𝐶? 𝜔: 𝐾R ∆𝑀 = ∆𝑒E 𝜏R 𝑠 + 1 制御⼊⼒部分を１変数にまとめる 2𝜅𝐶? 𝜔: 𝐾R ∆𝑁 = ∆𝑒C 𝜏R 𝑠 + 1 Stamp Flyの概要とモデリング
Drone Control Workshop 制御⼊⼒とモータ⼊⼒電圧 制御⼊⼒とモータの⼊⼒電圧との関係を表す ∆𝑒? = ∆𝑒/ + ∆𝑒% + ∆𝑒5 + ∆𝑒6 ∆𝑒; = −∆𝑒/ − ∆𝑒% + ∆𝑒5 + ∆𝑒6 ∆𝑒E = ∆𝑒/ − ∆𝑒% − ∆𝑒5 + ∆𝑒6 ∆𝑒E = ∆𝑒/ − ∆𝑒% + ∆𝑒5 − ∆𝑒6 Stamp Flyの概要とモデリング
Drone Control Workshop ミキシング 制御⼊⼒を各モータの⼊⼒電圧に振り分ける ∆𝑒K 1 1 1 1 ∆𝑒L −1 −1 1 1 = ∆𝑒M 1 −1 −1 1 ∆𝑒M 1 −1 1 −1 ∆𝑒G ∆𝑒H ∆𝑒I ∆𝑒J ∆𝑒G 0.25 −0.25 0.25 0.25 ∆𝑒H 0.25 −0.25 −0.25 −0.25 = ∆𝑒I 0.25 0.25 −0.25 0.25 ∆𝑒J 0.25 0.25 0.25 −0.25 Stamp Flyの概要とモデリング ∆𝑒K ∆𝑒L ∆𝑒M ∆𝑒M
Drone Control Workshop 各軸の独⽴制御 ミキシングの式を⽤いることにより 各軸を独⽴して制御することが可能となる Stamp Flyの概要とモデリング
Drone Control Workshop 実際の制御⼊⼒ 𝑒 = 𝑒! + ∆𝑒 𝑒:実際に加える⼊⼒電圧 𝑒: :ホバリングのための⼊⼒電圧 ∆𝑒:制御器が算出した制御⼊⼒ Stamp Flyの概要とモデリング
Drone Control Workshop ⾓速度系の伝達関数まとめ ロール⾓速度 ピッチ⾓速度 ヨー⾓速度 ∆𝑝 = ∆𝑞 = ∆𝑟 = %1'! (" )# *& +(-# +./) %1'! (" )# *% +(-# +./) %&'! (" )# *$ +(-# +./) 𝛿; 𝛿E 𝛿C Stamp Flyの概要とモデリング
Drone Control Workshop 姿勢の推定 Lesson 8 マルチコプタの制御 279
Drone Control Workshop 加速度センサによる姿勢の推定 重⼒加速度のx軸成分 重⼒加速度のy軸成分 x軸 z軸 重⼒加速度 マルチコプタの制御 280
Drone Control Workshop 加速度センサによる姿勢の推定 重⼒加速度のx軸成分 重⼒加速度のy軸成分 z軸 θ 重⼒加速度 x軸 𝑔 D 01 y軸周り 𝜃 = tan 𝑔E y軸が地⾯に⽔平だとθは計算できそうだが ⽔平でない場合はどうか︖ 他の軸周りの⾓度は︖ マルチコプタの制御 281
Drone Control Workshop 姿勢の推定 • 重⼒・地磁気による⽅向の推定 • ⾓速度の積分 • カルマンフィルタ 弊ブログも参考にしてください • 推定誤差の分散を最⼩化 • Madgwickフィルタ • 最急降下法で予測（複数の推定を重み付で合成） • 相補フィルタ • 加速度からの推定と⾓速度からの推定をそれぞれローパス・ハイパス フィルタを通してから合成 マルチコプタの制御 282
Drone Control Workshop IMUによる２⽅法の姿勢の推定 • ⾓速度の積分からの推定 • 重⼒ベクトル⽅向からの推定 ２種類の情報をうまくブレンド カルマンフィルタ マルチコプタの制御 283
Drone Control Workshop ⾓度の推定 ジャイロから得られた ⾓速度 積分 ⾓度 加速度計から得られた 加速度 逆三⾓関数 ⾓度 284
Drone Control Workshop ⾓度の推定 ジャイロから得られた ⾓速度 積分 ⾓度 オフセットが積分されて 時間と共に増⼤ 加速度計から得られた 加速度 逆三⾓関数 ⾓度 ノイズ成分が⼤きい 285
Drone Control Workshop ⾓度の推定の実際 ジャイロから得られた ⾓速度 ハイパス フィルタ 積分 重み K ＋ 加速度計から得られた 加速度 ローパス フィルタ 逆三⾓関数 ⾓度 重み １ーK 286
Drone Control Workshop ルドルフ・カルマン １９３０年ハンガリのブタペスト⽣まれのア メリカの学者（２０１６年没） １９６０年、⼤学院の時にカルマンフィルタ を考案する。 アポロ宇宙船の軌道推定に使⽤され，その実 ⽤性が⾼く評価される。 Wikipediaより転載 現代制御理論の⽣みの親
Drone Control Workshop 姿勢の表現 • オイラー⾓ （3） • クォータニオン （4） • ⽅向余弦⾏列 （5） 弊ブログも参考にしてください マルチコプタの制御 288
Drone Control Workshop 289
Drone Control Workshop 戦闘機の座標系の各軸のベクトル 戦闘機のX軸の ベクトル成分 （⼤きさ1） 𝑒1D 𝒆1 = 𝑒1$ 𝑒1E 戦闘機のY軸の ベクトル成分 （⼤きさ1） 𝑒5D 𝒆5 = 𝑒5$ 𝑒5E 戦闘機のZ軸の ベクトル成分 （⼤きさ1） 𝑒FD 𝒆F = 𝑒F$ 𝑒FE これらは全て地球の座標系から⾒たベクトル 290
Drone Control Workshop ⽅向余弦⾏列 𝑒1D 𝐄 = 𝑒1$ 𝑒1E 戦闘機のX軸の ベクトル成分 𝑒5D 𝑒5$ 𝑒5E 戦闘機のY軸の ベクトル成分 𝑒FD 𝑒F$ 𝑒FE 戦闘機のZ軸の ベクトル成分 この⾏列をもって戦闘機の姿勢（⾓度）とする 291
Drone Control Workshop 回転⾏列（ベクトルを回す⾏列） Ｚ軸周り cos 𝜓 − sin 𝜓 0 𝐑 Y (𝜓) = sin 𝜓 cos 𝜓 0 0 0 1 Y軸周り cos 𝜃 0 sin 𝜃 𝐑 Z (𝜃) = 0 1 0 −sin 𝜃 0 cos 𝜃 X軸周り 1 0 0 𝐑[ (𝜙) = 0 cos 𝜙 −sin 𝜙 0 sin 𝜙 cos 𝜙 マルチコプタの制御 292
Drone Control Workshop 座標変換⾏列（座標を回す⾏列） Ｚ軸周り cos 𝜓 sin 𝜓 0 𝐂Y (𝜓) = −sin 𝜓 cos 𝜓 0 0 0 1 Y軸周り cos 𝜃 0 −sin 𝜃 𝐂Z (𝜃) = 0 1 0 sin 𝜃 0 cos 𝜃 X軸周り 1 0 0 𝐂[ (𝜙) = 0 cos 𝜙 sin 𝜙 0 − sin 𝜙 cos 𝜙 マルチコプタの制御 弊ブログも参考にしてください 293
Drone Control Workshop 地球から機体への座標変換 地球座標から機体座標への座標変換を表す式を記述します Ｚ軸周り 𝑥( 𝑥! 𝑦( = 𝐂, (𝜓) 𝑦! 𝑧( 𝑧! Y軸周り 𝑥) 𝑥( 𝑦) = 𝐂- (𝜃) 𝑦( 𝑧) 𝑧( X軸周り 𝑥* 𝑥) 𝑦* = 𝐂. (𝜙) 𝑦) 𝑧* 𝑧) センサ座標のベクトル 地球座標のベクトル 𝑥` 𝑥7 𝑦` = 𝐂[ 𝜙 𝐂Z (𝜃)𝐂Y (𝜓) 𝑦7 𝑧` 𝑧7 𝑥7 = 𝐄(𝜙, 𝜃, 𝜓) 𝑦7 𝑧7 𝐄 𝜙, 𝜃, 𝜓 = 𝐂[ (𝜙)𝐂Z (𝜃)𝐂Y (𝜓) ここで出てる⾓度をオイラー⾓と⾔います オイラー⾓は座標を回す順で違ってきます マルチコプタの制御 294
Drone Control Workshop 機体から地球への座標変換 地球座標から機体座標への座標変換を表す式を記述します 地球座標のベクトル センサ座標のベクトル 𝑥7 𝑥` 𝑦7 = 𝐑 a 𝜓 𝐑 Z (𝜃)𝐑[ (𝜙) 𝑦` 𝑧7 𝑧` 𝑥7 = 𝐑(𝜙, 𝜃, 𝜓) 𝑦7 𝑧7 回転⾏列（⽅向余弦⾏列） 𝐑 𝜙, 𝜃, 𝜓 = 𝐑 a 𝜓 𝐑 Z (𝜃)𝐑[ (𝜙) 機体の姿勢を表します マルチコプタの制御 295
Drone Control Workshop ⽅向余弦⾏列の性質 ⽅向余弦⾏列は回転⾏列なので素晴らしい性質があります • 実対象⾏列 • 逆⾏列が転置⾏列に等しい マルチコプタの制御 296
Drone Control Workshop 重⼒の座標変換 𝑔b 0 𝑔c = 𝐂[ 𝜙 𝐂Z (𝜃)𝐂Y (𝜓) 0 𝑔a 𝑔 0 = 𝐄(𝜙, 𝜃, 𝜓) 0 𝑔 センサから得られるのは 𝑔[ マルチコプタの制御 𝑔* 𝑔\ ] 297
Drone Control Workshop 重⼒の座標変換 センサから得られるのは 𝑔[ 𝑔* 𝑔\ ] 𝑔b 0 𝑔c = 𝐑d (𝜙, 𝜃, 𝜓) 0 𝑔a 𝑔 𝑔b 0 0 = 𝐑(𝜙, 𝜃, 𝜓) 𝑔c 𝑔a 𝑔 地球座標系の重⼒ベクトル を機体座標系のベクトルに 変換 機体で検知した重⼒ベクト ルを地球座標系の重⼒ベク トルに変換 マルチコプタの制御 298
Drone Control Workshop 姿勢制御について ロール⾓⽬標 ⾓度PID ロール⾓ 姿勢推定 + 誤差 - ⾓速度PID u StampFly ロール運動 StampFly ロール⾓速度 Gyro ロールとピッチはこの様に⾓度制御する ヨーは⾓度推定ができないため⾓度制御をしない（⾓速度制御のみ） マルチコプタの制御 299
Drone Control Workshop 資料最終ページ マルチコプタの制御 300