hakoniwa-drone-mujoco

MuJoCo版・箱庭ドローン パラメータチューニング編 合同会社箱庭ラボ 森崇

アジェンダ • 背景：SimToRealギャップ • MuJoCo版・箱庭ドローンとは？ • MuJoCo版・箱庭ドローンSimToReal開発フロー • MuJoCo版・箱庭ドローンでの開発詳細 • MuJoCo版・箱庭ドローンのツール構成 • PIDチューニング詳細 • ドキュメント構成 • PID自動チューニング概要 2

背景：SimToRealギャップ 「シミュレーションではうまくいくのに、実機だとうまくいかない！」 • 原因：Simと実機の構造差 • ① ハード特性（質量・慣性・形状） • ② 非線形特性（推力・飽和） • ③ 動的特性（遅延・ノイズ） →SimToReal は一致問題ではない 構造差の問題である • 方針：ギャップを埋めるのではなく、ギャップを理解し、扱う • Simで扱う範囲を決める • 扱えない要因を特定する • SimToRealプロセスとして設計する 3

箱庭での整理 方針 箱庭での実装 Simで扱う範囲を決める → Sim Envelope （✓/✗を明示） 扱えない要因を特定する SimToRealプロセスとして設計する → Gap Taxonomy （Physics/Sensor/Control/Environment） → 箱庭ドローンSimToRealプロセス この整理で得られること • リスクを事前に可視化できる • Gap Taxonomyで「どのGapが残ってるか」を関係者で共有できる • 検証コストをコントロールできる • → 「何をSimで済ませ、何を実機でやるか」を意図的に決められる • 実機検証の手戻りを減らす • 例：PIDゲインをSimで詰めたのに実機でオシレーションが止まらない 4

箱庭SimToRealプロセスの取り組み できることを全部やった上で、それでも残るギャップを正直に認識する • 箱庭の強み：組み合わせの自由度 • 故障シナリオ環境変動・センサノイズ・剛体物理（MuJoCo）を自由に組み合わせ可能 • これらを総動員してSimの期待値を最大化する • それでも残るギャップを正直に認識する • Sim Envelopeの✗＝「やれてない」ではなく「わかってやらない」 • Gap Taxonomyで残課題を実機検証に明示的に引き継ぐ 5

箱庭ドローンSimToRealプロセス 環境Sim Domain Randomization 故障シナリオ センサ ノイズ 本資料の 解説範囲 Fault Injection Hakoniwa DroneSim Drone Config ベース機 (x500等) MuJoCo Definition PX4/Ardupilot Params PX4/ Ardupilot 実機ログ 風・気温 衝突 実機移行時の課題一覧 姿勢制御 群制御 通信遅延 空力 GPSマルチパス 機体振動 Gap Taxonomy ——————— MAVLINK 通信データ 比較・検証 Sim Envelope ——————— Physics gap Sensor gap Control gap Environment gap Simログ ・この課題をどれだけ出せるかが重要 ・比較・検証の具体的な評価指標 （何をもって「実機に近い」とするか） 6

MuJoCo版・箱庭ドローンとは？ • 箱庭ドローンのアーキテクチャ • 標準版・箱庭ドローンの物理モデル • MuJoCo版・箱庭ドローンの物理モデル • 箱庭ドローンのローターモデル • 箱庭ドローンのローター故障シナリオ • 箱庭ドローンの制御モデル • 内部制御 • PX4/ArduPilot(TODO) 7

箱庭ドローンのアーキテクチャ 8

標準版・箱庭ドローンの物理モデル ドローンの物理運動方程式を数値計算してます。 →ゲームロジックではありません。 詳細はこちらを参照ください。 • https://speakerdeck.com/hiranabe/math-physics-and-dynamics-of-drone-in-hakoniwa 9

• https://speakerdeck.com/hiranabe/math-physics-and-dynamics-of-drone-in-hakoniwa

