PyBulletでドローンを飛ばす その２
前回の PyBulletでドローンを飛ばす その１ ではURDFのファイルを読み込むところだけで終わってしまいましたので、今回はせめてドローンを飛ばすところまで話を進めたいとお思います。
ということでドローンの飛行原理について、実機を触ったこともないくせに説明してみます。
１．ドローンの飛行原理
１．１ ドローンの形態
一般的なドローンの形態としては、４つのロータがついたクアッド型と呼ばれるものを思い浮かべますが、クアッド型も以下に示すような２つのタイプに大別できるそうです。
図１はクアッドコプターを上方から見た際の概略図です。図１(a)はクアッド・エックス型と呼ばれるタイプで、４つのロータが進行方向に対してX型に配置されている形態となっています。進行方向にロータが配置されていないので、カメラなどを取り付ける場合にメリットがあるそうです。
図１(b)はクアッド・プラス型です、４つのロータが進行方向に対し十字型に配置されています。進行方向とロータの配置位置が一致しているため、ドローンの幾何的な力学を考える上で比較的楽に考察することができます。ついては、本記事ではクアッド・プラス型を前提に説明をしていきます。
１．２ 推力とトルク
図２はローターが回転したときに発生する推力（スラスト）と回転力（トルク）を示したものです。ブレードが回転するとこでローターは推力（スラスト）を得ますが、同時にブレードの回転と反対方向に回転しようとする反作用（トルク）が発生します。
メインのローターが１つしかない一般的なヘリコプターには、お尻のほうに小さな横向きのローターがついていますが、これはメインロータによって発生したトルクで本体が回転しないよう制御するためのものだそうです。
では、クアッドコプターの場合、どのようにトルクを制御するかというと、図１の黒い矢印で示すように２対のローターがお互いのトルクを相殺する方向に回転するよう設定されています。つまり、ドローンに用いるプロペラは、右回転で推力を得るタイプと、左回転で推力を得るタイプという２種類があり、適切に使い分けられているということですね。
ドローンの実機を扱う人にとっては当然の知識かもしれませんが、私は今回調査するまで知らなかったので地味に驚きました。
１．３ 推力によって生じる回転モーメント
上記のようになローターに直接生じるトルクに加え、ドローンの構造上、スラストによって生じる以下のような回転モーメントがあります。
図３はスラストによって生じる回転モーメントを
２．シミュレーションの実装
２．１ PyBulletによる実装
ドローン周りの主要な力学が出そろったところで、PyBulletを用いてドローの力学モデルを実装していきます。
推力・トルクはローターの回転数の2乗に比例するそうです。それをそのままプログラム上に定義します。比例定数は前回読み込んだURDFファイルから取り込んだデータを用います。
今回作成したプログラムは下記リポジトリに上げていますので全体を確認したい場合はご参照ください（尚、参考にしている bym-pybullet-drones とは実装内容が異なりますので、きちんと勉強したい方はそちらをご確認ください）。
# DroneBltEnvクラスの関数
def apply_rotor_physics(self, rpm: np.ndarray):
"""
4つのローターの動きによって発生する個々の推力とトルクを単純に適用。
Parameters
----------
rpm : A list with 4 elements. Specify the 'rpm' for each of the four rotors.
４つのロータの回転数を引数として与えて指定する
rpm = np.array([14000, 14000, 14000, 14000])
という感じ
"""
# forces(スラスト）、torques（トルク）は回転数の2乗に比例します
#「self._dp.kf」、「self._dp.km」はurdfファイルから読み込んだ比例定数です
forces = (np.array(rpm) ** 2) * self._dp.kf
torques = (np.array(rpm) ** 2) * self._dp.km
# z軸回りのトルクは、各ローターのトルクを総和になります
# それぞれ相殺する向きに回転する前提なので±がついてます
z_torque = (-torques[0] + torques[1] - torques[2] + torques[3])
# urdfには4つのローターがLinkとして定義されており、id番号[0to3]で指定できます
# 4つのローターにそれぞれ計算したスラストを p.applyExternalForce()で適用します
# （各ロータの位置に指定した力を発生させるようなイメージです）
for i in range(4):
p.applyExternalForce(
objectUniqueId=self._drone_id,
linkIndex=i, # link id of the rotors.
forceObj=[0, 0, forces[i]],
posObj=[0, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self._client,
)
# 4つのローターのトルクによって生じるz軸回りのトルクは
# ドローの重心1か所にまとめてp.applyExternalTorque()で適用します
p.applyExternalTorque(
objectUniqueId=self._drone_id,
linkIndex=4, # link id of the center of mass.
torqueObj=[0, 0, z_torque],
flags=p.LINK_FRAME,
physicsClientId=self._client,
)
はい、ドローンの主要な力学モデルはたったこれだけです。
先に説明した「１．３ 推力によって生じる回転モーメント」が適用されていないように思えますが、このトルクに関しては、URDFファイルに幾何的なモデルが定義されている（この定義内容が適切かどうかという議論の余地はありますが...）ので、ローターに推力を定義した時点で自ずと計算されます。
あとは、以下のような感じでp.stepSimulation()を1回呼び出す度に、1ステップ分のシミュレーションが実行されます。
ステップ幅はp.setTimeStep()で事前に設定できます。
def step(self, rpm: np.ndarray):
self.apply_rotor_physics(rpm)
p.stepSimulation()
例示した関数名のstep()という名前は特に重要ではなく何でも構いません。p.stepSimulation()を呼び出していることがポイントです。
（将来的にOpenAIGymに繋げていくことを意識してstep()という関数名にしています）
今回は/blt_env/drone.py にDroneBltEnvというクラスを定義していますが、この場合p.setTimeStep()で1ステップの時間を1/240に指定していますので、以下のように240回step()を進めることで1秒間分のシミュレーションが実施されることになります。
...
env = DroneBltEnv(...)
...
step_num = 240
for i in range(step_num):
_ = env.step(rpm)
...
./b01_bullet_vs_dyn.pyを実行すると以下のような画面が表示されます。
各ローターの回転数をGUIから変更できるようになっているので、動かしてみてください。瞬時にドローンがバランスを崩して墜落すると思います（笑）。
２．２ 運動方程式から実装
ではPyBulletのp.setTimeStep()関数を利用せずに、運動方程式から地道にシミュレーションする場合はどうなるでしょうか。
# DroneBltEnvクラスの関数
def apply_dynamics(self, rpm: np.ndarray):
"""
慣性モーメントなどを考慮した力学を陽解法を用いて適用
Parameters
----------
rpm : A list with 4 elements. Specify the 'rpm' for each of the four rotors.
４つのロータの回転数を引数として与えて指定する
"""
# current state
# ドローンの現在位置、姿勢、速度などを取得
pos = self._ki.pos
quat = self._ki.quat
rpy = self._ki.rpy
vel = self._ki.vel
rpy_rates = self._ki.rpy_rates
rotation = np.array(p.getMatrixFromQuaternion(quat)).reshape(3, 3)
# compute thrust and torques
# 指定したローター回転数からスラストやトルクを計算
thrust, x_torque, y_torque, z_torque = self.rpm2forces(rpm)
thrust = np.array([0, 0, thrust])
thrust_world_frame = np.dot(rotation, thrust)
forces_world_frame = thrust_world_frame - np.array([0, 0, self._dp.gf])
torques = np.array([x_torque, y_torque, z_torque])
torques = torques - np.cross(rpy_rates, np.dot(self._dp.J, rpy_rates))
rpy_rates_deriv = np.dot(self._dp.J_inv, torques)
no_pybullet_dyn_accs = forces_world_frame / self._dp.m
# update state
# 指定したステップ時間後の速度や姿勢を計算
vel = vel + self._sim_time_step * no_pybullet_dyn_accs
rpy_rates = rpy_rates + self._sim_time_step * rpy_rates_deriv
pos = pos + self._sim_time_step * vel
rpy = rpy + self._sim_time_step * rpy_rates
# Set PyBullet state
# アニメーションとして表示させるために、計算結果をPyBulletに渡す
p.resetBasePositionAndOrientation(
bodyUniqueId=self._drone_id,
posObj=pos,
ornObj=p.getQuaternionFromEuler(rpy),
physicsClientId=self._client,
)
# Note: the base's velocity only stored and not used
p.resetBaseVelocity(
objectUniqueId=self._drone_id,
linearVelocity=vel,
angularVelocity=[-1, -1, -1], # ang_vel not computed by DYN
physicsClientId=self._client,
)
def rpm2forces(self, rpm: np.ndarray) -> Tuple:
"""
Compute thrust and x, y, z axis torque at specified rotor speed.
スラストやトルクを指定したローターの回転数から算出する
"""
forces = np.array(rpm) ** 2 * self._dp.kf
thrust = np.sum(forces)
z_torques = np.array(rpm) ** 2 * self._dp.km
z_torque = (-z_torques[0] + z_torques[1] - z_torques[2] + z_torques[3])
x_torque = (forces[1] - forces[3]) * self._dp.l
y_torque = (-forces[0] + forces[2]) * self._dp.l
return thrust, x_torque, y_torque, z_torque
はい、途端に大変になりました。真面目に数値計算しようとすると回転モーメントやら座標変換やらの計算が必要で大変そうです。
何より大変なのはイナーシャとかの物理定数をどうやって求めるかということですよね。
System Identification of the Crazyflie 2.0 Nano Quadrocopter
上記参考文献をみるとその苦労が伺えます（内容はよく分からないので画像やグラフから判断...）。データ公開してくださって感謝しかありありません。
PyBulletでシミュレートした場合と、運動方程式を真面目に適用した場合の違いについては、./sample_00_bullet_vs_dyn.pyの下記部分をコメントアウトして書き換えることで、切り替えることができるようになっていますので挙動を確認してみてください。だいたい同じような動きになると思います。
...
## Select a pysical model.
# phy_mode = PhysicsType.PYB # Pybulletを用いる場合
phy_mode = PhysicsType.DYN # 運動方程式でまじめに計算
...
３．今回のまとめ
PyBulletでドローンを飛ばすだけでも、いくつかの方法を選択できることが分かりました。p.stepSimulation()で対応できる分にはPyBulletで実施したほうが遥かに楽そうですね。もし対応できないような挙動を反映したい場合も、真面目に物理モデルを適用すれば大抵のことはどうにかなりそうです。
PyBulletでは、このような様々な挙動の計算結果も容易に融合できるようになっているので、いろいろと応用が利くのかもしれません（他のシミュレーションをやったことが無いので適当に言ってます）。
本記事の参考にしている下記オリジナルのリポジトリでは、さらにダウンウオッシュ（吹き下ろし）や空気抵抗係数などが適用された実装がなされています。
次回は、この辺の挙動モデルの追加や複数のドローンの読み込みに対応した実装の部分を書こうかなと考えています。
次の記事
Discussion