# cost_scaling_factorの計算とinflation_radius
## Inflation Costmap plugin Wiki
http://wiki.ros.org/costmap_2d/hydro/inflation


## cost_scaling_factorの計算

costmapのcostの計算式は以下の通り
$$J = e^{-\rm{cost\_scaling\_factor}*((\rm{distance\_from\_obstacle} - \rm{inscribed\_radius)})}* (\rm{costmap\_2d::INSCRIBED\_INFLATED\_OBSTACL}E - 1)$$

ただし
$\rm{distance\_from\_obstacle}$ : 障害物とロボット間の距離
$\rm{inscribed\_radius}$ : ロボットの内径
$\rm{costmap\_2d::INSCRIBED\_INFLATED\_OBSTACLE}$ : デフォルトでは254

下図を見ると, ~~ロボットの外接領域におけるコストが127もしくはそれより大きくなっていれば~~[^1], 内接領域はdefinitely in collision, 内接領域と外接領域の間はpossibly in collision, それより外の領域はdefinitely not in collisionとなり, ロボットの形状を考慮した経路計画が可能になると考えた.

(http://wiki.ros.org/costmap_2d より引用)

下図より, turtlebot3 burgerの内径は0.066 m, 外径は0.105 mである.
(実際はfootprintで指定した五角形から自動計算されると思われるので, それを考慮してロボットの内径は0.069より少し小さい値としている)[^2]


これより, cost_scaling_factorを $C$ とすると以下の不等式を解けばよい.

$$e^{-C\times(0.105-0.066)}\times(254-1)\geq127$$

これを解くと, 等号が成立するとき $C=17.6718...$ となるため, cost_scaling_factorを17.671とした.

この時のコスト関数を図示すると以下のようになる.


## inflation_radiusの決定
inflation_radiusはbuffer zoneの幅を規定するパラメータであり, inflation_radiusを超えるコストは0になると解釈した.例えば, inflation_radiusを0.25とすればコスト関数は以下のようになると考えた.


ここで, 元のコスト関数の概形を見ると, 0.50あたりでほとんど0になっているように見えるので, inflation_radiusを0.50とすることでコスト関数にできるだけ不連続点が生じないようにした.

以上により定めた
cost_scaling_factor=17.671
inflation_radius=0.50
は本選走行時のパラメータとして採用した.

[^1]:後程調べたところ、この図で言うcost_possibly_circumscribed=128という値はあくまで例に過ぎず、ここの値はcircumscribed_radiusの値とコスト関数によって変わる変数であるということがわかりました。よって、以降の127という値を用いた不等式評価には意味がありません。([こちら](http://wiki.ros.org/costmap_2d)のinflationの章参照。また、[こちら](https://docs.ros.org/en/electric/api/costmap_2d/html/costmap__2d_8cpp_source.html#l00067)の67行目にその記述があります(これは昔のversionで、melodicではcircumscribed_radiusの記述すらありませんが)。大会参加時はこのことに気づいておりませんでした)
代わりに筆者の現在(2021/03/09)の考えを書いておきます。cost_scaling_factorを定める際は、コスト関数はあくまで離散的な値をとるということに気を付けたいです。コスト関数が急すぎたり緩すぎたりするとそれぞれ値が急に落ちたり変化しなかったりしてしまいます。特に急すぎてcircumscribed_radius以前にコストが0になってしまうと障害物にぶつかってしまう可能性があります。
inflation_radiusについては、最低限circumscribed_radius以上の値は指定するようにしましょう。こちらの値が大きすぎることに対する弊害はあまりないと思います。(タイムを縮めるためにコーナーを攻めたい！という場合には調整に工夫がいります)

[^2]:後程調べたところ、inscribed_radius,
circumscribed_radiusはfootprintから自動で計算されるようです。中心と辺上の点間の距離で一番短いものを内径、一番長いものを外径とするようです。
http://docs.ros.org/en/melodic/api/costmap_2d/html/layered__costmap_8cpp_source.html#l00173