MuJoCo版・箱庭ドローンの物理モデル Velocity (BodyFrame) Sensors Controller MuJoCo Dynamics AngularVelocity (BodyFrame) Position Velocity Thrust[N] AntiTorque[N] 検証用に外して、テスト することも可能 Thrust/AntiTorque Thrust[N] Mixer PWM Duty Rotor Dynamics Add Thrust/AntiTorque AntiTorque[N] 10

箱庭ドローンのローターモデル ローターの角速度 𝜔 𝑡 がバッテリー電圧 𝑉𝑏𝑎𝑡 のデューティー比 𝑑 𝑡 で制御されるモデルです Rotor Dynamics Battery Dynamics Vbat PWM Duty(d) Current (電流) 放電電圧モデル (温度依存) ローター回転速度(ω) 推力/アンチトルク計算 Thrust(T) MuJoCo Dynamics AntiTorque(τ) 各種パラメータの計算： https://docs.google.com/spreadsheets/d/1JACdNpwAYr-OUya5DmPa8r_1QfsI-4M4G-kJ_zLCvQ0/edit?gid=828965483#gid=828965483 11

• https://docs.google.com/spreadsheets/d/1JACdNpwAYr-OUya5DmPa8r_1QfsI-4M4G-kJ_zLCvQ0/edit?gid=828965483#gid=828965483

箱庭ドローンのローター故障シナリオ 故障シナリオ 箱庭で再現する故障系ダイナミクス シミュレーションで見えるインパクト （箱庭アセットとして外だし/PDU通信） Rotor Dynamics モーター巻線断 （R → ∞） ESC過熱 （R ↑） R モーター 故障系ダイナミクス C_T プロペラ破損 （C_T, C_Q 低下） 片軸摩擦増加 （D ↑） 軸曲がり （C_Q ↑、振動→IMUノイズ） C_Q シャフト 故障系ダイナミクス ノイズ バッテリ劣化 （Vbat 低下） 機体姿勢への影響（Attitude Instability） •ロール/ピッチの微振動 •特定軸だけ姿勢が乱れやすい •ヨー（回頭）方向のふらつき •低周波〜中周波の姿勢揺れ（シャフト振動） D センサー異常（IMU / 振動） •ジャイロノイズ増加 •加速度センサーのスパイク •フィルタ（EKF）推定が乱れる •VIO（視覚オドメトリ）が吹っ飛ぶ Gyro/Acc Sensor 電気系の影響（Electrical） •電流増大（加熱しやすい） •過電流→ESC保護モード •バッテリ急降下（Vbat sag） •飛行時間の急減 MuJoCo Dynamics 推力T／反トルクτ へのノイズ（＋バイアス） 推力系への影響（Thrust / Anti-Torque） •推力低下（Hover維持できない） •推力の周期的揺らぎ（回転同期ノイズ） •反トルクの乱れによるヨー振動 •上昇しない／急に落ちる •１モーターだけ応答遅延 Battery Dynamics 飛行挙動の最終結果（Flight Behavior） •ホバリングしない •急に流される •Yawが安定しない •突然のフリップ（反転クラッシュ） •ミッション（自律飛行）中断 12

箱庭ドローンの制御モデル(内部制御) 12ブロックの制御で、全てPID制御：36パラメータ(12x3) 目標高度 高度制御 目標姿勢 角度(ψ) ヨー角度 制御 目標速度 目標姿勢 角速度(r) 高度速度 制御 ヨー角速度 制御 推力 デューティ1 トルク (Tz) デューティ2 目標水平位置(y) 水平位置 制御(y) 水平位置 制御(x) 目標水平 速度(vy) 目標水平 速度(vx) 水平速度 制御 水平速度 制御 目標姿勢 角度(φ) 水平姿勢 角度制御 (θ) 水平姿勢 角度制御 (φ) 目標姿勢 角速度(q) 目標姿勢 角速度(p) 水平姿勢 角速度 制御(q) 水平姿勢 角速度 制御(p) トルク (Ty) 物理モデル 物理モデル (質点) モータ プロペラ系モ デル ミキサー 目標姿勢 角度(θ) 推力 デューティ3 物理モデル (MuJoCo) トルク (Tx) デューティ4 トルク 電圧 目標水平位置(x) 補足：赤字部分は評価ツールから直接入力可能 電流 バッテリーモデル

