お疲れ様です。秋並です。
今回は、シミュレーション用に自作の移動ロボットを作成する際に注意すべき7つのポイントを紹介します。
CADでモデリングする際
1. 移動ロボットの方向
移動ロボットをcadで設計する際は、x方向が進行方向になるように設定しましょう。
実体験:
一度、y軸方向を正方向として移動ロボットを作成し、シミュレーション+navigation2で動作させようとしたときに、正しく動作しないことがありました。
(おそらく、navigation2などのアプリケーションが、移動ロボットの進行方向がx軸の正方向という前提で設計されているためだと思われます)
urdfファイルの設定(衝突領域、物理パラメータの設定)
ここからは、urdfファイルでパラメータを設定する際に気を付けるべき点を述べます。
今回は、例としてurdfファイルを出していますが、一般的なシミュレーション用ロボットのパラメータ設定にも共通する内容になっています。
2. キャスターのcollision設定
キャスターのcollision領域は、形状はbox、jointはfixedにした方が、それっぽい動きになることが多いです。
urdf例(一部抜粋):
<link name="caster">
<inertial>
<origin rpy="0 0 0" xyz="2.7478019859472624e-15 -0.003000000000000034 -3.469446951953614e-18"/>
<mass value="0.0008831820150689292"/>
<inertia ixx="0.000177" ixy="0.0" ixz="0.0" iyy="9.7e-05" iyz="0.0" izz="9.6e-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.2 -0.018 0.0095"/>
<geometry>
<mesh filename="$(find mobile_robot_description)/mesh/caster.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<!--collisionはboxにする。-->
<box size="0.006 0.006 0.006"/>
</geometry>
</collision>
</link>
<joint name="caster_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.2 0.148 -0.0095"/>
<parent link="base_link"/>
<child link="caster"/>
</joint>
特に、下画像のような球状のキャスターはstlやdaeなどのメッシュファイルでcollisionを指定してしまうと、シミュレーション上で動作させる際に滑らかに動作しない原因になることもあるので注意してください。
画像引用:https://emanual.robotis.com/docs/en/platform/turtlebot3/features/
3. ホイールのcollisionの設定
ホイールのcollisionは、形状はcylinder、jointはcontinuousにすることが一般的です。
urdf例(一部抜粋):
<link name="wheel_left">
<inertial>
<origin rpy="0 0 0" xyz="8.049116928532385e-16 0.012250000000000094 -2.7755575615628914e-17"/>
<mass value="0.6427240838548368"/>
<inertia ixx="0.000206" ixy="0.0" ixz="-0.0" iyy="0.000349" iyz="0.0" izz="0.000206"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.021 -0.161 -0.016"/>
<geometry>
<mesh filename="S(find mobile_robot_description)/meshes/wheel_left.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.0245" radius="0.03375"/>
</geometry>
</collision>
</link>
<joint name="wheel_left_joint" type="continuous">
<origin rpy="0 0 -1.57" xyz="0.021 0.161 0.016"/>
<parent link="base_link"/>
<child link="wheel_left"/>
<axis xyz="0.0 0.0 1.0"/>
</joint>
4. その他リンクのcollisionの設定
上記で説明した以外のリンクについても、
- visualのみにメッシュファイルを指定
- collisionについては基本的に、boxなどのプリミティブな形状を指定
した方がよいです。
collisionのメッシュが複雑だと、シミュレーションの処理が重くなったり、ロボットの動作の不具合につながることもあるので、衝突領域の厳密性を求めないのであれば、visualのみにメッシュファイルを適用したほうが動作が安定することが多いです。
(厳密性が必要な例:ロボットアームの手先などは厳密に衝突判定ができた方がよいと思うので、メッシュファイルの方が適切など)
urdf例(一部抜粋):
<visual>
<origin rpy="0 0 0" xyz="-0.1655 -0.075 -0.0585"/>
<geometry>
<mesh filename="$(find mobile_robot_description)/mesh/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<!--stlやdaeを適用するのは、visualのみにし、collisionはプリミティブな形状を適用-->
<box size="0.04 0.04 0.04"/>
</geometry>
</collision>
5. 車輪半径、車輪間の幅の設定
二輪駆動の移動ロボットのコントローラを使用する際に、車輪半径、車輪間の幅を適切に設定しないと動作に影響が出る可能性があります。
urdf例(一部抜粋、diff drive controllerを使用する場合の例)
<gazebo>
<plugin filename="libignition-gazebo-diff-drive-system.so" name="ignition::gazebo::systems::DiffDrive">
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<topic>/cmd_vel</topic>
<odom_topic>/odom</odom_topic>
<wheel_separation>0.1865</wheel_separation> <!--車輪の半径-->
<wheel_radius>0.03375</wheel_radius> <!--左右の車輪間の距離-->
<wheelTorque>10</wheelTorque>
</plugin>
</gazebo>
6. 最小速度、最大速度の設定
最小速度、最大速度(並進、回転)は、小さすぎても大きすぎても移動ロボットが正しく動作しない原因になるので、適切に設定する必要があります(加速度に関しても同様)。
urdf例(ファイル一部)
<gazebo>
<plugin filename="libignition-gazebo-diff-drive-system.so" name="ignition::gazebo::systems::DiffDrive">
<left_joint>Revolute 6</left_joint>
<right_joint>Revolute 7</right_joint>
<topic>/cmd_vel</topic>
<odom_topic>/odom</odom_topic>
<wheel_separation>0.1865</wheel_separation>
<wheel_radius>0.03375</wheel_radius>
<wheelTorque>10</wheelTorque>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity> <!--最大並進速度の設定-->
<min_linear_velocity>-0.5</min_linear_velocity> <!--最小並進速度の設定-->
<max_angular_velocity>1</max_angular_velocity> <!--最大回転速度の設定-->
<min_angular_velocity>-1</min_angular_velocity> <!--最小回転速度の設定-->
</plugin>
</gazebo>
7. ホイール、キャスターの摩擦係数の設定
ホイールやキャスターの摩擦係数は、低すぎると滑り、高すぎると摩擦が大きすぎて動かない等の動作の不具合につながります。
どの程度の値に設定すればわかない場合は、とりあえず0.1程度に設定してみてシミュレーションを動かしながら調整してみるのもよいと思います。
(もちろん、ホイールの詳細な材質などが分かる場合は、材質に合わせて設定するのもよいです)
urdf例(ファイル一部)
<gazebo reference="caster">
<!--摩擦係数の設定、基本的にはmu1, mu2には同じ値を入れればOK-->
<mu1>0.1</mu1>
<mu2>0.1</mu2>
</gazebo>
おわりに
今回は、シミュレーション用の自作移動ロボット作成の際の注意ポイントをまとめました。
今回は、私が実際につまずいた点をまとめたので、間違っている点や他のつまずきポイントがあれば教えていただけると幸いです。
参考サイト