ブログ

SLEIPNIR-ZERO の機体にはエルロンがありません。動かせるのはエレベータ(ピッチ)・ラダー(ヨー)・スロットルの3つです。この記事では、この機体のための飛行制御の設計を解説します。制御則は実装済みですが、現段階では計算結果をテレメトリに載せるだけで、まだ舵は駆動していません。

この機体の難しさ

設計上、素直でない点が2つありました(うち1つは機体側の改善で解消済みです)。

• 舵の効きがスロットルで変わる。プロペラと動翼が近く、動翼に当たる気流の大半はプロペラ後流です。つまりスロットルを絞ると舵が効かなくなり、開けると効きすぎます。
• (解消済み)初期の機体はピッチが静的に不安定でした。空力中心が重心より前にあり、放っておくと機首上げが加速する特性で、常に当て舵が必要でした。現在は機体側の見直しで静安定を確保しています。制御則は不安定な機体でも成立するように設計してあり(運動モデルに不安定項を持つ)、安定化後もそのまま使えます。

設計方針

効きの正規化。軸ごとの舵の効きをスロットル指令の関数 k(スロットル) としてテーブルで持ち、制御出力を k で割ってから舵に渡します。これでループ全体の利得がスロットルに依らず一定になり、ゲイン調整が1組で済みます。ピッチ側では、後流がエレベータを直撃することによるスロットル起因の機首外乱も前置きで打ち消します。

軸ごとのループ構成。ピッチは姿勢一定の PD 制御(静的不安定だった初期機体を能動安定化するために組んだ構成で、静安定を確保した現在の機体では姿勢保持として働きます)、ヨーは方位保持の PD、高度は外側ループとしてスロットルの PI で保ちます。対気速度を測るピトー管が無いので、本来の「速度保持」の代わりに「スロットル→高度」の関係で代用しています。

状態量はどう作るか

制御に使う高度と上下速度は、気圧計と加速度計の相補フィルタで融合します。気圧高度は室内の気流やドアの開閉、プロペラ後流で数十 cm 単位のノイズが乗るため、短期は加速度の積分を信じ、長期のドリフトだけを気圧で引き戻す配分です。

水平速度は位置の基準(GPS など)が無いため、加速度の積分だけではドリフトが止まりません。そこでリーク付きの積分で値を有界化しています。将来的には、外部カメラトラッキングの位置推定がこの「無い基準」を埋める計画です。

これから:同定

現在入っている k テーブルと各ゲインは同定前の仮の値です。実機のステップ応答を計測して舵の効きを同定し、ゲインを差し替えるのが次のステップです。テレメトリが 50 Hz で姿勢と制御指令を記録しているのは、まさにこの同定のためでもあります。

実装は danfunc/flight_robocon_telemetory_sender の FLIGHT_CONTROLLER にあります。

テレメトリ送信機のファームウェアは、既製の RTOS ではなく自作のマイクロカーネル「Shizuku」の上に組んでいます。この記事では、その設計を「テレメトリに求められる性質」から解説します。

テレメトリに求められる2つの性質

飛行データを送り続ける機械には、普通のマイコン工作とは違う要求があります。

• 止まらないこと。どこかの処理が無限ループに落ちても、送信全体が道連れになってはいけない
• 黙って死なないこと。クラッシュするなら、なぜ死んだかを地上に伝えてから死んでほしい

Shizuku の設計判断の多くは、この2つに帰着します。

基本構造:オブジェクトとスレッド

Shizuku では、センサドライバ・センサ融合・BLE 送信・LED 制御などがそれぞれ独立した「オブジェクト」で、オブジェクトの境界を越える呼び出しは必ず SVC 例外(Arm の システムコール命令)を通ります。オブジェクト間のデータは、メソッド呼び出しか、ロックフリーの一方向ストリーム(リングバッファ)で受け渡します。境界がはっきりしているので、どの部品が悪さをしたかを切り分けやすくなります。

RP2350 の2コアも使い分けます。core1 はセンサの I2C 読み出し専任、core0 は融合・BLE・スケジューリングの担当です。時間に厳しい読み出しを、負荷の変動する通信処理から物理的に隔離しています。

止まらないために

実行時間の予算制。スケジューラは各スレッドに実行予算(3 ミリ秒)を与え、使い切ったスレッドは強制的に CPU を取り上げます。どこかの処理が無限ループに落ちても、他のスレッドは回り続けます。信頼できる少数の基幹スレッドだけが予算無制限です。

