---
# System prepended metadata

title: balanced infantry
tags: [機械, 理論]

---

## 1 ロボットモデリング
車輪脚2足歩行ロボットの全身運動は、車輪運動と脚運動に分離することができる。 車輪運動には、平面運動(ボディバランス、前後運動)と操舵運動が含まれる。 脚部運動には、ジャンプ運動、高さ調整運動、地面適応運動が含まれる。 本章では、これらの各運動過程のダイナミクスをモデル化し、物理量間の数学的関係を見つけることに焦点を当て、その後のコントローラ設計と関節モータの選択に役立てる。
## 2.1 足周りモデル
ロボットの車輪運動については、次のような理想的な仮定を置くことができる。 脚の動きが車輪の動きに与える影響を無視する。 脚連結部の質量を無視する。 駆動輪と地面は滑らない転がり摩擦である。
以上の理想化された仮定に基づき、車輪脚型二足歩行ロボットの車輪運動は等価的に、図1 に示すような重心位置が可変の二輪倒立振子モデルに簡略化することができ、図中のパラメータは表 2-1 に示す通りである。
![image](https://hackmd.io/_uploads/rJ00NGo-Jg.png)
**図１二輪倒立振子モデル**

**表2-1 二輪倒立振子モデルのパラメータ**

| パラメータ記号           | パラメータの意味                    | 単位     |
|-------------------------|---------------------------------|---------|
| $M$                     | 機体質量                          | kg      |
| $m$                     | 駆動輪質量                        | kg      |
| $l$                     | 機体質心と車軸の距離              | m       |
| $r$                     | 駆動輪半径                        | m       |
| $D$                     | 左右駆動輪間の距離                | m       |
| $g$                     | 重力加速度                        | m/s²    |
| $\theta$                | 機体の俯仰角                     | rad     |
| $\dot{\theta}$          | 機体の俯仰角速度                 | rad/s   |
| $\ddot{\theta}$         | 機体の俯仰角加速度               | rad/s²  |
| $T_L, T_R$              | 左、右輪の駆動トルク              | N・m    |
| $N_L, N_R$              | 機体と左、右輪のx方向の相互作用力 | N       |
| $P_L, P_R$              | 機体と左、右輪のy方向の相互作用力 | N       |
| $F_L, F_R$              | 左、右輪と地面の摩擦力            | N       |
| $x_L, x_R$              | 左、右輪の車軸位置                | m       |
| $\dot{x}_L, \dot{x}_R$  | 左、右輪の車軸速度               | m/s     |
| $\ddot{x}_L, \ddot{x}_R$| 左、右輪の車軸加速度            | m/s²    |
| $\omega_L, \omega_R$    | 左、右輪の回転角速度             | rad/s   |
| $\dot{\omega}_L, \dot{\omega}_R$ | 左、右輪の回転角加速度 | rad/s²  |
| $J_w$                   | 駆動輪の回転慣性モーメント         | kg・m²  |
| $J_z$                   | 機体のz軸の回転慣性モーメント     | kg・m²  |
| $J_y$                   | 機体のy軸の回転慣性モーメント     | kg・m²  |
| $J_x$                   | 機体のx軸の回転慣性モーメント     | kg・m²  |
| $\delta$                | 機体の航向角                     | rad     |
| $\dot{\delta}$          | 機体の航向角速度                 | rad/s   |
| $\ddot{\delta}$         | 機体の航向角加速度               | rad/s²  |
| $F_y$                   | 機体のy方向の仮想力               | N       |
| $T_x$                   | 機体x軸の仮想トルク              | N・m    |
| $y$                     | 機体質心のy方向の位置             | m       |
| $\dot{y}$               | 機体質心のy方向の速度             | m/s     |
| $\ddot{y}$              | 機体質心のy方向の加速度           | m/s²    |
| $\beta$                 | 機体のロール角                   | rad     |
| $\dot{\beta}$           | 機体のロール角速度               | rad/s   |
| $\ddot{\beta}$          | 機体のロール角加速度             | rad/s²  |
| $\tau$                  | 左側モータの電気トルク            | N・m    |

### 2.1.1 平面運動の運動モデリング
2輪倒立振子モデルは、左右の駆動輪と車体の2つの部分に分割され、これら2つの部分は別々に解析される。 左駆動輪の簡略化モデルを図 2 に、物理パラメータを表 2-1 に示す。
![image](https://hackmd.io/_uploads/rJoFHGiZyl.png)
**図２　左駆動輪の簡易モデル**
左の駆動輪のX方向の力の釣り合い式：
$$
m\ddot{x_L}=F_L-N_L\tag{2.1}
$$
左駆動輪の車軸周りのモーメントバランス方程式：
$$
I\dot{\omega_L}=T_L-F_Lr\tag{2.2}
$$
式(2.1)と式(2.2)を関連づけて:
$$
I\dot{\omega_L}=T_L-r(m\ddot{x_L}+N_L)\tag{2.3}
$$
車輪軸の加速度に対する駆動輪の回転角加速度の式：
$$
\dot{\omega_L}=\frac{\ddot{x_L}}{r}\tag{2.4}
$$
式(2.4)を式(2.3)に代入して照合すると、左駆動輪の力学方程式が得られる：
$$
(m+\frac{I}{r^2})\ddot{x_L}=\frac{T_L}{r}-N_L\tag{2.5}
$$
同様に右駆動輪の力学方程式が得られる：
$$
(m+\frac{I}{r^2})\ddot{x_R}=\frac{T_R}{r}-N_R\tag{2.6}
$$
機体加速度は左右の駆動輪加速度の平均値：
$$
\ddot{x}=\frac{\ddot{x_L}+\ddot{x_R}}{2}\tag{2.7}
$$
(2.5)、(2.6)、(2.7)式より次式が得られる：
$$
(m+\frac{1}{r^2})\ddot{x}=\frac{T_L+T_r}{2r}-\frac{N_L+N_R}{2}\tag{2.8}
$$
図 3 に示すように、本体は x-y 平面において倒立振子として扱われ、物理パラメータは表 2-1 に示されている。
![image](https://hackmd.io/_uploads/ry9EsViZ1x.png)
**図３ボディ倒立振子モデル**
機械の質量中心を対象として力の分析を行い、$N_L + N_R$ および $P_L + P_R$を質量中心に移動すると、トルクが発生し、これを $T_N$ および $T_P$ と呼ぶ：
$$
T_N=(N_L+N_R)lcos\theta\;,\;T_P=(P_L+P_R)lsin\theta\tag{2.9}
$$
質量中心の絶対速度は$v_x$$v_y$に分解される：
$$
v_x=\dot{x}+l\dot{\theta}cos\theta\;,\;v_y=l\dot{\theta}sin\theta\tag{2.10}
$$


質量の$M$方向の力の平衡方程式：

$$
M\ddot{x} = N_L + N_R \tag{2.11}
$$

式(2.10)を式(2.11)に代入して得られる式は：

$$
M(\ddot{x} + l\dot{\theta}\cos\theta - l\dot{\theta}^2\sin\theta) = N_L + N_R \tag{2.12}
$$

質量の $y$方向の力の平衡方程式：

$$
M\ddot{y} = Mg - (P_L + P_R) \tag{2.13}
$$

式(2.10)を式(2.13)に代入すると：

$$
Mg - M(l\dot{\theta}^2\cos\theta + l\ddot{\theta}\sin\theta) = P_L + P_R \tag{2.14}
$$

質量の$z$ 軸のモーメント平衡方程式：

$$
J_z\ddot{\theta} = T_P - T_N - (T_L + T_R) \tag{2.15}
$$

式(2.9), (2.12), (2.15)を式(2.15)に代入し、機体部分の動力学式を簡約化すると：

$$
(J_z + M l^2)\ddot{\theta} = Ml g \sin\theta - M \ddot{x}\cos\theta - (T_L + T_R) \tag{2.16}
$$

式(2.12)を式(2.8)に代入して、駆動力学方程式を得る：

$$
\left(M + 2m + \frac{2m l}{r}\right)\ddot{x} = (T_L + T_R) - M l \dot{\theta} \cos\theta + M l \ddot{\theta} \sin\theta \tag{2.17}
$$

機体が僅かに傾く場合、機体はその平衡位置付近の小角度範囲内で平衡を保ち、作動可能と仮定します。線形化処理は以下の通り：

小角度近似で：

$$
\cos\theta = 1, \quad \sin\theta = 0, \quad \dot{\theta}^2 = 0
$$

これを式(2.18)および式(2.17)に代入して、線形化された平面運動方程式を得ます：

$$
\left(r \left(M + 2m + \frac{2m l}{r}\right)\right) \ddot{x} = (T_L + T_R) - M r \ddot{\theta} \tag{2.18}
$$

$$
(J_z + M l^2) \ddot{\theta} = Mgl \theta - M \ddot{x} l - (T_L + T_R) \tag{2.19}
$$
#### 2.1.2 転向運動の動力学モデル
機体の転向は、左右の車輪の速度差によって実現される。簡化モデルは図4に示す通りであり、図中の物理パラメータは表2-1に記載されている。
![image](https://hackmd.io/_uploads/ryLFjEi-kx.png)
図４ステアリングモーションの簡易モデル

機体の$z$軸のモーメント平衡方程式は次の通りである。

$$
J_y \ddot{\delta} = (N_L - N_R) \frac{D}{2} \tag{2.20}
$$

機体の航向角加速度と左右車輪の加速度の関係は次の通りである。

$$
\ddot{\delta} = \frac{\dot{x}_L - \dot{x}_R}{D} \tag{2.21}
$$

式(2.20)および式(2.21)を式(2.8)に代入し、整理すると転向運動の動力学方程式は次のようになる。

$$
\ddot{\delta} = \frac{T_L - T_R}{r \left(m D + \frac{J_y D}{2 l_y} \right)} \tag{2.22}
$$

### 2.2 脚部運動

本課題では、車輪式ロボットやロボットの脚部運動を支持相と跳躍相の2つの特定の相に分けて考える。支持相では、下方にある運動体（車輪や脚部）が地面に接触している間に運動が行われる。この間、脚部の速度は一定に保たれ、脚部は着地点を安定して維持する。一方、跳躍相では、脚部が地面から離れ、ロボットは空中に跳躍し、その後再び地面に接触して次の運動が開始される。

#### 2.2.1 支持相の動力学モデル

支持相の間、脚部の運動はモータの角度変化に応じて行われ、モータによる制御が重要である。図5は、脚部運動の静力学モデルを示している。図中の各項目の物理パラメータは表2-1に記載されている。
![image](https://hackmd.io/_uploads/B1PAiNs-ye.png)
**図5 支持相の静的モデリング**
$y$方向における力の平衡方程式は次の通りである。

$$
F_y = P_L + P_R - M g \tag{2.23}
$$

機体の$x$軸のモーメント平衡方程式は次の通りである。

$$
T_x = (P_L - P_R) \frac{D}{2} \tag{2.24}
$$

$F_y$は機体が$y$方向における仮想力であり、これは脚部の高度調節運動を制御するためのものである。この仮想力が機体に$y$方向の加速度を与える。

$$
F_y = \ddot{y} M \tag{2.25}
$$

$T_x$は機体が$x$軸に対しての仮想モーメントであり、これは脚部が地面との適応運動を制御するためのものである。このモーメントが機体に$x$軸のロール角加速度を与える。

$$
T_x = J_x \ddot{\beta} \tag{2.26}
$$

式(2.23)および式(2.24)を連立して解くと次の通りである。

$$
\begin{cases} 
P_L = \frac{F_y - M g}{2} + \frac{T_x}{D} \\
P_R = \frac{F_y - M g}{2} - \frac{T_x}{D} 
\end{cases} \tag{2.27}
$$
![image](https://hackmd.io/_uploads/SkUz24sWyx.png)
**図6 リンク脚のメカニカル・モデリング**
主動力有する2つの関節モータトルク$\tau$と相互作用力$P_L$について、仮想理論方程式を列挙する。

$$
2 \tau \cdot \delta \alpha = P_L \cdot \delta y \tag{2.28}
$$

式(3.2)に示された機体高度$y$と関節モータの回転角$\alpha$の関係式を両辺微分して得ると次の通りである。

$$
\delta y = \left [ \frac{(l_1 - l_2 \cos \alpha) l_2 \sin \alpha}{l_3^2 - (l_1 - l_2 \cos \alpha)^2} + l_2 \cos \alpha \right ] \delta \alpha \tag{2.29}
$$

式(2.29)を式(2.28)に代入し、脚部末端に作用する相互作用力$P_L$と関節力モータのトルク$\tau$の関係式は次の通りである。

$$
\tau = \left [ \frac{(l_1 - l_2 \cos \alpha) l_2 \sin \alpha}{l_3^2 - (l_1 - l_2 \cos \alpha)^2} + l_2 \cos \alpha \right ] \frac{P_L}{2} \tag{2.30}
$$

#### 2.2.2 跳躍相の動力学モデル

輪脚式あるいはロボットの跳躍運動は図7に示される。図の状態①は、初期位置で関節モータの角度が最小角度位置にある状態を表す。その後、関節は急速に回転し、脚部は伸長し、仮想力$F_y$は機体に上向きの加速度を与え、機体は上昇し続ける。状態②は跳躍状態において、関節角度が最大角度位置に達し、機体は一定の跳躍速度を持っている。その後、関節が急速に反対方向に回転し、脚部が収縮する。状態③は最高位置における状態を示しており、関節の角度が最小角度に戻り、機体が最大の高さ$h_{\text{max}}$に達する。最大高さ$h_{\text{max}}$は次の通り決定される。

$$
h_{\text{max}} = h_1 + h_2
$$

ここで、$h_2 = \frac{1}{2} g t^2$であり、$h_2$は脚部の縮小時間$t$により決定される。
![image](https://hackmd.io/_uploads/SJ0BhEs-1x.png)
**図７　ロボットのジャンププロセス**
脚部の縮小時間$t$が起跳速度$v$を決定する。したがって、ロボットが跳躍できるかどうかは、伸脚段階の終了後に関節モータの角度が最大角度に達したとき、機体の速度$v$が$一定の値$を超えているかどうかに依存する。よって、伸脚段階のみを分析すればよい。この段階の動力学モデルは図8に示される。図中の各物理パラメータは表2-1を参照する。
![image](https://hackmd.io/_uploads/ryFOhVobyl.png)
**図8 伸脚の動的モデリング**
機体の運動エネルギーは次のようである：
$$E_k = \frac{1}{2} My^2$$

機体の位置エネルギーは次のようである：
$$V = Mgy$$

ラグランジュ方程式は次のようである：
$$L = E_k - V = \frac{1}{2} My^2 - Mgy$$

関節モータの回転角度$\alpha$を座標として選択し、広義のラグランジュ方程式で4つの関節モータのトルク$\tau$を列挙すると次のようになる：
$$\frac{d}{dt} \frac{\partial L}{\partial \dot{\alpha}} - \frac{\partial L}{\partial \alpha} = 4\tau$$

式(2.34)を展開すると、以下のように簡化される：
$$\frac{1}{4} M y_1 (y_2 \dot{a}^2 + y_1 \ddot{a} + g) = \tau$$

ここで、$y_1$, $y_2$の表現式は次の通りである：
$$y = \sqrt{l_3^2 - (l_1 - l_2 \cos{\alpha})^2 - l_2^2 \sin{\alpha}}$$
$$y_1 = \frac{\partial y}{\partial \alpha}, \quad y_2 = \frac{\partial^2 y}{\partial \alpha^2}$$