MuJoCo版・箱庭ドローンSimToReal開発フロー ● 想定ゴール ● ● 想定された検証シナリオにおいて、PX4/Ardupilot＋ユーザ制御プログラムの妥当性を 箱庭ドローンシミュレーション環境で検証 開発フェーズ構成 ○ ○ 段階的にゴールを積み上げることで、開発リスクの低減とサポート品質を上げる 3フェーズ構成： ● フェーズ１：実機の仮想化と物理特性（質量、イナーシャ）の計算 ● フェーズ２：故障シナリオでの故障制御プログラム+箱庭ドローン連携シミュレーション ● フェーズ３：故障シナリオでの故障制御プログラム+箱庭ドローン+ PX4/Ardupilot連携シミュレーション 14

MuJoCo版・箱庭ドローンSimToReal開発フロー フェーズ１ フェーズ３ フェーズ２ 実機モデル 仮想モデル 検証シナリオ お客様の 制御プログラム 標準PX4/Ardupilot （未改修） PX4/Ardupilot パラメータ 部品の体積・質量・ 取り付け位置調査 箱庭ドローンで物理特 性計算 仮想実験環境の アーキテクチャ設計 箱庭ドローン への組み込み (内部制御) PX4/Ardupilotへの組 み込み方法調査 PX4/Ardupilot &箱庭での故障テスト 部品データ 組み立て資料 イナーシャ 質量等 仮想実験 アーキテクチャ 仮想実験結果 (ロジック検証) お客様の 制御プログラム組み込み 版PX4/Ardupilot 仮想実験結果 (PX4/ArdupilotSITL検証) MuJoCo で再現 MATLAB等 でSimulation (Option) お客様の 制御プログラム作成 (C/C++) 実験結果の 妥当性評価 (ロジック検証) 箱庭ドローン との接続テスト &パラメータ・ チューニング 実験結果の 妥当性評価 (PX4/ArdupilotSITL検証) 比較 15

MuJoCo版・箱庭ドローンでの開発詳細 • 物理・制御モデルの整理 • MuJoCo上での機体再現 • PIDパラメータの設計とチューニング • 評価方法と実験フロー 16

物理・制御モデルの整理 実機の物理・制御モデルを箱庭ドローンで仮想化する範囲 実機・物理モデル 全体質量 ローター位置/ /COM 回転方向 全体質量 /COM 部品群の 配置場所 ローター特性 部品群の 配置場所 ローター特性 物理的な各種 上限値(導出値) センサ特性 物理的な各種 上限値(導出値) センサ特性 仮想・物理モデル(MuJoCo) ローター位置/ 回転方向 仮想・物理モデル (箱庭ドローン) 実機・制モデル 仮想・制御モデル(箱庭ドローン) PX4/ArduPilot(SITL) PIDパラメータ PIDパラメータ PIDパラメータ 制御上限値 制御上限値 制御上限値 制御ロジック 制御ロジック 制御ロジック 17

MuJoCo上での機体再現 X500 v2を題材に再現します • Holybro 製品ページ: • https://holybro.com/products/p x4-development-kit-x500v2?variant=43018371596477 • Holybro モータ仕様: • https://cdn.shopify.com/s/files/ 1/0604/5905/7341/files/X500M otorSpec.png?v=1678791632 18

• https://holybro.com/products/px4-development-kit-x500-v2?variant=43018371596477
• https://cdn.shopify.com/s/files/1/0604/5905/7341/files/X500MotorSpec.png?v=1678791632

機体の再現 実機の情報をMuJoCo/箱庭ドローンを使用して再現する 剛体特性 ローター特性 実機の取り付け情報/ 各種部品の質量等 各種物理パラメータ モーター仕様/ ローター特性 MuJoCoモデル の部品定義 MuJoCoモデル の部品定義 ローターモデルの パラメータ導出 MuJoCoモデル (MJCF) 物理的な各種 上限値(導出値) ローターのパラメータ群 (Ct/Cq/抵抗等) 質量/イナーシャ /COM計測 箱庭ドローン コンフィグに反映 (json) 19

