全方向移動台車のオドメトリ

概要

ロボコンでよく行われる「接地エンコーダとジャイロセンサを用いたオドメトリ」についてのメモ。（実際はほかの記事から参照する予定なので書いただけ…）間違っている部分があるかもしれません。

機体中心を向いた接地エンコーダ2個

上の図はあくまでも一例です。接地エンコーダの場所は何でもいいです（機体中心を向いてさえいれば）。樽の軸方向のみの力が作用することに注意すると、

$\displaystyle v_1 = v_x\cos\theta + v_y\sin\theta \\ v_2 = -v_x\sin\theta + v_y\cos\theta$

これより、

$\displaystyle v_x = v_1\cos\theta - v_2\sin\theta \\ v_y = v_1\sin\theta + v_2\cos\theta$

$v_x$ 、 $v_y$ 、 $\omega$ を積分してやれば姿勢を推定できます。

接地エンコーダ2個

上の図はあくまでも一例です。接地エンコーダの場所は何でもいいです。樽の軸方向のみの力が作用することに注意すると、

$\displaystyle v_1 = v_x\cos\theta + v_y\sin\theta + l\omega\sin\alpha \\ v_2 = -v_x\sin\theta + v_y\cos\theta$

これより、

$\displaystyle v_x = (v_1-l\omega\sin\alpha)\cos\theta - v_2\sin\theta \\ v_y = (v_1-l\omega\sin\alpha)\sin\theta + v_2\cos\theta$

$v_x$ 、 $v_y$ 、 $\omega$ を積分してやれば姿勢を推定できます。

接地エンコーダ3個

上の図はあくまでも一例です。接地エンコーダの場所は何でもいいです。樽の軸方向のみの力が作用することに注意すると、

$\displaystyle \begin{bmatrix} v_1 \\ v_2 \\ v_3 \end{bmatrix} = \begin{bmatrix} \cos\theta & \sin\theta & l_1\sin\alpha \\ -\sin\theta & \cos\theta & 0 \\ -\sin\theta & \cos\theta & l_3\cos\beta \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}$

これより、

$\displaystyle \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix} = \begin{bmatrix} \cos\theta & \frac{l_1\sin\alpha\cos\theta}{l_3\cos\beta}-\sin\theta & -\frac{l_1 \sin\alpha\cos\theta}{l_3\cos\beta} \\ \sin\theta & \frac{l_1\sin\alpha\sin\theta}{l_3\cos\beta} + \cos\theta & -\frac{l_1 \sin\alpha\sin\theta}{l_3\cos\beta} \\ 0 & -\frac{1}{l_3\cos\beta} & \frac{1}{l_3\cos\beta} \end{bmatrix} \begin{bmatrix} v_1 \\ v_2 \\ v_3 \end{bmatrix}$

$v_x$ 、 $v_y$ 、 $\omega$ を積分してやれば姿勢を推定できます。

naokichiのブログ

しがないロボコニストの雑多なブログ

全方向移動台車のオドメトリ

概要

機体中心を向いた接地エンコーダ2個

接地エンコーダ2個

接地エンコーダ3個