[足式机器人]Part4 南科大高等机器人控制课 Ch03 Operator View of Rigid-Body Transformation

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

---

1. Rotation Operation via Differential Equation

1.1 Skew Symmetric Matrices

Recall that cross product is a special linear transformation.
For any $\omega \in {\mathbb{R}}^3$, there is a matrix $\tilde{\omega }\in {\mathbb{R}}^{3\times 3}$ such that $\omega \times R=\tilde{\omega }R$
ω ⃗ = [ ω 1 ω 2 ω 3 ] ⟷ ω ⃗ ~ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] \vec{\omega}=\left[ \begin{array}{c} \omega _1\\ \omega _2\\ \omega _3\\ \end{array} \right] \longleftrightarrow \tilde{\vec{\omega}}=\left[ \begin{matrix} 0& -\omega _3& \omega _2\\ \omega _3& 0& -\omega _1\\ -\omega _2& \omega _1& 0\\ \end{matrix} \right] ω = ​ω1​ω2​ω3​​ ​⟷ω ~= ​0ω3​−ω2​​−ω3​0ω1​​ω2​−ω1​0​ ​
Note that $\tilde{a}=-{\tilde{a}}^{\mathrm{T}}$ (called skew stmmetric)
$\tilde{\omega }$ is called a skew-symmetric matrix representation of the vector $\omega$
The set of skew-symmetric matrices in : $so\left(n\right)\triangleq \left\{S\in {\mathbb{R}}^{n\times n}:S^{\mathrm{T}}=-S\right\}$
We are interested in case $n=2,3$

Rotation matrix $\in SO\left(3\right)\left\{R^{\mathrm{T}}R=E,\det \left(R\right)=1\right\}$

1.2 Rotation Operation via Differential Equation

• Consider a point initially located at $R_{p_0}$ at time $t=0$
• Rotate the point with unit angular velocity $\hat{\omega }$. Assuming the rotation axis passing through the origin, the motion is describe by
  $${\dot{R}}_p\left(t\right)=\hat{\omega }\times R_p=\tilde{\hat{\omega }}{R}_p\left(t\right),p\left(0\right)=p_0$$
  linear velocity at time $t$, recall $\dot{x}=Ax,x\left(0\right)=x_0\Rightarrow x\left(t\right)=e^{At}{x}_0$
• This is a linear ODE with solution : ${\dot{R}}_p\left(t\right)=\tilde{\hat{\omega }}{R}_p\left(t\right)\Rightarrow {\dot{R}}_p\left(t\right)=e^{\tilde{\hat{\omega }}t}{R}_{p_0}$
• After $t=\theta$, the point has been rotated by $\theta$ degree. Note ${\dot{R}}_p\left(\theta \right)=e^{\tilde{\hat{\omega }}\theta }{R}_{p_0}$, $e^{\tilde{\hat{\omega }}\theta }$ is rotation operator
• $\mathrm{Rot}\left(\hat{\omega },\theta \right)\triangleq e^{\tilde{\hat{\omega }}\theta }$ can be viewed as a rotation operator that rotates a point about $\hat{\omega }$ through $\theta$ degree

The discussion holds for any reference frame

1.3 Raotation Matrix as a Rotation Operator

• Theorem: Every rotation matrix $R$ can be written as $R=\mathrm{Rot}\left(\hat{\omega },\theta \right)\triangleq e^{\tilde{\hat{\omega }}\theta }\in SO\left(3\right)\left\{R^{\mathrm{T}}R=E,\det \left(R\right)=1\right\}$, i.e. , it represents a rotation operation about $\hat{\omega }$ by $\theta$
• We have seen how to use $R$ to represent frame orientation and change of coordinate between different frames. They are quite different from the operator interpretation of $R$
• To apply the rotation operation, all the vectors / matrices have to be expressed in the same refrence frame