PIDパラメータの設計とチューニング 闇雲にやると沼にハマるので、物理および制御面での探索範囲を見極める →PIDはその後に決める mixer-offでの 姿勢角速度制御系 のパラメータ候補 姿勢角/垂直制御系 のパラメータ 姿勢角/垂直/水平/ 高度制御系のパラメータ 姿勢角速度制御系 の設計範囲を求める mixer-onでの姿勢 角/垂直制御系検証 (重力on/地面off) mixer-onでの 高度/水平移動検証 (重力on/地面on) RC操作での検証 (重力on/地面on) 姿勢角速度制御系の PID探索範囲 mixer-onでの 姿勢角/垂直制御系 のパラメータ候補 姿勢角/垂直/水平/ 高度制御系のパラメータ 候補 姿勢角/垂直/水平移動 制御系のパラメータ (確定) mixer-offでの検証 (重力off/地面off) 姿勢角制御系の 安定性検証 (重力on/地面off) replay-log での検証 (重力on/地面on) 物理的な各種 上限値(導出値) 制御面での制約 (最大姿勢角度等) 20

評価対象/各評価で見る指標（例） 評価手法 評価目的 見るべき内容 合否基準 過渡応答の確認 - 立ち上がり時間(63%) - オーバーシュート - 制定時間 - cross-axis - 目標時間内 - overshoot 許容内 - cross-axis 許容内 滑らかな追従性と飽和の確認 - 追従遅れ - 飽和 - ヒステリシス - 大きな遅れなし - 飽和が支配的でない Sine信号 周波数応答の確認 - gain - phase lag - cross-axis - Z/Vz 干渉 - 必要帯域で gain/phase が許容内 - 安定性 - 高周波で破綻しない Replay log 実運用シナリオの再現評価 - リプレイログとのズレ - リプレイログとのズレ幅が許容内 最終確認 - 操作感 - hover 安定性 - forward 時の Z 崩れ - yaw 回り込み - 操作して違和感がない - 安全に飛行できる - 実用上問題ない ステップ信号 RAMP信号 RC実験 21

MuJoCo版・箱庭ドローンのツール構成 シグナル抽象。Step信号、Sine 信号などを隠蔽化 箱庭ドローンのベースコン フィグ。煩雑さを防ぐため に、評価中は、同じ設定 ファイルを利用し、シナリオ に応じて、異なる値が採 用される。 シナリオの雛形。実験信 号の雛形。 type / axis / events / input_signal / prepare を持つ 実験段階の共通条件。 mode切替、環境条件、 期待値を設定する 実験段階毎のPIDパラ メータ設定を入力する。 実験段階時にPIDパラ メータを自動チューニング する場合、その探索範囲 を設定する drone physics config (json) drone control param (txt) Input Signal (Library) Drone Simulator (RC Library) ドローンシミュレーション実行本体 Plotter Pid Tuning Runner (main) Results 各種グラフ MuJoCo model (xml) Evaluator Command Arguments 評価結果 Base-Scenario (json) Phase (json) Suite Param Set (json) Search Space (json) Generated-Scenario (json) 実験結果をPhaseで設定した期待値と 照らし合わせて、OK/NG判定をする Suite が merge して生成する、 runner が直接読む唯一の入力。 Base-Scenario, Phase, PramSet, CommandArgumentsから生成。 Autotune (optuna) 22

mixer-offでの検証 • 対象 • 角速度制御(roll/pitch) Roll/Pitch方向の回転 • 目的 • 理論P値の探索範囲の妥当性チェック • 条件 • 重力-off / mixer-off / 位置固定 • 固定位置はアーム中心とする • 方法 • roll/pitch rate の step / sine 信号 を与えて応答を観測する 23

