当前位置:   article > 正文

四旋翼无人机动力学模型及控制

无人机动力学模型

Overview

我想通过这个系列的笔记,给大家分享关于四旋翼无人机的控制基础知识 (其中包括一些前沿的论文导读)。主要的思路如下:

  1. 先从robotics的基础—欧拉角与旋转矩阵开始,引入四旋翼无人机的控制动力学模型。然后通过matlab实现简单的控制算法。
  2. 介绍旋转矩阵的gimbal lock(死锁/万象锁)问题,同时介绍目前robotics旋转建模中最受欢迎的模型:quaternion(四元数)。以及对应的matlab代码(如何通过quaternion修改对四旋翼无人机的建模,并做简单控制)。
  3. 四旋翼无人机differential flatness(微分平坦)的详细推导,以及如何利用微分平坦特性进行控制。

以上的知识点在很多其它文章/百科里面都有提到,我只是想把它们系统的整理结合起来,通过我认为容易理解的方式呈现出来,同时分享如何用简单的代码实现。我会尽可能的引用我能找到的原文出处。内容估计有不严谨的地方,希望跟大家一起学习交流。

我们这一篇,就从欧拉角与旋转矩阵开始。

欧拉角与旋转矩阵

就拿无人机来说,想要表示它在 R 3 \mathbb{R}^3 R3中的旋转,我们可以首先定义三个轴(Z-Y-X)如下图Fig. 1所示。

Fig. 1

Fradi, H., Bracco, L., Canino, F. and Dugelay, J.L., 2018, September. Autonomous person detection and tracking framework using unmanned aerial vehicles (UAVs). In 2018 26th European Signal Processing Conference (EUSIPCO) (pp. 1047-1051). IEEE.

我们可以把X轴想象成鸟的躯干,Y轴是鸟的翅膀,Z轴从下往上垂直穿过鸟的身体平面。有旋转,就一定要有一个参考坐标系(第三人称视角坐标) N e x {}_N\mathbf{e}_x Nex, N e y {}_N\mathbf{e}_y Ney, N e z {}_N\mathbf{e}_z Nez。我们定义当前位置大地地面正北方为 N e x = [ 1 , 0 , 0 ] T {}_N\mathbf{e}_x=[1,0,0]^T Nex=[1,0,0]T,正东方为 N e y = [ 0 , 1 , 0 ] T {}_N\mathbf{e}_y=[0,1,0]^T Ney=[0,1,0]T,垂直地面正上方为 N e z = [ 0 , 0 , 1 ] T {}_N\mathbf{e}_z=[0,0,1]^T Nez=[0,0,1]T

定义好全局参考系(global frame)之后,我们需要定义本地参考系(body frame),也就是第一人称视角坐标。鸟躯干为 B e x = [ 1 , 0 , 0 ] T {}_B\mathbf{e}_x=[1,0,0]^T Bex=[1,0,0]T,头为正方向。鸟翅膀为 B e y = [ 0 , 1 , 0 ] T {}_B\mathbf{e}_y=[0,1,0]^T Bey=[0,1,0]T, 右手为正方向。 B e z = [ 0 , 0 , 1 ] T {}_B\mathbf{e}_z=[0,0,1]^T Bez=[0,0,1]T从下往上垂直穿过鸟身体平面。

到这里有人会问,你的 N e x {}_N\mathbf{e}_x Nex B e x {}_B\mathbf{e}_x Bex都用 [ 1 , 0 , 0 ] T [1,0,0]^T [1,0,0]T表示(另外两轴也同理),那怎么去区分呢?其实它们表示的是在各自视角下的方向,我们先不考虑位移(假设两个坐标系原点重合),那么只有当这只鸟头冲北,右翅膀朝东,身体平行地面的时候,两个坐标系才一致。我们假设这只鸟身体还是平行地面( N e z = B e z {}_N\mathbf{e}_z={}_B\mathbf{e}_z Nez=Bez),但是头逆时针旋转了 ψ = 3 0 ∘ \psi=30^{\circ} ψ=30,此时从第三人称视角看过去,鸟的头方向 B e x = [ 1 , 0 , 0 ] T {}_B\mathbf{e}_x=[1,0,0]^T Bex=[1,0,0]T变成多少呢?我们画一个从上往下的俯视图Fig. 2