For example, assume R = [ 1 0 0 0 0 − 1 0 1 0 ] = R o t ( x ^ , π 2 ) R=\left[ \begin{matrix} 1& 0& 0\\ 0& 0& -1\\ 0& 1& 0\\ \end{matrix} \right] =\mathrm{Rot}\left( \hat{x},\frac{\pi}{2} \right) R= ​100​001​0−10​ ​=Rot(x^,2π​)
Consider a relation $q=Rp$:

• Change reference frame interpretation : two frames $\left\{A\right\},\left\{B\right\}$ , one physical Point $a$
  $R$ : orientation of $\left\{B\right\}$ relative to $\left\{A\right\}$ : i.e. $R=\left[Q_{\mathrm{B}}^A\right]$
  then : $p=a^B,q=a^A,q=Rp\Rightarrow a^A=\left[Q_{\mathrm{B}}^A\right]a^B$
• Rotation operator interpretation : one frame and two points
  $a\to a^{\prime },p=a^A,q={a^{\prime }}^A\Rightarrow {a^{\prime }}^A=R{a}^A$

Consider the frame operation:

• Change of reference frame :
  Have one “frame object”, two reference frames
  Frame object $\left\{A\right\}$, orientation in $\left\{O\right\}$ , is $R_{\mathrm{A}}^O,R_{\mathrm{A}}^B$, $\Rightarrow R_{\mathrm{A}}^O=R_{\mathrm{B}}^O{R}_{\mathrm{A}}^B=R{R}_{\mathrm{A}}^B$
• Rotation a frame : ${R^{\prime }}_A=R{R}_A$
  two frame objects, one refrence frame
  more precisely : ${R^{\prime }}_A=R{R}_A\Rightarrow R_{{\mathrm{A}}^{\prime }}^O=R{R}_{\mathrm{A}}^O$

2. Rotation Operation in Different Frames

2.1 Rotation Martix Properties

$\left[Q\right]{\left[Q\right]}^{\mathrm{T}}=E$

$\left[Q_1\right]\left[Q_2\right]\in SO\left(3\right),if\left[Q_1\right],\left[Q_2\right]\in SO\left(3\right)$： product of two rotation matrices is also a rotation matrix
p , q ∈ R 3 , ∥ [ Q ] R ⃗ p − [ Q ] R ⃗ q ∥ 2 = ∥ [ Q ] ( R ⃗ p − R ⃗ q ) ∥ 2 = ( R ⃗ p − R ⃗ q ) T [ Q ] T [ Q ] ( R ⃗ p − R ⃗ q ) = ∥ R ⃗ p − R ⃗ q ∥ 2 p,q\in \mathbb{R} ^3,\left\| \left[ Q \right] \vec{R}_p-\left[ Q \right] \vec{R}_{\mathrm{q}} \right\| ^2=\left\| \left[ Q \right] \left( \vec{R}_p-\vec{R}_{\mathrm{q}} \right) \right\| ^2=\left( \vec{R}_p-\vec{R}_{\mathrm{q}} \right) ^{\mathrm{T}}\left[ Q \right] ^{\mathrm{T}}\left[ Q \right] \left( \vec{R}_p-\vec{R}_{\mathrm{q}} \right) =\left\| \vec{R}_p-\vec{R}_{\mathrm{q}} \right\| ^2 p,q∈R3, ​[Q]R p​−[Q]R q​ ​2= ​[Q](R p​−R q​) ​2=(R p​−R q​)T[Q]T[Q](R p​−R q​)= ​R p​−R q​ ​2

∥ R ⃗ p − R ⃗ q ∥ = ∥ [ Q ] R ⃗ p − [ Q ] R ⃗ q ∥ \left\| \vec{R}_p-\vec{R}_{\mathrm{q}} \right\| =\left\| \left[ Q \right] \vec{R}_p-\left[ Q \right] \vec{R}_{\mathrm{q}} \right\| ​R p​−R q​ ​= ​[Q]R p​−[Q]R q​ ​ : rotation operator preserves distance

