四元数理解 基于匿名四轴的四元数姿态解算
冀亮 钱正洪 白茹
摘 要： 介绍一种只需少量计算就可实现四轴无人机姿态的估计和控制的新颖方法。整个算法采用四元数表示，四元数运算基于惯性测量单元（IMU）的加速度和陀螺仪传感器。姿态估计使用四元数代替三角函数，允许用加速度数据分析推导和优化梯度下降算法来减少由陀螺仪传感器产生的误差。姿态控制将当前姿态与目标姿态的姿态误差反馈到PD控制器来实现想要的目标姿态。姿态估计中用到的梯度下降算法通过四元数对应的欧拉角的均方误差验证，静态均方误差小于0.945°。实际的飞行测试可以实现一个稳定的飞行，同时验证了姿态控制算法。
关键词： 四轴无人机； 四元数； 姿态估计； 姿态控制
中图分类号： TN967?34 文献标识码： A 文章编号： 1004?373X（2015）11?0112?05
Attitude estimation and control of quaternion based quad?axis UAV
JI Liang， QIAN Zheng?hong， BAI Ru
（Center for Integrated Spintronic Devices， Hangzhou Dianzi University， Hangzhou 310018， China）
Abstract： A novel method with few calculations is presented to realize estimation and control of quad?axis UAV. The whole algorithm is expressed with quaternion， and the quaternion calculation is based on the acceleration of inertial measurement units （IMU） and gyroscope sensor. The quaternion is applied in attitude estimation instead of trigonometric function. Gradient descent algorithm is analyzed， deduced and optimized with acceleration data to reduce the error generated by gyroscope sensor. The error of current attitude and target attitude is fed back to PD controller with attitude control， to realize the expectant target attitude. The gradient descent algorithm applied in attitude estimation is verified with RMS error which is obtained from Euler angle corresponded with quaternion. The static RMS error is less than 0.945°. The stable flying is realized by actual flight test， meanwhile the attitude control algorithm is verified.
Keywords： quad?axis UAV； quaternion； attitude estimation； attitude control
0 引 言
四轴飞行器是众多无人机中的典型代表，它通过四个电机作为引擎来实现空中飞行。四轴飞行器类似于传统的直升机，可以垂直起飞和降落，但是它还具有其他的众多优势，如机械结构简单、机动性强、成本低廉。因此，四轴飞行器受到业余爱好者和学术研究领域越来越多的关注。
由于其体积小、重量轻、可以携带一定的载荷以及具有自动导航的能力，四轴飞行器可以解决一些特定的任务，如交通监视、电影录制以及到人类无法到达或者很难到达的地方去实施营救。
系统采用了四元数[1]的方法，相比于欧拉角，四元数不仅可以减小大量的浮点数运算，而且可以避免奇异性问题，以便实现全姿态工作能力。
四轴飞行器的姿态估计使用的是梯度下降算法[2?3]，相比于卡尔曼算法，梯度下降算法具有计算量小和效率高的优势。四轴飞行器的姿态控制是通过改变四个电机的相应速度来改变其推力和扭矩的。为了实现更好的飞行效果，在调试过程中需要不断调整PID[4?5]的三个参数，直到飞行器可以稳定飞行。
1 结构和飞行原理
1.1 结构
四轴飞行器的实物如图1所示，四轴飞行器的结构模型如图2所示。四个电机分别固定在垂直对角线的四个顶点上，分成前后和左右两组。两组电机的旋转方向相反，而每组的两个电机的旋转方向相同，这样是为了抵消反扭力矩。四轴飞行器具有固定的螺距，因此仅仅通过改变四个电机的转速就可以实现各种复杂的动作。
1.2 飞行原理[6]
四轴飞行器的几种基本飞行动作包括：悬停、垂直起降、俯仰运动、横滚运动和偏航运动。下面对飞行器的几种基本飞行动作和飞行原理做简要的分析：
悬停：当四轴飞行器的四个电机的转速相等且四个电机的升力总和等于其自身的重力时，四轴飞行器处于悬停状态。
垂直起降：在悬停的基础上，如果四个电机的升力总和大于或小于其自身重力，那么四轴飞行器将实现垂直起降。
俯仰和横滚运动：在悬停的基础上，如果前面电机的转速增加（或减小），并且后面电机的转速相应地减小（或增加）来保持升力和重力的平衡，四轴飞行器可以实现俯仰运动。横滚运动与俯仰运动的原理一样。
偏航运动：在悬停的基础上，如果一组对角线电机的转速增加，而另一组对角线电机的转速相应地减小来保持升力与重力的平衡，四轴飞行器可实现偏航运动。
2 四元数
2.1 定义
四元数法是利用一种超复数的乘法来等效反映姿态变换的方法。四元数的定义如式（1）所示：
[q=qw+qx?i+qy?j+qz?k=qwqxqyqzT] （1）
用于表示旋转的四元数都归一化为单位化的四元数，即：
[q2=q2w+q2x+q2y+q2z=1] （2）
通过旋转轴和绕该旋转轴旋转的角度可以构造一个四元数，如图3所示。
[qw=cosθ2] （3）
[qx=sinθ2?cosβx] （4）
[qy=sinθ2?cosβy] （5）
[qz=sinθ2?cosβz] （6）
其中： [cos（βx）cos（βy）cos（βz）T]为旋转轴；[cosβx，][cosβy，][cosβz]分别为[x，][y，][z]轴的方向余弦；[θ]为绕该旋转轴所旋转的角度。
其中，操作符[?]代表四元数乘法，四元数乘法将在下面的姿态估计推导过程中起到很重要的作用。
2.3 四元数的逆
[q-1=q?=qw-qx-qy-qzT] （8）
式中：-1表示逆；*表示共轭；T表示转置。
3 姿态估计
3.1 来自陀螺仪的姿态估算
三轴陀螺仪固定在载体上，可以测量机体坐标系下[x，][y，][z]轴的角速度量，分别用[ωx，][ωy，][ωz]表示。四元数的变化率[7]可以用角速度量来表示：
[q=qwqxqyqz=0-ωx2-ωy2-ωz2ωx20ωz2-ωy2ωy2-ωz20ωx2ωz2ωy2-ωx20?qwqxqyqz] （9）
在已知第[n]次的姿态估算值的情况下，可以通过四元数的变化率，得出只有陀螺仪的情况下估算出第[n+1]次的姿态：
[qω，n+1=qest，n+qw，n?Δt] （10）
式中：下标est表示最终估算的结果；下标[ω]表示仅通过陀螺仪传感器估算的结果；[Δt]表示姿态解算周期。
将式（9）代入式（10），可以获得式（11）：
[qω，n+1=1-Δθx2-Δθy2-Δθz2Δθx21Δθz2-Δθy2Δθy2-Δθz21Δθx2Δθz2-Δθy2-Δθx21?qest，n，wqest，n，xqest，n，yqest，n，z] （11）
[Δθ=ω?Δt] （12）
通过比较式（7）和式（11），可以简化来自陀螺仪的姿态更新方程：
[qω，n+1=qest，n?Δq] （13）
[Δq=1Δθx2Δθy2Δθz2] （14）
3.2 来自加速度计的姿态纠正
在地理坐标系下的重力可表示为：
[Ed=001T] （15）
在机体坐标系下，加速度计测量的重力可以表示为：
[Ss=axayazT] （16）
式中，上标E表示地理坐标系；上标S表示机体坐标系。认为加速度计仅仅测量重力或者由于非惯性力产生的额外加速度可以忽略不计。由地理坐标系到机体坐标系的转换矩阵为：
[T=1-2q2y-2q2z2qxqy+2qwqz2qxqz-2qwqy2qxqy-2qwqz1-2q2x-2q2z2qyqz+2qwqx2qxqz+2qwqy2qyqz-2qwqx1-2q2x-2q2y] （17）
那么通过加速度计表示的姿态误差函数为：
[fg（q）=T?Ed-Ss] （18）
即：
[fg（q）=2qxqz-2qwqy-ax2qyqz+2qwqx-ay1-2q2x-2q2y-az] （19）
如果姿态四元数没有误差的话，式（19）的结果应该为0，正是由于存在误差，所以才通过该目标误差函数来求解误差最小值的姿态四元数。由于式（19）的结果有正有负，为了使其一直保持为正，可以进行式（20）的处理：
[f（q）=fg（q）?fg（q）T] （20）
梯度下降算法是众多最优化算法中在实现和计算方面最简单的一种算法。式（21）是梯度下降算法的第[k]次迭代；[?f]是目标误差函数的梯度，描述了目标误差函数上升最快的方向，相反，负梯度描述了目标误差函数下降最快的方向；[λk]是梯度下降算法第[k]次迭代的最优步长[3]；[H]是海森（Hesse）矩阵[3，8]，在这里是目标误差函数对四元数的二次偏导构成的[4×4]矩阵。
[qk+1=qk-λk??fk] （21）
[?f=?f?qw?f?qx?f?qy?f?qzT] （22）
[λk=（?fk）T??fk（?fk）T?Hk??fk] （23）
[H=?2f?qw2?2f?qw?qx?2f?qw?qy?2f?qw?qz?2f?qx?qw?2f?qx2?2f?qx?qy?2f?qx?qz?2f?qy?qw?2f?qy?qx?2f?qy2?2f?qy?qz?2f?qz?qw?2f?qz?qx?2f?qz?qy?2f?qz2] （24）
传统上梯度下降算法需要多次迭代才能寻求最优解，但是在姿态解算中，每个解算周期只需进行一次迭代就可满足要求。因此，通过陀螺仪更新，加速度进行误差纠正后的姿态更新方程变为：
[qest，n+1=qest，n?Δq-λn??f] （25）
4 姿态控制[9]
4.1 计算当前姿态与目标姿态的姿态误差
通过前面的姿态估计已经确定当前姿态，现在应该确定目标姿态。姿态四元数可以用欧拉角来表示[10]：
[q=qwqxqyqz=cos?2cosθ2cosφ2+sin?2sinθ2sinφ2sin?2cosθ2cosφ2-cos?2sinθ2sinφ2cos?2sinθ2cosφ2+sin?2cosθ2sinφ2cos?2cosθ2sinφ2-sin?2sinθ2cosφ2] （26）
式中：[?，][θ，][φ]分别代表横滚角，俯仰角和偏航角。
假定系统上电后的初始姿态为：
[b=qwqxqyqzT] （27）
结合式（26），令俯仰角[θ，]横滚角[?]为0，得到初始目标姿态为：
[m=qw00qzT] （28）
来自手持遥控器的运动增量为[n，]则可以确定最终的目标姿态为：
[t=m?n] （29）
目前已知道当前姿态[a]和目标姿态[t，]令当前姿态与目标姿态的姿态误差为[Δr，]则：
[a?Δr=t] （30）
两边同时乘以[a]的逆，可得姿态误差：
[Δr=a-1?t] （31）
4.2 用姿态误差得出需要的力矩
姿态误差为一四元数，结合式（3）～式（6），可将姿态误差表示为：
[Δr=ΔrwΔrxΔryΔrz=cosθ2vx?sinθ2vy?sinθ2vz?sinθ2] （32）
结合图3，可知姿态误差的旋转轴为[v=（vxvyvz），]是一个单位向量，并且可以用来表示力矩的方向。当[θ]足够小时，[sinθ2]与[θ]成正比，因此可以用来计算力矩强度，令：
[hn=ΔrxΔryΔrzTn] （33）
如果用PD控制，便可以得到力矩[mon：]
[mon=KP?hn+KD?（hn-hn-1）] （34）
式中：[KP]为PID中的比例系数；[KD]为PID中的微分系数。这里未采用积分是因为积分虽然可以消除静态误差，但是会产生一定延时，对于实时控制的四轴飞行器来说很不利，所以在这里未采用PID中的积分项。
4.3 使用力矩来控制油门
四轴飞行器有“X”型和“+”型模型，本文采用“+”模型，其结构如图2所示。四轴飞行器的输出油门是基础值和增量值两个值的和。基础值用来控制整体升力，各个电机相等；增量值用来实现稳定、加速等动作，相当于对基础值的微调，各个电机各不相同，且它们的和为0。力矩仅取决于增量值。
如果需要[x]轴的力矩（不妨取前飞），则油门增量值应为：0、2号电机不变，1号电机负，3号电机正，油门需要变化的方向（direction）为：[dx=0-101T，]引入[x]轴的力矩（moment）修正系数（灵敏度）[MOx，]则需要增加[x]轴力矩[Δmox]时，油门（throttle）增量为：
[Δthx=MOx?dx?Δmox] （35）
即：
[Δthx=0-MOx?Δmox0MOx?ΔmoxT] （36）
[y，][z]轴同理可得：
[dy=-1010T] （37）
[dz=1-11-1T] （38）
[Δthy=MOy?dy?Δmoy] （39）
[Δthz=MOz?dz?Δmoz] （40）
力矩修正系数用于平衡各轴的响应灵敏度。[x，][y]轴的力矩由螺旋桨的升力直接提供，响应够灵敏，[MOx]和[MOy]相等，取较小的值；而[z]轴的力矩需要靠螺旋桨的反转力矩来提供，相同油门增量，能提供的力矩较小，所以[MOz]应该取较大的值，为了方便也可以都设为1，但最好根据实际情况进行调整。
把各轴分量叠加起来就是任意轴的情况，其中力矩为[mo=ΔmoxΔmoyΔmozT，]则油门增量值为：
[Δth=Δthx+Δthy+Δthz] （41）
即：
[Δth=0 -MOy?Δmoy -MOz?Δmoz-MOx?Δmox +0 +MOz?Δmoz0 +MOy?Δmoy -MOz?ΔmozMOx?Δmox +0 +MOz?Δmoz] （42）
最后把增量值和基础值叠加起来，就完成一个控制周期了。假定油门基础量（Base）为base，四个电机的油门量用motor0，motor1，motor2，motor3表示，则四个电机的油门量为：
[motor0=base-MOy?Δmoy-MOz?Δmozmotor1=base-MOx?Δmox+MOz?Δmozmotor2=base+MOy?Δmoy-MOz?Δmozmotor3=base+MOx?Δmox+MOz?Δmoz] （43）
5 实验与结果
5.1 姿态估计的实验
姿态估计可以用如图4的平台进行验证。
5.2 PID调试平台
为四轴飞行器制作了一个PID调试平台，如图6所示。
结合图2，将0号和2号电机固定在平台的固定轴上，来调节[x]轴的PID参数。需要不断调节PID参数，直到四轴飞行器可以稳定的水平飞行。同样方法获得[y]轴与[z]轴的PID参数。
5.3 飞行测试
四轴飞行器在空中可以实现稳定的飞行，飞行效果如图7所示。
6 结 语
本文介绍了一种基于四元数的姿态估计和姿态控制算法。姿态估计采用梯度下降算法，通过姿态的静态均方误差验证了姿态估计的可行性。姿态控制算法采用的是PID算法，通过飞行测试，可以验证姿态控制算法的可行性。
参考文献
[1] 秦永元.惯性导航[M].北京：科学出版社，2006.
[2] MADQWICK S O， HARRISON A J， VAIDVANATHAN A. Estimation of IMU and MARG orientation using a gradient descent algorithm [C]// 2011 IEEE International Conference on Rehabilitation Robotics. [S.l.]： IEEE， 2011： 1?7.
[3] 佚名.无约束最优化方法[EB/OL].[2012?07?21].http：//www.cnblogs.com/zhangchaoyang/articles/2600491.html.
[4] 佚名.PID控制[EB/OL].[2015?03?11].http：//baike.baidu.com/view/262316.htm？fr=aladdin.
[5] 佚名.PID控制理论[EB/OL].[2010?12?20].http：//wenku.baidu.com/link？url=6k8WZ81gQMog7kOHu?PGQ5QDHFSmWKZ3UwIhM_
cgXgTMeo1m7VcReguP0K5Iplzn7riTEAP1MSX3MvrhelO_82HXDSmADJ0_c4TC9gIO4m.
[6] 刘峰，吕强，王国胜，等.四轴飞行器姿态控制系统设计[J].计算机测量与控制，2011，19（3）：583?585.
[7] 邓正隆.惯性技术[M].哈尔滨：哈尔滨工业大学出版社，2006.
[8] 林亚坤，钱正洪，白茹，等.基于GMR传感器的姿态检测系统设计[J].仪表技术与传感器，2014（8）：53?55.
[9] 佚名.四轴：完善中的控制算法[EB/OL].[2012?09?15].http：//blog.sina.com.cn/s/blog_81f1e26801019sg4.html.
[10] 佚名.四元数与欧拉角之间的转换[EB/OL].[2010?11?21]. http：//www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html.
