【關于四足機器人那些事】雅克比矩陣

一、引入

假設有6個函數，每個函數有6個獨立的變量，即：

y 1 = f 1 ( x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ) y 2 = f 2 ( x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ) ⋮ y 6 = f 6 ( x 1 , x 2 , x 3 , x 4 , x 5 , x 6 ) \begin{matrix} y_1 = f_1(x_1, x_2, x_3, x_4, x_5, x_6)\\ y_2 = f_2(x_1, x_2, x_3, x_4, x_5, x_6)\\ \vdots \\ y_6 = f_6(x_1, x_2, x_3, x_4, x_5, x_6)\\ \end{matrix} y1​=f1​(x1​,x2​,x3​,x4​,x5​,x6​)y2​=f2​(x1​,x2​,x3​,x4​,x5​,x6​)⋮y6​=f6​(x1​,x2​,x3​,x4​,x5​,x6​)​

我們用矢量形式表達上式，即：

Y = F ( X ) Y = F(X) Y=F(X)

現在，如果我們要計算 y i y_i yi​的微分關于 x i x_i xi​的微分的函數，通過多元函數求導法則，可以計算出：

δ y 1 = ∂ f 1 ∂ x 1 δ x 1 + ∂ f 1 ∂ x 2 δ x 2 + ⋯ + ∂ f 1 ∂ x 6 δ x 6 δ y 2 = ∂ f 2 ∂ x 1 δ x 1 + ∂ f 2 ∂ x 2 δ x 2 + ⋯ + ∂ f 2 ∂ x 6 δ x 6 ⋮ δ y 6 = ∂ f 6 ∂ x 1 δ x 1 + ∂ f 6 ∂ x 2 δ x 2 + ⋯ + ∂ f 6 ∂ x 6 δ x 6 \begin{matrix} \delta y_1 = \frac{\partial f_1}{\partial x_1}\delta x_1 + \frac{\partial f_1}{\partial x_2}\delta x_2 + \cdots + \frac{\partial f_1}{\partial x_6}\delta x_6 \\ \\ \delta y_2 = \frac{\partial f_2}{\partial x_1}\delta x_1 + \frac{\partial f_2}{\partial x_2}\delta x_2 + \cdots + \frac{\partial f_2}{\partial x_6}\delta x_6\\ \vdots \\ \delta y_6 = \frac{\partial f_6}{\partial x_1}\delta x_1 + \frac{\partial f_6}{\partial x_2}\delta x_2 + \cdots + \frac{\partial f_6}{\partial x_6}\delta x_6 \end{matrix} δy1​=∂x1​∂f1​​δx1​+∂x2​∂f1​​δx2​+⋯+∂x6​∂f1​​δx6​δy2​=∂x1​∂f2​​δx1​+∂x2​∂f2​​δx2​+⋯+∂x6​∂f2​​δx6​⋮δy6​=∂x1​∂f6​​δx1​+∂x2​∂f6​​δx2​+⋯+∂x6​∂f6​​δx6​​

同樣，我們以矢量形式表示：

δ Y = ∂ F ∂ X δ X (1) \delta Y = \frac{\partial{F}}{\partial{X}}\delta X \tag{1} δY=∂X∂F​δX(1)

式（1）中的 ∂ F ∂ X \frac{\partial{F}}{\partial{X}} ∂X∂F​為 6 × 6 6\times6 6×6偏導數矩陣。它，就是我們所說的雅克比矩陣 J J J

速度映射

如果 f 1 ( X ) f_1(X) f1​(X)到 f 6 ( X ) f_6(X) f6​(X)都是非線性函數，那麼，這些偏導數都是關于 x i x_i xi​的函數，我們可以用以下式子表達：

δ Y = J ( X ) δ X (2) \delta Y = J(X) \delta X \tag{2} δY=J(X)δX(2)

式（2）兩邊同時除以時間微分 d t d_t dt​，我們就可以将雅克比矩陣看作是 X X X中的速度映射為 Y Y Y中的速度：

Y ˙ = J ( X ) X ˙ \dot{Y} = J(X)\dot X Y˙=J(X)X˙

在任一瞬間，X都有一個确定的值， J ( X ) J(X) J(X)是個線性變換，在每一新時刻，如果X發生改變， J ( X ) J(X) J(X)也會發生改變

在機器人學中，通常使用雅克比将關節速度與操作臂末端的速度聯系起來：

v = J ( θ ) Θ ˙ (3) v = J(\theta)\dot \Theta \tag 3 v=J(θ)Θ˙(3)