$\left[Q\right]\left(v\times w\right)=\left[Q\right]v\times \left[Q\right]w$

$\left[Q\right]\tilde{w}{\left[Q\right]}^{\mathrm{T}}=\left[Q\right]w$ —— important

2.2 Rotation Operation in Different Frames

Consider two frames $\left\{A\right\}$ and $\left\{B\right\}$， the actual numerical values of the operator $\mathrm{Rot}\left(\hat{\omega },\theta \right)$ depend on both the reference frame to repersent $\hat{\omega }$ and the reference frame to represent the operator itself

Consider a rotation axis $\hat{\omega }$ (coordinate free vector), with $\left\{A\right\}$ - frame coordinate ${\hat{\omega }}^A$ and $\left\{B\right\}$ - frame. We know ${\omega }^A=\left[Q_{\mathrm{B}}^A\right]{\omega }^B$

Let ${}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)$ and ${}^A\mathrm{Rot}\left({}^A\hat{\omega },\theta \right)$ be the two rotation matrices, representing the same rotation operatioon $\mathrm{Rot}\left(\hat{\omega },\theta \right)$ in frames $\left\{A\right\}$ and $\left\{B\right\}$

We have the relation : ${}^A\mathrm{Rot}\left({}^A\hat{\omega },\theta \right)={\left[Q_{\mathrm{B}}^A\right]}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)\left[Q_{\mathrm{A}}^B\right]$

• Approach 1 : two points $p\to p^{\prime }\Rightarrow R_{{\mathrm{p}}^{\prime }}^A{=}^A\mathrm{Rot}\left({}^A\hat{\omega },\theta \right)R_{\mathrm{p}}^A$
  $\left\{B\right\}$ - frame : $R_{{\mathrm{p}}^{\prime }}^B{=}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)R_{\mathrm{p}}^B$
  $\Rightarrow \left[Q_{\mathrm{B}}^A\right]R_{{\mathrm{p}}^{\prime }}^B={\left[Q_{\mathrm{B}}^A\right]}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)R_{\mathrm{p}}^B\Rightarrow R_{{\mathrm{p}}^{\prime }}^A={\left[Q_{\mathrm{B}}^A\right]}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{p}}^A{\Rightarrow }^A\mathrm{Rot}\left({}^A\hat{\omega },\theta \right)={\left[Q_{\mathrm{B}}^A\right]}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right)\left[Q_{\mathrm{B}}^A\right]$

• Approach 2 : for $a\in {\mathbb{R}}^3,\tilde{a}\in so\left(3\right),\left[Q\right]\in SO\left(3\right)$
  $\Rightarrow \left[Q\right]a\in {\mathbb{R}}^3,\left[Q\right]a=\left[Q\right]\tilde{a}{\left[Q\right]}^{\mathrm{T}}$
  $Rot\left({\omega }^A,\theta \right)=e^{{\tilde{\omega }}^A\theta }=e^{\left[Q_{\mathrm{B}}^A\right]{\omega }^B\theta }=e^{\left[Q_{\mathrm{B}}^A\right]{\tilde{\omega }}^B{\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\theta }=\left[Q_{\mathrm{B}}^A\right]e^{{\tilde{\omega }}^B\theta }{\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\leftarrow e^{PA{P}^{-1}}=P{e}^A{P}^{-1}$

3. Rigid-Body Operation via Diffeential Equation

Recall: Every $\left[Q\right]\in SO\left(3\right)$ can be viewed as the state transition matrix associated with the rotation ODE(1). It maps the initial position to the current position(after the rotation motion)

• $p\left(\theta \right)=Rot\left(\omega ,\theta \right)p_0$ viewed as solution to $\dot{p}\left(t\right)=\tilde{\omega }p\left(t\right),p\left(0\right)=p_0$ at $t=\theta$
• The above relation requires that the rotation axis passes through the origin.

We can obtain similar ODE characterization for $\left[T\right]\in SE\left(3\right)$ , which will lead to exponential coordinate of SE(3)

Recall : Theorem (Chasles): Every rigid body motion can be realized by a screw motion

Consider a point $p$ undergoes a screw motion with screw axis $\mathcal{S}$ and unit speed ($\dot{\theta }=1$). Let the cooresponding twist be $\mathcal{V}=\dot{\theta }\mathcal{S}=\left(\omega ,v\right)$ . The motion can be described be the following ODE.
$$\dot{p}\left(t\right)=\omega \times p\left(t\right)+v\Rightarrow \left[\begin{array}{c}\dot{p}\left(t\right)\\ 0\end{array}\right]=\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]\left[\begin{array}{c}p\left(t\right)\\ 1\end{array}\right]$$

Solution in homogeneous coordinate is :
$$\left[\begin{array}{c}p\left(t\right)\\ 1\end{array}\right]=\exp \left(\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]t\right)\left[\begin{array}{c}p\left(0\right)\\ 1\end{array}\right]$$