送信の優先度分離。BLE の送信キューが 1 本だと、大きなテレメトリの塊の後ろに小さな制御コマンドの応答が並んでしまい、応答遅延が跳ね上がります(いわゆる Head-of-Line ブロッキング)。Shizuku では送信ストリームを優先度別に分け、制御系の往復遅延を約 4 ミリ秒に抑えています。

黙って死なないために

panic の記録は電源が落ちても残す。クラッシュ時のメッセージは、リセットで消えない RAM 領域(noinit 領域)のリングバッファに書き込みます。再起動後の起動時に前回の死因を印字できるので、「なんか黙って再起動してた」で終わらせずに済みます。

HardFault でもログを出し切る。最高優先度の例外中は USB 通信の割り込みが動けず、printf しても地上へ届かない=無言フリーズに見えます。そこで HardFault ハンドラの中から USB スタックを手動で回し、レジスタダンプを届けてから止まります。

暴走を早期に捕まえる。スタックオーバーフローはハードウェアのスタック下限レジスタ(PSPLIM)で即検出します。さらに MPU で「書ける領域は実行できない・実行できる領域には書けない」(W^X)を張り、フラッシュへの誤書き込みやスタック上でのコード実行を即座にフォールトさせます。

現状

予算制スケジューラ・優先度付き送信・2コア動作・panic リングまでは実機で動作確認済みです。MPU 保護と DMA によるストリーム転送はビルドまで済んでおり、実機検証がこれからの段階です。コードは danfunc/flight_robocon_telemetory_sender にあります。

SLEIPNIR-ZERO の機体に搭載する電装は、自作のアビオニクス基板(通称:ヒコロボ基板)にまとめます。この記事では、この基板の役割と設計を解説します。

核心:手動と自動の切り替え

この基板の最も重要な機能は、操縦系統の切り替えです。プロポの受信機から来る 6 チャンネルの操舵信号と、マイコンが生成する制御信号を、4053(アナログマルチプレクサ IC)で選択してサーボへ届けます。

自動制御の実験では、制御則のバグや同定不足で機体が変な動きをすることが十分あり得ます。そのときにマルチプレクサを手動側へ切り替えれば、マイコンを経由しない生の操縦信号で機体を取り戻せます。ソフトウェアがどれだけ壊れても操縦者の手が最後に残る、という安全設計です。

1枚にまとめる理由

受信機・マイコン・センサ・サーボをジャンパ線でつないでも動きはします。しかし機体は離着陸のたびに振動と衝撃を受けるので、ジャンパ線の接触不良は空中でのトラブルに直結します。基板に起こすことで、配線の固定(信頼性)、重量と体積の削減、同じものをもう一式作れる再現性が得られます。

拡張コネクタ

センサの増設用に、I2C・SPI・アナログの各インタフェースコネクタを用意しています。BME280(気圧)と BNO055(IMU)には専用のコネクタがあり、テレメトリ送信機系のセンサ構成をそのまま受けられます。マイコンは Raspberry Pi Pico のフットプリントに加えて Seeed XIAO も載せられるようにしてあります。

設計と製造

設計にはオープンソースの基板 CAD である KiCad を使っています。リポジトリには回路図・基板データに加えて、製造発注に使うガーバ・BOM・部品座標データも含まれており、基板製造サービスにそのまま出せる状態で管理しています。

設計データは waiteu-git/hikorobo で公開しています。

SLEIPNIR-ZERO の機体は、飛行中のセンサ計測値を BLE で地上へ送り続けます。この記事では、テレメトリ送信機のハードウェア構成と、限られた無線帯域で「送り切る」ための設計を解説します。

ハードウェア構成

部品	役割
Raspberry Pi Pico 2 W(RP2350)	デュアルコア Cortex-M33 マイコン。無線チップ CYW43 で BLE を担う
BME280	気圧・気温・湿度センサ。気圧から高度を推定する
BNO055	9軸 IMU(加速度・角速度・地磁気)。姿勢と加速度を計測する
WS2812	フルカラー LED。機体の状態を色で表示し、離れた場所から目視確認できる

2つのコアは役割分担しています。core1 がセンサの I2C 読み出し専任、core0 がセンサ融合と BLE 送信の担当で、コア間はロックフリーのリングバッファでつながっています。読み出しの取りこぼしを融合や通信の負荷から切り離すための構成です。

