StampFlyで学ぶマルチコプタ制御

Drone Control Workshop StampFlyで学ぶ マルチコプタ制御 2024.2.13〜2.19 伊藤研究室ワークショップ資料 伊藤恒平

Drone Control Workshop スケジュール PM イントロダクション・クアッドコプタの運動原理 Lesson 0 環境構築（確認・事前にインストール済み） Lesson 1 モータを回す Lesson 2 送信機からの操縦指令の受信 2⽇⽬（14⽇） AM PM Lesson 3 LED表⽰やテレメトリ受信 Lesson 4 ミキシング 3⽇⽬（15⽇） AM PM Lesson 5 ⾓速度・加速度の取得 Lesson 6 モデルとシステム同定 Lesson 7 PID制御のループ整形による設計 4⽇⽬（16⽇） AM PM Lesson 8 姿勢推定 Lesson9 フライト制御システム実装 5⽇⽬（17⽇） 終⽇ 制御システム実装 6⽇⽬（19⽇） AM ドローンレース 社会⼈講習（企業の⽅の講演） 申し送りゼミ 追い出しコンパ 1⽇⽬（13⽇） AM PM ⼣⽅ マルチコプタの制御 2

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

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

Drone Control Workshop StampFly 300mAh1S電池 RGBLED RGBLED M5StampS3 距離センサ ブラシ付 DCモータ 距離センサ マルチコプタの制御 5

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

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

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

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

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

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

Drone Control Workshop クアッドコプタの運動原理 全てのプロペラを 同時に 同じだけ 強くしたり 弱くしたり すると 上昇・下降する X 上昇 下降 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 クアッドコプタの運動原理 直接制御できる運動は ・上下運動 ・各軸周りの回転運動 これらの合計4つの運動のみ 前後運動と左右運動は直接制御できない マルチコプタの制御 16

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

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 18

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

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

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

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

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

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

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

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

Drone Control Workshop サンプルのコードはGithubにあります cloneして使⽤ください． https://github.com/M5Fly-kanazawa/StampFly_Workshop 端末からは以下のようにします git clone https://github.com/M5Fly-kanazawa/StampFly_Workshop.git cloneについては後でも説明します マルチコプタの制御 26

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

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

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

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

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

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

Drone Control Workshop RepositoryのFork Forkというのは⾃分のGithubアカウントに他のアカウントのレポジトリを コピーする事です StampFly_Workshopのレポジトリを開きます マルチコプタの制御 32

Drone Control Workshop Repository Fork 👆 マルチコプタの制御 33

Drone Control Workshop ⾃分のGithubアカウントへのFork確認 ⾃分のか確認する マルチコプタの制御 34

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

Drone Control Workshop VSCodeでclone マルチコプタの制御 36

Drone Control Workshop VSCodeでclone StampFly_Workshopと打ち込んで 検索する ⾃分のGithubアカウントにある ForkしたStampFly_Workshopリポ ジトリを選択する マルチコプタの制御 37

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

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

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

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

Drone Control Workshop Lesson0を選択 マルチコプタの制御 42

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

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

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

Drone Control Workshop StampFlyとコントローラの関連付け L R ②機体の電源を⼊れる ③ ② Peering ① 👆 ④ ①パネルボタンを押したまま電源を⼊れる マルチコプタの制御 46

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

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

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

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

Drone Control Workshop Lesson0 Exercise 1. ⾃分のGithubアカウントにStampFly_workshop レポジトリをFork 2. ForkしたStampFly_workshopレポジトリをclone 3. クローンしたプログラムをビルドしStampFly に書き込む 4. StampFlyを⾶ばしてみる マルチコプタの制御 51

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

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

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

Drone Control Workshop Lesson1を⾏う準備 VSCodeの 「開く」またはフォルダを開で lesson1フォルダを開く マルチコプタの制御 55

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

Drone Control Workshop PWMの出⼒⽤の低レベル関数 セットアップ 0〜15 Duty100%を2の 何乗にするか を決める uint32_t ledcSetup(uint8_t channel, uint32_t freq, uint8_t bit_num); 使⽤するピンとチャンネルの割り当て void ledcAttachPin(uint8_t pin, uint8_t chan); Dutyの変更 void ledcWrite(uint8_t chan, uint32_t duty); これらはmotor.cppの中で使われているが、⾃分で書く必要はない マルチコプタの制御 57

Drone Control Workshop モータの制御 モータを使える状態にする init_motor() モータをDutyに応じて回転させる関数 set_motor_duty(motor_handle, duty) モータを番号で指定 Dutyを指定 使⽤例 set_motor_duty(FRONT_LEFT_MOTOR, 0.15) 左前のモータをDuty 15％で回す例 マルチコプタの制御 58

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

Drone Control Workshop サンプルプログラムについて 👈グローバル変数Loop_counter の値で流れが制御されている 👈全てのモータをDuty15％にセット 👈全てのモータを停⽌ 👈 Loop_counterを1増やす マルチコプタの制御 60

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

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

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

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

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

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

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

Drone Control Workshop Lesson2を⾏う準備 VSCodeの 「開く」またはフォルダを開で lesson2フォルダを開く マルチコプタの制御 68

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

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

Drone Control Workshop サンプルプログラムについて 👈Stickの値をみてます マルチコプタの制御 71

Drone Control Workshop Lesson2 Exercise スロットルスティックの倒し量に応じて４つのプロ ペラの回転速度が変わるようにしなさい。 スティックが中⽴位置にいる時が停⽌ スティックが⼀番上にいる時が最⾼速 マルチコプタの制御 72

Drone Control Workshop LED表⽰やテレメトリ受信 Lesson 3 マルチコプタの制御 73

Drone Control Workshop Lesson3を⾏う準備 VSCodeの 「開く」またはフォルダを開で lesson3フォルダを開く マルチコプタの制御 74

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

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

Drone Control Workshop 今回⽤意したLED⽤の関数 init_led(0); 👈StampFlyのLEDを使える状態にする初期化関数． 引数を1にしておくと点灯状態で初期化される board_bottom_led(RED, 1); board_tail_led(GREEN, 1); stamp_led(BLUE, 1); ⾊ 0︓消灯 1︓点灯 👈StampFlyの3つLEDの⾊を変え点灯す るか消すかを予約する FastLED.show(64); 👈この関数が呼ばれるとLEDが点灯する64は明るさ で0〜255にできる、なしの時は255と同じ マルチコプタの制御 76

Drone Control Workshop サンプルプログラムについて 👈各LEDの⾊と点灯を予約 👈10秒経ったら⾊が変わる 👈各LEDの⾊と点灯を予約 👈ここで初めて予約が実⾏される（重要） マルチコプタの制御 77

Drone Control Workshop プログラムの複数ファイルへの分割 motor.cpp rc.cpp led.cpp control.cpp motor.hpp rc.cpp led.hpp control.hpp ソース ファイル ヘッダー ファイル プログラムの中 ⾝（定義）はこ ちらに記述する 他のファイルに 公開したい変数 や関数の指定は こちらに書く 機能やものごとにファイルを分割する マルチコプタの制御 78

