一种无人机飞行控制器姿态解算和控制方法.pdf

本发明公开了一种无人机飞行控制器姿态解算和控制方法，姿态解算方法使用由加速度计与磁力计融合得到的第一姿态矩阵，陀螺仪和上个控制周期的第三姿态矩阵融合得到的第二姿态矩阵，第一姿态矩阵和第二姿态矩阵融合得到包含高精度姿态信息的第三姿态矩阵。姿态控制方法以姿态空间矢量为控制对象，根据第三姿态矩阵得到各个电机的控制量，实现对无人机飞行器的姿态控制。本发明能适用于无人机全姿态的控制，实时性高，姿态解算和控制稳定。

1.一种无人机飞行控制器姿态解算方法，其特征在于，通过无人机飞行控制器内部的
IMU模块采集原始三轴加速度计数据、原始三轴磁力计数据和原始三轴陀螺仪数据；通过以
下步骤进行姿态解算：
1)对原始三轴加速度计数据进行误差修正和规范化，得到修正后的三轴加速度计数据
记为：aXN，aYN，aZN；
2)对原始三轴磁力计数据进行误差修正和规范化，得到修正后的三轴磁力计数据记
为：mXN，mYN，mZN；
3)对步骤1)和2)中的两组修正后的数据进行融合，得到第一姿态矩阵；
4)对原始三轴陀螺仪数据进行误差修正，得到修正后的三轴陀螺仪数据记为gX、gY、
gZ；
将修正后的三轴陀螺仪数据与上一个控制周期得到的第三姿态矩阵进行融合，得到第
二姿态矩阵；第三姿态矩阵的初始值等于第一个控制周期得到的第一姿态矩阵的值；
5)将第一姿态矩阵和第二姿态矩阵进行融合，得到该控制周期的第三姿态矩阵，即融
合后的方向余弦矩阵。
2.根据权利要求1所述的无人机飞行控制器姿态解算方法，其特征在于，所述步骤1)具
体为：
读取原始三轴加速度计数据(aX、aY、aZ)；
根据以下公式对原始三轴加速度计数据进行零偏修正和灵敏度误差修正：
aXZ＝aX-zoaX
aYZ＝(aY-zoaY)*saY
aZZ＝(aZ-zoaZ)*saZ
其中，zoaX，zoaY和zoaZ分别为三轴加速度计偏移值常量，saY和saZ分别为Y轴和Z轴加
速度计灵敏度修正系数与X轴灵敏度修正系数的相对值；偏移值常量和灵敏度修正系数是
由该仪器生产过程中的误差决定的；
再对三轴加速度计数据进行水平-磁偏角校正：
aXH＝h11*aXZ+h12*aY+h13*aZ
aYH＝h21*aXZ+h22*aY+h23*aZ
aZH＝h31*aXZ+h32*aY+h33*aZ
其中，水平-磁偏角校正矩阵通过校准测量得到；
对三轴加速度计数据进行规范化处理：
$aSquareRoot=\sqrt{{\mathrm{aXH}}^2+{\mathrm{aYH}}^2+{\mathrm{aZH}}^2}$
$aXN=\frac{aXH}{aSquareRoot}$
$aYN=\frac{aYH}{aSquareRoot}$
$aZN=\frac{aZH}{aSquareRoot}.$
3.根据权利要求2所述的无人机飞行控制器姿态解算方法，其特征在于，所述步骤2)具
体为：
读取原始三轴磁力计数据(magX、magY、magZ)；
先根据以下公式对原始三轴磁力计数据进行零偏修正和灵敏度误差修正：
mXZ＝magX-zomX
mYZ＝(magY-zomY)*smY
mZZ＝(magZ-zomZ)*smZ
其中，zomX，zomY和zomZ分别为三轴磁力计偏移值常量，smY和smZ分别为Y轴和Z轴磁力
计灵敏度修正系数与X轴磁力计灵敏度修正系数的相对值；
再对三轴磁力计数据进行水平-磁偏角校正：
mXH＝h11*mXZ+h12*mYZ+h13*mZZ
mYH＝h21*mXZ+h22*mYZ+h23*mZZ
mZH＝h31*mXZ+h32*mYZ+h33*mZZ
然后对三轴磁力计数据进行规范化处理：
$mSquareRoot=\sqrt{{\mathrm{mXH}}^2+{\mathrm{mYH}}^2+{\mathrm{mZH}}^2}$
$mXN=\frac{mXH}{mSquareRoot}$
$mYN=\frac{mYH}{mSquareRoot}$
$mZN=\frac{mZH}{mSquareRoot}.$
4.根据权利要求3所述的无人机飞行控制器姿态解算方法，其特征在于，所述步骤3)
中，第一姿态矩阵记为：
$\left[\begin{array}{ccc}a11& a12& a13\\ a21& a22& a23\\ a31& a32& a33\end{array}\right];$
其中，第三行元素为：
a31＝aXN，a32＝aYN，a33＝aZN；
第一行元素为：
$a11=\frac{mXN-v*aXN}{\sqrt{{(mXN-v*aXN)}^2+{(mYN-v*aYN)}^2+{(mZN-v*aZN)}^2}}$
$a12=\frac{mYN-v*aYN}{\sqrt{{(mXN-v*aXN)}^2+{(mYN-v*aYN)}^2+{(mZN-v*aZN)}^2}}$
$a13=\frac{mZN-v*aZN}{\sqrt{{(mXN-v*aXN)}^2+{(mYN-v*aYN)}^2+{(mZN-v*aZN)}^2}}$
其中，v为垂直化因子，v＝mXN*aXN+mYN*aYN+mZN*aZN；
第二行元素为：
a21＝a13*a32-a12*a33
a22＝a11*a33-a31*a13
a23＝a31*a12-a11*a32。
5.根据权利要求4所述的无人机飞行控制器姿态解算方法，其特征在于，所述步骤4)
中，第二姿态矩阵记为：
$\left[\begin{array}{ccc}b11& b12& b13\\ b21& b22& b23\\ b31& b32& b33\end{array}\right];$
第三姿态矩阵记为：
$\left[\begin{array}{ccc}c11& c12& c13\\ c21& c22& c23\\ c31& c32& c33\end{array}\right]$
第二姿态矩阵的计算过程为：
首先将修正后的三轴陀螺仪数据分别乘以更新周期T，得到三轴角矢量αX、αY、αZ：
αX＝gX*T
αY＝gY*T
αZ＝gZ*T
计算四元数更新因子(r0、r1、r2、r3)：
r0＝1-sSquareSum/8
$r1=0.5*\mathrm{\α}X*(1-\frac{sSquareSum}{24})$
$r2=0.5*\mathrm{\α}Y*(1-\frac{sSquareSum}{24})$
$r3=0.5*\mathrm{\α}Z*(1-\frac{sSquareSum}{24})$
其中，sSquareSum＝αX2+αY2+αZ2
将上一控制周期得到的第三姿态矩阵转换成四元数：
$q0=0.5*\sqrt{(1+c11+c22+c33)}$
若q0≠0，则有：
$\left\{\begin{array}{c}q1=\frac{0.25}{q0}*(c32-c23)\\ q2=\frac{0.25}{q0}*(c13-c31)\\ q3=\frac{0.25}{q0}*(c21-c12)\end{array}\right.$
若q0＝0，则有：
$\left\{\begin{array}{c}q1=c32-c23\\ q2=c13-c31\\ q3=c21-c12\end{array}\right.$
规范化上述四元数：
$qSquareRoot=\sqrt{q{0}^2+q{1}^2+q{2}^2+q{3}^2}$
$q0N=\frac{q0}{qSquareRoot}$
$q1N=\frac{q1}{qSquareRoot}$
$q2N=\frac{q2}{qSquareRoot}$
$q3N=\frac{q3}{qSquareRoot}$
更新四元数：
q0＝q0N*r0-q1N*r1-q2N*r2-q3N*r3
q1＝q1N*r0+q0N*r1-q3N*r2+q2N*r3
q2＝q2N*r0+q3N*r1+q0N*r2-q1N*r3
q3＝q3N*r0-q2N*r1+q1N*r2+q0N*r3
对上述更新的四元数进行规范化处理，得到新的q0N，q1N，q2N和q3N，并将其转换成第
二姿态矩阵：
b11＝q0N2+q1N2-q2N2-q3N2
b12＝2*q1N*q2N-2*q0N*q3N
b13＝2*q1N*q3N+2*q0N*q2N
b21＝2*q1N*q2N+2*q0N*q3N
b22＝q0N2-q1N2+q2N2-q3N2
b23＝2*q2N*q3N-2*q0N*q1N
b31＝2*q1N*q3N-2*q0N*q2N
b32＝2*q2N*q3N+2*q0N*q1N
b33＝q0N2-q1N2-q2N2+q3N2。
6.根据权利要求5所述的无人机飞行控制器姿态解算方法，其特征在于，所述步骤5)
中，该控制周期的第三姿态矩阵计算方法为：
计算修正后的三轴加速度计数据与第二姿态矩阵第三行元素的偏差向量：
dX3＝aXN-b31
dY3＝aYN-b32
dZ3＝aZN-b33
乘以一个比例系数m3，得到用于修正的矢量：
cX3＝dX3*m3
cY3＝dY3*m3
cZ3＝dZ3*m3
计算修正后的三轴加速度计数据与第二姿态矩阵第三行融合后的矢量：
vX3＝b31+cX3
vY3＝b32+cY3
vZ3＝b33+cZ3
对融合后的矢量规范化：
$vSquareRoot3=\sqrt{vX{3}^2+vY{3}^2+vZ{3}^2}$
$vX3N=\frac{vX3}{vSquareRoot3}$
$vY3N=\frac{vY3}{vSquareRoot3}$
$vZ3N=\frac{vZ3}{vSquareRoot3}$
计算修正后的三轴磁力计数据与第二姿态矩阵第一行元素的偏差向量：
dX1＝mXN-b11
dY1＝mYN-b12
dZ1＝mZN-b13
乘以一个比例系数m1得到用于修正的矢量：
cX1＝dX1*m1
cY1＝dY1*m1
cZ1＝dZ1*m1
计算修正后的三轴磁力计数据与第二姿态矩阵第一行融合后的矢量：
vX1＝b11+cX1
vY1＝b12+cY1
vZ1＝b13+cZ1
对磁力计融合矢量垂直化：
vX1V＝vX1-k*vX3N
vY1V＝vY1-k*vY3N
vZ1V＝vZ1-k*vZ3N
其中，k为垂直化因子；k＝vX1*vX3N+vY1*vY3N+vZ1*vZ3N；对垂直化后的磁力计融合矢
量规范化：
$vSquareRoot1=\sqrt{\begin{array}{c}vX1V^2\\ +vY1V^2\\ +vZ1V^2\end{array}}$
$vX1N=\frac{vX1V}{vSquareRoot1}$
$vY1N=\frac{vY1V}{vSquareRoot1}$
$vZ1N=\frac{vZ1V}{vSquareRoot1}$
得到第三姿态矩阵第一行元素和第三行元素：
c11＝vX1N，c12＝vY1N，c13＝vZ1N；
c31＝vX3N，c32＝vY3N，c33＝vZ3N；
由方向余弦矩阵的正交特性求出第三姿态矩阵第二行元素：
c21＝c13*c32-c12*c33
c22＝c11*c33-c31*c13
c23＝c31*c12-c11*c32。
7.根据权利要求1～6中任一项所述的无人机飞行控制器姿态解算方法，其特征在于，
所述无人机飞行器采用以下结构：包括飞行控制器顶盖(1)，上层减震海绵(2)，IMU模块
(3)，下层减震海绵(5)，飞行控制器底板(9)；
飞行控制器顶盖(1)，上层减震海绵(2)，IMU模块(3)，下层减震海绵(5)和飞行控制器
底板(9)从上到下依次排列；IMU模块(3)包裹于上层减震海绵(2)和下层减震海绵(5)之间。
8.根据权利要求7所述的无人机飞行控制器姿态控制方法，其特征在于，所述无人机飞
行控制器还包括设置于飞行控制器底板(9)上的内置电源；
所述内置电源包括开关稳压器和线性稳压器；开关稳压器的输入端经电池连接线接电
池，输出端接线性稳压器，线性稳压器的输出端为飞行控制器的内部设备供电。
9.一种无人机飞行控制器姿态控制方法，其特征在于，首先基于权利要求6所述的无人
机飞行器姿态控制方法得到第三姿态矩阵，然后进行以下步骤：
从第三姿态矩阵中提取出平衡矢量B和航向矢量Y；其中平衡矢量为第三姿态矩阵中第
三行元素构成的向量：(c31，c32，c33)；航向矢量为第三姿态矩阵第一行第一列的元素与第
二行第一列的元素构成的向量：(c11，c21)；
再利用期望平衡矢量与实际平衡矢量之差来控制机体平衡；当期望平衡矢量不等于实
际平衡矢量时，计算平衡矢量差；然后将平衡矢量差转换成电机矩阵控制量，最后将电机矩
阵控制量解耦为各个电机的控制量；
利用期望航向矢量与实际航向矢量之差来控制机体航向；当期望航向矢量不等于实际
航向矢量时，计算航向矢量差；然后将航向矢量差转换成电机矩阵控制量，最后将电机矩阵
控制量解耦为各个电机的控制量。
10.一种无人机飞行控制器姿态控制方法，其特征在于，以空间姿态矢量为控制对象；
根据第三姿态矩阵对无人机飞行器的姿态进行控制，包括以下步骤：
(A)计算三轴位移自由度控制因子(mX，mY，mZ)；
令mX＝0；
mY＝0；
将遥控器的输入信号油门控制因子throttleController直接赋给Z轴位移自由度的控
制因子：
mZ＝throttleController；
(B)计算三轴旋转自由度控制因子(sX，sY，sZ)：
步骤B1：由第三姿态矩阵的转置矩阵左乘期望姿态矩阵E得到变换矩阵D：D＝C^T*E：
步骤B2：将变换矩阵D转化为四元数：
p0＝0.5*(1+d11+d22+d33)^(0.5)；
p1＝0.25/p0*(d32‐d23)；
p2＝0.25/p0*(d13‐d31)；
p3＝0.25/p0*(d21‐d12)；
步骤B3：计算当前角度偏差(期望旋转角度)θ：
θ＝arccos(p0)*2，θ∈[0,π]；
步骤B4：将当前角度偏差(期望旋转角度)θ分配到三个旋转自由度上，得到三轴的当前
角度偏差：
θX＝θ*p1；
θY＝θ*p2；
θZ＝θ*p3；
步骤B5：计算三轴预测角速度、三轴预测角度偏差、三轴期望角速度、三轴预测角速度
偏差
B5.1)计算三轴预测角速度：
每一轴的预测角速度＝该轴当前角速度+滞后时间段内该轴旋转控制因子作用时间
(积分得到，正负方向抵消)的百分比乘以该轴旋转控制因子角速度增量系数：
B5.2)计算三轴预测角度偏差：
每一轴的预测角度偏差＝该轴当前角度偏差-(该轴当前角速度+该轴预测角速度)/2*
(2*滞后时间；
B5.3)计算三轴期望角速度：
每一轴的期望角速度＝该轴预测角度偏差*该轴角度偏差角速度系数+该轴最小期望
角速度；
B5.4)计算三轴预测角速度偏差：
每一轴的预测角速度偏差＝该轴期望角速度-该轴预测角速度；
步骤B6：计算三轴超速控制因子：
每一轴的超速控制因子＝该轴超速控制因子预设值*该轴预测角速度偏差；
步骤B7：计算三轴旋转控制因子(sX，sY，sZ)
每一轴的旋转控制因子＝该轴超速控制因子：
(C)根据实际机型，将6个自由度的控制因子转换成各个电机的控制量；对于X4机型，其
4个电机的控制量分别为：
m1＝-sX+sY+sZ+mZ；
m2＝-sX-sY-sZ+mZ；
m3＝sX-sY+sZ+mZ；
m4＝sX+sY-sZ+mZ；
对于X6机型，其6个电机的控制量分别为：：
m1＝-0.5*sX+sY+sZ+mZ；
m2＝-sX+sZ*-1+mZ；
m3＝-0.5*sX-sY+sZ+mZ；
m4＝0.5*sX-sY+sZ+mZ；
m5＝sX+sZ+mZ；
m6＝0.5*sX+sY-sZ+mZ。