3.1 SE(3)

For any twist $\mathcal{V}=\left(\omega ,v\right)$, let $\left[\mathcal{V}\right]$ be its matrix representation of twist $\mathcal{V}$
$$\left[\mathcal{V}\right]=\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

• The abve definition also applies to a screw axis $\mathcal{S}=\left(\omega ,v\right),\left[\mathcal{S}\right]=\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]$

• With this notation, the solution is $\left[\begin{array}{c}p\left(t\right)\\ 1\end{array}\right]=e^{\left[\mathcal{S}\right]t}\left[\begin{array}{c}p\left(0\right)\\ 1\end{array}\right]$

• Fact: $e^{\left[\mathcal{S}\right]t}\in SE\left(3\right)$ is always a valid homogeneous transformation matrix.
  $\left[T\right]=e^{\left[\mathcal{S}\right]t}=\left[\begin{array}{cc}\left[Q\right]& R\\ 0& 1\end{array}\right]$

• Fact: Any $T\in SE\left(3\right)$ can be written as $\left[T\right]=e^{\left[\mathcal{S}\right]t}$, i.e. , it can be viewed as an operator that moves a point/frame along the screw axis $\mathcal{S}$ at unit speed for time $t$

3.2 se(3)

$$\begin{gathered} \forall \omega \in {\mathbb{R}}^3\to \tilde{\omega }\in so\left(3\right)\to e^{\tilde{\omega }\theta }\in SO\left(3\right) \\ \forall \mathcal{S}\in {\mathbb{R}}^6\to {\left[\mathcal{S}\right]|}_{4\times 4}=\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]\in se\left(3\right)\to e^{\left[\mathcal{S}\right]\theta }\in SE\left(3\right) \end{gathered}$$
Similar to $so\left(3\right)$ , we can define $se\left(3\right)$ :
$$se\left(3\right)=\left\{\left(\tilde{\omega },v\right),\tilde{\omega }\in so\left(3\right),v\in {\mathbb{R}}^3\right\}$$

• $se\left(3\right)$ contains all matrix representation of twists or equivalently all twists.
• In some references, $\left[\mathcal{V}\right]$ is called a twist
• Sometimes, we may abuse notation by writing $\mathcal{V}\in se\left(3\right)$

4. Homogeneous Transformation Matrix as Rigid-Body Operator

• ODE for rigid motion under $\mathcal{V}=\left(\omega ,v\right)$
  $\dot{p}=v+\omega \times p\Rightarrow \left[\begin{array}{c}\dot{p}\\ 0\end{array}\right]=\left[\begin{array}{cc}\tilde{\omega }& v\\ 0& 0\end{array}\right]\left[\begin{array}{c}p\\ 1\end{array}\right]\Rightarrow \left[\begin{array}{c}\dot{p}\\ 0\end{array}\right]=e^{\left[\mathcal{V}\right]t}\left[\begin{array}{c}p\\ 1\end{array}\right]$

