![cover](https://img-blog.csdnimg.cn/direct/c4bbb98e53c84786a105eb2515b6e7d5.png)
固定翼飞行器建模(一)——六自由度动力学
前两期文章讲完了气动力和气动力矩的模型,本期将来讲解一下六自由度部分的动力学与运动学方程。
前两期文章讲完了气动力和气动力矩的模型,本期将来讲解一下六自由度部分的动力学与运动学方程。
1、六自由度动力学方程
三维空间中的运动可以分为平动(线运动)和转动(角运动)。平动:前后、左右、上下;转动:滚转、俯仰、偏航。
线运动动力学方程可以表示如下:
∑
F
=
d
d
t
(
m
V
)
\sum \boldsymbol{F}=\frac{\mathrm{d}}{\mathrm{d}t}(m\boldsymbol{V})
∑F=dtd(mV)
角运动动力学方程可以表示如下:
∑
M
=
d
d
t
(
L
)
\sum \boldsymbol{M}=\frac{\mathrm{d}}{\mathrm{d}t}(\boldsymbol{L})
∑M=dtd(L)
式中,
F
\boldsymbol{F}
F、
M
\boldsymbol{M}
M为飞行器所收到的合外力、合外力距。
在动坐标系(机体坐标系)中将地面坐标系中得到的机体速度
V
\boldsymbol{V}
V及动量矩
L
\boldsymbol{L}
L向机体轴分解:
d
V
d
t
=
e
V
δ
V
δ
t
+
Ω
×
V
d
L
d
t
=
e
L
δ
L
δ
t
+
Ω
×
L
\begin{aligned}\frac{\mathrm{d}\boldsymbol{V}}{\mathrm{d}t}&=\boldsymbol{e}_{\boldsymbol{V}}\frac{\delta_{\boldsymbol{V}}}{\delta_{t}}+\boldsymbol{\Omega}\times\boldsymbol{V}\\ \frac{\mathrm{d}\boldsymbol{L}}{\mathrm{d}t}&=\boldsymbol{e}_{\boldsymbol{L}}\frac{\delta_{\boldsymbol{L}}}{\delta_{t}}+\boldsymbol{\Omega}\times\boldsymbol{L}\end{aligned}
dtdVdtdL=eVδtδV+Ω×V=eLδtδL+Ω×L
e
V
,
e
L
\boldsymbol{e}_{\boldsymbol{V}},\boldsymbol{e}_{\boldsymbol{L}}
eV,eL为沿飞行速度矢量
V
\boldsymbol{V}
V和动量矩
L
\boldsymbol{L}
L的单位向量;
δ
V
δ
t
,
δ
L
δ
t
\frac{\delta_{\boldsymbol{V}}}{\delta_{t}},\frac{\delta_{\boldsymbol{L}}}{\delta_{t}}
δtδV,δtδL为动坐标系内的相对导数;
Ω
\boldsymbol{\Omega}
Ω为动坐标系相对静坐标系的总角速度向量。
将
Ω
\boldsymbol{\Omega}
Ω,
V
\boldsymbol{V}
V,
L
\boldsymbol{L}
L在动坐标系(机体系)中分解:
V
=
i
u
+
j
v
+
k
w
Ω
=
i
p
+
j
q
+
k
r
L
=
i
(
p
I
x
−
r
I
x
z
)
+
j
(
q
I
y
)
+
k
(
r
I
z
−
p
I
x
z
)
\begin{aligned} \boldsymbol{V} & =\boldsymbol{i} u+\boldsymbol{j} v+\boldsymbol{k} w \\ \boldsymbol{\Omega} & =\boldsymbol{i} p+\boldsymbol{j} q+\boldsymbol{k} r \\ \boldsymbol{L} & =\boldsymbol{i}\left(p I_x-r I_{x z}\right)+\boldsymbol{j}\left(q I_y\right)+\boldsymbol{k}\left(r I_z-p I_{x z}\right) \end{aligned}
VΩL=iu+jv+kw=ip+jq+kr=i(pIx−rIxz)+j(qIy)+k(rIz−pIxz)
据此给出下面六自由度方程组(详细推导过程可参考《飞行控制系统》):
- 力方程组(Force Equations)
{ u ˙ = v r − w q − g sin θ + F x m v ˙ = − u r + w p + g cos θ sin ϕ + F y m w ˙ = u q − v p + g cos θ cos ϕ + F z m \begin{cases}\dot{u}=vr-wq-g\sin\theta+\frac{F_x}m\\ \dot{v}=-ur+wp+g\cos\theta\sin\phi+\frac{F_y}m\\ \dot{w}=uq-vp+g\cos\theta\cos\phi+\frac{F_z}m\end{cases} ⎩ ⎨ ⎧u˙=vr−wq−gsinθ+mFxv˙=−ur+wp+gcosθsinϕ+mFyw˙=uq−vp+gcosθcosϕ+mFz
式中, F x , F y , F z F_x,F_y,F_z Fx,Fy,Fz为机体系下气动力和发动力所产生的力的合力; g g g为重力加速度 u , v , w u,v,w u,v,w为机体系下三轴速度矢量; u ˙ , v ˙ , w ˙ \dot{u},\dot{v},\dot{w} u˙,v˙,w˙为机体系下三轴加速度矢量; p , q , r p,q,r p,q,r为机体系下三轴角速度。 - 运动学方程组(Kinematic Equations)
{ ϕ ˙ = p + ( r cos ϕ + q sin ϕ ) tan θ θ ˙ = q cos ϕ − r sin ϕ ψ ˙ = 1 cos θ ( r cos ϕ + q sin ϕ ) \begin{cases}\dot{\phi}=p+(r\text{cos }\phi+q\text{sin }\phi)\tan\theta\\[1ex] \dot{\theta}=q\text{cos }\phi-r\text{sin }\phi\\[1ex] \dot{\psi}=\dfrac{1}{\cos\theta}(r\cos\phi+q\text{sin }\phi)\end{cases} ⎩ ⎨ ⎧ϕ˙=p+(rcos ϕ+qsin ϕ)tanθθ˙=qcos ϕ−rsin ϕψ˙=cosθ1(rcosϕ+qsin ϕ)
式中, ϕ , θ , ψ \phi,\theta,\psi ϕ,θ,ψ为姿态欧拉角; ϕ ˙ , θ ˙ , ψ ˙ \dot{\phi},\dot{\theta},\dot{\psi} ϕ˙,θ˙,ψ˙为姿态角变化率。这里需要注意 ϕ ˙ , θ ˙ , ψ ˙ \dot{\phi},\dot{\theta},\dot{\psi} ϕ˙,θ˙,ψ˙与 p , q , r p,q,r p,q,r的区别, ϕ ˙ , θ ˙ , ψ ˙ \dot{\phi},\dot{\theta},\dot{\psi} ϕ˙,θ˙,ψ˙是在北东地地面坐标系下角度变化率, p , q , r p,q,r p,q,r是在机体系下的机体角速度;两者之间互相转化需要乘以对应的旋转矩阵,转换关系如下:
{ p = ϕ ˙ − ϕ ˙ sin θ q = θ ˙ cos ϕ + ψ ˙ cos θ sin ϕ r = − θ ˙ sin ϕ + ψ ˙ cos θ cos ϕ \begin{cases}p=\dot{\phi}-\dot{\phi}\sin\theta\\q=\dot{\theta}\cos\phi+\dot{\psi}\cos\theta\sin\phi\\r=-\dot{\theta}\sin\phi+\dot{\psi}\cos\theta\cos\phi&\end{cases} ⎩ ⎨ ⎧p=ϕ˙−ϕ˙sinθq=θ˙cosϕ+ψ˙cosθsinϕr=−θ˙sinϕ+ψ˙cosθcosϕ
{ ϕ ˙ = p + ( r cos ϕ + q sin ϕ ) tan θ θ ˙ = q cos ϕ − r sin ϕ ψ ˙ = 1 cos θ ( r cos ϕ + q sin ϕ ) \begin{cases}\dot{\phi}=p+(r\cos\phi+q\sin\phi)\tan\theta\\\dot{\theta}=q\cos\phi-r\sin\phi\\\dot{\psi}=\frac{1}{\cos\theta}(r\cos\phi+q\sin\phi)\end{cases} ⎩ ⎨ ⎧ϕ˙=p+(rcosϕ+qsinϕ)tanθθ˙=qcosϕ−rsinϕψ˙=cosθ1(rcosϕ+qsinϕ) - 力矩方程组(Moment Equations)
{ p ˙ = ( c 1 r + c 2 p ) q + c 3 L + c 4 N q ˙ = c 5 p r − c 6 ( p 2 − r 2 ) + c 7 M r ˙ = ( c 8 p − c 2 r ) q + c 4 L + c 9 N \begin{cases}\dot{p}=(c_1r+c_2p)q+c_3L+c_4N\\\dot{q}=c_5pr-c_6(p^2-r^2)+c_7M\\\dot{r}=(c_8p-c_2r)q+c_4L+c_9N\end{cases} ⎩ ⎨ ⎧p˙=(c1r+c2p)q+c3L+c4Nq˙=c5pr−c6(p2−r2)+c7Mr˙=(c8p−c2r)q+c4L+c9N
式中: L , M , N L,M,N L,M,N为机体系下受到的合外力矩; p ˙ , q ˙ , r ˙ \dot{p},\dot{q},\dot{r} p˙,q˙,r˙为机体角速度变化率; c 1 − c 9 c_1-c_9 c1−c9表达如下:
c 1 = ( I y − I z ) I z − I x z 2 Σ , c 2 = ( I x − I y + I z ) I x z Σ , c 3 = I z Σ c 4 = I x x ∑ , c 5 = I z − I x I y , c 6 = I x x I y , c 7 = 1 I y c 8 = I x ( I x − I y ) + I x x 2 ∑ , c 9 = I x ∑ , Σ = I x I x − I x x 2 \begin{aligned} & c_1=\frac{\left(I_y-I_z\right) I_z-I_{x z}^2}{\Sigma}, \quad c_2=\frac{\left(I_x-I_y+I_z\right) I_{x z}}{\Sigma}, \quad c_3=\frac{I_z}{\Sigma} \\ & c_4=\frac{I_{x x}}{\sum}, \quad c_5=\frac{I_z-I_x}{I_y}, \quad c_6=\frac{I_{x x}}{I_y}, \quad c_7=\frac{1}{I_y} \\ & c_8=\frac{I_x\left(I_x-I_y\right)+I_{x x}^2}{\sum}, \quad c_9=\frac{I_x}{\sum}, \quad \Sigma=I_x I_x-I_{x x}^2 \end{aligned} c1=Σ(Iy−Iz)Iz−Ixz2,c2=Σ(Ix−Iy+Iz)Ixz,c3=ΣIzc4=∑Ixx,c5=IyIz−Ix,c6=IyIxx,c7=Iy1c8=∑Ix(Ix−Iy)+Ixx2,c9=∑Ix,Σ=IxIx−Ixx2 - 导航方程组(Navigation Equations)
{ x ˙ g = u cos θ cos ψ + v ( sin ϕ sin θ cos ψ − cos ϕ sin ψ ) + w ( sin ϕ sin ψ + cos ϕ sin θ cos ψ ) y ˙ g = u cos θ sin ψ + v ( sin ϕ sin θ sin ψ + cos ϕ cos ψ ) + w ( − sin ϕ cos ψ + cos ϕ sin θ sin ψ ) h ˙ = u sin θ − v sin ϕ cos θ − w cos ϕ cos θ \begin{cases} \dot{x}_g=u\cos\theta\cos\psi+v(\sin\phi\sin\theta\cos\psi-\cos\phi\sin\psi)+w(\sin\phi\sin\psi+\cos\phi\sin\theta\cos\psi)\\ \dot{y}_g=u\cos\theta\sin\psi+v(\sin\phi\sin\theta\sin\psi+\cos\phi\cos\psi)+w(-\sin\phi\cos\psi+\cos\phi\sin\theta\sin\psi)\\ \dot{h}=u\sin\theta-v\sin\phi\cos\theta-w\cos\phi\cos\theta \end{cases} ⎩ ⎨ ⎧x˙g=ucosθcosψ+v(sinϕsinθcosψ−cosϕsinψ)+w(sinϕsinψ+cosϕsinθcosψ)y˙g=ucosθsinψ+v(sinϕsinθsinψ+cosϕcosψ)+w(−sinϕcosψ+cosϕsinθsinψ)h˙=usinθ−vsinϕcosθ−wcosϕcosθ
式中, x g ˙ , y g ˙ , h ˙ \dot{x_g},\dot{y_g},\dot{h} xg˙,yg˙,h˙为位置变化率,
2、Simulink 6DOF
下面介绍一下Simulink中的六自由度动力学模块。
六自由度动力学模块以刚体质心为参考点,输入量为作用在质心的合外力与和外力矩。输出为北东地速度
v
n
,
v
e
,
v
d
v_n,v_e,v_d
vn,ve,vd;北东地位置
x
e
,
y
e
,
z
e
x_e,y_e,z_e
xe,ye,ze;姿态欧拉角
ϕ
,
θ
,
ψ
\phi,\theta,\psi
ϕ,θ,ψ;北东地坐标系到机体系的旋转矩阵
D
C
M
b
e
DCM_{be}
DCMbe;机体系下速度
u
,
v
,
w
u,v,w
u,v,w;机体角速度
p
,
q
,
r
p,q,r
p,q,r;机体角加速度
p
˙
,
q
˙
,
r
˙
\dot{p},\dot{q},\dot{r}
p˙,q˙,r˙;机体系下加速度
u
˙
,
v
˙
,
w
˙
\dot{u},\dot{v},\dot{w}
u˙,v˙,w˙。
按住ctrl+u可以进入查看封装模块内部构造。
内部构造可以大致按照第一节中讲述的几大方程组进行划分。
- 力方程组计算出 u ˙ , v ˙ , w ˙ \dot{u},\dot{v},\dot{w} u˙,v˙,w˙,之后通过积分获得 u , v , w u,v,w u,v,w;
- 力矩方程组计算出 p ˙ , q ˙ , r ˙ \dot{p},\dot{q},\dot{r} p˙,q˙,r˙,之后通过积分获得 p , q , r p,q,r p,q,r;
- 运动学方程输入
p
,
q
,
r
p,q,r
p,q,r后进行姿态解算,获得欧拉角和旋转矩阵;
- 导航部分先使用旋转矩阵将 u , v , w u,v,w u,v,w转换得到 v n , v e , v d v_n,v_e,v_d vn,ve,vd,对 v n , v e , v d v_n,v_e,v_d vn,ve,vd积分得到 x e , y e , z e x_e,y_e,z_e xe,ye,ze
3、小结
综上,我们完成了六自由度动力学部分的介绍。在实际工程中,只要定义好合外力与合外力矩的输入,就可以使用Simulink中现有的的6 DOF模块进行状态求解。
下一期将会带来固定翼飞行器在Simulink中的建模、配平及线性化的讲解。
技术交流查看评论区。
END
迅翼SwiftWing致力于固定翼技术共享,汇聚固定翼领域技术极客,推动固定翼技术持续创新!
![Logo](https://devpress.csdnimg.cn/79de2bf0b7994defa4242ef90d5513fa.jpg)
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)