PX4 四元数旋翼姿态控制修正
版本:V1.10.2;
源码位置:~\src\modules\ma_att_control\ma_att_control_main.cpp;
1. 旋翼姿态控制分为两部分,第一部分是外环角度控制,通过偏差角度作为输入得到期望角速率输入到内环角速率控制,这里先来解释外环控制。
2. 之前有一篇博客(https://blog.csdn.net/zhao23333/article/details/103310916)也讲了旋翼姿态控制,但是写的有点繁琐和复杂,这里用简单的方式来理解这个过程。
3. 通过simulink仿真验证可知公式推导是正确的。模型可以在此处下载。
1. 四元数介绍
具体四元数介绍可以参考我之前博客,里面关于四元数基础知识的介绍是对的,这里我只需要介绍一下本文的符号规则。
任意一个四元数都可以表示物体姿态,这里我们用 q q q 表示姿态四元数,同时这个四元数也可以表示坐标变换,一般而言,姿态四元数表示的是体轴系和惯性系之间的坐标关系,因此这里我们用 q I B q_I^B qIB表示当前飞机的姿态四元数,同时这个四元数也是表示从惯性系到体轴系的坐标变换。
以此类推,四元数 q B 1 B 2 q_{B1}^{B2} qB1B2表示的是坐标轴系 B 2 B2 B2相对与 B 1 B1 B1的位置关系,如果B2是体轴系的话,那么就是表示飞机姿态在坐标系B1下的表示,这个四元数转化为欧拉角的角度也是在坐标系B1下的。
2. 旋翼四元数角度控制逻辑
旋翼角度控制分为两个部分,倾转运动和旋转运动,倾转运动是指飞行器对其Z轴的运动,旋转运动是飞行器Z轴对齐之后绕Z轴旋转之后最终到达指定姿态。
因此这里就会出现四个坐标系,惯性系 I I I,初始体轴系 B B B,中间状态坐标系 M M M,最终体轴系 D D D。
这里我们将最初姿态四元数定义为 q I B q_I^{B} qIB,倾转运动之后的姿态四元数定义为 q I M q_I^{M} qIM,这里的M表示的是中间状态坐标系,最终完成运动之后的姿态四元数为 q I D q_I^D qID。
3. 最终控制实现逻辑
初始姿态四元数为 q = q I B q=q_I^B q=qIB,目标姿态四元数为 q d = q I D q_d=q_I^D qd=qID。
- 算出Z轴在惯性系下的向量,计算得到对其Z轴之后得姿态四元数(
q
B
M
q_B^M
qBM)。
这一步通过最基础的四元数表示旋转的物理意义就可以得到,由旋转轴和旋转角度就可以表示出来。 - 将姿态四元数表示成相对于惯性系下的姿态四元数(
q
I
M
q_I^M
qIM)
通过四元数计算公式可以得到:
q I M = q I B ∗ q B M q_I^M=q_I^B*q_B^M qIM=qIB∗qBM - 得到旋转运动的姿态四元数(
q
M
D
q_M^D
qMD)
旋转运动也可以由公式计算得到
q M D = ( q I M ) ′ ∗ q I D q_M^D= (q_I^M)'*q_I^D qMD=(qIM)′∗qID
中间经过权重处理,根据前文的四元数几何意义,旋转运动又只是绕着Z轴旋转,因此上面的四元数可以表示成:
q ⃗ M D = [ cos α mix 2 0 0 sin α mix 2 ] T \vec{q}_M^D=\left[\cos \frac{\alpha_{\operatorname{mix}}}{2} \quad 0 \quad 0 \quad \sin \frac{\alpha_{\operatorname{mix}}}{2}\right]^{\mathrm{T}} qMD=[cos2αmix00sin2αmix]T
加上限制就有
q ⃗ M D = [ cos k α mix 2 0 0 sin k α mix 2 ] T , k ∈ [ 0 , 1 ] \vec{q}_M^D=\left[\cos \frac{k\alpha_{\operatorname{mix}}}{2} \quad 0 \quad 0 \quad \sin \frac{k\alpha_{\operatorname{mix}}}{2}\right]^{\mathrm{T}},k\in [0,1] qMD=[cos2kαmix00sin2kαmix]T,k∈[0,1]
这里的 K K K就表示偏航旋转的比例。 - 得到最终状态姿态四元数(
q
I
D
q_I^D
qID)
q I D = q I M ∗ q M D q_I^D=q_I^M*q_M^D qID=qIM∗qMD
4. 仿真验证
建立仿真模型。给定初始角度为10都俯仰角,目标角度为30度俯仰角个10度偏航角,偏航比例k=0.5。
-
查看对其z轴之后的四元数,发现是相对原先姿态有一个20度的俯仰。
-
查看相对惯性系下的四元数,发现有一个30度的俯仰
-
查看旋转运动四元数,发现差不多由10度偏航
-
查看最终姿态,是一个30度俯仰,5度偏航
-
最终显示的角度偏差