一种无人机飞行控制器姿态解算和控制方法

所属技术领域

本发明涉及无人机飞行控制领域，具体涉及一种无人机飞行控制器姿态解算和控
制方法。

背景技术

目前主流的无人机飞行控制器的姿态解算方法的输出量是是欧拉角，而飞行控制
方法是以欧拉角作为内部控制对象对机体的姿态进行控制，欧拉角具有万向锁缺陷，不能
适用于全姿态的控制，限制了无人机的运动幅度，同时由于包含三角函数运算，增加了处理
器负担，降低了控制的实时性。

无人机飞行控制器是无人机的核心控制部件，其任务是接收来自内部传感器(陀
螺仪、加速度计、磁力计、气压计、温度计、电压计)和外部传感器(GNSS定位装置、外部磁力
计)的数据，通过特定的飞行控制算法转换成电子调速器所需的控制信号，从而改变和控制
无人机的姿态(俯仰/横滚/航向情况)、地理位置和高度。

目前，大多数无人机飞行控制器通过外部供电模块进行供电，外部供电模块供电
电压噪声和波动较大，同时由于地线与飞行控制器的距离较远，容易对飞行控制器造成干
扰；大多数无人机飞行控制器采用IMU(Inertial measurement unit，惯性测量单元)硬连
接，由于无人机飞行时螺旋桨产生的震动，将导致IMU采集的数据容易受到干扰，从而造成
姿态解算的不稳定性。