何を送るか

現在飛んでいるのは 50 Hz のテレメトリです。気圧高度・融合高度・上下速度・姿勢(ロール/ピッチ/方位)・線形加速度・制御指令(エレベータ/ラダー/スロットル)などを 1 サンプル 1 行の形式で送り、地上局が CSV として記録します。

さらに、センサの生値をフルレートで降ろすバイナリストリームプロトコルを設計しています。チャンネル構成は次のとおりです。

チャンネル	レート	内容
加速度	1000 Hz	3軸 raw 値(int16 × 3)
角速度	500 Hz	3軸 raw 値(int16 × 3)
地磁気・気圧	20 Hz	3軸 raw 値/気圧+気温
STATUS	1 Hz	取りこぼし数・キャリブレーション状態・死活
CONFIG	変更時	スケール係数・レート設定の通知

生値を raw のまま送るのがポイントです。物理量への換算は PC 側で行い、換算に必要なスケール係数は CONFIG チャンネルで別途通知します。機体側の計算を減らせるだけでなく、換算で情報を落とさずに PC 側の大規模なフィルタ(EKF)へそのまま食わせられます。

帯域の収支

BLE の実効帯域は実測で上限 125 kbps 程度です。上の全チャンネルを合計するとフレーミング込みで約 85 kbps、上限の 68% に収まり、32% のヘッドルームが残ります。また 1 回の notification は 244 バイト以下に厳守しています(これを超えると無線チップが不安定になる実測既知の問題があるため)。

電波状況が悪化して帯域が細った場合は、チャンネルごとに間引き率を上げて縮退します(例:加速度を 1/4 の 250 Hz に)。機内の融合処理は常にフルレートで動き続けるので、最悪「ダウンリンクほぼ停止」までプロトコルを変えずに落とせます。

墜落してもデータは残る

機体内に記録するフライトレコーダ方式と違い、テレメトリは送信した時点でデータが機体を離れます。実験機にとって墜落は起こり得ることですが、直前までの計測値が地上に残っていれば原因分析ができます。0 号機の任務は教訓を集めることなので、これは設計上の重要な性質です。

リポジトリ

ファームウェアと地上局ツール(Python 製の受信 GUI・記録クライアント)は danfunc/flight_robocon_telemetory_sender で公開しています。ファームウェアの土台になっている自作カーネルについては「自作マイクロカーネル Shizuku」で解説しています。

SLEIPNIR-ZERO の最大の特徴は、機体の位置を機体自身ではなく地上から測ることです。この記事では、その仕組みと理由を解説します。

なぜ機体に測位装置を積まないのか

飛行ロボコンは屋内(体育館)での競技なので、GPS は使えません。屋内で機体に自位置を測らせようとすると、カメラや測距センサと、その処理をこなす計算機を機体に積むことになります。ラジコン飛行機クラスの機体にとって、この重量と消費電力は無視できません。

そこで発想を逆にして、位置推定を地上に任せることにしました。地上なら重量制限も電力制限もないので、カメラも計算機も好きなだけ使えます。機体側はセンサの値を無線で送るだけの身軽な構成になります。

仕組み

1. 飛行エリアを見渡せる位置に固定カメラを設置する
2. カメラ映像から画像処理で機体を検出し、位置を推定する
3. 機体から届くテレメトリ(気圧・姿勢など)と合わせて、地上局で機体の状態を把握する

カメラは動かさず固定にしています。カメラが動くと「機体がどこに見えるか」と「カメラがどこを向いているか」の両方を推定する必要が生まれ、問題が一気に難しくなるためです。固定カメラなら、画像内の見え方から機体位置への変換を事前の校正で決めておけます。

テレメトリとの融合

カメラの位置推定は、機体から降りてくるセンサ生値(加速度 1000 Hz・角速度 500 Hz など)と PC 上で突き合わせる計画です。カメラは「位置は正確だがレートが低く、見失うこともある」、IMU は「レートは高いがドリフトする」という互いに逆の性質を持つので、フィルタ(EKF)で融合すると両者の弱点を打ち消せます。機体側の飛行制御に欠けている「位置の基準」を、この融合結果が埋めることになります。

開発状況

地上局のトラッキングソフトは開発中で、リポジトリは準備でき次第公開します。進捗はこのブログで随時報告します。