プログラム仕様

----------------------------------------------------------------

Hotal　プログラム仕様

2022年1月10日から

例 https://drone-ai.blogspot.com/2022/03/blog-post_30.html

----------------------------------------------------------------

（１）プログラムはステートメント（命令文）の並びから構成される。ステートメントは１行に書かれ、複数の行にまたがることはできない。ステートメントの塊はブロックである。ブロックは、それを一つのプログラムとして実行できる。ただし、空行は無視される。１行の中に'#'があると、それも含めそれより右側の記述は無視される。インデントは、空白で行われることが前提である。

（２）実行可能ステートメントは1行から成り立ち、loop、if、マクロを除き、順に実行される。

1,代入文：変数 = 式（評価される、演算子と変数、値を返す関数から成立している）

2,if文

3,loop文

4,関数（実行される）

5.マクロ

のいずれかである。

（３）数値

数値はすべて倍精度数で把握される。小数は小数点を使って表される。浮動小数点は正しく処理されない。

（４）変数

変数は、$から始まる文字列でなければならない。$以降は、アルファベット、数字、'_'アンダーラインのいずれかでなければならない。文字列を変数に入れることはできない。値は全て倍精度である。

例. $abcdef,  $e123,   $abc_d123 など

変数は、最初に現れたところで定義される。同じものが再定義されると、値が無条件に上書きされる

（５）関数

関数は、'^'で始まり、名前、引数をつなげたものである。引数は '('と')'に挟まれる。実引数がない場合も'()'が必要である。関数名は変数名の規則に準じる。引数が複数ある場合は、カンマで区切る。関数は、値を返す機能と機体に関わる何らかの動作を実行するものとがある。また、その両方の場合もある。

（注１）　複数の値を返す関数（getPropellaRpmなど）は、その関数自体は値を返さない（ゼロが返される）が、実行後に、その関数名に '_0', '_1', '_2',・・・がついた一連の変数に値が保存される。次に実行されるときには、上書きされてしまう。持続的に使いたい場合は、それまでに別な名前の変数に代入、保存しておくなどの処理が必要。配列を返す関数が代入文の左辺にある場合、その値はゼロになってしまう。

（６）マクロ

マクロは、プログラムの中（位置はどこでも良い）で定義され

defmacro マクロ名

の行から始まり

endmacro

の行で終わっているブロックである。その部分は、メインの実行行には含まれず、無視される。マクロを呼び出すには、

@マクロ名

と書いた行をメインプログラムの中に置くことにより、その位置でマクロ全体が実行される。何度呼び出されてもよい。マクロの内容は通常の実行ブロックと同じである。変数も、名前空間はないので、同じ名前の変数は、メインと共通になる。変数を区別するためには、変数名の頭にマクロ名をつけるなどの工夫が必要になる。マクロの中で、再帰的にそのマクロを呼び出してもよいが、マクロの中で同じ、あるいは別のマクロを定義してはならない。マクロを正常に呼び出すことができなくなる。

（７）if文

if文は、ステートメントを空白で分解したときに最初にifが記載され、次に論理式が現れるものである論理式が真の場合は、対応するendifか対応するelseのいずれか最初に現れたところまでが実行される。論理式が偽の場合は対応するendifの前に、対応するelseがあれば、そのelseからendifまでが、実行される。elseがなければ、endif以降のブロックの実行に移る。

（８）論理式

論理式は ==, <=, >=, <, >, !=の演算子を持ち。両辺いずれも式として評価され、数値的値の比較が行われ、真偽が判定される。論理式は、現状では単独でしか評価できない。いずれ、論理式も評価できるようにしたい。

（９）loop文

loop文は、ステートメントを空白で分解したときに、最初の項がloopであるもの。続いて、論理式が与えられる。論理式が真である限り、対応するendloopのステートメントまでが繰り返される。論理式が偽になったばあいは、endloopの次のステートメントからの実行に移る。

（１０）今の段階で、実行、評価が可能な関数（今後さらに拡張される）

^throttle():

    スロットルレベルを指定する。引数にスロットルレベル（%）を与える。0が最小。100は、ESCのキャリブレーションで、MAXで設定された電圧レベルになる。