Drone Control Workshop プログラムの複数ファイルへの分割 • 各ファイルにはそのファイルでしか使わない変数や関数が ある • 他のファイルに公開する関数や変数をヘッダファイルで指 定して他のファイルに使ってもらえる • 違うモータに変えたり制御⽅法を変えるなどしたときに関 係するファイルだけ置き換えることで変更が簡単に⾏える （公開する関数の名前や引数などは元のものと合わせる） マルチコプタの制御 79

Drone Control Workshop ファイルの中⾝の例 ソースファイル #include “motor.hpp” #include “rc.hpp” #include “led.cpp” uint8_t Control_counter=0; 👈このファイル内だ uint8_t Control_state=0; けのグローバル変数 uint8_t init_control(uint8_t……….) { ….. } ヘッダファイル #ifndef MOTOR_HPP #define MOTOR_HPP extern uint8_t Control_state; uint8_t init_control(uint8_t………..); 外部に公開したい 変数や関数 uint8_t calc_err(uint8_t……….) { 👈このファイル内だ …….. けのローカルな関数 } マルチコプタの制御 80

Drone Control Workshop Telemetry機能について Telemetryは遠隔でロボットの情報を送って受け取れる機能です #include “rc.hpp” によりTelemetry機能が使えます PC M5ATOM等 USB-Cケーブル MACアドレス ESP-NOW StampFly rc.cppの13⾏⽬にM5ATOM等のMACアドレスを記述する マルチコプタの制御 81

Drone Control Workshop Telemetry機能でデータを送る 以下の様にすることでfloat型データ⼀つをM5ATOM経由でUART通信 で送れます。配列を⼤きくするとその分多くのデータを送れます uint8_t data[6]; float x = 3.14; uint8_t* p; p=(uint8_t*)&x; data[0]=0; 👈この⼆つは必ず0にする data[1]=0; data[2]=p[0]; data[3]=p[1]; 👈float型変数を４個のバイト列に変換 data[4]=p[2]; data[5]=p[3]; datalen =6; 👈送るバイト数は6バイト send_telemetry(data, datalen)👈送信︕ マルチコプタの制御 82

Drone Control Workshop TelemetryデータをPCで確認 全部のOS共通 VSCode MacやUbuntu（Linux） minicom screen マルチコプタの制御 Windows TereTerm 83

Drone Control Workshop Lesson3 Exercise led.cpp内のblink_led関数を完成してStampFlyのLEDを 点滅させなさい。点滅の様⼦や⾊や数はなるべく華 やかになる様に⼯夫してください。 ただし、完成したblink_led関数はflight_control.cppの loop_400Hz関数内で使⽤すること。そのためヘッダ ファイル内も適切に変更しなさい。 ※loop_400Hz関数は0.0025周期で動く必要があるためその中のプログ ラムは速やかに終わる必要があります マルチコプタの制御 84

Drone Control Workshop プロペラの発⽣する 推⼒・モーメントと ミキシング Lesson 4 マルチコプタの制御 85

Drone Control Workshop Lesson4を⾏う準備 VSCodeの 「開く」またはフォルダを開で lesson4フォルダを開く マルチコプタの制御 86

Drone Control Workshop ミキシングとは ドローンはプロペラの⾓速度を増減し回転運動や 上下運動を⽣み出す。 発⽣したい推⼒やモーメントを考えてそこから各 モータの回転指令を計算することをミキシングと 呼ぶ マルチコプタの制御 87

Drone Control Workshop プロペラの発⽣する 推⼒とトルク 推力 𝑇 Thrust プロペラの角速度 𝜔 プロペラの回転方向と トルクの向きは逆です トルク Torque 𝑄 マルチコプタの制御 88

Drone Control Workshop 推⼒とトルクの算出式 𝑇 = 𝐶! 𝜔 " 𝑑𝜔 " 𝑄 = 𝐶# 𝜔 + 𝐽 𝑑𝑡 記号の説明 𝐶! 推⼒係数 𝐶" トルク係数 𝐽 ロータ・プロペラの慣性モーメント マルチコプタの制御 89

Drone Control Workshop プロペラのトルクシミュレーション マルチコプタの制御 90

Drone Control Workshop 推⼒とトルクの線形化 推⼒とトルクの算出式 𝑇 = 𝐶! 𝜔 推⼒やトルクはプロペラの ⾓速度には⽐例しない " 𝑑𝜔 " 𝑄 = 𝐶# 𝜔 + 𝐽 𝑑𝑡 マルチコプタの制御 ⼯学的には⽐例する現象の ⽅が取り扱いやすいが なんとかならないか︖ 91

Drone Control Workshop 釣り合いからの変動 𝑇 = 𝑇! + ∆𝑇 = 𝐶" 𝜔! + ∆𝜔 # 𝑄 = 𝑄! + ∆𝑄 = 𝐶$ 𝜔! + ∆𝜔 # ホバリング時 の量 増減 釣り合った状態からの増減分に着⽬します マルチコプタの制御 92

Drone Control Workshop 釣り合いからの変動 $ 𝑇# + ∆𝑇 = 𝐶! 𝜔# + 2𝜔# ∆𝜔 + ∆𝜔 $ $ $ 𝑄# + ∆𝑄 = 𝐶" 𝜔# + 2𝜔# ∆𝜔 + ∆𝜔 マルチコプタの制御 93

Drone Control Workshop 釣り合いからの変動 $ 𝑇# + ∆𝑇 = 𝐶! 𝜔# + 2𝜔# ∆𝜔 + ∆𝜔 $ $ $ 𝑄# + ∆𝑄 = 𝐶" 𝜔# + 2𝜔# ∆𝜔 + ∆𝜔 マルチコプタの制御 94

Drone Control Workshop 釣り合いからの変動 ∆𝑇 = 𝐶! 2𝜔# ∆𝜔 ∆𝑄 = 𝐶" 2𝜔# ∆𝜔 マルチコプタの制御 95

Drone Control Workshop 釣り合いからの変動 ∆𝑇 = 2𝐶! 𝜔# ∆𝜔 ∆𝑄 = 2𝐶" 𝜔# ∆𝜔 推⼒・トルク線形化モデル 推⼒もトルクも変動分はプロペラの⾓速度の変化に⽐例する マルチコプタの制御 96

Drone Control Workshop 𝑇ℎ𝑟𝑢𝑠𝑡 推⼒の配分 𝑇ℎ𝑟𝑢𝑠𝑡 = 𝑇% + 𝑇# + 𝑇& + 𝑇' マルチコプタの制御 97

Drone Control Workshop Left Right 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 推⼒の配分 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 = −𝑥% 𝑇% − 𝑥# 𝑇# + 𝑥& 𝑇& + 𝑥' 𝑇' マルチコプタの制御 98