Fig. 2

很明显,此时如果一个向量 m \mathbf{m} m在body frame下面写为:

  1. B m = [ B m x , 0 , 0 ] T {}_B\mathbf{m}=[{}_Bm_x,0,0]^T Bm=[Bmx,0,0]T, 那么在global frame下面就可以看做 N m = [ B m x ⋅ c o s ψ , B m x ⋅ s i n ψ , 0 ] T {}_N\mathbf{m}=[{}_Bm_x \cdot cos\psi, {}_Bm_x \cdot sin\psi, 0]^T Nm=[Bmxcosψ,Bmxsinψ,0]T
  2. B m = [ 0 , B m y , 0 ] T {}_B\mathbf{m}=[0,{}_Bm_y,0]^T Bm=[0,Bmy,0]T,那么 N m = [ B m x ⋅ − s i n ψ , B m x ⋅ c o s ψ , 0 ] T {}_N\mathbf{m}=[{}_Bm_x \cdot -sin\psi, {}_Bm_x \cdot cos\psi, 0]^T Nm=[Bmxsinψ,Bmxcosψ,0]T
  3. B m = [ 0 , 0 , B m z ] T {}_B\mathbf{m}=[0,0,{}_Bm_z]^T Bm=[0,0,Bmz]T N m = [ 0 , 0 , B m z ] T = B m {}_N\mathbf{m}=[0,0,{}_Bm_z]^T={}_B\mathbf{m} Nm=[0,0,Bmz]T=Bm

也就是说,如果我有一个向量 B m = [ B m x , B m y , B m z ] T {}_B\mathbf{m}=[{}_Bm_x,{}_Bm_y,{}_Bm_z]^T Bm=[Bmx,Bmy,Bmz]T,那么:

  1. N e x {}_N\mathbf{e}_x Nex轴上: B m x {}_Bm_x Bmx会分解 B m x ⋅ c o s ψ {}_Bm_x \cdot cos\psi Bmxcosψ N e x {}_N\mathbf{e}_x Nex轴上, B m y {}_Bm_y Bmy会分解 B m y ⋅ − s i n ψ {}_Bm_y \cdot -sin\psi Bmysinψ N e x {}_N\mathbf{e}_x Nex轴上。 B m z {}_Bm_z Bmz无分解。
  2. N e y {}_N\mathbf{e}_y Ney轴上: B m x {}_Bm_x Bmx会分解 B m x ⋅ s i n ψ {}_Bm_x \cdot sin\psi Bmxsinψ N e y {}_N\mathbf{e}_y Ney轴上, B m y {}_Bm_y Bmy会分解 B m y ⋅ c o s ψ {}_Bm_y \cdot cos\psi Bmycosψ N e y {}_N\mathbf{e}_y Ney轴上。 B m z {}_Bm_z Bmz无分解。
  3. N e z {}_N\mathbf{e}_z Nez轴上: B m x {}_Bm_x Bmx无分解, B m y {}_Bm_y Bmy无分解, B m z {}_Bm_z Bmz会分解 B m z ⋅ 1 {}_Bm_z \cdot 1 Bmz1 N e z {}_N\mathbf{e}_z Nez轴上。

我们可以用矩阵来描述 B m {}_B\mathbf{m} Bm N m {}_N\mathbf{m} Nm之间的关系:

Eq. 1
N ψ m = R z ( ψ ) ⋅ B m = [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\psi}\mathbf{m}=R_z(\psi)\cdot{}_B\mathbf{m}=

