一种基于四元数的无人机姿态解算方法与流程

本发明涉及一种姿态解算方法，特别是涉及一种基于四元数的无人机姿态解算方法，属于无人机姿态控制技术领域。

背景技术：

四元数的应用非常广泛，相比于旋转矩阵四元数有更少的计算量，相比于欧拉角，四元数描述刚体转动不会有万向节死锁问题，在本章中我们将会介绍基于三轴陀螺仪和三轴加速度计的互补滤波姿态解算方法，用于得到无人机的姿态并进行闭环控制。

采用欧拉角表述刚体绕轴旋转，是在描述刚体的绕定点旋转时我们需要三个独立坐标变量，而描述刚体绕固定轴旋转只需要一个独立变量即转角，把刚体绕定点旋转的过程分解成三个互相独立的绕定轴转动即是欧拉角，如图1所示。

使用欧拉角来表示一个刚体的绕轴旋转只需要三个角度(α,β,γ)分别表示刚体绕(x,y,z)三个轴的旋转，由于用欧拉角来表示旋转需要有先后次序，很容易出现万向节死锁的问题，在无人机的姿态解算中不适合直接使用欧拉角来做为无人机的姿态。

万向节死锁(gimballock)：由于使用欧拉角描述刚体旋转的时候有旋转的次序，当旋转时最外层和最内层旋转轴同轴时将会使当前次序的欧拉角描述失去一个自由度。

举个常见的例子：假设一台高射炮有两个方向的旋转自由度，假设炮台底座可以自由自传，炮管以与底座连接处为旋转轴，垂直与底座平面旋转，此时一架飞机由正东方向对着炮台飞行，此时炮台可以指向目标射击，当飞机飞行到炮台正上方时突然转向正北方飞行，此时炮台就丢失了目标，炮台不能再连续追踪飞机了，必须要自转90度才能继续追踪目标。

技术实现要素：

本发明的主要目的是为了提供一种基于四元数的无人机姿态解算方法，通过三轴陀螺仪和三轴加速度计互补滤波姿态解算得到无人机的姿态并进行闭环控制。

本发明的目的可以通过采用如下技术方案达到：

一种基于四元数的无人机姿态解算方法，该基于四元数的无人机姿态解算方法是通过姿态四元数作为媒介，融合陀螺仪和加速度计到姿态四元数中，最后再从姿态四元数中将pitch俯仰角，roll横滚角解算出来。

该基于四元数的无人机姿态解算方法，包括如下步骤：

步骤1：初始化姿态四元数q＝(1,0,0,0)，即对准初始坐标系；

步骤2：采集三轴陀螺仪的值，求解四元数微分方程；

步骤3：采集加速度计的值，融合进姿态四元数进行互补滤波；

步骤4：规范化姿态四元数；

步骤5：从四元数中求解出roll和pitch。

融合进姿态四元数进行互补滤波需要的imu惯性测量单元为三轴陀螺仪和三轴加速度计。

三轴加速度计可以直接换算成对地倾角；

三轴陀螺仪积分在短时间内可以获得准确角度；

将三轴陀螺仪和三轴加速度计的数据分别计算角度，然后将这两者获得的角度加权得出优化后的角度作为姿态角。

四元数为维空间的实矢量α＝a1i+a2j+a3k与一个实数α0组合成一个a＝a0+α的数；

其中，实数a0称为四元数的标部，实矢量α称为四元数的矢部；

虚数单位

四元数与欧拉角的转换关系如下：

q＝((x，y，z)，w)

给定一个欧拉角(x,y,z)，即分别绕x轴、y轴和z轴旋转x、y、z度。

四元数的模方如下：

与四元数q＝a0+a对应的另一个四元数是q＝a0-a，而四元数a(a0+a)的模方为：

只有模方为1的四元数可以用来表示旋转。

本发明的有益技术效果：本发明提供的基于四元数的无人机姿态解算方法，是通过姿态四元数作为媒介，融合陀螺仪和加速度计到姿态四元数中，最后再从姿态四元数中将pitch俯仰角，roll横滚角解算出来。

附图说明

图1为欧拉角示意图。

具体实施方式

为使本领域技术人员更加清楚和明确本发明的技术方案，下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