Drone Control Workshop トルクの発⽣の向き はプロペラの回転⽅ 向とは逆なので注意 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 トルクの配分 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑄% − 𝑄# + 𝑄& − 𝑄' マルチコプタの制御 99

Drone Control Workshop 機体に働く推⼒とモーメント 𝑇ℎ𝑟𝑢𝑠𝑡 = 𝑇% + 𝑇# + 𝑇& + 𝑇' 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 = −𝑥% 𝑇% − 𝑥# 𝑇# + 𝑥& 𝑇& + 𝑥' 𝑇' 𝑃𝑖𝑡𝑐ℎ 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑦% 𝑇% − 𝑦# 𝑇# − 𝑦& 𝑇& + 𝑦' 𝑇' 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑄% − 𝑄# + 𝑄& − 𝑄' マルチコプタの制御 100

Drone Control Workshop 機体に働く推⼒とモーメント 𝑇 = 𝑇% + 𝑇# + 𝑇& + 𝑇' 𝐿 = −𝑥% 𝑇% − 𝑥# 𝑇# + 𝑥& 𝑇& + 𝑥' 𝑇' 𝑀= 𝑦% 𝑇% − 𝑦# 𝑇# − 𝑦& 𝑇& + 𝑦' 𝑇' 𝑁 = 𝑄% − 𝑄# + 𝑄& − 𝑄' マルチコプタの制御 101

Drone Control Workshop 釣り合い状態からの変動分 ∆𝑇 = ∆𝑇% + ∆𝑇# + ∆𝑇& + ∆𝑇' ∆𝐿 = −𝑥% ∆𝑇% − 𝑥# ∆𝑇# + 𝑥& ∆𝑇& + 𝑥' ∆𝑇' ∆𝑀 = 𝑦% ∆𝑇% − 𝑦# ∆𝑇# − 𝑦& ∆𝑇& + 𝑥' ∆𝑇' ∆𝑁 = ∆𝑄% − ∆𝑄# + ∆𝑄& + ∆𝑄' マルチコプタの制御 102

Drone Control Workshop 推⼒・トルクとプロペラ⾓速度 ∆𝑇 = 2𝐶" 𝜔! ∆𝜔% + ∆𝜔# + ∆𝜔& + ∆𝜔' ∆𝐿 = 2𝐶" 𝜔! −𝑥% ∆𝜔% − 𝑥# ∆𝜔# + 𝑥& ∆𝜔& + 𝑥' ∆𝜔' ∆𝑀 = 2𝐶" 𝜔! 𝑦% ∆𝜔% − 𝑦# ∆𝜔# − 𝑦& ∆𝜔& + 𝑦' ∆𝜔' ∆𝑁 = 2𝐶$ 𝜔! ∆𝜔% − ∆𝜔# + ∆𝜔& − ∆𝜔' マルチコプタの制御 103

Drone Control Workshop ⾏列版 推⼒・トルクとプロペラ⾓速度 𝐶" ∆𝑇 0 ∆𝐿 = 2𝜔! 0 ∆𝑀 0 ∆𝑁 ドローンの 推⼒・モーメント 0 𝐶" 0 0 0 0 𝐶" 0 0 0 0 𝐶# 1 −𝑥$ 𝑦$ 1 ペラの回転数と 推⼒・トルクの関係 1 −𝑥% −𝑦% −1 1 𝑥& −𝑦& 1 プロペラの位置や 回転⽅向の関係 マルチコプタの制御 1 𝑥' 𝑦' −1 ∆𝜔$ ∆𝜔% ∆𝜔& ∆𝜔' プロペラの⾓速度 104

Drone Control Workshop 希望の運動 運動を発⽣さ せる推⼒と モーメント 推⼒とモーメント を⽣み出すモータ プロペラの⾓速度 各軸周りのモーメントや推⼒からモータ・プロペラの ⾓速度を求める必要がある。 マルチコプタの制御 105

Drone Control Workshop ベクトル・⾏列部分を⽂字で置き換え 𝐶" 0 0 0 1 1 ∆𝑇 0 𝐶" 0 0 −𝑥$ −𝑥% ∆𝐿 = 2𝜔! 0 0 𝐶" 0 𝑦$ −𝑦% ∆𝑀 0 0 0 𝐶# ∆𝑁 1 −1 𝛅 = 𝛅 = 推⼒ モーメント 𝐂 1 𝑥& −𝑦& 1 𝐀 係数⾏列 𝐌 マルチコプタの制御 1 𝑥' 𝑦' −1 配置⾏列 ∆𝜔$ ∆𝜔% ∆𝜔& ∆𝜔' 𝛚 𝛚 プロペラ ⾓速度 106

Drone Control Workshop 𝐌が正⽅⾏列の場合は逆⾏列が 存在するため𝐌の逆⾏列を計算す れば良く 𝛚=𝐌 *+ 𝛅 ・・・・（M) *𝟏 𝐌 を変換⾏列と呼ぶことにします マルチコプタの制御 107

Drone Control Workshop 配置⾏列 前後が⻑いクアッドコプタ プロペラ推⼒ 𝑀' 𝑀& 2 𝑀% 𝑀# 𝑀& 𝑀' 1 1 1 1 Thrust −1 −1 1 1 Roll 2 −2 −2 2 Pitch 1 −1 1 −1 Yaw 𝑀# ⾚い⽮印はプロペラの 推⼒やモーメント 4 𝑀% ※⾏列の成分の数値は⽐率で良い 回転⽅向 マルチコプタの制御 108

Drone Control Workshop 配置⾏列 ⾚い⽮印 はプロペ ラの回転 ⽅向 1 1⁄2 𝑀! 3 2 ヘキサコプタ 𝑀! 𝑀" 𝑀# 𝑀$ 𝑀% 𝑀& 1 1 1 1 1 1 Thrust 1 1 1 1 − −1 − 1 Roll 2 2 2 2 3 3 3 3 Pitch 0 − − 0 2 2 2 2 1 1 −1 1 −1 −1 Yaw ※⾏列の成分の数値は⽐率で良い 𝑀" 逆⾏列が存在しない︕ マルチコプタの制御 109

Drone Control Workshop 逆⾏列が作れない場合 𝛅 = 𝐌𝛚 𝛚は無数に存在する 上式を満⾜する マルチコプタの制御 110

Drone Control Workshop 擬似逆⾏列を利⽤する 𝛅 = 𝐀𝛚 ⊺ ⊺ "𝟏 𝛚 = 𝐌 (𝐌𝐌 ) プロペラ ⾓速度 変換⾏列 擬似逆⾏列 マルチコプタの制御 𝛅 推⼒と ロール ピッチ ヨーの モーメント 111