• Consider “unit velocity” $\mathcal{V}=\mathcal{S}$, then time $t$ means degree
  if not unit speed : $\mathcal{V}=\dot{\theta }\mathcal{S}$

• $\left[\begin{array}{c}{p}^{\prime }\\ 1\end{array}\right]=\left[T\right]\left[\begin{array}{c}p\\ 1\end{array}\right]$ : “rotate” $p$ about screw axis $\mathcal{S}$ by $\theta$ degree
  $\left[T\right]=e^{\left[\mathcal{S}\right]\theta }$, two points : $\left[\begin{array}{c}p\\ 1\end{array}\right]\to \left[\begin{array}{c}{p}^{\prime }\\ 1\end{array}\right]$ , more precisely : $\left[\begin{array}{c}{p^O}^{\prime }\\ 1\end{array}\right]=\left[T^O\right]\left[\begin{array}{c}{p}^O\\ 1\end{array}\right]$
  For $\left[T\right]\in SE\left(3\right)$ config representative $\left[T_{\mathrm{B}}^A\right]$ : config of $\left\{B\right\}$ relative to $\left\{A\right\}$ —— $\left[\begin{array}{c}{p}^A\\ 1\end{array}\right]=\left[T_{\mathrm{B}}^A\right]\left[\begin{array}{c}{p}^B\\ 1\end{array}\right]$——same physical point but two different frames

• $\left[T\right]\left[T_A\right]$ : “rotate” $\left\{A\right\}$-frame about $\mathcal{S}$ by $\theta$ degree

Rigid-Body Operator in Different Frames
Expression of $\left[T\right]$ in another frame (other than $\left\{O\right\}$):
$$\left[T^O\right]\leftrightarrow {\left[T_{\mathrm{B}}^O\right]}^{-1}\left[T^O\right]\left[T_{\mathrm{B}}^O\right]$$

5. Rigid-Body Operation of Screw Axis

Consider an arbitrary screw axis $\mathcal{S}$ , suppose the axis has gone through a rigid transformation $\left[T\right]=\left(\left[Q\right],R\right)$ and the resulting new screw axis is ${\mathcal{S}}^{\prime }$ , then
$${\mathcal{S}}^{\prime }=\left[A{d}_T\right]\mathcal{S}$$

Let’s work an arbitrary frame $\left\{A\right\}$ (rigidly attached to the screw axis)
Let $\left\{B\right\}$ be the frame obtained be apply $\left[T\right]$ operation
the coordinate of $\mathcal{S}$ in $\left\{A\right\}$ is the same as the coordinate of ${\mathcal{S}}^{\prime }$ in $\left\{B\right\}$ : ${\mathcal{S}}^A={{\mathcal{S}}^{\prime }}^B$
We also know $\left[T_B\right]=\left[T\right]\left[T_{\mathrm{A}}\right],\left[T\right]=\left[T_{\mathrm{B}}^A\right]$
Multiply $\left[X_{\mathrm{B}}^A\right]$ : $\Rightarrow \left[X_{\mathrm{B}}^A\right]{\mathcal{S}}^A=\left[X_{\mathrm{B}}^A\right]{{\mathcal{S}}^{\prime }}^B={{\mathcal{S}}^{\prime }}^A\Rightarrow {{\mathcal{S}}^{\prime }}^A=\left[X_{\mathrm{B}}^A\right]{\mathcal{S}}^A$
$$\left[X_{\mathrm{B}}^A\right]=\left[\begin{array}{cc}\left[Q_{\mathrm{B}}^A\right]& 0\\ {\tilde{R}}_{\mathrm{B}}^A\left[Q_{\mathrm{B}}^A\right]& \left[Q_{\mathrm{B}}^A\right]\end{array}\right]=\left[A{d}_T\right]$$

1
2
3
4
5
6
7
8
9