国際宇宙ステーション（ISS）のCanadarm2は、全長17.6メートルのブームを2本連結した巨大なロボットアームです。このアームの質量は約1,800kgですが、地上で同じ可搬重量を実現するなら数十トン級の産業用ロボットが必要になるでしょう。宇宙では重力がほぼゼロなので軽量化が可能ですが、その代償として「アームがしなる」「動かすと振動する」という問題が避けられません。
地上のロボットでは、リンクは十分に剛いと仮定して制御を設計します。関節の角度を測れば手先の位置はキネマティクスで正確に計算できます。ところが宇宙マニピュレータでは、17メートルのブームが数十センチメートルもたわむことがあり、関節角度だけから手先位置を求めると大きな誤差が生じます。さらに、アームを動かすたびに弾性振動が励起され、振動が収まるまで精密な作業ができません。ドッキングやペイロードの受け渡しでは、振動による位置誤差がミッションの成否を左右します。
柔軟マニピュレータの力学と振動抑制を理解すると、以下のような応用が広がります。
- 軌道上サービス: 衛星の捕獲・修理で、長尺アームのたわみを補償して精密位置決めを行う
- 大型構造物の軌道上組立: 宇宙ステーションのモジュール結合や将来の宇宙望遠鏡組立に不可欠
- 惑星探査ローバー: 軽量化された探査アームの制御精度向上
本記事の内容
- なぜ宇宙マニピュレータで柔軟性が問題になるのか
- オイラー・ベルヌーイ梁による柔軟リンクのモデリング
- モード解析と固有振動数の導出
- 仮定モード法による無限次元系の有限次元離散化
- 剛体運動と弾性振動の連成方程式
- 端点位置推定の問題と補償
- 入力整形法（Input Shaping）による振動抑制制御
- Pythonによる柔軟リンクシミュレーションと振動の可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
- ラグランジュ法によるマニピュレータ動力学 — ラグランジュ方程式によるロボットの運動方程式導出
- 自由浮遊ロボットの動力学 — 宇宙空間での反力のないロボットの動力学
なぜ宇宙マニピュレータで柔軟性が問題になるのか
軽量化の必然性
地上のロボットアームを設計するとき、リンクは「剛体」として扱うのが一般的です。鋼鉄やアルミ合金の厚い断面を使い、外力で変形しないほど頑丈に作るからです。しかし宇宙では事情が大きく異なります。
宇宙にペイロードを打ち上げるコストは、1kgあたり数万ドルから数十万ドルに達します。たとえばファルコン9で低軌道への打上げコストが約1kgあたり2,700ドル、スペースシャトル時代には約54,000ドルでした。このコスト構造の下では、ロボットアームの1kgの軽量化が数万ドルの節約に直結します。そのため宇宙マニピュレータは極限まで軽量化されます。
Canadarm2の場合、各ブームは長さ約7メートル、直径35センチメートルの炭素繊維強化プラスチック（CFRP）パイプです。壁厚はわずか数ミリメートルで、手先に116,000kgの質量を搬送できる能力がありながら、ブーム自体は非常に軽量です。この設計は微小重力下でのみ成立するもので、地上では自重だけで大きくたわんでしまいます。
柔軟性の影響
宇宙マニピュレータにおける柔軟性の影響は、大きく3つに分類できます。
1. 静的たわみ: ペイロードを把持した状態でアームを伸ばすと、リンクが自身の慣性力やペイロードの質量によってたわみます。微小重力下では重力によるたわみはほぼゼロですが、加速・減速時の慣性力でたわみが発生します。Canadarm2では手先の位置誤差が数十センチメートルに達することもあります。
2. 弾性振動: アームを動かす（特に急加速・急減速する）と、リンクの弾性変形が振動として励起されます。この振動は宇宙空間では空気抵抗による減衰がないため、非常にゆっくりとしか収まりません。構造減衰比は通常0.01以下で、振動が数十秒から数分間持続します。
3. 剛体運動と弾性振動の連成: 柔軟リンクでは、関節の回転運動（剛体モード）とリンクのたわみ（弾性モード）が互いに影響し合います。関節を動かすとリンクがたわみ、たわんだリンクの復元力が関節にトルクを及ぼします。この連成は制御設計を大幅に複雑にします。
剛体仮定の破綻
剛体マニピュレータでは、関節角度ベクトル $\bm{q} = (q_1, q_2, \dots, q_n)^T$ を測定すれば、順運動学によって手先位置 $\bm{p}$ を正確に計算できます。
$$ \bm{p} = f(\bm{q}) $$
ところが柔軟リンクでは、リンクのたわみ $w(x,t)$ が加わるため、実際の手先位置は関節角度だけでは決まりません。
$$ \bm{p}_{\text{actual}} = f(\bm{q}) + \Delta \bm{p}(\bm{q}, w) $$
ここで $\Delta \bm{p}$ はたわみによる位置偏差です。たわみ $w$ は時間的に変動する（振動する）ため、手先位置も振動します。この問題に対処するためには、まず柔軟リンクの力学を正しくモデリングする必要があります。
次のセクションでは、柔軟リンクをモデリングするための古典的な理論であるオイラー・ベルヌーイ梁のモデルを導入します。
オイラー・ベルヌーイ梁による柔軟リンクのモデリング
梁理論の選択
柔軟なリンクを力学的にモデリングする最も基本的なアプローチは、リンクを連続体の梁として扱うことです。梁のモデルにはいくつかの選択肢があります。
- オイラー・ベルヌーイ梁: 断面が変形後も平面を保ち、中立軸に直交する（せん断変形を無視）
- ティモシェンコ梁: せん断変形と回転慣性を考慮する
- レイリー梁: 回転慣性のみを考慮する
宇宙マニピュレータのブームは「細長い梁」（アスペクト比が大きい）なので、せん断変形の影響は小さく、オイラー・ベルヌーイ梁の仮定が十分に良い近似を与えます。Canadarm2のブームのように長さ/直径比が20以上の場合、この仮定は妥当です。
オイラー・ベルヌーイ梁の運動方程式
水平に配置された一様断面の梁を考えます。梁の左端を原点とし、梁に沿った方向を $x$ 軸、たわみの方向を $w$ とします。オイラー・ベルヌーイ梁の基本仮定は「曲げモーメントと曲率が比例する」ということです。釣り竿を曲げるとき、強く曲げるほど大きな復元力が生じます。この関係を数式にすると、曲げモーメント $M$ は曲率（=たわみの2階空間微分）に比例します。
$$ M(x,t) = EI \frac{\partial^2 w}{\partial x^2} $$
ここで $E$ はヤング率（材料の硬さ）、$I$ は断面二次モーメント（断面形状による曲げにくさ）、$EI$ は曲げ剛性と呼ばれます。
この曲げモーメントと梁の微小要素に対する力の釣り合い・モーメントの釣り合いを組み合わせると、横振動の運動方程式が導かれます。導出の流れを見ていきましょう。
梁の位置 $x$ における微小要素（長さ $dx$）に作用する力を考えます。この微小要素には、せん断力 $V(x,t)$ と曲げモーメント $M(x,t)$ が両端に作用し、分布外力 $f(x,t)$（単位長さあたりの力）が作用します。
まず、微小要素の $w$ 方向の力の釣り合いから、
$$ \rho A \frac{\partial^2 w}{\partial t^2} dx = V(x,t) – V(x+dx,t) + f(x,t) dx $$
ここで $\rho$ は密度、$A$ は断面積です。$V(x+dx,t) \approx V(x,t) + \frac{\partial V}{\partial x}dx$ とテイラー展開すると、
$$ \rho A \frac{\partial^2 w}{\partial t^2} = -\frac{\partial V}{\partial x} + f(x,t) $$
次に、微小要素のモーメントの釣り合いから（回転慣性を無視するオイラー・ベルヌーイの仮定のもとで）、
$$ V = \frac{\partial M}{\partial x} $$
という関係が得られます。曲げモーメントの式 $M = EI \frac{\partial^2 w}{\partial x^2}$ を代入すると、
$$ V = \frac{\partial}{\partial x}\left(EI \frac{\partial^2 w}{\partial x^2}\right) $$
これを力の釣り合い式に代入します。一様断面（$EI$ が $x$ によらない）の場合、
$$ \begin{equation} \rho A \frac{\partial^2 w}{\partial t^2} + EI \frac{\partial^4 w}{\partial x^4} = f(x,t) \end{equation} $$
これがオイラー・ベルヌーイ梁の運動方程式です。左辺第1項が慣性項（質量 $\times$ 加速度）、第2項が弾性復元力（曲げ剛性による力）を表します。外力がない自由振動の場合は $f(x,t) = 0$ となり、
$$ \begin{equation} \rho A \frac{\partial^2 w}{\partial t^2} + EI \frac{\partial^4 w}{\partial x^4} = 0 \end{equation} $$
この偏微分方程式（PDE）は、4階の空間微分と2階の時間微分を含んでおり、解くためには4つの境界条件と2つの初期条件が必要です。
境界条件
宇宙マニピュレータの1つのリンクを考えるとき、境界条件の選び方がモデルの性質を決定します。代表的な境界条件の組み合わせは以下の通りです。
片持ち梁（Clamped-Free）: 関節側が固定端、手先側が自由端。マニピュレータのリンクを最も簡単にモデル化する場合に使います。
固定端（$x = 0$）では、たわみとたわみ角がゼロです。
$$ w(0,t) = 0, \quad \frac{\partial w}{\partial x}(0,t) = 0 $$
自由端（$x = L$）では、曲げモーメントとせん断力がゼロです。
$$ EI\frac{\partial^2 w}{\partial x^2}(L,t) = 0, \quad EI\frac{\partial^3 w}{\partial x^3}(L,t) = 0 $$
マニピュレータのリンクでは、先端にペイロードやエンドエフェクタが付いている場合があり、その場合は自由端の条件が修正されます（集中質量の境界条件）。しかし、まずは基本的な片持ち梁で理論を構築し、後から一般化するのが理解への近道です。
ここまでで、柔軟リンクの運動方程式と境界条件が定式化されました。次は、この偏微分方程式を解いて、リンクがどのような振動パターン（モード）を持つのかを調べましょう。
モード解析と固有振動数
変数分離法による解法
オイラー・ベルヌーイ梁の自由振動方程式を解くために、変数分離法を使います。たわみ $w(x,t)$ を空間的な形状と時間的な振動に分離できると仮定します。
$$ w(x,t) = \phi(x) \cdot T(t) $$
直感的には、「梁は特定の形状 $\phi(x)$ を保ったまま時間的に振動する $T(t)$」と考えるということです。ギターの弦が特定の形状（腹と節）を保ったまま振動するのと同じイメージです。
これを自由振動方程式に代入します。
$$ \rho A \, \phi(x) \, \ddot{T}(t) + EI \, \phi””(x) \, T(t) = 0 $$
$\phi(x) \cdot T(t)$ で割って変数を分離すると、
$$ \frac{\ddot{T}(t)}{T(t)} = -\frac{EI}{\rho A} \cdot \frac{\phi””(x)}{\phi(x)} $$
左辺は $t$ のみの関数、右辺は $x$ のみの関数です。両辺が等しいためには、どちらも定数でなければなりません。この定数を $-\omega^2$ とおきます（$\omega$ は角振動数に対応します）。
すると、時間方向の方程式は単純な調和振動になります。
$$ \ddot{T}(t) + \omega^2 T(t) = 0 $$
この解は $T(t) = A\cos(\omega t) + B\sin(\omega t)$ です。
空間方向の方程式は、
$$ \phi””(x) – \beta^4 \phi(x) = 0 $$
ここで $\beta^4 = \frac{\rho A \omega^2}{EI}$ とおきました。$\beta$ は波数に対応するパラメータです。
空間モード方程式の一般解
空間方程式 $\phi”” – \beta^4 \phi = 0$ の特性方程式は $s^4 = \beta^4$ なので、$s = \pm\beta, \pm i\beta$ の4つの根を持ちます。したがって一般解は、
$$ \phi(x) = C_1 \cosh(\beta x) + C_2 \sinh(\beta x) + C_3 \cos(\beta x) + C_4 \sin(\beta x) $$
4つの定数 $C_1, C_2, C_3, C_4$ は境界条件で決まります。
片持ち梁の固有振動数
片持ち梁の境界条件を適用して固有振動数を求めましょう。
固定端 $x = 0$ の条件 $\phi(0) = 0$ と $\phi'(0) = 0$ を代入すると、
$\phi(0) = 0$ より $C_1 + C_3 = 0$、すなわち $C_3 = -C_1$ が得られます。
$\phi'(0) = 0$ より $\beta(C_2 + C_4) = 0$、すなわち $C_4 = -C_2$ が得られます。
これらを使って一般解を書き直すと、
$$ \phi(x) = C_1[\cosh(\beta x) – \cos(\beta x)] + C_2[\sinh(\beta x) – \sin(\beta x)] $$
次に、自由端 $x = L$ の条件を適用します。$\phi”(L) = 0$ と $\phi”'(L) = 0$ を代入すると、$C_1$ と $C_2$ に関する連立方程式が得られます。この連立方程式が非自明な解（$C_1 = C_2 = 0$ でない解）を持つためには、係数行列の行列式がゼロでなければなりません。
この条件を計算すると（途中の行列式の展開は省略しますが）、以下の振動数方程式（特性方程式）が得られます。
$$ \begin{equation} \cos(\beta L) \cdot \cosh(\beta L) + 1 = 0 \end{equation} $$
この超越方程式は解析的には解けませんが、数値的に解くことができます。$\beta_n L$ の最初のいくつかの解は、
| モード $n$ | $\beta_n L$ | 近似値 |
|---|---|---|
| 1 | 1.8751 | $\approx 1.875$ |
| 2 | 4.6941 | $\approx 4.694$ |
| 3 | 7.8548 | $\approx 7.855$ |
| $n \geq 4$ | $\approx (2n-1)\pi/2$ |
$n$ が大きくなると $\beta_n L \approx (2n-1)\pi/2$ に近づきます。
$\beta^4 = \frac{\rho A \omega^2}{EI}$ の関係から、第 $n$ モードの固有振動数は、
$$ \begin{equation} \omega_n = (\beta_n L)^2 \sqrt{\frac{EI}{\rho A L^4}} \end{equation} $$
ここで重要なのは、固有振動数が $(\beta_n L)^2$ に比例するという点です。第2モードの振動数は第1モードの $(4.694/1.875)^2 \approx 6.27$ 倍、第3モードは $(7.855/1.875)^2 \approx 17.5$ 倍にもなります。つまり、高次モードの振動数は急速に増大します。これは後述する「低次モードだけを考慮すればよい」という仮定モード法の根拠になります。
モード形状
各固有振動数 $\omega_n$ に対応するモード形状 $\phi_n(x)$ は、以下のように表されます。
$$ \phi_n(x) = \cosh(\beta_n x) – \cos(\beta_n x) – \sigma_n [\sinh(\beta_n x) – \sin(\beta_n x)] $$
ここで $\sigma_n$ は、
$$ \sigma_n = \frac{\cosh(\beta_n L) + \cos(\beta_n L)}{\sinh(\beta_n L) + \sin(\beta_n L)} $$
モード形状は、梁が $n$ 番目の固有振動数で振動するときの変形パターンを表します。第1モードは梁全体が一方向にたわむ形、第2モードは1つの節を持つS字型、第3モードは2つの節を持つ形、というように高次モードほど複雑な形状になります。
これらのモード形状は直交性を持ちます。すなわち、
$$ \int_0^L \rho A \, \phi_m(x) \phi_n(x) \, dx = 0 \quad (m \neq n) $$
この直交性は、連続体の偏微分方程式を離散的な常微分方程式に変換するための鍵となります。
ここまでで、柔軟リンクの固有振動数とモード形状がわかりました。しかし、制御設計には連続体のモデルのままでは扱いにくいため、有限個のモードに離散化する必要があります。次のセクションでは、その離散化手法である仮定モード法を紹介します。
仮定モード法（Assumed Modes Method）による離散化
なぜ離散化が必要か
オイラー・ベルヌーイ梁の運動方程式は偏微分方程式であり、無限次元の系です。これは無限個の固有振動数とモード形状を持つことを意味します。しかし制御工学では、状態空間モデルのような有限次元の常微分方程式として扱いたい場面がほとんどです。
仮定モード法は、たわみ $w(x,t)$ を有限個のモード形状の重ね合わせで近似する手法です。アイデアは非常にシンプルです。フーリエ級数で関数を三角関数の重ね合わせで近似するのと同じように、梁のたわみをモード形状の重ね合わせで近似します。
$$ \begin{equation} w(x,t) \approx \sum_{i=1}^{N} \phi_i(x) \, q_i^e(t) \end{equation} $$
ここで $\phi_i(x)$ は第 $i$ モードの形状関数（既知）、$q_i^e(t)$ は第 $i$ モードの弾性一般化座標（未知の時間関数）、$N$ はモードの打ち切り数です。
物理的には、$q_i^e(t)$ は「第 $i$ モードがどの程度励起されているか」を表す振幅のような量です。梁の変形は、各モード形状にその時刻での振幅を掛けて足し合わせたものとして表現されます。
離散化の手順
仮定モード法では、ラグランジュ法と組み合わせて運動方程式を導出します。手順は以下の通りです。
ステップ1: 運動エネルギーの表現
梁の運動エネルギーは、
$$ \mathcal{T}_{\text{elastic}} = \frac{1}{2} \int_0^L \rho A \left(\frac{\partial w}{\partial t}\right)^2 dx $$
仮定モード展開 $w(x,t) = \sum_{i=1}^N \phi_i(x) q_i^e(t)$ を代入すると、
$$ \frac{\partial w}{\partial t} = \sum_{i=1}^N \phi_i(x) \dot{q}_i^e(t) $$
よって、
$$ \mathcal{T}_{\text{elastic}} = \frac{1}{2} \sum_{i=1}^N \sum_{j=1}^N \left[\int_0^L \rho A \, \phi_i(x) \phi_j(x) \, dx\right] \dot{q}_i^e \dot{q}_j^e $$
ここで括弧内の積分を $M_{ij}^e$ とおくと、行列形式で、
$$ \mathcal{T}_{\text{elastic}} = \frac{1}{2} \dot{\bm{q}}_e^T \bm{M}_e \, \dot{\bm{q}}_e $$
$\bm{M}_e$ は $N \times N$ の弾性質量行列です。モード形状の直交性を使えば、$\bm{M}_e$ は対角行列になります。
ステップ2: ポテンシャルエネルギーの表現
梁の弾性ポテンシャルエネルギー（ひずみエネルギー）は、
$$ \mathcal{V}_{\text{elastic}} = \frac{1}{2} \int_0^L EI \left(\frac{\partial^2 w}{\partial x^2}\right)^2 dx $$
同様に仮定モード展開を代入すると、
$$ \mathcal{V}_{\text{elastic}} = \frac{1}{2} \sum_{i=1}^N \sum_{j=1}^N \left[\int_0^L EI \, \phi_i”(x) \phi_j”(x) \, dx\right] q_i^e q_j^e $$
括弧内を $K_{ij}^e$ とおくと、
$$ \mathcal{V}_{\text{elastic}} = \frac{1}{2} \bm{q}_e^T \bm{K}_e \, \bm{q}_e $$
$\bm{K}_e$ は $N \times N$ の弾性剛性行列です。モード形状が梁の固有モードであれば、$\bm{K}_e$ も対角行列になり、対角要素は $K_{ii}^e = \omega_i^2 M_{ii}^e$ を満たします。
ステップ3: ラグランジュ方程式の適用
ラグランジアン $\mathcal{L} = \mathcal{T}_{\text{elastic}} – \mathcal{V}_{\text{elastic}}$ にラグランジュ方程式を適用すると、
$$ \bm{M}_e \, \ddot{\bm{q}}_e + \bm{K}_e \, \bm{q}_e = \bm{f}_e $$
ここで $\bm{f}_e$ は弾性座標に対応する一般化力です。モード形状の直交性により質量行列と剛性行列が対角化されている場合、各モードは独立な1自由度の振動方程式になります。
$$ M_{ii}^e \, \ddot{q}_i^e + K_{ii}^e \, q_i^e = f_i^e \quad (i = 1, 2, \dots, N) $$
構造減衰を考慮する場合は、減衰項を追加します。
$$ M_{ii}^e \, \ddot{q}_i^e + D_{ii}^e \, \dot{q}_i^e + K_{ii}^e \, q_i^e = f_i^e $$
ここで $D_{ii}^e = 2\zeta_i \omega_i M_{ii}^e$ とするのが一般的です。$\zeta_i$ は第 $i$ モードの減衰比で、宇宙構造物では典型的に $\zeta_i = 0.005 \sim 0.02$ 程度の非常に小さい値です。
モード打ち切りの妥当性
実用上、何モードまで考慮すればよいのでしょうか。先ほど見たように、固有振動数は $\omega_n \propto (\beta_n L)^2$ で急速に増大します。制御帯域は通常、第1モード固有振動数の数倍程度までなので、最初の2〜3モードを考慮すれば十分な場合がほとんどです。
Canadarm2の場合、第1モードの固有振動数は約0.04Hz（周期約25秒）です。関節駆動系の帯域はこれよりも低く、制御設計では主に第1モードと第2モードが問題になります。第3モード以降は振動数が十分高く、制御帯域外に位置するため、モデルから省略しても制御性能への影響は限定的です。
ここまでで、連続体の柔軟リンクを有限次元の弾性座標系で表現できるようになりました。しかし実際のマニピュレータでは、リンクのたわみだけでなく関節の回転運動も同時に起きます。次のセクションでは、剛体運動と弾性振動を統一的に扱う連成方程式を導出します。
剛体運動と弾性振動の連成方程式
問題の設定
ここでは最も基本的なモデルとして、1自由度回転関節 + 1本の柔軟リンクの系を考えます。関節はモータで駆動され、リンクは関節から水平に伸びる片持ち梁です。この系の一般化座標は以下の2種類から構成されます。
- $\theta(t)$: 関節角度（剛体モード）
- $q_1^e(t), q_2^e(t), \dots, q_N^e(t)$: 弾性一般化座標（弾性モード）
つまり、系の一般化座標ベクトルは、
$$ \bm{q} = [\theta, \, q_1^e, \, q_2^e, \, \dots, \, q_N^e]^T $$
です。以下では $N = 2$（2モード）の場合を示します。
運動エネルギーの導出
リンク上の位置 $x$ にある微小要素の絶対位置を考えます。関節が角度 $\theta$ だけ回転し、さらにたわみ $w(x,t)$ が加わると、微小要素の絶対速度は剛体運動とたわみの両方の寄与を含みます。
関節を原点とする回転座標系で、リンク上の点の位置は $(x, w(x,t))$ です。この点の絶対速度の2乗は（微小たわみの仮定のもとで）、
$$ v^2(x,t) \approx \left[(x + w)\dot{\theta}\right]^2 + \left[\dot{w} + x\dot{\theta}\right]^2 – 2(x + w)\dot{\theta}\dot{w}\sin(\alpha) + \cdots $$
ここでは微小たわみを仮定して $w \ll x$ とし、2次以上の微小量を無視した近似を用います。厳密な導出は煩雑ですが、結果として運動エネルギーは以下の形にまとめられます。
$$ \mathcal{T} = \frac{1}{2} J_{\text{eff}} \dot{\theta}^2 + \frac{1}{2} \dot{\bm{q}}_e^T \bm{M}_e \dot{\bm{q}}_e + \dot{\theta} \, \bm{h}^T \dot{\bm{q}}_e $$
第1項は関節回転の運動エネルギー（$J_{\text{eff}}$ は等価慣性モーメント）、第2項は弾性振動の運動エネルギー、第3項が剛体と弾性の連成項です。連成ベクトル $\bm{h}$ の各成分は、
$$ h_i = \int_0^L \rho A \, x \, \phi_i(x) \, dx $$
この積分は、モード形状 $\phi_i(x)$ と梁に沿った位置 $x$ の積を質量密度で重み付けしたもので、剛体回転が弾性モードにどの程度結合するかを表します。
ポテンシャルエネルギー
微小重力下ではポテンシャルエネルギーは弾性ひずみエネルギーのみです。
$$ \mathcal{V} = \frac{1}{2} \bm{q}_e^T \bm{K}_e \bm{q}_e $$
重力項がないことが宇宙環境の特徴です。地上のロボットでは重力ポテンシャルも考慮する必要がありますが、宇宙ではこの項がゼロになります。
ラグランジュ方程式の適用
ラグランジアン $\mathcal{L} = \mathcal{T} – \mathcal{V}$ に対してラグランジュ方程式を適用します。一般化座標 $\theta$ に関する方程式と弾性座標 $\bm{q}_e$ に関する方程式を行列形式でまとめると、
$$ \begin{equation} \begin{bmatrix} J_{\text{eff}} & \bm{h}^T \\ \bm{h} & \bm{M}_e \end{bmatrix} \begin{bmatrix} \ddot{\theta} \\ \ddot{\bm{q}}_e \end{bmatrix} + \begin{bmatrix} 0 & \bm{0}^T \\ \bm{0} & \bm{D}_e \end{bmatrix} \begin{bmatrix} \dot{\theta} \\ \dot{\bm{q}}_e \end{bmatrix} + \begin{bmatrix} 0 & \bm{0}^T \\ \bm{0} & \bm{K}_e \end{bmatrix} \begin{bmatrix} \theta \\ \bm{q}_e \end{bmatrix} = \begin{bmatrix} \tau \\ \bm{0} \end{bmatrix} \end{equation} $$
ここで $\tau$ は関節トルク、$\bm{D}_e$ は弾性モードの減衰行列です。
この方程式の構造に注目してください。質量行列の非対角ブロック $\bm{h}$ が剛体と弾性の慣性連成を表しています。これは「関節を加速すると弾性モードが励起される」「弾性振動が関節に反力トルクを及ぼす」という双方向の影響を意味します。
もし $\bm{h} = \bm{0}$ であれば（連成がなければ）、関節の回転運動と弾性振動は完全に独立になり、剛体ロボットと同じように制御できます。しかし実際には $\bm{h} \neq \bm{0}$ であり、この連成こそが柔軟マニピュレータの制御を困難にする本質的な原因です。
数値的なオーダー感
Canadarm2のようなシステムで具体的な数値を考えてみましょう。
| パラメータ | 値 |
|---|---|
| リンク長 $L$ | 7 m |
| 線密度 $\rho A$ | 10 kg/m |
| 曲げ剛性 $EI$ | $5 \times 10^4$ N$\cdot$m$^2$ |
| 第1モード固有振動数 | $\approx 0.28$ Hz |
| 減衰比 $\zeta$ | 0.005 |
| 振動の減衰時間（$1/e$） | $\approx 113$ 秒 |
振動の減衰時間が2分近くになることに注目してください。宇宙空間では空気抵抗がなく、構造減衰だけに頼るため、振動が非常にゆっくりとしか減衰しません。これが「振動抑制制御」を必要とする理由です。
次のセクションでは、柔軟リンクのたわみが手先位置推定にどのような影響を与えるかを定量的に見ていきます。
端点位置推定の問題
関節角度だけでは手先がわからない
剛体マニピュレータでは、関節角度 $\theta$ を測定すれば、手先位置は幾何学的に一意に決まります。
$$ \bm{p}_{\text{rigid}} = \begin{bmatrix} L\cos\theta \\ L\sin\theta \end{bmatrix} $$
しかし柔軟リンクでは、端点のたわみ $w(L,t)$ とたわみ角 $\frac{\partial w}{\partial x}(L,t)$ の分だけ手先位置がずれます。仮定モード展開を使うと、端点のたわみは、
$$ w(L,t) = \sum_{i=1}^N \phi_i(L) \, q_i^e(t) $$
端点のたわみ角は、
$$ \frac{\partial w}{\partial x}(L,t) = \sum_{i=1}^N \phi_i'(L) \, q_i^e(t) $$
これらを考慮すると、実際の手先位置は、
$$ \bm{p}_{\text{actual}} = \begin{bmatrix} L\cos\theta – w(L,t)\sin\theta \\ L\sin\theta + w(L,t)\cos\theta \end{bmatrix} $$
より正確には、端点のたわみ角も考慮して、有効リンク長と有効角度の両方が修正されます。
位置誤差の大きさ
位置誤差 $\Delta \bm{p} = \bm{p}_{\text{actual}} – \bm{p}_{\text{rigid}}$ の大きさは、主に $w(L,t)$ に支配されます。片持ち梁の端点に集中力 $F$ が作用する場合の静的たわみは、
$$ w_{\text{static}}(L) = \frac{FL^3}{3EI} $$
これを簡単に数値評価してみましょう。$L = 7$ m、$EI = 5 \times 10^4$ N$\cdot$m$^2$、$F = 10$ N（軌道上での制御力の典型値）とすると、
$$ w_{\text{static}}(L) = \frac{10 \times 7^3}{3 \times 5 \times 10^4} = \frac{3430}{150000} \approx 0.023 \text{ m} = 2.3 \text{ cm} $$
静的たわみだけで2.3cmです。動的な加減速を含むと、振動の振幅はこの数倍になることがあります。ドッキング精度が数センチメートル以内を要求されるミッションでは、このたわみは無視できない大きさです。
推定手法
端点位置のたわみを補償するアプローチは2つあります。
センサベースのアプローチ: リンク上にひずみゲージ、加速度計、あるいは光ファイバーセンサを配置し、たわみを直接測定します。Canadarm2には複数のひずみゲージが装備されており、リンクのたわみをリアルタイムで推定しています。しかし、センサの数には制限があるため、完全な変形場を直接測定することはできません。
モデルベースのアプローチ: 仮定モード法で離散化されたモデルを使い、関節トルクの測定値からオブザーバ（状態推定器）で弾性座標 $\bm{q}_e(t)$ を推定します。推定された弾性座標から $w(L,t) = \sum \phi_i(L) q_i^e(t)$ を計算して端点位置を補正します。
実際のシステムでは両方を組み合わせたハイブリッドアプローチが用いられます。センサで低次モードを直接測定し、モデルベースの推定で補完するのです。
端点位置を正しく推定できたとしても、振動が残っている限り精密な作業は困難です。そこで次のセクションでは、振動そのものを抑制する制御手法 — 入力整形法について解説します。
振動抑制制御 — 入力整形法（Input Shaping）
振動を「起こさない」アプローチ
柔軟マニピュレータの振動を抑制する手法には、大きく2つの哲学があります。
- フィードバック制御: 振動を検知して、それを打ち消すトルクを関節にフィードバックする
- フィードフォワード制御: 振動を「そもそも起こさない」ように、入力コマンドを事前に整形する
入力整形法（Input Shaping）は後者のアプローチです。Singer & Seering（1990）によって体系化されたこの手法は、実装が簡単で、ロバスト性が高く、追加のセンサを必要としないという優れた特長を持ちます。
入力整形法の原理
入力整形法の基本アイデアは、1つのインパルス入力を複数のインパルスに分割し、先に発生した振動を後のインパルスで打ち消すというものです。
最も簡単なZV（Zero Vibration）整形器を考えましょう。1自由度の減衰振動系（固有振動数 $\omega_n$、減衰比 $\zeta$）に単位インパルスを加えると、以下の振動応答が生じます。
$$ w(t) = \frac{1}{\omega_d} e^{-\zeta \omega_n t} \sin(\omega_d t) $$
ここで $\omega_d = \omega_n\sqrt{1-\zeta^2}$ は減衰固有振動数です。
ここで、2つのインパルスを適切なタイミングと振幅で入力することを考えます。第1インパルス（時刻 $t_1 = 0$、振幅 $A_1$）による振動と、第2インパルス（時刻 $t_2$、振幅 $A_2$）による振動が、$t > t_2$ でちょうど打ち消し合うようにします。
2つの振動が完全に打ち消し合うための条件を導きましょう。時刻 $t > t_2$ における合成振動のcos成分とsin成分がそれぞれゼロになればよいので、
cos成分のキャンセル条件:
$$ A_1 e^{-\zeta \omega_n t} \cos(\omega_d t) + A_2 e^{-\zeta \omega_n (t-t_2)} \cos[\omega_d (t-t_2)] = 0 $$
sin成分のキャンセル条件:
$$ A_1 e^{-\zeta \omega_n t} \sin(\omega_d t) + A_2 e^{-\zeta \omega_n (t-t_2)} \sin[\omega_d (t-t_2)] = 0 $$
これらが全ての $t > t_2$ で成り立つためには、第2インパルスのタイミングを減衰固有振動の半周期に設定します。
$$ t_2 = \frac{\pi}{\omega_d} $$
このタイミングにすると、第1インパルスの振動がちょうど逆位相になったところで第2インパルスが入り、残留振動がゼロになります。
ZV整形器のパラメータは以下のようにまとめられます。
$$ K = e^{-\zeta \pi / \sqrt{1-\zeta^2}} $$
$$ A_1 = \frac{1}{1+K}, \quad A_2 = \frac{K}{1+K} $$
$$ t_1 = 0, \quad t_2 = \frac{\pi}{\omega_d} $$
$A_1 + A_2 = 1$ であることに注意してください。整形器はコマンドの最終値を変えません（定常ゲインが1）。
ZVD整形器
ZV整形器は $\omega_n$ と $\zeta$ が正確にわかっている場合に振動をゼロにしますが、実際にはモデルの不確かさがあります。よりロバストな整形器として、ZVD（Zero Vibration and Derivative）整形器が使われます。
ZVD整形器は3つのインパルスを使い、振動のキャンセル条件だけでなく、振動数に関する感度もゼロにします。つまり、$\omega_n$ が少しずれていても振動抑制効果が大きく劣化しません。
$$ A_1 = \frac{1}{1 + 2K + K^2}, \quad A_2 = \frac{2K}{1 + 2K + K^2}, \quad A_3 = \frac{K^2}{1 + 2K + K^2} $$
$$ t_1 = 0, \quad t_2 = \frac{\pi}{\omega_d}, \quad t_3 = \frac{2\pi}{\omega_d} $$
ZVD整形器は振動の1全周期分の時間遅れが生じますが、その代わりに固有振動数の10〜20%程度のモデル誤差に対してもロバストです。
入力整形の実装方法
入力整形の実装は非常にシンプルです。元の入力コマンド $u(t)$ に整形器のインパルス列を畳み込むだけです。
$$ u_{\text{shaped}}(t) = \sum_{k=1}^{M} A_k \, u(t – t_k) $$
ここで $M$ は整形器のインパルス数（ZVなら2、ZVDなら3）です。元のコマンドのコピーを時間をずらして重み付けして足し合わせるだけなので、リアルタイム実装のコストは極めて低いです。
ただし、入力整形法にはトレードオフがあります。
- 利点: 実装が簡単、追加センサ不要、フィードバック制御と組み合わせ可能
- 制約: 応答時間が整形器の長さだけ遅れる（ZVで半周期、ZVDで1周期）
- 前提: 振動系のモデル（$\omega_n, \zeta$）がある程度わかっている必要がある
宇宙マニピュレータでは、事前のシステム同定により固有振動数が精度よく分かっているため、入力整形法は非常に有効です。Canadarm2でも、移動コマンドの整形による振動抑制が実際に行われています。
ここまでで、柔軟マニピュレータの力学モデルから振動抑制制御の原理まで、理論的な枠組みを見てきました。次のセクションでは、これらの理論をPythonで実装し、柔軟リンクの振動と入力整形の効果を具体的にシミュレーションして可視化します。
Pythonによるシミュレーション
固有振動数とモード形状の計算
まず、片持ち梁の固有振動数を求めるために、振動数方程式 $\cos(\beta L)\cosh(\beta L) + 1 = 0$ を数値的に解き、モード形状を描画します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import brentq
# --- 梁のパラメータ（Canadarm2のブームを想定） ---
L = 7.0 # リンク長 [m]
rho_A = 10.0 # 線密度 [kg/m]
EI = 5e4 # 曲げ剛性 [N·m^2]
# --- 振動数方程式: cos(beta*L)*cosh(beta*L) + 1 = 0 ---
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
# 数値的に最初の5つの根を求める
n_modes = 5
betaL_vals = []
search_intervals = [(1.0, 2.5), (4.0, 5.5), (7.0, 8.5), (10.0, 11.5), (13.0, 15.0)]
for (a, b) in search_intervals:
root = brentq(freq_eq, a, b)
betaL_vals.append(root)
betaL_vals = np.array(betaL_vals)
beta_vals = betaL_vals / L
# 固有振動数 [rad/s] と [Hz]
omega_n = betaL_vals**2 * np.sqrt(EI / (rho_A * L**4))
freq_hz = omega_n / (2 * np.pi)
print("=" * 50)
print(f"{'モード':>6} {'βnL':>10} {'ωn [rad/s]':>14} {'fn [Hz]':>10}")
print("=" * 50)
for i in range(n_modes):
print(f"{i+1:>6d} {betaL_vals[i]:>10.4f} {omega_n[i]:>14.4f} {freq_hz[i]:>10.4f}")
このコードを実行すると、5つのモードの固有振動数が得られます。第1モードは約0.28Hz（周期3.6秒）で、高次モードになるほど急速に振動数が上がることが確認できます。宇宙マニピュレータの関節駆動は通常0.1Hz以下の帯域で行われるため、第1モードの振動数が制御帯域に近く、振動問題の主要因であることがわかります。
次に、モード形状を描画します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.optimize import brentq
# 梁パラメータ
L = 7.0
rho_A = 10.0
EI = 5e4
# 振動数方程式の根
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
betaL_vals = []
search_intervals = [(1.0, 2.5), (4.0, 5.5), (7.0, 8.5), (10.0, 11.5), (13.0, 15.0)]
for (a, b) in search_intervals:
betaL_vals.append(brentq(freq_eq, a, b))
betaL_vals = np.array(betaL_vals)
# モード形状関数
def mode_shape(x, beta_n, L):
bL = beta_n * L
sigma = (np.cosh(bL) + np.cos(bL)) / (np.sinh(bL) + np.sin(bL))
bx = beta_n * x
phi = (np.cosh(bx) - np.cos(bx)) - sigma * (np.sinh(bx) - np.sin(bx))
return phi
x = np.linspace(0, L, 500)
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
for i, ax in enumerate(axes):
beta_n = betaL_vals[i] / L
phi = mode_shape(x, beta_n, L)
# 正規化（端点で最大値が1になるように）
phi = phi / np.max(np.abs(phi))
ax.plot(x, phi, linewidth=2, color=f'C{i}')
ax.axhline(0, color='gray', linewidth=0.5, linestyle='--')
omega_i = betaL_vals[i]**2 * np.sqrt(EI / (rho_A * L**4))
freq_i = omega_i / (2 * np.pi)
ax.set_ylabel(f'$\\phi_{i+1}(x)$', fontsize=12)
ax.set_title(f'Mode {i+1}: $f_{i+1}$ = {freq_i:.3f} Hz', fontsize=12)
ax.grid(True, alpha=0.3)
axes[-1].set_xlabel('Position along beam $x$ [m]', fontsize=12)
plt.tight_layout()
plt.savefig('mode_shapes.png', dpi=150, bbox_inches='tight')
plt.show()
描画されたモード形状から、以下の特徴が読み取れます。
- 第1モードは梁全体が一方向にたわむ滑らかな形状で、自由端（$x = L$）で最大変位を持ちます。これが「ブームがしなる」ときの最も基本的な変形パターンです。
- 第2モードはリンクの途中に1つの節（変位がゼロの点）を持つS字型の形状です。振動周波数は第1モードの約6.3倍です。
- 第3モードは2つの節を持ち、さらに複雑な変形パターンになります。振動周波数は第1モードの約17.5倍に達します。
モード次数が上がるほど空間的に細かい変形パターンになりますが、同時に振動数も急速に上がります。制御帯域内で問題になるのは主に第1モードと第2モードです。
剛体-弾性連成系のシミュレーション
次に、1関節+1柔軟リンクの連成系を時間シミュレーションします。関節にステップ状のトルクを加えたときに、弾性振動がどのように励起されるかを観察します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.optimize import brentq
# --- パラメータ ---
L = 7.0
rho_A = 10.0
EI = 5e4
zeta = 0.005 # 減衰比
J_hub = 50.0 # 関節のハブ慣性モーメント [kg·m^2]
N_modes = 2 # 考慮するモード数
# --- 固有振動数・モード形状の計算 ---
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
betaL_list = []
intervals = [(1.0, 2.5), (4.0, 5.5)]
for (a, b) in intervals:
betaL_list.append(brentq(freq_eq, a, b))
betaL_arr = np.array(betaL_list)
omega_modes = betaL_arr**2 * np.sqrt(EI / (rho_A * L**4))
def mode_shape(x, beta_n):
bL = beta_n * L
sigma = (np.cosh(bL) + np.cos(bL)) / (np.sinh(bL) + np.sin(bL))
bx = beta_n * x
return (np.cosh(bx) - np.cos(bx)) - sigma * (np.sinh(bx) - np.sin(bx))
# --- 質量行列・剛性行列の数値計算 ---
from scipy.integrate import quad
beta_arr = betaL_arr / L
# 弾性質量行列の対角成分（直交モードなので対角）
M_e = np.zeros(N_modes)
for i in range(N_modes):
integrand = lambda x: rho_A * mode_shape(x, beta_arr[i])**2
M_e[i], _ = quad(integrand, 0, L)
# 弾性剛性行列の対角成分
K_e = np.array([omega_modes[i]**2 * M_e[i] for i in range(N_modes)])
# 減衰行列の対角成分
D_e = np.array([2 * zeta * omega_modes[i] * M_e[i] for i in range(N_modes)])
# 連成ベクトル h_i = ∫ rho_A * x * phi_i(x) dx
h_vec = np.zeros(N_modes)
for i in range(N_modes):
integrand = lambda x: rho_A * x * mode_shape(x, beta_arr[i])
h_vec[i], _ = quad(integrand, 0, L)
# リンクの全慣性モーメント
J_link, _ = quad(lambda x: rho_A * x**2, 0, L)
J_eff = J_hub + J_link
print(f"等価慣性モーメント J_eff = {J_eff:.1f} kg·m^2")
print(f"連成ベクトル h = {h_vec}")
print(f"弾性質量 M_e = {M_e}")
print(f"固有振動数 = {omega_modes / (2*np.pi)} Hz")
ここで計算された連成ベクトル $\bm{h}$ の大きさに注目してください。第1モードの連成成分 $h_1$ は比較的大きな値を持ち、関節トルクが第1モードを強く励起することを示しています。第2モードの連成成分 $h_2$ はそれより小さく、高次モードほど関節回転との結合が弱くなる傾向があります。
続いて、連成方程式の時間積分を行います。
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp, quad
from scipy.optimize import brentq
# --- パラメータ（再定義） ---
L = 7.0
rho_A = 10.0
EI = 5e4
zeta = 0.005
J_hub = 50.0
N_modes = 2
# 固有値計算
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
betaL_arr = np.array([brentq(freq_eq, a, b) for a, b in [(1.0, 2.5), (4.0, 5.5)]])
beta_arr = betaL_arr / L
omega_modes = betaL_arr**2 * np.sqrt(EI / (rho_A * L**4))
def mode_shape(x, beta_n):
bL = beta_n * L
sigma = (np.cosh(bL) + np.cos(bL)) / (np.sinh(bL) + np.sin(bL))
bx = beta_n * x
return (np.cosh(bx) - np.cos(bx)) - sigma * (np.sinh(bx) - np.sin(bx))
# 行列要素の計算
M_e = np.array([quad(lambda x, i=i: rho_A * mode_shape(x, beta_arr[i])**2, 0, L)[0]
for i in range(N_modes)])
K_e = omega_modes**2 * M_e
D_e = 2 * zeta * omega_modes * M_e
h_vec = np.array([quad(lambda x, i=i: rho_A * x * mode_shape(x, beta_arr[i]), 0, L)[0]
for i in range(N_modes)])
J_link = quad(lambda x: rho_A * x**2, 0, L)[0]
J_eff = J_hub + J_link
# --- 状態方程式 ---
# 状態: [theta, q1_e, q2_e, theta_dot, q1_e_dot, q2_e_dot]
def dynamics(t, state, torque_func):
n = 1 + N_modes # 座標数 = 3
q = state[:n]
qd = state[n:]
# 質量行列の組立
M = np.zeros((n, n))
M[0, 0] = J_eff
for i in range(N_modes):
M[0, 1+i] = h_vec[i]
M[1+i, 0] = h_vec[i]
M[1+i, 1+i] = M_e[i]
# 減衰・剛性
C = np.zeros(n)
Kq = np.zeros(n)
for i in range(N_modes):
C[1+i] = D_e[i] * qd[1+i]
Kq[1+i] = K_e[i] * q[1+i]
# 外力
F = np.zeros(n)
F[0] = torque_func(t)
# 加速度: M * qdd = F - C - Kq
qdd = np.linalg.solve(M, F - C - Kq)
return np.concatenate([qd, qdd])
# --- ステップトルク入力 ---
tau_max = 20.0 # [N·m]
def step_torque(t):
if t < 5.0:
return tau_max
elif t < 10.0:
return -tau_max
else:
return 0.0
# 初期条件（静止）
y0 = np.zeros(2 * (1 + N_modes))
t_span = (0, 40)
t_eval = np.linspace(0, 40, 4000)
sol = solve_ivp(dynamics, t_span, y0, t_eval=t_eval,
args=(step_torque,), method='RK45', rtol=1e-10, atol=1e-12)
# --- 端点位置の計算 ---
phi_at_tip = np.array([mode_shape(L, beta_arr[i]) for i in range(N_modes)])
# 端点たわみ
w_tip = sol.y[1,:] * phi_at_tip[0] + sol.y[2,:] * phi_at_tip[1]
# 手先位置（2D）
theta = sol.y[0, :]
px_rigid = L * np.cos(theta)
py_rigid = L * np.sin(theta)
px_actual = L * np.cos(theta) - w_tip * np.sin(theta)
py_actual = L * np.sin(theta) + w_tip * np.cos(theta)
# --- 可視化 ---
fig, axes = plt.subplots(4, 1, figsize=(12, 12), sharex=True)
# (a) 関節角度
axes[0].plot(sol.t, np.degrees(theta), linewidth=1.5)
axes[0].set_ylabel('Joint angle [deg]', fontsize=11)
axes[0].set_title('Rigid-Flexible Coupled System Response (Step Torque)', fontsize=13)
axes[0].grid(True, alpha=0.3)
# (b) 弾性座標
axes[1].plot(sol.t, sol.y[1,:], label='$q_1^e$ (Mode 1)', linewidth=1.5)
axes[1].plot(sol.t, sol.y[2,:], label='$q_2^e$ (Mode 2)', linewidth=1.5)
axes[1].set_ylabel('Elastic coordinates', fontsize=11)
axes[1].legend(fontsize=10)
axes[1].grid(True, alpha=0.3)
# (c) 端点たわみ
axes[2].plot(sol.t, w_tip * 100, linewidth=1.5, color='C2')
axes[2].set_ylabel('Tip deflection [cm]', fontsize=11)
axes[2].grid(True, alpha=0.3)
# (d) 入力トルク
torque_vals = np.array([step_torque(t) for t in sol.t])
axes[3].plot(sol.t, torque_vals, linewidth=1.5, color='C3')
axes[3].set_ylabel('Torque [N·m]', fontsize=11)
axes[3].set_xlabel('Time [s]', fontsize=11)
axes[3].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('coupled_response.png', dpi=150, bbox_inches='tight')
plt.show()
このシミュレーション結果から、以下の重要な特徴が読み取れます。
- トルク印加時の弾性振動の励起: $t = 0$ でステップトルクが加わった瞬間、弾性座標が振動し始めます。第1モード（$q_1^e$）の振幅が大きく、第2モード（$q_2^e$）は相対的に小さいことがわかります。これは連成ベクトル $h_1 > h_2$ から予測される通りです。
- トルク停止後の残留振動: $t = 10$ 秒でトルクがゼロになった後も、弾性振動が長時間持続しています。減衰比 $\zeta = 0.005$ では、振動の包絡線が半分になるまで数十秒かかります。
- 端点たわみの大きさ: 先端のたわみは数センチメートルのオーダーに達しており、精密な位置決めには無視できない量です。
- 剛体運動への影響: 関節角度 $\theta$ の応答にも弾性振動の影響が見られ、純粋な剛体応答からの微小な偏差が確認できます。
入力整形法の効果を比較
最後に、同じ関節角度の変更を行うときに、入力整形なし（ステップトルク）と入力整形あり（ZVD整形器を適用）で残留振動がどう変わるかを比較します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp, quad
from scipy.optimize import brentq
# --- パラメータ ---
L = 7.0
rho_A = 10.0
EI = 5e4
zeta = 0.005
J_hub = 50.0
N_modes = 2
# 固有値計算
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
betaL_arr = np.array([brentq(freq_eq, a, b) for a, b in [(1.0, 2.5), (4.0, 5.5)]])
beta_arr = betaL_arr / L
omega_modes = betaL_arr**2 * np.sqrt(EI / (rho_A * L**4))
def mode_shape(x, beta_n):
bL = beta_n * L
sigma = (np.cosh(bL) + np.cos(bL)) / (np.sinh(bL) + np.sin(bL))
bx = beta_n * x
return (np.cosh(bx) - np.cos(bx)) - sigma * (np.sinh(bx) - np.sin(bx))
# 行列要素
M_e = np.array([quad(lambda x, i=i: rho_A * mode_shape(x, beta_arr[i])**2, 0, L)[0]
for i in range(N_modes)])
K_e = omega_modes**2 * M_e
D_e = 2 * zeta * omega_modes * M_e
h_vec = np.array([quad(lambda x, i=i: rho_A * x * mode_shape(x, beta_arr[i]), 0, L)[0]
for i in range(N_modes)])
J_link = quad(lambda x: rho_A * x**2, 0, L)[0]
J_eff = J_hub + J_link
phi_at_tip = np.array([mode_shape(L, beta_arr[i]) for i in range(N_modes)])
# --- 状態方程式 ---
def dynamics(t, state, torque_func):
n = 1 + N_modes
q = state[:n]
qd = state[n:]
M = np.zeros((n, n))
M[0, 0] = J_eff
for i in range(N_modes):
M[0, 1+i] = h_vec[i]
M[1+i, 0] = h_vec[i]
M[1+i, 1+i] = M_e[i]
C = np.zeros(n)
Kq = np.zeros(n)
for i in range(N_modes):
C[1+i] = D_e[i] * qd[1+i]
Kq[1+i] = K_e[i] * q[1+i]
F = np.zeros(n)
F[0] = torque_func(t)
qdd = np.linalg.solve(M, F - C - Kq)
return np.concatenate([qd, qdd])
# --- ZVD整形器の設計（第1モード向け） ---
omega1 = omega_modes[0]
omega_d1 = omega1 * np.sqrt(1 - zeta**2)
K_zvd = np.exp(-zeta * np.pi / np.sqrt(1 - zeta**2))
denom = 1 + 2 * K_zvd + K_zvd**2
A_zvd = np.array([1.0 / denom, 2 * K_zvd / denom, K_zvd**2 / denom])
t_zvd = np.array([0.0, np.pi / omega_d1, 2 * np.pi / omega_d1])
print(f"第1モード固有振動数: {omega1/(2*np.pi):.4f} Hz")
print(f"ZVD整形器:")
print(f" 振幅: A = {A_zvd}")
print(f" 時刻: t = {t_zvd} s")
print(f" 整形器の全長: {t_zvd[-1]:.3f} s")
# --- 入力コマンド ---
tau_max = 20.0
t_accel = 3.0 # 加速時間
# 整形なしのバンバン入力
def bang_bang(t):
if t < t_accel:
return tau_max
elif t < 2 * t_accel:
return -tau_max
else:
return 0.0
# ZVD整形されたバンバン入力
def bang_bang_zvd(t):
val = 0.0
for k in range(3):
val += A_zvd[k] * bang_bang(t - t_zvd[k])
return val
# --- シミュレーション ---
t_end = 30.0
t_eval = np.linspace(0, t_end, 3000)
y0 = np.zeros(2 * (1 + N_modes))
sol_raw = solve_ivp(dynamics, (0, t_end), y0, t_eval=t_eval,
args=(bang_bang,), method='RK45', rtol=1e-10, atol=1e-12)
sol_zvd = solve_ivp(dynamics, (0, t_end), y0, t_eval=t_eval,
args=(bang_bang_zvd,), method='RK45', rtol=1e-10, atol=1e-12)
# 端点たわみ
w_raw = sol_raw.y[1,:] * phi_at_tip[0] + sol_raw.y[2,:] * phi_at_tip[1]
w_zvd = sol_zvd.y[1,:] * phi_at_tip[0] + sol_zvd.y[2,:] * phi_at_tip[1]
# --- 可視化 ---
fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)
# (a) 入力トルクの比較
torque_raw = np.array([bang_bang(t) for t in t_eval])
torque_zvd = np.array([bang_bang_zvd(t) for t in t_eval])
axes[0].plot(t_eval, torque_raw, label='Bang-bang (unshaped)', linewidth=1.5, alpha=0.7)
axes[0].plot(t_eval, torque_zvd, label='ZVD-shaped', linewidth=1.5)
axes[0].set_ylabel('Torque [N·m]', fontsize=11)
axes[0].set_title('Input Shaping: Vibration Suppression Comparison', fontsize=13)
axes[0].legend(fontsize=10)
axes[0].grid(True, alpha=0.3)
# (b) 関節角度
axes[1].plot(t_eval, np.degrees(sol_raw.y[0,:]), label='Unshaped', linewidth=1.5, alpha=0.7)
axes[1].plot(t_eval, np.degrees(sol_zvd.y[0,:]), label='ZVD-shaped', linewidth=1.5)
axes[1].set_ylabel('Joint angle [deg]', fontsize=11)
axes[1].legend(fontsize=10)
axes[1].grid(True, alpha=0.3)
# (c) 端点たわみ
axes[2].plot(t_eval, w_raw * 100, label='Unshaped', linewidth=1.5, alpha=0.7)
axes[2].plot(t_eval, w_zvd * 100, label='ZVD-shaped', linewidth=1.5)
axes[2].set_ylabel('Tip deflection [cm]', fontsize=11)
axes[2].set_xlabel('Time [s]', fontsize=11)
axes[2].legend(fontsize=10)
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('input_shaping_comparison.png', dpi=150, bbox_inches='tight')
plt.show()
# 残留振動の定量比較
# t = 8秒以降（動作完了後）のたわみのRMS
t_settle = 8.0
mask = t_eval > t_settle
rms_raw = np.sqrt(np.mean(w_raw[mask]**2)) * 100
rms_zvd = np.sqrt(np.mean(w_zvd[mask]**2)) * 100
reduction = (1 - rms_zvd / rms_raw) * 100
print(f"\n残留振動のRMS（動作完了後）:")
print(f" 整形なし: {rms_raw:.4f} cm")
print(f" ZVD整形: {rms_zvd:.4f} cm")
print(f" 振動抑制率: {reduction:.1f}%")
このシミュレーション結果は、入力整形法の効果を鮮明に示しています。
- トルク波形の違い: ZVD整形後のトルクは、元のバンバン入力に比べて立ち上がりと立ち下がりが「なまされた」形状になっています。これは3つのインパルスの畳み込みによるもので、入力の急激な変化が緩和されています。
- 関節角度: 両者とも最終的には同じ角度に到達しますが、ZVD整形の方がやや遅れて収束します。この時間遅れがZVD整形器の長さ（第1モードの振動1周期分）に相当します。
- 端点たわみ: 整形なしの場合、動作完了後に大きな残留振動が数十秒間持続しているのに対し、ZVD整形を適用した場合は残留振動がほぼゼロに抑制されています。振動のRMS値で比較すると、90%以上の抑制効果が得られます。
- トレードオフ: 振動抑制と引き換えに、応答時間が整形器の長さだけ遅れます。しかし、整形なしの場合は振動が収まるまで作業を開始できないため、実効的な作業開始時刻ではZVD整形の方がむしろ早いことが多いのです。
柔軟リンクの振動アニメーション
最後に、柔軟リンクの変形をアニメーション的に可視化するために、複数の時刻でのリンク形状をスナップショットとして描画します。
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp, quad
from scipy.optimize import brentq
# --- パラメータ ---
L = 7.0
rho_A = 10.0
EI = 5e4
zeta = 0.005
J_hub = 50.0
N_modes = 2
# 固有値計算
def freq_eq(betaL):
return np.cos(betaL) * np.cosh(betaL) + 1.0
betaL_arr = np.array([brentq(freq_eq, a, b) for a, b in [(1.0, 2.5), (4.0, 5.5)]])
beta_arr = betaL_arr / L
omega_modes = betaL_arr**2 * np.sqrt(EI / (rho_A * L**4))
def mode_shape(x, beta_n):
bL = beta_n * L
sigma = (np.cosh(bL) + np.cos(bL)) / (np.sinh(bL) + np.sin(bL))
bx = beta_n * x
return (np.cosh(bx) - np.cos(bx)) - sigma * (np.sinh(bx) - np.sin(bx))
# 行列要素
M_e = np.array([quad(lambda x, i=i: rho_A * mode_shape(x, beta_arr[i])**2, 0, L)[0]
for i in range(N_modes)])
K_e = omega_modes**2 * M_e
D_e = 2 * zeta * omega_modes * M_e
h_vec = np.array([quad(lambda x, i=i: rho_A * x * mode_shape(x, beta_arr[i]), 0, L)[0]
for i in range(N_modes)])
J_link = quad(lambda x: rho_A * x**2, 0, L)[0]
J_eff = J_hub + J_link
phi_at_x = lambda x_val: np.array([mode_shape(x_val, beta_arr[i]) for i in range(N_modes)])
# 状態方程式
def dynamics(t, state, torque_func):
n = 1 + N_modes
q = state[:n]
qd = state[n:]
M = np.zeros((n, n))
M[0, 0] = J_eff
for i in range(N_modes):
M[0, 1+i] = h_vec[i]
M[1+i, 0] = h_vec[i]
M[1+i, 1+i] = M_e[i]
C = np.zeros(n)
Kq = np.zeros(n)
for i in range(N_modes):
C[1+i] = D_e[i] * qd[1+i]
Kq[1+i] = K_e[i] * q[1+i]
F = np.zeros(n)
F[0] = torque_func(t)
qdd = np.linalg.solve(M, F - C - Kq)
return np.concatenate([qd, qdd])
# パルストルク
tau_max = 20.0
def pulse_torque(t):
if t < 2.0:
return tau_max
elif t < 4.0:
return -tau_max
else:
return 0.0
y0 = np.zeros(2 * (1 + N_modes))
t_end = 20.0
t_eval = np.linspace(0, t_end, 2000)
sol = solve_ivp(dynamics, (0, t_end), y0, t_eval=t_eval,
args=(pulse_torque,), method='RK45', rtol=1e-10, atol=1e-12)
# --- スナップショット描画 ---
x_beam = np.linspace(0, L, 200)
snapshot_times = [0, 1, 2, 3, 4, 5, 7, 10, 15]
fig, ax = plt.subplots(figsize=(10, 10))
# たわみの拡大係数（可視化のため）
scale = 50.0
colors = plt.cm.viridis(np.linspace(0, 1, len(snapshot_times)))
for idx, t_snap in enumerate(snapshot_times):
# t_snapに最も近い時刻のインデックス
i_t = np.argmin(np.abs(sol.t - t_snap))
theta_val = sol.y[0, i_t]
q_e = sol.y[1:1+N_modes, i_t]
# 梁に沿った各点でのたわみ
w_beam = np.zeros_like(x_beam)
for m in range(N_modes):
phi_vals = np.array([mode_shape(xi, beta_arr[m]) for xi in x_beam])
w_beam += q_e[m] * phi_vals
# 拡大して可視化
w_beam_scaled = w_beam * scale
# 回転座標系から慣性座標系への変換
cos_t = np.cos(theta_val)
sin_t = np.sin(theta_val)
px = x_beam * cos_t - w_beam_scaled * sin_t
py = x_beam * sin_t + w_beam_scaled * cos_t
ax.plot(px, py, color=colors[idx], linewidth=2, label=f't = {t_snap:.0f} s')
# 端点にマーカー
ax.plot(px[-1], py[-1], 'o', color=colors[idx], markersize=6)
ax.plot(0, 0, 'ks', markersize=10, label='Joint')
ax.set_xlabel('X [m]', fontsize=12)
ax.set_ylabel('Y [m]', fontsize=12)
ax.set_title(f'Flexible Link Snapshots (deflection magnified {scale:.0f}x)', fontsize=13)
ax.set_aspect('equal')
ax.legend(fontsize=9, loc='lower left')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.savefig('flexible_link_snapshots.png', dpi=150, bbox_inches='tight')
plt.show()
このスナップショット図から、柔軟リンクの運動の本質が視覚的に理解できます。
- 回転と変形の同時進行: トルク印加中（$t = 0 \sim 4$ 秒）は、リンクが回転しながら同時に曲がっている（たわんでいる）ことが見て取れます。リンクの先端は剛体の場合の位置から明らかにずれています。
- 残留振動: トルク停止後（$t > 4$ 秒）も、リンクは揺れ続けています。$t = 5, 7, 10, 15$ 秒のスナップショットを比較すると、たわみの振幅が非常にゆっくりとしか減少していないことがわかります。
- 端点軌跡の分散: 端点マーカーの位置を見ると、剛体の場合は関節角度だけで一意に決まるはずの位置が、弾性振動により周期的にブレていることが確認できます。これが精密位置決めの障害になります。
まとめ
本記事では、宇宙マニピュレータの柔軟性問題とその補償手法について、理論的基礎からシミュレーションまで一貫して解説しました。
- 柔軟性の原因と影響: 宇宙マニピュレータは打上げコスト削減のために極限まで軽量化されるため、リンクが柔軟になる。これにより静的たわみ、弾性振動、端点位置誤差の問題が生じる
- オイラー・ベルヌーイ梁モデル: 柔軟リンクの横振動を記述する偏微分方程式 $\rho A \ddot{w} + EI w”” = f$ を導出し、片持ち梁の境界条件で固有振動数とモード形状を求めた
- 仮定モード法: たわみを有限個のモード形状の重ね合わせで近似し、偏微分方程式を常微分方程式系に変換した。モードの直交性により質量行列と剛性行列が対角化される
- 剛体-弾性連成方程式: 関節回転と弾性振動が質量行列の非対角成分を通じて連成することを示し、これが制御を困難にする本質的な原因であることを議論した
- 端点位置推定: 関節角度だけでは手先位置が決まらず、弾性座標の推定が必要であることを定量的に示した
- 入力整形法: ZV / ZVD整形器の原理を導出し、振動を「起こさない」フィードフォワード手法の有効性をシミュレーションで確認した。残留振動のRMSを90%以上抑制できることを示した
宇宙マニピュレータの柔軟性は、軽量化の代償として必然的に生じる問題ですが、適切なモデリングと制御手法を用いれば対処可能です。入力整形法のようなフィードフォワード手法と、状態推定に基づくフィードバック制御を組み合わせることで、Canadarm2のような長尺アームでもセンチメートル級の精密位置決めが実現されています。
次のステップとして、以下の記事も参考にしてください。
- ラグランジュ法によるマニピュレータ動力学 — 剛体マニピュレータの動力学をラグランジュ法で導出する基礎
- 自由浮遊ロボットの動力学 — 固定基台のない宇宙ロボットの運動方程式