箱庭で作ったドローン機体の制御方法について考えてみよう

今の箱庭ドローン・シミュレータについて説明する連載ブログ、第２回目は、制御側のお話です（前回のブログはこちら）。

箱庭で作ったドローン機体、やっぱり動かしてみたいですよね。

でも前回の式の意味を理解しないと、どう手を出せば良いかわからないので、前回の数式をベースにして、ドローンの動作原理の話を少しだけ挟もうと思います。

ローターのプロペラを回転するとどうなるか

ドローン機体を動かすためには、ローターのプロペラを回転させる必要があります。

ただ、どのローターをどれだけ回転させれば良いでしょうか。

思い通りにドローンを動かすためには、ローターの回転数と推力、トルクの関係式が必要となりそうです。

そして、そのための変換式が、こちらですね。

ω1〜4 が各ローターの回転数です。ローターの回転数の二乗が、推力u、トルク（τφ、τθ、τψ）になります。

例えば、ω1〜4 をすべて同じ回転数で重力を超える推力uが得られれば浮上します。

また、ω4がω2 よりも大きな回転数であれば、τφが正の値となります。

τφは、機体の姿勢を左側に回転するトルクなので、ドローンは左に傾き、結果として左方向に移動します。

同じように、ω3がω1よりも大きな回転数であれば、τθが正の値となります。

τθは、機体の姿勢を前方向に回転するトルクなので、ドローンは前に傾き、結果として前方向に移動します。

τψは、ドローンを z 軸方向の回転を与えるトルクです。

ローターの特性として、プロペラを回転すると回転の逆方向にトルクがかかります。

ローターのプロペラが全部同じ方向で回転してしまうと、ドローンがくるくる回ってしまいます（そのため、ドローンのローターは交互に回転方向が逆になっています）。

ω１とω３の方が、ω２とω４よりも回転数が大きければ、τψは正の値になり、z軸方向に回転します。

推力とトルクからローターの回転数を求めたい

以上より、ローターの回転数を適切に与えれば、ドローンを期待した動きを作れそうです。

ただ、ωの回転数だけでドローンの動きをイメージするのは難しいと思いませんか。

できれば、式9の左辺の値で考えたいですよね。「推力はこれだけで、トルクはこれだけで」と。

そのためには、式9の左辺値を与えて、各ローターの回転数を決めたくなると思います。

そのために、以下のように逆行列を作りました。

これで、ドローンに与えたい力（u、τφ、τθ、τψ）を決めると、各ローターの ω^2 が決まります。

これをC言語で書いたものがこちらです。

#define CONST_P    (0.25f)
#define CONST_LP   (0.5f)
#define CONST_K    (0.25f)

    out.w[0] =    ( ( CONST_P  / phys.param.p ) * signal.thrust )
                - ( ( CONST_LP / (phys.param.l * phys.param.p )  ) * signal.torque.y )
                + ( ( CONST_K  / (phys.param.k )  ) * signal.torque.z )
                ;
    out.w[1] =    ( ( CONST_P  / phys.param.p ) * signal.thrust )
                - ( ( CONST_LP / (phys.param.l * phys.param.p )  ) * signal.torque.x )
                - ( ( CONST_K  / (phys.param.k )  ) * signal.torque.z )
                ;
    out.w[2] =    ( ( CONST_P  / phys.param.p ) * signal.thrust )
                + ( ( CONST_LP / (phys.param.l * phys.param.p )  ) * signal.torque.y )
                + ( ( CONST_K  / (phys.param.k )  ) * signal.torque.z )
                ;
    out.w[3] =    ( ( CONST_P  / phys.param.p ) * signal.thrust )
                + ( ( CONST_LP / (phys.param.l * phys.param.p )  ) * signal.torque.x )
                - ( ( CONST_K  / (phys.param.k )  ) * signal.torque.z )