本实施例提供的基于四元数的无人机姿态解算方法，该基于四元数的无人机姿态解算方法是通过姿态四元数作为媒介，融合陀螺仪和加速度计到姿态四元数中，最后再从姿态四元数中将pitch俯仰角，roll横滚角解算出来，包括如下步骤：

步骤1：初始化姿态四元数q＝(1,0,0,0)，即对准初始坐标系；

步骤2：采集三轴陀螺仪的值，求解四元数微分方程；

步骤3：采集加速度计的值，融合进姿态四元数进行互补滤波；

步骤4：规范化姿态四元数；

步骤5：从四元数中求解出roll和pitch。

融合进姿态四元数进行互补滤波需要的imu惯性测量单元为三轴陀螺仪和三轴加速度计，三轴加速度计可以直接换算成对地倾角，三轴陀螺仪积分在短时间内可以获得准确角度，将三轴陀螺仪和三轴加速度计的数据分别计算角度，然后将这两者获得的角度加权得出优化后的角度作为姿态角。

四元数为维空间的实矢量α＝a1i+a2j+a3k与一个实数α0组合成一个a＝a0+α的数，其中，实数a0称为四元数的标部，实矢量α称为四元数的矢部，其中，虚数单位

四元数与欧拉角的转换关系如下：

q＝((x，y，z)，w)

给定一个欧拉角(x,y,z)，即分别绕x轴、y轴和z轴旋转x、y、z度。

四元数的模方如下：

与四元数q＝a0+a对应的另一个四元数是q＝a0-a，而四元数a(a0+a)的模方为||a||＝||a||＝aa＝aa＝a0+a·a；

只有模方为1的四元数可以用来表示旋转。

四元数定：一个三维空间的实矢量α＝a1i+a2j+a3k与一个实数α0组合成一个数a＝a0+α称为四元数，其中实数a0称为四元数的标部，实矢量α称为四元数的矢部。其中虚数单位可以用下面方法

如图2所示：

两个四元数p(p0，p1，p2，p3)和q(q0，q1，q2，q3)相乘可以展开成如下：

四元数的乘法可以用下述表达式简化表达(sa为四元数标量部分，va为四元数矢量部分)

qa＝[sa，va]，qb＝[sb，b]

从上式可以看出四元数乘法中涉及到向量的叉乘，所以四元数乘法不满足交换率，但是四元数乘法满足结合律。此外四元数的乘法是完全封闭的，两个四元数相乘其结果还是一个四元数。

四元数共轭和模方：与四元数q＝a0+a对应的另一个四元数是而四元数a(a0+a)的模方为(只有模方为1的四元数可以用来表示旋转)

四元数求逆：(我们一般用共轭四元数除以模方的方式来求取四元数的逆以降低运算量)

四元数与旋转：模方为1的四元数可以表示一个绕转轴(n＝icosa+jcosβ+kcosγ)的旋转，其中n表示旋转轴(我们只需要用到四元数与旋转的关系，具体推导请参考秦永元的《惯性导航》)我们可以使用这个旋转四元数q来执行一个旋转。把三维空间的p绕着n表示的旋转轴旋转θ角度，我们首先把点p扩展到四元数空间，即四元数p＝(p，0)。那么，旋转后新的点p′＝(p′，0)对应的四元数(计算而得的四元数的标部为0，虚部系数就是新的坐标)为：旋转四元数的微分方程如下，具体推导在此不赘述，请参考秦永元的《惯性导航》。

互补滤波：姿态解算需要的imu(惯性测量单元)为三轴陀螺仪与三轴加速度计。其中加速度计可以直接换算成对地倾角可是加速度计受震动影响较大，长时间可以作为稳定的角度参考，而陀螺仪积分在短时间内可以获得很准的角度但是长时间会有累积误差。将陀螺仪和加速度计的数据分别计算角度然后将这两者获得的角度加权得出优化后的角度作为姿态角，这种方法就叫做互补滤波。

欧拉(euler)方法：虽然求解常微分方程有各种各样的解析方法，但解析方法只能用来求解一些特殊类型的方程。求解从实际问题当中归结出来的微分方程主要靠数值解法。欧拉方法是一类重要的数值解法。这类方法回避解y(x)的函数表达式，而是寻求它在一系列离散节点上的近似值。

对于一个简单的一阶微分方程：

y′＝f(x，y)

其中初始值为：

