20241026StampFly_Seminarドクセル版

StampFly Seminar Peatix オンラインイベント M5Stack Stampfly 開発ストーリー と今後の可能性: 2024年10月26日（土） 19:30 こうへい

StampFly Seminar StampFly開発の経緯 マルチコプタの制御 10

StampFly Seminar

StampFly Seminar これがきっかけ

StampFly Seminar

StampFly Seminar 2022/8/26 到着 2022/8/30 初フライトテスト

StampFly Seminar 2022/9/3 フライトテスト

StampFly Seminar

StampFly Seminar

StampFly Seminar

StampFly Seminar

StampFly Seminar ドローンの制御 マルチコプタの制御 21

StampFly Seminar マルチコプタの制御 マルチコプタの制御 22

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

StampFly Seminar 課題みたいな • モータが回せる • センサの値が読み込める • 通信ができる • 送信機の指令が受信できる • 各種センサの値が受信できる（テレメ） マルチコプタの制御 24

StampFly Seminar 課題みたいな • モータが回せる • センサの値が読み込める • 通信ができる これらができれば • 送信機の指令が受信できる • 各種センサの値が受信できる（テレメ） 誰でも飛ばせない？ マルチコプタの制御 25

StampFly Seminar 仮説 ジャイロで角速度取れるが、そこで終了？ そもそもジャイロで取れる角速度って何だかわからない？ ジャイロで角度を取れると考えられていないか？ マルチコプタの飛行原理がよくわからない マルチコプタの制御 26

StampFly Seminar 知っておいた方が良いこと マルチコプタの制御 27

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

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

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

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

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

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

StampFly Seminar 結論 マルチコプタで制御したい量 マルチコプタの回転の速度 マルチコプタの傾きや方向 マルチコプタの制御 34

StampFly Seminar 問題：回転速度や傾きの取得方法は 解答 IMUから加速度と角速度を得る 傾きはそれらの値から推定する マルチコプタの制御 35

StampFly Seminar IMUセンサ 加速度センサ 角速度センサ マルチコプタの制御 36

StampFly Seminar BMI270の軸の取り方 マルチコプタの制御 37

StampFly Seminar 航空工学での軸の取り方 𝜔 Y X 𝑉 Z マルチコプタの制御 38

StampFly Seminar IMUの軸の取り方との比較 𝜔 Y 前 X 𝑉 Z マルチコプタの制御 39

StampFly Seminar ジャイロでは傾きは取れない ？ マルチコプタの制御 40

StampFly Seminar 加速度センサ おもり 正の方向に加速すると この曲がりを検知し て加速度とする 加速度 X X 正方向 正方向 マルチコプタの制御 41

StampFly Seminar 加速度センサでの重力加速度の検知 重力があると この様に反る 動的加速度によって この様に反る 正の方向の動的加速度 正方向 動的加速度と 反りが反対 静止 重力 軸が下向きの場合は重力（静的加速度）は正の方向に働 いてもマイナスの値として検出される マルチコプタの制御 42

StampFly Seminar 加速度センサによる姿勢の推定 重力加速度のy軸成分 𝑔𝑦 重力加速度のz軸成分 𝑔𝑧 z軸 y軸 重力加速度 マルチコプタの制御 43

StampFly Seminar 加速度センサによる姿勢の推定 ロール角 重力加速度のy軸成分 𝑔𝑦 重力加速度のz軸成分 𝑔𝑧 z軸 𝜙 y軸 𝑔 −1 𝑦 𝜙 = tan 𝑔𝑧 ピッチ角（参考） 𝜃 = − tan−1 重力加速度 𝑔𝑥 cos 𝜙 𝑔𝑧 ロールとピッチの傾きだけがわかる マルチコプタの制御 44

StampFly Seminar 参考：姿勢（オイラー角）と3軸角速度 𝜙ሶ = 𝑝 + 𝑞 sin 𝜙 tan 𝜃 + 𝑟 cos 𝜙 tan 𝜃 𝜃ሶ = 𝑞 cos 𝜙 − 𝑟 sin 𝜙 𝜓ሶ = 𝑞 sin 𝜙Τcos 𝜃 + 𝑟 cos 𝜙Τcos 𝜃 θ＝π/2に不連続点がある。（ジンバルロックが起きる時） マルチコプタの制御 45

