[足式机器人]Part4 南科大高等机器人控制课 Ch06 Product of Exponential and Kinematics of Open Chain

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

---

1. Motivating Example

1.1 Kinematics

• Kinematics is a branch of classical mechanics that describes the motion of points, bodies(objects), and systems of bodies(groups of objects) without considering the mass of each or the forces that caused the motion

• Robot : Multiple rigid bodies interconnected through joints
  

1. Forward Kinematics: calculation of the configuration $\left[T\right]=\left(\left[Q\right],R\right)$ of the end-effector frame from joint variables $\theta =\left({\theta }_1,\cdots {\theta }_{\mathrm{n}}\right)$
2. Velocity Kinematics(Next Lecture): Deriving the Jacobian matrix: linearized map fro the joint velocities $\dot{\theta }$ to the spatial velocity $\mathcal{V}$ of the end-effector

1.2 Illustration Example

Consider a 2R robot:

• Three links and two joints ${\theta }_1,{\theta }_2$

• Link/body frame attached to link $i$ at joint $i$ (one of possible choices)

• Fixed/world frame $\left\{s\right\}$ frame, end-effector frame $\left\{b\right\}$

• Goal : compute $\left[T_{\mathrm{B}}^S\left({\theta }_1,{\theta }_2\right)\right]$ : function of ${\theta }_1,{\theta }_2$

• Initial pose: $\left[M\right]=\left[T_{\mathrm{B}}^S\left(0,0\right)\right]$
  
  [ M ] = [ T B S ( 0 , 0 ) ] = [ 1 0 0 L 1 + L 2 0 1 0 0 0 0 1 L 0 0 0 0 1 ] \left[ M \right] =\left[ T_{\mathrm{B}}^{S}\left( 0,0 \right) \right] =\left[ \begin{matrix} 1& 0& 0& L_1+L_2\\ 0& 1& 0& 0\\ 0& 0& 1& L_0\\ 0& 0& 0& 1\\ \end{matrix} \right] [M]=[TBS​(0,0)]= ​1000​0100​0010​L1​+L2​0L0​1​ ​

• Fixed joint 1 at ${\theta }_1=0$, rotate about joint 2 by ${\theta }_2$ , we have $T_{\mathrm{B}}^S\left(0,{\theta }_2\right)$
  Rigid body motion for Link2/$\left\{b\right\}$ , represented by screw motion
  In coordinate-free way : $\underset{{\theta }_1=0,{\theta }_2=0}{\left[M\right]}\stackrel{\left[T\right]=e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}}{⟶}\left[T\right]\left[M\right]=e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}\left[M\right]$
  In $\left\{s\right\}$ \ $\left\{o\right\}$ frame $\left[T_{\mathrm{B}}^S\left(0,{\theta }_2\right)\right]=e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}\left[T_{\mathrm{B}}^S\left(0,0\right)\right]$
  ${\mathcal{S}}_2^0$ is a function of ${\theta }_1$ , more precisely ${\mathcal{S}}_2^0\left({\theta }_1\right)$
  Now ${\theta }_1=0$, define S ˉ 2 0 = S 2 0 ( 0 ) , S ˉ 2 0 = [ ω ⃗ 2 0 v ⃗ 2 0 ] , ω ⃗ 2 0 = [ 0 0 1 ] \bar{\mathcal{S}}_{2}^{0}=\mathcal{S} _{2}^{0}\left( 0 \right) ,\bar{\mathcal{S}}_{2}^{0}=\left[ \begin{array}{c} \vec{\omega}_{2}^{0}\\ \vec{v}_{2}^{0}\\ \end{array} \right] ,\vec{\omega}_{2}^{0}=\left[ \begin{array}{c} 0\\ 0\\ 1\\ \end{array} \right] Sˉ20​=S20​(0),Sˉ20​=[ω 20​v 20​​],ω 20​= ​001​ ​
  h = 0 , v ⃗ 2 0 = − ω ⃗ 2 0 × R ⃗ 2 0 = [ 0 0 − 1 ] × [ L 1 0 0 ] = [ 0 − L 1 0 ] h=0,\vec{v}_{2}^{0}=-\vec{\omega}_{2}^{0}\times \vec{R}_{2}^{0}=\left[ \begin{array}{c} 0\\ 0\\ -1\\ \end{array} \right] \times \left[ \begin{array}{c} L_1\\ 0\\ 0\\ \end{array} \right] =\left[ \begin{array}{c} 0\\ -L_1\\ 0\\ \end{array} \right] h=0,v 20​=−ω 20​×R 20​= ​00−1​ ​× ​L1​00​ ​= ​0−L1​0​ ​
  ⇒ S ˉ 2 0 = [ 0 0 1 0 − L 1 0 ] ⇒ [ T B S ( 0 , θ 2 ) ] = e [ S ˉ 2 0 ] θ 2 [ M ] \Rightarrow \bar{\mathcal{S}}_{2}^{0}=\left[ \begin{array}{l} 0\\ 0\\ 1\\ 0\\ -L_1\\ 0\\ \end{array} \right] \Rightarrow \left[ T_{\mathrm{B}}^{S}\left( 0,\theta _2 \right) \right] =e^{\left[ \bar{\mathcal{S}}_{2}^{0} \right] \theta _2}\left[ M \right] ⇒Sˉ20​= ​0010−L1​0​ ​⇒[TBS​(0,θ2​)]=e[Sˉ20​]θ2​[M]

