UnityでPID制御を使って位置と姿勢をコントロールする

空中に浮かんでいるオブジェクトをインタラクティブに動かしたかったため，PID制御を使って実装してみました．宙に浮かんでいる謎物体を対象にしているため，力とトルクは任意の方向に発揮できるという前提で組んでいます．
移動中に衝突が発生しない，移動中に目標位置を変更しないといった決め打ちで動かす場合は Dotween などの Tween 系アセットを使いましょう．

なお，今回の話は起動追従ではありませんし，ミサイルや誘導弾には向かないのでご注意ください．

コード
位置制御
姿勢制御
人工衛星って…

コード

実装したコードはこちらです．

位置制御

位置制御はPIDの基本に乗っ取った簡単な実装で実現できます．つまり，目標位置 $\vec{x}^d$ と現在位置 $\vec{x}$ を使って，以下のような計算で推力 $\vec{f}$ を計算します．

\vec{e} = \vec{x}^d-\vec{x} \\ \vec{f} = k_p \vec{e} + k_i \int \vec{e} dt + k_d \dot {\vec{e}}

実装では target.trasnsform.position と gameObject.transform.position の差からforceを計算してrigidbody.AddForce()で推力を適用します．

姿勢制御

おそるべしクォータニオン

姿勢制御は位置制御と比べると非常に厄介です．目標姿勢 $q^d$ と現在姿勢 $q$ を使うとします(クォータニオン)．位置制御と同じ考え方でトルク $\vec{\tau}$ 計算すると次のようになります．

e = q^d-q \\ \vec{\tau} = k_p e + k_i \int e dt + k_i \dot{e}

ところが，次のような問題があります．
– クォータニオンを引いても意味がない
– 姿勢の差を計算するためには逆クォータニオンをかけなければいけない
– クォータニオンの微分積分の持つ意味とは？
– $\vec{\tau}$ はベクトル，姿勢はクォータニオン．2番目の式の左辺，右辺の次元が合わない．
残念ながら私にはこの問題を解決することはできませんでした．

慣性テンソルの罠

微分積分は忘れて，現在の目標との姿勢の差だけを考えます．この時姿勢の差$e$は次のように書けます．

e = q^d q^{-1}

$e$ を回転ベクトルに変換したものを $\vec{r}$ とします．さらに目標と制御対象の角速度ベクトルの差を $\vec{v}$ とします．そうすれば． $\vec{\tau}$ は次のように計算できそうです．

\vec{\tau} = k_p \vec{r} + k_i \vec{v}

残念ながら，この $\vec{\tau}$ を適用してもうまくいきません(収束しません)．原因は慣性テンソルの非対角成分です．例えば，x軸周りにのみ誤差がある場合， $\vec{\tau} = (1\ 0 \ 0)$ みたいになりますが，非対角成分のせいで y軸，z軸周りにも角運動量が発生します．したがって，このアプローチによる制御は困難です．