StampFly Seminar 角度の推定 ジャイロから得られた 角速度 積分 角度 加速度計から得られた 加速度 逆三角関数 角度 マルチコプタの制御 46

StampFly Seminar 角度の推定 ジャイロから得られた 角速度 積分 角度 オフセットが積分されて 時間と共に増大 加速度計から得られた 加速度 逆三角関数 角度 ノイズ成分が大きい マルチコプタの制御 47

StampFly Seminar 角度の推定の実際 ジャイロから得られた 角速度 ハイパス フィルタ 積分 重み ＋ 加速度計から得られた 加速度 ローパス フィルタ 逆三角関数 マルチコプタの制御 角度 重み 48

StampFly Seminar 角度推定の実際 • 相補フィルタ • Extended Kalman Filter • Madgwick Filter マルチコプタの制御 49

StampFly Seminar 回転運動の源 𝑉𝑐 マルチコプタの制御 50

StampFly Seminar プロペラの発生する 推力とトルク 𝑇 推力 Thrust プロペラの角速度 𝜔 プロペラの回転方向と トルクの向きは逆です トルク Torque 𝑄 マルチコプタの制御 51

StampFly Seminar 推力とトルクの算出式 2 𝑇 = 𝐶𝑇 𝜔 𝑑𝜔 2 𝑄 = 𝐶𝑄 𝜔 + 𝐽 𝑑𝑡 記号の説明 𝐶𝑇 推力係数 𝐶𝑄 トルク係数 𝐽 ロータ・プロペラの慣性モーメント マルチコプタの制御 52

StampFly Seminar モータプロペラ系モデル 𝑅 𝐿 インダクタンスは無視 モータプロペラの運動方程式 角速度 𝜔 𝑒 𝐾𝜔 𝐾𝜔 モータ 2 𝐽𝜔ሶ + 𝐶𝑄 𝜔 = 𝐾𝑖 𝐽 電気の方程式 𝑅𝑖 + 𝐾𝜔 = 𝑒 𝐾 逆起電力係数 マルチコプタの制御 53

StampFly Seminar モータプロペラ系モデル モータプロペラの非線形モデル 2 𝐾 𝐾 2 𝐽𝜔ሶ + 𝜔 + 𝐶𝑄 𝜔 = 𝑒 𝑅 𝑅 マルチコプタの制御 54

StampFly Seminar 推力（N） 電圧と推力 電圧（V） マルチコプタの制御 55

StampFly Seminar 𝑇 = 𝑇0 + 𝐶𝑇𝑒 ∆𝑒 𝑄 = 𝑄0 + 𝐶𝑄𝑒 ∆𝑒 推力（N） トリム点周りで線形とみる 𝑇0 部分的には線形 𝐶𝑇𝑒 ∆𝑒 ∆𝑒 𝑒0 電圧（V） マルチコプタの制御 56

StampFly Seminar 機体に働く力とモーメント マルチコプタの制御 57

StampFly Seminar 𝑇ℎ𝑟𝑢𝑠𝑡 𝑇4 𝑇1 𝑇3 𝑇2 推力の配分 𝑇ℎ𝑟𝑢𝑠𝑡 = 𝑇1 + 𝑇2 + 𝑇3 + 𝑇4 マルチコプタの制御 58

StampFly Seminar Left Right 𝑇4 𝑇1 𝑇3 𝑇2 𝑥4 𝑥1 𝑥3 𝑥2 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 推力の配分 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 = −𝑥1 𝑇1 − 𝑥2 𝑇2 + 𝑥3 𝑇3 + 𝑥4 𝑇4 マルチコプタの制御 59

StampFly Seminar 𝑄4 𝑄1 トルクの発生の向き はプロペラの回転方 向とは逆なので注意 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 𝑄3 𝑄2 トルクの配分 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑄1 − 𝑄2 + 𝑄3 − 𝑄4 マルチコプタの制御 60