[c(ψ)s(ψ)0s(ψ)c(ψ)0001]
\cdot
[BmxBmyBmz]
Nψm=Rz(ψ)Bm= c(ψ)s(ψ)0s(ψ)c(ψ)0001 BmxBmyBmz
ps: s ( ⋅ ) = s i n ( ⋅ ) , c ( ⋅ ) = c o s ( ⋅ ) , t ( ⋅ ) = t a n ( ⋅ ) s(\cdot)=sin(\cdot), c(\cdot)=cos(\cdot), t(\cdot)=tan(\cdot) s()=sin(),c()=cos(),t()=tan()

由于此时的 m \mathbf{m} m向量只经过了一次旋转(Z), 还剩下两次旋转(Y)与(X),所以我把第一次旋转后的 N m {}_N\mathbf{m} Nm暂时写为 N ψ m {}_{N\psi}\mathbf{m} Nψm。在新的坐标系 N ψ e {}_{N\psi}\mathbf{e} Nψe下,我们继续旋转,不过此时围绕 B e y {}_B\mathbf{e}_y Bey轴,旋转角度为 θ \theta θ。用同样的方法,我们可以得到 B m {}_{B}\mathbf{m} Bm N θ ψ m {}_{N\theta\psi}\mathbf{m} ψm之间的关系:

Eq. 2
N θ ψ m = R y ( θ ) ⋅ N ψ m = R y ( θ ) ⋅ R z ( ψ ) ⋅ B m = [ c ( θ ) 0 s ( θ ) 0 1 0 − s ( θ ) 0 c ( θ ) ] ⋅ [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\theta\psi}\mathbf{m}=R_y(\theta)\cdot{}_{N\psi}\mathbf{m}=R_y(\theta)\cdot R_z(\psi) \cdot {}_B\mathbf{m}=

[c(θ)0s(θ)010s(θ)0c(θ)]
\cdot
[c(ψ)s(ψ)0s(ψ)c(ψ)0001]
\cdot
[BmxBmyBmz]
ψm=Ry(θ)Nψm=Ry(θ)Rz(ψ)Bm= c(θ)0s(θ)010s(θ)0c(θ) c(ψ)s(ψ)0s(ψ)c(ψ)0001 BmxBmyBmz

最后,我们沿着 B e x {}_B\mathbf{e}_x Bex旋转 ϕ \phi ϕ。同理, B m {}_{B}\mathbf{m} Bm N ϕ θ ψ m {}_{N\phi\theta\psi}\mathbf{m} Nϕθψm之间的关系如下:

Eq. 3
N ϕ θ ψ m = R x ( ϕ ) ⋅ R y ( θ ) ⋅ R z ( ψ ) ⋅ B m = [ 1 0 0 0 c ( ϕ ) − s ( ϕ ) 0 s ( ϕ ) c ( ϕ ) ] ⋅ [ c ( θ ) 0 s ( θ ) 0 1 0 − s ( θ ) 0 c ( θ ) ] ⋅ [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\phi\theta\psi}\mathbf{m}=R_x(\phi)\cdot R_y(\theta)\cdot R_z(\psi) \cdot {}_B\mathbf{m}=

[1000c(ϕ)s(ϕ)0s(ϕ)c(ϕ)]
\cdot
[c(θ)0s(θ)010s(θ)0c(θ)]
\cdot
[c(ψ)s(ψ)0s(ψ)c(ψ)0001]
\cdot
[BmxBmyBmz]
Nϕθψm=Rx(ϕ)Ry(θ)Rz(ψ)Bm= 1000c(ϕ)s(ϕ)0s(ϕ)c(ϕ) c(θ)0s(θ)010s(θ)0c(θ) c(ψ)s(ψ)0s(ψ)c(ψ)0001 BmxBmyBmz

最终,我们把这三个旋转矩阵乘起来,可以得到如下等式:

