パラメータを調整する - TukamotoRyuzo/rostest GitHub Wiki

とにかくNavigation Stackを試すでNavigationが動くところまでは行きました。その後何を調整すればいいかが難しいと思います。

パラメータのチューニングについては以下の3つのURLを参照にするべきです。

下2つのURLはmove_baseとamclのパラメータについて日本語で解説している貴重なサイトですので参考にしてください。

本格的に調整するなら上記3つは必須なのですが，最初は難しいと思いますので，私なりの言葉で補足しながら説明します。

パラメータは大きく分けて4つあります。

ロボットの物理パラメータ（my_robo_description/my_robo.urdf）
コントローラパラメータ(my_robo_control/config/controller.yaml)
move_baseパラメータ(my_robo_2dnav/***.yaml)
amclパラメータ(my_robo_2dnav/launch/move_base.launch)

実機で動かす場合には，さらに

YP-Spurパラメータ(robot_launcher/ypspur_param/ yp-h29ki-3600-1.param)

も加わります。

このうち，Navigation Stackに関係するパラメータは

move_baseパラメータ
amclパラメータ

です。しかし，実行時に発生する問題の大部分は

ロボットの物理パラメータ
コントローラパラメータ

にあります。つまり問題はNavigation Stackとは関係ない部分にあるということです。最初に遭遇する問題は，オドメトリのずれでしょう。まずはここから調整します。（調整の順番を知っておくだけでもだいぶ効率が違うと思います）また，ファイルパスはrostestでのパスを示しますので，チュートリアルとは違う場所にあることをご了承ください。（大きくは変わりません）

1. ロボットの物理パラメータが正しいことを確認する

物理パラメータが間違っていると，正しくシミュレートされません。 my_robo_descripsion/my_robo.urdfがロボット形状を定義しているファイルです。チュートリアルそのままでやる場合は正しく設定されていますので変更する必要がありませんが，チュートリアルからロボット形状を変更することがほとんどでしょう。接客ロボットのurdfは図6-3のような見た目にしました。

robot
図6-3 my_robo.urdf

まず，inertiaの正しい計算式を知っておいてください。ここでのinertiaは慣性モーメントを表しており、物体の質量の偏りを表す物理量です（そう理解しているのですが違ったら教えてください）。ロボットの形状が変更されるたびにinertiaの計算が必要になります。inertiaは3×3回転慣性行列ですが，回転慣性行列は対称なので，属性ixx、ixy、ixz、iyy、iyz、izzを使用して、この行列の6つの対角要素のみを指定します。このうちixy, ixz, iyzは，直方体，円柱，球の場合は０でいいです。

（例）奥行きd,幅w,高さh,重さmの直方体のinertiaは

ixx = 1.0/12.0*m*(w*w+h*h)
iyy = 1.0/12.0*m*(h*h+d*d)
izz = 1.0/12.0*m*(w*w+d*d)

半径r, 長さl,重さmの円柱の場合

ixx = 1.0/12.0*m*(w*w+h*h)
iyy = 1.0/12.0*m*(h*h+d*d)
izz = 1.0/12.0*m*(w*w+d*d)

半径r,重さmの球体の場合

ixx = iyy = izz = 1.0/5.0*m*r*r

のように計算します。

以下の例のように，linkごとに奥行きや幅を変数宣言しておき，inertiaはマクロで計算できるようにしておくと楽です。

例：base_linkのマクロ計算例(my_robo_description/my_robo.urdf)

<!-- base -->
<property name="bd"  value="0.330"/>
<property name="bw"  value="0.330"/>
<property name="bh"  value="0.900"/>
<property name="base_mass" value="30.0"/>
<property name="base_ixx" value="${1.0/12.0*base_mass*(bw*bw+bh*bh)}"/>
<property name="base_iyy" value="${1.0/12.0*base_mass*(bh*bh+bd*bd)}"/>
<property name="base_izz" value="${1.0/12.0*base_mass*(bw*bw+bd*bd)}"/>

…(省略)
<!-- base_link -->
<link name="base_link">
  <visual>
    <geometry>
      <box size="${bd} ${bw} ${bh}"/>
    </geometry>
  </visual>
 
  <collision>
    <geometry>
      <box size="${bd} ${bw} ${bh}"/>
    </geometry>
  </collision>
 
  <inertial>
    <origin xyz="0 0 0"/>
    <mass value="${base_mass}"/>
<inertia ixx="${base_ixx}" ixy="0" ixz="0" iyy="${base_iyy}" iyz="0" izz="${base_izz}"/>
  </inertial>
</link>

私が結局解決できなかったのは，base_footprintの位置です。base_footprintは回転座標系を示す座標なので回転軸の場所に持ってくるべきなのかと思ったのですが，そうしなくても動くようです。私はbase_linkの真下，ロボット最下部にある部品の高さに設定しました。この座標を中心として「ロボットを上から見た形状」を定義していくことになります。それは2.4で行います。

2. オドメトリを調整する

my_robo_control/config/controller.yamlを調整します。これが非常に大事で，チュートリアルそのままだと回転時に自己位置がずれるのはこのパラメータが間違っているからです。オドメトリには回転成分と直進成分があり，これらが正しく計算されているかをテストします。

回転成分のテスト

ロボットをセットアップします。
rvizを開き，フレームをodomに設定し，ロボットが提供するレーザースキャンを表示します。
トピックの減衰時間を長めに設定（20秒くらい）します。
その場で回転します。

理想的には，回転の2周目でレーザの線は1周目とほぼ一致しているはずです。しかし，回転のずれがある場合は2周目のレーザは1周目のレーザより大きくずれることになります。ずれている場合は，wheel_separationパラメータを調整してください。

※ wheel_separationはタイヤ中心間の距離（左タイヤの中心と右タイヤ中心の距離）で，my_robo.urdfの記述上は0.26で正しいのですが，gazeboで動かす場合はタイヤの外側間の距離でないといけないようです。(なぜそういった仕様なのかは不明)よってチュートリアルで作ったロボットでの正しいwheel_separationは

wheel_separation : 0.31

です。しかし，実機の場合は「タイヤの中心間の距離」が正しいので注意です。

直進成分のテスト

ロボットを壁から数メートル離れたところでセットアップします。
rvizを開き，回転成分と同じように設定します。
ロボットを壁に向かってまっすぐ移動させます。

理想的には壁にそって表示されているレーザは1回のスキャンのように見えるはずです。厚く表示される場合はwheel_radiusパラメータを調整してください。

以上のテストでオドメトリの調整が完了します。ここまでの設定は，Navigation Stackとは関係ない部分です。

3. my_robo_2dnav/params/base_local_planner.yaml

ここからはNavigation Stackに関係します。まずはmy_robo_2dnav/params/base_local_planner.yamlを調整します。これはアクセルとブレーキの力をmove_baseが正しく見積もるために必要な設定です。ここが間違っていると，ロボットが止まりきれずに壁にぶつかったりします。

これを正しく設定するためには，my_robo_control/config/controller.yamlの値を確認する必要があります。

以下の記述には接客ロボットに用いた値が入っています。ハードウェア性能に応じて書き換えてください。

my_robo_control/config/controller.yaml

  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 0.5  # m/s
      min_velocity           : -0.5 # m/s
      has_acceleration_limits: true
      max_acceleration       : 0.5  # m/s^2
      min_acceleration       : -0.5 # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 0.78 # rad/s
      min_velocity           : -0.78
      has_acceleration_limits: true
      max_acceleration       : 0.78  # rad/s^2
      min_acceleration       : -0.78

my_robo_2dnav/params/base_local_planner.yaml

TrajectoryPlannerROS:
  max_vel_x: 0.5
  min_vel_x: 0.1
  max_vel_theta: 0.78
  min_vel_theta: -0.78
  min_in_place_vel_theta: 0.4
  acc_lim_theta: 0.78
  acc_lim_x: 0.5
  acc_lim_y: 0.5
  holonomic_robot: false
  yaw_goal_tolerance: 0.25
  xy_goal_tolerance: 0.25
  p_dist_scale: 1.0
  g_dist_scale: 0.5
  occdist_scale : 0.1
# heading_scoring: true
# dwa: true
  meter_scoring: true

ここで設定されているmax_vel_***, acc_lim_***は，controller.yamlで設定した値を超えないようにする必要があります。超えた値を設定しても，その力を出すことはできません。つまり，

controller.yaml：限界性能
base_local_planner.yaml：案内時に出したい性能という感じです。

base_local_planner.yamlはcontroller.yamlと同一に設定するのがわかりやすいです。

他にも良く使うパラメータを解説しておきます。

yaw_goal_tolerance: 目的地にたどり着いたと判定する許容角度誤差
xy_goal_tolerance: 許容座標誤差

この2つのパラメータに設定した値だけゴール座標からずれている場合でも「ゴールにたどり着いた」と判定されます。甘めに設定しておかないと，なかなかゴールにたどり着いたと判定されません。

4. my_robo_2dnav/params/costmap_common_params.yaml

ここではコストマップに関するパラメータを設定します。障害物に対してどういう挙動をするかを決めるパラメータなので，ここを間違えると障害物にぶつかります。

obstacle_range: 3.5
raytrace_range: 4.0
footprint: [[-0.25, -0.35], [-0.25, 0.35], [0.55, 0.35], [0.55, -0.35]]
inflation_radius: 0.55
observation_sources: laser_scan_sensor

laser_scan_sensor: 
  sensor_frame: hokuyo_link
  # sensor_frame: urg_laser_link
  data_type: LaserScan
  topic: /scan
  observation_persistence: 0.2
  expected_update_rate: 0.2
  marking: true
  clearing: true
  inf_is_valid: true

obctacle_range: ロボットとの距離がobstacle_range以下のオブジェクトは障害物としてみなし，コストマップに反映します。
raytrace_range : ロボットとの距離がraytrace_range以下のオブジェクトが検出された場合，そのオブジェクトの内側のコストマップの障害物がクリアされます。
footprint : ロボットの形状を多角形で指示することができます。footprintの形状に応じてコストマップのdefinitery in collisionとpossibly in collisionの径が決定されます。
inflation_radius：障害物までの距離がinflation_radius以下の場所をロボットの中心が絶対に通らないようにします。

接客ロボットではロボットを上から見た形(=footprint)を四角形にしましたが，別に何角形でも大丈夫なので，もっと細かく設定するべきでした。皆さんはそうしてください。

5 amclパラメータを調整する

amclパラメータは自己位置推定の精度を上げるために必要です。 amclは次の図のような仕事をします。（パワポから抜粋）要はオドメトリだけではなく，LRFの情報も使って自己位置推定をするよ，というパッケージです。

図6-4 オドメトリのみの自己位置推定　図6－5 amclを使った自己位置推定

チュートリアルではlaunchファイルにパラメータが書かれているのですが，私はmove_baseパラメータと同じようにparamsディレクトリにyamlファイルとしてまとめました。どちらにするかは好みにお任せします。なお，内容は次のようにしています。

my_robo_2dnav/params/amcl_params.yaml
# http://wiki.ros.org/amcl#Parameters
# overall filter parameters
min_particles: 500  
max_particles: 5000
kld_err: 0.05
kld_z: 0.99
update_min_d: 0.2
update_min_a: 0.5
resample_interval: 1
transform_tolerance: 0.2
recovery_alpha_slow: 0.001
recovery_alpha_fast: 0.1
gui_publish_rate: 10.0
use_map_topic: true 
first_map_only : false

# laser model parameters
laser_min_range: -1.0
laser_max_range: -1.0
laser_max_beams: 30
laser_z_hit: 0.95 
laser_z_short: 0.05 
laser_z_max: 0.05
laser_z_rand: 0.05
laser_sigma_hit: 0.2
laser_lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_model_type: likelihood_field

# odometry model parameters
odom_model_type: diff
odom_alpha1: 0.2
odom_alpha2: 0.2
odom_alpha3: 0.2 
odom_alpha4: 0.2 
odom_alpha5: 0.2
odom_frame_id: odom 
base_frame_id: base_footprint
global_frame_id: map

重要なのはodom_alpha1～4です。これらはそれぞれ

オドメトリの直進成分がオドメトリの直進成分に与える誤差
オドメトリの直進成分がオドメトリの回転成分に与える誤差
オドメトリの回転成分がオドメトリの直進成分に与える誤差
オドメトリの回転成分がオドメトリの回転成分に与える誤差

を表しています。これらをすべて0にした場合，amclは仕事をしないので，オドメトリが絶対的に正しいものだとみなして動作します。逆にすべて1にした場合，補正がかかりすぎて，ロボットが実際に進んでいるにもかかわらずrviz上では進まないように見えてしまいます。実験した結果では0.2くらいがちょうど良かったのでこのように設定しました。

その他のamclパラメータの詳細は http://wiki.ros.org/amcl#Parameters を参考にしてください。

以上でパラメータ調整は終わりです。これより細かいチューニングは実際の動作と，本文中に示したURLを参考に行ってください。