はじめに
本記事では、オムニホイールロボットの制御(逆運動学)および自己位置推定(順運動学・オドメトリ)に必要な数式を導出・解説します。
1. 前提と定義
まずは本記事で用いる座標系および変数を定義します。
1.1 座標系の定義
ROS 2などのロボット開発で標準的に用いられる右手直交座標系を採用します。
(JIS B 8437:2016で規定されているみたいです)
ワールド座標系
フィールドに固定された絶対的な座標系 $(X, Y, \Theta)$ です。
- 原点: フィールド上の任意の点(スタート位置など)
- 角度: X軸を0とし、反時計回りを正とする
本記事では、ワールド座標系における物理量に大文字 $(X, Y, \Theta)$ を用います。
ロボット座標系
ロボットの旋回中心(幾何中心または重心)を原点とし、ロボットと共に移動・回転するローカルな座標系 $(x, y, \theta) $です。
- X軸: ロボットの正面方向
- Y軸: ロボットの左方向
本記事では、ロボット座標系における物理量に小文字 $(x, y, \theta)$ を用います。
1.2 変数とパラメータの定義
機体の状態に関する変数
機体の速度ベクトルを以下のように定義します。
- ワールド座標系での速度: $\boldsymbol{V} = [V_X, V_Y, \Omega]^T$
- ロボット座標系での速度: $\boldsymbol{v} = [v_x, v_y, \omega]^T$
ここで、角速度 $\Omega$ と $\omega$ は、座標系に依存しない同一の値となります($\Omega = \omega$)。
ホイールに関する変数
$i$ 番目のホイールに関するパラメータを以下のように定義します。ここでの座標 $(x_i, y_i)$ はすべてロボット座標系上の値です。
-
ホイールの位置 $(x_i, y_i)$
中心からホイール接地点までの座標 -
ホイールの配置距離 $L_i = \sqrt{x_i^2 + y_i^2}$
中心からホイールまでの直線距離。 -
配置角度 $\phi_i$
ホイール $i$ が配置されている角度 -
駆動角度 $\alpha_i$
そのホイール $i$ が駆動力を発生させる方角(ホイールが進もうとする方向) -
ホイール半径 $R$
すべてのホイールで同一であることを想定しています。
ホイールの出力に関する変数
-
ホイール速度 $v_i$ [m/s]
地面に対する接線方向の速度。 -
ホイール角速度 $\omega_i$ [rad/s]
モーターの回転速度。 $v_i = R\omega_i$ の関係にあります。
2. 入力からホイール出力まで(逆運動学)
上位システム(自律移動の経路プランナーや、手動操作時のコントローラー)から与えられた速度指令を実現するために、各モーターが回るべき速度を導出します。これを「逆運動学」と呼びます。
2.1 速度指令の座標変換
一般に、ロボットの移動目標はワールド座標系で計算されることが多いです。例えば、自律移動において「X軸に沿って進む」や「ゴール地点に向かって進む」といった処理は、ロボットがどちらを向いているかに関わらず、絶対座標系上の速度指令 $\boldsymbol{V} = [V_x, V_y, \Omega]^T$ として与えられます。
一方、ロボットの足回り(ホイール)は、機体正面を基準としたロボット座標系で物理的に配置されています。そのため、ワールド座標系で与えられた指令を、ロボット自身から見た速度ベクトルに変換する必要があります。
現在のロボットの方位(Yaw角)を $\theta$ とすると、回転行列 $R(-\theta)$ を用いて以下のように変換できます。
\begin{bmatrix}
v_x \\
v_y \\
\omega \\
\end{bmatrix}
=
\begin{bmatrix}
\cos\theta & \sin\theta & 0 \\
-\sin\theta & \cos\theta & 0 \\
0 & 0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
V_X \\
V_Y \\
\Omega \\
\end{bmatrix}
展開すると以下のようになります。
v_x = V_X\cos\theta + V_Y\sin\theta
v_y = -V_X\sin\theta + V_Y\cos\theta
\omega = \Omega
この変換を挟むことで、上位の経路生成ロジックはロボットの現在の向きを気にすることなく、地図上の移動ベクトルだけを計算すれば良くなります。
※旋回速度 $\omega$ については、座標系が変わっても値自体は変わらないため、そのまま $\omega = \Omega$ となります。
補足1: なぜ座標の差分だけではだめなのか?
現在位置 $(x_1, y_1, \theta_1)$ 目標位置 $(x_2, y_2, \theta_2)$ の差分 $(x_2 - x_1, y_2 - y_1, \theta_2 - \theta_1)$ をそのままロボットへの指令にしてはいけない理由を、具体的なケースで考えてみます。例えば現在位置 $(0, 0, 90^\circ)$ 、目標位置 $(10, 0, 90^\circ)$ のとき、引き算をすると $(10 - 0, 0 - 0, 90^\circ - 90^\circ) = (10, 0, 0)$ となり、「x方向に $+10$ 進めばいい」となります。
この $(10, 0, 0)$ をそのままロボットに入力してしまうと、ロボットはx軸に沿って、つまり自分の正面に向かって進み始めます。しかし、ロボットは $90^\circ$ を向いているため、ワールド座標系で見るとy軸に沿って動いてしまいます。これではゴールには辿り着けません。
このズレを防ぐために、回転行列を用いて座標変換をする必要があるのです。
\begin{bmatrix}
\cos90^\circ & \sin90^\circ & 0 \\
-\sin90^\circ & \cos90^\circ & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
10 \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & 0 \\
-1 & 0 & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
10 \\
0 \\
0 \\
\end{bmatrix}
=
\begin{bmatrix}
0 \\
-10 \\
0 \\
\end{bmatrix}
座標変換後の指令 $(0, -10, 0)$ を渡せば、ロボットは $90^\circ$ を向いたまま右方向に動き、正しくゴールへ向かうことができます。
補足2: なぜ $R(\theta)$ ではなく $R(-\theta)$ なのか?
通常の回転行列 $R(\theta)$ ではなく、なぜ $R(-\theta)$ をかけるのか疑問に思うかもしれませんが、その理由を考えてみます。例えばロボットがフィールドに対して左( $+\theta$ 方向)を向いたとします。このとき、ロボット自身の視点(ロボット座標系)から見ると、ゴールや景色は相対的に右( $-\theta$ 方向)に回転したように見えるはずです。
そのため、ワールド座標系からロボット座標系への変換行列は $R(-\theta)$ となります。
R(-\theta) =
\begin{bmatrix}
\cos-\theta & -\sin-\theta \\
\sin-\theta & \cos-\theta \\
\end{bmatrix}
=
\begin{bmatrix}
\cos\theta & \sin\theta \\
-\sin\theta & \cos\theta \\
\end{bmatrix}
2.2 ベクトル分解と射影
ロボット座標系での目標速度 $\boldsymbol{v} = [v_x, v_y, \omega]^T$ が決まったら、これを各ホイールの回転速度に分解します。
ホイールの速度ベクトル
まず、ロボットが速度ベクトル $\boldsymbol{v}$ で動くとき、ホイール $i$ が持つ速度ベクトル $\boldsymbol{v}_i$ は以下のようになります。
\boldsymbol{v}_i =
\begin{bmatrix}
v_x \\
v_y
\end{bmatrix}
+ \boldsymbol{v}_{rot}
回転成分 $\boldsymbol{v}_rot$ は、以下のようにして変形できます。
方法1: 基底ベクトルの微分
ロボットが角速度 $\omega$ で回転しているため、ロボット座標系も角速度 $\omega$ で回転しており、基底ベクトル $\boldsymbol{e}_x, \boldsymbol{e}_y$ は以下のように表すことができます。
\boldsymbol{e}_x(t) =
\begin{bmatrix}
\cos\omega t \\
\sin\omega t
\end{bmatrix}
\quad
\boldsymbol{e}_y(t) =
\begin{bmatrix}
-\sin\omega t \\
\cos\omega t
\end{bmatrix}
これを微分すると以下のようになります。
\begin{align}
\frac{d\boldsymbol{e}_x}{dt} &= \frac{d}{dt}
\begin{bmatrix}
\cos\omega t \\
\sin\omega t \\
\end{bmatrix} \\
&=
\begin{bmatrix}
-\omega\sin\omega t \\
\omega\cos\omega t \\
\end{bmatrix} \\
&= \omega
\begin{bmatrix}
-\sin\omega t \\
\cos\omega t \\
\end{bmatrix} \\
&= \omega\boldsymbol{e}_y
\end{align}
\begin{align}
\frac{d\boldsymbol{e}_y}{dt} &= \frac{d}{dt}
\begin{bmatrix}
-\sin\omega t \\
\cos\omega t \\
\end{bmatrix} \\
&=
\begin{bmatrix}
-\omega\cos\omega t \\
-\omega\sin\omega t \\
\end{bmatrix} \\
&= -\omega
\begin{bmatrix}
\cos\omega t \\
\sin\omega t \\
\end{bmatrix} \\
&= -\omega\boldsymbol{e}_x
\end{align}
ホイール $i$ の位置ベクトル $\boldsymbol{r}_i$ は以下のように表せます。
\boldsymbol{r}_i = x_i\boldsymbol{e}_x + y_i\boldsymbol{e}_y
これを微分すると、回転成分が得られます。
\begin{align}
\boldsymbol{v}_{rot} &= \frac{d\boldsymbol{r}_i}{dt} \\
&= x_i\frac{d\boldsymbol{e}_x}{dt} + y_i\frac{d\boldsymbol{e}_y}{dt} \\
&= x_i(\omega\boldsymbol{e}_y) + y_i(-\omega\boldsymbol{e}_x) \\
&= (-y_i\omega)\boldsymbol{e}_x + (x_i\omega)\boldsymbol{e}_y
\end{align}
これを成分表示に戻すと、以下のようになります。
\boldsymbol{v}_{rot} =
\begin{bmatrix}
-y_i\omega \\
x_i\omega \\
\end{bmatrix}
したがって、ホイール位置での合成速度ベクトルは以下のようになります。
\boldsymbol{v}_i =
\begin{bmatrix}
v_x - y_i \omega \\
v_y + x_i \omega \\
\end{bmatrix}
方法2: 外積
回転による速度ベクトル $\boldsymbol{v}$ は、角速度ベクトル $\boldsymbol{\omega}$ と位置ベクトル $\boldsymbol{r}$ の外積(クロス積)で計算できます。
\boldsymbol{v}_{rot} = \boldsymbol{\omega} \times \boldsymbol{r}
これを3次元ベクトルとして計算します。
角速度ベクトル: $\boldsymbol{\omega} = [0, 0, \omega]^T$ (Z軸周りの回転)
位置ベクトル: $\boldsymbol{r} = [x_i, y_i, 0]^T$
\begin{align}
\boldsymbol{v}_{rot} &= \boldsymbol{\omega} \times \boldsymbol{r} \\
&=\det
\begin{bmatrix}
\boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \\
0 & 0 & \omega \\
x_i & y_i & 0
\end{bmatrix} \\
&=
\boldsymbol{e}_x (0\cdot0 - \omega y_i) - \boldsymbol{e}_y (0\cdot0 - \omega x_i) + \boldsymbol{e}_z (0\cdot y_i - 0\cdot x_i)
\end{align}
したがって、ホイール位置での合成速度ベクトルは以下のようになります。
\boldsymbol{v}_i =
\begin{bmatrix}
v_x - y_i \omega \\
v_y + x_i \omega \\
\end{bmatrix}
駆動方向への射影
オムニホイールの最大の特徴は、ホイールが向いている方向( $\alpha_i$ )の力しか伝えないという点です。横方向の力はフリーローラーが空転して逃がします。
したがって、ホイール $i$ が負担すべき速度 $v_i$ は、ホイールの速度ベクトル $\boldsymbol{v}_i$ と、ホイールの駆動方向ベクトル $[\cos\alpha_i, \sin\alpha_i]^T$ の内積(正射影)として求まります。
\begin{align}
v_i &= \boldsymbol{v}_i \cdot [\cos\alpha_i, \sin\alpha_i]^T \\
&=
\begin{bmatrix}
v_x - y_i\omega \\
v_y + x_i\omega \\
\end{bmatrix}
\cdot
\begin{bmatrix}
\cos\alpha_i \\
\sin\alpha_i \\
\end{bmatrix}\\
&= (v_x - y_i \omega)\cos \alpha_i + (v_y + x_i \omega)\sin \alpha_i
\end{align}
これを展開して整理すると、オムニホイール制御の基本的な式が得られます。
v_i = v_x \cos \alpha_i + v_y \sin \alpha_i + \omega (x_i \sin \alpha_i - y_i \cos \alpha_i)
2.3 項の簡略化 (Lの導出)
上記の式の $\omega$ の係数(旋回項)は一見複雑ですが、ホイールの配置を考慮すると簡単になります。
ホイールの位置を極座標 $L_i\cos\phi_i$ で表し、加法定理 $\sin(A - B) = \sin A \cos B - \cos A \sin B$ を適用してみましょう。
\begin{aligned}
\text{旋回項} &= x_i \sin \alpha_i - y_i \cos \alpha_i \\
&= (L \cos \phi_i) \sin \alpha_i - (L \sin \phi_i) \cos \alpha_i \\
&= L (\sin \alpha_i \cos \phi_i - \cos \alpha_i \sin \phi_i) \\
&= L \sin(\alpha_i - \phi_i)
\end{aligned}
一般的なオムニホイールロボットでは、ホイールは機体中心に向かって垂直(接線方向)に取り付けられます。つまり、駆動角 $\alpha_i$ と配置角 $\phi_i$ の差は $90^\circ$ になります。
L \sin(90^\circ) = L
したがって、旋回項も以下のようになります。
v_i = v_x \cos \alpha_i + v_y \sin \alpha_i + \omega L
※ $L$ の符号は、ホイールの回転方向の定義によって変わるので注意してください。
$\alpha_i - \phi_i = 90^\circ$ の場合: $\sin(\alpha_i - \phi_i) = +1$ → 旋回項は $+\omega L$
$\alpha_i - \phi_i = -90^\circ$ の場合: $\sin(\alpha_i - \phi_i) = -1$ → 旋回項は $-\omega L$
2.4 行列にまとめる
最後に、先程の式を行列にまとめると以下のようになります。
機体の速度ベクトルからホイールの速度への変換です。
\begin{bmatrix}
v_1 \\
v_2 \\
\vdots \\
v_n
\end{bmatrix}
=
\begin{bmatrix}
\cos\alpha_1 & \sin\alpha_1 & L_1 \\
\cos\alpha_2 & \sin\alpha_2 & L_2 \\
\vdots & \vdots & \vdots \\
\cos\alpha_n & \sin\alpha_n & L_n \\
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y \\
\omega
\end{bmatrix}
ワールド座標系からの変換を含めると以下のようになります。
\begin{align}
\begin{bmatrix}
v_1 \\
v_2 \\
\vdots \\
v_n \\
\end{bmatrix}
&=
\begin{bmatrix}
\cos\alpha_1 & \sin\alpha_1 & L_1 \\
\cos\alpha_2 & \sin\alpha_2 & L_2 \\
\vdots & \vdots & \vdots \\
\cos\alpha_n & \sin\alpha_n & L_n \\
\end{bmatrix}
\begin{bmatrix}
\cos\theta & \sin\theta & 0 \\
-\sin\theta & \cos\theta & 0 \\
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
V_X \\
V_Y \\
\Omega \\
\end{bmatrix} \\
&=
\begin{bmatrix}
\cos\alpha_1\cos\theta - \sin\alpha_1\sin\theta & \cos\alpha_1\sin\theta + \sin\alpha_1\cos\theta & L_1 \\
\cos\alpha_2\cos\theta - \sin\alpha_2\sin\theta & \cos\alpha_2\sin\theta + \sin\alpha_2\cos\theta & L_2 \\
\vdots & \vdots & \vdots \\
\cos\alpha_n\cos\theta - \sin\alpha_n\sin\theta & \cos\alpha_n\sin\theta + \sin\alpha_n\cos\theta & L_n \\
\end{bmatrix}
\begin{bmatrix}
V_X \\
V_Y \\
\Omega \\
\end{bmatrix} \\
&=
\begin{bmatrix}
\cos(\alpha_1 + \theta) & \sin(\alpha_1 + \theta) & L_1 \\
\cos(\alpha_2 + \theta) & \sin(\alpha_2 + \theta) & L_2 \\
\vdots & \vdots & \vdots \\
\cos(\alpha_n + \theta) & \sin(\alpha_n + \theta) & L_n \\
\end{bmatrix}
\begin{bmatrix}
V_X \\
V_Y \\
\Omega \\
\end{bmatrix}
\end{align}
ホイールの回転速度は以下のようになります。
\begin{bmatrix}
\omega_1 \\
\omega_2 \\
\vdots \\
\omega_n \\
\end{bmatrix}
= \frac{1}{R}
\begin{bmatrix}
v_1 \\
v_2 \\
\vdots \\
v_n
\end{bmatrix}
3. 自己位置推定: センサ値から現在地まで(順運動学)
2章では、目標速度を各ホイールの回転速度に変換する「逆運動学」を扱いました。本章では、その逆の処理、つまり各ホイールのエンコーダ値から機体の速度・位置を推定する「順運動学」と「オドメトリ(自己位置推定)」について解説します。
3.1 順運動学
順運動学では、各ホイールの速度 $v_1, v_2, \ldots, v_n$ から、機体の速度 $v_x, v_y, \omega$ を求めます。
n輪の場合
2.4で導出した逆運動学の式を再掲します。
\begin{bmatrix}
v_1 \\
v_2 \\
\vdots \\
v_n
\end{bmatrix}
=
\begin{bmatrix}
\cos\alpha_1 & \sin\alpha_1 & L_1 \\
\cos\alpha_2 & \sin\alpha_2 & L_2 \\
\vdots & \vdots & \vdots \\
\cos\alpha_n & \sin\alpha_n & L_n
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y \\
\omega
\end{bmatrix}
これを以下のように表記します。
\boldsymbol{v}_{wheel} = J \boldsymbol{v}_{robot}
すると、順運動学は以下のようになります。
\boldsymbol{v}_{robot} = J^{-1} \boldsymbol{v}_{wheel}
ただし、この逆行列 $J^{-1}$ が存在するためには、$J$ が正則行列である必要があります。つまり、ホイールが3つである必要があります。
3輪の場合
3輪オムニホイールの最も一般的な配置は、機体中心から等距離 $L$ に120度間隔で配置する構成です。この場合、各ホイールの駆動角度は以下のようになります。
\alpha_1 = 90^\circ, \quad \alpha_2 = 210^\circ, \quad \alpha_3 = 330^\circ
※駆動角度の定義は機体設計によって異なる場合があります。ここでは、ホイール1が機体左方(y軸正方向)に駆動力を発生させる配置を想定しています。
このとき、行列 $J$ は以下のようになります。
J =
\begin{bmatrix}
\cos 90^\circ & \sin 90^\circ & L \\
\cos 210^\circ & \sin 210^\circ & L \\
\cos 330^\circ & \sin 330^\circ & L
\end{bmatrix}
=
\begin{bmatrix}
0 & 1 & L \\
-\frac{\sqrt{3}}{2} & -\frac{1}{2} & L \\
\frac{\sqrt{3}}{2} & -\frac{1}{2} & L
\end{bmatrix}
この行列の逆行列を計算すると、以下のようになります。
J^{-1} = \frac{1}{3}
\begin{bmatrix}
0 & -\sqrt{3} & \sqrt{3} \\
2 & -1 & -1 \\
\frac{1}{L} & \frac{1}{L} & \frac{1}{L}
\end{bmatrix}
したがって、機体の速度は以下のように計算できます。
\begin{bmatrix}
v_x \\
v_y \\
\omega
\end{bmatrix}
= \frac{1}{3}
\begin{bmatrix}
0 & -\sqrt{3} & \sqrt{3} \\
2 & -1 & -1 \\
\frac{1}{L} & \frac{1}{L} & \frac{1}{L}
\end{bmatrix}
\begin{bmatrix}
v_1 \\
v_2 \\
v_3
\end{bmatrix}
展開すると、以下のようになります。
v_x = \frac{1}{3}(-\sqrt{3}v_2 + \sqrt{3}v_3)
v_y = \frac{1}{3}(2v_1 - v_2 - v_3)
\omega = \frac{1}{3L}(v_1 + v_2 + v_3)
この式から、120度配置の3輪オムニホイールでは、各ホイールの速度の和が旋回速度に、差が並進速度に寄与していることがわかります。
4輪以上の場合(Moore-Penroseの疑似逆行列)
4輪以上のホイールを持つ場合、方程式の数(ホイールの数)が未知数(機体の速度3成分)よりも多くなります。そのため $J$ は4*3行列となり、通常の逆行列は存在しません。
この場合、Moore-Penroseの疑似逆行列(pseudo-inverse) $J^+$ を用いることで、解のない連立方程式に対しても、最小二乗法のようにして解を求めることができます。
J^+ = (J^T J)^{-1} J^T
疑似逆行列を用いると、順運動学は以下のようになります。
\boldsymbol{v}_{robot} = J^+ \boldsymbol{v}_{wheel}
4輪オムニホイールの例
例えば、4輪を90度間隔で配置した場合を考えます。
\alpha_1 = 45^\circ, \quad \alpha_2 = 135^\circ, \quad \alpha_3 = 225^\circ, \quad \alpha_4 = 315^\circ
このとき、行列 $J$ は以下のようになります。
J =
\begin{bmatrix}
\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & L \\
-\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & L \\
-\frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & L \\
\frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & L
\end{bmatrix}
疑似逆行列 $J^+$ を計算すると、以下のようになります。
J^+ = \frac{1}{4}
\begin{bmatrix}
\sqrt{2} & -\sqrt{2} & -{\sqrt{2}} & {\sqrt{2}} \\
{\sqrt{2}} & {\sqrt{2}} & -{\sqrt{2}} & -{\sqrt{2}} \\
\frac{1}{L} & \frac{1}{L} & \frac{1}{L} & \frac{1}{L}
\end{bmatrix}
実装例
実際の計算では、数値計算ライブラリ(NumPy、Eigenなど)を使用することが推奨されます。
Eigen::Matrix<double, N, 3> j;
for (int i = 0; i < N; i++) {
j(i, 0) = cos(wheels[i].theta);
j(i, 1) = sin(wheels[i].theta);
j(i, 2) = (wheels[i].x * j(i, 1) - wheels[i].y * j(i, 0));
}
// 逆行列 (N>=4のときMoore-Penroseの疑似逆行列)
Eigen::Matrix<double, 3, N> j_inv;
if constexpr (N == 3) {
j_inv = j.inverse();
} else {
j_inv = (j.transpose() * j).inverse() * j.transpose();
}
3.2 変位の算出
順運動学によって機体の速度 $\boldsymbol{v}_{robot} = [v_x, v_y, \omega]^T$ が得られたら、これを積分することで機体の位置 $(X, Y, \Theta)$ を推定します。この処理をオドメトリと呼びます。
速度と変位
理論的には、速度を時間積分して変位を求めますが、プログラム実装では2つのアプローチがあります。
方法1: 速度を計算してから積分
エンコーダのカウント値から角速度 $\omega_i$ を計算し、それをホイール周速度 $v_i = R\omega_i$ に変換してから順運動学を適用する方法です。
\omega_i = \frac{\Delta \text{count}_i}{\Delta t \cdot \text{CPR}} \cdot 2\pi
ここで、$\Delta t$ はサンプリング周期、CPRはエンコーダの1回転あたりのカウント数です。
問題点
$\Delta t$ の計測誤差(ジッター)や、制御ループの実行タイミングが一定でない場合、速度計算に影響します。
方法2: 変位を直接計算(推奨)
エンコーダの差分 $\Delta \text{count}_i$ から直接ホイールの移動距離 $\Delta l_i$ を計算し、それを順運動学の式に入力する方法です。
\Delta l_i = \frac{\Delta \text{count}_i}{\text{CPR}} \cdot 2\pi R
このとき、順運動学の式はそのまま使えます。
\begin{bmatrix}
\Delta x \\
\Delta y \\
\Delta \theta
\end{bmatrix}
= J^{-1}
\begin{bmatrix}
\Delta l_1 \\
\Delta l_2 \\
\vdots \\
\Delta l_n
\end{bmatrix}
利点
時間 $\Delta t$ を計測する必要がなく、エンコーダのカウント値だけから位置を推定できます。時間の計測誤差やの影響を受けません。また $\Delta t$ の計算が省略される分、浮動小数点の丸め誤差が小さくなります。
注意
この方法で得られるのは速度ではなく変位であることに注意してください。速度が必要な場合は、変位を $\Delta t$ で割る必要があります。オドメトリだけが目的なら、変位を直接積分するほうが精度は高くなります。
サンプリング周期と精度の関係
オドメトリの精度は、サンプリング周期(エンコーダを読み取る頻度)に大きく依存します。
- 高頻度サンプリング(小さい $\Delta t$): 各ステップでの変位が小さくなり、積分誤差が減少する
- 低頻度サンプリング(大きい $\Delta t$): 各ステップでの変位が大きくなり、特に旋回時の積分誤差が増大する
3.3 積分手法
最も単純な積分手法は、オイラー法です。(ステップ値を $k$ として表します)
\begin{align}
X_{k+1} &= X_k + \Delta x_k \cos\Theta_k - \Delta y_k \sin\Theta_k \\
Y_{k+1} &= Y_k + \Delta x_k \sin\Theta_k + \Delta y_k \cos\Theta_k \\
\Theta_{k+1} &= \Theta_k + \Delta\theta_k
\end{align}
この方法は実装が簡単ですが、旋回しながら移動する場合に大きな誤差が生じます。
オイラー法では、ステップ開始時の角度 $\Theta_k$ を使って座標変換を行います。しかし、実際にはロボットは $\Delta\theta_k$ だけ回転しながら移動しているため、移動の途中で角度が変化しています。
この差が累積すると、長時間の走行で位置推定が大きくずれていきます。
1. 中点法(2次 Runge-Kutta法)による精度改善
精度を向上させる簡単な方法が、中点法または2次 Runge-Kutta法です。
具体的には、移動の開始時と終了時の平均の角度を使って座標変換を行います。
\Theta_{mid} = \Theta_k + \frac{\Delta\theta_k}{2}
\begin{align}
X_{k+1} &= X_k + \Delta x_k \cos\Theta_{mid} - \Delta y_k \sin\Theta_{mid} \\
Y_{k+1} &= Y_k + \Delta x_k \sin\Theta_{mid} + \Delta y_k \cos\Theta_{mid} \\
\Theta_{k+1} &= \Theta_k + \Delta\theta_k
\end{align}
この修正だけで、旋回しながら移動するときの誤差が減少します。
なぜ中点法が有効なのか
中点法では、移動の平均的な方向を考慮しています。
例えば、ロボットが $0^\circ$ から $90^\circ$ まで旋回しながら前進する場合を考えます。
-
オイラー法: $0^\circ$ の方向に進むと仮定して座標変換
→ 実際には $45^\circ$ 付近の方向に進んでいるのに、$0^\circ$ として計算するため誤差が大きい -
中点法: $45^\circ$ の方向に進むと仮定して座標変換
→ 実際の移動方向に近いため、誤差が小さい
計算コストはほとんど変わらないため、オドメトリでは中点法を使うことが強く推奨されます。
実装例
実装の複雑さはほとんど変わりません。
# エンコーダから変位を計算
Eigen::Vector<double, N> delta_l = encoder_counts_to_distance(delta_counts)
# 順運動学: ロボット座標系での変位
Eigen::Vector3d delta_robot = j_inv * delta_l;
# 中点の角度を計算
theta_mid = current_theta + delta_robot(2) / 2.0
# ワールド座標系への変換(中点法)
cos_mid = cos(theta_mid)
sin_mid = sin(theta_mid)
current_X += delta_robot(0) * cos_mid - delta_robot(1) * sin_mid
current_Y += delta_robot(0) * sin_mid + delta_robot(1) * cos_mid
current_theta += delta_robot(2)
2. 円弧近似による精度改善
中点法よりもさらに精度を上げたい場合、円弧近似と呼ばれる手法を用います。これは、 $\Delta t$ の間、ロボットが一定の角速度と並進速度で移動すると仮定し、その軌跡(円弧)を定積分する手法です。
(ただ計算コストを要するので、中点法を使ったまま制御周波数を上げたほうが精度は良くなるかもしれません)
導出
ロボット座標系での速度 $v_x, v_y, \omega$ が一定であるとき、時間 $t$ (ただし $0 <= t <= \Delta t$ ) における機体の角度 は以下のようになります。
\Theta(t) = \Theta_k + \omega t
このとき、ワールド座標系での速度は以下のように記述できます。
\begin{align}
\dot{X}(t) &= v_x \cos(\Theta_k + \omega t) - v_y \sin(\Theta_k + \omega t) \\
\dot{Y}(t) &= v_x \sin(\Theta_k + \omega t) + v_y \cos(\Theta_k + \omega t)
\end{align}
これを $0$ から $t$ まで定積分することで、より誤差の小さい移動量 $\Delta X, \Delta Y$ を算出できます。
\begin{align}
\Delta X &= \int_0^{\Delta t} (v_x\cos(\Theta_k + \omega t) - v_y\sin(\Theta_k + \omega t))\,dt \\
&= v_x \left[\frac{\sin(\Theta_k + \omega t)}{\omega} \right]_0^{\Delta t} - v_y \left[-\frac{\cos(\Theta_k + \omega t)}{\omega} \right] \\
&= \frac{v_x}{\omega}(\sin(\Theta_k + \omega \Delta t) - \sin\Theta_k) + \frac{v_y}{\omega}(\cos(\Theta_k + \omega \Delta t) - \cos\Theta_k) \\
&= \frac{v_x}{\omega}(\sin(\Theta_k + \Delta \theta) - \sin\Theta_k) + \frac{v_y}{\omega}(\cos(\Theta_k + \Delta \theta) - \cos\Theta_k) \\
&= \frac{v_x}{\omega}(\sin\Theta_k \cos\Delta \theta +\cos\Theta_k \sin\Delta\theta - \sin\Theta_k) + \frac{v_y}{\omega}(\cos\Theta_k \cos\Delta\theta - \sin\Theta_k \sin\Delta\theta - \cos\Theta_k) \\
&= \frac{v_x}{\omega}(\sin\Theta_k (\cos\Delta\theta - 1) + \cos\Theta_k \sin\Delta\theta) + \frac{v_y}{\omega}(\cos\Theta_k (\cos\Delta\theta - 1) - \sin\Theta_k \sin\Delta\theta) \\
&= \frac{1}{\omega}(\cos\Theta_k(v_x\sin\Delta\theta - v_y(1-\cos\Delta\theta)) - \sin\Theta_k(v_x(1-\cos\Delta\theta) + \sin\Delta\theta)) \\
&= \frac{1}{\omega}
\begin{bmatrix}
\cos\Theta_k & -\sin\Theta_k
\end{bmatrix}
\begin{bmatrix}
v_x\sin\Delta\theta - v_y(1-\cos\Delta\theta) \\
v_x(1-\cos\Delta\theta) + v_y\sin\Delta\theta
\end{bmatrix} \\
&= \frac{1}{\omega}
\begin{bmatrix}
\cos\Theta_k & -\sin\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y
\end{bmatrix} \\
&= \frac{\Delta t}{\Delta\theta}
\begin{bmatrix}
\cos\Theta_k & -\sin\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y
\end{bmatrix} \\
&= \frac{1}{\Delta\theta}
\begin{bmatrix}
\cos\Theta_k & -\sin\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
\Delta x \\
\Delta y
\end{bmatrix} \\
\end{align}
\begin{align}
\Delta Y &= \int_0^{\Delta t} (v_x\sin(\Theta_k + \omega t) + v_y\cos(\Theta_k + \omega t))\,dt \\
&= v_x \left[-\frac{\cos(\Theta_k + \omega t)}{\omega} \right]_0^{\Delta t} + v_y \left[\frac{\sin(\Theta_k + \omega t)}{\omega} \right]_0^{\Delta t} \\
&= -\frac{v_x}{\omega} (\cos(\Theta_k + \omega \Delta t) - \cos\Theta_k) + \frac{v_y}{\omega} (\sin(\Theta_k + \omega \Delta t) - \sin\Theta_k) \\
&= -\frac{v_x}{\omega} (\cos(\Theta_k + \Delta \theta) - \cos\Theta_k) + \frac{v_y}{\omega} (\sin(\Theta_k + \Delta \theta) - \sin\Theta_k) \\
&= -\frac{v_x}{\omega} (\cos\Theta_k \cos\Delta\theta - \sin\Theta_k \sin\Delta\theta - \cos\Theta_k) + \frac{v_y}{\omega}(\sin\Theta_k \cos\Delta\theta + \cos\Theta_k \sin\Delta\theta - \sin\Theta_k) \\
&= -\frac{v_x}{\omega} (\cos\Theta_k (\cos\Delta\theta - 1) - \sin\Theta_k \sin\Delta\theta) + \frac{v_y}{\omega}(\sin\Theta_k (\cos\Delta\theta - 1) + \cos\Theta_k \sin\Delta\theta) \\
&= \frac{1}{\omega} (\sin\Theta_k(v_x\sin\Delta\theta - v_y(1 - \cos\Delta\theta)) + \cos\Theta_k(v_x(1-\cos\Delta\theta) + v_y\sin\Delta\theta)) \\
&= \frac{1}{\omega}
\begin{bmatrix}
\sin\Theta_k & \cos\Theta_k
\end{bmatrix}
\begin{bmatrix}
v_x\sin\Delta\theta - v_y(1-\cos\Delta\theta) \\
v_x(1-\cos\Delta\theta) + v_y\sin\Delta\theta
\end{bmatrix} \\
&= \frac{1}{\omega}
\begin{bmatrix}
\sin\Theta_k & \cos\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y
\end{bmatrix} \\
&= \frac{\Delta t}{\Delta\theta}
\begin{bmatrix}
\sin\Theta_k & \cos\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
v_x \\
v_y
\end{bmatrix} \\
&= \frac{1}{\Delta\theta}
\begin{bmatrix}
\sin\Theta_k & \cos\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
\Delta x \\
\Delta y
\end{bmatrix} \\
\end{align}
まとめると以下のようになります。
\begin{bmatrix}
\Delta X \\
\Delta Y
\end{bmatrix}
=
\frac{1}{\Delta\theta}
\begin{bmatrix}
\cos\Theta_k & -\sin\Theta_k \\
\sin\Theta_k & \cos\Theta_k
\end{bmatrix}
\begin{bmatrix}
\sin\Delta\theta & -(1-\cos\Delta\theta) \\
1-\cos\Delta\theta & \sin\Delta\theta
\end{bmatrix}
\begin{bmatrix}
\Delta x \\
\Delta y
\end{bmatrix}
ゼロ除算への対処
上記の式において、分母に $\Delta \theta$ が含まれているため、ロボットが停止・直進するとき、つまり $\Delta \theta = 0$ のとき、ゼロ除算を起こします。
丸め誤差のことも考え、実装では $|\Delta \theta|$ がある閾値を下回った場合、中点法やテイラー展開による近似に切り替えることにしました。
実装例
// delta_robot = [dx, dy, dtheta]^T
double d_theta = delta_robot.z();
double sin_term, cos_term;
// 10^-6で閾値処理
if (abs(d_theta) < 1e-6) {
// テイラー展開で近似
// sin(x)/x ≈ 1 - x^2/6
sin_term = 1.0 - (d_theta * d_theta) / 6.0;
// (1-cos(x))/x ≈ x/2 - x^3/24
cos_term = d_theta / 2.0 - (d_theta * d_theta * d_theta) / 24.0;
} else {
sin_term = sin(d_theta) / d_theta;
cos_term = (1.0 - cos(d_theta)) / d_theta;
}
// 回転行列
Eigen::Rotation2Dd matrix_r(position_.z());
// 変換行列
Eigen::Matrix2d matrix_c;
matrix_c << sin_term, -cos_term, cos_term, sin_term;
Eigen::Vector2d delta_global =
matrix_r * matrix_c * delta_local.head<2>();
current_X += delta_global.x();
current_Y+= delta_global.y();
current_theta += d_theta;