^sleep():

    実行を引数秒だけ中断する。秒は実数（小数可）で与える。

^pidbase():

    PIDの基底値を与える（実数）。３つの値をそれぞれ引数とする

^pidscale():

    PIDのスケール値を与える（整数）。スケールは、50がちょうど、PIDの規定値と同じになるように調整される。３つの値をそれぞれ引数とする。内部的には倍精度で処理されるが、実際の速度調整時にはまた整数化されて処理。PIDスケールは、configファイルで、一定高度にならないと実行されない設定もあるので注意。

^motorSpeedAjustment():

    モータースピードの修正値を与える。引数に四つのモーターの修正値を与える。

^getHight():

    その瞬間の高度を整数で取得する。超音波距離センサー HCSR04を使っている

^getAverageHight():

    ４ループ分の平均高度を得る　HCSR04を使っている

^print():

    引数の内容を標準出力する。引数は、文字列または変数を＋でつなげることができる。変数でなければ、文字列とみなしそのまま出力する。変数は、単独か、+ で区切られていなければならない。

^getTakeoffAltitude(): 

    60センチまでの高度をより正確に取るために使う。倍精度で値を返す。光距離センサーGP2Y0E03を使っている。

^getPropellaRpm(): 

    現在のプロペラ回転数を取得する。変数、getPropellaRpm_0. getPropellaRpm_1. getPropellaRpm_2. getPropellaRpm_3 に自動的に代入される。 

プログラムモードとモーター回転数バランスの自動調整

 ドローンの離陸をマニュアルで試みるのは、なかなか困難だ。私が不器用なのか、歳をとって手の運動神経が老化しているのもあるだろう。そこで、離陸はプログラムで制御するようにしている。

https://youtu.be/RCNzAaKuwZU

これは、私の部屋にちょっとした離陸ベースを置いてプログラムで飛ばしたものだ。コマンドは、１行ずつ実行する。途中に、最後で定義されているマクロが挟まれる。$で始まる名前は、変数だ。^で始まるものは関数。@で始まるものがマクロだ。その他、細かいプログラミングルールは、こちらに書いておいた。

離陸直前のスロットルの状態をすこい維持して、その間にプロペラの回転バランスをとるように、モータースピードを調整するマクロ（@setspeedadjustment）が挟んである。

#-------------------------
# pidscale ver.0 2022年2月28日
# プログラムオリジナル作成
# pidscale ver.1 2022年3月20日
# ホバリング高度に到達した時点でPID制御に入る
# pidscale ver.1 2022年3月27日
# モータースピードの調整を自動化するマクロ追加
#
# Attitudeの姿勢制御ループに対するコマンドは、
# 繰り返しの時間を念頭に置かないとコマンドが有効にならない
#-------------------------
#基本定数
# テスト用
#$takeoffthrottle = 30
#$hoveringthrottle_max = 30;
#$hoveringthrottle_min = 30;
# 本番用
$takeoffthrottle = 39
$hoveringthrottle_max = 43;
$hoveringthrottle_min = 37;
#
^motor(on)
# motor on の有効化を0.1秒待つ：必須
^sleep(0.1)
# Arming
^throttle(20)
# sleep の引数は実数値可
^sleep(1.5)
^throttle(30)
# 回転数が上がる時間を稼ぐ
^sleep(0.7)
# モータースピード調整マクロの実行
# このスピードで、1秒使われる
@setspeedadjustment
# 調整に馴染ませる時間をとる
^sleep(0.5)
# 離陸
^print(離陸開始します)
^throttle($takeoffthrottle)
##############################
# 離陸高度チェックループ 開始 
# ループを正しく抜けれるかどうか事前テストすること
##############################
$count = 0
$flag = -1
loop $flag < 0
    $altitude = ^getTakeoffAltitude()
    ^print(離陸：No.+$count+ 現在高度 [ +$altitude+ ])
    # 高度が指定の高さ以上になるまで空回りする
    if $altitude > 30
        ^print(離陸：指定高度の30cmを超えたのでループを抜けます)
        $flag = 1
    endif
    # 10msec 停止する
    ^sleep(0.01)
    $count = $count + 1;
    # 指定高度に0.5秒で到達しなかったら強制的にループを抜ける
    if $count > 50
        ^print(離陸：規定チェック回数を超えたのでループを抜けます)
        $flag = 1
    endif
