ArduPilotのシミュレーション（SITL + Gazebo）でも6発機（ヘキサコプター）に対応してみる

普通の状態でインストールして実行

ArduPilotのシミュレーション（SITL + Gazebo）インストールは以下の通りです。VM上でも良いので、Ubuntuを強く推奨。
Ubuntuならば、はまるところはありません。

一応、Mac でもGazeboさえ入れれば動作可能ですが、Gazeboがとても重く、実用に耐えませんでした。
（ちゃんと調べていので、原因が分かる方は教えて頂きたいです）
以下の方法でMacにもGazeboは入れられます。

あとは、 https://qiita.com/ikeyasu/items/9d244bcd0dd9b776ff84 とほぼ同じ手順です。

QGroundControllerを入れて、以下のコマンドを実行します。

Gazeboを起動し

cd ardupilot_gazebo
gazebo --verbose worlds/iris_arducopter_runway.world

別のターミナルで ardupilotを実行します。

cd ardupilot/ArduCopter
../Tools/autotest/sim_vehicle.py -f gazebo-iris --console --map

そして、QGroundController でMission Startすれば飛びます。

Hexaへの対応

さて、FlightGearのときと同じように Hexaへ対応してみます。

今回はソースコードの変更はほとんど必要無く、モデルとパラメータの変更だけですみます。
以下変更したコミットです。

https://github.com/ikeyasu/ardupilot_gazebo/commit/38376ec5ade3ce0698f2060bfb0f76617b01056e
https://github.com/ikeyasu/ardupilot/commit/5183ebf8bc0c7ffb6adf26218be0901bdb5ced0f

修正後の動画は以下のような感じです。

ardupilot_gazeboの修正

https://github.com/ikeyasu/ardupilot_gazebo/commit/38376ec5ade3ce0698f2060bfb0f76617b01056e
の説明をします

models/iris_with_ardupilot/model.sdf

models/iris_with_ardupilot/model.sdf に以下のような記述があります。これは、それぞれプロペラ（ローター）の効力・揚力の設定です。ブレードが2つ組み合わさって、1つのプロペラになっています。

    <plugin name="rotor_4_blade_1" filename="libLiftDragPlugin.so">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.00</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris::rotor_4</link_name>
    </plugin>
    <plugin name="rotor_4_blade_2" filename="libLiftDragPlugin.so">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.00</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris::rotor_4</link_name>
    </plugin>

models/iris_with_ardupilot/model.sdf にローターを2つ追加しました。
また、ローターの回転方向が、CWかCCWかで<forward>のパラメーターが違います。CWの1番ローター（models/iris_with_ardupilot/model.sdf の記述はrotor_0）では、rotor_0_blade_1の<forward>は0 -1 0、rotor_0_blade_2の<forward>は0 1 0です。CCWの2番ローター（models/iris_with_ardupilot/model.sdf の記述はrotor_1）では、rotor_1_blade_1の<forward>は0 1 0、rotor_1_blade_2の<forward>は0 -1 0です。
https://ardupilot.org/copter/docs/connect-escs-and-motors.html にしたがって、CW・CCWの設定をしました。今回は、Hexacopterの”HEXA X”で試しています。

また、libLiftDragPlugin.soの各パラメーターについては、こちらに説明があります。
http://gazebosim.org/tutorials?tut=aerodynamics&cat=physics

あと、models/iris_with_ardupilot/model.sdf ではもう一箇所、<control>を2つ足しています。これはパラメータは特に気にせず、channel=“3”のをコピーしました。

      <control channel="4">
        <type>VELOCITY</type>
        <offset>0</offset>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <jointName>iris::rotor_4_joint</jointName>
        <multiplier>838</multiplier>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>

models/iris_with_standoffs/model.sdf

次に、models/iris_with_standoffs/model.sdf では、物理モデルと見た目の設定をします。

<link name='rotor_0'>のような要素は、各プロペラの設定になります。ここの<pose>で、プロペラの位置をHexaの位置にします。座標は適当な3Dソフトて当たりをつけてください。

    <link name='rotor_0'>
      <pose>0 -0.288 0.023 0 -0 0</pose>

以下の、<mesh>は、プロペラの3Dモデルです。先ほどの、CW・CCWの設定に合うように変更してください。

      <visual name='rotor_2_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>

さらに、models/iris_with_standoffs/model.sdf には、2つ、プロペラ、rotor_4とrotor_5を追加しています。

ardupilotの修正

Tools/autotest/default_params/gazebo-iris.parm を変えているだけです。

Tools/autotest/default_params/gazebo-iris.parm

ここでは、FRAME_CLASSを2にしています。

Ardupilotには、FRAME_CLASSとFRAME_TYPEがあり以下に解説があります。

今回、FRAME_TYPEはX、FRAME_CLASSはHEXAです。
それぞれの値は以下で分かります。

https://ardupilot.org/copter/docs/parameters-Copter-latest-V4.0.1-dev.html#frame-class
https://ardupilot.org/copter/docs/parameters-Copter-latest-V4.0.1-dev.html#frame-type

ドローの形の変更

このままだと、ドローンの見た目ががirisのままです。
これは、models/iris_with_standoffs/meshes/iris.dae を修正すれば変わります。
ただし、Gazeboは、レポジトリ内のファイルを読み込まず、以下のファイルを読み込みます。
Iris.daeを変更後、以下にroot権限でコピーすれば、見た目が変わります。

/usr/share/gazebo-9/models/iris_with_standoffs/meshes/iris.dae

hexaで飛ばしてみる

動画はこちら
https://youtu.be/s_lIOHutUFw

https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html
のインストールが済んでいると、ardupilotと、ardupilot_gazebo のレポジトリの2つがある状態かと思います。
以下の通りRepositoryを足して、それぞれ、hexa_gazebo_qiitaのブランチをCheckoutすると動きます。

おまけ：風をおこしてみる

ardupilot_gazebo の方の、worlds/iris_arducopter_runway.world に風のプラグインを追加します。

そして、gazebo上で、風を設定すると、ドローンの挙動が変わるのが分かります。
以下の動画では、iris_demo の enable_windの設定を確認していますが、上記のコミットでtrueにしているので、通常は変更不要です。