StampFly Seminar 機体に働く推力とモーメント 𝑇ℎ𝑟𝑢𝑠𝑡 = 𝑇1 + 𝑇2 + 𝑇3 + 𝑇4 𝑅𝑜𝑙𝑙 𝑚𝑜𝑚𝑒𝑛𝑡 = −𝑥1 𝑇1 − 𝑥2 𝑇2 + 𝑥3 𝑇3 + 𝑥4 𝑇4 𝑃𝑖𝑡𝑐ℎ 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑦1 𝑇1 − 𝑦2 𝑇2 − 𝑦3 𝑇3 + 𝑦4 𝑇4 𝑌𝑎𝑤 𝑚𝑜𝑚𝑒𝑛𝑡 = 𝑄1 − 𝑄2 + 𝑄3 − 𝑄4 マルチコプタの制御 61

StampFly Seminar 機体に働く推力とモーメント ∆𝑇 = 𝐶𝑇𝑒 ∆𝑒1 + 𝐶𝑇𝑒 ∆𝑒2 + 𝐶𝑇𝑒 ∆𝑒3 + 𝐶𝑇𝑒 ∆𝑒4 ∆𝐿 = −𝑥1 𝐶𝑇𝑒 ∆𝑒1 − 𝑥2 𝐶𝑇𝑒 ∆𝑒2 + 𝑥3 𝐶𝑇𝑒 ∆𝑒3 + 𝑥4 𝐶𝑇𝑒 ∆𝑒4 ∆𝑀 = 𝑦1 𝐶𝑇𝑒 ∆𝑒1 − 𝑦2 𝐶𝑇𝑒 ∆𝑒2 − 𝑦3 𝐶𝑇𝑒 ∆𝑒3 + 𝑦4 𝐶𝑇𝑒 ∆𝑒4 ∆𝑁 = 𝐶𝑄𝑒 ∆𝑒1 − 𝐶𝑄𝑒 ∆𝑒2 + 𝐶𝑄𝑒 ∆𝑒3 − 𝐶𝑄𝑒 ∆𝑒4 マルチコプタの制御 62

StampFly Seminar ベクトル・行列部分を文字で置き換え 𝐶𝑇𝑒 0 0 0 1 1 ∆𝑇 0 𝐶𝑇𝑒 0 0 −𝑥1 −𝑥2 ∆𝐿 = 0 0 𝐶𝑇𝑒 0 𝑦1 −𝑦2 ∆𝑀 0 0 0 𝐶𝑄𝑒 ∆𝑁 1 −1 𝛅 = 𝛅 = 推力 モーメント 𝐂 1 𝑥3 −𝑦3 1 𝐀 係数行列 𝐌 マルチコプタの制御 1 𝑥4 𝑦4 −1 配置行列 ∆𝑒1 ∆𝑒2 ∆𝑒3 ∆𝑒4 𝒆 𝒆 入力行列 63

StampFly Seminar 𝛅 = 𝒆 = 𝐀 𝐂 −𝟏 𝐀 𝐂 マルチコプタの制御 −𝟏 𝒆 𝛅 64

StampFly Seminar ミキシングの式 ∆𝑒1 1 ∆𝑒2 −𝑥1 = ∆𝑒3 𝑦1 ∆𝑒4 1 1 −𝑥2 −𝑦2 −1 1 𝑥3 −𝑦3 1 1 −1 1Τ𝐶𝑇𝑒 0 𝑥4 0 𝑦4 0 −1 マルチコプタの制御 0 1Τ𝐶𝑇𝑒 0 0 0 0 1Τ𝐶𝑇𝑒 0 0 0 0 1Τ𝐶𝑄𝑒 ∆𝑇 ∆𝐿 ∆𝑀 ∆𝑁 65

StampFly Seminar ミキシングの式 ∆𝑒1 1 ∆𝑒2 −1 = ∆𝑒3 1 ∆𝑒4 1 1 −1 −1 −1 1 1 −1 1 1 −1 1Τ𝐶𝑇𝑒 0 1 0 1 −1 0 マルチコプタの制御 0 1Τ𝑑𝐶𝑇𝑒 0 0 0 0 1Τ𝑑𝐶𝑇𝑒 0 0 0 0 1Τ𝐶𝑄𝑒 ∆𝑇 ∆𝐿 ∆𝑀 ∆𝑁 66

StampFly Seminar 各軸の独立制御 ミキシングの式を用いることにより各軸を 独立して制御することが可能となる ドローンに働く推力・モーメントとモータ に与える電圧との関係が明らかになった マルチコプタの制御 67

