动力学仿真笔记.pdf

PLCT xuzhongxing@iscas.ac.cn 1 h q⊤ = [x , x , x , ρ , ρ , ρ , ρ ] = [x, ρ] 1 2 3 0 1 2 3 v⊤ = [x˙ , x˙ , x˙ , ω , ω , ω ] = [ν, ω] 1 2 3 1 2 3 g⊤ = [0, 0, −9.8, 0, 0, 0] z z V = −mg x 3 3 2 1 θ = ∥ω ∥h 2 ω δρ = [cosθ, sinθ] ∥ω ∥ 1

3 2 ρk+1 = δρ ∗ ρk 3 3 3 q ∈ R , v ∈ R Mvk+1 = Mvk + hmg qk+1 = qk + hvk+1 M q 4 L = Iω s b dL dL s s s = R + ω × I ω dt dt s b body body R b b b L I ω s dL b b s s s = RI ω˙ + ω × I ω dt s s s s

ω × I ω gyroscopic torque.

gyro 0 q 7 v 6 g 6 Mvk+1 = Mvk + hmg q

5 3 ω h n = ω ∥ω ∥

θ = ∥ω ∥h θ θ δρ = [cos , nsin ] 2 2 body ρk+1 = δρ ∗ ρk xk+1 = xk + hνk+1 5 DAE

HHT Lagrange L = T − V + λ⊤ϕ

Lagrange d ∂T + ∂V − G⊤λ = 0 dt ∂q˙⊤ ∂q⊤ ϕ = 0 G ϕ Jacobian T (q) ∈ R7 ×6 6 ∂ϕ G = T (q) ∂q Rubin Ungar 1957

Lagrange λ = 1 ϕ, ϵ → 0 ϵ