endloop
##############################
# 離陸高度チェックループ 終了 
##############################
^print(PID制御を開始します)
# PID制御に入る
# 最高高度に入る直前だと思われる
^pidscale(50,20,50)
^throttle($hoveringthrottle_max)
^sleep(0.7)
^throttle($hoveringthrottle_min)
##############################
# ホバリングループ 開始 
# $hoveringthrottle_min では、下降してしまうので
# スロットルを調整する
##############################
# 変数再初期化
$count = 0
$flag = -1
loop $flag < 0
    # 60cm 以上の高度を想定していないので、こちらの高度センサーを使う
    $altitude = ^getTakeoffAltitude()
    ^print(ホバリング：No.+$count+ 現在高度 [ +$altitude+ ])
    # 高度が下がったら再上昇を試みる
    if $altitude < 45
        ^print(ホバリング：高度が45cm以下になったのでスロットルを上昇させます)
        ^throttle($hoveringthrottle_max)
    endif
    if $altitude > 55
        ^print(ホバリング：高度が55cm以上になったのでスロットルを低下させます)
        ^throttle($hoveringthrottle_min)
    endif
    # 10msec 停止する
    ^sleep(0.01)
    $count = $count + 1;
    # ホバリングを指定時間試みたらループを抜ける
    if $count > 100
        ^print(ホバリング：1秒経過したのでループを抜けます)
        $flag = 1
    endif
endloop
##############################
# ホバリングループ 終了
##############################
# ^sleep(0.5)
#########################
# 着陸に入る
# スロットルを少しずつ下げる
#########################
^print(着陸します)
^throttle($hoveringthrottle_min)
^sleep(0.3)
^throttle(30)
^sleep(0.4)
^throttle(25)
^sleep(0.4)
# throttleの有効化の前にmotor off にならないように待つ。0.1秒以上
# また、モーターが完全に停止するまでのログを取るという意味もある。それで3秒にしている
^print(モーターを停止させます)
^throttle(0)
^sleep(2)
^motor(off)
# メインプログラム終了

defmacro setspeedadjustment
#################
# スピード調整設定マクロ
# スロットル関係の関数は使っていないので、
# ここに入る前にスロットルは調整すべき
 #################
^print(スピード調整設定マクロを実行します)
# 調整スピードをゼロに設定：初期化
^motorSpeedAjustment(0.0,0.0,0.0,0.0)
# 各プロペラ回転数の合計
$psum_0 = 0
$psum_1 = 0
$psum_2 = 0
$psum_3 = 0
^print(調整スピードをゼロに設定：初期化 ループの開始)
# 誤差取得のループ
$flag = -1
$count = 0
loop $flag < 0
    #プロペラ回転数を取得する
    ^getPropellaRpm()
    ^print(No.+$count+ 回転数 + $getPropellaRpm_0 +,+ $getPropellaRpm_1 +,+ $getPropellaRpm_2 +,+ $getPropellaRpm_3)
    # 取得した回転数を加える
    $psum_0 = $psum_0 + $getPropellaRpm_0
    $psum_1 = $psum_1 + $getPropellaRpm_1
    $psum_2 = $psum_2 + $getPropellaRpm_2
    $psum_3 = $psum_3 + $getPropellaRpm_3
    $count = $count + 1
    if $count >= 100
        # 1秒測る
        $paverage_0 = $psum_0 / $count
        $paverage_1 = $psum_1 / $count
        $paverage_2 = $psum_2 / $count
        $paverage_3 = $psum_3 / $count
        ^print(1秒経過で測定終了 平均： + $paverage_0 +,+ $paverage_1 +,+ $paverage_2 +,+ $paverage_3)
        # 終了フラグを立てる
        $flag = 1
    endif
    ^sleep(0.01)