StampFly Seminar アクロモードの制御 目標角速度 StampS3 ESP-NOW Transmitter ATOMS3 ESP-NOW + - Rate Controller StampFly ロール ピッチ ヨー 角速度 Gyro ※赤い枠のついている部分のプログラムを作成する マルチコプタの制御 68

StampFly Seminar スタビライズモードの制御 ロール ピッチ ヨー角 速度 目標角度 StampS3 ESP-NOW Transmitter ATOMS3 + - Angle Controller + - Rate Controller Angle Estimater StampFly 1 𝑠 ロール ピッチ ヨー角 Gyro Accel ESP-NOW マルチコプタの制御 69

StampFly Seminar PID制御について マルチコプタの制御 70

StampFly Seminar 不完全微分付きのPID補償器 𝑐 𝑠 = 𝐾𝑝 1 𝑇𝑑 𝑠 1+ + 𝑇𝑖 𝑠 𝜂𝑇𝑑 𝑠 + 1 双1次変換で離散化 双1次変換とは次の変換です 2(𝑧 − 1) 𝑠= 𝑡𝑠 (𝑧 + 1) PID補償器を一変に離散化するのは 骨が折れるので積分と微分の部分 にわけてやっていきます。 積分補償器の離散化 𝑦𝑘 = 𝑡𝑠 (𝑧 + 1) 𝑒 2𝑇𝑖 (𝑧 − 1) 𝑘 2𝑇𝑖 (𝑧 − 1)𝑦𝑘 = 𝑡𝑠 (𝑧 + 1)𝑒𝑘 2𝑇𝑖 (𝑦𝑘+1 − 𝑦𝑘 ) = 𝑡𝑠 (𝑒𝑘+1 + 𝑒𝑘 ) 𝑡𝑠 𝑦𝑘+1 = 𝑦𝑘 + (𝑒 + 𝑒𝑘 ) 2𝑇𝑖 𝑘+1 ※これは台形積分と同じです 不完全微分補償器の離散化 2(𝑧 − 1) 𝑡𝑠 (𝑧 + 1) 𝑦𝑘 = 𝑒𝑘 2(𝑧 − 1) 𝜂𝑇𝑑 +1 𝑡𝑠 (𝑧 + 1) 2𝑇𝑑 (𝑧 − 1) 𝑦𝑘 = 𝑒 2𝜂𝑇𝑑 (𝑧 − 1) + 𝑡𝑠 (𝑧 + 1) 𝑘 2𝑇𝑑 (𝑧 − 1) 𝑦𝑘 = 𝑒 2𝜂𝑇𝑑 + 𝑡𝑠 𝑧 − (2𝜂𝑇𝑑 − 𝑡𝑠 ) 𝑘 𝑇𝑑 2𝜂𝑇𝑑 + 𝑡𝑠 𝑧 − (2𝜂𝑇𝑑 − 𝑡𝑠 ) 𝑦𝑘 = 2𝑇𝑑 (𝑧 − 1)𝑒𝑘 1 1 𝑇𝑑 𝜂𝑇𝑑 不完全微分の周波数特性 2𝜂𝑇𝑑 + 𝑡𝑠 𝑦𝑘+1 − (2𝜂𝑇𝑑 − 𝑡𝑠 )𝑦𝑘 = 2𝑇𝑑 (𝑒𝑘+1 − 𝑒𝑘 ) 𝑦𝑘+1 = 2𝜂𝑇𝑑 − 𝑡𝑠 2𝑇𝑑 𝑦𝑘 + (𝑒 − 𝑒𝑘 ) 2𝜂𝑇𝑑 + 𝑡𝑠 2𝜂𝑇𝑑 + 𝑡𝑠 𝑘+1 //積分 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) マルチコプタの制御 71

StampFly Seminar 頑張ってゲインを試行錯誤で決めましょう！ マルチコプタの制御 72

StampFly Seminar 頑張ってゲインを試行錯誤で決めましょう！ という感じになってしまい大変申し訳ないですm(_ _)m マルチコプタの制御 73