发明内容

本发明所解决的技术问题是，针对现有技术的不足，提供一种无人机飞行控制器
姿态解算和控制方法，能适用于无人机全姿态的控制，实时性高，姿态解算和控制稳定。

本发明解决技术问题所采用的技术方案是：

一种无人机飞行控制器姿态解算方法，通过无人机飞行控制器内部的IMU模块采
集原始三轴加速度计数据、原始三轴磁力计数据和原始三轴陀螺仪数据；通过以下步骤进
行姿态解算：

1)对原始三轴加速度计数据进行误差修正和规范化，得到修正后的三轴加速度计
数据记为：aXN，aYN，aZN；

2)对原始三轴磁力计数据进行误差修正和规范化，得到修正后的三轴磁力计数据
记为：mXN，mYN，mZN；

3)对步骤1)和2)中的两组修正后的数据进行融合，得到第一姿态矩阵；

4)对原始三轴陀螺仪数据进行误差修正，得到修正后的三轴陀螺仪数据记为gX、
gY、gZ；

将修正后的三轴陀螺仪数据与上一个控制周期得到的第三姿态矩阵进行融合，得
到第二姿态矩阵；第三姿态矩阵的初始值等于第一个控制周期得到的第一姿态矩阵的值；

5)将第一姿态矩阵和第二姿态矩阵进行融合，得到该控制周期的第三姿态矩阵，
即融合后的方向余弦矩阵。