endloop
# ４つのモーターについてさらに平均を求める
$totalaverage = ($paverage_0 + $paverage_1 + $paverage_2 + $paverage_3)/4
^print(全体平均回転数 + $totalaverage + rpm)
# 平均との差から調整値を求める
# 調整係数
$adjusttingdeflator = 200
$adjust_0 = ($paverage_0 - $totalaverage)/$adjusttingdeflator
$adjust_1 = ($paverage_1 - $totalaverage)/$adjusttingdeflator
$adjust_2 = ($paverage_2 - $totalaverage)/$adjusttingdeflator
$adjust_3 = ($paverage_3 - $totalaverage)/$adjusttingdeflator
# 調整値をセットする
^print(モータースピード調整値： + $adjust_0 +,+ $adjust_1 +,+ $adjust_2 +,+ $adjust_3)
^motorSpeedAjustment($adjust_0,$adjust_1,$adjust_2,$adjust_3)
# マクロの終了
^print(スピード調整設定マクロを終了します)
endmacro

剛体のシミュレーションとフライトコントローラーのパラメータをリンクさせる

先の 剛体としてののドローンのシミュレーションでは、無駄時間を考慮してもPD制御で安定化が図れることがわかった。

新たな機体が組み上がったので、そのフライトコントローラーのシステム（以下Hotalとよぶ）にシミュレーションのパラメーターを適応させることにした。

Hotalでは、加速度センサーから取られた値を相補フィルターにかけて、ラジアンで機体角度を出してきている。その値を100倍して（スロットルの%をイメージしている）PあるいはD制御のパラメーターをかけて、PWMモジュール（PCA9685）にその値を送り、ESCに回転速度要求信号として送り込んでいる。

PWMモジュールに送った値とモーターの回転数との関係がわからなければ、この関係をとらえられないので最新のフライトのログをもとに回帰分析をした。

飛行を開始した時点から終了までの、モーター１だけを取り出してまず相関関係をみた。

まあ、ばらついているが正の相関を確認できる。

回帰分析した結果は、

これをもとに計算する。シミュレーションの300に対するHotalのPパラメーターを$H_{p}$とすると、

$$100H_{p}\times 117.48=300$$

よって、

$$H_{p}=0.0255$$

となるDパラメータも同じである。

これは直感的に、いいところをついたパラメータとなっている。このパラメータの前後で実機のフライトコントローラーのPD制御パラメータを探れば良い。

raspberrypi 起動時にIPアドレスをAQM0802に表示する

 基本、wifiでドローンと繋げているので、以前は固定IPをラズパイに設定してつないでいた。しかし、ラズパイの固定IP設定はトラブルが多いので、やめた。ルーターDHCPからIPアドレスをとるとある程度固定していて、たまに変わっても１増やすくらいなので、それでもいいのだが、やはり面倒。

そこで、DHCPで取った IPアドレスを起動時にLCD、キャラクタディスプレイに表示させるようにした。

LCDは、AQM0802という定番のものを使った。基盤付きのやつがいい。最初間違って、Arudinoのようのものを買ったが、プルアップ電圧が違うようで、I2Cアドレスの認識すらできなかった。ラズパイ用にセットアップされたものが820円で秋月電子で売っている。

https://akizukidenshi.com/catalog/g/gK-11354/

これを使った。自分でプログラムを書かなければと思っていたが、全く同じことをやっている方がおられた。

https://qiita.com/JHiyama/items/158ab35ed0247a7fc406

助かった。

なお、無線LANのみの場合はほぼそのままでいいが、同時にケーブルを繋いでいる場合は、f2にしなければ正しく取れない。無線LANのIPアドレスは２番目に書かれているので。

剛体としてのドローンのシミュレーション（３）：実行

先の、剛体としてのドローンのシミュレーション（２）：むだ時間ありモデルで定式化したモデルを実際にシミュレーションしてみよう。

シミュレーションで使ったプログラム（C++）は、以下のサイトからダウンロードできる。

https://github.com/toyowa/pidsimulation