StampFly Seminar 古典制御での設計の勘所 2023.7.8 こうへい ゲインを調整しながら、時間応答を見て良さそうな値を探しても良いのですが、それ だと良い値を見つけるための試行錯誤時間が割と長くなることが多いです。その方法で も、経験を積むと割と勘が働くようになりますが、古典制御では周波数応答の世界であ たりをつけてから微修正するというのが良い方法だと思います。 制御対象と制御器を含めた開ループ伝達関数（これを一巡伝達関数と呼びます。）こ の一巡伝達関数の周波数応答をボード線図で確認して図★の様になる様に形を整えます。 これをループ整形と呼びます。各種の制御器とそのゲインの調整で形がどの様に変化す るかは法則性がある程度あるので、それを基にループ整形します。 目標 − 𝛿𝑒 𝐾𝑞𝑝 𝐾𝑞 𝑠(𝜏𝑠 + 1) 比例制御器 𝑞 gc付近の傾きを緩く -20dB/dc 高周波のゲインを下げる 安定性向上 ノイズの影響減・ロバスト性 ０dB低周波のゲインを上げる gc -20dB/dcより傾ける 定常偏差が減少 ゲイン交差周波数gcを大きくする 速応性向上（バンド幅大になる） 機体 フィードバックを切る -180deg 位相余裕を大きく 安定度増大 図★ 望ましいボード線図の形 𝐾𝑞𝑝 𝛿𝑒 比例制御器 𝐾𝑞 𝑠(𝜏𝑠 + 1) 𝑞 機体 これが一巡伝達関数（開ループ伝達関数） 𝐾𝑞𝑝𝐾𝑞 𝐿 𝑠 = 𝑠(𝜏𝑠 + 1) 大雑把にまとめると以下の様になるかと思います 安定性：位相余裕を大きくする 速応性：ゲイン交差周波数を大きくする 定常性：低周波のゲインを上げる ロバスト性・対ノイズ性：高周波のゲインを下げる 74／？

StampFly Seminar StampFlyについて Introduction マルチコプタの制御 75

StampFly Seminar StampFly 300mAh1S電池 距離センサ オプティカル フローセンサ M5StampS3 RGBLED 距離センサ ブラシ付 DCモータ RGBLED マルチコプタの制御 76

StampFly Seminar Y X StampFlyの前方 BMM150 X Y X Z 機体の座標 BMI270 IMU Y 地磁気計 IMUセンサの取り付け方

StampFly Seminar 0716モータの性能 https://nfpmotor.com/3-3-7v-dc-7mm-mini-micro-coreless-motor-16mm-model-nfp-d0716 78／？

• https://nfpmotor.com/3-3-7v-dc-7mm-mini-micro-coreless-motor-16mm-model-nfp-d0716

StampFly Seminar スケルトンコードがGithubにあります cloneして使用ください． https://github.com/M5Fly-kanazawa/M5StampFly_skeleton 端末からは以下のようにします git clone https://github.com/M5Fly-kanazawa/ M5StampFly_skeleton.git • • • • モータを回せます コントローラからの指令が受信できます コントローラにテレメを送れます 各種センサの値を読み取れます（全部ではない） マルチコプタの制御 79

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

StampFly Seminar 開発環境構築 Lesson 0 マルチコプタの制御 80

StampFly Seminar マルチコプタの制御 81

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

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

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

StampFly Seminar VSCodeでclone マルチコプタの制御 84

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

StampFly Seminar VSCodeでclone レポジトリを格納するフォルダを選ぶ マルチコプタの制御 86

StampFly Seminar VSCodeでclone 開く マルチコプタの制御 87

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

StampFly Seminar フォルダを開く マルチコプタの制御 89

StampFly Seminar Lesson0を選択 マルチコプタの制御 90

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

StampFly Seminar StampFlyの操縦 無線通信 ESP-NOW WiFi BlueTooth マルチコプタの制御 92

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

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

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

StampFly Seminar 2023.7.8 こうへい ESP-NOWの混信についてのメモ 混信が起きる状況（推定） • 基本的にはMACアドレスを指定することで ドローンと送信機は１対１の通信をするこ とができる • この際、同じチャンネルを使っていても混 信することはない（と思う） • ただしブロードキャストは受信してしまう StampFlyのMAC アドレスを指定 して送信 MACアドレスを FF:FF:FF:FF:FF:FFにして ブロードキャスト 対策案 送信データに受信側のMACアドレスを含め 受信側で自分に送られてきたデータか識別 し、ブロードキャストの影響を低減するこ とを試みた ESP-NOWを使用している他の製品

