绕x轴: R x ( θ ) = [ x ^ ′ y ^ ′ z ^ ′ ] = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] \textit{\textbf{R}}_x(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos{\theta } & -\sin{\theta } \\ 0 & \sin{\theta } & \cos{\theta }\end{bmatrix} Rx(θ)=[x^′y^′z^′]=1000cosθsinθ0−sinθcosθ
Definition: R y ( θ ) = [ x ^ ′ y ^ ′ z ^ ′ ] = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] \textit{\textbf{R} }_y(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix}\cos{\theta}&0&\sin{\theta}\\0&1&0\\-\sin{\theta}&0&\cos{\ theta }\end{bmatrix}Ry( i )=[x^′y^′z^′]=cosi0−sini010sini0cosi
Definition: R z ( θ ) = [ x ^ ′ y ^ ′ z ^ ′ ] = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] \textit{\textbf{R} }_z(\theta )=[\hat{x}'\;\; \hat{y}'\;\; \hat{z}'] =\begin{bmatrix}\cos{\theta}&-\sin{\theta}&0\\\sin{\theta}&\cos{\theta}&0\\0& 0 & 1\end{bmatrix}Rz( i )=[x^′y^′z^′]=cosisini0−sinicosi0001
Properties of Rotation Matrix in Three-Dimensional Space
Orthogonality
The three column vectors of the rotation matrix are a set of orthonormal basis
Each column vector is a unit vector, orthogonal to each other
The rotation matrix is an orthogonal matrix with orthogonality
The determinant is always 1
Det ( R sb ) = 1 {\rm det}(\textit{\textbf{R}}_{sb})= 1it ( Rsb)=1 (meaning: describe the scale, the rotation matrix will not scale the object)
reversibility
Inverse matrix of rotation matrix = transpose matrix R sb − 1 = R bs \textit{\textbf{R}}_{sb}^{-1} = \textit{\textbf{R}}_{bs}\; \;Rsb−1=Rbs R s b − 1 = R s b T \textit{\textbf{R}}_{sb}^{-1}=\textit{\textbf{R}}_{sb}^{\rm T} Rsb−1=RsbT
名得律: ( R ab ⋅ R bc ) ⋅ R cd = R ab ⋅ ( R bc ⋅ R cd ) = R ad (\textit{\textbf{R}}_{ab} \cdot \textit{\textbf{R }}_{bc}) \cdot \textit{\textbf{R}}_{cd} = \textit{\textbf{R}}_{ab} \cdot (\textit{\textbf{R}}_{ bc} \cdot \textit{\textbf{R}}_{cd}) = \textit{\textbf{R}}_{ad}(Rab⋅Rbc)⋅Rcd=Rab⋅(Rbc⋅Rcd)=Rad
Not commutative: R ac = R ab ⋅ R bc ≠ R bc ⋅ R ab \textit{\textbf{R}}_{ac} = \textit{\textbf{R}}_{ab} \cdot \textit {\textbf{R}}_{bc} \neq \textit{\textbf{R}}_{bc} \cdot \textit{\textbf{R}}_{ab}Rac=Rab⋅Rbc=Rbc⋅Rab
2. Rotate a matrix around an arbitrary axis
2.1. Properties of cross product operation
vector product form
a × b = [ 0 − azayaz 0 − ax − ayax 0 ] ⋅ b = [ a ] × ⋅ b 0 & -a_z & a_y\\ a_z & 0 & -a_x\\ -a_y & a_x & 0\end{bmatrix}\cdot\textit{\textbf{b}}=[\textit{\textbf{a}}] _{\times}\cdot\text{\textbf{b}}a×b=0az−ay−az0axay−ax0⋅b=[a]×⋅b
Cross multiplication anti-commutative law: a × b = − b × a \textit{\textbf{a}}\times\textit{\textbf{b}}=-\textit{\textbf{b}}\times\textit{ \textbf{a}}a×b=−b×a
[ a ] × b = − [ b ] × a [\textit{\textbf{a}}]_\times\textit{\textbf{b}}=-[\textit{\textbf{b}}]_\times\textit{\textbf{a}} [a]×b=−[b]×a
Anti-symmetric matrix: [ a ] × = − [ a ] × T [\textit{\textbf{a}}]_\times=-[\textit{\textbf{a}}]_\times^{\rm T }[a]×=−[a]×T
二重向量积展开: a × ( b × c ) = ( a ⋅ c ) b − ( a ⋅ b ) c \textit{\textbf{a}}\times(\textit{\textbf{b}}\times\textit{\textbf{c}})=(\textit{\textbf{a}}\cdot\textit{\textbf{c}})\textit{\textbf{b}}-(\textit{\textbf{a}}\cdot\textit{\textbf{b}})\textit{\textbf{c}} a×(b×c)=(a⋅c)b−(a⋅b)c
向量点积运算: a ⋅ b = a T b = b T a \textit{\textbf{a}}\cdot\textit{\textbf{b}}=\textit{\textbf{a}}^{\rm T}\textit{\textbf{b}}=\textit{\textbf{b}}^{\rm T}\textit{\textbf{a}} a⋅b=aTb=bT a
[ a ] × [ b ] × c = ( a T c ) b − ( a T b ) c [\textit{\textbf{a}}]_\times[\textit{\textbf{b}}]_\times\textit{\textbf{c}}=(\textit{\textbf{a}}^{\rm T}\textit{\textbf{c}})\textit{\textbf{b}}-(\textit{\textbf{a}}^{\rm T}\textit{\textbf{b}})\textit{\textbf{c}} [a]×[b]×c=(aTc)b−(aTb)c
2.2. Vector product and matrix form
{ a = a × b ∣ c ∣ = ∣ a ∣ ∣ b ∣ sin θ \left\{\begin{matrix}\textit{\textbf{a}}=\textit{\textbf{a}}\times \textit{\textbf{b}} \\|\textit{\textbf{c}}|=|\textit{\textbf{a}}||\textit{\textbf{b}}|\sin {\theta }\end{matrix}\right. {
a=a×b∣c∣=∣a∣∣b∣sini
2.3. Linear speed of rotation around the axis
∣ v ω ∣ = ∣ ω ∣ ⋅ ∣ p ∣ ⋅ sin ∠ AOP |\textbf{v}}_\omega|=|{\bm\omega}|\cdot|\textbf{p }}|\cdot\sin\angle AOP∣voh∣=∣ω∣⋅∣p∣⋅sin∠AOP
v ω = ω × p = [ ω ] × p \textbf{v}}_{\omega}={\bm\omega}\times\textbf{p}}=[{\bm\ omega}]_{\times}\text{\textbf{p}}voh=oh×p=[ oh ]×p
R ˙ s b = [ x ^ ˙ y ^ ˙ z ^ ˙ ] = [ [ ω s ] × x ^ b [ ω s ] × y ^ b [ ω s ] × z ^ b ] = [ ω s ] × [ x ^ b y ^ b z ^ b ] = [ ω s ] × R s b \dot{\bm R}_{sb}=[\dot{\hat{\bm x}}\; \dot{\hat{\bm y}}\; \dot{\hat{\bm z}}] = [\;[{\bm\omega}_s]_{\times}\hat{\bm x}_b \;\; [{\bm\omega}_s]_{\times}\hat{\bm y}_b \;\; [{\bm\omega}_s]_{\times}\hat{\bm z}_b\;] = [{\bm \omega}_s]_{\times}[\hat{\bm x}_b\;\;\hat{\bm y}_b\;\;\hat{\bm z}_b] = [{\bm \omega}_s]_{\times}{\bm R}_{sb} R˙sb=[x^˙y^˙z^˙]=[[ ohs]×x^b[ ohs]×y^b[ ohs]×z^b]=[ ohs]×[x^by^bz^b]=[ ohs]×Rsb
Angular velocity of rotation ω s {\bm\omega}_sohsand the rotation matrix R sb {\bm R}_{sb}Rsb的关系: [ ω s ] × = R ˙ s b R s b − 1 = R ˙ s b R s b T [{\bm\omega}_s]_{\times}=\dot{\bm R}_{sb}{\bm R}_{sb}^{-1}=\dot{\bm R}_{sb}{\bm R}_{sb}^{\rm T} [ ohs]×=R˙sbRsb−1=R˙sbRsbT
2.4. First-order linear ordinary differential equations
x ˙ ( t ) = a x ( t ) , x ( 0 ) = x 0 → x ( t ) = e a t x 0 \dot{x}(t)=ax(t), x(0)=x_0\rightarrow x(t)=e^{at}x_0 x˙(t)=ax(t),x(0)=x0→x(t)=eatx0
The first-order linear differential equation of a vector: x ˙ ( t ) = A x ( t ) , x ( 0 ) = x 0 → x ( t ) = e A tx 0 \dot{\bm x}(t)={\ bm A}{\bm x}(t), {\bm x}(0)={\bm x}_0\rightarrow {\bm x}(t)=e^{ {\bm A}t}{
\ bm x}_0x˙(t)=Ax(t),x(0)=x0→x(t)=eAtx0
Matrix exponent e A te^{
{\bm A}t}eAt nature_
对角矩阵: e D t = [ e d 1 t 0 ⋯ 0 0 e d 2 t ⋯ 0 ⋮ ⋮ ⋮ 0 0 ⋯ e d n t ] e^{Dt}=\begin{bmatrix} e^{d_1t} & 0 & \cdots & 0\\ 0 & e^{d_2t} & \cdots & 0\\ \vdots & \vdots & & \vdots \\ 0 & 0 & \cdots & e^{d_nt}\end{bmatrix} eDt=ed1t0⋮00ed2t⋮0⋯⋯⋯00⋮ednt
Non-diagonal matrix, first diagonalize A = PDP − 1 {\bm A}={\bm PDP}^{-1}A=PDP−1: e A t = P e D t P − 1 e^{
{\bm A}t}={\bm P}e^{Dt}{\bm P}^{-1} eAt=PeDtP−1
若AB = BA {\bm AB}={\bm BA}AB=B Acompatible, 则e A e B = e A + B e^{\bm A}e^{\bm B} = e^{ {
\bm A}+{\bm B}}eAeB=eA+B
( e A ) ( − 1 ) = e − A (e^{\bm A})^{(-1)}=e^{-{\bm A}} (eA)(−1)=e−A
2.5. The rotation matrix represented by the axis-angle method
P P The initial coordinates of point P p 0 {\bm p_0}p0, the unit vector ω ^ \hat{\omega} around the rotation axisoh^ rotated byθ \thetaθ angle--equivalent –> unit vectorω ^ \hat{\omega}oh^ In units of velocity 1rad/s {\rm rad/s}rad/s rotated by timeθ \thetai
There is a differential equation: p ˙ ( t ) = [ ω ] × p ( t ) \dot{\bm p}(t)=[\bm\omega]_\times{\bm p}(t)p˙(t)=[ oh ]×p (t), functionp ( θ ) = e [ ω ^ ] × θ p 0 {\bm p}(\theta)=e^{[\hat{\bm \omega}]_\times\theta} {\bmp}_0p (θ)=e[oh^]×θp0
Around the unit axis ω ^ \hat{\bm \omega}oh^ rotatetheta \thetaSolve the θ infinitive:R ot ( ω ^ , θ ) = e [ ω ^ ] × θ = ∑ n = 0 ∞ [ ω ^ × n θ n ] n ! {\rm Rot}(\hat{\bm\omega},\theta)=e^{[\hat{\bm\omega}]_\times\theta}=\sum_{n=0}^{\infty }\frac{[\hat{\bm\omega}_\times^n\theta^n]}{n!}Rot(oh^,i )=e[oh^]×i=∑n=0∞n![oh^×nin]
Definite equations : R ot ( ω ^ , θ ) = e [ ω ^ ] × θ = I + sin θ [ ω ^ ] × + ( 1 − cos θ ) [ ω ^ ] × 2 {\rm Rot}(\hat{\bm\omega},\theta)=e^{[\hat{\bm\omega}]_\times\theta}={\bm I}+\sin\theta[\hat{ \bm \omega}]_\times + (1-\cos\theta)[\hat{\bm\omega}]_\times^2Rot(oh^,i )=e[oh^]×i=I+sini [oh^]×+(1−cos(i ) [oh^]×2
Arbitrary matrices can be easily computed
2.6. Matrix Logarithm & Exponential Coordinates
Matrix logarithm: antisymmetric matrix [ ω ^ ] × θ [\hat{\bm\omega}]_\times\theta[oh^]×θ is called the matrixR \bm RMatrix logarithm in R
Exponential coordinates: three-dimensional vector ω ^ θ \hat{\bm\omega}\thetaoh^ θis called the matrixR \bm RExponential coordinates of R
{ ω 1 = r 11 + 1 2 ω 2 = r 12 2 ω 1 ω 3 = r 13 2 ω 1 \left\{\begin{matrix}\omega_1=\sqrt{\frac{r_{11}+1}{2}} \\\omega_2=\frac{r_{12}}{2\omega_1}\\\omega_3=\frac{r_{13}}{2\omega_1}\end{matrix}\right. ⎩⎨⎧oh1=2r11+1oh2=2 o1r12oh3=2 o1r13
3. Rigid body motion in three-dimensional space
Two-dimensional planar rigid body motion: T sb = [ R sb OO ′ → 0 1 × 2 1 ] {\bm T}_{sb}=\begin{bmatrix} {\bm R}_{sb} &\overrightarrow{OO '}\\ 0_{1\times 2} &1\end{bmatrix}Tsb=[Rsb01×2O O′1]
3.1. Properties of homogeneous transformation matrix
可逆性: T − 1 = [ R p 0 1 × 3 1 ] − 1 = [ R T − R T p 0 1 × 3 1 ] {\bm T}^{-1}=\begin{bmatrix} {\bm R} & {\bm p}\\ {\bm 0}_{1\times 3} & 1\end{bmatrix}^{-1}=\begin{bmatrix} {\bm R}^{\rm T} & -{\bm R}^{\rm T}{\bm p}\\ {\bm 0}_{1\times 3} & 1\end{bmatrix} T−1=[R01×3p1]−1=[RT01×3−RTp1]
Closure: The product of homogeneous transformation matrices is still a homogeneous transformation matrix
满足结合律: ( T a b ⋅ T b c ) ⋅ T c d = T a b ⋅ ( T b c ⋅ T c d ) = T a d ({\bm T}_{ab}\cdot{\bm T}_{bc})\cdot{\bm T}_{cd}={\bm T}_{ab}\cdot({\bm T}_{bc}\cdot{\bm T}_{cd})={\bm T}_{ad} (Tab⋅Tbc)⋅Tcd=Tab⋅(Tbc⋅Tcd)=Tad
Definition: T ac = T ab ⋅ T bc ≠ T bc ⋅ T ab {\bm T}_{ac}={\bm T}_{ab}\cdot{\bm T}_{bc}\ neq{\bm T}_{bc}\cdot{\bm T}_{ab}Tac=Tab⋅Tbc=Tbc⋅Tab
3.2. Homogeneous transformation matrix T {\bm T}The role of T
Describe rigid body position and attitude (pose)
1. T {\bm T} T as a state quantity
Transform the reference coordinate system for vector, point, and coordinate systems
2.3. T {\bm T} T is an operator, multiplied with a vector/matrix to complete coordinate system transformation/motion
3.2.1. Describe rigid body pose
Given the world coordinate system { s } \{s\}{
s } lower local coordinate system{ a } { b } { c } \{a\}\{b\}\{c\}Rotation matrix R sa R sb R sc \;{\bm R}_{sa}\;{\bm R}_{sb}\;{\bm R}_{sc of { a } { b } { c } }\;Rs aRsbRsc, origin coordinates psa psb psc {\bm p}_{sa}\;{\bm p}_{sb}\;{\bm p}_{sc}ps apsbpsc
have to
{ c } \{c\} {
c } relative to{ b } \{b\}{
b}的位姿 R b c = R s b T ⋅ R s c {\bm R}_{bc} = {
{\bm R}_{sb}}^{\rm T} \cdot {\bm R}_{sc} Rbc=RsbT⋅Rsc
[ p b c 0 ] = T b s ⋅ [ p s c − p s b 0 ] \begin{bmatrix}{\bm p}_{bc} \\0\end{bmatrix} = {\bm T}_{bs}\cdot \begin{bmatrix}{\bm p}_{sc}-{\bm p}_{sb} \\0\end{bmatrix} [pbc0]=Tbs⋅[psc−psb0]
T b c = [ R b c p b c 0 1 × 3 1 ] {\bm T}_{bc}=\begin{bmatrix}{\bm R}_{bc} & {\bm p}_{bc}\\{\bm 0}_{1\times 3} & 1\end{bmatrix} Tbc=[Rbc01×3pbc1]
3.2.2. Vector, point, coordinate transformation reference coordinate system
Only rotation can change the vector coordinates in space, and translation cannot change
[ v s 0 ] = T s b [ v b 0 ] = [ R s b p s b 0 1 × 3 1 ] [ v b 0 ] = [ R s b v b 0 ] \begin{bmatrix}{\bm v}_s \\0\end{bmatrix} = {\bm T}_{sb}\begin{bmatrix}{\bm v}_b \\0\end{bmatrix} = \begin{bmatrix}{\bm R}_{sb} & {\bm p}_{sb} \\\textbf{0}_{1\times3} & 1\end{bmatrix}\begin{bmatrix}{\bm v}_{b}\\0\end{bmatrix}=\begin{bmatrix}{\bm R}_{sb}{\bm v}_b\\0\end{bmatrix} [vs0]=Tsb[vb0]=[Rsb01×3psb1][vb0]=[Rsbvb0]
v s = R s b v b {\bm v}_s={\bm R}_{sb}{\bm v}_b vs=Rsbvb
For vector coordinate transformation, only the rotation matrix can be multiplied to the left, without homogeneous matrix
Point homogeneous coordinates increase by 1
[ p s 1 ] = T s b [ p b 1 ] = [ R s b p s b 0 1 × 3 1 ] [ p b 1 ] = [ R s b p b + p s b 1 ] \begin{bmatrix}{\bm p}_s \\1\end{bmatrix} = {\bm T}_{sb}\begin{bmatrix}{\bm p}_b \\1\end{bmatrix} = \begin{bmatrix}{\bm R}_{sb} & {\bm p}_{sb} \\\textbf{0}_{1\times3} & 1\end{bmatrix}\begin{bmatrix}{\bm p}_{b}\\1\end{bmatrix}=\begin{bmatrix}{\bm R}_{sb}{\bm p}_b+{\bm p}_{sb} \\1\end{bmatrix} [ps1]=Tsb[pb1]=[Rsb01×3psb1][pb1]=[Rsbpb+psb1]
p s = R s b p b + p s b {\bm p}_{s}={\bm R}_{sb}{\bm p}_b+{\bm p}_{sb} ps=Rsbpb+psb
Homogeneous transformation matrix calculation T sc = T sb ⋅ T bc {\bm T}_{sc}={\bm T}_{sb}\cdot{\bm T}_{bc}Tsc=Tsb⋅Tbc
3.2.3. Motion vector, point, pose
左乘运动 p s ′ = T s b ′ p b = T T s b p b = T p s {\bm p}_s'={\bm T}_{sb'}{\bm p}_b={\bm TT}_{sb}{\bm p}_b={\bm Tp}_s ps′=Tsb′pb=TTsbpb=Tps
Transform the coordinate system first, and move in {s}
右乘运动 p s ′ ′ = T s b ′ ′ p b = T s b T p b = T s b p b ′ ′ {\bm p}_s''={\bm T}_{sb''}{\bm p}_b={\bm T}_{sb}{\bm T}{\bm p}_b={\bm T}_{sb}{\bm p}_b'' ps′′=Tsb′′pb=TsbTpb=Tsbpb′′
First move in {b}, then transform the coordinate system
4. Euler angles
roll roll, pitch pitch, yaw
YPR sequence: The initial object coordinate system {b} coincides with the world coordinate system {s}, first rotate around the z-axis of {b} –>{b'}, then rotate around the y-axis of {b'} –>{b' '}, rotate around the x-axis of {b''} –>{b'''}
Rotation relative to the object coordinate system: Calculated by right-multiplying the rotation matrix method
R s b = I {\bm R}_{sb}={\bm I} Rsb=I
R s b ′ = R s b R z ( y a w ) = R z ( y a w ) {\bm R}_{sb'}={\bm R}_{sb}R_z({\rm yaw})={\bm R}_z({\rm yaw}) Rsb′=RsbRz(yaw)=Rz(yaw)
R s b ′ ′ = R s b ′ R y ( p i t c h ) = R z ( y a w ) R y ( p i t c h ) {\bm R}_{sb''}={\bm R}_{sb'}R_y({\rm pitch})={\bm R}_z({\rm yaw}){\bm R}_y({\rm pitch}) Rsb′′=Rsb′Ry(pitch)=Rz(yaw)Ry(pitch)
R s b ′ ′ ′ = R s b ′ ′ R x ( r o l l ) = R z ( y a w ) R y ( p i t c h ) R x ( r o l l ) {\bm R}_{sb'''}={\bm R}_{sb''}R_x({\rm roll})={\bm R}_z({\rm yaw}){\bm R}_y({\rm pitch}){\bm R}_x({\rm roll}) Rsb′′′=Rsb′′Rx(roll)=Rz(yaw)Ry(pitch)Rx(roll)
R s b ′ ′ ′ {\bm R}_{sb'''} Rsb′′′ → \rightarrow → { tan γ = cos β sin γ cos β cos γ = r 32 r 33 sin β = − r 31 tan α = sin α cos β cos α cos β = r 21 r 11 \left\{\begin{matrix}\tan\gamma = \frac{\cos\beta\sin\gamma}{\cos\beta\cos\gamma} = \frac{r{32}}{r_{3 }}\\\sin\beta = -r_{31}\\\tan\alpha = \frac{\sin\alpha\cos\beta}{\cos\alpha\cos\beta} = \frac{r{2 }}{r_{11}}\end{matrix}\right.⎩⎨⎧tanc=cosbcosccosbsinc=r33r32sinb=−r31tana=cosacosbsinacosb=r11r 21 → \rightarrow → { r o l l = arctan 2 ( r 32 , r 33 ) p i t c h = arcsin ( − r 31 ) y a w = arctan 2 ( r 21 , r 11 ) \left\{\begin{matrix}{\rm roll} = \arctan2(r_{32}, r_{33}) \\{\rm pitch} = \arcsin(-r_{31})\\{\rm yaw} = \arctan2(r_{21},r_{11})\end{matrix}\right. ⎩⎨⎧roll=arctan2(r32,r33)pitch=arcsin(−r31)yaw=arctan2(r21,r11)
ref
"Quadruped robot control algorithm - modeling, control and implementation"