そのサイトの中の、timelag-2.cppというプログラムを選択してダウンロードしてください。前の記事の最後の縮約したものではなく、その手前の漸化式をそのまま入れている。実行すると、実行した日時（秒単位）の付いたファイルの中に、ログを吐き出す。それはCVS形式なので、EXCELなどで計算やグラフィカルな処理ができる。ログファイルの冒頭に、実行時のパラメータが書かれている。

パラメータは基本以下のように定義されている。

$ホバリングプロペラ回転数 V_{0} = 13000 rpm$
$モーター起動力パラメータ \delta = 1e-08$
$機体慣性モーメント I = 0.01 Kg*m^{2}$
$\eta = 0.01924 $
$中心からモーター位置までの長さ L = 0.37 m$
$PID制御パラメータ P = 300$
$PID制御パラメータ D = 300$
$シミュレーションステップのh = 0.01 sec$
$シミュレーション期間（1期間10ms） T = 2000 periods$
$むだ時間：遅延 \tau = 20 期間、時間では 200ms$
$初期 \theta = 0.5 radian$
$初期 \omega = 0 radian/sec$

パラメータは、基本自作のドローンから取ってきているが、慣性モーメントIだけは、実測する元気が湧かず、似たような機体を使った論文の値に近いものを考えている。そうなるとI=0.1くらいなのだが、このリファレンスパラメータでは、あえてその十分の１と、小さな値に設定している。すなわち、他の諸元はやや大きなドローンだが、慣性モーメントだけは小型ドローン、下手すればマイクロドローンくらいのこじんまりしたドローンになっている感じだ。実測もしていないので、この想定も外れているかもしれないが。

このパラメータを基本に、シミュレーションを実行する。変更する場合は、変更したパラメータのみを表示する。

（１）基本パラメータによるシミュレーション

まず、上記のパラメータをそのままに実行した場合の結果を示そう。

かなり短い時間で安定したホバリングを回復していると思われる。オレンジ色がプロペラの回転数で、ホバリングの回転数は1300rpmにしているので、そこへ収束している。機体のロール角（青色の線）もまたゼロ（右の目盛りで）に向かって収束する。

収束に至るまで、約1秒周期の揺れが起こっている。機体の慣性モーメントは小さいが、モーターまでの腕の長さはかなり大型のドローンになっているので、それほど周期の短い揺れが現れることはないのだと思う。

何よりも、200msのむだ時間があるにもかかわらず、このような収束結果を見せることには驚いた。

（２）慣性モーメントを0.1でシミュレーション

慣性モーメントを10倍にした結果は次のとおりである。

収束に向けてより時間がかかっているように見えるが、そもそも揺れの振幅が小さいので、それはあまり問題にならない。慣性モーメントが大きくなったことによって、揺れの周期が大きくなっている。１周期が5秒くらいになっているようだが、収束への過程として見ると、あまり問題にはならないのではないか。

（３）慣性モーメントを0.1、制御パラメータをP=100, D=50に低下させた

慣性モーメントを大きくしたまま、制御を弱めた。

当然ながら、大きな揺れのまま、収束させる力が弱まっている。

（４）慣性モーメント 0.1で、P制御パラメータを1000に増加させた

結果は次のようになる。

慣性モーメントは大きくしたままであるにもかかわらず、収束に向けた揺れの周期が短くなった。

他にも色々試みたが、全体として収束傾向は変わらなかった。すなわち、むだ時間がしっかりあるにもかかわらず、剛体としての慣性モーメントを考慮すると、ホバリングの安定性がある程度確保できるのである。これは大きな発見だった。

これで、ドローンのホバリング安定性に関わる数値的分析は、一通りやったことになる。

剛体としてのドローンのシミュレーション（２）：むだ時間ありモデル

 PID制御に信号と動作の間の遅延があった場合を分析する。この遅延は「むだ時間」と呼ばれている。

先のシミュレーションの目的もこのむだ時間があった場合の分析を目的としていた。そして、むだ時間があるとPID制御に大きな困難をもたらすことがわかった。その点が、モデルを剛体を考慮したものにしたときどうなるのかをここで調べる。

以前のシミュレーション記事のリストは以下にあるので、必要に応じて参照していただきたい。

