[足式机器人]Part4 南科大高等机器人控制课 Ch01 Linear Differential Equations and Matrix Exponential

2023-12-05

本文仅供学习使用
本文参考：
B站：CLEAR_LAB
课程主讲教师：
Prof. Wei Zhang

南科大高等机器人控制课 Ch01 Linear Differential Equations and Matrix Exponential

1. ODEs-Ordinary Differential Equations
2. Matrix Exponential
3. Solution to Linear Differential Equations
- 3.1 Autonomous Linear Systems
- 3.2 Solution to General Linear Systems

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} R m 1 = R B = l 1 ⋅ cos θ 1 I ^ + l 1 ⋅ sin θ 1 K ^
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} V m 1 = R ˙ m 1 = ( − θ ˙ 1 l 1 ⋅ sin θ 1 ) I ^ + ( θ ˙ 1 l 1 ⋅ 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 K 1 = 2 1 m 1 ( V m 1 ) 2 = 2 1 θ ˙ 1 2 l 1 2 m 1 , P 1 = m 1 ( g K ^ ) ⋅ R m 1 = m 1 g l 1 sin θ 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} R m 2 = R C = R B + R BC = [ l 1 ⋅ cos θ 1 + l 2 ⋅ cos ( θ 1 + θ 2 ) ] I ^ + [ l 1 ⋅ sin θ 1 + l 2 ⋅ 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} 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 ^
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] K 2 = 2 1 m 2 ( V m 2 ) 2 = 2 1 m 2 [ θ ˙ 1 2 l 1 2 + ( θ ˙ 1 + θ ˙ 2 ) 2 l 2 2 + 2 θ ˙ 1 ( θ ˙ 1 + θ ˙ 2 ) l 1 l 2 cos θ 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] P 2 = m 2 ( g K ^ ) ⋅ R m 2 = m 1 g [ l 1 sin θ 1 + l 2 ⋅ 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 = ( K 1 + K 2 ) − ( P 1 + P 2 )
= 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] = 2 1 θ ˙ 1 2 l 1 2 m 1 + 2 1 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 ) ]
= 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) = 2 1 ( 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 + 2 1 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 )
= 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) = 2 1 ( m 1 l 1 2 + m 2 l 1 2 + m 2 l 2 2 ) θ ˙ 1 2 + 2 1 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 )
求解力矩 τ 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 = dt d ∂ θ ˙ 1 ∂ L − ∂ θ 1 ∂ L = dt d ( ( 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 )
求解力矩 τ 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 = dt d ∂ θ ˙ 2 ∂ L − ∂ θ 2 ∂ L = dt d ( 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 )
整理可得：
[ τ 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 ] = [ ( 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 ) ]
即： τ = 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 ) + x 2 ( t ) = 0 x ˙ 2 ( t ) + x 1 ( t ) + x 2 ( 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 ) = [ 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 )

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 ) = 0 z ˙ ( 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) → 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 0 − 1 1 0 0 0 − 1 0 x 1 ( t ) x 2 ( t ) x 3 ( t ) = A x ( t )

State-space form(1-st-order ODE with vector variables)
Linear x ˙ = A x \dot{x}=Ax x ˙ = A x —— 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 ) ∈ R n ——state vector, f : R n → R n f:\mathbb{R} ^n\rightarrow \mathbb{R} ^n f : R n → R n ——vector field
“Autonomous” means ‘f’ does not depend on non-‘x’ variables—— x ˙ = A x + b \dot{x}=Ax+b x ˙ = A x + 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 ˙ = A x + 2 t , x ˙ = A x + 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 : R n → R m depends on external variable u ( t ) ∈ R m u\left( t \right) \in \mathbb{R} ^m u ( t ) ∈ R m —— x ˙ = A x + sin ⁡ ( u ) = f ( x , u ) \dot{x}=Ax+\sin \left( u \right) =f\left( x,u \right) x ˙ = A x + 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 ) = A x ( t ) + B u ( t ) y ( t ) = C x ( t ) + D u ( t ) , with x ( 0 ) = x 0 x\left( 0 \right) =x_0 x ( 0 ) = x 0
x ∈ R n x\in \mathbb{R} ^n x ∈ R n —— system state , u ∈ R m u\in \mathbb{R} ^m u ∈ R m —— control input , y ∈ R p y\in \mathbb{R} ^p y ∈ R p —— system output