mixer-onでの検証(ホバーチェック) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz Roll/Pitch/yawを0固定して、 ホバリングできるようにする • 目的 • 落下せず hover 条件を作る • 条件 • 重力-on / mixer-on / 地面なし • 方法 • 目標角度：roll=pitch=yaw = 0 • 目標速度：Vz = 0 • 評価対象 • ホバー達成までの時間 • Vz/ roll / pitch 24

mixer-onでの検証(姿勢制御安定性) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz Roll/Pitch/yawにステップ/Sine信号を 与えて安定性を見る • 目的 • 姿勢角制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面なし • 方法 • prepareフェーズで、ホバー状態を作り ステップ応答/周波数応答をみる 反応速度重視で、振動なし 目標値のオーバーシュートは許容しても良い 25

mixer-onでの検証(高度速度) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz ホバリング状態にしてから、 垂直速度制御の評価を行う • 目的 • 垂直速度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • prepareフェーズで、 ホバー状態を作り ステップ応答/周波数応答をみる 26

mixer-onでの検証(高度１) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz / 高度制御 Z ホバリング状態にしてから、 高度制御の評価を行う • 目的 • 高度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • prepareフェーズで、ホバー状態を作り ステップ応答/周波数応答をみる • 補足 • この段階では、x/y 方向のdrift が発生 する場合があるが、その対応は、水平速 度制御を入れた状態で評価する 27

mixer-onでの検証(高度２) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz / 高度制御 Z • 水平速度制御 Vx / Vy ホバリング状態にしてから、 高度制御の評価を行う • 目的 • 高度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • prepareフェーズで、 ホバー状態を作りステップ応答/周波数応答をみる • 補足 • この段階では、x/y 方向のdrift が発生しないこと を確認すること 28

mixer-onでの検証(水平速度) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz / 高度制御 Z • 水平速度制御 Vx / Vy ホバリング状態にしてから、 水平速度制御の評価を行う • 目的 • 水平速度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • prepareフェーズで、ホバー状態を作りス テップ応答/周波数応答をみる 29

mixer-onでの検証(replay-log) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz / 高度制御 Z • 水平速度制御 Vx / Vy stick操作ログをベースにした安定飛行確認 • 目的 • 水平速度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • replay用のログを流し込む 30

mixer-onでの検証(RC操作) • 対象 • 角速度制御 roll / pitch / yaw • 角度制御 roll/pitch / yaw • 高度速度制御 Vz / 高度制御 Z • 水平速度制御 Vx / Vy RC操作しながら違和感ないか最終チェック • 目的 • 水平速度制御の安定なPID探索 • 条件 • 重力-on / mixer-on / 地面あり • 方法 • 箱庭の本番環境で操作する 31

ドキュメント構成 前提情報 箱庭ドローン制御 箱庭ドローンの コンフィグファイル構成 座標系の整理 機体再現編 剛体特性 各種剛体パラメータ群 ローター特性 ローターのパラメータ群 (Ct/Cq/抵抗等) 評価・判定基準編 実験フェーズの概要 MuJoCoでの設定方法 X500での設定方法 (MuJoCo) X500での設定方法 (箱庭ドローン) PIDチューニング準備編 物理的な各種 上限値の導出方法 制御面での制約の 導出方法 P値の理論的な導出方法 X500での物理的な各種 上限値 X500での制御面 での制約値 X500でのP値 評価指標 判定基準 導入・運用編 PIDチューニング実験編 ツール構成 mixer-offでの検証 mixer-onでの検証 (姿勢制御安定性) mixer-onでの検証 (高度１) mixer-onでの検証 (水平速度) Quickスタート mixer-onでの検証 (ホバーチェック) mixer-onでの検証 (高度速度) mixer-onでの検証 (高度２) mixer-onでの検証 (replay-log) ツールの使い方 mixer-onでの検証 (RC操作) 回帰・自動化方法 32