Drone Control Workshop [参考]擬似逆⾏列について 𝛅 = 𝐌𝛚 の条件のもと 𝛚 𝟐 = 𝛚⊺ 𝛚 を最⼩にする𝛚を求める 𝐌 ⊺ 𝛌 = 2𝛚 𝐌𝑴⊺ 𝛌 = 2𝐌𝛚 𝐌𝐌 ⊺ 𝛌 = 2𝛅 𝛚 = 𝐌 ⊺ 𝐌𝐌 ⊺ 0$ 𝛅 擬似逆⾏列 𝛌 = 2 𝐌𝐌 ⊺ "# 𝛅 𝑚" ラグランジュの未定乗数法を⽤いる 𝐿 = 𝛚⊺ 𝛚 − 𝛌⊺ 𝐌𝛚 − 𝛅 𝜕𝐿 = 2𝛚 − 𝐌 ⊺ 𝛌 = 𝟎 𝜕𝛚 𝜕𝐿 = 𝑴𝛚 − 𝛅 = 𝟎 𝜕𝛌 𝑚% + 𝑚# = 1 𝑚! マルチコプタの制御 112

Drone Control Workshop Lesson 4 Exercise 1/3 • StampFlyの配置⾏列を求めなさい • 推⼒係数、トルク係数、ホバリング⾓速度を以下とする変換⾏列を 求めなさい • 推⼒計数 2.88e-8 Ns2/rad2 • トルク係数 4.53e-13 kgm2 • ホバリング⾓速度 7000rad/s • 変換⾏列を⽤いて式を展開し各モータごとの式に展開しなさい ※⾏列計算を⼿計算する必要はありません。使えるツールを使って楽をしてください マルチコプタの制御 113

Drone Control Workshop Lesson 4 Exercise 2/3 • コントローラの各スティックの倒し量が以下だとします • 推⼒ ∆𝑇 • ロールモーメント ∆𝐿 • ピッチモーメント ∆𝑀 • ヨーモーメント ∆𝑁 • ミキシングの式を使ってモータの回転量を決めてスティッ クの操作でドローンが上昇、ロール、ピッチ、ヨー回転す る様にプログラムを組みなさい マルチコプタの制御 114

Drone Control Workshop Lesson 4 Exercise 3/3 • プログラムはflight_control.cpp内のloop_400Hz()関数の中に ⾏いなさい • それ以外のファイルや関数の中⾝を書き換えてはいけない • flight_control.cpp内に関数を増やすのは可 • ソースファイル（.cpp）とヘッダファイル（.hpp）は必要 があれば増やしても良い マルチコプタの制御 115

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

Drone Control Workshop Lesson5を⾏う準備 VSCodeの 「開く」またはフォルダを開で lesson5フォルダを開く マルチコプタの制御 117

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

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

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

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

Drone Control Workshop 今回⽤意したIMU⽤の関数 imu_init(); 👈StampFlyのIMUを使える状態にする imu_update(); 👈加速度と⾓速度を更新するLoom_400Hz内で毎回 ⼀度だけ実⾏する imu_get_acc_x(); imu_get_acc_y(); imu_get_acc_z(); imu_get_gyro_x(); imu_get_gyro_y(); imu_get_gyro_z(); float型(実数型)で加速度の値を返す 返す値 -1〜1 float型(実数型)で⾓速度の値を返す 返す値 rad/s単位の⾓速度 マルチコプタの制御 122

Drone Control Workshop Lesson5 Exercise • 加速度三軸分と⾓速度三軸分をコンマで区切って表⽰しなさい • その際、⼀番最初の列に今の時刻を秒単位で合わせて表⽰しなさい • X軸周りの回転速度がある程度以上ならLEDを⾚く光らす • Y軸周りの回転速度がある程度以上ならLEDを緑に光らす • Z軸周りの回転速度がある程度以上ならLEDを⻘く光らす マルチコプタの制御 123

Drone Control Workshop モデルとシステム同定 Lesson 6 マルチコプタの制御 124

Drone Control Workshop モータプロペラ系モデル 𝑅 𝐿 インダクタンスは無視 モータプロペラの運動⽅程式 ⾓速度 𝜔 𝑒 𝐾𝜔 𝐾𝜔 モータ " 𝐽𝜔̇ + 𝐶! 𝜔 = 𝐾𝑖 𝐽 電気の⽅程式 𝑅𝑖 + 𝐾𝜔 = 𝑒 𝐾 逆起電⼒係数 マルチコプタの制御 125

Drone Control Workshop モータプロペラ系モデル モータプロペラの運動⽅程式 " 𝐽𝜔̇ + 𝐶! 𝜔 = 𝐾𝑖 電気の⽅程式 𝑅𝑖 + 𝐾𝜔 = 𝑒 （2）式を変形し（1）式に代⼊ マルチコプタの制御 126

Drone Control Workshop モータプロペラ系モデル モータプロペラの⾮線形モデル " 𝐾 𝐾 " 𝐽𝜔̇ + 𝜔 + 𝐶! 𝜔 = 𝑒 𝑅 𝑅 マルチコプタの制御 127

Drone Control Workshop モータプロペラ系モデルの線形化 " 𝐾 𝐾 " 𝐽𝜔̇ + 𝜔 + 𝐶! 𝜔 = 𝑒 𝑅 𝑅 𝜔 = 𝜔# + ∆𝜔 𝑒 = 𝑒# + ∆𝑒 マルチコプタの制御 ホバリングの状態からの変 化量という記述にする これをモデルに代⼊ 128

Drone Control Workshop モータプロペラ系モデルの線形化 ホバリング状態を表すつり合いの式 " 𝐾 𝐾 " 𝜔# + 𝐶! 𝜔# = 𝑒# 𝑅 𝑅 ホバリング状態は速度の変化は無い定常状態なので微分項は0となる マルチコプタの制御 129

Drone Control Workshop モータプロペラ系モデルの線形化 つり合いの式 # 𝐾 𝐾 # 𝜔! + 𝐶$ 𝜔! = 𝑒! 𝑅 𝑅 モータモデル 𝐾# 𝐾 # ̇ 𝐽∆𝜔 + 𝜔! + ∆𝜔 + 𝐶$ 𝜔! + ∆𝜔 = 𝑒! + ∆𝑒 𝑅 𝑅 この式はつりあいの式と照らし合わせて削除できる項があり簡単になる マルチコプタの制御 130

Drone Control Workshop モータプロペラ系モデルの線形化 線形化モータプロペラモデル # 𝐾 𝐾 ̇ + 𝐽∆𝜔 + 2𝐶$ 𝜔! ∆𝜔 = ∆𝑒 𝑅 𝑅 ホバリング状態からの微⼩な運動の変化を表す式 マルチコプタの制御 131

Drone Control Workshop モータプロペラ系の伝達関数導出 # 𝐾 𝐾 ̇ + 𝐽∆𝜔 + 2𝐶$ 𝜔! ∆𝜔 = ∆𝑒 𝑅 𝑅 ラプラス変換する # 𝐾 𝐾 𝐽𝑠Ω(𝑠) + + 2𝐶$ 𝜔! Ω(𝑠) = 𝐸(𝑠) 𝑅 𝑅 Ω(𝑠)について解くと マルチコプタの制御 132

