はじめに

TWELITEでドローンを作ったので報告連絡相談。
TWELITEはモノワイヤレス株式会社製の無線機能を持ったマイコンモジュール。表面実装できるので、コンパクトに仕上がる。すき。
以下にドローンが飛んでいる様子を示す。

主要部品

1. TWE LITE(青いやつ)
2. MONOSTICK(USBタイプのTWE LITE)
3. TWELITE® R2-トワイライター2(TWE LITE書き込み装置)
4. MPU6050(ジャイロ・加速度センサ)
5. GY271(地磁気センサ)
6. VL53L1X(ToFセンサ)
7. XCL1035(5V DCDC)
8. AP7333(3.3Vレギュレータ)
9. AO3400A(N-FET)
10. チップ抵抗(10kΩと100Ω)
11. マイクロモーター(Telloの修理用DCモータを使用)
12. プロペラ(Tello用のやつ)
13. バッテリ(1s 1000mAh)
14. フレーム(3Dプリンタ出力)
15. 実装用基板(FusionPCBに作成依頼)

無駄なく作れば1.5万円くらいで出来上がる。

システム構成

以下にハードウェア構成を示す。
PCからMONOSTICKを経由してTWELITEにコマンドを送信している。TWELITEはコマンドに応じて目標姿勢と高度を算出し、各種センサ値を考慮しながらモータ制御量を算出する。制御量に応じたPWM信号をFETのゲートに入力することでプロペラのついたモータを駆動させ、目標姿勢とか高度とかに追従する。

以下にドローン操作用のデスクトップアプリケーションの画面を示す。ゲームパッドを接続すれば、ジョイスティックで操作可能。

ソフトウェア

TWELITE_Stageという、モノワイヤレス株式会社が用意している環境を使用して開発した。
このURLの「TWELITE STAGE SDK」をダウンロードしてTWELITE_Stageで書き込む。
ファイル構成はこんな感じ。
drone_v001/
　├ build/
　│　└Makefile
　├ 000desc.txt
　└ drone_v001.cpp
「drone_v001」というフォルダ内に「drone_v001.cpp」というソースと「000desc.txt」というソースの内容を記載するファイルと「build」というフォルダという構成。
テンプレが充実しているので、それらを参考に開発した。
ソースコードはまたの機会に・・・

以下にソフトウェア構成を示す。うん、まあ飛べばいいから・・・

制御則

シンプルが一番いい。
まずはピッチのみの二次元でモデル化。


ドローンの慣性能率を二点吊法で概算



モーメントのつり合いを伝達関数で表現

モーターへの制御量と推力の換算係数を得るため、PWMのduty比を変化させてプロペラ推力を計測した。

PD制御。Scilabさんで二次元シミュレーションを行った。

ステップ応答の結果を以下に示す。

まとめ

制御則は制御できるかできないか事前にわかるからまだいい。
問題はセンサー値から姿勢制御に使える姿勢角や高度を得ること。これが難しい。KalmanさんやMadgwickさんの様々な派生形が作られていることからも、万能なフィルタはいまだ存在せず、モデルに応じて考えながら実装せざる負えないのが現状なのだろう。
またTWELITEはIoT向けの無線付マイコンであるためか、高頻度のデータ伝送に向いていないと感じた。100ms周期のコマンド送信でヒーヒー言ってるのでドローンの操作性はあまり良くはない。なにか高頻度でデータ伝送できる方法があれば教えて欲しい。