Eq. 4
N ϕ θ ψ m = R z y x ( ψ , θ , ϕ ) ⋅ B m = [ c ( θ ) c ( ψ ) s ( ϕ ) s ( θ ) c ( ψ ) − c ( ϕ ) s ( ψ ) c ( ϕ ) s ( θ ) c ( ψ ) + s ( ϕ ) s ( ψ ) c ( θ ) s ( ψ ) s ( ϕ ) s ( θ ) s ( ψ ) + c ( ϕ ) c ( ψ ) c ( ϕ ) s ( θ ) s ( ψ ) − s ( ϕ ) c ( ψ ) − s ( θ ) s ( ϕ ) c ( θ ) c ( ϕ ) c ( θ ) ] ⋅ [ B m x B m y B m z ] {}_{N\phi\theta\psi}\mathbf{m}=R_{zyx}(\psi,\theta,\phi)\cdot {}_B\mathbf{m}=

[c(θ)c(ψ)s(ϕ)s(θ)c(ψ)c(ϕ)s(ψ)c(ϕ)s(θ)c(ψ)+s(ϕ)s(ψ)c(θ)s(ψ)s(ϕ)s(θ)s(ψ)+c(ϕ)c(ψ)c(ϕ)s(θ)s(ψ)s(ϕ)c(ψ)s(θ)s(ϕ)c(θ)c(ϕ)c(θ)]
\cdot
[BmxBmyBmz]
Nϕθψm=Rzyx(ψ,θ,ϕ)Bm= c(θ)c(ψ)c(θ)s(ψ)s(θ)s(ϕ)s(θ)c(ψ)c(ϕ)s(ψ)s(ϕ)s(θ)s(ψ)+c(ϕ)c(ψ)s(ϕ)c(θ)c(ϕ)s(θ)c(ψ)+s(ϕ)s(ψ)c(ϕ)s(θ)s(ψ)s(ϕ)c(ψ)c(ϕ)c(θ) BmxBmyBmz

说了这么半天,到底什么是欧拉角,什么是旋转矩阵呢?
其实欧拉角就是我们分别沿着(Z-Y-X)旋转时对应的三个角度( ψ , θ , ϕ \psi, \theta, \phi ψ,θ,ϕ)。每次旋转时,所对应的 R z ( ψ ) , R y ( θ ) , R x ( ϕ ) R_z(\psi), R_y(\theta), R_x(\phi) Rz(ψ),Ry(θ),Rx(ϕ),包括它们三个的乘积 R z y x ( ϕ , θ , ψ ) R_{zyx}(\phi,\theta,\psi) Rzyx(ϕ,θ,ψ),都可以称之为旋转矩阵。其实旋转矩阵也不止这一个形式,如果我们调整旋转的顺序(Z-X-Y, X-Z-Y…)都会得到不一样的旋转矩阵,甚至也有Z-X-Z, Z-Y-Z, X-Z-X等旋转顺序。但无论哪种旋转顺序,我们都可以从原始姿态旋转成任意一个姿态。

这里必须提一下旋转矩阵的重要性质,如果你对每一个旋转矩阵求行列式,你会发现无论旋转角度是多少,它们结果都等于1。也就说明:

  1. 一个向量旋转前后,模长保持不变
  2. 由于旋转前后可以用矩阵来描述,说明旋转是一个线性操作。那么两个向量旋转前夹角为 α \alpha α,经过同样的旋转后,夹角仍为 α \alpha α
  3. 同理可知,两个向量先叉乘再旋转,等于先各自旋转再叉乘。说明旋转前后相对姿态保持不变。

以上三个性质,大家有兴趣可以去验证一下。