Drone Control Workshop モータプロペラ系の伝達関数導出 Ω 𝑠 = 𝐾 𝑅 𝐾# 𝐽𝑠 + 𝑅 + 2𝐶$ 𝜔! マルチコプタの制御 𝐸(𝑠) 133

Drone Control Workshop モータプロペラ系の伝達関数導出 Ω 𝑠 = 𝐾 𝐽𝑅𝑠 + 𝐾 # + 2𝑅𝐶$ 𝜔! マルチコプタの制御 𝐸(𝑠) 134

Drone Control Workshop モータプロペラ系の伝達関数導出 𝐾 𝐾 " + 2𝑅𝐶' 𝜔( Ω 𝑠 = 𝐸(𝑠) 𝐽𝑅 𝑠+1 " 𝐾 + 2𝑅𝐶' 𝜔( ここで 𝐽𝑅 𝜏1 = % 𝐾 + 2𝑅𝐶# 𝜔! 𝐾 𝐾1 = % 𝐾 + 2𝑅𝐶# 𝜔! とすると マルチコプタの制御 135

Drone Control Workshop モータプロペラ系の伝達関数導出 モータプロペラ系の伝達関数 𝐾( Ω 𝑠 = 𝐸(𝑠) 𝜏( 𝑠 + 1 𝜏# モータプロペラ系の時定数 𝐾# モータプロペラ系のゲイン マルチコプタの制御 136

Drone Control Workshop Dutyと電圧の関係 ∆𝑒 = 𝑉+,! ∆𝑢 電源電圧 Duty ラプラス変換して伝達関数に代⼊すると 𝐾( 𝑉+,! Ω 𝑠 = 𝑈(𝑠) 𝜏( 𝑠 + 1 マルチコプタの制御 137

Drone Control Workshop 機体の回転運動モデル 前後左右対象のマルチコプタの回転運動の運動⽅程式 𝐼) 𝑝̇ + 𝐼* − 𝐼+ 𝑞𝑟 = 𝐿 𝐼+ 𝑞̇ + 𝐼) − 𝐼* 𝑟𝑝 = 𝑀 𝐼* 𝑟̇ + 𝐼+ − 𝐼) 𝑝𝑞 = 𝑁 マルチコプタの制御 138

Drone Control Workshop 回転運動モデルの線形化 前後左右対象のマルチコプタの回転運動の運動⽅程式 𝐼) 𝑝̇ + 𝐼* − 𝐼+ 𝑞𝑟 = 𝐿 𝐼+ 𝑞̇ + 𝐼) − 𝐼* 𝑟𝑝 = 𝑀 𝐼* 𝑟̇ + 𝐼+ − 𝐼) 𝑝𝑞 = 𝑁 𝑝 = 𝑝# + ∆𝑝 𝑞 = 𝑞# + ∆𝑞 𝑟 = 𝑟# + ∆𝑟 𝐿 = 𝐿# + ∆𝐿 ホバリング状態か らの微⼩変動につ いて考える 𝑀 = 𝑀# + ∆𝑀 𝑁 = 𝑁# + ∆𝑁 マルチコプタの制御 139

Drone Control Workshop 回転運動モデルの線形化 𝑝 = 𝑝# + ∆𝑝 𝑞 = 𝑞# + ∆𝑞 𝑟 = 𝑟# + ∆𝑟 ホバリング状態か らの微⼩変動につ いて考える 𝑝# = 𝑞# = 𝑟# = 0 𝐿# = 𝑀# = 𝑁# = 0 𝐿 = 𝐿# + ∆𝐿 𝑀 = 𝑀# + ∆𝑀 𝑁 = 𝑁# + ∆𝑁 マルチコプタの制御 140

Drone Control Workshop 回転運動モデルの線形化 運動⽅程式に微⼩変動分を代⼊ 𝐼) ∆𝑝̇ + 𝐼* − 𝐼+ ∆𝑞∆𝑟 = ∆𝐿 微⼩変動分同⼠の積はさらに無視できるほど微⼩と考えると 𝐼) ∆𝑝̇ = ∆𝐿 他も同様に考えると マルチコプタの制御 141

Drone Control Workshop 回転運動モデルの線形化 線形化された機体の運動⽅程式 𝐼) ∆𝑝̇ = ∆𝐿 𝐼+ ∆𝑞̇ = ∆𝑀 𝐼+ ∆𝑟̇ = ∆𝑁 マルチコプタの制御 142

Drone Control Workshop 回転運動の伝達関数 𝐼) ∆𝑝̇ = ∆𝐿 𝐼+ ∆𝑞̇ = ∆𝑀 𝐼+ ∆𝑟̇ = ∆𝑁 ラプラス変換 𝐼) 𝑠∆𝑝 = ∆𝐿 𝐼+ 𝑠∆𝑞 = ∆𝑀 𝐼* 𝑠∆𝑟 = ∆𝑁 11 ∆𝑝 = ∆𝐿 𝑠 𝐼) 11 ∆𝑞 = ∆𝑀 𝑠 𝐼+ 11 ∆𝑟 = ∆𝑁 𝑠 𝐼* マルチコプタの制御 143

Drone Control Workshop 推⼒・トルクとプロペラ⾓速度 ∆𝑇 = 2𝐶$ 𝜔% ∆𝜔& + ∆𝜔' + ∆𝜔( + ∆𝜔) ∆𝐿 = 2𝐶$ 𝜔% −𝑥& ∆𝜔& − 𝑥' ∆𝜔' + 𝑥( ∆𝜔( + 𝑥) ∆𝜔) ∆𝑀 = 2𝐶$ 𝜔% 𝑦& ∆𝜔& − 𝑦' ∆𝜔' − 𝑦( ∆𝜔( + 𝑦) ∆𝜔) ∆𝑁 = 2𝐶* 𝜔% ∆𝜔& − ∆𝜔' + ∆𝜔( − ∆𝜔) ラプラス変換すると マルチコプタの制御 144

Drone Control Workshop 推⼒・トルクとプロペラ⾓速度 ∆𝑇 = 2𝐶$ 𝜔% Ω& + Ω' + Ω( + Ω) ∆𝐿 = 2𝐶$ 𝜔% −𝑥& Ω& − 𝑥' Ω' + 𝑥( Ω( + 𝑥) Ω) ∆𝑀 = 2𝐶$ 𝜔% 𝑦& Ω& − 𝑦' Ω' − 𝑦( Ω( + 𝑦) Ω) ∆𝑁 = 2𝐶* 𝜔% Ω& − Ω' + Ω( − Ω) モータプロペラ系の伝達関数を代⼊すると マルチコプタの制御 145