所述步骤1)具体为：

读取原始三轴加速度计数据(aX、aY、aZ)；

根据以下公式对原始三轴加速度计数据进行零偏修正和灵敏度误差修正：

aXZ＝aX-zoaX

aYZ＝(aY-zoaY)*saY

aZZ＝(aZ-zoaZ)*saZ

其中，zoaX，zoaY和zoaZ分别为三轴加速度计偏移值常量，saY和saZ分别为Y轴和Z
轴加速度计灵敏度修正系数与X轴灵敏度修正系数的相对值；偏移值常量和灵敏度修正系
数是由该仪器生产过程中的误差决定的；

再对三轴加速度计数据进行水平-磁偏角校正：

aXH＝h11*aXZ+h12*aY+h13*aZ

aYH＝h21*aXZ+h22*aY+h23*aZ

aZH＝h31*aXZ+h32*aY+h33*aZ

其中，水平-磁偏角校正矩阵通过校准测量得到；

对三轴加速度计数据进行规范化处理：

所述步骤2)具体为：

读取原始三轴磁力计数据(magX、magY、magZ)；

先根据以下公式对原始三轴磁力计数据进行零偏修正和灵敏度误差修正：

mXZ＝magX-zomX

mYZ＝(magY-zomY)*smY

mZZ＝(magZ-zomZ)*smZ

其中，zomX，zomY和zomZ分别为三轴磁力计偏移值常量，smY和smZ分别为Y轴和Z轴
磁力计灵敏度修正系数与X轴磁力计灵敏度修正系数的相对值；【再对三轴磁力计数据进行
椭圆修正，即将三轴磁力计数据进行二次多项式拟合；二次多项式是预先校准磁力计时得
到的，得到实际的三轴磁力计数据后，用二次多项式曲线上与之距离最近的点，替换三轴磁
力计数据；】

再对三轴磁力计数据进行水平-磁偏角校正：

mXH＝h11*mXZ+h12*mYZ+h13*mZZ

mYH＝h21*mXZ+h22*mYZ+h23*mZZ

mZH＝h31*mXZ+h32*mYZ+h33*mZZ

然后对三轴磁力计数据进行规范化处理；后面有规范化，本来这里可以不用，但是
考虑到浮点运算过程中数量级接近的浮点数之间运算的结果精度更高，因此这里执行规范
化：

所述步骤3)中，第一姿态矩阵记为：

其中，第三行元素为：

a31＝aXN，a32＝aYN，a33＝aZN；

第一行元素为：

其中，v为垂直化因子，v＝mXN*aXN+mYN*aYN+mZN*aZN；

第二行元素为：

a21＝a13*a32-a12*a33

a22＝a11*a33-a31*a13

a23＝a31*a12-a11*a32

上述第一姿态矩阵第一行元素与第二行元素计算原理为：

对修正后的三轴磁力计数据mXN，mYN，mZN进行垂直化处理；

由于磁力计不与加速度计垂直，需要垂直化处理，需要在磁力计添加一个K*加速
度计矢量，再根据相互垂直的向量点积为0原理有：

(mX-v*aX,mY-v*aY,mZ-v*aZ)*(aX,aY,aZ)＝0

可得垂直化因子：

对磁力计数据进行垂直化处理：

mXV＝mXN-v*aXN

mYV＝mYN-v*aYN

mZV＝mZN-v*aZN

再次规范化处理：

得到第一姿态矩阵第一行元素为：

a11＝mXVN

a12＝mYVN

a13＝mZVN

由方向余弦矩阵正交特性求出第一姿态矩阵第二行元素为：

a21＝a13*a32-a12*a33

