車輪

永井 忠一 2025.5.5

資料

速度 v, ω で移動するロボット

v, ω が入力（対向２輪ロボット。前進後退、旋回）。xc, yc は回転中心。r は、回転半径（半径に角速度を掛けると速度。\( v = r\omega \)） \[ \begin{align} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} &= \begin{pmatrix} x_\mathrm c + r\cos(\theta - 90^\circ + \omega\Delta t) \\ y_\mathrm c + r\sin(\theta - 90^\circ + \omega\Delta t) \\ \theta + \omega\Delta t \end{pmatrix} \\ &= \begin{pmatrix} x_\mathrm c + r\sin(\theta + \omega\Delta t) \\ y_\mathrm c - r\cos(\theta + \omega\Delta t) \\ \theta + \omega\Delta t \end{pmatrix} \\ &= \begin{pmatrix} x_\mathrm c + \frac{v}{\omega}\sin(\theta + \omega\Delta t) \\ y_\mathrm c - \frac{v}{\omega}\cos(\theta + \omega\Delta t) \\ \theta + \omega\Delta t \end{pmatrix} \end{align} \]

回転中心\[ \begin{align} x_\mathrm c &= x - r\cos(\theta - 90^\circ) \\ &= x - r\sin\theta \\ &= x - \frac{v}{\omega}\sin\theta \end{align} \] \[ \begin{align} y_\mathrm c &= y - r\sin(\theta - 90^\circ) \\ &= y + r\cos\theta \\ &= y + \frac{v}{\omega}\cos\theta \end{align} \]

式を代入\[ \begin{align} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} &= \begin{pmatrix} x_\mathrm c + \frac{v}{\omega}\sin(\theta + \omega\Delta t) \\ y_\mathrm c - \frac{v}{\omega}\cos(\theta + \omega\Delta t) \\ \theta + \omega\Delta t \end{pmatrix} \\ &= \begin{pmatrix} x - \frac{v}{\omega}\sin\theta + \frac{v}{\omega}\sin(\theta + \omega\Delta t) \\ y + \frac{v}{\omega}\cos\theta - \frac{v}{\omega}\cos(\theta + \omega\Delta t) \\ \theta + \omega\Delta t \end{pmatrix} \\ &= \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} + \begin{pmatrix} -\frac{v}{\omega}\sin\theta + \frac{v}{\omega}\sin(\theta + \omega\Delta t) \\ \frac{v}{\omega}\cos\theta - \frac{v}{\omega}\cos(\theta + \omega\Delta t) \\ \omega\Delta t \end{pmatrix} \end{align} \]

Δt 秒後の、（理想的な）ロボットの位置・姿勢 \( \vec x_t = (x', y', \theta')^\top \)

作図プログラム

Python
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib import patches, collections

from math import pi as π, sin, cos, degrees, tan
W = 0.3 # m
x = 0.5 + W/2; y = 0.5; θ = π*(2/3)
r = 0.5 # m

plt.clf()
ax = plt.gca()
ax.grid()
ax.set_xticks([0, x]); ax.set_yticks([0, y])
ax.set_xticklabels([0, 'x']); ax.set_yticklabels([0, 'y'])
ax.spines['top'].set_visible(False); ax.spines['right'].set_visible(False)
ax.set_aspect('equal')

ax.add_patch(patches.Circle((x, y), W/2, edgecolor='k', facecolor='none'))
ax.add_collection(collections.LineCollection([[[x, y], [x + (W/2)*cos(θ), y + (W/2)*sin(θ)]]], colors='k'))
ax.add_patch(patches.Arc((x - r*cos(θ - π/2), y - r*sin(θ - π/2)), width=r*2, height=r*2, theta1=degrees((θ - π/2)), theta2=90, edgecolor='gray', ls='--'))
ax.add_collection(collections.LineCollection([[[x - r*cos(θ - π/2), y - r*sin(θ - π/2)], [x, y]],
                                              [[x - r*cos(θ - π/2), y - r*sin(θ - π/2)], [x, y - r*sin(θ - π/2)]]], colors='gray'))
ax.add_patch(patches.Arc((x - r*cos(θ - π/2), y - r*sin(θ - π/2)), width=0.3, height=0.3, theta1=0, theta2=degrees((θ - π/2)), edgecolor='gray'))
ax.text((x - (r*cos(θ - π/2))/2), (y - (r*sin(θ - π/2))/2), r'$ r\, $', ha='right')
ax.text((x - r*cos(θ - π/2)), (y - r*sin(θ - π/2)), r'$ \langle x_\mathrm{c}, y_\mathrm{c} \rangle $', va='top', ha='center')
ax.text((x - r*cos(θ - π/2) + 0.3/2), (y - r*sin(θ - π/2) + (0.3/2*tan(θ - π/2)/2)), r'$ \theta - 90^\circ $', va='center')
ax.annotate(r'$ (x, y, \theta)^\top $', (x, y), xytext=(x + W/2, y + W/2), arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=0.3', color='gray'))

plt.savefig('fig1.svg')

教科書

微分運動学

R が回転半径、W が車輪間距離(track width)\[ \left\{ \begin{align} &v_\mathrm L = \left(R - \frac{W}{2}\right)\omega \\ &v_\mathrm R = \left(R + \frac{W}{2}\right)\omega \\ &v = R\omega \end{align} \right. \]

R を消去して、（v と ω について）連立方程式を解く

Maxima
$ maxima

Maxima 5.46.0 https://maxima.sourceforge.io
using Lisp GNU Common Lisp (GCL) GCL 2.6.14 git tag Version_2_6_15pre7
Distributed under the GNU Public License. See the file COPYING.
Dedicated to the memory of William Schelter.
The function bug_report() provides bug reporting information.
(%i1) v_L = (R - W/2)*\\omega;
                                        W
(%o1)                        v_L = (R - -) \omega
                                        2
(%i2) v_R = (R + W/2)*\\omega;
                                    W
(%o2)                        v_R = (- + R) \omega
                                    2
(%i3) v = R*\\omega;
(%o3)                            v = R \omega
(%i4) solve(%, R);
                                        v
(%o4)                            [R = ------]
                                      \omega
(%i5) subst(%, [%th(4), %th(3)]);
                             v      W                   v      W
(%o5)       [v_L = \omega (------ - -), v_R = \omega (------ + -)]
                           \omega   2                 \omega   2
(%i6) solve(%, [v, \\omega]);
                          v_R + v_L             v_L - v_R
(%o6)               [[v = ---------, \omega = - ---------]]
                              2                     W

（微分）順運動学 FK\[ \begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{pmatrix} \frac{v_\mathrm R + v_\mathrm L}{2} \\ \frac{v_\mathrm R - v_\mathrm L}{W} \end{pmatrix} = \begin{bmatrix} \frac{1}{2} & \frac{1}{2} \\ \frac{1}{W} & -\frac{1}{W} \end{bmatrix}\begin{bmatrix} v_\mathrm R \\ v_\mathrm L \end{bmatrix} \]

（微分）逆運動学 IK\[ \begin{bmatrix} v_\mathrm R \\ v_\mathrm L \end{bmatrix} = \begin{pmatrix} \frac{1}{2} & \frac{1}{2} \\ \frac{1}{W} & -\frac{1}{W} \end{pmatrix}^{-1}\begin{pmatrix} v \\ \omega \end{pmatrix} = \begin{bmatrix} 1 & \frac{W}{2} \\ 1 & -\frac{W}{2} \end{bmatrix}\begin{bmatrix} v \\ \omega \end{bmatrix} \]

（逆行列。記号計算）

MaximaPython SymPy
(%i1) invert(matrix([1/2, 1/2], [1/W, -1/W]));
                                  [     W  ]
                                  [ 1   -  ]
                                  [     2  ]
(%o1)                             [        ]
                                  [      W ]
                                  [ 1  - - ]
                                  [      2 ]

$ ipython3
Python 3.12.3 (main, Feb  4 2025, 14:48:35) [GCC 13.3.0]
Type 'copyright', 'credits' or 'license' for more information
IPython 8.20.0 -- An enhanced Interactive Python. Type '?' for help.

In [1]: from sympy import symbols, Matrix

In [2]: W = symbols('W')

In [3]: Matrix([[1/2, 1/2], [1/W, -1/W]]).inv()
Out[3]: 
Matrix([
[1.0,  0.5*W],
[1.0, -0.5*W]])

In [4]: from sympy import Rational

In [5]: Matrix([[Rational(1, 2), Rational(1, 2)], [1/W, -1/W]]).inv()
Out[5]: 
Matrix([
[1,  W/2],
[1, -W/2]])

２自由度（全方位移動は、できない）。ロボットは、横方向に動けない

作図プログラム

Python
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib import patches, collections

plt.clf(); plt.axis('off')
ax = plt.gca()
ax.set_aspect('equal')
ax.set_xlim((-0.5, 0.5)); ax.set_ylim((-0.5, 0.5))

W = 0.3 # m
R = 0.5 # m
x = 0; y = 0
ax.add_patch(patches.Circle((x, y), W/2, edgecolor='k', facecolor='none'))
ax.add_collection(collections.LineCollection([[[x, y], [x, y + W/2]]], colors='k'))
ax.text(x, y, r'$ \, (v, \omega)^\top $')
ax.add_patch(patches.FancyArrowPatch((x - W/2, y), (x - W/2, y + W), arrowstyle='->', mutation_scale=20, color='gray'))
ax.add_patch(patches.FancyArrowPatch((x + W/2, y), (x + W/2, y + W), arrowstyle='->', mutation_scale=20, color='gray'))
ax.text(x - W/2, y + W/2, r'$ \ v_\mathrm{l} $', ha='center', backgroundcolor=ax.get_facecolor())
ax.text(x + W/2, y + W/2, r'$ \ v_\mathrm{r} $', ha='center', backgroundcolor=ax.get_facecolor())
ax.add_patch(patches.FancyArrowPatch((x - R, y), (x, y), arrowstyle='->', mutation_scale=20, color='gray'))
ax.text(x - R/2, y, r'$ \ R $', ha='center', va='center', backgroundcolor=ax.get_facecolor())
ax.add_patch(patches.FancyArrowPatch((x - W/2, y - W*(2/3)), (x + W/2, y - W*(2/3)), arrowstyle='<->', mutation_scale=20, color='gray'))
ax.text(x, y - W*(2/3), r'$ \ W $', ha='center', va='center', backgroundcolor=ax.get_facecolor())

plt.savefig('fig2.svg')

デッドレコニング。ロボットの位置・姿勢 \( \vec p(t) = (x(t), y(t), \theta(t))^\top \)

連続時間\[ \begin{align} &x(t) = x(t_0) + \int_{t_0}^{t}v\cdot\cos\theta(\tau)\,d\tau \\ &y(t) = y(t_0) + \int_{t_0}^{t}v\cdot\sin\theta(\tau)\,d\tau \\ &\theta(t) = \theta(t_0) + \int_{t_0}^{t}\omega(\tau)\,d\tau \end{align} \]ここで、\( v\cdot\cos\theta(\tau) \) は、時刻 τ における速度のX軸成分。\( v\cdot\sin\theta(\tau) \) は、速度のY軸成分

離散時間（の積分計算）\[ \begin{align} &x(t) = x(t_0) + \sum_{\tau = t_0}^{t}v(\tau)\Delta\tau\cdot\cos\theta(\tau) \\ &y(t) = y(t_0) + \sum_{\tau = t_0}^{t}v(\tau)\Delta\tau\cdot\sin\theta(\tau) \\ &\theta(t) = \theta(t_0) + \sum_{\tau = t_0}^{t}\omega(\tau)\Delta\tau \end{align} \]

\( x(t_0) \)、\( y(t_0) \)、\( \theta(t_0) \) は、それぞれの初期値（初期位置・姿勢）

作図プログラム

Python
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib import patches, collections

plt.clf()
ax = plt.gca()
ax.set_xticklabels([]); ax.set_yticklabels([])
ax.set_xticks([]); ax.set_yticks([])
ax.spines['top'].set_visible(False); ax.spines['right'].set_visible(False)
ax.set_aspect('equal')

from math import pi as π, sin, cos
W = 0.3 # m
x = 0.5; y = 0.5; θ = π/4
ax.add_patch(patches.Circle((x, y), W/2, edgecolor='k', facecolor='none'))
ax.add_collection(collections.LineCollection([[[x, y], [x + (W/2)*cos(θ), y + (W/2)*sin(θ)]]], colors='k'))
ax.text(x, y, r'$ \, (v, \omega)^\top $', va='top')
ax.annotate(r'$ (x, y, \theta)^\top $', (x, y), xytext=(x, y - W), arrowprops=dict(arrowstyle='->', connectionstyle='arc3,rad=-0.3', color='gray'))

ax.set_xlabel('X'); ax.set_ylabel('Y')
plt.savefig('fig3.svg')

教科書

© 2025 Tadakazu Nagai