Drone Control Workshop 推⼒・トルクとDuty 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑇 = 𝑈$ + 𝑈% + 𝑈& + 𝑈' 𝜏1 𝑠 + 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝐿 = −𝑥$ 𝑈$ − 𝑥% 𝑈% + 𝑥& 𝑈& + 𝑥' 𝑈' 𝜏1 𝑠 + 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑀 = 𝑦$ 𝑈$ − 𝑦% 𝑈% − 𝑦& 𝑈& + 𝑦' 𝑈' 𝜏1 𝑠 + 1 2𝐶# 𝜔! 𝐾1 𝑉23" ∆𝑁 = 𝑈$ − 𝑈% + 𝑈& − 𝑈' 𝜏1 𝑠 + 1 マルチコプタの制御 回転の運動⽅程式に代⼊すると 146

Drone Control Workshop 回転の運動の伝達関数 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑇 = 𝑈$ + 𝑈% + 𝑈& + 𝑈' 𝜏1 𝑠 + 1 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑝 = −𝑥$ 𝑈$ − 𝑥% 𝑈% + 𝑥& 𝑈& + 𝑥' 𝑈' 𝐼4 𝑠 𝜏1 𝑠 + 1 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑞 = 𝑦$ 𝑈$ − 𝑦% 𝑈% − 𝑦& 𝑈& + 𝑦' 𝑈' 𝐼5 𝑠 𝜏1 𝑠 + 1 1 2𝐶# 𝜔! 𝐾1 𝑉23" ∆𝑟 = 𝑈$ − 𝑈% + 𝑈& − 𝑈' 𝐼6 𝑠 𝜏1 𝑠 + 1 マルチコプタの制御 147

Drone Control Workshop 回転の運動の伝達関数 ここで新たな制御⼊⼒を 𝛿$ = 𝑈! + 𝑈" + 𝑈% + 𝑈& 𝛿' = −𝑥!𝑈! − 𝑥"𝑈" + 𝑥%𝑈% + 𝑥&𝑈& 𝛿( = 𝑦!𝑈! − 𝑦"𝑈" − 𝑦%𝑈% + 𝑦&𝑈& 𝛿) = 𝑈! − 𝑈" + 𝑈% − 𝑈& とすると マルチコプタの制御 148

Drone Control Workshop 回転の運動の伝達関数 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑇 = 𝛿" 𝜏1 𝑠 + 1 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑝 = 𝛿7 𝐼4 𝑠 𝜏1 𝑠 + 1 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑞 = 𝛿8 𝐼5 𝑠 𝜏1 𝑠 + 1 1 2𝐶# 𝜔! 𝐾1 𝑉23" ∆𝑟 = 𝛿9 𝐼6 𝑠 𝜏1 𝑠 + 1 マルチコプタの制御 149

Drone Control Workshop 新たな制御⼊⼒とDutyとのミキシング 𝛿$ 1 𝛿+ −𝑥& = 𝛿, 𝑦& 1 𝛿- 𝛅 = 各軸の制御⼊⼒ 1 −𝑥' −𝑦' −1 1 𝑥( −𝑦( 1 𝐀 配置⾏列 マルチコプタの制御 1 𝑥) 𝑦) −1 𝑈& 𝑈' 𝑈( 𝑈) 𝐔 Duty 150

Drone Control Workshop 新たな制御⼊⼒とDutyとのミキシング !" 𝐔=𝐀 𝛅 !" 𝐀 をミキシング⾏列と呼びます この場合は新たな制御⼊⼒とDutyとの関連付けをミキシングと呼んでいる。 ここから三軸回りと上下運動は独⽴して制御できることがわかる マルチコプタの制御 151

Drone Control Workshop 伝達関数についてのまとめ 回転運動の伝達関数 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑝 = 𝛿7 𝐼4 𝑠 𝜏1 𝑠 + 1 ただし 1 2𝐶" 𝜔! 𝐾1 𝑉23" ∆𝑞 = 𝛿8 𝐼5 𝑠 𝜏1 𝑠 + 1 𝐾 𝐾1 = % 𝐾 + 2𝑅𝐶# 𝜔! 𝐽𝑅 𝜏1 = % 𝐾 + 2𝑅𝐶# 𝜔! 1 2𝐶# 𝜔! 𝐾1 𝑉23" ∆𝑟 = 𝛿9 𝐼6 𝑠 𝜏1 𝑠 + 1 これらの伝達関数にもとづいて 制御システムを設計していきます マルチコプタの制御 152

Drone Control Workshop モータの内部構造 マルチコプタの制御 153

Drone Control Workshop モータコイルの概形 重量 抵抗 12.4 0.58g 0.65ohm 6.5 21 慣性モーメント3.3e-8 kgm2 マルチコプタの制御 軸径 0.8 コイル外径 6 コイル厚 0.65 単位 mm 154

Drone Control Workshop プロペラ 全重量 ハブ ⽻4枚 0.36g 0.18g 0.18g 7.4 30.6 マルチコプタの制御 155

Drone Control Workshop Lesson6 Exercise • これまでたくさんの数式が出てきましたが今後必要になるのは 伝達関数です。「伝達関数についてのまとめ」ページに出てく る全ての記号について何のことなのか確認してください • それらの記号において定数についてはその値を明らかにする必 要があります。それをシステム同定といいますがどうやったそ れらを調べることができるかグループで話し合い⽅法を⾒つけ てください マルチコプタの制御 156

Drone Control Workshop PID制御と設計 Lesson 7 マルチコプタの制御 157

Drone Control Workshop 簡単な制御システムの一例 ⾓速度制御システムの簡単な例を以下に⽰します。⾓速度をPID制御で安定化しています。制御 ⼊⼒は最終的にはミキシングして各モータに分配します。スロットルに関しては操縦装置からの 信号に適当にゲインをかけて𝛿! とします。 コントローラ 𝐾!# 角速度指令値 − 機体 𝐾!" 𝑠 𝛿$ 𝐾! 𝑠(𝜏𝑠 + 1) 𝑞 𝐾!" 𝑠 ピッチ角速度制御系（ロール、ヨーも同様） スロットル指令値 𝐾%# 𝛿% スラスト制御系 マルチコプタの制御 158

Drone Control Workshop MATLABのインストール 情報処理サービスセンターのページにアクセスしてMATLABをインス トールしてください マルチコプタの制御 159

Drone Control Workshop 伝達関数を作る 1 𝐺 𝑠 = ' 3𝑠 + 2𝑠 + 1 num=1 den=[3 2 1] sys = tf(num,den) マルチコプタの制御 160

Drone Control Workshop ボード線図を描く bode(sys) マルチコプタの制御 161

Drone Control Workshop ロールレートの伝達関数 1 2𝐶" 𝜔! 𝐾) 𝑉*+" 𝐺(𝑠) = 𝐼( 𝑠 𝜏) 𝑠 + 1 2𝐶" 𝜔! 𝐾) 𝑉*+" = 𝐼( 𝜏) 𝑠 # + 𝐼( 𝑠 マルチコプタの制御 162

