[足式机器人]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 $\vec{\omega }\in {\mathbb{R}}^3$, there is a matrix $\widetilde{\stackrel{⃗}{\omega }}\in {\mathbb{R}}^{3\times 3}$ such that $\vec{\omega }\times \vec{R}=\widetilde{\stackrel{⃗}{\omega }}\vec{R}$
$$\vec{\omega }=\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right]⟷\widetilde{\stackrel{⃗}{\omega }}=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$
Note that $\widetilde{\stackrel{⃗}{a}}=-{\widetilde{\stackrel{⃗}{a}}}^{\mathrm{T}}$ (called skew stmmetric)
$\widetilde{\stackrel{⃗}{\omega }}$ is called a skew-symmetric matrix representation of the vector $\vec{\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 ${\vec{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{\vec{R}}}_p\left(t\right)=\hat{\omega }\times {\vec{R}}_p=\tilde{\hat{\omega }}{\vec{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{\vec{R}}}_p\left(t\right)=\tilde{\hat{\omega }}{\vec{R}}_p\left(t\right)\Rightarrow {\dot{\vec{R}}}_p\left(t\right)=e^{\tilde{\hat{\omega }}t}{\vec{R}}_{p_0}$
• After $t=\theta$, the point has been rotated by $\theta$ degree. Note ${\dot{\vec{R}}}_p\left(\theta \right)=e^{\tilde{\hat{\omega }}\theta }{\vec{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=\left[\begin{array}{ccc}1& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]=\mathrm{Rot}\left(\hat{x},\frac{\pi }{2}\right)$
Consider a relation $q=Rp$:

• Change reference frame interpretation : two frames { A } , { B } \left\{ A \right\} ,\left\{ B \right\} {A},{B} , one physical Point $a$
  $R$ : orientation of { B } \left\{ B \right\} {B} relative to { A } \left\{ A \right\} {A} : 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 { A } \left\{ A \right\} {A}, orientation in { O } \left\{ O \right\} {O} , 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\in {\mathbb{R}}^3,{\Vert \left[Q\right]{\vec{R}}_p-\left[Q\right]{\vec{R}}_{\mathrm{q}}\Vert }^2={\Vert \left[Q\right]\left({\vec{R}}_p-{\vec{R}}_{\mathrm{q}}\right)\Vert }^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)={\Vert {\vec{R}}_p-{\vec{R}}_{\mathrm{q}}\Vert }^2$

$\Vert {\vec{R}}_p-{\vec{R}}_{\mathrm{q}}\Vert =\Vert \left[Q\right]{\vec{R}}_p-\left[Q\right]{\vec{R}}_{\mathrm{q}}\Vert$ : rotation operator preserves distance

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

$\left[Q\right]\widetilde{\stackrel{⃗}{w}}{\left[Q\right]}^{\mathrm{T}}=\widetilde{\left[Q\right]\vec{w}}$ —— important

2.2 Rotation Operation in Different Frames

Consider two frames { A } \left\{ A \right\} {A} and { B } \left\{ B \right\} {B}， 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 { A } \left\{ A \right\} {A} - frame coordinate ${\hat{\omega }}^A$ and { B } \left\{ B \right\} {B} - frame. We know ${\vec{\omega }}^A=\left[Q_{\mathrm{B}}^A\right]{\vec{\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 { A } \left\{ A \right\} {A} and { B } \left\{ B \right\} {B}

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 {\vec{R}}_{{\mathrm{p}}^{\prime }}^A{=}^A\mathrm{Rot}\left({}^A\hat{\omega },\theta \right){\vec{R}}_{\mathrm{p}}^A$
  { B } \left\{ B \right\} {B} - frame : ${\vec{R}}_{{\mathrm{p}}^{\prime }}^B{=}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right){\vec{R}}_{\mathrm{p}}^B$
  $\Rightarrow \left[Q_{\mathrm{B}}^A\right]{\vec{R}}_{{\mathrm{p}}^{\prime }}^B={\left[Q_{\mathrm{B}}^A\right]}^B\mathrm{Rot}\left({}^B\hat{\omega },\theta \right){\vec{R}}_{\mathrm{p}}^B\Rightarrow {\vec{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]{\vec{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 $\vec{a}\in {\mathbb{R}}^3,\widetilde{\stackrel{⃗}{a}}\in so\left(3\right),\left[Q\right]\in SO\left(3\right)$
  $\Rightarrow \left[Q\right]\vec{a}\in {\mathbb{R}}^3,\widetilde{\left[Q\right]\vec{a}}=\left[Q\right]\widetilde{\stackrel{⃗}{a}}{\left[Q\right]}^{\mathrm{T}}$
  $Rot\left({\vec{\omega }}^A,\theta \right)=e^{{\widetilde{\stackrel{⃗}{\omega }}}^A\theta }=e^{\widetilde{\left[Q_{\mathrm{B}}^A\right]{\vec{\omega }}^B}\theta }=e^{\left[Q_{\mathrm{B}}^A\right]{\widetilde{\stackrel{⃗}{\omega }}}^B{\left[Q_{\mathrm{B}}^A\right]}^{\mathrm{T}}\theta }=\left[Q_{\mathrm{B}}^A\right]e^{{\widetilde{\stackrel{⃗}{\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)

• $\vec{p}\left(\theta \right)=Rot\left(\vec{\omega },\theta \right){\vec{p}}_0$ viewed as solution to $\dot{\vec{p}}\left(t\right)=\widetilde{\stackrel{⃗}{\omega }}\vec{p}\left(t\right),\vec{p}\left(0\right)={\vec{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(\vec{\omega },\vec{v}\right)$ . The motion can be described be the following ODE.
$$\dot{\vec{p}}\left(t\right)=\vec{\omega }\times \vec{p}\left(t\right)+\vec{v}\Rightarrow \left[\begin{array}{c}\dot{\vec{p}}\left(t\right)\\ 0\end{array}\right]=\left[\begin{array}{cc}\widetilde{\stackrel{⃗}{\omega }}& \vec{v}\\ 0& 0\end{array}\right]\left[\begin{array}{c}\vec{p}\left(t\right)\\ 1\end{array}\right]$$

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

3.1 SE(3)

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

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

• With this notation, the solution is $\left[\begin{array}{c}\vec{p}\left(t\right)\\ 1\end{array}\right]=e^{\left[\mathcal{S}\right]t}\left[\begin{array}{c}\vec{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]& \vec{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 \vec{\omega }\in {\mathbb{R}}^3\to \widetilde{\stackrel{⃗}{\omega }}\in so\left(3\right)\to e^{\widetilde{\stackrel{⃗}{\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}\widetilde{\stackrel{⃗}{\omega }}& \vec{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(\widetilde{\stackrel{⃗}{\omega }},\vec{v}\right),\widetilde{\stackrel{⃗}{\omega }}\in so\left(3\right),\vec{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(\vec{\omega },\vec{v}\right)$
  $\dot{\vec{p}}=\vec{v}+\vec{\omega }\times \vec{p}\Rightarrow \left[\begin{array}{c}\dot{\vec{p}}\\ 0\end{array}\right]=\left[\begin{array}{cc}\widetilde{\stackrel{⃗}{\omega }}& \vec{v}\\ 0& 0\end{array}\right]\left[\begin{array}{c}\vec{p}\\ 1\end{array}\right]\Rightarrow \left[\begin{array}{c}\dot{\vec{p}}\\ 0\end{array}\right]=e^{\left[\mathcal{V}\right]t}\left[\begin{array}{c}\vec{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}{\vec{p}}^{\prime }\\ 1\end{array}\right]=\left[T\right]\left[\begin{array}{c}\vec{p}\\ 1\end{array}\right]$ : “rotate” $\vec{p}$ about screw axis $\mathcal{S}$ by $\theta$ degree
  $\left[T\right]=e^{\left[\mathcal{S}\right]\theta }$, two points : $\left[\begin{array}{c}\vec{p}\\ 1\end{array}\right]\to \left[\begin{array}{c}{\vec{p}}^{\prime }\\ 1\end{array}\right]$ , more precisely : $\left[\begin{array}{c}{{\vec{p}}^O}^{\prime }\\ 1\end{array}\right]=\left[T^O\right]\left[\begin{array}{c}{\vec{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 { B } \left\{ B \right\} {B} relative to { A } \left\{ A \right\} {A} —— $\left[\begin{array}{c}{\vec{p}}^A\\ 1\end{array}\right]=\left[T_{\mathrm{B}}^A\right]\left[\begin{array}{c}{\vec{p}}^B\\ 1\end{array}\right]$——same physical point but two different frames

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

Rigid-Body Operator in Different Frames
Expression of $\left[T\right]$ in another frame (other than { O } \left\{ O \right\} {O}):
$$\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],\vec{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 { A } \left\{ A \right\} {A} (rigidly attached to the screw axis)
Let { B } \left\{ B \right\} {B} be the frame obtained be apply $\left[T\right]$ operation
the coordinate of $\mathcal{S}$ in { A } \left\{ A \right\} {A} is the same as the coordinate of ${\mathcal{S}}^{\prime }$ in { B } \left\{ B \right\} {B} : ${\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\\ {\widetilde{\stackrel{⃗}{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