目次：PID制御によるドローンの揺れに関する数値シミュレーション

剛体としてみたドローンモデルの運動式は、

$$I\frac{d\omega}{dt}=L(f_{1}-f_{2})$$

となり、前の記事と同じである。ESCからの信号とトルク発生の間にむだ時間が発生するとしよう。むだ時間を$\tau$で表す。これによって回転トルクを発生させる力$f_{1}, f_{2}$は次のように表される。ただし、$\omega$は角速度で、$\omega=d\theta/dt$である。

$$f_{1}=\delta\left(V_{0}-P\theta_{t-\tau}-D\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)^{2}=\delta\left(V_{0}-P\theta_{t-\tau}-D\omega_{t-\tau}\right)^{2}$$

$$f_{2}=\delta\left(V_{0}+P\theta_{t-\tau}+D\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)^{2}=\delta\left(V_{0}+P\theta_{t-\tau}+D\omega_{t-\tau}\right)^{2}$$

右辺は、$\tau$期前のPID制御の信号に基づいてプロペラの回転数が実現しトルクが発生していることを示している。この右辺をラグ関数として$lag(\theta, \omega)$としよう（システムが前のものとは簡単になったのであえて定義する必要がないかもしれないが）。すなわち、

$$lag(\theta, \omega)=-\frac{4L\delta V_{0}}{I}(P\theta+D\omega)$$

となる。そして、次のような、一階の連立微分方程式を考える。

$$\frac{d\theta_{t}}{dt}=\omega_{t}$$

$$\frac{d\omega_{t}}{dt}=lag(\theta_{t-\tau}, \omega_{t-\tau})$$

これに4次のルンゲ・クッタ法を適応する。（ルンゲ・クッタ法については、前のシミュレーションの（２）の記事などを参考にしていただきたい）

これは（６）の記事にある漸化式から$\cos \theta$に関わるものを除いた式と、形が一致するので、それを載せると次のようになる。

$$\begin{eqnarray*}k_{1}^{\theta}&=&h\omega_{n}\\k_{1}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{2}^{\theta}&=&h(\omega_{n}+\frac{k_{1}^{\omega}}{2})\\k_{2}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{3}^{\theta}&=&h(\omega_{n}+\frac{k_{2}^{\omega}}{2})\\k_{3}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{4}^{\theta}&=&h(\omega_{n}+k_{3}^{\omega})\\k_{4}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\\theta_{n+1}&=&\theta_{n}+\frac{1}{6}(k_{1}^{\theta}+k_{2}^{\theta}+k_{3}^{\theta}+k_{4}^{\theta})\\\omega_{n+1}&=&\omega_{n}+\frac{1}{6}(k_{1}^{\omega}+k_{2}^{\omega}+k_{3}^{\omega}+k_{4}^{\omega})\end{eqnarray*}$$

これは機械的に変換したが、結果的に $k_{1}^{\omega}=k_{2}^{\omega}=k_{3}^{\omega}=k_{4}^{\omega}$ であるからこの値を$k^{\omega}$とおく。漸化式は次のようになる。

$$k^{\omega}=h\cdot lag(\theta_{n-\tau}, \omega_{n-\tau})$$

$$k_{1}^{\theta}=h\omega_{n}$$

$$k_{2}^{\theta}=h(\omega_{n}+\frac{k^{\omega}}{2})$$

$$k_{3}^{\theta}=h(\omega_{n}+\frac{k^{\omega}}{2})$$

$$k_{4}^{\theta}=h(\omega_{n}+k^{\omega})$$

$$\theta_{n+1}=\theta_{n}+\frac{h}{3}(2\omega_{n}+ lag(\theta_{n-\tau}, \omega_{n-\tau}))$$

$$\omega_{n+1}=\omega_{n}+\frac{2h}{3} \cdot lag(\theta_{n-\tau}, \omega_{n-\tau})$$

これによるシミュレーション結果は、次の記事で示す。

剛体としてのドローンのシミュレーション（１）：基本モデル