Drone Control Workshop PID制御器 𝐶 𝑠 = 𝐾, 1 1 + 𝑇- 𝑠 + 𝑇. 𝑠 𝐾, (𝑇- 𝑠 " + 𝑠 + 1⁄𝑇. ) = 𝑠 マルチコプタの制御 163

Drone Control Workshop ⼀巡伝達関数 𝐿 𝑠 = 𝐶 𝑠 𝐺(𝑠) MATLAB sys_L= sys_C*sys_G マルチコプタの制御 164

Drone Control Workshop MATLABフィードバック結合 ＋ G1 ー MATLAB G2 sys=feedback(G1,G2) マルチコプタの制御 165

Drone Control Workshop 位相余裕・ゲイン余裕 MATLAB ゲイン余裕 margin(sys) 位相余裕 マルチコプタの制御 166

Drone Control Workshop ボード線図について 伝達関数の掛け算はボード線図上では⾜し算です + = G(s) C(s) C(s)G(s) 制御対象 制御器 ⼀巡伝達関数 マルチコプタの制御 167

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

Drone Control Workshop Lesson6 Exercise • ⾓速度制御系の⼀巡伝達関数をMATLABで作成しなさい（三軸と も） • ⼀巡伝達関数のボード線図を描きなさい • PとIとDのゲインをそれぞれ変えて⼀巡伝達関数を書き換えて以 下の様になるまで調整を繰り返しなさい • 位相余裕が40度〜60度 • できるだけゲイン交差周波数を⼤きく マルチコプタの制御 169

Drone Control Workshop 姿勢の推定 Lesson 8 マルチコプタの制御 170

Drone Control Workshop 姿勢の推定 • 重⼒・地磁気による⽅向の推定 • ⾓速度の積分 • カルマンフィルタ 弊ブログも参考にしてください • 推定誤差の分散を最⼩化 • Madgwickフィルタ • 最急降下法で予測（複数の推定を重み付で合成） • 相補フィルタ • 加速度からの推定と⾓速度からの推定をそれぞれローパス・ハイパス フィルタを通してから合成 マルチコプタの制御 171

Drone Control Workshop ルドルフ・カルマン １９３０年ハンガリのブタペスト⽣まれのア メリカの学者（２０１６年没） １９６０年、⼤学院の時にカルマンフィルタ を考案する。 アポロ宇宙船の軌道推定に使⽤され，その実 ⽤性が⾼く評価される。 Wikipediaより転載 現代制御理論の⽣みの親

Drone Control Workshop 加速度センサによる姿勢の推定 重⼒加速度のx軸成分 重⼒加速度のy軸成分 x軸 z軸 重⼒加速度 マルチコプタの制御 173

Drone Control Workshop 加速度センサによる姿勢の推定 重⼒加速度のx軸成分 重⼒加速度のy軸成分 z軸 θ 重⼒加速度 x軸 𝑔 ) /! y軸周り 𝜃 = tan 𝑔* y軸が地⾯に⽔平だとθは計算できそうだが ⽔平でない場合はどうか︖ 他の軸周りの⾓度は︖ マルチコプタの制御 174

Drone Control Workshop 姿勢の表現 • オイラー⾓ （3） • クォータニオン （4） • ⽅向余弦⾏列 （5） 弊ブログも参考にしてください マルチコプタの制御 175

Drone Control Workshop 176

Drone Control Workshop 戦闘機の座標系の各軸のベクトル 戦闘機のX軸の ベクトル成分 （⼤きさ1） 𝑒!) 𝒆! = 𝑒!+ 𝑒!* 戦闘機のY軸の ベクトル成分 （⼤きさ1） 𝑒") 𝒆" = 𝑒"+ 𝑒"* 戦闘機のZ軸の ベクトル成分 （⼤きさ1） 𝑒#) 𝒆# = 𝑒#+ 𝑒#* これらは全て地球の座標系から⾒たベクトル 177

Drone Control Workshop ⽅向余弦⾏列 𝑒!) 𝐄 = 𝑒!+ 𝑒!* 戦闘機のX軸の ベクトル成分 𝑒") 𝑒"+ 𝑒"* 戦闘機のY軸の ベクトル成分 𝑒#) 𝑒#+ 𝑒#* 戦闘機のZ軸の ベクトル成分 この⾏列をもって戦闘機の姿勢（⾓度）とする 178

Drone Control Workshop 座標変換⾏列（座標を回す⾏列） Ｚ軸周り cos 𝜓 sin 𝜓 0 𝐂: (𝜓) = − sin 𝜓 cos 𝜓 0 0 0 1 Y軸周り cos 𝜃 0 − sin 𝜃 𝐂; (𝜃) = 0 1 0 sin 𝜃 0 cos 𝜃 X軸周り 1 0 0 𝐂< (𝜙) = 0 cos 𝜙 sin 𝜙 0 − sin 𝜙 cos 𝜙 マルチコプタの制御 弊ブログも参考にしてください 179

Drone Control Workshop 地球座標系からセンサ座標へ 地球座標から機体座標への座標変換を表す式を記述します Ｚ軸周り 𝑥# 𝑥% 𝑦# = 𝐂$ (𝜓) 𝑦% 𝑧# 𝑧% Y軸周り 𝑥& 𝑥# 𝑦& = 𝐂' (𝜃) 𝑦# 𝑧& 𝑧# X軸周り 𝑥( 𝑥& 𝑦( = 𝐂) (𝜙) 𝑦& 𝑧( 𝑧& センサ座標のベクトル 地球座標のベクトル 𝑥& 𝑥! 𝑦& = 𝐂< 𝜙 𝐂; (𝜃)𝐂: (𝜓) 𝑦! 𝑧& 𝑧! 𝑥! = 𝐄(𝜙, 𝜃, 𝜓) 𝑦! 𝑧! 回転の座標変換⾏列（⽅向余弦⾏列） 𝐄 𝜙, 𝜃, 𝜓 = 𝐂< (𝜙)𝐂; (𝜃)𝐂: (𝜓) ここで出てる⾓度をオイラー⾓と⾔います オイラー⾓は座標を回す順で違ってきます マルチコプタの制御 180

Drone Control Workshop 重⼒の座標変換 𝑔4 0 𝑔5 = 𝐂< 𝜙 𝐂; (𝜃)𝐂: (𝜓) 0 𝑔6 𝑔 0 = 𝐄(𝜙, 𝜃, 𝜓) 0 𝑔 センサから得られるのは 𝑔4 マルチコプタの制御 𝑔5 𝑔6 7 181

Drone Control Workshop ⽅向余弦⾏列の性質 ⽅向余弦⾏列は回転⾏列なので素晴らしい性質があります • 実対象⾏列 • 逆⾏列が転置⾏列に等しい マルチコプタの制御 182

