🛸

PyBulletでドローンを飛ばす　その３

2022/03/11に公開

~~今回で最後です~~（次の記事追加しました）。実際にドローンをPID制御で飛ばしてみます。
PID制御に関しては、参考になるような情報がネット上に多々ありますので割愛。それ以外のポイントして、私がオリジナルのリポジトリをトレースする際に引っかかった箇所について説明してみます。

前回、ドローン周りの力学について説明しました。

その際にも記述しましたが、ドローンが有する4つの各ロータに発生するトルクは、ロータ回転数の2乗に比例するそうです。

このモデルに基づき、力学的なつり合いを連立方程式で表現すると以下のようになります。

\begin{equation} \begin{pmatrix} 1 & 1 & 1 & 1 \\ 0 & 1 & 0 & -1 \\ -1 & 0 & 1 & 0 \\ -1 & 1 & -1 & 1 \\ \end{pmatrix} \begin{pmatrix} {n_0}^2 \\ {n_1}^2 \\ {n_2}^2 \\ {n_3}^2 \\ \end{pmatrix} = \begin{pmatrix} T / K_t \\ Q_x / (K_t \cdot L) \\ Q_y /(K_t \cdot L) \\ Q_z / K_m \\ \end{pmatrix} \end{equation}

$n_i , (0{\leq}i{\leq}3)$ : 各ロータの回転数
$T$ : スラスト（推進力）
$Q_x, Q_y, Q_z$ : 各軸周りのトルク
$K_t$ : ロータに生じるスラストの比例定数（ロータ回転数の二乗に比例）
$K_m$ : ロータに生じるモーメントの比例定数（ロータ回転数の二乗に比例）
$L$ : ドローンの重心からロータ設置位置までの距離

この式 $(1)$ は意外と単純で、第一式は4つのロータのスラストを合計しただけのものです。第二、三式は、上図fig3.に基づきX軸、Y軸回りに生じるトルクを表現したもの。第四式はZ軸回りのトルクですが、これは4つのロータのモーメントを合計したものにすぎません。

では、ドローンを制御することを考え、各軸周りのトルク $Q_x, Q_y, Q_z$ という状態で、ドローンに推力 $T$ を与えたい場合、各ロータの回転数 $n_0 , n_1, n_2, n_3$ をどのように設定すればよいでしょうか？

これもあっさり解決で、式 $(1)$ に逆行列をかけてやれば欲しかった $n_0 , n_1, n_2, n_3$ に関する解が求まります。

\begin{equation} \begin{pmatrix} {n_0}^2 \\ {n_1}^2 \\ {n_2}^2 \\ {n_3}^2 \\ \end{pmatrix} = \begin{pmatrix} 1 & 1 & 1 & 1 \\ 0 & 1 & 0 & -1 \\ -1 & 0 & 1 & 0 \\ -1 & 1 & -1 & 1 \\ \end{pmatrix} ^{-1} \begin{pmatrix} T / K_t \\ Q_x / (K_t \cdot L) \\ Q_y /(K_t \cdot L) \\ Q_z / K_m \\ \end{pmatrix} \end{equation}

はい、これにて一件落着といきたいところですが、実際にはちょっと厄介な問題が発生します。

式 $(2)$ を解いて求まる答えは、各ロータの回転数の2乗に対応する解です。この解が負の値になった場合、どうすれば良いでしょうか？
実際のドローンのロータ回転数に虚数を割り当てることはできませんよね・・・

この問題を回避する手段として、オリジナルのリポジトリではNon-negative least squareという方法が採られていました。nnlsは回帰分析の一種で、解が非負となる制約を設けた状態で、誤差が最小となるようなフィッティングをしてくれるものだそうです（非負解で最小二乗法を解く？）。そして、nnlsはscipyに組み込まれており、本件でもこの実装を利用しています。すげーなscipy守備範囲がヤバい。

実際には以下のような関数として纏めています。

./cntrol/drone_ctrl.py

from scipy.optimize import nnls

def compute_rpm_by_nnls(
        thrust: float,
        x_torque: float,
        y_torque: float,
        z_torque: float,
        b_coeff: np.ndarray,
        a: np.ndarray,
        inv_a: np.ndarray = None,
):
    """
    Parameters
    ----------
    thrust : Desired thrust along the drone's z-axis
    x_torque : Desired drone's x-axis torque. 理想的なX軸周りのトルク
    y_torque : Desired drone's y-axis torque. 理想的なY軸周りのトルク
    z_torque : Desired drone's z-axis torque. 理想的なZ軸周りのトルク
    b_coeff : (4,1)-shaped array of floats containing the coefficients to re-scale thrust and torques.
    ↑式(1)の右辺の行列
    a : (4, 4)-shaped array of floats containing the motors configuration.
    ↑式(1)の1と-1で構成されている行列
    inv_a : (4, 4)-shaped array of floats, inverse of 'a'. 「a」の逆行列

    Returns
    -------
        (4,)-shaped array of ints containing the desired squared RPMs of each rotor.
    """
    B = np.multiply(np.array([thrust, x_torque, y_torque, z_torque]), b_coeff)
    inv_a = np.linalg.inv(a) if inv_a is None else inv_a
    sq_rpm = np.dot(inv_a, B)
    # sq_rpmは4つのモータのそれぞれの回転数を2乗したものが格納されているという前提
    # ただし、普通に行列式を解くとマイナスになる解が出る場合がある。回転数を2乗したものを解としたいのでこれは都合が悪い。
    # そのような場合に、解の全てを正にするという制約を与えて回帰する方法としてNNLSがある。
    sq_rpm_nnls, res = None, None
    # NNLS if any of the desired ang vel is negative
    if np.min(sq_rpm) < 0:
        sq_rpm_nnls, res = nnls(a, B, maxiter=3 * a.shape[1])

    return sq_rpm, sq_rpm_nnls, res

これらのツールを使って実装したものがこちらです。

（尚、参考元の bym-pybullet-drones とは実装内容が異なりますので、きちんと勉強したい方はそちらをご確認ください）。

./b02_pid_ctrl.py を実行すると、以下のようなPID制御のデモが実施できます。

./b03_downwash.py はダウンウォッシュというドローンの吹き下ろしによる負荷をシミュレートしたものです。下側のドローンが上側のドローンのせいでポニョポニョ動きます。

以上、最後は駆け足になってしまいましたが、とりあえず「ドローンを飛ばす」ことができたのでここでいったん終了とします。（また進展があれば追加するかも・・・）

Discussion