a22＝a11*a33-a31*a13

a23＝a31*a12-a11*a32。

所述步骤4)中，第二姿态矩阵记为：

第三姿态矩阵记为：

第二姿态矩阵的计算过程为：

首先将修正后的三轴陀螺仪数据分别乘以更新周期T，得到三轴角矢量αX、αY、αZ：

αX＝gX*T

αY＝gY*T

αZ＝gZ*T

计算四元数更新因子(r0、r1、r2、r3)：

r0＝1-sSquareSum/8

其中，sSquareSum＝αX2+αY2+αZ2

将上一控制周期得到的第三姿态矩阵转换成四元数：

若q0≠0，则有：

若q0＝0，则有：

规范化上述四元数：

更新四元数：

q0＝q0N*r0-q1N*r1-q2N*r2-q3N*r3

q1＝q1N*r0+q0N*r1-q3N*r2+q2N*r3

q2＝q2N*r0+q3N*r1+q0N*r2-q1N*r3

q3＝q3N*r0-q2N*r1+q1N*r2+q0N*r3

对上述更新的四元数进行规范化处理(将四元数的各个分量分别除以各个分量的
平方和的开方)，得到新的q0N，q1N，q2N和q3N：

将其转换成第二姿态矩阵【陀螺仪方向余弦矩阵】：

b11＝q0N2+q1N2-q2N2-q3N2

b12＝2*q1N*q2N-2*q0N*q3N

b13＝2*q1N*q3N+2*q0N*q2N

b21＝2*q1N*q2N+2*q0N*q3N

b22＝q0N2-q1N2+q2N2-q3N2

b23＝2*q2N*q3N-2*q0N*q1N

b31＝2*q1N*q3N-2*q0N*q2N

b32＝2*q2N*q3N+2*q0N*q1N

b33＝q0N2-q1N2-q2N2+q3N2。

所述步骤5)中，该控制周期的第三姿态矩阵【融合后的方向余弦矩阵】计算方法
为：计算修正后的三轴加速度计数据与第二姿态矩阵第三行元素的偏差向量：

dX3＝aXN-b31

dY3＝aYN-b32

dZ3＝aZN-b33

乘以一个比例系数m3，得到用于修正的矢量：

cX3＝dX3*m3

cY3＝dY3*m3

cZ3＝dZ3*m3

计算修正后的三轴加速度计数据与第二姿态矩阵第三行融合后的矢量：

vX3＝b31+cX3

vY3＝b32+cY3

vZ3＝b33+cZ3

对融合后的矢量规范化：

计算修正后的三轴磁力计数据与第二姿态矩阵第一行元素的偏差向量：

dX1＝mXN-b11

dY1＝mYN-b12

dZ1＝mZN-b13

乘以一个比例系数m1得到用于修正的矢量：

cX1＝dX1*m1

cY1＝dY1*m1

cZ1＝dZ1*m1

计算修正后的三轴磁力计数据与第二姿态矩阵第一行融合后的矢量：

vX1＝b11+cX1

vY1＝b12+cY1

vZ1＝b13+cZ1

由于矢量(vX1，vY1，vZ1)不与(vX3N，vY3N，vZ3N)垂直，需要垂直化处理，需要在该
矢量添加一个k*(vX3N，vY3N，vZ3N)；再根据相互垂直的向量点积为0原理有：

(vX1-k*vX3N,vY1-k*vY3N,vZ1-k*vZ3N)*(vX3N,vY3N,vZ3N)＝0

可得垂直化因子：

对磁力计融合矢量垂直化：

vX1V＝vX1-k*vX3N

vY1V＝vY1-k*vY3N

vZ1V＝vZ1-k*vZ3N

其中，k为垂直化因子；k＝vX1*vX3N+vY1*vY3N+vZ1*vZ3N；

对垂直化后的磁力计融合矢量规范化：

得到第三姿态矩阵第一行元素和第三行元素：

c11＝vX1N，c12＝vY1N，c13＝vZ1N；

c31＝vX3N，c32＝vY3N，c33＝vZ3N；

由方向余弦矩阵的正交特性求出第三姿态矩阵第二行元素：

c21＝c13*c32-c12*c33

c22＝c11*c33-c31*c13

c23＝c31*c12-c11*c32。

一种无人机飞行控制器姿态控制方法，以空间姿态矢量为控制对象；根据第三姿
态矩阵对无人机飞行器的姿态进行控制，包括以下步骤：

从第三姿态矩阵中提取出平衡矢量B和航向矢量Y；

其中平衡矢量为第三姿态矩阵中第三行元素构成的向量：(c31，c32，c33)

航向矢量为第三姿态矩阵第一行第一列的元素与第二行第一列的元素构成的向
量：(c11，c21)。

再利用期望平衡矢量与实际平衡矢量之差来控制机体平衡；当期望平衡矢量不等
于实际平衡矢量时，计算平衡矢量差；然后将平衡矢量差转换成电机矩阵控制量，最后将电
机矩阵控制量解耦为各个电机的控制量。

利用期望航向矢量与实际航向矢量之差来控制机体航向；当期望航向矢量不等于
实际航向矢量时，计算航向矢量差；然后将航向矢量差转换成电机矩阵控制量，最后将电机
矩阵控制量解耦为各个电机的控制量。

一种无人机飞行控制器姿态控制方法，根据第三姿态矩阵对无人机飞行器的姿态
进行控制，包括以下步骤：

(A)计算三轴位移自由度控制因子(mX，mY，mZ)；

令mX＝0；

mY＝0；

将遥控器的输入信号油门控制因子throttleController直接赋给Z轴位移自由度
的控制因子：

mZ＝throttleController；

(B)计算三轴旋转自由度控制因子(sX，sY，sZ)：