Drone Control Workshop 逆変換 𝑔4 0 𝑔5 = 𝐄(𝜙, 𝜃, 𝜓) 0 𝑔6 𝑔 𝑔4 0 0 = 𝐄 0$ (𝜙, 𝜃, 𝜓) 𝑔5 𝑔6 𝑔 𝑔4 0 0 = 𝐄 F (𝜙, 𝜃, 𝜓) 𝑔5 𝑔6 𝑔 マルチコプタの制御 183

Drone Control Workshop サンプルプログラムの説明 数学の関数が使える様になりました プログラム冒頭に #include <math.h> があります． C⾔語の数学関数は調べてください マルチコプタの制御 184

Drone Control Workshop Lesson8 Exercise • 以上から加速度センサからの値からZYXオイラー⾓を計算するアルゴリズ ムを⾒つけなさい • ネットから直接答えを探すことは不可 • 三⾓関数の確認など周辺知識の確認は良い • StampFlyの加速度センサ値を使ってオイラー各表現の傾斜計を作りなさい • 最後に検討中のエピソード難しさ気がついたことを発表してもらうので各 ⼈ごと常にメモをして班内で共有してまとめておいてください • Teleplotと⾔うアドインをVSCodeに⼊れるとシリア ル通信データでグラフを書けるそうなので調べて⼊ れて姿勢推定結果を可視化してください マルチコプタの制御 185

Drone Control Workshop フライト制御システム実装 Lesson 9 マルチコプタの制御 186

Drone Control Workshop いよいよプログラムです︕ ここまでの知識をフル活⽤ して⾃分のフライトプログ ラムを作りましょう︕ マルチコプタの制御 187

Drone Control Workshop プログラムの⽬的 StampFlyの３軸のジャイロセンサから の⾓速度をフィードバックして⾓速度 を制御し⾶⾏することを実現する フライトコントロールプログラムを作 成する マルチコプタの制御 188

Drone Control Workshop 処理の内容と流れの概要 setup関数にて 各種グローバル変数の初期化 各種機能の初期化 ジャイロバイアス（オフセット）の軽減 その他必要なこと loop_400Hz関数にて モード分岐 送信機からの信号うけとりと処置 ⾓速度の受け取り PID制御とフィルタ処理 各モータへのDutyのセット LEDの処理など マルチコプタの制御 189

Drone Control Workshop ジャイロバイアスの軽減 ジャイロバイアスは回転運動をしていない際にも⾓速度が発⽣している様に信号が出て しまいます。気温や時と共にその量は変化しますがその変化はゆっくりですので、ある 程度の時間信号を取得して平均値を取得し毎回、測定値から平均値を引くことで軽減で きます。 制御に使⽤する⾓速度 ＝ センサからの⽣の値 ー マルチコプタの制御 あらかじめ取得したバイアスの平均値 190

Drone Control Workshop モード分岐 Arm（⾶⾏可能）状態 Disarm（安全）状態 モータ停⽌ 各種変数の初期化 PIDの積分リセット [右Stickボタン押された] ⾶⾏制御ループの実⾏ [右Stickボタン押された] [⼤きな加速度を検知した] マルチコプタの制御 191

Drone Control Workshop ロール⾓速度制御（他も同様） ⽬標⾓速度 StampS3 ESP-NOW ＋ ー PID制御 ミキシ ング Gyro Transmitter ATOMS3 ESP-NOW 機体 モータ ロール ⾓速度 ⾓速度取得 ※⾚い枠のついている部分のプログラムを作成する マルチコプタの制御 192

Drone Control Workshop PID制御の基本的な実装 誤差 ℎ 刻み幅（制御周期） 𝑦-./ ⽬標値 𝑒0 = 𝑦123 0 − 𝑦0 0 数値積分（積分器） 𝑦0 𝑠0 = 𝑠0/! + 𝑒0 ℎ センサ出⼒ 数値微分 𝑑𝑒0 = (𝑒0 − 𝑒0/! )⁄ℎ PID制御の操作量 𝑢0 = 𝐾, (𝑒0 + 𝑠0 ⁄𝑇. + 𝑇- 𝑑𝑒0 ) マルチコプタの制御 193

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

Drone Control Workshop PID制御の実装形態 pid.cppファイル pid.hppファイル #include “pid.hpp” PID制御⽤の構造体の定義 float pid(pid_t p) { //PIDの計算をして制御⼊⼒を返す関数 } 外部に公開する変数のextern宣⾔ 外部に公開する関数の宣⾔ float pid(pid_t p); PID制御器の実装は必ずぺつファイルで⾏ってください マルチコプタの制御 195

Drone Control Workshop フィルタ処理の基本 • モータにノイズの⼤きいDutyを⼊れると発熱や故障の原因となる • PIDの微分はノイズを増⼤するためフィルタをかけた⽅が良い • かけすぎると遅れが顕著になり不都合もある フィルタの式 𝑦$ = (1 − 𝐾)𝑦$%& + 𝐾𝑢$ ノイズの激しい信号𝑢* をフィルタリングして𝑦* とする フィルタリングゲイン𝐾を0〜1の範囲で実験しながら適当に決める 𝐾が⼤きいと今のセンサの値を信頼し⼩さいと過去の値を重めに採⽤する //コード例 y=(1-K)*y + K*u マルチコプタの制御 196

Drone Control Workshop サンプルプログラムについて imu_update() FastLED.show() 以外は書いてありません． setupでは必要なinitはすべて書いてあ りますが 必要な初期化は⾃分たちで書き加え てください マルチコプタの制御 197

Drone Control Workshop プログラム作成時のアドバイス • 全てのプログラムを１箇所に集中して書くことをやめる • どんな流れでどんな処理が必要かをあらかじめ書き留めてからはじ める • 必要な処理がわかってきて機能としてまとめられるのなら関数とし て実装する • 全体の流れを好きな⽅法で図的に書いておく • 関数の名前を決めておいて最初に空っぽの関数の定義を並べて書い ておく • メインに部分に関数を並べて空っぽだけど流れがわかる⾻組み（ス ケルトン）をつくる（この時点でコンパイルが通るように書いてい ると⼤変良い） マルチコプタの制御 198

Drone Control Workshop Lesson 9 Exercise • グループ作業 • フライト制御プログラムに必要な機能を話し合いなさい • その機能はさらに下請け機能が必要ではないか考えなさい • 機能を関数にするために関数の名前をつけなさい • 関数の戻り値（あれば）と引数（あれば）考えなさい • 関数の中⾝の具体はこの時点では考えなくて良いが何をするかは決めておく • フライト制御プログラムの流れをsetup関数内とloop_400Hz関数内とで分けて 考え、流れの図解を作成し全員で共有しなさい • スケルトンを作成し、コンパイルが通ることを全員で確認する • 個⼈またはグループ作業 • 関数の中⾝を具体的に書いていく • 最初に決めた事に不具合があればグループ内で情報共有する • 定期的な情報交換 マルチコプタの制御 199

Drone Control Workshop おしまい 今は、これが精⼀杯 この資料作りを献⾝的に⽀えてくれた妻に感謝したいと思います マルチコプタの制御 200