模型假设
- 四旋翼为刚体,不考虑变形
- 四旋翼质量不变,质量分布不变(严格对称),重力加速度不变
- 认为地面坐标系不变,为惯性坐标系
- 忽略空气阻力
- 无人机只做小角度姿态变化
- X型布局,1,3电机逆时针转;2,4电机顺时针转,如图:
变量声明
- 外部输入变量
U
=
[
U
1
U
2
U
3
U
4
]
\ U = \begin{bmatrix} U1 \\ U2 \\ U3 \\ U4 \end{bmatrix}
U=⎣⎢⎢⎡U1U2U3U4⎦⎥⎥⎤
U1表示对地坐标系下四个螺旋桨的升力和,即:
U 1 = ∑ i = 0 4 F i U1 = \sum_{i=0}^4{F_i} U1=i=0∑4Fi
再进行坐标系转化。
U2表示对地坐标系下俯仰力矩,即:
U 2 = − F 1 − F 2 + F 3 + F 4 U2 = -F1-F2+F3+F4 U2=−F1−F2+F3+F4
再进行坐标系转化。
U3表示对地坐标系下滚转力矩,即:
U 3 = F 1 − F 2 − F 3 + F 4 U3 = F1-F2-F3+F4 U3=F1−F2−F3+F4
再进行坐标系转化。
U4表示对地坐标系下偏航力矩,即:
U 4 = F 1 − F 2 + F 3 − F 4 U4 = F1-F2+F3-F4 U4=F1−F2+F3−F4
再进行坐标系转化。 - 状态变量 X = [ x y z θ ϕ ψ ] \ X = \begin{bmatrix} x \\ y \\ z \\ \theta \\ \phi \\ \psi \end{bmatrix} X=⎣⎢⎢⎢⎢⎢⎢⎡xyzθϕψ⎦⎥⎥⎥⎥⎥⎥⎤
- 输出变量 Y = [ x y z θ ϕ ψ ] \ Y = \begin{bmatrix} x \\ y \\ z \\ \theta \\ \phi \\ \psi \end{bmatrix} Y=⎣⎢⎢⎢⎢⎢⎢⎡xyzθϕψ⎦⎥⎥⎥⎥⎥⎥⎤
牛顿方程
对力的分析,采用牛顿方程
机体坐标系分析
对于机体坐标系,沿着 z b \ z^b zb轴,无人机受到四个电机升力的合力U1, F B = [ 0 0 U 1 ] \ F_B = \begin{bmatrix} 0 \\ 0 \\ U1 \end{bmatrix} FB=⎣⎡00U1⎦⎤
对地坐标系转换
F
E
=
C
B
E
×
F
B
=
[
c
o
s
θ
c
o
s
ψ
s
i
n
θ
s
i
n
ϕ
c
o
s
ψ
−
c
o
s
ϕ
s
i
n
ψ
s
i
n
θ
c
o
s
ϕ
c
o
s
ψ
+
s
i
n
ϕ
s
i
n
ψ
c
o
s
θ
s
i
n
ψ
s
i
n
θ
s
i
n
ϕ
s
i
n
ψ
+
c
o
s
ϕ
c
o
s
ψ
s
i
n
θ
c
o
s
ϕ
s
i
n
ψ
−
s
i
n
ϕ
c
o
s
ψ
−
s
i
n
θ
s
i
n
ϕ
c
o
s
θ
c
o
s
ϕ
c
o
s
θ
]
×
[
0
0
U
1
]
=
[
U
1
⋅
(
s
i
n
θ
c
o
s
ϕ
c
o
s
ψ
+
s
i
n
ϕ
s
i
n
ψ
)
U
1
⋅
(
s
i
n
θ
c
o
s
ϕ
s
i
n
ψ
−
s
i
n
ϕ
c
o
s
ψ
)
U
1
⋅
c
o
s
ϕ
c
o
s
θ
]
F_E = C_B^E \times F_B = \begin{bmatrix} cos\theta cos\psi & sin\theta sin\phi cos\psi - cos\phi sin\psi & sin\theta cos\phi cos\psi + sin\phi sin\psi \\ cos\theta sin\psi & sin\theta sin\phi sin\psi + cos\phi cos\psi & sin\theta cos\phi sin\psi - sin\phi cos\psi \\ -sin\theta & sin\phi cos\theta & cos\phi cos\theta \end{bmatrix} \times \begin{bmatrix} 0 \\ 0 \\ U1 \end{bmatrix} = \begin{bmatrix} U1 \cdot (sin\theta cos\phi cos\psi + sin\phi sin\psi) \\ U1 \cdot (sin\theta cos\phi sin\psi - sin\phi cos\psi) \\ U1 \cdot cos\phi cos\theta \end{bmatrix}
FE=CBE×FB=⎣⎡cosθcosψcosθsinψ−sinθsinθsinϕcosψ−cosϕsinψsinθsinϕsinψ+cosϕcosψsinϕcosθsinθcosϕcosψ+sinϕsinψsinθcosϕsinψ−sinϕcosψcosϕcosθ⎦⎤×⎣⎡00U1⎦⎤=⎣⎡U1⋅(sinθcosϕcosψ+sinϕsinψ)U1⋅(sinθcosϕsinψ−sinϕcosψ)U1⋅cosϕcosθ⎦⎤
由牛顿方程:
F
=
d
(
m
v
)
d
t
\ F = \frac{d(mv)}{dt}
F=dtd(mv),即力等于动量的一阶微分,得:
[
x
¨
y
¨
z
¨
]
=
[
U
1
⋅
(
s
i
n
θ
c
o
s
ϕ
c
o
s
ψ
+
s
i
n
ϕ
s
i
n
ψ
)
m
U
1
⋅
(
s
i
n
θ
c
o
s
ϕ
s
i
n
ψ
−
s
i
n
ϕ
c
o
s
ψ
)
)
m
U
1
⋅
c
o
s
ϕ
c
o
s
θ
−
m
g
m
]
\begin{bmatrix} \ddot x \\ \ddot y \\ \ddot z \end{bmatrix} = \begin{bmatrix} \frac{U1 \cdot (sin\theta cos\phi cos\psi + sin\phi sin\psi)}{m} \\ \frac{U1 \cdot (sin\theta cos\phi sin\psi - sin\phi cos\psi))}{m} \\ \frac{U1 \cdot cos\phi cos\theta-mg}{m} \end{bmatrix}
⎣⎡x¨y¨z¨⎦⎤=⎣⎢⎡mU1⋅(sinθcosϕcosψ+sinϕsinψ)mU1⋅(sinθcosϕsinψ−sinϕcosψ))mU1⋅cosϕcosθ−mg⎦⎥⎤
欧拉方程
由欧拉方程: M = d L d t = J ⋅ ω ˙ + ω × J ⋅ ω \ M = \frac{dL}{dt} = J \cdot \dot \omega + \omega \times J \cdot \omega M=dtdL=J⋅ω˙+ω×J⋅ω得:
M = [ J x x 0 0 0 J y y 0 0 0 J z z ] ⋅ [ ϕ ¨ θ ¨ ψ ¨ ] + [ ϕ ˙ θ ˙ ψ ˙ ] × [ J x x 0 0 0 J y y 0 0 0 J z z ] ⋅ [ ϕ ˙ θ ˙ ψ ˙ ] = M = \begin{bmatrix} J_{xx} & 0 & 0 \\ 0 & J_{yy} & 0 \\ 0 & 0 & J_{zz} \end{bmatrix} \cdot \begin{bmatrix} \ddot \phi \\ \ddot \theta \\ \ddot \psi \end{bmatrix} + \begin{bmatrix} \dot \phi \\ \dot \theta \\ \dot \psi \end{bmatrix} \times \begin{bmatrix} J_{xx} & 0 & 0 \\ 0 & J_{yy} & 0 \\ 0 & 0 & J_{zz} \end{bmatrix} \cdot \begin{bmatrix} \dot \phi \\ \dot \theta \\ \dot \psi \end{bmatrix} = M=⎣⎡Jxx000Jyy000Jzz⎦⎤⋅⎣⎡ϕ¨θ¨ψ¨⎦⎤+⎣⎡ϕ˙θ˙ψ˙⎦⎤×⎣⎡Jxx000Jyy000Jzz⎦⎤⋅⎣⎡ϕ˙θ˙ψ˙⎦⎤=