[足式机器人]Part4 南科大高等机器人控制课 Ch07 Velocity Kinematics: Geometric and Analytic Jacobian of Open Chain

本文仅供学习使用
本文参考：
B站：CLEAR_LAB
笔者带更新-运动学
课程主讲教师：
Prof. Wei Zhang

---

1. Background

1.1 Velocity Kinematics

• Velocity Kinematics : How does the velocity of $\left\{b\right\}$ relate to the joint velocities ${\dot{\theta }}_1,\cdots ,{\dot{\theta }}_{\mathrm{n}}$ (Note : $\left\{b\right\}$'s velocity is due to joint velocity)

FK: Find the func of $\left[T_b\left({\theta }_1,\cdots ,{\theta }_{\mathrm{n}}\right)\right]$
Result: $\left[T_b\left({\theta }_1,\cdots ,{\theta }_{\mathrm{n}}\right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}{e}^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}\cdots e^{\left[{\bar{\mathcal{S}}}_{\mathrm{n}}^0\right]{\theta }_{\mathrm{n}}}\left[M\right]$ —— meaning screw axis $i$ when ${\theta }_{\mathrm{i}}=0$ wrt to frame $\left\{0\right\}$

Can we use $\left[{\dot{T}}_b\right]$ to represent velocity of $\left\{b\right\}$? —— NO- [ T ˙ b ] ∣ 4 × 4 \left. \left[ \dot{T}_b \right] \right|_{4\times 4} [T˙b​] ​4×4​

• This depends on how to represent $\left\{b\right\}$'s velocity

1. Twist representation $⟶$ Geometric Jacobian
   ${\mathcal{V}}_b=\left[\begin{array}{c}\omega \\ v\end{array}\right],{\mathcal{V}}_b\left(\theta ,\dot{\theta }\right)$ , it turns out , ${\mathcal{V}}_b$ is a linear func of $\dot{\theta }$ , $\Rightarrow {\mathcal{V}}_b\left(\theta ,\dot{\theta }\right)=J\left(\theta \right)\dot{\theta },J\left(\theta \right)\in {\mathbb{R}}^{6\times n}$
   This matrix is the Geometric
2. Local coordinate of SE(3) $⟶$ Analytic Jacobian
   θ 1 , ⋯   , θ n ⟶ F K [ T b ( θ 1 , ⋯   , θ n ) ] = [ T b ( [ Q ] , R ⃗ p ) ] ⟶ [ p x p y p z α β γ ] ∈ R 6 \theta _1,\cdots ,\theta _{\mathrm{n}}\overset{FK}{\longrightarrow}\left[ T_b\left( \theta _1,\cdots ,\theta _{\mathrm{n}} \right) \right] =\left[ T_b\left( \left[ Q \right] ,\vec{R}_{\mathrm{p}} \right) \right] \longrightarrow \left[ \begin{array}{l} p_{\mathrm{x}}\\ p_{\mathrm{y}}\\ p_{\mathrm{z}}\\ \alpha\\ \beta\\ \gamma\\ \end{array} \right] \in \mathbb{R} ^6 θ1​,⋯,θn​⟶FK​[Tb​(θ1​,⋯,θn​)]=[Tb​([Q],R p​)]⟶ ​px​py​pz​αβγ​ ​∈R6
   ⇒ θ 1 , ⋯   , θ n ⟶ g ( ⋅ ) [ p x p y p z α β γ ] ⇒ x = g ( θ 1 , ⋯   , θ n ) ⇒ x ˙ = [ ∂ g ∂ θ ] ∣ 6 × n [ θ ˙ 1 ⋮ θ ˙ n ]    , [ ∂ g ∂ θ ] i j = [ ∂ g i ∂ θ j ] \Rightarrow \theta _1,\cdots ,\theta _{\mathrm{n}}\overset{g\left( \cdot \right)}{\longrightarrow}\left[ \begin{array}{l} p_{\mathrm{x}}\\ p_{\mathrm{y}}\\ p_{\mathrm{z}}\\ \alpha\\ \beta\\ \gamma\\ \end{array} \right] \Rightarrow x=g\left( \theta _1,\cdots ,\theta _{\mathrm{n}} \right) \Rightarrow \dot{x}=\left. \left[ \frac{\partial g}{\partial \theta} \right] \right|_{6\times n}\left[ \begin{array}{c} \dot{\theta}_1\\ \vdots\\ \dot{\theta}_{\mathrm{n}}\\ \end{array} \right] \,\,, \left[ \frac{\partial g}{\partial \theta} \right] _{\mathrm{ij}}=\left[ \frac{\partial g_{\mathrm{i}}}{\partial \theta _{\mathrm{j}}} \right] ⇒θ1​,⋯,θn​⟶g(⋅)​ ​px​py​pz​αβγ​ ​⇒x=g(θ1​,⋯,θn​)⇒x˙=[∂θ∂g​] ​6×n​ ​θ˙1​⋮θ˙n​​ ​,[∂θ∂g​]ij​=[∂θj​∂gi​​]

$\left[\frac{\partial g}{\partial \theta }\right]$ 称为 Analytic Jacobian

1.2 Simple Illustration Example: Geometric Jacobian

• Coordinate-free
  screw axis: Joint 1 ${\mathcal{S}}_1$-independs of ${\theta }_1,{\theta }_2$ ; Joint 2 ${\mathcal{S}}_2\left({\theta }_1\right)$ depends on ${\theta }_1$
  Spatial velocity of each link (when ${\dot{\theta }}_1,{\dot{\theta }}_2$)
  1.Link 0 : ${\mathcal{V}}_{{\mathrm{L}}_0}=0\in {\mathbb{R}}^6$
  2.Link 1 : ${\mathcal{V}}_{{\mathrm{L}}_1}={\mathcal{S}}_1{\dot{\theta }}_1$
  3.Link 2 : ${\mathcal{V}}_{{\mathrm{L}}_2}\to {\mathcal{V}}_{{\mathrm{L}}_2/{\mathrm{L}}_0}={\mathcal{V}}_{{\mathrm{L}}_2/{\mathrm{L}}_1}+{\mathcal{V}}_{{\mathrm{L}}_1/{\mathrm{L}}_0}={\mathcal{S}}_2{\dot{\theta }}_2+{\mathcal{S}}_1{\dot{\theta }}_1=\left[\begin{array}{cc}{\mathcal{S}}_1& {\mathcal{S}}_2\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$

• $\left\{b\right\}$
  ${\mathcal{V}}_{\mathrm{b}}={\mathcal{V}}_{{\mathrm{L}}_2}=\left[\begin{array}{cc}{\mathcal{S}}_1& {\mathcal{S}}_2\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]=\left[\begin{array}{cc}{J}_1\left(\theta \right)& J_2\left(\theta \right)\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$
  $J_1\left(\theta \right)$ 1st-column of Geomatric Jacobian; $J_2\left(\theta \right)$ 2st-column of Geomatric Jacobian
  $J_{\mathrm{i}}\left(\theta \right)$ : the twist of $\left\{b\right\}$ when ${\theta }_{\mathrm{i}}=1,{\theta }_{\mathrm{j}}=0,i\ne j$

• Computation : Let’s work with $\left\{o\right\}$
  S 1 0 ( θ ) = S 1 0 ( θ = 0 ) = S ˉ 1 0 = [ 0 0 1 0 0 0 ] \mathcal{S} _{1}^{0}\left( \theta \right) =\mathcal{S} _{1}^{0}\left( \theta =0 \right) =\bar{\mathcal{S}}_{1}^{0}=\left[ \begin{array}{l} 0\\ 0\\ 1\\ 0\\ 0\\ 0\\ \end{array} \right] S10​(θ)=S10​(θ=0)=Sˉ10​= ​001000​ ​
  S 2 0 ( θ 1 ) = S 2 0 ( θ 1 = 0 ) = S ˉ 2 0 = [ 0 0 1 0 − L 1 0 ] \mathcal{S} _{2}^{0}\left( \theta _1 \right) =\mathcal{S} _{2}^{0}\left( \theta _1=0 \right) =\bar{\mathcal{S}}_{2}^{0}=\left[ \begin{array}{l} 0\\ 0\\ 1\\ 0\\ -L_1\\ 0\\ \end{array} \right] S20​(θ1​)=S20​(θ1​=0)=Sˉ20​= ​0010−L1​0​ ​
  Let’s ${\theta }_1\ne 0$ : ${\bar{\mathcal{S}}}_2^0={\mathcal{S}}_2^0\left(0\right)\stackrel{\left[T\left({\theta }_1\right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}}{⟶}{\mathcal{S}}_2^0\left({\theta }_1\right)=\left[A{d}_{\left[T\left({\theta }_1\right)\right]}\right]{\bar{\mathcal{S}}}_2^0$

$$\Rightarrow J^0\left(\theta \right)=\left[\begin{array}{cc}{\bar{\mathcal{S}}}_1^0& \left[A{d}_{\left[T\left({\theta }_1\right)\right]}\right]{\bar{\mathcal{S}}}_2^0\end{array}\right]$$

2. Geometric Jacobian Derivations

2.1 Geometric Jacobian : General Case

• Let $\mathcal{V}=\left(\omega ,v\right)$ be the end-effector twist(coordinate-free notation), we aim to find $J\left(\theta \right)$ such that :
  V = J ( θ ) θ ˙ = J 1 ( θ ) θ ˙ 1 + ⋯ + J n ( θ ) θ ˙ n = [ J 1 ( θ ) ⋯ J n ( θ ) ] [ θ ˙ 1 ⋮ θ ˙ n ] , J i ( θ ) : θ ˙ i = 1 , θ ˙ j = 0 , i ≠ j \mathcal{V} =J\left( \theta \right) \dot{\theta}=J_1\left( \theta \right) \dot{\theta}_1+\cdots +J_{\mathrm{n}}\left( \theta \right) \dot{\theta}_{\mathrm{n}}=\left[ \begin{matrix} J_1\left( \theta \right)& \cdots& J_{\mathrm{n}}\left( \theta \right)\\ \end{matrix} \right] \left[ \begin{array}{c} \dot{\theta}_1\\ \vdots\\ \dot{\theta}_{\mathrm{n}}\\ \end{array} \right] ,J_{\mathrm{i}}\left( \theta \right) :\dot{\theta}_{\mathrm{i}}=1,\dot{\theta}_{\mathrm{j}}=0,i\ne j V=J(θ)θ˙=J1​(θ)θ˙1​+⋯+Jn​(θ)θ˙n​=[J1​(θ)​⋯​Jn​(θ)​] ​θ˙1​⋮θ˙n​​ ​,Ji​(θ):θ˙i​=1,θ˙j​=0,i=j

• The $i$th column $J_{\mathrm{i}}\left(\theta \right)$ is the end-effector velocity when the robot is rotating about ${\mathcal{S}}_{\mathrm{i}}$ at unit speed ${\dot{\theta }}_{\mathrm{i}}=1$ while all other joints do not move (${\dot{\theta }}_{\mathrm{j}}=0,i\ne j$)

• Therefore, in coordinate free notation, $J_{\mathrm{i}}$ is just the screw axis of joint $i$ :
  $$J_{\mathrm{i}}\left(\theta \right)={\mathcal{S}}_{\mathrm{i}}\left(\theta \right)$$

• The simplest way to write Jacobian is to use local coordinate:
  $$J_{\mathrm{i}}^i={\mathcal{S}}_{\mathrm{i}}^i,i=1,\cdots ,n,J^0=\left[J_1^0,J_2^0,\cdots ,J_{\mathrm{n}}^0\right]$$

• In fixed frame $\left\{0\right\}$ , we have
  $$J_{\mathrm{i}}^0=\left[X_{\mathrm{i}}^0\left(\theta \right)\right]{\mathcal{S}}_{\mathrm{i}}^i,i=1,\cdots ,n$$
  Recall : $\left[X_{\mathrm{i}}^0\right]$ is the change of coordinate matrix for spatial velocities.

• Assume $\theta =\left({\theta }_1,{\theta }_2\cdots ,{\theta }_{\mathrm{n}}\right)$ then (pose of frame $\left\{i\right\}$ relative to $\left\{0\right\}$)
  $$\left[T_{\mathrm{i}}^0\left(\theta \right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}{e}^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}\cdots e^{\left[{\bar{\mathcal{S}}}_{\mathrm{i}}^0\right]{\theta }_{\mathrm{i}}}\left[M\right]\Rightarrow \left[X_{\mathrm{i}}^0\left(\theta \right)\right]=\left[A{d}_{\left[T_{\mathrm{i}}^0\left(\theta \right)\right]}\right]$$

The Jacobian formula above is conceptually simple, but can be cumbersome for calculation. We now derive a recursive Jacobian formula

Note : $J_{\mathrm{i}}^0\left(\theta \right)={\mathcal{S}}_{\mathrm{i}}^0\left(\theta \right)$

• For $i=1$, ${\mathcal{S}}_1^0\left(\theta \right)={\mathcal{S}}_1^0\left(0\right)={\bar{\mathcal{S}}}_1^0$ (independent of $\theta$)
• For $i=2$, ${\mathcal{S}}_2^0\left(\theta \right)={\mathcal{S}}_2^0\left({\theta }_1\right)=\left[A{d}_{\left[T\left({\theta }_1\right)\right]}\right]{\bar{\mathcal{S}}}_2^0$ where $\left[T\left({\theta }_1\right)\right]\triangleq e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}$
• For general $i$, we have
  $$J_{\mathrm{i}}^0\left(\theta \right)={\mathcal{S}}_{\mathrm{i}}^0\left(\theta \right)=\left[A{d}_{\left[T_{\mathrm{i}}^0\left({\theta }_1,\cdots ,{\theta }_{\mathrm{i}-1}\right)\right]}\right]{\bar{\mathcal{S}}}_{\mathrm{i}}^0$$
  where : $\left[T_{\mathrm{i}}^0\left({\theta }_1,\cdots ,{\theta }_{\mathrm{i}-1}\right)\right]\triangleq e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}\cdots e^{\left[{\bar{\mathcal{S}}}_{\mathrm{i}-1}^0\right]{\theta }_{\mathrm{i}-1}}$

2.2 Geometric Jacobian Example

3. Analytic Jacobian

• Let $x\in {\mathbb{R}}^p$ be the task space variable of interest with desired reference $x_d$
  E.g : $x$ can be Cartesian(spherical coordinate) + Euler angle(ZYZ ZYX) of end-effctor frame
  $p<6$ os allowed, which means a partial parameterization of SE(3), e.g. we only care about the position or the orientation of the end-effector frame

• Analytic Jacobian : $x=g\left(\theta \right),\dot{x}=J_{\mathrm{a}}\left(\theta \right)\dot{\theta },J_{\mathrm{a}}\left(\theta \right)=\frac{\partial g}{\partial \theta }$

• Recall Geometric Jacobian : $\mathcal{V}=\left[\begin{array}{c}\omega \\ v\end{array}\right]=J\left(\theta \right)\dot{\theta }$

• They are related by : $J_{\mathrm{a}}\left(\theta \right)=E\left(x\right)J\left(\theta \right)=E\left(\theta \right)J\left(\theta \right)$
  $E\left(x\right)$ can be found with given parameterization $x$