步骤B1：由第三姿态矩阵的转置矩阵左乘期望姿态矩阵E得到变换矩阵D：D＝C^T*
E：

步骤B2：将变换矩阵D转化为四元数：

p0＝0.5*(1+d11+d22+d33)^(0.5)；

p1＝0.25/p0*(d32‐d23)；

p2＝0.25/p0*(d13‐d31)；

p3＝0.25/p0*(d21‐d12)；

步骤B3：计算当前角度偏差(期望旋转角度)θ：

θ＝arccos(p0)*2，θ∈[0,π]；

步骤B4：将当前角度偏差(期望旋转角度)θ分配到三个旋转自由度上，得到三轴的
当前角度偏差：

θX＝θ*p1；

θY＝θ*p2；

θZ＝θ*p3；

步骤B5：计算三轴预测角速度、三轴预测角度偏差、三轴期望角速度、三轴预测角
速度偏差

B5.1)计算三轴预测角速度：

每一轴的预测角速度＝该轴当前角速度(陀螺仪直接测量得到，即修正后的三轴
陀螺仪数据记为gX、gY、gZ)+滞后时间段(其取值是预先给定的，取值范围为：0.1～0.3；)内
该轴旋转控制因子作用时间(积分得到，正负方向抵消)的百分比乘以该轴旋转控制因子角
速度增量系数(其取值是预先给定的，取值范围为：5～10；)：(对滞后时间段内，旋转控制因
子的符号(大于0为1，小于0为-1)进行积分，再除以滞后时间，得到控制因子作用时间的百
分比)

B5.2)计算三轴预测角度偏差：

每一轴的预测角度偏差＝该轴当前角度偏差-(该轴当前角速度+该轴预测角速
度)/2*(2*滞后时间；

B5.3)计算三轴期望角速度：

每一轴的期望角速度＝该轴预测角度偏差*该轴角度偏差角速度系数(其取值是
预先给定的，取值范围为：10～20)+该轴最小期望角速度(其取值是预先给定的，取值范围
为：0～0.2；)；

B5.4)计算三轴预测角速度偏差：

每一轴的预测角速度偏差＝该轴期望角速度-该轴预测角速度；

步骤B6：计算三轴超速控制因子：

每一轴的超速控制因子＝该轴超速控制因子预设值(其取值是预先给定的，取值
范围为：0.05～0.2；)*该轴预测角速度偏差；

步骤B7：计算三轴旋转控制因子(sX，sY，sZ)

每一轴的旋转控制因子＝该轴超速控制因子：

(C)根据实际机型，将6个自由度的控制因子转换成各个电机的控制量；

对于X4机型，其4个电机的控制量分别为：

m1＝-sX+sY+sZ+mZ；

m2＝-sX-sY-sZ+mZ；

m3＝sX-sY+sZ+mZ；

m4＝sX+sY-sZ+mZ；

对于X6机型，其6个电机的控制量分别为：：

m1＝-0.5*sX+sY+sZ+mZ；

m2＝-sX+sZ*-1+mZ；

m3＝-0.5*sX-sY+sZ+mZ；

m4＝0.5*sX-sY+sZ+mZ；

m5＝sX+sZ+mZ；

m6＝0.5*sX+sY-sZ+mZ。

上述无人机飞行器姿态解算和控制方法的无人机飞行控制器采用以下结构：包括
飞行控制器顶盖1，上层减震海绵2，IMU模块3，下层减震海绵5，飞行控制器底板9；

飞行控制器顶盖1，上层减震海绵2，IMU模块3，下层减震海绵5和飞行控制器底板9
从上到下依次排列；IMU模块3包裹于上层减震海绵2和下层减震海绵5之间。

IMU模块中集成有三轴加速度计、三轴磁力计和三轴陀螺仪；IMU模块的测量数据
传输到飞行控制器底板上的CPU进行姿态解算；

飞行控制器底板9上设有印刷电路板和CPU11；CPU与印刷电路板通过电子线路连
接；IMU模块3与飞行控制器底板9上分别设置有IMU模块14P上接翻盖式FPC座4和主控底板
14P上接翻盖式FPC座10；IMU模块3与飞行控制器底板9通过FPC座及FPC软排线连接；

所述飞行控制器顶盖1，上层减震海绵2，IMU模块3，下层减震海绵5和飞行控制器
底板9从上到下依次排列连接，连接介质均为双面不导电胶。

所述上层减震海绵2与下层减震海绵5的面积均大于IMU模块3的面积；IMU模块3设
置于上层减震海绵2与下层减震海绵5之间的中心位置；上层减震海绵2与下层减震海绵5对
应于IMU模块3之外的部分相连接；连接介质为双面不导电胶。

所述无人机飞行控制器，还包括设置于飞行控制器底板9上的内置电源；

所述内置电源包括开关稳压器和线性稳压器；开关稳压器的输入端经电池连接线
接电池，输出端接线性稳压器，线性稳压器的输出端为飞行控制器的内部设备供电；飞行控
制器的内部设备包括飞行控制器底板9上面的CAN收发器、CPU、辅助CPU、通信缓冲器、非易
失性存储器和LED指示器和IMU模块3等。

所述开关稳压器为Buck型DC-DC开关稳压器TPS54160；所述线性稳压器为高精度
低压差SPX1117M3线性稳压器；Buck型DC-DC开关稳压器TPS54160的5V输出连接线通过内部
线路连接至线性稳压器SPX1117M3，输出高精度低纹波的3.3V直流电压；

TPS54160Buck型DC-DC开关稳压器的环路补偿部分采用一颗102K-1％电阻和一颗
2.2nF/50V低ESR陶瓷电容串联，再并联一颗1.5pF/50V低ESR陶瓷电容；TPS54160Buck型DC-
DC开关稳压器的输出反馈电阻设置为52.6K和10K串联，输出调整为5V直流；

