在 上一篇文章中,我们得到了一条
输入量
为
机体角速度
,
输出量
为
机体姿态
的微分方程:
[ ϕ ˙ θ ˙ ψ ˙ ] = [ 1 s i n ϕ t a n θ c o s ϕ t a n θ 0 c o s ϕ − s i n ϕ 0 s i n ϕ / c o s θ c o s ϕ / c o s θ ] [ ω b x ω b y ω b z ] \begin{bmatrix} \dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix}= \begin{bmatrix} 1& sin\phi tan\theta & cos\phi tan\theta \\ 0& cos\phi& -sin\phi \\ 0& sin\phi / cos\theta& cos\phi/cos\theta \end{bmatrix} \begin{bmatrix} \omega_{bx}\\ \omega_{by}\\ \omega_{bz} \end{bmatrix} ϕ˙θ˙ψ˙ = 100sinϕtanθcosϕsinϕ/cosθcosϕtanθ−sinϕcosϕ/cosθ ωbxωbyωbz
至此,为了得到动力学模型,我们还希望得到一条
输入量
为
力或者力矩
(对于姿态而言,肯定是力矩啦),
输出量
为
机体角速度
的微分方程。下面进行分析与推导。
预备知识
符号标识说明
在下面的推导中,将会出现大量的符号,下面对符号标识进行大致说明。符号右下标,表示当前符号所归属的物理域;右上标,表示对符号进行描述的物理域。如符号 ω B O \omega_{B}^{O} ωBO,表示 坐标系 B 坐标系B 坐标系B的角速度在 坐标系 O 坐标系O 坐标系O中的描述。
纯转动(无平动)矢量的微分
对于纯转动(无平动)的情况,我们先讨论无限小转动的情况。
如图所示,在
参考系
S
参考系S
参考系S中建立直角坐标系
O
x
y
z
Oxyz
Oxyz;在参考系
S
′
S'
S′中,建立直角坐标系
O
′
x
′
y
′
z
′
O'x'y'z'
O′x′y′z′。令参考系
S
S
S为惯性系,让
参考系
S
′
参考系S'
参考系S′相对
S
系
S系
S系以角速度
ω
\omega
ω转动,瞬时转轴为
z
′
轴
z'轴
z′轴,令
O
z
轴
Oz轴
Oz轴与
O
′
z
′
轴
O'z'轴
O′z′轴重合。
在极短时间 Δ t \Delta{t} Δt内, 参考系 S ′ 参考系S' 参考系S′相对 S 系 S系 S系发生了无限小转动,角位移是 Δ θ ⃗ \Delta \vec{\theta} Δθ(这是一个矢量,方向根据右手定则,指向 O z → \overrightarrow{Oz} Oz的方向,大小为 Δ θ \Delta \theta Δθ)。
设某一个矢量 r ⃗ \vec{r} r在 S ′ 系 S'系 S′系上,也跟随着 S ′ 系 S'系 S′系发生了转动,得到了 r ′ ⃗ \vec{r'} r′(图中未画出),矢量 r ⃗ \vec{r} r的变化量是 Δ r ⃗ \Delta \vec{r} Δr。
由于
Δ
θ
⃗
\Delta \vec{\theta}
Δθ是无限小量,那么
Δ
r
⃗
\Delta \vec{r}
Δr也是无限小量,此时
Δ
r
⃗
\Delta \vec{r}
Δr必与
r
⃗
\vec{r}
r和
Δ
θ
⃗
\Delta \vec{\theta}
Δθ构成的平面垂直。且有以下关系成立
∣
Δ
r
⃗
∣
=
A
M
‾
⋅
∣
Δ
θ
∣
=
∣
r
⃗
∣
sin
φ
⋅
∣
Δ
θ
⃗
∣
\begin{aligned} |\Delta \vec{r}|&=\overline{AM} \cdot |\Delta{\theta}|\\&=|\vec{r}|\sin{\varphi}\cdot |\Delta{\vec{\theta}}| \end{aligned}
∣Δr∣=AM⋅∣Δθ∣=∣r∣sinφ⋅∣Δθ∣
根据垂直和长度,我们可以根据叉乘
的定义,将上面的关系表达为
Δ
r
⃗
=
Δ
θ
⃗
×
r
⃗
\Delta \vec{r} = \Delta \vec{\theta}\times\vec{r}
Δr=Δθ×r
对上式两边分别除以
Δ
t
\Delta{t}
Δt并取极限有
lim
Δ
t
→
0
Δ
r
Δ
t
=
lim
Δ
t
→
0
(
Δ
θ
Δ
t
×
r
)
=
lim
Δ
t
→
0
(
Δ
θ
Δ
t
)
×
r
\begin{aligned} \lim_{\Delta t\to0}\frac{\Delta r}{\Delta t}& =\lim_{\Delta t\to0}(\frac{\Delta\theta}{\Delta t}\times r) \\ &=\lim_{\Delta t\to0}(\frac{\Delta\theta}{\Delta t})\times r \end{aligned}
Δt→0limΔtΔr=Δt→0lim(ΔtΔθ×r)=Δt→0lim(ΔtΔθ)×r
其中
lim
Δ
t
→
0
Δ
r
⃗
Δ
t
=
d
r
⃗
d
t
=
v
lim
Δ
t
→
0
Δ
θ
⃗
Δ
t
=
d
θ
⃗
d
t
=
ω
\begin{aligned} \lim_{\Delta t\to0}\frac{\Delta \vec{r}}{\Delta t}&=\frac{\mathrm{d}\vec{r}}{\mathrm{d}t}=v \\ \lim_{\Delta t\to0}\frac{\Delta\vec\theta}{\Delta t}&=\frac{\mathrm{d}\vec\theta}{\mathrm{d}t}=\omega \end{aligned}
Δt→0limΔtΔrΔt→0limΔtΔθ=dtdr=v=dtdθ=ω所以
d
r
⃗
d
t
=
ω
×
r
⃗
=
v
\boxed{ \frac{\mathrm{d}\vec r}{\mathrm{d}t}=\omega\times \vec r =v }
dtdr=ω×r=v其中,
v
v
v可以表示为这个矢量的速度。
可见,一个
非惯性系
S
′
非惯性系S'
非惯性系S′对一个
惯性系
S
惯性系S
惯性系S做纯转动时,
非惯性系
S
′
非惯性系S'
非惯性系S′上跟随转动的某一矢量
r
⃗
\vec r
r的微分可以表示为转动的角速度
ω
\omega
ω和矢量本身
r
⃗
\vec r
r的叉乘;又因为微分可以看作是速度,而矢量
r
⃗
\vec r
r又是跟随着
非惯性系
S
′
非惯性系S'
非惯性系S′转动,那么
ω
×
r
⃗
\omega\times\vec r
ω×r又可以看作是
非惯性系
S
′
非惯性系S'
非惯性系S′于
惯性系
S
惯性系S
惯性系S的牵连速度。
一般平面运动(转动+平动)矢量的微分(科里奥利公式)
-
定性分析
首先,可以不失一般性地假设所研究的矢量是表达位移的矢量(即位矢),那么对位矢的微分,得到的便是速度矢量。我们可以假设所研究的位矢在一个非惯性系上(比如无人机的机体坐标系),这时候位矢的所有平面运动,都可以分解成位矢跟随非惯性系对惯性系的转动
+位矢在非惯性系上的平动
。
有一个概念是很显然的:
绝对速度 = 相对速度 + 牵连速度 绝对速度=相对速度+牵连速度 绝对速度=相对速度+牵连速度
所以我们可以说,对于任何位矢 P ⃗ \vec{P} P在 非惯性系 B 非惯性系B 非惯性系B中, 非惯性系 B 非惯性系B 非惯性系B相对于 惯性系 O 惯性系O 惯性系O的纯转动角速度是 ω B O \omega_{B}^{O} ωBO的情况,结合前面纯转动(无平动)矢量的微分
分析结尾的结论:
d P ⃗ O d t = d P B ⃗ d t + ω B O × P ⃗ O \boxed{ {\mathrm{d}\vec{P}^O \over \mathrm{d}t}={\mathrm{d}\vec{P^B} \over \mathrm{d}t} +\omega_{B}^{O} \times \vec{P}^O } dtdPO=dtdPB+ωBO×PO
其中位矢 P ⃗ \vec{P} P在 惯性系 O 惯性系O 惯性系O下的微分 d P ⃗ O d t {\mathrm{d}\vec{P}^O \over \mathrm{d}t} dtdPO便是绝对速度, P ⃗ \vec{P} P在 非惯性系 B 非惯性系B 非惯性系B下的微分 d P ⃗ B d t {\mathrm{d}\vec{P}^B \over \mathrm{d}t} dtdPB便是相对速度, ω B O × P ⃗ O \omega_{B}^{O} \times \vec{P}^O ωBO×PO便是 非惯性系 B 非惯性系B 非惯性系B于 惯性系 O 惯性系O 惯性系O的牵连速度。 -
严格推导
严格的推导可以查阅:【清华大学 理论力学 高云峰】 【精准空降到 04:53】 ,此处不赘述。 -
推广至矩阵形式
上面是对矢量进行讨论,我们知道矢量是可以用列矩阵来表示的,如3维矢量可以用3行1列的列矩阵来表示,那么m行n列的矩阵,可以视作n个m维矢量的合集。假设当前存在矩阵 M = [ r ⃗ m 1 r ⃗ m 2 ⋯ r ⃗ m n ] \mathbb{M}= \begin{bmatrix} \vec{r}_{m1} & \vec{r}_{m2} & \cdots& \vec{r}_{mn} \end{bmatrix} M=[rm1rm2⋯rmn],则
M ˙ = [ d r ⃗ m 1 O d t d r ⃗ m 2 O d t ⋯ d r ⃗ m n O d t ] = [ d r ⃗ m 1 B d t + ω × r ⃗ m 1 O d r ⃗ m 2 B d t + ω × r ⃗ m 2 O … d r ⃗ m n B d t + ω × r ⃗ m n O ] \begin{aligned} \dot{\mathbb{M}}&= \begin{bmatrix} {\mathrm{d}\vec{r}_{m1}^O \over \mathrm{d}t} & {\mathrm{d}\vec{r}_{m2}^O \over \mathrm{d}t} & \cdots& {\mathrm{d}\vec{r}_{mn}^O \over \mathrm{d}t} \end{bmatrix} \\ &=\begin{bmatrix} {\mathrm{d}\vec{r}_{m1}^B \over \mathrm{d}t}+\omega \times \vec{r}_{m1}^O& {\mathrm{d}\vec{r}_{m2}^B \over \mathrm{d}t}+\omega \times\vec{r}_{m2}^O& \dots& {\mathrm{d}\vec{r}_{mn}^B \over \mathrm{d}t}+\omega \times\vec{r}_{mn}^O \end{bmatrix} \end{aligned} M˙=[dtdrm1Odtdrm2O⋯dtdrmnO]=[dtdrm1B+ω×rm1Odtdrm2B+ω×rm2O…dtdrmnB+ω×rmnO]其中,可以看作有n个m维矢量做一般平面运动,分解为:跟随着 非惯性系 B 非惯性系B 非惯性系B相对于 惯性系 O 惯性系O 惯性系O做角速度为 ω \omega ω的纯转动的同时,在 非惯性系 B 非惯性系B 非惯性系B内做相对速度是 d r ⃗ m i B d t {\mathrm{d}\vec{r}_{mi}^B \over \mathrm{d}t} dtdrmiB的平动。至此,对于这样作为矢量集合的矩阵的微分,我们也找到了表示方法。
刚体转动欧拉方程的推导
根据角动量定理,可以得到
M
O
=
d
d
t
(
J
O
ω
B
O
)
M^O= {\mathrm{d}\over\mathrm{d}t}(J^O\omega^O_B)
MO=dtd(JOωBO)其中
M
O
M^O
MO是无人机机体在
惯性系
O
惯性系O
惯性系O下受到的合外力矩,
J
O
J^O
JO是机体转动惯量3*3矩阵在
惯性系
O
惯性系O
惯性系O下的表达,
ω
B
O
\omega_B^O
ωBO是和
非惯性系
B
非惯性系B
非惯性系B固连的机体角速度在
惯性系
O
惯性系O
惯性系O下的表达。
根据求导链式法则(用符号上加点代表对符号进行微分)
d
d
t
(
J
O
ω
B
O
)
=
J
O
˙
ω
B
O
+
J
O
ω
B
O
˙
\begin{aligned} {\mathrm{d}\over\mathrm{d}t}(J^O\omega^O_B)&=\dot{J^O}\omega^O_B+{J^O}\dot{\omega^O_B} \end{aligned}
dtd(JOωBO)=JO˙ωBO+JOωBO˙其中,分析3*3转动惯量矩阵的微分
J
O
˙
\dot{J^O}
JO˙
J
O
˙
=
[
J
1
B
˙
+
ω
B
O
×
J
1
O
J
2
B
˙
+
ω
B
O
×
J
2
O
J
3
B
˙
+
ω
B
O
×
J
3
O
]
\dot{J^O}= \begin{bmatrix} \dot{J^B_1}+\omega_B^O\times J^O_1& \dot{J^B_2}+\omega_B^O\times J^O_2& \dot{J^B_3}+\omega_B^O\times J^O_3 \end{bmatrix}
JO˙=[J1B˙+ωBO×J1OJ2B˙+ωBO×J2OJ3B˙+ωBO×J3O]又因为当机体构型固定后,机体转动惯量在机体
非惯性系
B
非惯性系B
非惯性系B的表达不变,即
J
i
B
˙
≡
0
\dot{J^B_i}\equiv0
JiB˙≡0,所以
J
O
˙
=
[
ω
B
O
×
J
1
O
ω
B
O
×
J
2
O
ω
B
O
×
J
3
O
]
=
ω
B
O
×
[
J
1
O
J
2
O
J
3
O
]
=
ω
B
O
×
J
O
\begin{aligned} \dot{J^O}&= \begin{bmatrix} \omega_B^O\times J^O_1& \omega_B^O\times J^O_2& \omega_B^O\times J^O_3 \end{bmatrix} \\ &=\omega_B^O\times\begin{bmatrix} J^O_1& J^O_2& J^O_3 \end{bmatrix}\\ &=\omega_B^O\times J^O \end{aligned}
JO˙=[ωBO×J1OωBO×J2OωBO×J3O]=ωBO×[J1OJ2OJ3O]=ωBO×JO因此可以推出刚体转动欧拉方程为
M
O
=
d
d
t
(
J
O
ω
B
O
)
=
ω
B
O
×
J
O
ω
B
O
+
J
O
ω
B
O
˙
\boxed{ M^O={\mathrm{d}\over\mathrm{d}t}(J^O\omega^O_B)=\omega_B^O\times J^O\omega^O_B+{J^O}\dot{\omega^O_B} }
MO=dtd(JOωBO)=ωBO×JOωBO+JOωBO˙
无人机姿态动力学模型
我们希望得到一条输入量
为力或者力矩
(对于姿态而言,肯定是力矩啦),输出量
为机体角速度
的微分方程为作为姿态动力学模型,同时,因为在机体坐标系研究姿态更直观,所以姿态动力学模型中的参数最好是在机体坐标系下的表达。则输出量为
ω
B
\omega_B
ωB,输入量为
M
B
M^B
MB。
首先对于
L
=
J
ω
L=J\omega
L=Jω,有
L
B
=
J
B
ω
B
L
O
=
J
O
ω
O
=
J
O
R
B
O
ω
B
=
R
B
O
L
B
=
R
B
O
J
B
ω
B
\begin{aligned} L^B & =J^B\omega^B\\ L^O & =J^O\omega^O\\ &=J^OR_B^O \omega^B\\ &=R_B^OL^B\\ &=R_B^OJ^B\omega^B \end{aligned}
LBLO=JBωB=JOωO=JORBOωB=RBOLB=RBOJBωB
所以可以得到
J
O
R
B
O
ω
B
=
R
B
O
J
B
ω
B
J
O
=
R
B
O
J
B
R
B
O
−
1
\begin{aligned} J^OR^O_B\omega_B&=R_B^OJ^B\omega^B\\ J^O&=R_B^OJ^B {R^O_B}^{-1} \end{aligned}
JORBOωBJO=RBOJBωB=RBOJBRBO−1
则由刚体转动欧拉方程
M
O
=
ω
B
O
×
J
O
ω
B
O
+
J
O
ω
B
O
˙
R
B
O
M
B
=
(
R
B
O
ω
B
)
×
(
R
B
O
J
B
R
B
O
−
1
)
R
B
O
ω
B
+
R
B
O
J
B
R
B
O
−
1
(
R
B
O
ω
B
)
˙
R
B
O
M
B
=
R
B
O
[
ω
B
]
x
R
B
O
−
1
R
B
O
J
B
ω
B
+
R
B
O
J
B
R
B
O
−
1
(
R
B
O
ω
B
)
˙
M
B
=
[
ω
B
]
x
J
B
ω
B
+
J
B
R
B
O
−
1
(
R
B
O
˙
ω
B
+
R
B
O
ω
B
˙
)
\begin{aligned} M^O&=\omega_B^O\times J^O\omega^O_B+{J^O}\dot{\omega^O_B}\\ R_B^O M^B &= (R_B^O \omega_B)\times (R_B^OJ^B{R^O_B}^{-1})R_B^O \omega_B+R_B^OJ^B {R^O_B}^{-1}\dot{(R_B^O\omega_B)}\\ R_B^O M^B &= R_B^O [\omega_B]_\mathrm{x} {R_B^O}^{-1} R_B^OJ^B \omega_B+R_B^OJ^B {R^O_B}^{-1}\dot{(R_B^O\omega_B)}\\ M^B &= [\omega_B]_\mathrm{x} J^B \omega_B+J^B {R^O_B}^{-1}(\dot {R_B^O}\omega_B + R^O_B\dot{\omega_B}) \end{aligned}
MORBOMBRBOMBMB=ωBO×JOωBO+JOωBO˙=(RBOωB)×(RBOJBRBO−1)RBOωB+RBOJBRBO−1(RBOωB)˙=RBO[ωB]xRBO−1RBOJBωB+RBOJBRBO−1(RBOωB)˙=[ωB]xJBωB+JBRBO−1(RBO˙ωB+RBOωB˙)
根据【旋转矩阵】对旋转矩阵导数的推导,可以知道
R
B
O
˙
=
ω
B
O
×
R
B
O
=
[
ω
B
O
]
x
R
B
O
=
R
B
O
[
ω
B
]
x
R
B
O
−
1
R
B
O
=
R
B
O
[
ω
B
]
x
\dot{R_B^O}=\omega_B^O \times {R_B^O}=[\omega_B^O]_\mathrm{x}R_B^O=R_B^O [\omega_B]_\mathrm{x}{R_B^O}^{-1}R_B^O=R_B^O[\omega_B]_\mathrm{x}
RBO˙=ωBO×RBO=[ωBO]xRBO=RBO[ωB]xRBO−1RBO=RBO[ωB]x,则
M
B
=
[
ω
B
]
x
J
B
ω
B
+
J
B
R
B
O
−
1
(
R
B
O
˙
ω
B
+
R
B
O
ω
B
˙
)
M
B
=
[
ω
B
]
x
J
B
ω
B
+
J
B
R
B
O
−
1
(
R
B
O
[
ω
]
x
ω
B
+
R
B
O
ω
B
˙
)
M
B
=
ω
B
×
J
B
ω
B
+
J
B
R
B
O
−
1
(
R
B
O
ω
B
×
ω
B
+
R
B
O
ω
B
˙
)
\begin{aligned} M^B &= [\omega_B]_\mathrm{x} J^B \omega_B+J^B {R^O_B}^{-1}(\dot {R_B^O}\omega_B + R^O_B\dot{\omega_B})\\ M^B &= [\omega_B]_\mathrm{x} J^B \omega_B+J^B {R^O_B}^{-1}(R_B^O[\omega]_\mathrm{x}\omega_B + R^O_B\dot{\omega_B})\\ M^B &= \omega_B\times J^B \omega_B+J^B {R^O_B}^{-1}(R_B^O\omega_B\times\omega_B + R^O_B\dot{\omega_B}) \end{aligned}
MBMBMB=[ωB]xJBωB+JBRBO−1(RBO˙ωB+RBOωB˙)=[ωB]xJBωB+JBRBO−1(RBO[ω]xωB+RBOωB˙)=ωB×JBωB+JBRBO−1(RBOωB×ωB+RBOωB˙)
又根据叉乘的定义,
a
×
a
=
0
a\times a = 0
a×a=0,则
M
B
=
ω
B
×
J
B
ω
B
+
J
B
R
B
O
−
1
R
B
O
ω
B
˙
M
B
=
ω
B
×
J
B
ω
B
+
J
B
ω
B
˙
\begin{aligned} M^B &= \omega_B\times J^B \omega_B+J^B {R^O_B}^{-1}R^O_B\dot{\omega_B}\\ M^B &= \omega_B\times J^B \omega_B+J^B \dot{\omega_B} \end{aligned}
MBMB=ωB×JBωB+JBRBO−1RBOωB˙=ωB×JBωB+JBωB˙
由此便在机体坐标系下建立了姿态动力学模型。又因为
M
B
=
τ
+
G
a
M^B=\bm\tau+\bm{G_a}
MB=τ+Ga,其中
τ
\bm \tau
τ为螺旋桨为机体提供的力矩,
G
a
\bm{G_a}
Ga为机体的陀螺力矩,则姿态动力学模型又可以表达为
τ
+
G
a
=
ω
B
×
J
B
ω
B
+
J
B
ω
B
˙
\bm\tau+\bm{G_a}= \omega_B\times J^B \omega_B+J^B \dot{\omega_B}
τ+Ga=ωB×JBωB+JBωB˙