以下のリストにある以前のシミュレーションは、ドローンをローターの位置のみを質点としてみたシミュレーションだった。

 目次：PID制御によるドローンの揺れに関する数値シミュレーション

すなわち、４ロータードローンならば、機体重が4つのローター位置に分散している4質点を想定したものだった。それでも、PID制御の特質を捉えることはできると思っているが、実体的には、ドローンを連続的な質点が分布している剛体として分析すべきだろう。

そこで、引き続き、極力、単純性を確保しながら剛体としてのPID制御によるドローン姿勢制御のシミュレーションを試みることにした。

空間を自由に姿勢を変えながら動くドローンの運動は、オイラーの運動方程式を使うかラグランジュの方程式を使うかという選択があるが、ここではオイラーの運動方程式を、一軸固定で簡単化して使うことにする。

そこで、ローターを前から右回りに１、２、３、４と番号をつけ、１番画のロータが最前方にあるとし、１と３のローターを貫く方向を軸とした回転をロールとして、２、４を軸とする回転をピッチとしよう。そして、ロールの回転だけ分析することにする。

ここで、ドローンのロール方向の慣性モーメントを$I$としよう。慣性モーメントは回転のしにくさ、あるいは回転のおもたさのようなものであり、ドローンの部品の重さと位置を悉く把握できれば、近似的に計算できなくもないが、なかなか面倒なものである。現実の物体の動きから逆算的に慣性モーメントを推定したりすることもあるようだ。ここでは何らかの方法で計算されているとしよう。

このように慣性モーメントが出てくるところが、剛体としてのドローンを捉える最も重要なステップであると言える。

二つのローターによる推力は、$f_{1}$と$f_{2}$によって与えられる。前後の軸、ここでは$x$方向の軸は固定されているとする。したがって、この固定された軸の回転のみを解析することになる。したがって上下方向の動きは想定していない。PID制御による揺らぎからの安定性のみを分析するということである。

左回りを正の方向とした機体角度を$\theta$で表す（モータの一関係上、ロール角の方向は負となるが特に問題ではない）。その正の方向に回転する角速度を$\omega$とする。したがって角速度ベクトルは画面手前側（$x$軸の正方向）に向いていることになる。ただ、この方向はあまり関係はない。機体の中心を原点$O$として、ここからローターまでの長さを$L$で表す。

この１軸固定の剛体の運動方程式は次のように表される。

$$I\frac{d\omega}{dt}=L(f_{1}-f_{2})$$

オイラーの運動方程式で、一つの回転方向しか見ない特殊な場合でもある。この辺りのわかりやすい理論的説明は原島鮮氏の『力学I 質点・剛体の力学（新装版）』（p.193）に与えられている。

運動方程式の右辺は、二つのローターによるトルクの差を表している。このトルク差が回転の動力を与えるのである。

プロペラの回転による$f_{1}$および$f_{2}$の変化は、次の各式で与えられると想定する。設定は、前のシミュレーションモデルと同じであり、必要ならば参照していただきたい。

$$f_{1}=\delta\left(V_{0}-P\theta-D\frac{d\theta}{dt}\right)^{2}$$

$$f_{2}=\delta\left(V_{0}+P\theta+D\frac{d\theta}{dt}\right)^{2}$$

$V_{0}$は、ホバリングを実現するプロペラの回転数で１軸固定を正当化する力である。$P$は、PID制御のPパラメータで、単位としては機体のロール角（ラディアン）をプロペラ回転数に変換するものである。また、$D$は、PIDの微分制御で、ロールの角速度をプロペラ回転数に変換するパラメータである。ここでは、機体の傾向的偏差を補正する　$I$制御は考慮しない。

プロペラの回転数は、その平方値が力に比例すると考える。また、その変換パラメータが$\delta$になっている。

このトルクに関する二つの式を運動方程式に代入して整理すると次の式を得る。

$$I\frac{d\omega}{dt}=-4L\delta V_{0}\left(P\theta+D\frac{d\theta}{dt}\right)$$

さらに、$w=\frac{d\theta}{dt}$を用いて整理すると次のようになる。