• Fix joint 2 at ${\theta }_2$ , and rotate joint 1 by ${\theta }_1$ $\left[T_{\mathrm{B}}^S\left({\theta }_1,{\theta }_2\right)\right]$
  rigidi body motion $\left[T_{\mathrm{B}}^S\left({\theta }_1,{\theta }_2\right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}\left[T_{\mathrm{B}}^S\left(0,{\theta }_2\right)\right]$
  ${\bar{\mathcal{S}}}_1^0$ indepent of ${\theta }_1,{\theta }_2$ , S ˉ 1 0 = [ ω ⃗ 1 0 v ⃗ 1 0 ] = [ 0 0 1 0 0 0 ] \bar{\mathcal{S}}_{1}^{0}=\left[ \begin{array}{c} \vec{\omega}_{1}^{0}\\ \vec{v}_{1}^{0}\\ \end{array} \right] =\left[ \begin{array}{l} 0\\ 0\\ 1\\ 0\\ 0\\ 0\\ \end{array} \right] Sˉ10​=[ω 10​v 10​​]= ​001000​ ​

$\Rightarrow \left[T_{\mathrm{B}}^S\left({\theta }_1,{\theta }_2\right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}{e}^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}\left[M\right]$—— product of exp

1.3 Noation Setup

Suppose that the robot has $n$ joints and $n$ links. Each joint has one degree of freedom represented bt joint variable ${\theta }_{\mathrm{i}},i=1,\cdots ,n$ (${\theta }_{\mathrm{i}}$ : the joint angle - Revolute Joint or joint placement - Primatic Joint )

Specify a fixed frame $\left\{s\right\}$ ： also referred to as frame $\left\{0\right\}$

Attach frame $\left\{i\right\}$ to link $i$ at joint $i$, for $i=1,\cdots ,n$

Attach frame $\left\{b\right\}$ at the end-effector : sometimes referred to as frame $\left\{n+1\right\}$

${\mathcal{S}}_{\mathrm{i}}^i$ : screw axis of joint $i$ expressed in frame $\left\{i\right\}$ (local coordinate of ${\mathcal{S}}_{\mathrm{i}}$)

${\mathcal{S}}_{\mathrm{i}}^0$ : screw axis of joint $i$ expressed in the fixed frame $\left\{0\right\}$ (i.e. frame $\left\{s\right\}$)

For simplicity, we write configuration as $\left[T_{\mathrm{SB}}\right]$ , which is the same as $\left[T_{\mathrm{B}}^S\right]$. Similarly $\left[T_{\mathrm{ij}}\right]=\left[T_{\mathrm{j}}^i\right]$

Note : ${\mathcal{S}}_{\mathrm{i}}^i$ does not change when the robot moves (i.e. when $\theta$ changes), but ${\mathcal{S}}_{\mathrm{i}}^0$ depends on ${\theta }_1,\cdots ,{\theta }_{\mathrm{i}}$. Sometimes, we write out the dependenct explicitly i.e. ${\mathcal{S}}_{\mathrm{i}}^0\left({\theta }_1,\cdots ,{\theta }_{\mathrm{i}}\right)$

Define home position : ${\theta }_1=0,\cdots ,{\theta }_{\mathrm{i}}=0$. This is the configuration when all the joint angles are zero. One can also choose other fixed angles as the home position.

Define ${\bar{\mathcal{S}}}_{\mathrm{i}}^0={\mathcal{S}}_{\mathrm{i}}^0\left(0,\cdots ,0\right)$ : the screw axis of joint $i$ expressed in frame $\left\{0\right\}$ , when the robot is at the home position

2. Product of Exponential Formula Derivations

2.1 Product of Exponential : Main Idea

• Goal : Derive $\left[T_{\mathrm{SB}}\left({\theta }_1,\cdots ,{\theta }_{\mathrm{n}}\right)\right]$
• (Step 1) Compute $\left[M\right]\triangleq \left[T_{\mathrm{SB}}\left(0,\cdots ,0\right)\right]$ : configuration of end-effector when the robot is at home position
• (Step 2) Apply screw motion to joint $n$ : $\left[T_{\mathrm{SB}}\left(0,\cdots ,0,{\theta }_{\mathrm{n}}\right)\right]=e^{\left[{\bar{\mathcal{S}}}_{\mathrm{n}}^0\right]{\theta }_{\mathrm{n}}}\left[M\right]$(this operator does not move ${\mathcal{S}}_{\mathrm{n}-1},{\mathcal{S}}_{\mathrm{n}-2}$)
• (Step 3) Apply screw motion to joint $n-1$ to obtain : $\left[T_{\mathrm{SB}}\left(0,\cdots ,0,{\theta }_{\mathrm{n}-1},{\theta }_{\mathrm{n}}\right)\right]=e^{\left[{\bar{\mathcal{S}}}_{\mathrm{n}-1}^0\right]{\theta }_{\mathrm{n}-1}}{e}^{\left[{\bar{\mathcal{S}}}_{\mathrm{n}}^0\right]{\theta }_{\mathrm{n}}}\left[M\right]$
• After $n$ screw motions, the overall forward kinematics:
  $$\left[T_{\mathrm{SB}}\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]$$

2.2 PoE : Screw Motion in Different Order

• PoE was obtained by applying screw motions along screw axes ${\bar{\mathcal{S}}}_{\mathrm{n}-1}^0,{\bar{\mathcal{S}}}_{\mathrm{n}}^0,\cdots$ What happens if the order is changed?
  

• For simplicity, assume that $n=2$, and let us apply screw motion along ${\bar{\mathcal{S}}}_1^0$ first
  $\left[T_{\mathrm{SB}}\left({\theta }_1,0\right)\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}\left[M\right]$
  Now screw axis for joint 2 has been changed. The new axis ${\mathcal{S}}_2^0={\mathcal{S}}_2^0\left({\theta }_1,0\right)\ne {\bar{\mathcal{S}}}_2^0$
  $\left[T_{\mathrm{SB}}\left({\theta }_1,{\theta }_2\right)\right]=e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}\left[T_{\mathrm{SB}}\left({\theta }_1,0\right)\right]$
  ${\bar{\mathcal{S}}}_2^0\stackrel{\left[T\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}}{⟶}{\mathcal{S}}_2^0{\left({\theta }_1\right)}_{6\times 1}={\left[A{d}_T\right]}_{6\times 6}{{\bar{\mathcal{S}}}_2^0}_{6\times 1},\left[T\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}$

• $\left[T_{\mathrm{SB}}\left({\theta }_1,{\theta }_2\right)\right]=e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}\left[T_{\mathrm{SB}}\left({\theta }_1,0\right)\right]$
  $e^{\left[{\mathcal{S}}_2^0\right]{\theta }_2}=e^{\left[\left[A{d}_T\right]{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}=e^{T\left[{\bar{\mathcal{S}}}_2^0\right]T^{-1}{\theta }_2}=\left[T\right]e^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}{\left[T\right]}^{-1}\stackrel{\left[T\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}}{\Rightarrow }\left[T_{\mathrm{SB}}\left({\theta }_1,{\theta }_2\right)\right]=\left[T\right]e^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}{\left[T\right]}^{-1}{e}^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}\left[M\right]=e^{\left[{\bar{\mathcal{S}}}_1^0\right]{\theta }_1}{e}^{\left[{\bar{\mathcal{S}}}_2^0\right]{\theta }_2}\left[M\right]$

2.3 PoE Example: 3R Spatial Open Chain

3. Practice Example

代码-待补充