📄
ヤコビ行列の時間微分を計算する

2025/01/07に公開
1件
 目的基礎ヤコビ行列J_vと、その時間微分\dot{J}_vを求めることが目的である。ヤコビ行列は、運動学における逆運動学計算を数値解法で求める場合に必要になる。その他にも、動力学計算におけるデカルト座標系r \in \mathbb{R}^6から関節座標系q \in \mathbb{R}^nへ変換する場合に必要になる。今回は直動関節を持たない回転関節のみの機構を対象に考える。

 内容
 0. 準備r \stackrel{\mathrm{def}}{=} (p, \phi, \theta, \psi)\in \mathbb{R}^6、p\in \mathbb{R}^3とする。

 基礎ヤコビ行列の定義[1]角速度ベクトル\omegaを用いた速度の関係式として、基礎ヤコビ行列J_vを次式で定義する。

\begin{align}
    \begin{bmatrix}
        \dot{p} \\
        \omega
    \end{bmatrix} = J_v \dot{q}
\end{align}
ここで、\dot{p}は位置pの時間微分である。
[NOTE]

基礎ヤコビ行列J_vに基づいて角速度を積分しても、回転は非可換であるため、姿勢を求めることはできない[1]

 1. 基礎ヤコビ行列の計算[1][NOTE]

基礎ヤコビ行列J_vの導出は、[1]を参照されたい。
{^{i-1}R_i}をリンクi-1の座標系\Sigma_{i-1}から見たときのリンクiの座標系\Sigma_{i}の姿勢行列とし、{^{0}R_i}を台座座標系\Sigma_0から見たときのリンクiの座標系\Sigma_{i}の姿勢行列とし、{^{i-1}p_{i-1,i}}をリンクi-1の座標系\Sigma_{i-1}から見たときの座標系\Sigma_{i-1}からリンクiの座標系\Sigma_{i}の原点へのベクトルとする。添え字Eはエンドエフェクトツール点を指し、E=n+1とする。
基礎ヤコビ行列J_vは次式である。

\begin{align}
    \dot{J} = \begin{bmatrix}
        {^{0}z_1\times ^{0}p_{E,1}} & 
        {^{0}z_2\times ^{0}p_{E,2}} & 
        \cdots & 
        {^{0}z_n\times ^{0}p_{E,n}} 
        \\
        {^{0}z_1} & 
        {^{0}z_2} &
        \cdots &
        {^{0}z_n}
    \end{bmatrix}
\end{align}
ここで、

\begin{align}
    {^{0}z_i} &\stackrel{\mathrm{def}}{=} {^{0}R_i} e_z \\
    {^{0}p_{E,i}} &\stackrel{\mathrm{def}}{=}
            \sum_{j=i}^{n}{^{0}R_j}{^{j}p_{j,j+1}} \\
    &= {^{0}p_{E,i+1}} + {^{0}R_i}{^{i}p_{i,i+1}}
\end{align}
であり、e_z = [0, 0, 1]^ \mathsf{T}である。

 2. 基礎ヤコビ行列の時間微分の計算基礎ヤコビ行列J_vを時間微分すると以下である。

\begin{align}
    \dot{J}_v = \begin{bmatrix}
        \frac{d}{dt}\left( ^{0}z_1\times ^{0}p_{E,1} \right) & 
        \frac{d}{dt}\left( ^{0}z_2\times ^{0}p_{E,2} \right) & 
        \cdots & 
        \frac{d}{dt}\left( ^{0}z_n\times ^{0}p_{E,n} \right) 
        \\
        \frac{d}{dt}\left( ^{0}z_1\right) & 
        \frac{d}{dt}\left( ^{0}z_2\right) &
        \cdots &
        \frac{d}{dt}\left( ^{0}z_n\right)
    \end{bmatrix}
\end{align}
\frac{d}{dt}\left( ^{0}z_i\times ^{0}p_{E,i} \right) を書き下すと