SPX1117M3线性稳压器的输入端，即5V输出连接线上并联有一颗4.7uF/10V陶瓷电
容，SPX1117M3线性稳压器的输出端与飞行控制器内部设备的电源端之间并联有一颗
100uF/10V低ESR陶瓷电容和一颗100nF/16V陶瓷电容。

所述无人机飞行控制器，还包括电子调速器信号输出插座板6和外部传感器和外
设模块信号插座板7；

电子调速器信号输出插座板6和外部传感器和外设模块信号插座板7；的底部均与
飞行控制器底板9垂直连接，连接点分别处于飞行控制器底板9相对两侧的边缘；

电子调速器信号输出插座板6和外部传感器和外设模块信号插座板7的顶部均与
飞行控制器顶盖1垂直连接，连接点分别处于飞行控制器顶盖1相对两侧的边缘；

连接介质采用固态焊锡膏。

飞行控制器底板9与电子调速器信号输出插座板6通过它们之间连接处的金手指
焊接相连，飞行控制器底板6与外部传感器和外部信号插座板7通过它们之间连接处的金手
指焊接相连。

所述电子调速器信号输出插座板6和外部传感器和外设模块信号插座板7的接插
件为2.54mm镀金排针8，其中电子调速器信号输出插座板6包含两排排针，每排18根，外部传
感器和外设模块信号插座板7包含两排排针，每排17根。

所述电池连接线分为两段，前面一段位于电池和飞行控制器之间，为硅胶导线，通
过外部传感器和外设模块信号插座板7与飞行控制器相连；第二段位于飞行控制器内部的
印刷电路板上，第二段导线上串联有500mA自恢复型保险丝；500mA自恢复型保险丝的电流
输出端还经4.7uF/50V低ESR陶瓷电容连接飞行控制器内部的公共地。

有益效果：

本发明设计了基于姿态空间矢量为控制对象的无人机飞行控制器姿态解算和控
制方法，该方法克服了现有的飞行控制器以欧拉角作为控制对象导致的万象锁缺陷，不存
在万象锁问题，可以实现全姿态运动的解算，完全释放了无人机的机动能力，使得全姿态运
动成为可能，同时去除了三角函数的运算过程，大大降低了CPU的负担。

本发明的无人机飞行控制器采用内置DC-DC和线性稳压器供电方式，供电系统采
用内置的高效率小封装，Buck型DC-DC开关降压稳压器，将7.2V至30V的外部电压转换成5V,
然后使用了1％精度的超低噪声线性稳压器进一步将5V转换成3.3V，为CPU和IMU模块供电。
该方式不仅提高了供电质量和供电稳定性，纹波电压小，不存在地线干扰问题，而且简化了
飞行控制器的安装。能有效克服现有的飞行控制器供电质量差和纹波电压较大的问题。

本发明的无人机飞行控制器上IMU模块采用了独立安装方式，并且采用性减震海
绵包覆IMU模块。上层减震海绵和下层减震海绵均采用高弹减震海绵。该方式解决了现有的
飞行控制器IMU硬连接易受震动干扰的问题,减弱了IMU模块受到的震动，从而降低了IMU原
始输出数据的波动，大大提高了IMU的稳定性，降低了IMU所受到的震动影响，从而提高飞行
控制稳定性。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1是本发明无人机飞行控制器姿态解算方法示意图。

图2是本发明无人机飞行控制器姿态控制方法流程图。

图3是本发明无人机飞行控制器组装结构图。

图4是本发明无人机飞行控制器的内置电源连接示意图。

图5是本发明无人机飞行控制器的开关稳压器电路结构图。

附图标记说明：

1.飞行控制器顶盖，2.上层减震海绵，3.IMU模块，4.IMU模块14P上接翻盖式FPC
座，5.下层减震海绵，6.电子调速器信号输出插座板，7.外部传感器和外设模块信号插座
板，8.2.54mm间距镀金排针，9.飞行控制器底板，10.主控底板14P上接翻盖式FPC座，
11.CPU。

具体实施方式

以下结合附图和具体实施方式对本发明进行进一步具体说明。

本发明公开了一种无人机飞行控制器姿态解算方法，其基本过程如图1所示。

如图1所示，原始三轴加速度计数据首先进行零偏、灵敏度误差修正，原始三轴磁
力计数据进行零偏、灵敏度误差和椭圆误差修正，这两组融合数据进行融合后得到第一姿
态矩阵，原始三轴陀螺仪数据进行零偏、灵敏度误差修正得到修正后的三轴陀螺仪数据，该
数据再与上一个控制周期得到的第三姿态矩阵进行融合，得到第二姿态矩阵，第一姿态矩
阵和第二姿态矩阵进行融合后得到第三姿态矩阵，第三姿态矩阵中包含平衡矢量和航向矢
量的信息，从第三姿态矩阵中提取出平衡矢量B和航向矢量Y。

如图1所示，原始三轴加速度计数据、原始三轴磁力计数据、原始三轴陀螺仪数据
均从IMU模块读取而来，在每个控制周期进行更新。

如图1所示，加速度计原始数据的零偏修正是在原始数据的基础上减去偏移值常
量实现的，该偏移值常量是由该加速度计生产过程中的误差决定的，加速度计原始数据的
灵敏度误差修正是在原始数据的基础上乘以灵敏度修正系数实现的，该修正系数是由该加
速度计生产过程中的误差决定的。

如图1所示，原始磁力计数据的零偏修正是在原始数据的基础上减去偏移值常量
实现的，该偏移值常量是由该磁力计生产过程中的误差决定的，原始磁力计数据的灵敏度
误差修正是在原始数据的基础上乘以灵敏度修正系数实现的，该修正系数是由该磁力计生
产过程中的误差决定的，原始磁力计数据的椭圆修正是在原始数据的基础上进行二次多项
式拟合实现的。

