ロボットの運動学計算

UNI-WR2は2輪差動駆動ロボットであるため、2輪差動モデルを構築して計算できます。

ロボットは2つの駆動輪の回転速度と回転方向を変えることで、特定の角速度と並進速度を実現できます。

2輪差動モデルに基づき、順運動学と逆運動学を用いて、目標の角速度・並進速度と2つの車輪の速度とを相互に変換するにはどうすればよいでしょうか。

順運動学：2つの車輪の速度が既知のとき、ロボット（車体など）の角速度と並進速度を求めます。

逆運動学：ロボットの目標角速度と並進速度が与えられたとき、2つの車輪の回転速度を求めます。

順運動学の導出

まず座標系を定義します。

ロボット本体座標系：ロボットの前進方向をx軸、左方向をy軸、ロボットの平面に垂直な方向をz軸とします。

グローバル座標系：x軸の正方向を右、y軸を前方、z軸を地面に垂直な方向とします。

座標系	X	Y	Z
ロボット本体座標系	前方	左方	ロボットの平面に垂直
グローバル座標系	右方	前方	地面に垂直

前提条件：

ロボットのx軸方向の速度：v

z軸周りの角速度：ω

左車輪の速度：vl

右車輪の速度：vr

2輪間の距離（トレッド）：l

問題設定：

短い時間t の間、ロボットの左右の車輪の並進速度vl、vrが一定かつ既知であると仮定し、2輪の取り付け間隔lを用いて、ロボットの並進速度vと角速度ωを求めます。

運動モデルは次の図のように単純化できます。

r：旋回半径

d：左右の車輪の相対移動距離

図から導出します。ロボットの並進速度の方向は常に車輪の回転方向と一致するため、ロボットの並進速度は左右の車輪の並進速度の平均となります。すなわち、

v = (vl + vr) / 2

また、次の関係が成り立ちます。

v = ω · r

上図より、

l = rr − rl = vr / ωr − vl / ωl

同一のロボットでは左右の角速度は等しいため、

ωl = ωr = ω

以上から、

ω = (vr − vl) / l

逆運動学の導出

順運動学の結果をそのまま逆解に利用できます。

v = (vl + vr) / 2、ω = (vr − vl) / l

したがって、

vl = v − ωl / 2、vr = v + ωl / 2

ROSのロボット座標系における車体の位置と向き（方位角）を計算するには、次の手順で行います。

車輪の円周を計算する：車輪の直径は0.048 mであるため、車輪の円周は π × 0.048 m となります。

左右の車輪の回転距離を計算する：左右の車輪の回転距離は、エンコーダの値から計算できます。左車輪のエンコーダ値をenc_left、右車輪のエンコーダ値をenc_rightとすると、左右の車輪の回転距離はそれぞれ次のとおりです。

車体の移動距離を計算する：車体の移動距離は左右の車輪の回転距離から計算できます。車体の中心点が2輪の中点にあると仮定すると、車体の移動距離は次のとおりです。

車体の回転角を計算する：車体の回転角は左右の車輪の回転距離から計算できます。車体の旋回半径をRとすると、車体の回転角は次のとおりです。

ROSのロボット座標系における車体の位置を計算する：車体の初期位置を(x0, y0)、初期方位角をθ0とすると、移動距離と回転角を積算することで、ROSのロボット座標系における車体の位置を更新できます。

車体の回転角を計算する：車体の回転角は左右の車輪の回転距離から計算できます。車体の旋回半径をRとすると、車体の回転角は次のとおりです。

以上の手順により、ROSのロボット座標系における車体の位置（x方向およびy方向の移動距離）とリアルタイムの方位角を計算できます。リアルタイムのエンコーダデータが得られる場合は、上記の計算を継続的に更新することで、車体の位置と向きを追跡できます。