\begin{align*}
    \frac{d}{dt}\left( ^{0}z_i\times ^{0}p_{E,i} \right) &= 
    \frac{d}{dt}\left( ^{0}z_i\right)\times ^{0}p_{E,i} +
    ^{0}z_i\times\frac{d}{dt}\left(^{0}p_{E,i} \right) \\
    &=
    \frac{\partial ^{0}z_i}{\partial q}\dot{q} \times ^{0}p_{E,i} +
    ^{0}z_i\times \frac{\partial ^{0}p_{E,i}}{\partial q}\dot{q}
\end{align*}
であるため、\frac{\partial ^{0}z_i}{\partial q}および\frac{\partial ^{0}p_{E,i}}{\partial q}の2つを求めれば、ヤコビ行列の時間微分\dot{J}_vが求まる。
まず、\frac{\partial ^{0}z_i}{\partial q}を求める。書き下すと以下である。

\begin{align*}
    \frac{\partial ^{0}z_i}{\partial q_j} &=
    \frac{\partial ^{0}R_i e_z}{\partial q_j}　\\
    &= \begin{cases}
        &^{0}R_{j-1}\left(\frac{\partial {^{j-1}R_{j}}}{\partial q_j}\right) {^{j}R_{i}}e_z &(i \geq j) \\
        &0 \ &(i < j)
    \end{cases}    
\end{align*}
つぎに \frac{\partial ^{0}p_{E,i}}{\partial q}を書き下すと以下である。

\begin{align*}
    \frac{\partial ^{0}p_{E,i}}{\partial q_j} &=
    \frac{\partial \sum_{k=i}^{n}{^{0}R_{k}}{^{k}p_{k,k+1}}}{\partial q_j}　\\
    &= \begin{cases}
        &0 \ &(i > j) \\
        &\sum_{k=i}^{n}{^{0}R_{j-1}}\left(\frac{\partial {^{j-1}R_{j}}}{\partial q_j}\right){^{j}R_{k}}{^{k}p_{k,k+1}} \ &(i \leq j)
    \end{cases}
\end{align*}
ここで、各関節は回転関節であるため、修正DHパラメータ表記すると、固定部の係数行列M(a, \alpha, d)と変動部のz軸回転行列となるため{^{i-1}R_{i}}は次式である。

\begin{align*}
    {^{i-1}R_{i}(q_i)} &= M(a, \alpha, d) \begin{bmatrix}
    \cos(q_i) & -\sin(q_i) & 0 \\
    \sin(q_i) &  \cos(q_i) & 0 \\
    0         &  0         & 1
    \end{bmatrix}
\end{align*}
したがって、{^{i-1}R_{i}}のq_iによる微分は次式となる。

\begin{align*}
    \frac{\partial{^{i-1}R_{i}(q_i)}}{\partial q_i} &= 
    M(a, \alpha, d)\begin{bmatrix}
    -\sin(q_i) & -\cos(q_i) & 0 \\
     \cos(q_i) & -\sin(q_i) & 0 \\
     0         &  0         & 0
    \end{bmatrix}
\end{align*}

 3. 計算の工夫以下のように書き下すと、{^{j}R_n}を作るn-1回の行列積の計算分を省略できるため行列積計算が2(n-1)回となる。

\begin{align}
\frac{\partial{^{0}P_{E,n}}}{\partial q}\dot{q} =&
\Bigg(
    \Bigg(
        \Bigg(
            \Bigg(
            \dot{q}_1\frac{\partial{^{0}R_{1}}}{\partial q_1}{^{1}R_{2}}
            + \dot{q}_2{^{0}R_{1}}\frac{\partial{^{1}R_{2}}}{\partial q_2}
            \Bigg){^{2}R_{3}}
            + \dot{q}_3{^{0}R_{2}}\frac{\partial{^{2}R_{3}}}{\partial q_3}
        \Bigg){^{3}R_{4}}\cdots
    \Bigg){^{n-1}R_{n}} \notag\\
    &+ 
\dot{q}_n{^{0}R_{n-1}}\frac{\partial{^{n-1}R_{n}}}{\partial q_n}
\Bigg)
{^{n}p_{n,E}}
\end{align}
\frac{\partial{^{0}P_{E,n-1}}}{\partial q}\dot{q}の計算に関して、{^{0}p_{E,n-1}}= {^{0}R_{n-1}}{^{n-1}p_{n-1,n}}+{^{n}p_{n,E}}の右辺第2項は計算済みであるため、{^{0}R_{n-1}}{^{n-1}p_{n-1,n}}の部分だけ考える。さきほどと同様に書き下すことができ、行列積計算が2(n-2)回となる。