其中， Θ \Theta Θ為關節角組成的向量， v v v為速度向量。對于6關節機械臂，雅克比矩陣為 J ∈ R 6 × 6 J \in \mathbb{R}^{6\times6} J∈R6×6，關節角 Θ ∈ R 6 × 1 \Theta \in \mathbb{R}^{6\times1} Θ∈R6×1，速度 v ∈ R 6 × 1 v \in \mathbb{R}^{6\times1} v∈R6×1，而 v v v是由一個 3 × 1 3\times1 3×1的線速度和一個 3 × 1 3\times1 3×1的角速度所組成，表達為：

v = [ v ω ] v = \begin{bmatrix} v\\ \omega \end{bmatrix} v=[vω​]

對于兩連杆機構，如下圖，我們很容易寫出它的關節速度(世界坐标系下)與執行器末端速度的關系：

v w = [ − l 1 s 1 θ ˙ 1 − l 2 s 12 ( θ ˙ 1 + θ ˙ 2 ) l 1 c 1 θ ˙ 1 + l 2 c 12 ( θ ˙ 1 θ ˙ 2 ) 0 ] = [ − l 1 s 1 − l 2 s 12 − l 2 s 12 l 1 c 1 + l 2 c 12 l 2 c 12 0 0 0 ] [ θ ˙ 1 θ ˙ 2 0 ] v_w = \begin{bmatrix} -l_1s_1\dot\theta_1-l_2s_{12}(\dot \theta_1 + \dot \theta_2)\\ l_1c_1\dot \theta_1 +l_2c_{12}(\dot \theta_1 \dot \theta_2)\\ 0 \end{bmatrix} = \begin{bmatrix} -l_1s_1-l_2s_{12} & -l_2s_{12}\\ l_1c_1 + l_2c_{12} & l_2c_{12} \\ 0 &0&0 \end{bmatrix} \begin{bmatrix} \dot \theta_1\\ \dot \theta_2 \\ 0 \end{bmatrix} vw​=⎣⎡​−l1​s1​θ˙1​−l2​s12​(θ˙1​+θ˙2​)l1​c1​θ˙1​+l2​c12​(θ˙1​θ˙2​)0​⎦⎤​=⎣⎡​−l1​s1​−l2​s12​l1​c1​+l2​c12​0​−l2​s12​l2​c12​0​0​⎦⎤​⎣⎡​θ˙1​θ˙2​0​⎦⎤​

以及相對于執行器末端的速度：

v b = [ l 1 s 2 θ ˙ 1 l 1 c 2 θ ˙ 1 + l 2 ( θ ˙ 1 + θ ˙ 2 ) 0 ] = [ l 1 s 2 0 0 l 1 c 2 + l 2 l 2 0 0 0 0 ] [ θ ˙ 1 θ ˙ 2 0 ] v_b = \begin{bmatrix} l_1s_2 \dot \theta_1 \\ l_1c_2\dot \theta_1 + l_2(\dot \theta_1 +\dot \theta_2) \\ 0 \end{bmatrix} = \begin{bmatrix} l_1s_2 & 0 &0\\ l_1c_2+l_2 & l_2 &0 \\ 0& 0&0 \end{bmatrix} \begin{bmatrix} \dot \theta_1\\ \dot \theta_2 \\ 0 \end{bmatrix} vb​=⎣⎡​l1​s2​θ˙1​l1​c2​θ˙1​+l2​(θ˙1​+θ˙2​)0​⎦⎤​=⎣⎡​l1​s2​l1​c2​+l2​0​0l2​0​000​⎦⎤​⎣⎡​θ˙1​θ˙2​0​⎦⎤​

是以，我們能夠算出世界坐标系的雅克比矩陣為：

J w ( Θ ) = [ − l 1 s 1 − l 2 s 12 − l 2 s 12 l 1 c 1 + l 2 c 12 l 2 c 12 ] J_w(\Theta) = \begin{bmatrix} -l_1s_1-l_2s_{12} & -l_2s_{12}\\ l_1c_1 + l_2c_{12} & l_2c_{12} \end{bmatrix} Jw​(Θ)=[−l1​s1​−l2​s12​l1​c1​+l2​c12​​−l2​s12​l2​c12​​]

末端執行器坐标系下的雅克比：

J b ( Θ ) = [ l 1 s 2 0 l 1 c 2 + l 2 l 2 ] J_b(\Theta) = \begin{bmatrix} l_1s_2 & 0\\ l_1c_2+l_2 & l_2 \end{bmatrix} Jb​(Θ)=[l1​s2​l1​c2​+l2​​0l2​​]