StampFly Seminar 飛行時の加速度のバイアス？ 97

StampFly Seminar カルマンフィルタによる高度推定（バイアス推定あり）メモ Kalman Filter 予測更新 ഥk = 𝐀 k 𝒙 ෝk−1 + 𝐆k 𝒖k−1 𝒙 ഥ𝑘 = 𝐀 k 𝐏 ෡k−1𝐀Tk + 𝐆k 𝐐𝐆kT 𝐏 観測更新 ഥk 𝐂 𝐓 + 𝐑 𝐒𝑘 = 𝐂𝐏 ഥk 𝐂 T𝐒k−1 𝐊𝑘 = 𝐏 𝒆𝑘 = 𝒛k − 𝐂ഥ 𝒙k ෝk = 𝒙 ഥ k + 𝐊 k 𝒆k 𝒙 ෡𝑘 = 𝐈 − 𝐊 𝑘 𝐂 𝐏 ഥk 𝐏 状態の予測 𝑤 ෝk−1 + ℎ(𝑎𝑧 k−1 −𝑏෠k−1 ) 𝑤 ഥk 1 𝑧kҧ = ℎ = 𝑧k−1 Ƹ + ℎ𝑤 ෝk−1 0 𝑏തk ෠ 𝑏 (1 + ℎ𝛽) k−1 𝑤 ෝk−1 0 −ℎ ℎ 𝑧k−1 Ƹ 1 0 + 0 𝑎𝑧 0 1 + 𝛽ℎ 𝑏෠k−1 0 誤差分散の予測 𝑝ො11 𝑝ො12 𝑝ො13 1 ℎ 0 −ℎ 0 ℎ 0 𝑞 0 ℎ 0 0 1 0 0 𝑝ො21 𝑝ො22 𝑝ො23 0 1 + 0 0 1 0 𝑞2 0 0 ℎ 0 1 + 𝛽ℎ 𝑝ො31 𝑝ො32 𝑝ො33 −ℎ 0 1 + 𝛽ℎ 0 ℎ 𝑝ො11 − ℎ(𝑝ො31 + 𝑝ො13 ) + ℎ2 𝑝ො33 + ℎ2 𝑞1 ℎ 𝑝ො11 − 𝑝ො32 − ℎ2 𝑝ො31 + 𝑝ො12 (1 + 𝛽ℎ)(𝑝ො13 − ℎ𝑝ො33) = ℎ 𝑝ො11 − 𝑝ො23 − ℎ2 𝑝ො13 + 𝑝ො21 ℎ2 𝑝ො11 + ℎ 𝑝ො21 + 𝑝ො12 + 𝑝ො22 (1 + 𝛽ℎ)(𝑝ො23 + ℎ𝑝ො13 ) (1 + 𝛽ℎ)(𝑝ො31 − ℎ𝑝ො33 ) (1 + 𝛽ℎ)(𝑝ො32 + ℎ𝑝ො31 ) (1 + 𝛽ℎ)2 𝑝ො33 + ℎ2 𝑞2 𝑝11 ҧ 𝑝12 ҧ 𝑝13 ҧ 0 𝑝 ҧ 𝑝 ҧ 𝑝 ҧ 𝑠 = 0 1 0 21 ҧ +𝑟 22 23 1 + 𝑟 = 𝑝22 𝑝31 ҧ 𝑝32 ҧ 𝑝33 ҧ 0 𝑝11 ҧ 𝑝ҧ21 𝑝31 ҧ 𝑝12 ҧ 𝑝ҧ22 𝑝32 ҧ 𝑝13 ҧ 1 𝑝23 ҧ = ℎ 𝑝33 ҧ 0 カルマンゲイン 𝑤:速度 𝑧:高度 𝑘1 𝑝11 ҧ 𝑘2 = 𝑝ҧ21 𝑝31 ҧ 𝑘3 𝑝12 ҧ 𝑝ҧ22 𝑝ҧ32 イノベーション 𝑝12 ҧ Τ𝑠 𝑝13 ҧ 0 1 𝑝ҧ23 1 = 𝑝ҧ22Τ𝑠 𝑝ҧ33 0 𝑠 𝑝ҧ32 Τ𝑠 𝑒 = 𝑧𝑘 − 𝑧kҧ 状態と誤差分散の推定 𝑤 ෝk 𝑤 ഥk 𝑤 ഥ k + 𝑘1 𝑒 𝑘1 𝑧kƸ = 𝑧kҧ + 𝑘2 e = 𝑧kҧ + 𝑘2 𝑒 𝑘3 𝑏തk 𝑏തk + 𝑘3 𝑒 𝑏෠k ℎ:更新周期 𝑎𝑧 :加速度 𝑏:加速度バイアス 𝛽:加速度バイアス係数 2023.7.8 こうへい 𝑝ො11 𝑝ො21 𝑝ො31 𝑝ො12 𝑝ො22 𝑝ො32 𝑝ො12 𝑝ො12 = 𝑝ො33 𝑘1 𝑝11 ҧ 𝑝12 ҧ 1 0 0 𝑝21 ҧ 𝑝22 ҧ 0 1 0 − 𝑘2 0 1 0 𝑝 ҧ 𝑝 ҧ32 𝑘 0 0 1 31 3 𝑝11 ҧ − 𝑘1 𝑝21 ҧ 𝑝12 ҧ − 𝑘1 𝑝22 ҧ 𝑝13 ҧ − 𝑘1 𝑝23 ҧ ҧ 1 − 𝑘2 𝑝22 ҧ 1 − 𝑘2 𝑝23 ҧ 1 − 𝑘2 = 𝑝21 𝑝31 ҧ − 𝑘3 𝑝21 ҧ 𝑝32 ҧ − 𝑘3 𝑝22 𝑝33 ҧ −𝑘3 𝑝23 ҧ 𝑝13 ҧ 𝑝23 ҧ 𝑝ҧ33

