本文仅供学习使用
本文参考:
B站:CLEAR_LAB
课程主讲教师:
Prof. Wei Zhang
南科大高等机器人控制课 Ch01 Linear Differential Equations and Matrix Exponential
1. ODEs-Ordinary Differential Equations
Most engineering system(including most robotics systems) are modeled by Ordinary Differential (or Difference) Equations (ODEs)
1.1 Dynamics of 2R linkage
1.1.1 拉格朗日法
-
对于构件1而言:
R ⃗ m 1 = R ⃗ B = l 1 ⋅ cos θ 1 I ^ + l 1 ⋅ sin θ 1 K ^ \vec{R}_{\mathrm{m}_1}=\vec{R}_B=l_1\cdot \cos \theta _1\hat{I}+l_1\cdot \sin \theta _1\hat{K} Rm1=RB=l1⋅cosθ1I^+l1⋅sinθ1K^
V ⃗ m 1 = R ⃗ ˙ m 1 = ( − θ ˙ 1 l 1 ⋅ sin θ 1 ) I ^ + ( θ ˙ 1 l 1 ⋅ cos θ 1 ) K ^ \vec{V}_{\mathrm{m}_1}=\dot{\vec{R}}_{\mathrm{m}_1}=\left( -\dot{\theta}_1l_1\cdot \sin \theta _1 \right) \hat{I}+\left( \dot{\theta}_1l_1\cdot \cos \theta _1 \right) \hat{K} Vm1=R˙m1=(−θ˙1l1⋅sinθ1)I^+(θ˙1l1⋅cosθ1)K^
K 1 = 1 2 m 1 ( V ⃗ m 1 ) 2 = 1 2 θ ˙ 1 2 l 1 2 m 1 , P 1 = m 1 ( g K ^ ) ⋅ R ⃗ m 1 = m 1 g l 1 sin θ 1 K_1=\frac{1}{2}m_1\left( \vec{V}_{\mathrm{m}_1} \right) ^2=\frac{1}{2}{\dot{\theta}_1}^2{l_1}^2m_1, P_1=m_1\left( g\hat{K} \right) \cdot \vec{R}_{\mathrm{m}_1}=m_1gl_1\sin \theta _1 K1=21m1(Vm1)2=21θ˙12l12m1,P1=m1(gK^)⋅Rm1=m1gl1sinθ1 -
对于构件2而言:
R ⃗ m 2 = R ⃗ C = R ⃗ B + R ⃗ B C = [ l 1 ⋅ cos θ 1 + l 2 ⋅ cos ( θ 1 + θ 2 ) ] I ^ + [ l 1 ⋅ sin θ 1 + l 2 ⋅ sin ( θ 1 + θ 2 ) ] K ^ \vec{R}_{\mathrm{m}_2}=\vec{R}_{\mathrm{C}}=\vec{R}_{\mathrm{B}}+\vec{R}_{\mathrm{BC}}=\left[ l_1\cdot \cos \theta _1+l_2\cdot \cos \left( \theta _1+\theta _2 \right) \right] \hat{I}+\left[ l_1\cdot \sin \theta _1+l_2\cdot \sin \left( \theta _1+\theta _2 \right) \right] \hat{K} Rm2=RC=RB+RBC=[l1⋅cosθ1+l2⋅cos(θ1+θ2)]I^+[l1⋅sinθ1+l2⋅sin(θ1+θ2)]K^
V ⃗ m 2 = R ⃗ ˙ m 2 = [ − θ ˙ 1 l 1 ⋅ sin θ 1 − ( θ ˙ 1 + θ ˙ 2 ) l 2 ⋅ sin ( θ 1 + θ 2 ) ] I ^ + [ θ ˙ 1 l 1 ⋅ cos θ 1 + ( θ ˙ 1 + θ ˙ 2 ) l 2 ⋅ cos ( θ 1 + θ 2 ) ] K ^ \vec{V}_{\mathrm{m}_2}=\dot{\vec{R}}_{\mathrm{m}_2}=\left[ -\dot{\theta}_1l_1\cdot \sin \theta _1-\left( \dot{\theta}_1+\dot{\theta}_2 \right) l_2\cdot \sin \left( \theta _1+\theta _2 \right) \right] \hat{I}+\left[ \dot{\theta}_1l_1\cdot \cos \theta _1+\left( \dot{\theta}_1+\dot{\theta}_2 \right) l_2\cdot \cos \left( \theta _1+\theta _2 \right) \right] \hat{K} Vm2=R˙m2=[−θ˙1l1⋅sinθ1−(θ˙1+θ˙2)l2⋅sin(θ1+θ2)]I^+[θ˙1l1⋅cosθ1+(θ˙1+θ˙2)l2⋅cos(θ1+θ2)]K^
K 2 = 1 2 m 2 ( V ⃗ m 2 ) 2 = 1 2 m 2 [ θ ˙ 1 2 l 1 2 + ( θ ˙ 1 + θ ˙ 2 ) 2 l 2 2 + 2 θ ˙ 1 ( θ ˙ 1 + θ ˙ 2 ) l 1 l 2 cos θ 2 ] K_2=\frac{1}{2}m_2\left( \vec{V}_{\mathrm{m}_2} \right) ^2=\frac{1}{2}m_2\left[ {\dot{\theta}_1}^2{l_1}^2+\left( \dot{\theta}_1+\dot{\theta}_2 \right) ^2{l_2}^2+2\dot{\theta}_1\left( \dot{\theta}_1+\dot{\theta}_2 \right) l_1l_2\cos \theta _2 \right] K2=21m2(Vm2)2=21m2[θ˙12l12+(θ˙1+θ˙2)2l22+2θ˙1(θ˙1+θ˙2)l1l2cosθ2]
P 2 = m 2 ( g K ^ ) ⋅ R ⃗ m 2 = m 1 g [ l 1 sin θ 1 + l 2 ⋅ sin ( θ 1 + θ 2 ) ] \,\,P_2=m_2\left( g\hat{K} \right) \cdot \vec{R}_{\mathrm{m}_2}=m_1g\left[ l_1\sin \theta _1+l_2\cdot \sin \left( \theta _1+\theta _2 \right) \right] P2=m2(gK^)⋅Rm2=m1g[l1sinθ1+l2⋅sin(θ1+θ2)] -
对于该系统,则有:
L = K − P = ( K 1 + K 2 ) − ( P 1 + P 2 ) L=K-P=\left( K_1+K_2 \right) -\left( P_1+P_2 \right) L=K−P=(K1+K2)−(P1+P2)
= 1 2 θ ˙ 1 2 l 1 2 m 1 + 1 2 m 2 [ θ ˙ 1 2 l 1 2 + ( θ ˙ 1 + θ ˙ 2 ) 2 l 2 2 + 2 θ ˙ 1 ( θ ˙ 1 + θ ˙ 2 ) l 1 l 2 cos θ 2 ] − m 1 g l 1 sin θ 1 − m 2 g [ l 1 sin θ 1 + l 2 ⋅ sin ( θ 1 + θ 2 ) ] =\frac{1}{2}{\dot{\theta}_1}^2{l_1}^2m_1+\frac{1}{2}m_2\left[ {\dot{\theta}_1}^2{l_1}^2+\left( \dot{\theta}_1+\dot{\theta}_2 \right) ^2{l_2}^2+2\dot{\theta}_1\left( \dot{\theta}_1+\dot{\theta}_2 \right) l_1l_2\cos \theta _2 \right] -m_1gl_1\sin \theta _1-m_2g\left[ l_1\sin \theta _1+l_2\cdot \sin \left( \theta _1+\theta _2 \right) \right] =21θ˙12l12m1+21m2[θ˙12l12+(θ˙1+θ˙2)2l22+2θ˙1(θ˙1+θ˙2)l1l2cosθ2]−m1gl1sinθ1−m2g[l1sinθ1+l2⋅sin(θ1+θ2)]
= 1 2 ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 + 2 m 2 l 1 l 2 cos θ 2 ) θ ˙ 1 2 + 1 2 m 2 θ ˙ 2 2 l 2 2 + ( m 2 l 2 2 + m 2 l 1 l 2 cos θ 2 ) θ ˙ 1 θ ˙ 2 − ( m 1 g l 1 + m 2 g l 1 ) sin θ 1 − m 2 g l 2 ⋅ sin ( θ 1 + θ 2 ) =\frac{1}{2}\left( m_1{l_1}^2+m_2{l_1}^2+m_2{l_2}^2+2m_2l_1l_2\cos \theta _2 \right) {\dot{\theta}_1}^2+\frac{1}{2}m_2{\dot{\theta}_2}^2{l_2}^2+\left( m_2{l_2}^2+m_2l_1l_2\cos \theta _2 \right) \dot{\theta}_1\dot{\theta}_2-\left( m_1gl_1+m_2gl_1 \right) \sin \theta _1-m_2gl_2\cdot \sin \left( \theta _1+\theta _2 \right) =21(m1l12+m2l12+m2l22+2m2l1l2cosθ2)θ˙12+21m2θ˙22l22+(m2l22+m2l1l2cosθ2)θ˙1θ˙2−(m1gl1+m2gl1)sinθ1−m2gl2⋅sin(θ1+θ2)
= 1 2 ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 ) θ ˙ 1 2 + 1 2 m 2 l 2 2 θ ˙ 2 2 + m 2 l 1 l 2 cos θ 2 θ ˙ 1 2 + m 2 l 2 2 θ ˙ 1 θ ˙ 2 + m 2 l 1 l 2 θ ˙ 1 θ ˙ 2 cos θ 2 − ( m 1 g l 1 + m 2 g l 1 ) sin θ 1 − m 2 g l 2 ⋅ sin ( θ 1 + θ 2 ) =\frac{1}{2}\left( m_1{l_1}^2+m_2{l_1}^2+m_2{l_2}^2 \right) {\dot{\theta}_1}^2+\frac{1}{2}m_2{l_2}^2{\dot{\theta}_2}^2+m_2l_1l_2\cos \theta _2{\dot{\theta}_1}^2+m_2{l_2}^2\dot{\theta}_1\dot{\theta}_2+m_2l_1l_2\dot{\theta}_1\dot{\theta}_2\cos \theta _2-\left( m_1gl_1+m_2gl_1 \right) \sin \theta _1-m_2gl_2\cdot \sin \left( \theta _1+\theta _2 \right) =21(m1l12+m2l12+m2l22)θ˙12+21m2l22θ˙22+m2l1l2cosθ2θ˙12+m2l22θ˙1θ˙2+m2l1l2θ˙1θ˙2cosθ2−(m1gl1+m2gl1)sinθ1−m2gl2⋅sin(θ1+θ2) -
求解力矩 τ 1 \tau _1 τ1可得:
τ 1 = d d t ∂ L ∂ θ ˙ 1 − ∂ L ∂ θ 1 = d d t ( ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 ) θ ˙ 1 + 2 m 2 l 1 l 2 θ ˙ 1 cos θ 2 + m 2 l 2 2 θ ˙ 2 + m 2 l 1 l 2 θ ˙ 2 cos θ 2 ) + ( m 1 g l 1 + m 2 g l 1 ) cos θ 1 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) = ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 ) θ ¨ 1 + 2 m 2 l 1 l 2 θ ¨ 1 cos θ 2 − 2 m 2 l 1 l 2 θ ˙ 1 θ ˙ 2 sin θ 2 + m 2 l 2 2 θ ¨ 2 + m 2 l 1 l 2 θ ¨ 2 cos θ 2 − m 2 l 1 l 2 θ ˙ 2 2 sin θ 2 + ( m 1 g l 1 + m 2 g l 1 ) cos θ 1 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) = ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 + 2 m 2 l 1 l 2 cos θ 2 ) θ ¨ 1 + ( m 2 l 2 2 + m 2 l 1 l 2 cos θ 2 ) θ ¨ 2 + ( − 2 m 2 l 1 l 2 θ ˙ 2 sin θ 2 ) θ ˙ 1 − m 2 l 1 l 2 sin θ 2 θ ˙ 2 2 + ( m 1 g l 1 + m 2 g l 1 ) cos θ 1 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) \tau _1=\frac{\mathrm{d}}{\mathrm{dt}}\frac{\partial L}{\partial \dot{\theta}_1}-\frac{\partial L}{\partial \theta _1}=\frac{\mathrm{d}}{\mathrm{dt}}\left( \left( m_1{l_1}^2+m_2{l_1}^2+m_2{l_2}^2 \right) \dot{\theta}_1+\,\,2m_2l_1l_2\dot{\theta}_1\cos \theta _2+m_2{l_2}^2\dot{\theta}_2+m_2l_1l_2\dot{\theta}_2\cos \theta _2 \right) +\left( m_1gl_1+m_2gl_1 \right) \cos \theta _1+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) \\ =\left( m_1{l_1}^2+m_2{l_1}^2+m_2{l_2}^2 \right) \ddot{\theta}_1+2m_2l_1l_2\ddot{\theta}_1\cos \theta _2-2m_2l_1l_2\dot{\theta}_1\dot{\theta}_2\sin \theta _2+m_2{l_2}^2\ddot{\theta}_2+m_2l_1l_2\ddot{\theta}_2\cos \theta _2-m_2l_1l_2{\dot{\theta}_2}^2\sin \theta _2+\left( m_1gl_1+m_2gl_1 \right) \cos \theta _1+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) \\ =\left( m_1{l_1}^2+m_2{l_1}^2+m_2{l_2}^2+2m_2l_1l_2\cos \theta _2 \right) \ddot{\theta}_1+\left( m_2{l_2}^2+m_2l_1l_2\cos \theta _2 \right) \ddot{\theta}_2+\left( -2m_2l_1l_2\dot{\theta}_2\sin \theta _2 \right) \dot{\theta}_1-m_2l_1l_2\sin \theta _2{\dot{\theta}_2}^2+\left( m_1gl_1+m_2gl_1 \right) \cos \theta _1+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) τ1=dtd∂θ˙1∂L−∂θ1∂L=dtd((m1l12+m2l12+m2l22)θ˙1+2m2l1l2θ˙1cosθ2+m2l22θ˙2+m2l1l2θ˙2cosθ2)+(m1gl1+m2gl1)cosθ1+m2gl2⋅cos(θ1+θ2)=(m1l12+m2l12+m2l22)θ¨1+2m2l1l2θ¨1cosθ2−2m2l1l2θ˙1θ˙2sinθ2+m2l22θ¨2+m2l1l2θ¨2cosθ2−m2l1l2θ˙22sinθ2+(m1gl1+m2gl1)cosθ1+m2gl2⋅cos(θ1+θ2)=(m1l12+m2l12+m2l22+2m2l1l2cosθ2)θ¨1+(m2l22+m2l1l2cosθ2)θ¨2+(−2m2l1l2θ˙2sinθ2)θ˙1−m2l1l2sinθ2θ˙22+(m1gl1+m2gl1)cosθ1+m2gl2⋅cos(θ1+θ2) -
求解力矩 τ 2 \tau _2 τ2可得:
τ 2 = d d t ∂ L ∂ θ ˙ 2 − ∂ L ∂ θ 2 = d d t ( m 2 l 2 2 θ ˙ 2 + m 2 l 2 2 θ ˙ 1 + m 2 l 1 l 2 θ ˙ 1 cos θ 2 ) + m 2 l 1 l 2 sin θ 2 θ ˙ 1 2 + m 2 l 1 l 2 θ ˙ 1 θ ˙ 2 sin θ 2 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) = m 2 l 2 2 θ ¨ 2 + m 2 l 2 2 θ ¨ 1 + m 2 l 1 l 2 cos θ 2 θ ¨ 1 + m 2 l 1 l 2 sin θ 2 θ ˙ 1 2 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) = ( m 2 l 2 2 + m 2 l 1 l 2 cos θ 2 ) θ ¨ 1 + m 2 l 2 2 θ ¨ 2 + m 2 l 1 l 2 sin θ 2 θ ˙ 1 2 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) \tau _2=\frac{\mathrm{d}}{\mathrm{dt}}\frac{\partial L}{\partial \dot{\theta}_2}-\frac{\partial L}{\partial \theta _2}=\frac{\mathrm{d}}{\mathrm{dt}}\left( m_2{l_2}^2\dot{\theta}_2+m_2{l_2}^2\dot{\theta}_1+m_2l_1l_2\dot{\theta}_1\cos \theta _2 \right) +m_2l_1l_2\sin \theta _2{\dot{\theta}_1}^2+m_2l_1l_2\dot{\theta}_1\dot{\theta}_2\sin \theta _2+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) \\ =m_2{l_2}^2\ddot{\theta}_2+m_2{l_2}^2\ddot{\theta}_1+m_2l_1l_2\cos \theta _2\ddot{\theta}_1+m_2l_1l_2\sin \theta _2{\dot{\theta}_1}^2+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) \\ =\left( m_2{l_2}^2+m_2l_1l_2\cos \theta _2 \right) \ddot{\theta}_1+m_2{l_2}^2\ddot{\theta}_2+m_2l_1l_2\sin \theta _2{\dot{\theta}_1}^2+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right) τ2=dtd∂θ˙2∂L−∂θ2∂L=dtd(m2l22θ˙2+m2l22θ˙1+m2l1l2θ˙1cosθ2)+m2l1l2sinθ2θ˙12+m2l1l2θ˙1θ˙2sinθ2+m2gl2⋅cos(θ1+θ2)=m2l22θ¨2+m2l22θ¨1+m2l1l2cosθ2θ¨1+m2l1l2sinθ2θ˙12+m2gl2⋅cos(θ1+θ2)=(m2l22+m2l1l2cosθ2)θ¨1+m2l22θ¨2+m2l1l2sinθ2θ˙12+m2gl2⋅cos(θ1+θ2) -
整理可得:
[ τ 1 τ 2 ] = [ ( m 1 + m 2 ) l 1 2 + m 2 l 2 2 + 2 m 2 l 1 l 2 cos θ 2 m 2 l 2 2 + m 2 l 1 l 2 cos θ 2 m 2 l 2 2 + m 2 l 1 l 2 cos θ 2 m 2 l 2 2 ] [ θ ¨ 1 θ ¨ 2 ] + [ − 2 m 2 l 1 l 2 θ ˙ 2 sin θ 2 θ ˙ 1 − m 2 l 1 l 2 sin θ 2 θ ˙ 2 2 m 2 l 1 l 2 sin θ 2 θ ˙ 1 2 ] + [ ( m 1 g l 1 + m 2 g l 1 ) cos θ 1 + m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) m 2 g l 2 ⋅ cos ( θ 1 + θ 2 ) ] \left[ \begin{array}{c} \tau _1\\ \tau _2\\ \end{array} \right] =\left[ \begin{matrix} \left( m_1+m_2 \right) {l_1}^2+m_2{l_2}^2+2m_2l_1l_2\cos \theta _2& m_2{l_2}^2+m_2l_1l_2\cos \theta _2\\ m_2{l_2}^2+m_2l_1l_2\cos \theta _2& m_2{l_2}^2\\ \end{matrix} \right] \left[ \begin{array}{c} \ddot{\theta}_1\\ \ddot{\theta}_2\\ \end{array} \right] +\left[ \begin{array}{c} -2m_2l_1l_2\dot{\theta}_2\sin \theta _2\dot{\theta}_1-m_2l_1l_2\sin \theta _2{\dot{\theta}_2}^2\\ m_2l_1l_2\sin \theta _2{\dot{\theta}_1}^2\\ \end{array} \right] +\left[ \begin{array}{c} \left( m_1gl_1+m_2gl_1 \right) \cos \theta _1+m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right)\\ m_2gl_2\cdot \cos \left( \theta _1+\theta _2 \right)\\ \end{array} \right] [τ1τ2]=[(m1+m2)l12+m2l22+2m2l1l2cosθ2m2l22+m2l1l2cosθ2m2l22+m2l1l2cosθ2m2l22][θ¨1θ¨2]+[−2m2l1l2θ˙2sinθ2θ˙1−m2l1l2sinθ2θ˙22m2l1l2sinθ2θ˙12]+[(m1gl1+m2gl1)cosθ1+m2gl2⋅cos(θ1+θ2)m2gl2⋅cos(θ1+θ2)]
即: τ = M ( θ ) θ ¨ + c ( θ , θ ˙ ) + g ( θ ) = M ( θ ) θ ¨ + h ( θ , θ ˙ ) \tau =M\left( \theta \right) \ddot{\theta}+c\left( \theta ,\dot{\theta} \right) +g\left( \theta \right) =M\left( \theta \right) \ddot{\theta}+h\left( \theta ,\dot{\theta} \right) τ=M(θ)θ¨+c(θ,θ˙)+g(θ)=M(θ)θ¨+h(θ,θ˙)
Screw theory, exponential coordinate, and Product of Exponential (PoE) are based on the (linear) differential equation view of robot kinematics.
1.1.2 else method?
1.2 Linear Differential Equations (Autonomous/ɔː’tɒnəməs/)
- Linear Differential Equation:ODEs that are linear wrt variables
eg1:
{ x ˙ 1 ( t ) + x 2 ( t ) = 0 x ˙ 2 ( t ) + x 1 ( t ) + x 2 ( t ) = 0 \begin{cases} \dot{x}_1\left( t \right) +x_2\left( t \right) =0\\ \dot{x}_2\left( t \right) +x_1\left( t \right) +x_2\left( t \right) =0\\ \end{cases} {x˙1(t)+x2(t)=0x˙2(t)+x1(t)+x2(t)=0
x ( t ) = [ x 1 ( t ) x 2 ( t ) ] x ˙ ( t ) = [ x ˙ 1 ( t ) x ˙ 2 ( t ) ] = [ 0 − 1 − 1 − 1 ] [ x 1 ( t ) x 2 ( t ) ] = A x ( t ) x\left( t \right) =\left[ \begin{array}{c} x_1\left( t \right)\\ x_2\left( t \right)\\ \end{array} \right] \\ \dot{x}\left( t \right) =\left[ \begin{array}{c} \dot{x}_1\left( t \right)\\ \dot{x}_2\left( t \right)\\ \end{array} \right] =\left[ \begin{matrix} 0& -1\\ -1& -1\\ \end{matrix} \right] \left[ \begin{array}{c} x_1\left( t \right)\\ x_2\left( t \right)\\ \end{array} \right] =Ax\left( t \right) x(t)=[x1(t)x2(t)]x˙(t)=[x˙1(t)x˙2(t)]=[0−1−1−1][x1(t)x2(t)]=Ax(t)
eg2:
{ y ¨ ( t ) + z ( t ) = 0 z ˙ ( t ) + y ( t ) = 0 \begin{cases} \ddot{y}\left( t \right) +z\left( t \right) =0\\ \dot{z}\left( t \right) +y\left( t \right) =0\\ \end{cases} {y¨(t)+z(t)=0z˙(t)+y(t)=0
→ x 1 ( t ) = y ( t ) , x 2 ( t ) = y ˙ ( t ) , x 3 ( t ) = z ( t ) x ˙ ( t ) = [ x ˙ 1 ( t ) x ˙ 2 ( t ) x ˙ 3 ( t ) ] = [ 0 1 0 0 0 − 1 − 1 0 0 ] [ x 1 ( t ) x 2 ( t ) x 3 ( t ) ] = A x ( t ) \rightarrow x_1\left( t \right) =y\left( t \right) ,x_2\left( t \right) =\dot{y}\left( t \right) ,x_3\left( t \right) =z\left( t \right) \\ \dot{x}\left( t \right) =\left[ \begin{array}{c} \dot{x}_1\left( t \right)\\ \dot{x}_2\left( t \right)\\ \dot{x}_3\left( t \right)\\ \end{array} \right] =\left[ \begin{matrix} 0& 1& 0\\ 0& 0& -1\\ -1& 0& 0\\ \end{matrix} \right] \left[ \begin{array}{c} x_1\left( t \right)\\ x_2\left( t \right)\\ x_3\left( t \right)\\ \end{array} \right] =Ax\left( t \right) →x1(t)=y(t),x2(t)=y˙(t),x3(t)=z(t)x˙(t)= x˙1(t)x˙2(t)x˙3(t) = 00−11000−10 x1(t)x2(t)x3(t) =Ax(t)
- State-space form(1-st-order ODE with vector variables)
Linear x ˙ = A x \dot{x}=Ax x˙=Ax——vector field
1.3 General Linear Control Systems
-
General(Autonomous) Dynamical Systems: x ˙ ( t ) = f ( x ( t ) ) \dot{x}\left( t \right) =f\left( x\left( t \right) \right) x˙(t)=f(x(t))
x ( t ) ∈ R n x\left( t \right) \in \mathbb{R} ^n x(t)∈Rn——state vector, f : R n → R n f:\mathbb{R} ^n\rightarrow \mathbb{R} ^n f:Rn→Rn——vector field
“Autonomous” means ‘f’ does not depend on non-‘x’ variables—— x ˙ = A x + b \dot{x}=Ax+b x˙=Ax+b -
Non-autonomous: x ˙ ( t ) = f ( x ( t ) , t ) \dot{x}\left( t \right) =f\left( x\left( t \right) ,t \right) x˙(t)=f(x(t),t)
captures all non-‘x’ dependence—— x ˙ = A x + 2 t , x ˙ = A x + b ( t ) \dot{x}=Ax+2t,\dot{x}=Ax+b\left( t \right) x˙=Ax+2t,x˙=Ax+b(t) -
Control System: x ˙ ( t ) = f ( x ( t ) , u ( t ) ) \dot{x}\left( t \right) =f\left( x\left( t \right) ,u\left( t \right) \right) x˙(t)=f(x(t),u(t))
vector field f : R n → R m f:\mathbb{R} ^n\rightarrow \mathbb{R} ^m f:Rn→Rm depends on external variable u ( t ) ∈ R m u\left( t \right) \in \mathbb{R} ^m u(t)∈Rm—— x ˙ = A x + sin ( u ) = f ( x , u ) \dot{x}=Ax+\sin \left( u \right) =f\left( x,u \right) x˙=Ax+sin(u)=f(x,u) -
General Linear Control Systems:
{ x ˙ ( t ) = A x ( t ) + B u ( t ) y ( t ) = C x ( t ) + D u ( t ) \begin{cases} \dot{x}\left( t \right) =Ax\left( t \right) +Bu\left( t \right)\\ y\left( t \right) =Cx\left( t \right) +Du\left( t \right)\\ \end{cases} {x˙(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t), with x ( 0 ) = x 0 x\left( 0 \right) =x_0 x(0)=x0
x ∈ R n x\in \mathbb{R} ^n x∈Rn——system state
, u ∈ R m u\in \mathbb{R} ^m u∈Rm——control input
, y ∈ R p y\in \mathbb{R} ^p y∈Rp——system output
y ( t ) = C x ( t ) + D u ( t ) y\left( t \right) =Cx\left( t \right) +Du\left( t \right) y(t)=Cx(t)+Du(t)——static relation(无微分项)
1.4 Existence and Uniqueness of ODE Solutions
- Function:
g
:
R
n
→
R
p
g:\mathbb{R} ^n\rightarrow \mathbb{R} ^p
g:Rn→Rp is called Lipschitz over domain
D
⊆
R
n
\mathcal{D} \subseteq \mathbb{R} ^n
D⊆Rn if
∃
L
<
∞
\exists L<\infty
∃L<∞
∥ g ( x ) − g ( x ′ ) ∥ ⩽ L ∥ x − x ′ ∥ , ∀ x , x ′ ∈ D \left\| g\left( x \right) -g\left( x\prime \right) \right\| \leqslant L\left\| x-x\prime \right\| ,\forall x,x\prime\in \mathcal{D} ∥g(x)−g(x′)∥⩽L∥x−x′∥,∀x,x′∈D
smooth, continuous, piecewise(分段) continuous—— g ( x ) = ∣ x ∣ g\left( x \right) =\sqrt{\left| x \right|} g(x)=∣x∣
直觉上,利普希茨连续函数限制了函数改变的速度,符合利普希茨条件的函数的斜率,必小于一个称为利普希茨常数的实数(该常数依函数而定)——学习一下
- Theorem [Existense & Uniqueness] Nonlinear ODE
x ˙ ( t ) = f ( x ( t ) , t ) , I . C . x ( t 0 ) = x 0 \dot{x}\left( t \right) =f\left( x\left( t \right) ,t \right) , \mathrm{I}.\mathrm{C}. x\left( t_0 \right) =x_0 x˙(t)=f(x(t),t),I.C.x(t0)=x0
has a unique solution if f ( x , t ) f\left( x,t \right) f(x,t) is Lipschitz in x x x and piecewise continuous in t t t
I . C . \mathrm{I}.\mathrm{C}. I.C.——initial condition
上述的唯一解(unique solution)指的是给定初值问题(Initial Value Problem, IVP),存在且只存在一个解决方案。具体来说,对于给定的初始条件 ,存在且只存在一个函数x(t),在区间内满足微分方程和初始条件。
证明这一唯一解的存在性和唯一性通常基于庞加莱-林德洛夫(Peano-Lindelöf)定理或者皮卡-林德洛夫(Picard-Lindelöf)定理,这两者都属于常微分方程理论的基本结果。
- Corollary/kə’rɒlərɪ/推论:Linear system
x
˙
(
t
)
=
A
x
(
t
)
+
B
u
(
t
)
\dot{x}\left( t \right) =Ax\left( t \right) +Bu\left( t \right)
x˙(t)=Ax(t)+Bu(t) has a unique solution for any piecewise continuous input
u
(
t
)
u\left( t \right)
u(t) +
I
.
C
.
\mathrm{I}.\mathrm{C}.
I.C.
f ( x , t ) = A x + B u ( t ) ⇒ ∥ f ( x , t ) − f ( x ′ , t ) ∥ = ∥ A x − A x ′ ∥ ⩽ ∥ A ∥ ∥ x − x ′ ∥ f\left( x,t \right) =Ax+Bu\left( t \right) \\ \Rightarrow \left\| f\left( x,t \right) -f\left( x\prime,t \right) \right\| =\left\| Ax-Ax\prime \right\| \leqslant \left\| A \right\| \left\| x-x\prime \right\| f(x,t)=Ax+Bu(t)⇒∥f(x,t)−f(x′,t)∥=∥Ax−Ax′∥⩽∥A∥∥x−x′∥
且 x x x is fixed
Suppose A A A becomes time-varying A ( t ) A\left( t \right) A(t), can you derive conditions to ensure existence and uniqueness of x ˙ ( t ) = A ( t ) x ( t ) + B u ( t ) \dot{x}\left( t \right) =A\left( t \right) x\left( t \right) +Bu\left( t \right) x˙(t)=A(t)x(t)+Bu(t)
矩阵函数 A(t) 应在所关心的时间区间内连续。
矩阵函数 A(t) 应在所关心的时间区间内连续。存在常数 M,使得 (t) 的范数在整个时间区间内都受到 M 的限制
输入函数 u(t) 应在时间区间上是分段连续的。
2. Matrix Exponential
2.1 How to Solve Linear Differential Equations?
General linear ODE: x ˙ ( t ) = A x ( t ) + d ( t ) = A x + B u ( t ) \dot{x}\left( t \right) =Ax\left( t \right) +d\left( t \right) =Ax+Bu\left( t \right) x˙(t)=Ax(t)+d(t)=Ax+Bu(t)
- The key is to derive solutions to the autonomous linear case: x ˙ ( t ) = A x ( t ) \dot{x}\left( t \right) =Ax\left( t \right) x˙(t)=Ax(t), with x ( t ) ∈ R n x\left( t \right) \in \mathbb{R} ^n x(t)∈Rn, A ∈ R n × n A\in \mathbb{R} ^{n\times n} A∈Rn×n and initial condition (IC) x ( 0 ) = x 0 x\left( 0 \right) =x_0 x(0)=x0
- By existence and unqueness theorem, the ODE x ˙ = A x \dot{x}=Ax x˙=Ax admits a unique solution.
- It turns out that the solution can be found analytically via the Matrix Exponential
2.2 What is the “Euler’s Number” e ?
Consider a scalar linear system : z ( t ) ∈ R n z\left( t \right) \in \mathbb{R} ^n z(t)∈Rn and a ∈ R a\in \mathbb{R} a∈R is a constant, z ˙ ( t ) = a z ( t ) \dot{z}\left( t \right) =az\left( t \right) z˙(t)=az(t) with IC z ( 0 ) = z 0 z\left( 0 \right) =z_0 z(0)=z0
- The above ODE has a unique solution: z ( t ) = z 0 e a t z\left( t \right) =z_0e^{at} z(t)=z0eat
- What is the number “e”?
- Euler’s Number
- Defined as the number such that: ( e x ) ′ = e x \left( e^x \right) \prime=e^x (ex)′=ex—— e = lim h → 0 ( h + 1 ) 1 h e=\underset{h\rightarrow 0}{\lim}\left( h+1 \right) ^{\frac{1}{h}} e=h→0lim(h+1)h1
2.3 What is the “Euler’s Number” e ?
For real variable
x
∈
R
x\in \mathbb{R}
x∈R, Taylor series expansion for
e
x
e^x
ex around
x
=
0
x=0
x=0:
e
x
=
∑
k
=
0
∞
z
k
k
!
=
1
+
z
+
z
2
2
!
+
z
3
3
!
+
⋯
e^x=\sum_{k=0}^{\infty}{\frac{z^k}{k!}}=1+z+\frac{z^2}{2!}+\frac{z^3}{3!}+\cdots
ex=k=0∑∞k!zk=1+z+2!z2+3!z3+⋯
This can be extended to complex variables:
e
z
=
∑
k
=
0
∞
z
k
k
!
=
1
+
z
+
z
2
2
!
+
z
3
3
!
+
⋯
e^z=\sum_{k=0}^{\infty}{\frac{z^k}{k!}}=1+z+\frac{z^2}{2!}+\frac{z^3}{3!}+\cdots
ez=k=0∑∞k!zk=1+z+2!z2+3!z3+⋯, this power series is well defined for all
z
∈
C
z\in \mathbb{C}
z∈C
In particular, we have:
e
j
θ
=
∑
k
=
0
∞
x
k
k
!
=
1
+
j
θ
−
θ
2
2
!
−
j
θ
3
3
!
+
⋯
e^{j\theta}=\sum_{k=0}^{\infty}{\frac{x^k}{k!}}=1+j\theta -\frac{\theta ^2}{2!}-j\frac{\theta ^3}{3!}+\cdots
ejθ=∑k=0∞k!xk=1+jθ−2!θ2−j3!θ3+⋯
Comparing with Taylor expansions for
cos
θ
\cos \theta
cosθ and
sin
θ
\sin \theta
sinθ leads to the Euler’s Formula
sin
θ
=
θ
−
θ
3
3
!
+
θ
5
5
!
−
θ
7
7
!
+
⋯
,
cos
θ
=
1
−
θ
2
2
!
+
θ
4
4
!
+
⋯
\sin \theta =\theta -\frac{\theta ^3}{3!}+\frac{\theta ^5}{5!}-\frac{\theta ^7}{7!}+\cdots ,\cos \theta =1-\frac{\theta ^2}{2!}+\frac{\theta ^4}{4!}+\cdots
sinθ=θ−3!θ3+5!θ5−7!θ7+⋯,cosθ=1−2!θ2+4!θ4+⋯
Euler’s Formula
e
j
θ
=
cos
θ
+
j
sin
θ
e^{j\theta}=\cos \theta +j\sin \theta
ejθ=cosθ+jsinθ
2.4 Matrix Exponential Definition
- Similar to the ral and complex cases, we can define the so-called matrix
exponential/ˌekspə'nenʃ(ə)l/
e A = ∑ k = 0 ∞ A k k ! = I + A + A 2 2 ! + A 3 3 ! + ⋯ e^A=\sum_{k=0}^{\infty}{\frac{A^k}{k!}}=I+A+\frac{A^2}{2!}+\frac{A^3}{3!}+\cdots eA=k=0∑∞k!Ak=I+A+2!A2+3!A3+⋯
eg: A = [ 0 1 0 0 ] A=\left[ \begin{matrix} 0& 1\\ 0& 0\\ \end{matrix} \right] A=[0010], A 2 = [ 0 0 0 0 ] A^2=\left[ \begin{matrix} 0& 0\\ 0& 0\\ \end{matrix} \right] A2=[0000]
e A = ∑ k = 0 ∞ A k k ! = I + A + A 2 2 ! + A 3 3 ! + ⋯ = I + A = [ 1 1 0 1 ] e^A=\sum_{k=0}^{\infty}{\frac{A^k}{k!}}=I+A+\frac{A^2}{2!}+\frac{A^3}{3!}+\cdots =I+A=\left[ \begin{matrix} 1& 1\\ 0& 1\\ \end{matrix} \right] eA=∑k=0∞k!Ak=I+A+2!A2+3!A3+⋯=I+A=[1011]
- This power series is well defined for any finite square martix
import numpy as np
import math
import scipy.linalg as la
import matplotlib.pyplot as plt
A = np.mat('1,2;3,4')
def matrixExp(A): # this is not a numerically stable way to compute matrix exponent
eA = ny.eye(len(A))
for i in rang(15):
eA = eA + la.fractional_martix_power(A,i+1)/math.factorial(i+1)
return eA
print(la.expm(A))
print(matrixExp(A))
- Some Important Properties of Matrix Exponential
A e A = e A A Ae^A=e^AA AeA=eAA
but A e B = e B A , A B ≠ B A Ae^B=e^BA,AB\ne BA AeB=eBA,AB=BA
e A e B = e A + B , A B = B A e^Ae^B=e^{A+B},AB=BA eAeB=eA+B,AB=BA
A = P D P − 1 , e A = P e D P − 1 A=PDP^{-1},e^A=Pe^DP^{-1} A=PDP−1,eA=PeDP−1
for every t , τ ∈ R t,\tau \in \mathbb{R} t,τ∈R, e A t e A τ = e A ( t + τ ) e^{At}e^{A\tau}=e^{A\left( t+\tau \right)} eAteAτ=eA(t+τ)
( e A ) − 1 = e − A \left( e^A \right) ^{-1}=e^{-A} (eA)−1=e−A
3. Solution to Linear Differential Equations
3.1 Autonomous Linear Systems
x
˙
(
t
)
=
A
x
(
t
)
\dot{x}\left( t \right) =Ax\left( t \right)
x˙(t)=Ax(t) , with initial condition
x
(
0
)
=
x
0
x\left( 0 \right) =x_0
x(0)=x0,
x
(
t
)
∈
R
,
A
∈
R
n
×
n
x\left( t \right) \in \mathbb{R} , A\in \mathbb{R} ^{n\times n}
x(t)∈R,A∈Rn×n is constant matrix,
x
0
∈
R
n
x_0\in \mathbb{R} ^n
x0∈Rn is given
With the definition of matrix exponential, we can show that the solution is given by :
x
(
t
)
=
e
A
t
x
0
x\left( t \right) =e^{At}x_0
x(t)=eAtx0
- Computation of Matrix Exponential
-
Directly from definition: e A t = ∑ k = 0 ∞ A t k k ! e^{At}=\sum_{k=0}^{\infty}{\frac{At^k}{k!}} eAt=∑k=0∞k!Atk——It’s hard to compute. But for special case, this series have analytical form(见2.4)
-
For diagonalizable martix: (见矩阵的对角化,相似矩阵,)
A = [ 1.5 − 0.5 − 0.5 1.5 ] = [ 1 1 1 − 1 ] [ 1 0 0 2 ] [ 0.5 0.5 0.5 − 0.5 ] A=\left[ \begin{matrix} 1.5& -0.5\\ -0.5& 1.5\\ \end{matrix} \right] =\left[ \begin{matrix} 1& 1\\ 1& -1\\ \end{matrix} \right] \left[ \begin{matrix} 1& 0\\ 0& 2\\ \end{matrix} \right] \left[ \begin{matrix} 0.5& 0.5\\ 0.5& -0.5\\ \end{matrix} \right] A=[1.5−0.5−0.51.5]=[111−1][1002][0.50.50.5−0.5]
⇒ e A t = [ 1 1 1 − 1 ] [ e t 0 0 e 2 t ] [ 0.5 0.5 0.5 − 0.5 ] \Rightarrow e^{At}=\left[ \begin{matrix} 1& 1\\ 1& -1\\ \end{matrix} \right] \left[ \begin{matrix} e^t& 0\\ 0& e^{2t}\\ \end{matrix} \right] \left[ \begin{matrix} 0.5& 0.5\\ 0.5& -0.5\\ \end{matrix} \right] ⇒eAt=[111−1][et00e2t][0.50.50.5−0.5] -
Using Laplace transform: x ˙ = A x , x ( 0 ) = x 0 ∈ R n \dot{x}=Ax,x\left( 0 \right) =x_0\in \mathbb{R} ^n x˙=Ax,x(0)=x0∈Rn
x ( t ) ↔ X ( s ) ∈ R n , X ( s ) = ∫ x ( t ) e − s t d t x\left( t \right) \leftrightarrow X\left( s \right) \in \mathbb{R} ^n,X\left( s \right) =\int{x\left( t \right) e^{-st}}\mathrm{d}t x(t)↔X(s)∈Rn,X(s)=∫x(t)e−stdt
x ˙ ( t ) ↔ s X ( s ) − x 0 = A X ( s ) ⇒ ( s I − A ) X ( s ) = x 0 ⇒ X ( s ) = ( s I − A ) − 1 x 0 \dot{x}\left( t \right) \leftrightarrow sX\left( s \right) -x_0=AX\left( s \right) \Rightarrow \left( sI-A \right) X\left( s \right) =x_0\Rightarrow X\left( s \right) =\left( sI-A \right) ^{-1}x_0 x˙(t)↔sX(s)−x0=AX(s)⇒(sI−A)X(s)=x0⇒X(s)=(sI−A)−1x0
x ( t ) = L − 1 [ ( s I − A ) − 1 x 0 ] ⇒ e A t = L − 1 [ ( s I − A ) − 1 x 0 ] x\left( t \right) =\mathcal{L} ^{-1}\left[ \left( sI-A \right) ^{-1}x_0 \right] \Rightarrow e^{At}=\mathcal{L} ^{-1}\left[ \left( sI-A \right) ^{-1}x_0 \right] x(t)=L−1[(sI−A)−1x0]⇒eAt=L−1[(sI−A)−1x0]
3.2 Solution to General Linear Systems
{
x
˙
(
t
)
=
A
x
(
t
)
+
B
u
(
t
)
y
(
t
)
=
C
x
(
t
)
+
D
u
(
t
)
\begin{cases} \dot{x}\left( t \right) =Ax\left( t \right) +Bu\left( t \right)\\ y\left( t \right) =Cx\left( t \right) +Du\left( t \right)\\ \end{cases}
{x˙(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t), with IC
x
(
0
)
=
x
0
x\left( 0 \right) =x_0
x(0)=x0
x
∈
R
n
x\in \mathbb{R} ^n
x∈Rn——system state,
u
∈
R
m
u\in \mathbb{R} ^m
u∈Rm——control input,
y
∈
R
p
y\in \mathbb{R} ^p
y∈Rp——system output, A, B, C, D are constant matrices with appropriate dimensions
-
The solution is given by { x ( t ) = e A t x 0 + ∫ 0 t e A ( t − τ ) B u ( τ ) d τ y ( t ) = C e A t x 0 + C ∫ 0 t e A ( t − τ ) B u ( τ ) d τ + D u ( t ) \begin{cases} x\left( t \right) =e^{At}x_0+\int_0^t{e^{A\left( t-\tau \right)}Bu\left( \tau \right)}\mathrm{d}\tau\\ y\left( t \right) =Ce^{At}x_0+C\int_0^t{e^{A\left( t-\tau \right)}Bu\left( \tau \right)}\mathrm{d}\tau +Du\left( t \right)\\ \end{cases} {x(t)=eAtx0+∫0teA(t−τ)Bu(τ)dτy(t)=CeAtx0+C∫0teA(t−τ)Bu(τ)dτ+Du(t)
-
证明见: