2017年03月13日,原文链接-四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案
笔者最近正在做四旋翼惯性导航的部分，利用加速度计进行速度估计、位置估计，从而实现四旋翼的垂直方向上的定高、水平方向上的定点控制。
首先在这里引用学长之前参考APM飞控里面互补滤波惯导融合方案：
四旋翼位置控制之-定高篇,原文将四轴在悬停油门附近的高度控制，等效为对光滑地面上滑块的位置、速度、加速度模型的控制。下图为王龙学长博客里面的融合框图：
首先将载体坐标系下的加速度（记作
A
(
b
)
=
（
a
x
,
a
y
,
a
z
）
T
A(b)=（ax,ay,az）^T
A(b)=（ax,ay,az）T）旋转到导航坐标系上下,导航坐标系下的加速度记为
A
（
n
）
=
(
A
X
,
A
Y
,
A
Z
)
T
A（n）=(AX,AY,AZ)^T
A（n）=(AX,AY,AZ)T,这里
A
(
b
)
,
A
(
n
)
A(b),A(n)
A(b),A(n)均为列向量，其中
T
T
T表示转置,
R
b
n
R_b^n
Rbn表示载体坐标系到导航系的旋转矩阵,有:
A
（
n
）
=
R
b
n
∗
A
(
b
)
A（n）=R_b^n*A(b)
A（n）=Rbn∗A(b)
R
b
n
=
[
c
o
s
ψ
∗
c
o
s
θ
−
c
o
s
Φ
∗
s
i
n
ψ
+
s
i
n
Φ
∗
s
i
n
θ
∗
c
o
s
ψ
s
i
n
ψ
∗
s
i
n
Φ
+
c
o
s
ϕ
∗
s
i
n
θ
∗
c
o
s
ψ
c
o
s
θ
∗
s
i
n
ψ
c
o
s
Φ
∗
c
o
s
ψ
+
s
i
n
Φ
∗
s
i
n
θ
∗
s
i
n
ψ
−
c
o
s
ψ
∗
s
i
n
Φ
+
c
o
s
Φ
∗
s
i
n
θ
∗
s
i
n
ψ
−
s
i
n
θ
c
o
s
θ
s
i
n
Φ
c
o
s
θ
c
o
s
Φ
]
R_b^n=\left[ \begin{array}{c} cos\psi*cos\theta & -cos\Phi*sin\psi+sin\Phi*sin\theta*cos\psi &sin\psi*sin\Phi+cos\phi*sin\theta*cos\psi \\ cos\theta*sin\psi & cos\Phi*cos\psi +sin\Phi*sin\theta*sin\psi & -cos\psi*sin\Phi+cos\Phi*sin\theta*sin\psi \\-sin\theta &cos\theta sin \Phi & cos\theta cos\Phi \end{array} \right]
Rbn=⎣⎡cosψ∗cosθcosθ∗sinψ−sinθ−cosΦ∗sinψ+sinΦ∗sinθ∗cosψcosΦ∗cosψ+sinΦ∗sinθ∗sinψcosθsinΦsinψ∗sinΦ+cosϕ∗sinθ∗cosψ−cosψ∗sinΦ+cosΦ∗sinθ∗sinψcosθcosΦ⎦⎤
其中，
Φ
,
θ
,
ψ
\Phi,\theta,\psi
Φ,θ,ψ分别表示横滚，俯仰，偏航角。
在计算 R b n R_b^n Rbn的过程中也可以采用四元数旋转矩阵来实现，避免进行大量的三角函数运算:
R
b
n
=
[
q
0
q
0
2
+
q
1
2
−
q
2
2
−
q
3
2
2
(
q
1
q
2
−
q
0
q
3
)
2
(
q
1
q
3
+
q
0
q
2
)
2
(
q
1
q
2
+
q
0
q
3
)
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
2
(
q
2
q
3
+
q
0
q
1
)
2
(
q
1
q
3
−
q
0
q
2
)
2
(
q
2
q
3
+
q
0
q
1
)
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
]
R_b^n=\left[ \begin{array}{c} q_0 q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3) &q_0^2-q_1^2+q_2^2-q_3^2 &2(q_2q_3+q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \end{array} \right]
Rbn=⎣⎡q0q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12+q22−q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3+q0q1)q02−q12−q22+q32⎦⎤
由此我们得到了导航系下 的加速度量，这里需要注意的是此时我们得到的加速度量并不全是用于惯性导航的运动加速度，因为加速度计为比力模型，所有作用在加速度计上的惯性力都将对其产生比力加速度。在载体坐标系中，重力
(
0
,
0
,
g
)
(0,0,g)
(0,0,g) 始终作用在加速度计上。
对于时间非常短的导航 ，当忽略地球自转和哥氏项所引起的 误差的情况下有：
v
˙
e
e
=
R
b
e
f
b
+
g
\dot{v}_e^e=R_b^ef^b+g
v˙ee=Rbefb+g
Acce(x)*=9.8/AcceMax;
Acce(x)*=100;//加速度cm/s^2
Acce(y)*=9.8/AcceMax;
Acce(y)*=100;//加速度cm/s^2
Acce(z)*=9.8/AcceMax;
Acce(z)-=9.8;//减去Z轴重力加速度
Acce(z)*=100;//加速度cm/s^2
至此我们得到了三组用于导航的运动加速度量，这里注意一点的是，水平运动加度Acce(x),Acce(y)在这里表示的是四旋翼沿着正北(纬度)、正东(经度)运动的加速度。AcceMax为加度素计1G量程下的值。
下面贴出本文参考APM三阶互补位置融合的代码：
#define TIME_CONTANST_Z5.0f
#define K_ACC_Z (5.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_VEL_Z (3.0f / (TIME_CONTANST_Z * TIME_CONTANST_Z))
#define K_POS_Z (3.0f / TIME_CONTANST_Z)
float High_Filter[3]={
0.03,//0.015
0.05,//0.05
0.02//0.03
};
float Altitude_Dealt=0;
void Strapdown_INS_High()
{
//Altitude_Dealt=HC_SR04_Distance-NamelessQuad.Position[_YAW];//气压计(超声波)与SINS估计量的差，单位cm
Altitude_Dealt=Altitude_Estimate-NamelessQuad.Position[_YAW];//气压计(超声波)与SINS估计量的差，单位cm
acc_correction[_YAW] += High_Filter[0]*Altitude_Dealt* K_ACC_Z ;//加速度校正量
vel_correction[_YAW] += High_Filter[1]*Altitude_Dealt* K_VEL_Z ;//速度校正量
pos_correction[_YAW] += High_Filter[2]*Altitude_Dealt* K_POS_Z ;//位置校正量
//原始加速度+加速度校正量=融合后的加速度
NamelessQuad.Acceleration[_YAW]=Origion_NamelessQuad.Acceleration[_YAW]+acc_correction[_YAW];
//融合后的加速度积分得到速度增量
SpeedDealt[_YAW]=NamelessQuad.Acceleration[_YAW]*CNTLCYCLE;
//得到速度增量后，更新原始位置
Origion_NamelessQuad.Position[_YAW]+=(NamelessQuad.Speed[_YAW]+0.5*SpeedDealt[_YAW])*CNTLCYCLE;
//原始位置+位置校正量=融合后位置
NamelessQuad.Position[_YAW]=Origion_NamelessQuad.Position[_YAW]+pos_correction[_YAW];
//原始速度+速度校正量=融合后的速度
Origion_NamelessQuad.Speed[_YAW]+=SpeedDealt[_YAW];
NamelessQuad.Speed[_YAW]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_YAW];
}
作者在去年寒假的之前基本上都是用的上述方案进行气压计定高融合，效果还比较好，实际融合调试过程中，只需要调整三个积分系数即可，如果你之前的位置观测量是采用气压计，当你直接把高度位置观测量换成超声波时，参数基本不用调整，融合就挺好的。
下面给出之前测试时候融合波形：
三阶互补融合方案，在飞机中、低速的上升或者下降的时候，不处理延时修正时，由于观测传感器带来的滞后感不明显，但是当你快速拖动飞机，你会发现位置融合波形在后半段会出现跟踪缓慢，强行被观测传感器拉至稳定值的情况，这里截取一小段融合波形给大家，大家对比一下速度快和慢的时候位置融合就一目了然了。
问题先留在这里，如何处理这样一类因观测传感器滞后的造成的惯导融合滞后的问题？贴上处理后的融合波形，大家仔细对比下：
附上之前寒假的三阶互补室外定高视频：
视频1：室外定高F330机架
视频2：QAV250穿越机
以上内容，均为作者学习四轴的个人总结与感悟，难免有错误之处，欢迎批评指出，谢谢！！！
作者将在下一节阐述观测传感器延时修正的处理，以及基于GPS速度+位置两个观测量的水平位置卡尔曼融合过程。
