wheel_odometry

役割

wheel_odometry は Rogidrive encoder から omni wheel odometry を計算します。全 wheel の encoder を受け取った後、差分を body 座標の \((\Delta x,\Delta y,\Delta\theta)\) に変換して積分します。

Interface

種別	名前	型 / 内容
subscribe	/rogidrive_status	Rogidrive status
publish	/odom	nav_msgs/msg/Odometry
tf	odom -> base_link	publish_tf=true の場合

Encoder 差分

motor position \(q_i\) は、rogidrive_position_is_revolutions=true なら revolution から radian へ変換します。

\[\phi_i=2\pi q_i\]

false の場合は \(\phi_i=q_i\) です。前回積分値との差分は、

\[\Delta\phi_i=\phi_i-\phi_{i,\mathrm{prev}}\]

wrap_angle_delta=true なら、

\[\Delta\phi_i\leftarrow\operatorname{atan2}(\sin\Delta\phi_i,\cos\Delta\phi_i)\]

です。omni wheel 側の角度差分は、

\[\Delta\omega_i=\frac{s_i\Delta\phi_i}{G}\]

ここで \(s_i\) は encoder_signs[i]、\(G\) は motor_to_omni_reduction_ratio です。

Wheel constraint

wheel 位置角を \(\alpha_i\)、駆動方向角を \(\beta_i\)、中心から wheel までの距離を \(R\) とします。

\[\begin{split}r_i= \begin{bmatrix} R\cos\alpha_i\\ R\sin\alpha_i \end{bmatrix}, \qquad u_i= \begin{bmatrix} \cos\beta_i\\ \sin\beta_i \end{bmatrix}\end{split}\]

body delta \(\xi=(\Delta x,\Delta y,\Delta\theta)^T\) による wheel 接地点の移動は、

\[\begin{split}\begin{bmatrix} \Delta x-\Delta\theta r_{iy}\\ \Delta y+\Delta\theta r_{ix} \end{bmatrix}\end{split}\]

です。これを駆動方向へ射影した量が wheel の接地点移動量 \(b_i\) になります。

\[\begin{split}u_i^T \begin{bmatrix} \Delta x-\Delta\theta r_{iy}\\ \Delta y+\Delta\theta r_{ix} \end{bmatrix} = \rho\Delta\omega_i\end{split}\]

ここで \(\rho\) は wheel_radius です。行列の 1 行は、

\[A_i= \begin{bmatrix} \cos\beta_i & \sin\beta_i & -\cos\beta_i r_{iy}+\sin\beta_i r_{ix} \end{bmatrix}\]

右辺は、

\[b_i=\rho\Delta\omega_i\]

です。

最小二乗

全 wheel について、

\[A\xi=b\]

を作り、正規方程式で解きます。

\[(A^TA)\xi=A^Tb\]

実装では 3x3 の Cramer 法で解きます。行列式が小さい場合は解けないため、その周期の odometry 更新をスキップします。

積分

body 座標の差分 \((\Delta x,\Delta y,\Delta\theta)\) は、中点 yaw で odom 座標へ回転して積分します。

\[\theta_m=\theta+\frac{1}{2}\Delta\theta\]

\[x\leftarrow x+\cos\theta_m\Delta x-\sin\theta_m\Delta y\]

\[y\leftarrow y+\sin\theta_m\Delta x+\cos\theta_m\Delta y\]

\[\theta\leftarrow\operatorname{wrap}(\theta+\Delta\theta)\]

速度は \(dt\) で割ります。

\[v_x=\frac{\Delta x}{dt},\qquad v_y=\frac{\Delta y}{dt},\qquad \omega=\frac{\Delta\theta}{dt}\]

設定ファイル

wheel_odometry はパッケージ同梱の rogi_localization/odometry/wheel_odometry/config/wheel_odometry.yaml を使います。rogi_nav.launch.py は sample profile から選んだ初期 pose だけを追加で上書きします。

key	sample/default 値	数式上の意味
rogidrive_status_topic	/rogidrive_status	encoder 入力 topic
odom_topic	/odom	odometry publish topic
odom_frame_id	odom	odom message frame / TF 親
base_frame_id	base_link	child frame
initial_pose_x/y/a	profile 由来	積分初期値 \((x,y,\theta)\)
motor_names	[wheel_fl, wheel_rl, wheel_rr, wheel_fr]	status から使う motor 名
wheel_radius	0.065	\(\rho\)
center_to_wheel	0.328	\(R\)
motor_to_omni_reduction_ratio	19.2	\(G\)
wheel_position_angles_deg	[45,135,-135,-45]	\(\alpha_i\)
wheel_drive_angles_deg	[135,-135,-45,45]	\(\beta_i\)
encoder_signs	[1,1,1,1]	\(s_i\)
rogidrive_position_is_revolutions	true	\(q_i\) を revolution として扱う
wrap_angle_delta	true	encoder 差分を \([-\pi,\pi]\) に畳む
publish_tf	true	odom -> base_link TF を publish

profile 側では launch.components.localization.wheel_odometry.enabled が起動可否です。sample の sim.yaml では false、real.yaml では true です。

cmd_vel_to_dcmotor_node と wheel 幾何が一致していないと、command と odometry が逆向きになります。少なくとも次の配列は同じ順序で揃えます。

\[\{\mathrm{motor\_names},\alpha_i,\beta_i,G,\rho,R\}_{cmd} = \{\mathrm{motor\_names},\alpha_i,\beta_i,G,\rho,R\}_{odom}\]