PID自動チューニング概要 • 手動チューニングの課題 • PIDのパラメータチューニングの手間 • 慣れてないと、何をチューニングすべきか試行錯誤して沼にハマりやすい • →ここはできるだけドキュメントにのこしていきたい。 • 自動チューニングの方針 • 人間が決めるべきことと自動化するところの境界を決める • →これやっとかないと、なぜそうなったか説明ができなくなる。 • 人間がきめること • フェーズ毎の評価関数 • 評価対象と重みづけ • フェーズ移行判断基準 • フェーズ毎に重視するポイントが違う 33

自動チューニングツール構成 ポイント： ・角速度のPIDパラメータを初期値とする ・Manifestでフェーズ毎に以下を定義 ・PID探索範囲 ・評価方法(score定義) ・トライアル回数 を決めて、 段階的にフェーズを上げていく方式 最初は、フェーズ単位でチェック →慣れて来たら、全自動化 人間が与える初期PIDパラメータ (rate roll/pitch/yaw) seed Manifest (json) AutoRunner 1..* Phase毎の定義 phase定義 PID探索範囲定義 (固定PID含む) Phase毎の AutoRunner Phase毎の実行結果 Best result トライアル毎の結果 score定義 トライアル回数 Best result -> 次フェーズの固定パラメータ 34

PIDパラメータ探索範囲 ・姿勢角まわりは追従性重視（安定性は最低限で、他軸影響や振動などないことが大事） ・速度も基本同様で、追従性重視。 ・高度は安定性が大事。とくに、水平移動中に上昇/下降が最小限におさまるかどうか。 制御 PIDパラメータ 角 速 度 roll/pitch 角 度 速 度 位 置 yaw 初期値 ホバーチェック 姿勢安定性 ◯ (直感/P) △ (←) ◯ (追従性/PID) (※Iはノイズで効く) roll/pitch ◯ (安定性/PID) ◯ (追従性/PID) yaw △ (安定性/P=1) △ (安定性/P=1) ◯ (ホバリング有無/P) △ (←) Vz ー Vx/Vy ー Z ◯ ：探索対象 △ ：固定 ー ：固定/対象外 ー 高度２ 水平移動 △ (←) △ (←) ◯ (追従性/PID) △ (←) △ (安定性/P=1) ◯ (追従性/PI) ◯ (安定性/PID) △ (←) 35

自動チューニング結果 PID自動チューニングの様子： https://youtu.be/NONdlYVOl28 自動チューニング・パラメータでのRC操作： https://youtu.be/Olp-FtsUqXE 36

• https://youtu.be/NONdlYVOl28
• https://youtu.be/Olp-FtsUqXE

以下、補足情報など 37

実測値のスコア化方法(kind) • ratio_upper_bound（非負値指標） • 上限目標を持つ項目 • ratio_lower_bound （非負値指標） • 下限目標を持つ項目 • band_upper • 小さいほど良い項目 • band_lower • 大きいほど良い項目 • band_center • 理想値の周辺を高く評価したい項目 • band_upper_abs • 絶対値で小さいほど良い項目 • penalty_upper • 大きすぎると減点したい項目 38

ratio_upper/lower_bound • upper（非負値指標） • ratio = min(1.0, target / actual) 1.0(target) actual • lower（非負値指標） • ratio = min(1.0, actual / target) actual > target 1.0(target) actual actual < target 39

band_upper/lower/center • upper • actual <= best なら 1.0 • actual >= worst なら 0.0 • ratio = (worst - actual) / (worst - best) • lower • actual >= best なら 1.0 • actual <= worst なら 0.0 • ratio = (actual - worst) / (best - worst) best worst actual worst best actual • center worst best • abs(actual - best) >= tolerance で 0.0 • ratio = 1.0 - abs(actual - best) / tolerance actual tolerance tolerance

ratio_upper_abs 1.0(target) actual abs(actual) > target actual > target 41

penalty_upper • upper • actual <= best なら 0.0 • actual >= worst なら 1.0 • penalty_ratio = (actual - best) / (worst - best) • earned_points = - penalty_ratio * points penalty_ratio best ペナルティなので、この値に重み(points)がかけら れた値が減算されます worst 1.0(target) actual 42