\begin{align}
\frac{\partial {^{0}R_{n-1}}{^{n-1}p_{n-1,n}}}{\partial q}\dot{q} 
=&
\Bigg(
    \Bigg(
        \Bigg(
        \dot{q}_{1}\frac{\partial{^{0}R_{1}}}{\partial q_{1}}{^{1}R_{2}}
        + \dot{q}_{2}{^{0}R_{1}}\frac{\partial{^{1}R_{2}}}{\partial q_{2}}
        \Bigg){^{2}R_{3}}\cdots
    \Bigg){^{n-2}R_{n-1}} \notag\\
    &+ \dot{q}_{n-1}{^{0}R_{n-2}}\frac{\partial{^{n-2}R_{n-1}}}{\partial q_{n-1}}
\Bigg)
{^{n-1}p_{n-1,n}}
\end{align}
したがって、\frac{\partial{^{0}P_{E,i}}}{\partial q}\dot{q}での計算は行列積計算がn(n-1)回となる。
式(7)、(8)を見ると、同じ計算をしている部分があるため、\frac{\partial {^{0}R_{1}}{^{1}p_{1,2}}}{\partial q}\dot{q}から順に計算することで、\frac{\partial{^{0}P_{E,i}}}{\partial q}\dot{q}での計算は行列積計算が2(n-1)回となる。
中間変数の計算を省略できることで、計算量を削減する効果とメモリ削減、書き込み・読み込み時間の削減の効果が期待できる。
[NOTE]

\frac{\partial{^{0}z_{i}}}{\partial q}\dot{q}でも同じ形の計算がでてくるため、行列積計算は増えない。(以下はEigenライブライを使用したC++での記入例
void RobotKinematics::calc_time_derivative_of_basic_Jacobian_matrix(
    Eigen::MatrixXd& jacobian_matrix_dot) {
    calc_dp_and_dR_from_prev_axis();
    calc_RdR_from_origin_axis();  // calc {0}^R_{i-1}{i-1}^dR_{i}
    //
    for (int i = 0; i < joint_num_; i++) {
        dz_[i] = Eigen::Vector3d::Zero();
        dpe_[i] = Eigen::Vector3d::Zero();
    }
    Eigen::Matrix3d temp_m = Eigen::Matrix3d::Zero();
    Eigen::Vector3d temp_v = Eigen::Vector3d::Zero();
    // calc dz and dpe
    for (int i = 0; i < joint_num_; i++) {
        temp_m *= R_from_prev_axis[i];
        temp_m += dq_[i] * RdR_from_origin_axis[i];
        dz_[i] = temp_m * Eigen::Vector3d::UnitZ();
        temp_v = temp_m * p_from_prev_axis[i + 1];
        for (int j = 0; j <= i; j++) {
            dpe_[j] += temp_v;
        }
    }
    // calc jacobian_matrix_dot
    for (int i = 0; i < joint_num_; i++) {
        jacobian_matrix_dot.block<3, 1>(0, i) =
            dz_[i].cross(pe_from_origin_axis[i]) +
            z_from_origin_axis[i].cross(dpe_[i]);
        jacobian_matrix_dot.block<3, 1>(3, i) = dz_[i];
    }
}

 参考文献細田, アールティ: 実践ロボット制御: 基礎から動力学まで, オーム社, 2019
Discussion

だーらぼん
基礎ヤコビ->ヤコビ行列への変換部分を後日補足すること