至此,我们在已知欧拉角的情况下,能够把body frame下的向量转换为global frame下。这对于无人机的动力学建模至关重要,因为无人机里面的IMU传感器,只能给我们提供欧拉角,body frame下的加速度 [ B a x , B a y , B a z ] T [{}_Ba_x, {}_Ba_y, {}_Ba_z]^T [Bax,Bay,Baz]T和角速度 [ B ω x , B ω y , B ω z ] T [{}_B\omega_x, {}_B\omega_y, {}_B\omega_z]^T [Bωx,Bωy,Bωz]T。但是我们控制的时候,需要global frame下面的位置以及速度等状态信息,方便我们控制无人机的飞行轨迹。比如我们想控制无人机从 [ 0 , 0 , 0 ] T [0,0,0]^T [0,0,0]T点飞往 [ 10 , 10 , 5 ] T [10,10,5]^T [10,10,5]T点,这些坐标都是global frame下的。我们都知道,无人机四个螺旋桨转起来,会提供一个 B e z {}_B\mathbf{e}_z Bez方向向上的力 f t ⋅ B e z f_t\cdot{}_B\mathbf{e}_z ftBez. 此时,我们就可以通过旋转矩阵,将其转换到global frame下,,与重力 m ⋅ g = m ⋅ [ 0 , 0 , − 9.81 ] T m\cdot\mathbf{g}=m\cdot[0,0,-9.81]^T mg=m[0,0,9.81]T求向量和,得到合力方向, 进而通过ODE解出global frame下的速度与位置。

这一篇的最后,我想再补充一个内容:欧拉角的导数 [ ϕ ˙ , θ ˙ , ψ ˙ ] T [\dot{\phi}, \dot{\theta}, \dot{\psi}]^ T [ϕ˙,θ˙,ψ˙]T与body frame angular velocity [ B ω x , B ω y , B ω z ] T [{}_B\omega_x, {}_B\omega_y, {}_B\omega_z]^T [Bωx,Bωy,Bωz]T之间的关系。这样,我们下一篇就能直接开始对四旋翼无人机进行建模。

Body Frame Angular Velocity and [ ϕ ˙ , θ ˙ , ψ ˙ ] T [\dot{\phi}, \dot{\theta}, \dot{\psi}]^T [ϕ˙,θ˙,ψ˙]T

回顾一下初中的知识:角速度不光有大小,也有方向,我们可以根据下图,用右手定则,来判断角速度的方向。

Fig. 3

这个时候,有很多人就会说:那这个关系多简单,不是跟上面一样吗? B ω x {}_B\omega_x Bωx就是绕着body frame x 轴转动的,右手一握,方向就是 B e x {}_B\mathbf{e}_x Bex的正方向。同理, B ω y {}_B\omega_y Bωy方向是 B e y {}_B\mathbf{e}_y Bey, B ω z {}_B\omega_z Bωz方向是 B e z {}_B\mathbf{e}_z Bez。我们想把它们转换成global frame下面的 [ ϕ ˙ , θ ˙ , ψ ˙ ] T [\dot{\phi}, \dot{\theta}, \dot{\psi}]^T [ϕ˙,θ˙,ψ˙]T,就直接用我们前面推出来的 R z y x ( ϕ , θ , ϕ ) R_{zyx}(\phi,\theta,\phi) Rzyx(ϕ,θ,ϕ)矩阵乘以 [ B ω x , B ω y , B ω z ] T [{}_B\omega_x, {}_B\omega_y, {}_B\omega_z]^T [Bωx,Bωy,Bωz]T不就好了?

答案肯定是不对的。因为这里的欧拉角 [ ϕ , θ , ψ ] T [\phi, \theta, \psi]^T [ϕ,θ,ψ]T可不是像速度、位置那样的向量。它们本身就有一个旋转顺序。比如我们上面一直用的Z-Y-X顺序, 在沿着最后一个轴(body frame x轴)旋转 ϕ \phi ϕ角度之后,完成整个旋转。那么你想,假设我们现在完成了前面两次(Z-Y)旋转,目前正沿着 B e x {}_B\mathbf{e}_x Bex 2.5 r a d / s 2.5rad/s 2.5rad/s的速度完成最后一次角度为 ϕ \phi ϕ的旋转,那是不是意味着, ϕ ˙ = 2.5 r a d / s \dot{\phi}=2.5rad/s ϕ˙=2.5rad/s. 同时,这个 2.5 r a d / s 2.5rad/s 2.5rad/s是沿着 B e x {}_B\mathbf{e}_x Bex方向的,那么它就是 B ω x {}_B\omega_x Bωx! 换句话说,如果我的测得现在的 B ω x {}_B\omega_x Bωx,那么它会完完全全投射到 ϕ ˙ \dot{\phi} ϕ˙上面,在 θ ˙ , ψ ˙ \dot{\theta}, \dot{\psi} θ˙,ψ˙上的投影为0.