y(x0)＝y0

欧拉方法等同于将函数微分转换为数值差分，如下式

故原方程变形为

yn+1＝yn+h(迭代此式即可求解微分方程)

欧拉法可以通过泰勒展开来证明：

若要获得更高精度的微分方程数值解法即可使用runge-kutta法，即加权的欧拉方法。(一阶的runge-kutta即为偶拉法)

下面介绍四旋翼中使用的姿态更新算法，需要注意的是此次姿态解算算法中的输入参数均为弧度制，跟新周期为5ms。

//欧拉法的四元数更新，输入为三轴角速度和三轴加速度计voidqua_update(floatgx，floatgy，floatgz，floatax，floatay，floataz)

{

staticfloatexint＝0，eyint＝0，ezint＝0；//定义积分，我们融合中一般不使用积分，只有当角度收敛慢时可以适当给一点积分加速收敛。

staticfloatkp＝7.2f，ki＝0.00f，halft＝0.0025f；//设置采样周期为5ms，则法f/2为0.0025ms

floattmp_q0，tmp_q1，tmp_q2，tmp_q3；//计算四元数缓冲

floatsize；//归一化模

floatvx，vy，vz；//四元数估算重力分量大小

floatex，ey，ez；

floatq0q0＝q0*q0；

floatq0q0＝q0*q1；

floatq0q0＝q0*q2；

floatq0q0＝q0*q3；

floatq0q0＝q0*q1；

floatq0q0＝q0*q2；

floatq0q0＝q0*q3；

floatq0q0＝q0*q2；

floatq0q0＝q0*q3；

floatq3q3＝q3*q3；//预计算，用以减少重复计算

size＝sqrt(ax*ax+ay*ay+az*az)；//归一化加速度计模

ax＝ax/size；

ay＝ay/size；

az＝az/size；//加速度计算重力分量大小

vx＝2*(q1q3-q0q2)；

vy＝2*(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；//分解姿态四元数中的重力分量

ex＝(ay*vz-az*vy)；

ey＝(az*vx-ax*vz)；

ez＝(ax*vy-ay*vx)；//计算角速度级测量的角度和姿态四元数的角度

exint＝exint+ex*ki；

eyint＝eyint+ey*ki；

ezint＝ezint+ez*ki；

/*

if(fabs(ex)＜＝0.01)

exint＝0；

if(fabs(ey)＜＝0.01)

eyint＝0；

if(fabs(ez)＜＝0.01)

ezint＝0；*///在此不使用积分

gx＝gx+kp*ex+exint；

gy＝gy+kp*ey+eyint；

gz＝gz+kp*ez+ezint；//互补滤波

tmp_q0＝q0+(-q1*gx-q2*gy-q3*gz)*halft；

tmp_q1＝q1+(q0*gx+q2*gz-q3*gy)*halft；

tmp_q2＝q2+(q0*gy-q1*gz+q3*gx)*halft；

tmp_q3＝q3+(q0*gz+q1*gy-q2*gx)*halft；//一定要使用缓冲

q0＝tmp_q0；

q1＝tmp_q1；

q2＝tmp_q2；

q3＝tmp_q3；//更新四元数

size＝sqrt(q0*q0+q1*q1+q2*q2+q3*q3)；//归一化姿态四元数模

q0＝q0/size；

q1＝q1/size；

q2＝q2/size；

q3＝q3/size；//规范化姿态四元数

pitch＝asinf(-2*q1*q3+2*q0*q2)*57.296f；//pitch

(57.296为换算单位180/π)

roll＝atan2f(2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1)*57.296f；

//roll其中atan2f函数比atan稳定，不会出现90度的反正切奇异值。

yaw＝atan2f(2*(q1*q2+q0*q3)，q0*q0+q1*q1-q2*q2-q3*q3)*57.296f；

//yaw四旋翼中不使用角度控制yaw

}//5ms执行一次四元数姿态解算函数。

综上所述，在本实施例中，本实施例提供的基于四元数的无人机姿态解算方法，是通过姿态四元数作为媒介，融合陀螺仪和加速度计到姿态四元数中，最后再从姿态四元数中将pitch俯仰角，roll横滚角解算出来。

以上所述，仅为本发明进一步的实施例，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的技术方案及其构思加以等同替换或改变，都属于本发明的保护范围。