如图1所示，原始陀螺仪数据的零偏修正是在原始数据的基础上减去偏移值常量
实现的，该偏移值常量是由该陀螺仪生产过程中的误差决定的，原始陀螺仪数据的灵敏度
误差修正是在原始数据的基础上乘以灵敏度修正系数实现的，该修正系数是由该陀螺仪生
产过程中的误差决定的。

如图1所示，三轴加速度计和三轴磁力计的修正后数据通过融合得到第一姿态矩
阵，该融合过程将原始三轴加速度计数据和原始三轴磁力计数据组合成完整的姿态矩阵。

如图1所示，三轴陀螺仪经修正后的数据与上一个控制周期得到的第三姿态矩阵
进行融合得到第二姿态矩阵，该融合过程将原始三轴陀螺仪数据和上一个控制周期得到的
第三姿态矩阵组合成完整的姿态矩阵。

如图1所示，第一姿态矩阵和第二姿态矩阵通过融合得到第三姿态矩阵，该融合过
程消除了第一姿态矩阵和第二姿态矩阵的大部分误差，得到更为准确的姿态信息，保存在
第三姿态矩阵。

如图1所示，第三姿态矩阵由于包含了较为精确的姿态信息，可以直接提取出平衡
矢量B和航向矢量Y。

本发明公开了一种无人机飞行控制器姿态解算方法，其程序流程如图2所示。

如图2所示，利用期望平衡矢量与实际平衡矢量之差来控制机体平衡，当期望平衡
矢量不等于实际平衡矢量时，计算平衡矢量差并进行规范化处理，规范化处理后的矢量差
转换成电机矩阵控制量，最后输入到姿态控制单元进行处理。

如图2所示，利用期望航向矢量与实际航向矢量之差来控制机体平衡，当期望航向
矢量不等于实际航向矢量时，计算航向矢量差并进行规范化处理，规范化处理后的矢量差
转换成电机矩阵控制量，最后输入到姿态控制单元进行处理。

本发明无人机飞行器姿态解算和控制方法的无人机飞行控制器采用如图3所示的
结构。

如图3所示，无人机飞行控制器包括飞行控制器顶盖1，上层减震海绵2，IMU模块3，
下层减震海绵5，飞行控制器底板9，电子调速器信号输出插座板6，外部传感器和外设模块
信号插座板7。

如图3所示，从上至下的叠层安装顺序是，飞行控制器顶盖1与上层减震海绵2相连
接，连接介质采用双面不导电胶，上层减震海绵2与IMU模块3相连接，连接介质采用双面不
导电胶，IMU模块3和下层减震海绵5相连接，同时上层减震海绵2也与下层减震海绵5相连
接，下层减震海绵5与飞行控制器底板9相连接，连接介质采用双面不导电胶，电子调速器信
号输出插座板6与飞行控制器底板9垂直连接，连接点处于电子调速器信号输出插座板6底
部与飞行控制器底板9最左侧边缘，外部传感器和外设模块信号插座板7与飞行控制器底板
9垂直连接，连接点处于外部传感器和外设模块信号插座板7底部与飞行控制器底板9最右
侧边缘。

如图3所示，飞行控制器顶盖的体积是52mm×42mm×1.2mm，上层减震海绵的装配
前体积是48mm×40mm×6mm，上层减震海绵的装配后体积是48mm×40mm×5mm，IMU模块的体
积是25mm×22mm×1mm，下层减震海绵的装配前体积是48mm×40mm×6mm，下层减震海绵的
装配后体积是48mm×40mm×5mm，飞行控制器底板的安装元器件前体积是52mm×42mm×
1.2mm，飞行控制器底板的安装元器件后体积是52mm×42mm×3.5m，电子调速器信号输出插
座板的体积是42mm×15mm×1mm，外部传感器和外设模块信号插座板的体积是42mm×15mm
×1mm。

如图3所示，电子调速器信号输出插座板和外部传感器和外设模块信号插座板采
用的接插件是2.54mm镀金排针8，其中电子调速器信号输出插座板包含18×2根排针，外部
传感器和外设模块信号插座板包含17×2根排针。

无人机飞行控制器还包括内置电源。在图4和图5所示，无人机专用锂电池通过一
段硅胶导线和接插件与飞行控制器相连接，在飞行控制器内部输入到Buck型DC-DC开关稳
压器，开关稳压器采用集成Buck型DC-DC控制器TPS54160，该控制器输入段串联500mA自恢
复型保险丝，并联一颗4.7uF/50V低ESR陶瓷电容，输出端并联一颗100uF/10V低ESR陶瓷电
容和一颗100nF/10V低ESR陶瓷电容，环路补偿部分(即COMP引脚)采用一颗102K-1％电阻和
一颗2.2nF/50V串联，再并联一颗1.5pF/50V低ESR陶瓷电容。输出反馈部分为：输出端经串
联的输出反馈电阻(R4和R9)52.6K和10K电阻接地，VSENSE引脚接两个电阻的连接点。通过
将输出反馈电阻设置为52.6K和10K串联，可将DC-DC开关稳压器输出调整为5V直流。

在图4中，开关稳压器的5V输出通过内部线路连接至高精度低压差线性稳压器
SPX1117M3，该稳压器输入端添加一颗4.7uF/10V陶瓷电容，输出端并联一颗100uF/10V低
ESR陶瓷电容和一颗100nF/16V陶瓷电容，即可输出高精度低纹波的3.3V直流电压，该直流
电压为大多数飞行控制器内部设备和传感器供电。

当然，以上仅为本发明的较佳实施例而已，非因此即局限本发明的专利范围，凡运
用本发明说明书及图式内容所为之简易修饰及等效结构变化，均应同理包含于本发明的专
利保护范围之内。