那么其它两个角度呢?其实很容易发现,这个转换其实有点类似于Z-Y-X的逆过程。 B ω z {}_B\omega_z Bωz直接对应最后一个旋转角 ϕ \phi ϕ B ω y {}_B\omega_y Bωy需要经过最后一次旋转 R ϕ R_{\phi} Rϕ,而 B ω z {}_B\omega_z Bωz需要经过两次旋转 R ϕ ⋅ R θ R_{\phi} \cdot R_{\theta} RϕRθ。这里的公式我就不再一一推导,根据旋转顺序,把对应的旋转矩阵逆过来,就可以解出如下关系:

Eq. 5
[ ϕ ˙ θ ˙ ψ ˙ ] = T ⋅ [ B ω x B ω y B ω z ] = [ 1 s ( ϕ ) t ( θ ) c ( ϕ ) t ( θ ) 0 c ( ϕ ) − s ( ϕ ) 0 s ( ϕ ) c ( θ ) c ( ϕ ) s ( θ ) ] ⋅ [ B ω x B ω y B ω z ]

[ϕ˙θ˙ψ˙]
= T\cdot
[BωxBωyBωz]
=
[1s(ϕ)t(θ)c(ϕ)t(θ)0c(ϕ)s(ϕ)0s(ϕ)c(θ)c(ϕ)s(θ)]
\cdot
[BωxBωyBωz]
ϕ˙θ˙ψ˙ =T BωxBωyBωz = 100s(ϕ)t(θ)c(ϕ)c(θ)s(ϕ)c(ϕ)t(θ)s(ϕ)s(θ)c(ϕ) BωxBωyBωz
对这个公式不是很理解的,可以去看一下这个链接:https://physics.stackexchange.com/questions/429081/rotational-kinematics-and-angular-velocity-vector-transformation

小结

至此,我们大致介绍了欧拉角,以及如何用欧拉角推出旋转矩阵,从而使旋转矩阵乘上body frame下面的任意一个向量,可以转换为global frame对应的向量。也简单介绍了body frame angular velocity与欧拉角导数之间的关系。

下一节,我们将以global frame position ( N p x , N p y , N p z {}_Np_x, {}_Np_y, {}_Np_z Npx,Npy,Npz), global frame velocity ( N v x , N v y , N v z {}_Nv_x, {}_Nv_y, {}_Nv_z Nvx,Nvy,Nvz), Euler angles ( ϕ , θ , ψ \phi, \theta, \psi ϕ,θ,ψ), 以及body frame angular velocity ( B ω x , B ω y , B ω z {}_B\omega_x, {}_B\omega_y, {}_B\omega_z Bωx,Bωy,Bωz)这12个参数作为我们四旋翼无人机系统状态,进行动力学建模,并介绍小角度近似线性模型,以及通过简单的MATLAB代码对小角度近似线性模型进行LQR控制。

之后,我们会介绍旋转矩阵的死锁问题,并讲解新的旋转建模方式:四元数(quaternion)。

最后,我们会详细了解并推导四旋翼无人机系统的微分平坦(differential flatness)特性,以及如何利用微分平坦特性直接对非线性系统进行控制,并对比它与小角度近似LQR, 以及非线性模型预测控制(NLMPC)的控制效果。

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/小惠珠哦/article/detail/753253
推荐阅读
相关标签
  

闽ICP备14008679号