[足式机器人]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 { b } \left\{ b \right\} {b} relate to the joint velocities ${\dot{\theta }}_1,\cdots ,{\dot{\theta }}_{\mathrm{n}}$ (Note : { b } \left\{ b \right\} {b}'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 { 0 } \left\{ 0 \right\} {0}

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

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

1. Twist representation $⟶$ Geometric Jacobian
   ${\mathcal{V}}_b=\left[\begin{array}{c}\vec{\omega }\\ \vec{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
   ${\theta }_1,\cdots ,{\theta }_{\mathrm{n}}\stackrel{FK}{⟶}\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]⟶\left[\begin{array}{l}{p}_{\mathrm{x}}\\ p_{\mathrm{y}}\\ p_{\mathrm{z}}\\ \alpha \\ \beta \\ \gamma \end{array}\right]\in {\mathbb{R}}^6$
   $\Rightarrow {\theta }_1,\cdots ,{\theta }_{\mathrm{n}}\stackrel{g\left(\cdot \right)}{⟶}\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[\frac{\partial g}{\partial \theta }\right]|}_{6\times n}\left[\begin{array}{c}{\dot{\theta }}_1\\ ⋮\\ {\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]$

$\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]$

• { b } \left\{ b \right\} {b}
  ${\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 { b } \left\{ b \right\} {b} when ${\theta }_{\mathrm{i}}=1,{\theta }_{\mathrm{j}}=0,i\ne j$

• Computation : Let’s work with { o } \left\{ o \right\} {o}
  ${\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]$
  ${\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]$
  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(\vec{\omega },\vec{v}\right)$ be the end-effector twist(coordinate-free notation), we aim to find $J\left(\theta \right)$ such that :
  $$\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{array}{ccc}{J}_1\left(\theta \right)& \cdots & J_{\mathrm{n}}\left(\theta \right)\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_1\\ ⋮\\ {\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$$

• 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 { 0 } \left\{ 0 \right\} {0} , 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 { i } \left\{ i \right\} {i} relative to { 0 } \left\{ 0 \right\} {0})
  $$\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}\vec{\omega }\\ \vec{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$