y ( t ) = C x ( t ) + D u ( t ) y\left( t \right) =Cx\left( t \right) +Du\left( t \right) y ( t ) = C x ( t ) + D u ( 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 : R n → R p is called Lipschitz over domain D ⊆ R n \mathcal{D} \subseteq \mathbb{R} ^n D ⊆ R n 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 ( t 0 ) = x 0
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 ) = A x ( t ) + B u ( 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 ) = A x + B u ( t ) ⇒ ∥ f ( x , t ) − f ( x ′ , t ) ∥ = ∥ A x − A x ′ ∥ ⩽ ∥ 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 ) + B u ( 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 ) = A x ( t ) + d ( t ) = A x + B u ( 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 ) = A x ( t ) , with x ( t ) ∈ R n x\left( t \right) \in \mathbb{R} ^n x ( t ) ∈ R n , A ∈ R n × n A\in \mathbb{R} ^{n\times n} A ∈ R n × n and initial condition (IC) x ( 0 ) = x 0 x\left( 0 \right) =x_0 x ( 0 ) = x 0
By existence and unqueness theorem, the ODE x ˙ = A x \dot{x}=Ax x ˙ = A x 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 ) ∈ R n 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 ) = a z ( t ) with IC z ( 0 ) = z 0 z\left( 0 \right) =z_0 z ( 0 ) = z 0

The above ODE has a unique solution: z ( t ) = z 0 e a t z\left( t \right) =z_0e^{at} z ( t ) = z 0 e a t
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 ( e x ) ′ = e x —— e = lim ⁡ h → 0 ( h + 1 ) 1 h e=\underset{h\rightarrow 0}{\lim}\left( h+1 \right) ^{\frac{1}{h}} e = h → 0 lim ( h + 1 ) h 1

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 e x 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 e x = k = 0 ∑ ∞ k ! z k = 1 + z + 2 ! z 2 + 3 ! z 3 + ⋯
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 e z = k = 0 ∑ ∞ k ! z k = 1 + z + 2 ! z 2 + 3 ! z 3 + ⋯ , 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 e j θ = ∑ k = 0 ∞ k ! x k = 1 + j θ − 2 ! θ 2 − j 3 ! θ 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 e j θ = cos θ + j sin θ

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 e A = k = 0 ∑ ∞ k ! A k = I + A + 2 ! A 2 + 3 ! A 3 + ⋯

eg: A = [ 0 1 0 0 ] A=\left[ \begin{matrix} 0& 1\\ 0& 0\\ \end{matrix} \right] A = [ 0 0 1 0 ] , A 2 = [ 0 0 0 0 ] A^2=\left[ \begin{matrix} 0& 0\\ 0& 0\\ \end{matrix} \right] A 2 = [ 0 0 0 0 ]
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] e A = ∑ k = 0 ∞ k ! A k = I + A + 2 ! A 2 + 3 ! A 3 + ⋯ = I + A = [ 1 0 1 1 ]

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 A e A = e A A
but A e B = e B A , A B ≠ B A Ae^B=e^BA,AB\ne BA A e B = e B A , A B  = B A
e A e B = e A + B , A B = B A e^Ae^B=e^{A+B},AB=BA e A e B = e A + B , A B = B A
A = P D P − 1 , e A = P e D P − 1 A=PDP^{-1},e^A=Pe^DP^{-1} A = P D P − 1 , e A = P e D P − 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)} e A t e A τ = e A ( t + τ )
( e A ) − 1 = e − A \left( e^A \right) ^{-1}=e^{-A} ( e A ) − 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 ) = A x ( t ) , with initial condition x ( 0 ) = x 0 x\left( 0 \right) =x_0 x ( 0 ) = x 0 , 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 ∈ R n × n is constant matrix, x 0 ∈ R n x_0\in \mathbb{R} ^n x 0 ∈ R n 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 ) = e A t x 0

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!}} e A t = ∑ k = 0 ∞ k ! A t k ——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.5 1.5 ] = [ 1 1 1 − 1 ] [ 1 0 0 2 ] [ 0.5 0.5 0.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] ⇒ e A t = [ 1 1 1 − 1 ] [ e t 0 0 e 2 t ] [ 0.5 0.5 0.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 ˙ = A x , x ( 0 ) = x 0 ∈ R n
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 ) ∈ R n , X ( s ) = ∫ x ( t ) e − s t d t
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 ) ↔ s X ( s ) − x 0 = A X ( s ) ⇒ ( s I − A ) X ( s ) = x 0 ⇒ X ( s ) = ( s I − A ) − 1 x 0
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 [ ( s I − A ) − 1 x 0 ] ⇒ e A t = L − 1 [ ( s I − A ) − 1 x 0 ]

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 ) = A x ( t ) + B u ( t ) y ( t ) = C x ( t ) + D u ( t ) , with IC x ( 0 ) = x 0 x\left( 0 \right) =x_0 x ( 0 ) = x 0
x ∈ R n x\in \mathbb{R} ^n x ∈ R n ——system state, u ∈ R m u\in \mathbb{R} ^m u ∈ R m ——control input, y ∈ R p y\in \mathbb{R} ^p y ∈ R p ——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 ) = 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 )
证明见：

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

机器人