3輪オムニの計算

# 3輪オムニの計算<br>
３輪オムニの計算を自分用にまとめる。
3輪オムニのロボットを軸として、x,y方向に移動する場合の各ホイールの速度を求める。ここでは、軸を中心とした回転については一旦取り扱わない。
筆者が行列を忘れたので行列計算すらしない。

## ローカル座標系で考える

![3_wheel_omni.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3587353/2888e25f-563c-84f9-bce3-dd279a2641bd.jpeg)

上記の図のような3輪オムニについて考える。
任意の速度$V(x,y)$で移動するとき各ホイールの速度は
```math
\begin{align}
V_{w1} &= V_{x}cos\theta_1 +V_{y}sin\theta_1\\
V_{w2} &= V_{x}cos\theta_2 +V_{y}sin\theta_2\\
V_{w3} &= V_{x}cos\theta_3 +V_{y}sin\theta_3\\
\end{align}
```
であらわすことができる。

:::note
上記式の導出について説明する。

![omni_wheel.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3587353/2bd7ab05-1c49-5f80-6bb0-2496b39ecb08.jpeg)

上記の、図のようなオムニホイールと、速度ベクトルを考える。
**オムニホイールは前提としてホイールの回転方向に対して垂直な方向$W_y$を無視できる。**
速度ベクトル$V(V_x,V_y)$がオムニホイールの回転方向に与える速度は、
```math
\begin{align}
V(V_x,V_y)&=V(Vcos\theta^{\prime},Vsin\theta^{\prime})\\
\\
W_x &= Vcos(\theta-\theta^{\prime})\\
&=V(cos\theta cos(-\theta^{\prime})-sin\theta sin(-\theta^{\prime}))\\
&=V(cos\theta cos\theta^{\prime}+sin\theta sin\theta^{\prime})\\
&=V_x cos\theta+V_y sin\theta\\
\end{align}
```
となる。
:::

上記の図のような3輪オムニでは各ホイール間の角度は$\frac{2}{3}\pi$radである。ここで、x軸を$W_3$と水平の軸として考えて、x軸を0radとして、反時計回りを正とする。ホイールの回転方向がなす角$\theta_n$は、

```math
\begin{align}
\theta_1=&\frac{\pi}{3}\\
\theta_2=&-\frac{\pi}{3}\\
\theta_3=&\pi
\end{align}
```
となる。したがって、各ホイールの速度は、
```math
\begin{align}
V_{w1} &= V_{x}(\frac{1}{2}) +V_{y}(\frac{\sqrt{3}}{2})\\
V_{w2} &= V_{x}(\frac{1}{2}) +V_{y}(-\frac{\sqrt{3}}{2})\\
V_{w3} &= -V_{x}\\
\end{align}
```
となる。

## グローバル座標系への適用

ロボット中心のローカル座標系から、グローバル座標系に変換する。
グローバル座標系における速度$V(X,Y)$ベクトルがグローバル座標系のX軸となす角を$\theta_V$とする。さらに、ロボットが回転したときのローカル座標系とグローバル座標系のなす角を$\theta$とする。
このときローカル座標系における速度$V(x,y)$は、
```math
\begin{align}
V_x &=V(X,Y)cos(\theta_V-\theta)\\ 
V_y &=V(X,Y)sin(\theta_V-\theta)\\
\end{align}
```
となる。これを変形すると、
```math
\begin{align}
V_x &=V_Xcos(\theta)+V_Ysin(\theta)\\ 
V_y &=V_Ycos(\theta)-V_Xsin(\theta)\\
\end{align}
```
であるから、グローバル座標系では、
```math
\begin{align}
V_{w1} &= [V_Xcos(\theta)+V_Ysin(\theta)](\frac{1}{2}) +[V_Ycos(\theta)-V_Xsin(\theta)](\frac{\sqrt{3}}{2})\\
V_{w2} &= [V_Xcos(\theta)+V_Ysin(\theta)](\frac{1}{2}) +[V_Ycos(\theta)-V_Xsin(\theta)](-\frac{\sqrt{3}}{2})\\
V_{w3} &= -[V_Xcos(\theta)+V_Ysin(\theta)]\\
\end{align}
```
で表せる。

# 追記 

## 回転を取り扱う前提条件
重心から、角オムニホイールまでの距離を$r_n$とする。ここで、
```math
\begin{align}
r_1=r_2=r_3=R
\end{align}
```
とする。

## 3輪オムニの並進運動に適用する
目標とする角速度を$\omega$とする。ロボットの回転方向に対する速度を$V_{\theta}$とする。
```math
\begin{align}
V_{\theta}=R\omega
\end{align}
```
また、$V_{\theta}$はロボットの傾きの影響を受けないためローカル座標系、グローバル座標系において、同様に扱うことができる。
したがって、
```math
\begin{align}
V_{w1} &= [V_Xcos(\theta)+V_Ysin(\theta)](\frac{1}{2}) +[V_Ycos(\theta)-V_Xsin(\theta)](\frac{\sqrt{3}}{2})+R\omega\\
V_{w2} &= [V_Xcos(\theta)+V_Ysin(\theta)](\frac{1}{2}) +[V_Ycos(\theta)-V_Xsin(\theta)](-\frac{\sqrt{3}}{2})+R\omega\\
V_{w3} &= -[V_Xcos(\theta)+V_Ysin(\theta)]+R\omega\\
\end{align}
```
となる。（ローカル座標系のとき$\theta=0$）

## 予想
計算してないから知らんけど、オムニホイールを一般化すると、($\theta_n$はホイールの角度)

```math
\begin{align}
W_n &= V(X,Y)[cos(\theta_V-\theta)cos\theta_n+sin(\theta_V-\theta) sin\theta_n]+R\omega\\ 

&= [V(X,Y)cos(\theta_V-\theta)]cos\theta_n+[V(X,Y)sin(\theta_V-\theta)] sin\theta_n+R\omega\\ 

&=[V_Xcos(\theta)+V_Ysin(\theta)] cos\theta_n+[V_Ycos(\theta)-V_Xsin(\theta)] sin\theta_n+R\omega

\end{align}
```
になりそう。