国際宇宙ステーション（ISS）のロボットアーム「カナダアーム2」が、接近してくる補給船を掴みにいく場面を想像してみてください。アームの手先はドッキングポートに向かって真っ直ぐ近づく必要があります。もし手先が関節の都合で曲がりくねった経路を通ると、補給船に衝突したり、ソーラーパネルを巻き込んだりする危険があります。また、衛星の外壁に沿って検査カメラを移動させるときは、壁面に沿った円弧状の経路が求められることもあります。
このように、宇宙ロボットの手先を「空間上の決められた経路」に沿って動かしたい場面は数多くあります。これが作業空間での経路計画（Task-space path planning）です。
作業空間の経路計画を理解すると、以下の応用が広がります。
- 軌道上サービス: 故障衛星への真っ直ぐなアプローチ経路の設計
- デブリ捕獲: 回転するデブリの把持点に合わせた姿勢補間
- 産業ロボット: 溶接・塗装で手先を直線・円弧に沿って動かす
- 手術ロボット: メスを正確な直線経路で移動させる
本記事の内容
- 関節空間計画と作業空間計画の本質的な違い
- デカルト空間での直線補間（位置のLERP）
- クォータニオンSLERPによる姿勢の滑らかな補間
- 円弧補間の幾何学的定式化
- 逆運動学との組み合わせ方
- 特異点通過問題とその対策
- Pythonでの直線・円弧・SLERP補間の実装と3D可視化
前提知識
この記事を読む前に、以下の記事を読んでおくと理解が深まります。
関節空間計画 vs 作業空間計画 — 2つのアプローチ
なぜ2つのアプローチがあるのか
ロボットアームの動きを計画する方法は、大きく2つに分かれます。「どの空間で経路を設計するか」が異なります。
関節空間計画（Joint-space planning）は、各関節の角度 $\bm{q} = (q_1, q_2, \ldots, q_n)^T$ を直接補間する方法です。スタートの関節角度 $\bm{q}_0$ からゴールの関節角度 $\bm{q}_f$ まで、たとえば3次多項式や5次多項式で滑らかに変化させます。この方法は計算が軽く、関節速度や加速度の制御が容易です。しかし、手先が空間上でどんな経路を通るかは保証されません。スタートとゴールの間で、手先は予期しないカーブを描くかもしれません。
作業空間計画（Task-space planning）は、手先の位置・姿勢をデカルト座標系（$x, y, z$ + 回転）で直接補間する方法です。手先が直線上を動く、円弧に沿って動く、といった幾何学的な経路を正確に指定できます。代わりに、各時刻で逆運動学を解いて関節角度に変換する必要があるため、計算コストが高くなります。
使い分けの判断基準
どちらを使うかは、「手先の経路形状が重要かどうか」で判断します。
| 条件 | 適するアプローチ |
|---|---|
| 手先の経路形状を厳密に制御したい | 作業空間計画 |
| 障害物を手先で避けたい | 作業空間計画 |
| 速度・関節負荷を優先したい | 関節空間計画 |
| 逆運動学の特異点が問題にならない | どちらでも可 |
宇宙ロボットの場合、ドッキングやデブリ捕獲では手先の経路精度が最優先なので、作業空間計画が不可欠です。
作業空間計画の基本フレームワーク
作業空間での経路計画は、次の3ステップで実行されます。
ステップ1: 経路生成 — 作業空間上で手先の位置 $\bm{p}(s)$ と姿勢 $\bm{R}(s)$ をパラメータ $s \in [0, 1]$ の関数として定義します。
ステップ2: 時間スケジューリング — パラメータ $s$ を時間 $t$ の関数 $s(t)$ として設定し、速度プロファイル（台形加減速など）を与えます。
ステップ3: 逆運動学 — 各時刻 $t_k$ における手先の位置・姿勢 $(\bm{p}(s(t_k)),\, \bm{R}(s(t_k)))$ から、逆運動学を解いて関節角度 $\bm{q}(t_k)$ を求めます。
このフレームワークの中核は「ステップ1: 経路生成」です。ここで直線補間や円弧補間が使われます。まず、最も基本的な直線補間から見ていきましょう。
デカルト空間での直線補間（LERP）
直線補間の直感
直線補間（Linear Interpolation, LERP）は、2点間を結ぶ最も素朴な経路です。日常で言えば、棚の上のコップを取るとき、手を「最短距離で真っ直ぐ伸ばす」動作に相当します。
宇宙ロボットがデブリに接近するとき、最終フェーズでは手先をデブリの把持点に向かって直線的に近づけます。曲がった経路では衝突リスクが増すため、「真っ直ぐアプローチ」が安全設計の基本です。
位置の直線補間
スタート位置 $\bm{p}_0 = (x_0, y_0, z_0)^T$ からゴール位置 $\bm{p}_f = (x_f, y_f, z_f)^T$ への直線補間は、パラメータ $s \in [0, 1]$ を用いて次のように表されます。
$$ \begin{equation} \bm{p}(s) = (1 – s)\,\bm{p}_0 + s\,\bm{p}_f \end{equation} $$
この式は「スタートからゴールへ、割合 $s$ だけ進んだ点」を表しています。$s = 0$ でスタート位置、$s = 1$ でゴール位置、$s = 0.5$ で中点です。
成分ごとに書くと次のようになります。
$$ x(s) = (1 – s)\,x_0 + s\,x_f $$
$$ y(s) = (1 – s)\,y_0 + s\,y_f $$
$$ z(s) = (1 – s)\,z_0 + s\,z_f $$
各成分が独立に線形に変化するため、手先は3次元空間上で厳密に直線を描きます。
速度の一様性
パラメータ $s$ が時間 $t$ に対して一定の割合で進む場合（$s = t / T$、$T$ は総移動時間）、手先の速度は次のように計算されます。
$$ \dot{\bm{p}} = \frac{d\bm{p}}{dt} = \frac{\bm{p}_f – \bm{p}_0}{T} $$
速度の大きさは一定で、$|\dot{\bm{p}}| = \|\bm{p}_f – \bm{p}_0\| / T$ となります。これは手先が等速で直線上を動くことを意味します。
ただし実際のロボットでは、いきなり等速で動き始めたり急停止したりすると関節に大きなトルクがかかります。そこで、$s(t)$ に台形加速度プロファイルやS字カーブを適用して、加速・定速・減速のフェーズを設けるのが一般的です。
台形速度プロファイル
$s(t)$ に滑らかさを持たせる最も簡単な方法は、台形速度プロファイルです。総移動時間 $T$ のうち、加速区間 $t_a$、定速区間、減速区間 $t_d$ に分けます。
加速区間（$0 \leq t \leq t_a$）では
$$ s(t) = \frac{1}{2}\frac{t^2}{t_a \cdot T} $$
ここでは時間の2乗に比例するため、速度は $t$ に比例して滑らかに増加します。
定速区間（$t_a \leq t \leq T – t_d$）では、パラメータ $s$ は時間に比例して一定速度で増加します。
減速区間（$T – t_d \leq t \leq T$）では、加速区間の対称形で速度が減少します。
位置の直線補間は幾何学的に単純ですが、ロボットの手先には位置だけでなく姿勢もあります。ドッキングポートに手先を差し込むには、位置だけでなく手先の向き（姿勢）も正確に制御しなければなりません。次に、姿勢をどのように補間するかを見ていきましょう。
クォータニオンによる姿勢補間 — SLERP
なぜ姿勢補間に工夫が必要なのか
3次元空間での回転（姿勢）の表現方法はいくつかあります。最も直感的なのはオイラー角（ロール・ピッチ・ヨー）ですが、オイラー角にはジンバルロックという致命的な問題があります。特定の姿勢で自由度が1つ失われ、補間が破綻します。
回転行列 $\bm{R} \in SO(3)$ を直接補間する方法もありますが、2つの回転行列を単純に線形補間すると、結果が回転行列の条件（直交性・行列式1）を満たさなくなります。
$$ (1-s)\bm{R}_0 + s\bm{R}_f \notin SO(3) \quad (\text{一般に}) $$
そこで登場するのがクォータニオン（四元数）とSLERP（Spherical Linear Interpolation, 球面線形補間）です。
クォータニオンの基礎
クォータニオンは4つの実数で3次元回転を表す数学的構造です。単位クォータニオン $\bm{q}$ は次のように書けます。
$$ \begin{equation} \bm{q} = q_w + q_x \mathbf{i} + q_y \mathbf{j} + q_z \mathbf{k} = (q_w, q_x, q_y, q_z) \end{equation} $$
ここで $q_w$ はスカラー部、$(q_x, q_y, q_z)$ はベクトル部です。単位クォータニオンは $\|\bm{q}\| = \sqrt{q_w^2 + q_x^2 + q_y^2 + q_z^2} = 1$ を満たします。
回転軸 $\hat{\bm{n}} = (n_x, n_y, n_z)$（単位ベクトル）まわりに角度 $\theta$ だけ回転する操作は、次のクォータニオンで表されます。
$$ \begin{equation} \bm{q} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2}\,(n_x \mathbf{i} + n_y \mathbf{j} + n_z \mathbf{k}) \end{equation} $$
角度の半分 $\theta/2$ が現れる理由は、クォータニオンによる回転操作が $\bm{v}’ = \bm{q}\,\bm{v}\,\bm{q}^{*}$ という「両側からの積」で定義されるためです（$\bm{q}^{*}$ は共役クォータニオン）。
クォータニオンが姿勢補間に適している理由は、単位クォータニオンが4次元単位球面 $S^3$ 上の点であり、球面上での補間が数学的に自然に定義できるからです。
SLERPの定式化
2つの姿勢をクォータニオン $\bm{q}_0$、$\bm{q}_f$ で表したとき、SLERP（球面線形補間）は次の式で定義されます。
$$ \begin{equation} \text{SLERP}(\bm{q}_0, \bm{q}_f; s) = \frac{\sin((1-s)\Omega)}{\sin\Omega}\,\bm{q}_0 + \frac{\sin(s\,\Omega)}{\sin\Omega}\,\bm{q}_f \end{equation} $$
ここで $\Omega$ は2つのクォータニオン間の角度で、内積から求められます。
$$ \begin{equation} \cos\Omega = \bm{q}_0 \cdot \bm{q}_f = q_{0w}q_{fw} + q_{0x}q_{fx} + q_{0y}q_{fy} + q_{0z}q_{fz} \end{equation} $$
この式の意味を直感的に理解しましょう。SLERPは4次元球面上で2点を結ぶ大円（最短経路）に沿った補間です。2次元で考えると、円周上の2点を弧に沿って等速で動く操作に相当します。通常の線形補間（LERP）が直線上を等速で動くのに対し、SLERPは球面上を等速で動きます。
SLERPの導出
SLERPの式がなぜ上のような形になるのか、2次元の場合から導出してみましょう。
2次元の単位円上に2つの点 $\bm{a}$、$\bm{b}$ があり、その間の角度が $\Omega$ であるとします。円弧上のパラメータ $s$ の点 $\bm{c}(s)$ を求めたいです。
$\bm{c}(s)$ を $\bm{a}$ と $\bm{b}$ の線形結合 $\bm{c}(s) = \alpha(s)\,\bm{a} + \beta(s)\,\bm{b}$ と仮定します。$\bm{c}(s)$ が単位円上にあること、そして $\bm{a}$ からの角度が $s\Omega$ であることから、次の2つの条件が導かれます。
$\bm{a}$ との内積をとると
$$ \bm{a} \cdot \bm{c}(s) = \alpha(s) + \beta(s)\cos\Omega = \cos(s\Omega) $$
$\bm{b}$ との内積をとると
$$ \bm{b} \cdot \bm{c}(s) = \alpha(s)\cos\Omega + \beta(s) = \cos((1-s)\Omega) $$
この連立方程式を $\alpha(s)$, $\beta(s)$ について解きます。まず第1式から $\alpha(s) = \cos(s\Omega) – \beta(s)\cos\Omega$ を求め、第2式に代入します。
$$ (\cos(s\Omega) – \beta(s)\cos\Omega)\cos\Omega + \beta(s) = \cos((1-s)\Omega) $$
$\beta(s)$ について整理すると
$$ \beta(s)(1 – \cos^2\Omega) = \cos((1-s)\Omega) – \cos(s\Omega)\cos\Omega $$
右辺を加法定理 $\cos((1-s)\Omega) = \cos(s\Omega)\cos\Omega + \sin(s\Omega)\sin\Omega$ で展開すると
$$ \beta(s)\sin^2\Omega = \sin(s\Omega)\sin\Omega $$
したがって $\beta(s) = \sin(s\Omega)/\sin\Omega$ が得られます。同様に $\alpha(s) = \sin((1-s)\Omega)/\sin\Omega$ が導かれます。
以上により、球面線形補間の公式が得られました。
$$ \bm{c}(s) = \frac{\sin((1-s)\Omega)}{\sin\Omega}\,\bm{a} + \frac{\sin(s\Omega)}{\sin\Omega}\,\bm{b} $$
この2次元の結果は、そのまま4次元の単位クォータニオン球面にも適用できます。
SLERPの実装上の注意点
SLERPを実装する際には、いくつかの注意点があります。
1. 内積の符号チェック: クォータニオン $\bm{q}$ と $-\bm{q}$ は同じ回転を表します。しかし、$\bm{q}_0 \cdot \bm{q}_f < 0$ のとき、SLERPは「遠回り」の経路を辿ってしまいます。これを避けるため、内積が負の場合は $\bm{q}_f$ の符号を反転させます。
$$ \bm{q}_0 \cdot \bm{q}_f < 0 \;\Longrightarrow\; \bm{q}_f \leftarrow -\bm{q}_f $$
2. $\Omega \approx 0$ の場合: 2つの姿勢がほぼ同じとき、$\sin\Omega \approx 0$ となり数値的に不安定になります。この場合は通常のLERP（線形補間）にフォールバックし、結果を正規化します。
$$ \Omega < \epsilon \;\Longrightarrow\; \bm{q}(s) \approx (1-s)\,\bm{q}_0 + s\,\bm{q}_f, \quad \text{その後正規化} $$
3. 正規化: 浮動小数点の誤差蓄積により、補間結果のノルムが1からずれることがあります。各ステップで正規化 $\bm{q} \leftarrow \bm{q} / \|\bm{q}\|$ を行うと安全です。
直線補間とSLERPを組み合わせると、手先を直線経路に沿って動かしながら姿勢も滑らかに変化させることができます。しかし、直線だけでは対応できない場面もあります。たとえば、衛星の外壁に沿った検査や、障害物を回避する円弧経路が必要な場合です。次に、円弧補間の定式化を見ていきましょう。
円弧補間の定式化
円弧補間が必要な場面
円弧補間は、ロボットの手先を空間上の円弧に沿って動かす経路計画です。以下のような場面で使われます。
- 円筒形の衛星表面に沿った検査: カメラを衛星の外壁に沿って一定距離で移動させる
- 障害物回避: 直線経路上に障害物がある場合、円弧で迂回する
- 溶接・切断: 曲面上のシーム溶接や円形部品の切断
宇宙ロボットの具体例として、ISSのロボットアームがモジュールの円筒面を検査する場面を考えましょう。手先に搭載されたカメラを一定の距離を保ちながら円筒面に沿って動かすには、円弧経路が最適です。
3点から円弧を定義する
3次元空間における円弧は、通過する3点 $\bm{p}_0$（始点）、$\bm{p}_m$（中間点）、$\bm{p}_f$（終点）で定義できます。これは、3点が一直線上にない限り、一意に円を決定できるという幾何学的事実に基づいています。
まず、3点を含む平面とその平面上での円の中心・半径を求めます。
平面の法線ベクトルを、2つのベクトル $\bm{v}_1 = \bm{p}_m – \bm{p}_0$ と $\bm{v}_2 = \bm{p}_f – \bm{p}_0$ の外積から求めます。
$$ \begin{equation} \hat{\bm{n}} = \frac{\bm{v}_1 \times \bm{v}_2}{\|\bm{v}_1 \times \bm{v}_2\|} \end{equation} $$
円の中心 $\bm{c}$ は、3点から等距離にある点です。$\|\bm{c} – \bm{p}_0\|^2 = \|\bm{c} – \bm{p}_m\|^2$ と $\|\bm{c} – \bm{p}_0\|^2 = \|\bm{c} – \bm{p}_f\|^2$ の連立方程式を解くことで求められます。
$\|\bm{c} – \bm{p}_0\|^2 = \|\bm{c} – \bm{p}_m\|^2$ を展開すると
$$ \bm{c} \cdot \bm{c} – 2\bm{c} \cdot \bm{p}_0 + \bm{p}_0 \cdot \bm{p}_0 = \bm{c} \cdot \bm{c} – 2\bm{c} \cdot \bm{p}_m + \bm{p}_m \cdot \bm{p}_m $$
両辺の $\bm{c} \cdot \bm{c}$ を消去すると
$$ 2\bm{c} \cdot (\bm{p}_m – \bm{p}_0) = \bm{p}_m \cdot \bm{p}_m – \bm{p}_0 \cdot \bm{p}_0 $$
同様にもう1つの等式からも
$$ 2\bm{c} \cdot (\bm{p}_f – \bm{p}_0) = \bm{p}_f \cdot \bm{p}_f – \bm{p}_0 \cdot \bm{p}_0 $$
これらの連立方程式と、中心が3点と同一平面上にあるという条件を組み合わせて $\bm{c}$ を求めます。
半径 $r$ は
$$ \begin{equation} r = \|\bm{c} – \bm{p}_0\| \end{equation} $$
円弧のパラメトリック表現
円の中心 $\bm{c}$、半径 $r$、法線 $\hat{\bm{n}}$ が求まったら、円弧上の点をパラメータ $s \in [0, 1]$ で表現します。
まず、円の平面上に2つの直交する単位ベクトル $\hat{\bm{u}}$, $\hat{\bm{v}}$ を構成します。
$$ \hat{\bm{u}} = \frac{\bm{p}_0 – \bm{c}}{\|\bm{p}_0 – \bm{c}\|} $$
$$ \hat{\bm{v}} = \hat{\bm{n}} \times \hat{\bm{u}} $$
$\hat{\bm{u}}$ は中心から始点 $\bm{p}_0$ への方向、$\hat{\bm{v}}$ は $\hat{\bm{n}}$ と $\hat{\bm{u}}$ に直交する方向です。これにより $\{\hat{\bm{u}}, \hat{\bm{v}}, \hat{\bm{n}}\}$ は右手系の正規直交基底を成します。
始点 $\bm{p}_0$ に対応する角度は $\phi_0 = 0$（$\hat{\bm{u}}$ 方向を基準としたため）です。終点 $\bm{p}_f$ に対応する角度 $\phi_f$ は、$\bm{p}_f – \bm{c}$ を $\hat{\bm{u}}$-$\hat{\bm{v}}$ 平面に投影して atan2 で求めます。
$$ \phi_f = \text{atan2}\!\big((\bm{p}_f – \bm{c}) \cdot \hat{\bm{v}},\;(\bm{p}_f – \bm{c}) \cdot \hat{\bm{u}}\big) $$
円弧上の点は、角度パラメータ $\phi(s) = s \cdot \phi_f$ を用いて次のように表されます。
$$ \begin{equation} \bm{p}(s) = \bm{c} + r\cos(\phi(s))\,\hat{\bm{u}} + r\sin(\phi(s))\,\hat{\bm{v}} \end{equation} $$
$s = 0$ で $\bm{p}(0) = \bm{c} + r\hat{\bm{u}} = \bm{p}_0$、$s = 1$ で $\bm{p}(1) = \bm{c} + r\cos\phi_f\,\hat{\bm{u}} + r\sin\phi_f\,\hat{\bm{v}} = \bm{p}_f$ となることが確認できます。
円弧長と速度
円弧の全長は
$$ L = r \cdot |\phi_f| $$
です。パラメータ $s$ が時間に対して一定速度で進む場合、手先は弧長に対して等速で動きます。速度ベクトルは
$$ \dot{\bm{p}} = r\,\dot{\phi}\,(-\sin\phi\,\hat{\bm{u}} + \cos\phi\,\hat{\bm{v}}) $$
と表され、速度の大きさは $|\dot{\bm{p}}| = r|\dot{\phi}|$ で一定です。これは手先が円弧上を等速で移動することを意味します。
直線補間と円弧補間の位置経路を定義できたところで、次に重要な問題に移ります。作業空間で定義された経路を、ロボットの関節角度に変換するためには逆運動学が必要です。この組み合わせにはいくつかの重要な課題があります。
逆運動学との組み合わせ
作業空間経路の離散化と逆運動学
作業空間で定義された連続経路を実際のロボットに実行させるには、経路を時間的に離散化し、各時刻で逆運動学を解きます。
パラメータ $s$ を $N$ 個のサンプル点に離散化します。
$$ s_k = \frac{k}{N}, \quad k = 0, 1, 2, \ldots, N $$
各サンプル点での手先の目標位置・姿勢 $(\bm{p}(s_k), \bm{R}(s_k))$ に対して、逆運動学を解いて関節角度 $\bm{q}_k$ を求めます。
$$ \bm{q}_k = \text{IK}(\bm{p}(s_k), \bm{R}(s_k)) $$
ここで IK は逆運動学の解法を表します。解析解が存在する場合はそれを用いますが、一般的な多自由度ロボットでは数値的に解くことが多いです。
逐次逆運動学（Resolved Motion Rate Control）
数値的逆運動学の代表的な手法が、逐次逆運動学（Resolved Motion Rate Control, RMRC）です。ヤコビ行列 $\bm{J}(\bm{q})$ を用いて、作業空間の微小変位 $\Delta\bm{x}$ を関節空間の微小変位 $\Delta\bm{q}$ に変換します。
$$ \Delta\bm{x} = \bm{J}(\bm{q})\,\Delta\bm{q} $$
この関係を逆に解くと
$$ \begin{equation} \Delta\bm{q} = \bm{J}^{-1}(\bm{q})\,\Delta\bm{x} \end{equation} $$
ただし、ヤコビ行列が正方でない場合（冗長ロボット）は、擬似逆行列を用います。
$$ \begin{equation} \Delta\bm{q} = \bm{J}^{\dagger}(\bm{q})\,\Delta\bm{x} = \bm{J}^T(\bm{J}\bm{J}^T)^{-1}\,\Delta\bm{x} \end{equation} $$
これを各サンプル点で逐次的に適用し、前のステップの関節角度 $\bm{q}_{k-1}$ を初期値として次の関節角度を計算します。
$$ \bm{q}_k = \bm{q}_{k-1} + \bm{J}^{\dagger}(\bm{q}_{k-1})\,(\bm{x}_k – \bm{x}_{k-1}) $$
サンプリング間隔の選び方
離散化の間隔が粗すぎると、実際の手先経路が目標経路から逸脱します。特に曲率の大きい円弧区間では、十分に細かいサンプリングが必要です。
目安として、サンプリング間隔 $\Delta s$ は以下の条件を満たすべきです。
$$ \Delta s \leq \frac{\epsilon}{r \cdot |\phi_f|} $$
ここで $\epsilon$ は許容される経路誤差、$r$ は曲率半径、$|\phi_f|$ は円弧の中心角です。直線区間ではサンプリングを粗く、曲率の大きい区間では細かくする適応的サンプリングも有効です。
逆運動学を各時刻で解くことで作業空間経路を実現できますが、特定の姿勢でヤコビ行列が退化する特異点の問題があります。これは作業空間経路計画の最大の課題の1つです。
特異点通過問題と対策
特異点とは何か
ロボットの特異点（Singularity）とは、ヤコビ行列 $\bm{J}(\bm{q})$ のランクが落ちる姿勢のことです。日常的な例で言えば、腕を完全に伸ばしきった状態がこれに当たります。肘が完全に伸びると、肘関節を動かしても手先を「腕の方向」には動かせなくなります。つまり、ある方向への手先の運動能力が失われます。
数学的には、ヤコビ行列の行列式がゼロになる条件で定義されます（正方行列の場合）。
$$ \det(\bm{J}(\bm{q})) = 0 $$
非正方行列の場合は、$\bm{J}\bm{J}^T$ の最小特異値がゼロに近づく条件です。
特異点近傍での問題
特異点の近くでは、逆運動学の解が数値的に不安定になります。$\bm{J}^{-1}$ や $\bm{J}^{\dagger}$ の要素が非常に大きくなり、小さな作業空間の変位 $\Delta\bm{x}$ に対して巨大な関節角速度 $\Delta\bm{q}$ が要求されます。
$$ \|\Delta\bm{q}\| = \|\bm{J}^{\dagger}\Delta\bm{x}\| \to \infty \quad (\text{特異点近傍}) $$
これは物理的には、関節が限界を超えた速度で回転しようとすることを意味し、ロボットの損傷や制御の破綻につながります。
対策1: ダンピング付き擬似逆行列（DLS法）
最も広く使われる対策が、Damped Least Squares（DLS）法、別名 Levenberg-Marquardt法 です。通常の擬似逆行列の代わりに、ダンピング項 $\lambda$ を加えた逆行列を使います。
$$ \begin{equation} \Delta\bm{q} = \bm{J}^T(\bm{J}\bm{J}^T + \lambda^2\bm{I})^{-1}\,\Delta\bm{x} \end{equation} $$
この式の直感的な意味を考えましょう。通常の擬似逆行列は「作業空間の誤差を最小化する」のに対し、DLS法は「作業空間の誤差と関節角変位の大きさのトレードオフ」を取ります。$\lambda$ が大きいほど関節角変位が抑制されますが、経路の追従精度は低下します。
ダンピング係数 $\lambda$ は、特異点からの距離に応じて適応的に変化させるのが効果的です。可操作度（Manipulability）$w$ を指標として使います。
$$ w = \sqrt{\det(\bm{J}\bm{J}^T)} $$
$w$ が小さいほど特異点に近いため、$\lambda$ を大きくします。
$$ \lambda^2 = \begin{cases} \lambda_{\max}^2\left(1 – \left(\frac{w}{w_{\text{th}}}\right)^2\right) & \text{if } w < w_{\text{th}} \\ 0 & \text{if } w \geq w_{\text{th}} \end{cases} $$
ここで $w_{\text{th}}$ は可操作度の閾値です。特異点から十分離れているときは $\lambda = 0$ となり、通常の擬似逆行列に戻ります。
対策2: 経路の再計画
特異点を通過する経路そのものを変更する方法もあります。
- 経路のオフセット: 特異点を避けるように経路を少しずらす
- 冗長自由度の活用: 冗長ロボットでは、手先の経路を変えずに関節の姿勢（ヌルスペース運動）を調整して特異点を回避
- Via点の追加: 直線経路を複数の区間に分割し、中間点（Via点）を追加して特異点を迂回
宇宙ロボットでは、ミッションの安全性が最優先のため、事前のシミュレーションで特異点の位置を特定し、経路設計の段階で回避するのが一般的です。
ここまでの理論を踏まえて、いよいよPythonで直線補間・円弧補間・SLERPを実装し、3Dで可視化してみましょう。
Pythonによる直線補間の実装と可視化
まず、3次元空間での直線補間を実装し、手先の位置経路を可視化します。台形速度プロファイルも実装して、加減速を含む実際的な動作を再現します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# --- 直線補間（LERP）---
def lerp(p0, pf, s):
"""2点間の線形補間"""
return (1 - s) * p0 + s * pf
# --- 台形速度プロファイル ---
def trapezoidal_profile(t, T, ta):
"""
台形速度プロファイルによるパラメータ s(t) を返す
t: 現在時刻, T: 総時間, ta: 加減速時間
"""
td = ta # 加速と減速を対称にする
if t < 0:
return 0.0
elif t < ta:
# 加速区間
return 0.5 * (t ** 2) / (ta * (T - ta))
elif t < T - td:
# 定速区間
return (t - ta / 2) / (T - ta)
elif t <= T:
# 減速区間
return 1.0 - 0.5 * ((T - t) ** 2) / (td * (T - td))
else:
return 1.0
# パラメータ設定
p0 = np.array([0.3, 0.0, 0.5]) # 始点
pf = np.array([0.7, 0.5, 0.8]) # 終点
T = 2.0 # 総移動時間 [s]
ta = 0.5 # 加減速時間 [s]
N = 200 # サンプル数
# 時間配列と経路計算
t_arr = np.linspace(0, T, N)
s_arr = np.array([trapezoidal_profile(t, T, ta) for t in t_arr])
path = np.array([lerp(p0, pf, s) for s in s_arr])
# --- 可視化 ---
fig = plt.figure(figsize=(14, 5))
# 3D経路
ax1 = fig.add_subplot(131, projection='3d')
ax1.plot(path[:, 0], path[:, 1], path[:, 2], 'b-', linewidth=2)
ax1.scatter(*p0, color='green', s=100, zorder=5, label='Start')
ax1.scatter(*pf, color='red', s=100, zorder=5, label='Goal')
ax1.set_xlabel('X [m]')
ax1.set_ylabel('Y [m]')
ax1.set_zlabel('Z [m]')
ax1.set_title('Linear Interpolation (3D)')
ax1.legend()
# パラメータ s(t)
ax2 = fig.add_subplot(132)
ax2.plot(t_arr, s_arr, 'b-', linewidth=2)
ax2.set_xlabel('Time [s]')
ax2.set_ylabel('s(t)')
ax2.set_title('Trapezoidal Profile: s(t)')
ax2.grid(True, alpha=0.3)
# 速度プロファイル（数値微分）
ds_dt = np.gradient(s_arr, t_arr)
speed = ds_dt * np.linalg.norm(pf - p0)
ax3 = fig.add_subplot(133)
ax3.plot(t_arr, speed, 'r-', linewidth=2)
ax3.set_xlabel('Time [s]')
ax3.set_ylabel('Speed [m/s]')
ax3.set_title('End-Effector Speed')
ax3.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
上のコードの実行結果から、3つの重要な特徴が確認できます。
- 3D経路（左図）: 始点（緑）から終点（赤）への経路が完全な直線になっています。作業空間での直線補間が正しく機能していることがわかります。
- パラメータ $s(t)$（中央図）: 台形速度プロファイルにより、$s(t)$ が S字状に変化しています。始点・終点付近で加減速し、中間区間では一定速度で進んでいます。
- 速度プロファイル（右図）: 手先の速度が台形状に変化しています。急激な加減速がないため、関節トルクへの負担が軽減されます。等速区間の速度は $\|\bm{p}_f – \bm{p}_0\| / (T – t_a) \approx 0.48$ m/s です。
PythonによるクォータニオンSLERPの実装
次に、クォータニオンSLERPを実装して姿勢補間を可視化します。姿勢を座標軸フレームとして描画し、滑らかに回転する様子を確認します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# --- クォータニオン演算 ---
def quat_normalize(q):
"""クォータニオンの正規化"""
return q / np.linalg.norm(q)
def quat_to_rotation_matrix(q):
"""クォータニオン (w, x, y, z) -> 回転行列"""
w, x, y, z = q
return np.array([
[1 - 2*(y**2 + z**2), 2*(x*y - w*z), 2*(x*z + w*y)],
[2*(x*y + w*z), 1 - 2*(x**2 + z**2), 2*(y*z - w*x)],
[2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x**2 + y**2)]
])
def axis_angle_to_quat(axis, angle):
"""回転軸と角度からクォータニオンを生成"""
axis = axis / np.linalg.norm(axis)
w = np.cos(angle / 2)
xyz = np.sin(angle / 2) * axis
return np.array([w, xyz[0], xyz[1], xyz[2]])
def slerp(q0, qf, s):
"""
球面線形補間（SLERP）
q0, qf: 単位クォータニオン (w, x, y, z)
s: 補間パラメータ [0, 1]
"""
q0 = quat_normalize(q0)
qf = quat_normalize(qf)
# 内積の符号チェック（最短経路を選択）
dot = np.dot(q0, qf)
if dot < 0:
qf = -qf
dot = -dot
# Omega が小さい場合は LERP にフォールバック
if dot > 0.9995:
result = (1 - s) * q0 + s * qf
return quat_normalize(result)
omega = np.arccos(np.clip(dot, -1, 1))
sin_omega = np.sin(omega)
coeff0 = np.sin((1 - s) * omega) / sin_omega
coeff1 = np.sin(s * omega) / sin_omega
return quat_normalize(coeff0 * q0 + coeff1 * qf)
SLERPの関数が定義できたので、次に姿勢の補間過程を3D座標フレームとして可視化します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# slerp, quat_normalize, quat_to_rotation_matrix, axis_angle_to_quat は上で定義済み
def quat_normalize(q):
return q / np.linalg.norm(q)
def quat_to_rotation_matrix(q):
w, x, y, z = q
return np.array([
[1 - 2*(y**2 + z**2), 2*(x*y - w*z), 2*(x*z + w*y)],
[2*(x*y + w*z), 1 - 2*(x**2 + z**2), 2*(y*z - w*x)],
[2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x**2 + y**2)]
])
def axis_angle_to_quat(axis, angle):
axis = axis / np.linalg.norm(axis)
w = np.cos(angle / 2)
xyz = np.sin(angle / 2) * axis
return np.array([w, xyz[0], xyz[1], xyz[2]])
def slerp(q0, qf, s):
q0 = quat_normalize(q0)
qf = quat_normalize(qf)
dot = np.dot(q0, qf)
if dot < 0:
qf = -qf
dot = -dot
if dot > 0.9995:
result = (1 - s) * q0 + s * qf
return quat_normalize(result)
omega = np.arccos(np.clip(dot, -1, 1))
sin_omega = np.sin(omega)
coeff0 = np.sin((1 - s) * omega) / sin_omega
coeff1 = np.sin(s * omega) / sin_omega
return quat_normalize(coeff0 * q0 + coeff1 * qf)
def draw_frame(ax, origin, R, length=0.15, labels=None):
"""座標フレームを描画"""
colors = ['r', 'g', 'b']
default_labels = ['x', 'y', 'z']
for i in range(3):
end = origin + length * R[:, i]
ax.quiver(origin[0], origin[1], origin[2],
R[0, i]*length, R[1, i]*length, R[2, i]*length,
color=colors[i], linewidth=2, arrow_length_ratio=0.15)
# 始点・終点の姿勢を定義
q_start = axis_angle_to_quat(np.array([0, 0, 1]), 0.0) # 単位姿勢
q_end = axis_angle_to_quat(np.array([1, 1, 0]), np.pi * 0.7) # 斜め軸で126°回転
# 位置は直線補間
p_start = np.array([0.0, 0.0, 0.0])
p_end = np.array([1.0, 0.5, 0.3])
# 補間
N_frames = 8
s_values = np.linspace(0, 1, N_frames)
fig = plt.figure(figsize=(12, 6))
ax = fig.add_subplot(111, projection='3d')
# 経路の線
path_points = np.array([(1-s)*p_start + s*p_end for s in np.linspace(0, 1, 100)])
ax.plot(path_points[:, 0], path_points[:, 1], path_points[:, 2],
'k--', alpha=0.3, linewidth=1)
for s in s_values:
p = (1 - s) * p_start + s * p_end
q = slerp(q_start, q_end, s)
R = quat_to_rotation_matrix(q)
draw_frame(ax, p, R, length=0.12)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('Position LERP + Orientation SLERP')
ax.set_xlim(-0.3, 1.3)
ax.set_ylim(-0.3, 0.8)
ax.set_zlim(-0.3, 0.6)
plt.tight_layout()
plt.show()
この可視化から、以下の重要な性質が読み取れます。
- 位置は直線: 手先の位置（座標フレームの原点）が一直線上に等間隔で配置されています。LERP による直線補間が正しく機能しています。
- 姿勢は滑らかに回転: 各フレームの座標軸（赤=X, 緑=Y, 青=Z）が滑らかに回転しています。SLERPは一定の角速度で回転を補間するため、途中で角速度が急変したり振動したりすることがありません。
- 最短経路: SLERPは姿勢空間（4次元球面上）の最短経路を辿るため、不要な回り道をしません。内積の符号チェックにより、常に180度未満の短い方の経路が選ばれています。
次に、SLERPとNLERP（正規化線形補間）の違いを定量的に比較してみましょう。
import numpy as np
import matplotlib.pyplot as plt
def quat_normalize(q):
return q / np.linalg.norm(q)
def axis_angle_to_quat(axis, angle):
axis = axis / np.linalg.norm(axis)
w = np.cos(angle / 2)
xyz = np.sin(angle / 2) * axis
return np.array([w, xyz[0], xyz[1], xyz[2]])
def slerp(q0, qf, s):
q0 = quat_normalize(q0)
qf = quat_normalize(qf)
dot = np.dot(q0, qf)
if dot < 0:
qf = -qf
dot = -dot
if dot > 0.9995:
result = (1 - s) * q0 + s * qf
return quat_normalize(result)
omega = np.arccos(np.clip(dot, -1, 1))
sin_omega = np.sin(omega)
coeff0 = np.sin((1 - s) * omega) / sin_omega
coeff1 = np.sin(s * omega) / sin_omega
return quat_normalize(coeff0 * q0 + coeff1 * qf)
def nlerp(q0, qf, s):
"""正規化線形補間（NLERP）"""
q0 = quat_normalize(q0)
qf = quat_normalize(qf)
dot = np.dot(q0, qf)
if dot < 0:
qf = -qf
result = (1 - s) * q0 + s * qf
return quat_normalize(result)
def quat_angle(q):
"""クォータニオンから回転角度を取得"""
w = np.clip(q[0], -1, 1)
return 2 * np.arccos(abs(w))
# 大きな回転角度でSLERPとNLERPの違いを比較
q0 = axis_angle_to_quat(np.array([0, 0, 1]), 0.0)
qf = axis_angle_to_quat(np.array([1, 0.5, 0.3]), np.pi * 0.8)
N = 200
s_values = np.linspace(0, 1, N)
# 各s値での回転角度を計算
angles_slerp = []
angles_nlerp = []
for s in s_values:
q_s = slerp(q0, qf, s)
q_n = nlerp(q0, qf, s)
angles_slerp.append(quat_angle(q_s))
angles_nlerp.append(quat_angle(q_n))
angles_slerp = np.array(angles_slerp)
angles_nlerp = np.array(angles_nlerp)
# 角速度（数値微分）
ds = s_values[1] - s_values[0]
omega_slerp = np.gradient(angles_slerp, ds)
omega_nlerp = np.gradient(angles_nlerp, ds)
fig, axes = plt.subplots(1, 2, figsize=(12, 5))
# 回転角度の比較
axes[0].plot(s_values, np.degrees(angles_slerp), 'b-', linewidth=2, label='SLERP')
axes[0].plot(s_values, np.degrees(angles_nlerp), 'r--', linewidth=2, label='NLERP')
axes[0].set_xlabel('Parameter s')
axes[0].set_ylabel('Rotation angle [deg]')
axes[0].set_title('Rotation Angle vs Parameter')
axes[0].legend()
axes[0].grid(True, alpha=0.3)
# 角速度の比較
axes[1].plot(s_values, np.degrees(omega_slerp), 'b-', linewidth=2, label='SLERP')
axes[1].plot(s_values, np.degrees(omega_nlerp), 'r--', linewidth=2, label='NLERP')
axes[1].set_xlabel('Parameter s')
axes[1].set_ylabel('Angular velocity [deg/unit s]')
axes[1].set_title('Angular Velocity vs Parameter')
axes[1].legend()
axes[1].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
この比較から、SLERPとNLERPの本質的な違いが明確にわかります。
- 回転角度（左図）: SLERPでは回転角度がパラメータ $s$ に対して完全に線形に変化します。一方、NLERPでは中間付近でわずかに非線形になり、$s$ と回転角度の関係にずれが生じます。
- 角速度（右図）: SLERPの角速度は $s$ によらず一定であるのに対し、NLERPの角速度は $s = 0.5$ 付近で最大になり、両端で小さくなります。この速度のムラは、宇宙ロボットの精密な姿勢制御では問題になります。
NLERPは計算コストがSLERPより低い（三角関数を使わない）ため、回転角度が小さい場合やリアルタイム性が重要な場合に使われることがありますが、精密な作業空間経路計画ではSLERPが標準です。
Pythonによる円弧補間の実装と可視化
3点を通る円弧補間を実装し、直線補間との違いを3Dで比較します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def compute_arc_params(p0, pm, pf):
"""
3点から円弧のパラメータ（中心, 半径, 法線, 基底, 角度範囲）を計算
p0: 始点, pm: 中間点, pf: 終点
"""
# 平面の法線ベクトル
v1 = pm - p0
v2 = pf - p0
normal = np.cross(v1, v2)
normal = normal / np.linalg.norm(normal)
# 円の中心を求める（垂直二等分面の交線）
# |c - p0|^2 = |c - pm|^2, |c - p0|^2 = |c - pf|^2
# c は p0, pm, pf と同一平面上にある
# c = p0 + alpha * v1 + beta * v2 と仮定
A = np.array([
[2 * np.dot(v1, v1), 2 * np.dot(v1, v2)],
[2 * np.dot(v2, v1), 2 * np.dot(v2, v2)]
])
b = np.array([
np.dot(v1, v1),
np.dot(v2, v2)
])
params = np.linalg.solve(A, b)
center = p0 + params[0] * v1 + params[1] * v2
radius = np.linalg.norm(center - p0)
# 平面上の直交基底
u_hat = (p0 - center) / np.linalg.norm(p0 - center)
v_hat = np.cross(normal, u_hat)
# 終点の角度
d = pf - center
phi_f = np.arctan2(np.dot(d, v_hat), np.dot(d, u_hat))
# 中間点の角度（方向チェック用）
dm = pm - center
phi_m = np.arctan2(np.dot(dm, v_hat), np.dot(dm, u_hat))
# 中間点が円弧上に正しく含まれるよう角度を調整
if phi_f < 0 and phi_m > 0:
phi_f += 2 * np.pi
elif phi_f > 0 and phi_m < 0:
phi_f -= 2 * np.pi
return center, radius, normal, u_hat, v_hat, phi_f
def arc_interpolation(center, radius, u_hat, v_hat, phi_f, s):
"""円弧上の点を計算"""
phi = s * phi_f
return center + radius * (np.cos(phi) * u_hat + np.sin(phi) * v_hat)
# 3点の定義
p0 = np.array([0.3, 0.0, 0.5]) # 始点
pm = np.array([0.5, 0.4, 0.7]) # 中間点
pf = np.array([0.8, 0.3, 0.4]) # 終点
# 円弧パラメータの計算
center, radius, normal, u_hat, v_hat, phi_f = compute_arc_params(p0, pm, pf)
print(f"円の中心: {center}")
print(f"半径: {radius:.4f} m")
print(f"中心角: {np.degrees(phi_f):.1f} deg")
print(f"円弧長: {radius * abs(phi_f):.4f} m")
# 円弧経路の計算
N = 200
s_arr = np.linspace(0, 1, N)
arc_path = np.array([arc_interpolation(center, radius, u_hat, v_hat, phi_f, s)
for s in s_arr])
# 直線経路（比較用）
line_path = np.array([(1-s)*p0 + s*pf for s in s_arr])
# --- 可視化 ---
fig = plt.figure(figsize=(14, 6))
# 3D比較
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(arc_path[:, 0], arc_path[:, 1], arc_path[:, 2],
'b-', linewidth=2, label='Arc interpolation')
ax1.plot(line_path[:, 0], line_path[:, 1], line_path[:, 2],
'r--', linewidth=2, label='Linear interpolation')
ax1.scatter(*p0, color='green', s=100, zorder=5, label='Start')
ax1.scatter(*pm, color='orange', s=100, zorder=5, label='Via point')
ax1.scatter(*pf, color='red', s=100, zorder=5, label='Goal')
ax1.scatter(*center, color='purple', s=80, marker='+', zorder=5, label='Center')
ax1.set_xlabel('X [m]')
ax1.set_ylabel('Y [m]')
ax1.set_zlabel('Z [m]')
ax1.set_title('Arc vs Linear Interpolation (3D)')
ax1.legend(fontsize=8)
# 中心からの距離（円弧が一定半径を保つことの確認）
dist_from_center_arc = np.linalg.norm(arc_path - center, axis=1)
dist_from_center_line = np.linalg.norm(line_path - center, axis=1)
ax2 = fig.add_subplot(122)
ax2.plot(s_arr, dist_from_center_arc, 'b-', linewidth=2, label='Arc')
ax2.plot(s_arr, dist_from_center_line, 'r--', linewidth=2, label='Linear')
ax2.axhline(y=radius, color='gray', linestyle=':', alpha=0.5, label=f'Radius = {radius:.3f}')
ax2.set_xlabel('Parameter s')
ax2.set_ylabel('Distance from center [m]')
ax2.set_title('Distance from Arc Center')
ax2.legend()
ax2.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
この可視化の結果から、直線補間と円弧補間の違いが明確にわかります。
- 3D経路（左図）: 青い実線（円弧補間）は3点（始点・中間点・終点）を全て通る滑らかな曲線を描いています。赤い破線（直線補間）は始点と終点を結ぶだけで、中間点は通りません。衛星表面に沿った経路が必要な場合、直線補間では表面から逸脱しますが、円弧補間なら一定距離を保てます。
- 中心からの距離（右図）: 円弧補間では中心からの距離が全区間で一定（= 半径）に保たれています。これが円弧経路の本質的な特徴です。一方、直線補間では中間付近で距離が半径より小さくなり、「内側にショートカット」していることがわかります。
直線補間・円弧補間・SLERPの統合実装
最後に、直線補間と円弧補間をSLERP姿勢補間と組み合わせた統合的な経路を実装し、宇宙ロボットのアプローチ・円弧回避・最終直線アプローチの3区間からなるミッション経路を可視化します。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# --- クォータニオン演算 ---
def quat_normalize(q):
return q / np.linalg.norm(q)
def quat_to_rotation_matrix(q):
w, x, y, z = q
return np.array([
[1 - 2*(y**2 + z**2), 2*(x*y - w*z), 2*(x*z + w*y)],
[2*(x*y + w*z), 1 - 2*(x**2 + z**2), 2*(y*z - w*x)],
[2*(x*z - w*y), 2*(y*z + w*x), 1 - 2*(x**2 + y**2)]
])
def axis_angle_to_quat(axis, angle):
axis = axis / np.linalg.norm(axis)
w = np.cos(angle / 2)
xyz = np.sin(angle / 2) * axis
return np.array([w, xyz[0], xyz[1], xyz[2]])
def slerp(q0, qf, s):
q0 = quat_normalize(q0)
qf = quat_normalize(qf)
dot = np.dot(q0, qf)
if dot < 0:
qf = -qf
dot = -dot
if dot > 0.9995:
result = (1 - s) * q0 + s * qf
return quat_normalize(result)
omega = np.arccos(np.clip(dot, -1, 1))
sin_omega = np.sin(omega)
coeff0 = np.sin((1 - s) * omega) / sin_omega
coeff1 = np.sin(s * omega) / sin_omega
return quat_normalize(coeff0 * q0 + coeff1 * qf)
def draw_frame(ax, origin, R, length=0.08):
colors = ['r', 'g', 'b']
for i in range(3):
ax.quiver(origin[0], origin[1], origin[2],
R[0, i]*length, R[1, i]*length, R[2, i]*length,
color=colors[i], linewidth=1.5, arrow_length_ratio=0.15)
# --- 円弧補間 ---
def compute_arc_params(p0, pm, pf):
v1 = pm - p0
v2 = pf - p0
normal = np.cross(v1, v2)
normal = normal / np.linalg.norm(normal)
A = np.array([
[2 * np.dot(v1, v1), 2 * np.dot(v1, v2)],
[2 * np.dot(v2, v1), 2 * np.dot(v2, v2)]
])
b_vec = np.array([np.dot(v1, v1), np.dot(v2, v2)])
params = np.linalg.solve(A, b_vec)
center = p0 + params[0] * v1 + params[1] * v2
radius = np.linalg.norm(center - p0)
u_hat = (p0 - center) / np.linalg.norm(p0 - center)
v_hat = np.cross(normal, u_hat)
d = pf - center
phi_f = np.arctan2(np.dot(d, v_hat), np.dot(d, u_hat))
dm = pm - center
phi_m = np.arctan2(np.dot(dm, v_hat), np.dot(dm, u_hat))
if phi_f < 0 and phi_m > 0:
phi_f += 2 * np.pi
elif phi_f > 0 and phi_m < 0:
phi_f -= 2 * np.pi
return center, radius, normal, u_hat, v_hat, phi_f
# --- ミッション経路の定義 ---
# 区間1: 直線アプローチ（初期位置 → 障害物回避開始点）
p_A = np.array([0.0, 0.0, 1.0]) # 初期位置
p_B = np.array([0.5, 0.2, 0.8]) # 円弧開始点
# 区間2: 円弧回避（障害物を迂回）
p_via = np.array([0.7, 0.5, 0.9]) # 円弧中間点
p_C = np.array([0.9, 0.3, 0.7]) # 円弧終了点
# 区間3: 直線アプローチ（目標へ最終接近）
p_D = np.array([1.2, 0.1, 0.5]) # 最終目標（ドッキングポート）
# 姿勢の定義
q_A = axis_angle_to_quat(np.array([0, 0, 1]), 0.0)
q_B = axis_angle_to_quat(np.array([0, 0, 1]), np.pi/6)
q_C = axis_angle_to_quat(np.array([0, 1, 0]), np.pi/3)
q_D = axis_angle_to_quat(np.array([1, 0, 0]), np.pi/4)
# 円弧パラメータ
center, radius, normal, u_hat, v_hat, phi_f = compute_arc_params(p_B, p_via, p_C)
# --- 経路の計算 ---
N_per_segment = 80 # 各区間のサンプル数
all_positions = []
all_quaternions = []
# 区間1: 直線 A→B
for i in range(N_per_segment):
s = i / (N_per_segment - 1)
pos = (1 - s) * p_A + s * p_B
quat = slerp(q_A, q_B, s)
all_positions.append(pos)
all_quaternions.append(quat)
# 区間2: 円弧 B→C
for i in range(N_per_segment):
s = i / (N_per_segment - 1)
phi = s * phi_f
pos = center + radius * (np.cos(phi) * u_hat + np.sin(phi) * v_hat)
quat = slerp(q_B, q_C, s)
all_positions.append(pos)
all_quaternions.append(quat)
# 区間3: 直線 C→D
for i in range(N_per_segment):
s = i / (N_per_segment - 1)
pos = (1 - s) * p_C + s * p_D
quat = slerp(q_C, q_D, s)
all_positions.append(pos)
all_quaternions.append(quat)
all_positions = np.array(all_positions)
# --- 可視化 ---
fig = plt.figure(figsize=(14, 8))
ax = fig.add_subplot(111, projection='3d')
# 経路を区間ごとに色分け
n = N_per_segment
ax.plot(all_positions[:n, 0], all_positions[:n, 1], all_positions[:n, 2],
'b-', linewidth=2.5, label='Segment 1: Linear approach')
ax.plot(all_positions[n:2*n, 0], all_positions[n:2*n, 1], all_positions[n:2*n, 2],
'orange', linewidth=2.5, label='Segment 2: Arc avoidance')
ax.plot(all_positions[2*n:, 0], all_positions[2*n:, 1], all_positions[2*n:, 2],
'g-', linewidth=2.5, label='Segment 3: Final approach')
# ウェイポイント
waypoints = [p_A, p_B, p_C, p_D]
wp_labels = ['A: Start', 'B: Arc begin', 'C: Arc end', 'D: Target']
wp_colors = ['blue', 'orange', 'orange', 'red']
for wp, label, c in zip(waypoints, wp_labels, wp_colors):
ax.scatter(*wp, color=c, s=100, zorder=5)
ax.text(wp[0]+0.03, wp[1]+0.03, wp[2]+0.03, label, fontsize=9)
# 障害物（仮想的な球体）
obstacle_center = np.array([0.65, 0.35, 0.85])
u_obs = np.linspace(0, 2*np.pi, 20)
v_obs = np.linspace(0, np.pi, 20)
r_obs = 0.08
x_obs = obstacle_center[0] + r_obs * np.outer(np.cos(u_obs), np.sin(v_obs))
y_obs = obstacle_center[1] + r_obs * np.outer(np.sin(u_obs), np.sin(v_obs))
z_obs = obstacle_center[2] + r_obs * np.outer(np.ones_like(u_obs), np.cos(v_obs))
ax.plot_surface(x_obs, y_obs, z_obs, color='red', alpha=0.3)
ax.text(obstacle_center[0], obstacle_center[1], obstacle_center[2]+0.12,
'Obstacle', fontsize=9, color='red')
# 姿勢フレームを間引いて描画
frame_indices = list(range(0, len(all_positions), 20))
for idx in frame_indices:
R = quat_to_rotation_matrix(all_quaternions[idx])
draw_frame(ax, all_positions[idx], R, length=0.06)
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
ax.set_title('Integrated Path: Linear + Arc + SLERP')
ax.legend(loc='upper left', fontsize=9)
plt.tight_layout()
plt.show()
この統合的な可視化から、作業空間経路計画の全体像が把握できます。
- 3区間の経路構成: 青い直線（区間1: 初期アプローチ）、オレンジの円弧（区間2: 障害物回避）、緑の直線（区間3: 最終接近）が滑らかに接続されています。宇宙ロボットの典型的なミッションでは、このように複数の区間を組み合わせて経路を設計します。
- 障害物回避: 赤い半透明の球体（障害物）を円弧経路で迂回しています。直線経路では障害物を貫通してしまいますが、中間点（Via点）を適切に設定した円弧経路により安全に回避できています。
- 姿勢の連続性: 各ウェイポイントで姿勢が滑らかに遷移しています。座標フレーム（赤=X, 緑=Y, 青=Z）の回転が急変する箇所がなく、SLERPによる補間が効果的に機能しています。区間の接続点（B点、C点）で姿勢の不連続が発生しないことも重要なポイントです。
特異点近傍でのDLS法の効果確認
最後に、ダンピング付き擬似逆行列（DLS法）が特異点近傍でどのように機能するか、簡単な2リンクロボットで確認します。
import numpy as np
import matplotlib.pyplot as plt
# --- 2リンク平面ロボット ---
L1, L2 = 1.0, 1.0 # リンク長
def forward_kinematics(q):
"""順運動学: 関節角度 → 手先位置"""
q1, q2 = q
x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
return np.array([x, y])
def jacobian(q):
"""ヤコビ行列"""
q1, q2 = q
return np.array([
[-L1*np.sin(q1) - L2*np.sin(q1+q2), -L2*np.sin(q1+q2)],
[ L1*np.cos(q1) + L2*np.cos(q1+q2), L2*np.cos(q1+q2)]
])
def ik_step_pinv(q, x_target):
"""通常の擬似逆行列による逆運動学ステップ"""
x_current = forward_kinematics(q)
dx = x_target - x_current
J = jacobian(q)
dq = np.linalg.pinv(J) @ dx
return dq
def ik_step_dls(q, x_target, lam):
"""DLS法による逆運動学ステップ"""
x_current = forward_kinematics(q)
dx = x_target - x_current
J = jacobian(q)
JJT = J @ J.T
dq = J.T @ np.linalg.solve(JJT + lam**2 * np.eye(2), dx)
return dq
# 特異点を通過する直線経路
# 腕が伸びきる付近（x ≈ 2.0）を通過
N_points = 100
x_path = np.linspace(1.5, 1.98, N_points)
y_path = np.full(N_points, 0.1)
targets = np.column_stack([x_path, y_path])
# 初期関節角度
q_init = np.array([0.3, 0.5])
# 擬似逆行列による追従
q_pinv = q_init.copy()
q_pinv_history = [q_pinv.copy()]
dq_pinv_norms = []
# DLS法による追従
q_dls = q_init.copy()
q_dls_history = [q_dls.copy()]
dq_dls_norms = []
lam_max = 0.1
w_threshold = 0.05
for i in range(1, N_points):
# 擬似逆行列
dq = ik_step_pinv(q_pinv, targets[i])
dq_norm = np.linalg.norm(dq)
dq_pinv_norms.append(dq_norm)
if dq_norm > 2.0: # 安全制限
dq = dq * 2.0 / dq_norm
q_pinv = q_pinv + dq
q_pinv_history.append(q_pinv.copy())
# DLS法（適応的ダンピング）
J = jacobian(q_dls)
w = np.sqrt(max(np.linalg.det(J @ J.T), 0))
if w < w_threshold:
lam = lam_max * np.sqrt(1 - (w / w_threshold)**2)
else:
lam = 0.0
dq = ik_step_dls(q_dls, targets[i], lam)
dq_dls_norms.append(np.linalg.norm(dq))
q_dls = q_dls + dq
q_dls_history.append(q_dls.copy())
# 手先の実際の軌跡
ee_pinv = np.array([forward_kinematics(q) for q in q_pinv_history])
ee_dls = np.array([forward_kinematics(q) for q in q_dls_history])
# 可操作度の推移
manip_pinv = [np.sqrt(max(np.linalg.det(jacobian(q) @ jacobian(q).T), 0))
for q in q_pinv_history]
manip_dls = [np.sqrt(max(np.linalg.det(jacobian(q) @ jacobian(q).T), 0))
for q in q_dls_history]
# --- 可視化 ---
fig, axes = plt.subplots(2, 2, figsize=(14, 10))
# 手先経路の比較
axes[0, 0].plot(targets[:, 0], targets[:, 1], 'k--', linewidth=1, label='Target path')
axes[0, 0].plot(ee_pinv[:, 0], ee_pinv[:, 1], 'r-', linewidth=2, label='Pseudo-inverse')
axes[0, 0].plot(ee_dls[:, 0], ee_dls[:, 1], 'b-', linewidth=2, label='DLS')
axes[0, 0].set_xlabel('X [m]')
axes[0, 0].set_ylabel('Y [m]')
axes[0, 0].set_title('End-Effector Path (Near Singularity)')
axes[0, 0].legend()
axes[0, 0].grid(True, alpha=0.3)
# 関節角速度のノルム
axes[0, 1].plot(dq_pinv_norms, 'r-', linewidth=1.5, label='Pseudo-inverse')
axes[0, 1].plot(dq_dls_norms, 'b-', linewidth=1.5, label='DLS')
axes[0, 1].set_xlabel('Step')
axes[0, 1].set_ylabel('||dq|| [rad]')
axes[0, 1].set_title('Joint Velocity Norm')
axes[0, 1].legend()
axes[0, 1].grid(True, alpha=0.3)
axes[0, 1].set_ylim(0, 3)
# 可操作度
axes[1, 0].plot(manip_pinv, 'r-', linewidth=1.5, label='Pseudo-inverse')
axes[1, 0].plot(manip_dls, 'b-', linewidth=1.5, label='DLS')
axes[1, 0].axhline(y=w_threshold, color='gray', linestyle=':', label=f'Threshold = {w_threshold}')
axes[1, 0].set_xlabel('Step')
axes[1, 0].set_ylabel('Manipulability w')
axes[1, 0].set_title('Manipulability')
axes[1, 0].legend()
axes[1, 0].grid(True, alpha=0.3)
# 経路追従誤差
error_pinv = np.linalg.norm(ee_pinv - targets, axis=1)
error_dls = np.linalg.norm(ee_dls - targets, axis=1)
axes[1, 1].plot(error_pinv, 'r-', linewidth=1.5, label='Pseudo-inverse')
axes[1, 1].plot(error_dls, 'b-', linewidth=1.5, label='DLS')
axes[1, 1].set_xlabel('Step')
axes[1, 1].set_ylabel('Position error [m]')
axes[1, 1].set_title('Path Tracking Error')
axes[1, 1].legend()
axes[1, 1].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
この実験結果から、特異点近傍でのDLS法の効果が明確に確認できます。
- 手先経路（左上）: 通常の擬似逆行列（赤）は特異点付近で目標経路から大きく逸脱しています。DLS法（青）は多少の逸脱はあるものの、全体として目標経路に近い軌跡を維持しています。
- 関節角速度（右上）: 擬似逆行列では特異点に近づくにつれて関節角速度が急激に増大し、安全制限（2.0 rad）に達しています。DLS法ではダンピング項の効果で関節角速度が適切な範囲に抑制されています。
- 可操作度（左下）: 両手法とも特異点に近づくと可操作度 $w$ が低下しますが、DLS法では関節姿勢が安定しているため、可操作度の回復も滑らかです。
- 経路追従誤差（右下）: DLS法は特異点近傍で意図的に経路追従精度を犠牲にして関節速度を抑制しています。これが「精度とロバスト性のトレードオフ」であり、ダンピング係数 $\lambda$ でそのバランスを調整できます。
まとめ
本記事では、ロボットアームの作業空間における経路計画を解説しました。
- 関節空間計画 vs 作業空間計画: 手先の経路形状が重要な場面（直線アプローチ、障害物回避、精密作業）では、作業空間での計画が不可欠です
- 直線補間（LERP）: 位置を線形に補間する最も基本的な手法。台形速度プロファイルと組み合わせて滑らかな加減速を実現します
- クォータニオンSLERP: 姿勢の補間にはクォータニオンと球面線形補間を使用します。SLERPは等角速度で最短経路を辿り、ジンバルロックの問題も回避できます
- 円弧補間: 3点から円弧を定義し、曲面に沿った移動や障害物回避に活用できます。中心・半径・角度範囲を幾何学的に求めることがポイントです
- 逆運動学との統合: 作業空間で定義した経路を各時刻で離散化し、逐次逆運動学（RMRC）で関節角度に変換します
- 特異点対策: DLS法（ダンピング付き擬似逆行列）により、特異点近傍でも安定した逆運動学計算が可能になります。適応的なダンピング係数の調整がポイントです
本記事で扱った経路計画は、障害物がない（または事前に回避点を設定できる）場合のアプローチです。複雑な環境で自動的に障害物を回避する経路を生成するには、RRTやPRMなどのサンプリングベースの経路計画アルゴリズムが必要になります。
次のステップとして、以下の記事も参考にしてください。
- 衝突回避のための経路計画 — RRTとPRM — サンプリングベースの経路計画アルゴリズム
- 逆運動学 — 逆運動学の詳細な解法
- 関節空間の軌道計画 — 関節空間ベースの軌道計画