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

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

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

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

Drone Control Workshop [参考]擬似逆⾏列について 𝛅 = 𝐌𝛚 の条件のもと 𝛚 𝟐 = 𝛚⊺ 𝛚 を最⼩にする𝛚を求める 𝐌 ⊺ 𝛌 = 2𝛚 𝐌𝑴⊺ 𝛌 = 2𝐌𝛚 𝐌𝐌 ⊺ 𝛌 = 2𝛅 𝛌 = 2 𝐌𝐌 ⊺ 𝛚 = 𝐌 ⊺ 𝐌𝐌 ⊺ 0$ 𝛅 擬似逆⾏列 "# 𝛅 𝑚" ラグランジュの未定乗数法を⽤いる 𝐿 = 𝛚⊺ 𝛚 − 𝛌⊺ 𝐌𝛚 − 𝛅 𝜕𝐿 = 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 𝑉23" ∆𝑟 = 𝛿9 𝐼6 𝑠 𝜏1 𝑠 + 1 ただし 𝐾 𝐾1 = % 𝐾 + 2𝑅𝐶# 𝜔! 𝐽𝑅 𝜏1 = % 𝐾 + 2𝑅𝐶# 𝜔! これらの伝達関数にもとづいて 制御システムを設計していきます マルチコプタの制御 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ファイル #include “pid.hpp” float pid(pid_t p) { //PIDの計算をして制御⼊⼒を返す関数 } pid.hppファイル 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