StampFly Seminar 電池電圧とホバリング入力 マルチコプタの制御 99

StampFly Seminar オプティカルフローセンサを用いた位置推定メモ Kalman Filter 予測更新 ഥk = 𝐀 k 𝒙 ෝk−1 + 𝐁k 𝒖k−1 𝒙 ഥ𝑘 = 𝐀k 𝐏 ෡k−1 𝐀Tk + 𝐁k 𝐐𝐁kT 𝐏 観測更新 ഥk 𝐂 𝐓 + 𝐑 𝐒𝑘 = 𝐂𝐏 ഥk 𝐂 T𝐒k−1 𝐊𝑘 = 𝐏 𝒆𝑘 = 𝒛k − 𝐂ഥ 𝒙k ෝk = 𝒙 ഥ k + 𝐊 k 𝒆k 𝒙 ෡𝑘 = 𝐈 − 𝐊 𝑘 𝐂 𝐏 ഥk 𝐏 2023.7.8 こうへい 状態の予測 𝑢ො k−1 + ℎ(𝑎𝑥 k−1 −𝑏෠k−1 ) 𝑢ത k 1 𝑥ҧk = ℎ = 𝑥ොk−1 + ℎ𝑢ො k−1 0 𝑏തk ෠ 𝑏 (1 + ℎ𝛽) k−1 𝑢ො k−1 0 −ℎ ℎ 𝑥ොk−1 + 0 𝑎𝑥 1 0 0 1 + 𝛽ℎ 𝑏෠k−1 0 誤差分散の予測 𝑝ො11 𝑝ො12 𝑝ො13 1 ℎ 0 −ℎ 0 ℎ 0 𝑞 0 ℎ 0 0 1 0 0 𝑝ො21 𝑝ො22 𝑝ො23 0 1 + 0 0 1 0 𝑞2 0 0 ℎ 0 1 + 𝛽ℎ 𝑝ො31 𝑝ො32 𝑝ො33 −ℎ 0 1 + 𝛽ℎ 0 ℎ 𝑝ො11 − ℎ(𝑝ො31 + 𝑝ො13 ) + ℎ2 𝑝ො33 + ℎ2 𝑞1 ℎ 𝑝ො11 − 𝑝ො32 − ℎ2 𝑝ො31 + 𝑝ො12 (1 + 𝛽ℎ)(𝑝ො13 − ℎ𝑝ො33) = ℎ 𝑝ො11 − 𝑝ො23 − ℎ2 𝑝ො13 + 𝑝ො21 ℎ2 𝑝ො11 + ℎ 𝑝ො21 + 𝑝ො12 + 𝑝ො22 (1 + 𝛽ℎ)(𝑝ො23 + ℎ𝑝ො13 ) (1 + 𝛽ℎ)(𝑝ො31 − ℎ𝑝ො33 ) (1 + 𝛽ℎ)(𝑝ො32 + ℎ𝑝ො31 ) (1 + 𝛽ℎ)2 𝑝ො33 + ℎ2 𝑞2 𝑝11 ҧ 𝑝12 ҧ 𝑝13 ҧ 1 𝑝 ҧ 𝑝 ҧ 𝑝 ҧ 𝑠 = 1 0 0 21 ҧ +𝑟 22 23 0 + 𝑟 = 𝑝11 𝑝31 ҧ 𝑝32 ҧ 𝑝33 ҧ 0 𝑝11 ҧ 𝑝ҧ21 𝑝31 ҧ 𝑝12 ҧ 𝑝ҧ22 𝑝32 ҧ 𝑝13 ҧ 1 𝑝23 ҧ = ℎ 𝑝33 ҧ 0 カルマンゲイン 𝑘1 𝑝11 ҧ 𝑘2 = 𝑝ҧ21 𝑝31 ҧ 𝑘3 𝑢:速度(オプティカルフロー) 𝑥:位置 𝑝11 ҧ Τ𝑠 𝑝13 ҧ 1 1 𝑝ҧ23 0 = 𝑝ҧ21 Τ𝑠 𝑝ҧ33 0 𝑠 𝑝ҧ31Τ𝑠 𝑝12 ҧ 𝑝22 ҧ 𝑝32 ҧ イノベーション 𝑒 = 𝑧𝑘 − 𝑧kҧ 状態と誤差分散の推定 𝑢ො k 𝑢ത k 𝑢ത k + 𝑘1 𝑒 𝑘1 𝑥ොk = 𝑥ҧk + 𝑘2 e = 𝑥ҧk + 𝑘2 𝑒 𝑘3 𝑏തk 𝑏തk + 𝑘3 𝑒 𝑏෠k ℎ:更新周期 𝑎𝑥 :加速度 𝑏:加速度バイアス 𝛽:加速度バイアス係数 𝑝ො11 𝑝ො21 𝑝ො31 𝑝ො12 𝑝ො22 𝑝ො32 𝑝ො12 𝑝ො12 = 𝑝ො33 𝑘1 𝑝11 ҧ 𝑝12 ҧ 1 0 0 𝑝21 ҧ 𝑝22 ҧ 0 1 0 − 𝑘2 0 1 0 𝑝 ҧ 𝑝 ҧ32 𝑘 0 0 1 31 3 𝑝11 ҧ 1 − 𝑘1 𝑝12 ҧ 1 − 𝑘1 𝑝13 ҧ 1 − 𝑘1 ҧ − 𝑘2 𝑝11 ҧ 𝑝22 ҧ − 𝑘2 𝑝12 ҧ 𝑝23 ҧ − 𝑘2 𝑝23 ҧ = 𝑝21 𝑝31 ҧ − 𝑘3 𝑝11 ҧ 𝑝32 ҧ − 𝑘3 𝑝12 𝑝33 ҧ −𝑘3 𝑝13 ҧ 𝑝13 ҧ 𝑝23 ҧ 𝑝ҧ33

StampFly Seminar オプティカルフローに基づく位置推定

StampFly Seminar 今後のStampFly マルチコプタの制御 102

StampFly Seminar マルチコプタの制御

StampFly Seminar マルチコプタの制御

StampFly Seminar マルチコプタの制御

StampFly Seminar マルチコプタの制御

StampFly Seminar Various types of positional landmarks Stampfly Autonomous drone race Landing （５） （４） （３） Checkpoint４ Goal Checkpoint３ （２） The coordinates and height of the landmarks are communicated in advance. Take off Checkpoint２ Checkpoint１ Start A competition where StampFly estimates its position using its sensors and races to reach the goal as quickly as possible, while ensuring it passes over the designated checkpoints. Once StampFly starts, it must fly completely autonomously until it reaches the goal.