$$\frac{d^{2}\theta}{dt^{2}}+4\frac{L\delta V_{0}D}{I}\frac{d\theta}{dt}+4\frac{L\delta V_{0}P}{I}=0$$

ここで、

$$4\frac{L\delta V_{0}}{I}=\eta$$とおく。この$\eta$は時間に依存しない定数である。すると式は、

$$\frac{d^{2}\theta}{dt^{2}}+\eta D\frac{d\theta}{dt}+\eta P=0$$

となり、典型的な二階の線形常微分方程式となった。もっと複雑になるかと思ったが、予想外に単純なモデルとなったことにとても驚いた。

これは微分方程式の教科書にも必ず書いてあるもので、解析的に解くことも容易である。

特性方程式は次のようになる。

$$\lambda^{2}+\eta D\lambda+\eta P=0$$

この式の判別式を$J$とすると、

$$J=\eta^{2}D^{2}-4\eta P = D^{2}\eta\left(\eta-\frac{4P}{D^{2}}\right)$$

三つの場合に分けることができる。

（１）$0<\eta<\frac{4P}{D^{2}}$のとき、$J<0$で、特性式を満たす$\lambda$は、虚数になる。

$$\lambda = \frac{-\eta D\pm \sqrt{\eta^{2}D^{2}-4\eta P }}{2}=\alpha\pm\beta I$$

とすると、解である$\theta$は、次のようになる。

$$\theta=e^{\alpha t}(C_{1}\cos{\beta t}+C_{2}\sin{\beta t})$$

ここで、$C_{1}, C_{2}$は、初期条件から与えられる未定条数で、これで初期の撹乱を与えることができる。。

$\alpha=-\frac{\eta D}{2} < 0$であるから、この場合は、機体角度は振動しながら減衰し、ホバリング状態に近づいていく。

（２）$J=0$で、重根のとき。

$\lambda=\alpha=\frac{-\eta D}{2}$として、

$$\theta=e^{\alpha t}(C_{1}+C_{2}t)$$

で、機体角度は、揺れることもなく、ホバリング状態に減衰していく。

（３）$J>0$で、実数解のとき。特性方程式の解を$\lambda_{1}, \lambda_{2}$とすると、

$$\theta=C_{1}e^{\lambda_{1}t}+C_{2}e^{\lambda_{2}t}$$

となる。特性方程式は二次曲線だが、その中心は負にあり、かつ$\lambda=0$の左辺の値は、正なので、この二つの解は、必ず負である。したがって、この場合もまた一方的に機体角度はホバリング状態に近づいていく。

機体に撹乱があったとき、どのような状況でも、時間の速さや揺れがある内の違いはあっても、ホバリング状態に戻ることがわかった。また、$\eta$の絶対値が大きくなると、ホバリング状態への復帰が早くなる。$\eta$の定義から、他が同じで、慣性モーメントが大きくなるとホバリング状態への復帰が遅くなることを意味している。期待が大きくなると必然的に慣性モーメントは大きくなるので、安定化に時間がかかるということだ。ある意味当然の結果だと言える。

他のパラメータの影響も色々調べることはできるが、それはまた機体の設計上、必要に応じてやるのが効率的なので、ここではこれ以上の分析はスキップする。

確認のために、数値的なシミュレーションを実行しよう。ただし、解析的にある程度解がわかっているので、パラメータを実機に近いものにする面倒は避けておく。

（１）虚数解のとき

パラメータを条件に合わせて次のようにする。$c_{1}=2, c_{2}= 1, \eta=0.0015, D=2, P=1$このときステップを1秒として実行すると解の機体角度$\theta$は次のようになる。

理論的に予測された通りである。

重根のときは省略。

（２）実数解のとき

パラメータは、虚数解と比べて$P=0.001$だけを変更する。P制御を弱めた形になる。結果は次のようだ。

一方的減数で、予想どおりだ。

これらの結果は、PID制御が有効なことを表しているが、問題は、制御に遅延がある場合、すなわちむだ時間がある場合である。先のシミュレーションでは、むだ時間がある場合は、安定化はとても難しかった。

また、むだ時間を入れる場合は、解析的な解は望めない。その辺りを次に検討するつもりだ。