桂林航天工业学院学报２０１９年第３期
(JOURNALOFGUILINUNIVERSITYOFAEROSPACETECHNOLOGY 信息与电子工程总第９５期)
捷联系统下的四元数姿态解算
()桂林电子科技大学 电子工程与自动化学院,广西 桂林 ５４１００４
∗
杨可晗 唐源 何川 黄杰华
∗∗
)摘 要 随着MEMS(微机电系统)技术的逐步发展,多旋翼飞行器飞行控制系统中的航姿参考系统(逐步摒弃AHRS
了机械陀螺仪和高昂的光纤陀螺仪的设计方案,普遍采用基于MEMS的传感器进行姿态测量.由于陀螺仪
自身存在温漂、零漂、不正交误差和积分累计误差,同时加速度计存在运动加速度干扰,磁罗盘也容易受外部环境的影响等客观不利因素的存在,如何利用AHRS信息更精准的解算姿态成为多旋翼飞行器飞行控制的难点,尤其是在抑制噪声干扰和姿态信息估计等方面.文章基于Cortex－M４的单片机系统和MPU６５００姿计算出飞行器姿态信息.实验结果表明,该系统较好地解决了噪声干扰与姿态估计的问题,达到了长时间稳定输出准确可靠的的要求.
态传感器搭建了一套飞行控制系统,内部调用DMP处理器,将陀螺仪、加速度计和磁罗盘三者数据进行融合,
关键词 多旋翼;姿态解算;四元数
()中图分类号:TH１２２ 文献标志码:A 文章编号:２０９５Ｇ４８５９２０１９０３Ｇ０３２６Ｇ０５
飞行器的姿态估计是飞行器自主飞行的先决
条件,在飞行过程中,实时准确地获取飞行器的姿态信息,决定了飞行器的控制精度和稳定性
航姿参考系统(用于测量无人机的姿AHRS)
[]１
动处理器,可以直接输出四元数,通过基于捷联惯导理论的坐标换算,将四元数转换为姿态角度.HMC５９８３中附带霍尼韦尔专利的集成电路可用于自检、自消磁驱动和偏差补偿.航姿参考系统外围电路如图１所示.
.
,态信息(俯仰角、横滚角和航向角)这些姿态信息会用于无人机的控制和导航,是无人机实现自主飞
２]
.目前,行和智能飞行的控制核心[市面上具有代
这些A３飞控和N３飞控以及拓攻的M２和T１等,
均被广泛应用在警用消防、航拍摄影和农用植保等领域.
[]统均采用低成本的AHR其主要包括:基于微S３,
表性的飞行控制系统主要有大疆创新的A２飞控、
随着微电子技术的发展,目前民用飞行控制系
机电系统(的三轴陀螺仪、三轴加速度计MEMS)
]４
.系统内部包含多个姿态测量传和三轴磁罗盘[
感器,在获取飞行器的姿态估计信息必须对多个传感器进行数据融合,并寻求最佳数据处理算法.
１ 姿态解算系统硬件架构
姿态解算系统微处理采用ST公司的
九轴航姿参考系统STM３２F４２７VIT６单片机系统,
采用T三轴陀螺仪＋三轴DK公司的MPU６５００(
加速度计)配合使用Honewell公司的HMC５９８３y(.MP三轴磁罗盘)U６５００内部包含DMP数字运
∗
图１ 航姿参考系统外围电路图
３２６
∗∗
().基金项目:国家级大学生创新创业训练项目“多旋翼送餐飞行器”２０１６１０５９５０３８作者简介:杨可晗,男,河北石家庄人.本科生.研究方向:无人机飞行控制.
桂林航天工业学院学报２０１９年第３期杨可晗 唐源 /文 ()JOURNALOFGUILINUNIVERSITYOFAEROSPACETECHNOLOGY总第９５期何川 黄杰华
２ 四元数转换欧拉角
２．１ 欧拉角
欧拉角是一种用来描述坐标系的三维旋转方
量.上文小节２．１中定义机载坐标系nb和地理坐标系n为等价坐标系,可得ωbωbωbnb＝n,n表示机体相对于地理坐标系的角速度矢量.设机体坐标系
５]
,式[即从一个坐标系到另外一个坐标系的变换,b和地理坐标系n之间的变换四元数的三角形
[１１]
可以通过绕参考坐标系的不同坐标轴的三次连续式为:
、旋转进行实现.欧拉角是由自转角ϕ(横滚角)章动角θ(俯仰角)和进动角ψ(偏航角)组成.例
如,从地理坐标系转换到机体坐标系可以表示
[]６
如下:
]７
.绕Z轴转动为偏航角ψ;绕Y轴转动为俯仰角θ[
ϕ;
如图２所示,绕地理坐标系X轴转动为横滚角du)此:＝０,可将式(化简为:４
dt•
•
d１σ•１σ•σduQ＝－sinσ＋ucosσ＋sindt２２２２２dt()４
为参考坐标系下的一个单位矢量,因为u因
σσos＋usinQ＝c２２
)对公式(求导可得:３
()３
１σ•１σ•
inσ＋ucosσQ＝－s
２２２２
图２ 欧拉角
σσöσæç＝ucos＋usin÷２è２２ø
)、())将公式(代入(式５得:２３
()５
２．２ 四元数
四元数姿态表达式是一个四参数的表达式.四元数复数表达式如公式(所示,其中q１)０为实部,i、k为两两相互正交的单１、２、３为虚部,qqqj、
８]
.位向量[
•１b()６Q＝Qωn２
忽略地球的自转和公转运动对飞行器造成的
影响,并假设飞行器为刚体运动且质量分布对称、均匀,中心为重心位置且不随着运动而发生变
]１２
.那么四元数矩阵表达式Q的微分方程解化[•
T••••
()Q＝[为:i＋k１０＋１２３Q＝qqqj＋q０q１q２q３]qT•由于载体的转动,四元数Q＝[０ １ ２ ３]qqqqéqé０－ωb－ωb－ωbxzùéy０ùqê０ùúêúêú、、、.是变量,是时间函数四元数姿态表•０q１q２q３qêúbbbê１qωx０ωz－ωyúê１úqêú＝１ê达式的基本思路是:坐标系与坐标系之间的变化可)ú７êú(•bbbêú２ωy－ωz０－ωxúê２úqê２q以通过绕定义在参考坐标系中的矢量单次转动来êúêbúêúbb•úêωω－ω０４ûqëûë[zxy９]ëû３q实现.因此定义刚体绕瞬时转轴u转过σ角度
b１０])中ω７ωbωbx,z分别表示机体坐标系相对于.来实现坐标系的转换[y,u为参考坐标系下的一公式(
个单位矢量,其角速度为:
地理坐标系沿各个轴向的角速度分量.
３ 四元数转换为欧拉角()２．ωbuσ２nb＝
方向余弦可以用欧拉角或四元数表示)公式(中b表示机体坐标系,２nb表示机载坐
１３]
:如下[b标系,表示机体相对于机载坐标系的角速度矢ωnb２２
(２b((c１cé－c－d２)２bc－ad)２bd＋ac)ù１２c１３ùê１úéêa＋ú
n２２２２
(((cCb＝ê２bc＋ad)a－b＋c－d)２cd－ab)ú＝２１c２２c２３ú＝ê
êúúêúêúê２２
(((cëû２bd－ac)２cd＋ab)a２－b－c＋d２)ë３１c３２c３３û
cosθcosinsinθcosossinossinθcosinsinéψsϕψ－cϕψcϕψ＋sϕψùêú
êcosθsininsinθsinoscosossinθsinincosψsϕψ＋cϕψcϕψ－sϕψúêúêúsinθsincosθcoscosθë－ûϕϕ()８３２７
桂林航天工业学院学报２０１９年第３期杨可晗 唐源 /文 ()JOURNALOFGUILINUNIVERSITYOFAEROSPACETECHNOLOGY总第９５期何川 黄杰华)中各元素对应关系,可以用方向余 根据式(８
弦表示欧拉角,具体关系如下:
cé１２ù
rctanêϕ＝aêúú
cë３３û
θ＝arcsin[－c３１]()９
ψ＝arctanéêêcëc１２ùú１１úû
姿态解算算法
姿态解算算法的目的在于将多个姿态测量传
感器的原始数据,通过解算算法进行融合,使得输出的数据相比于单个的传感器更加准确和可靠.单个陀螺仪可以测量得到机体转动的角速率,通过积分可以得到瞬时角度,但是积分存在累计误差,加速度计可以测量出机体的加速度,然后根据地球重力的分量,通过倾角求解计算出机体的角度,但是,机体容易受到振动以及转动和运动加速度的干扰;通过传感器与传感器之间存在自身三轴非完全正交,引入了不正交误差.因此,必须对三个姿态测量传感器进行数据融合,通过姿态解算算法消除误差,解算出飞行器的姿态信息.
本文基于四元数与欧拉角的换算关系,飞行控制系统中的微处理调用MPU６５００内部自带的
后根据公式MP数据融合库,
输出姿态解算需要的四元数,最(９
)换算为欧拉角. 姿态解算算法验证与数据分析
使用连接MPUSTM３２F４２７战舰开发板通过同时将姿态解算后的姿态I２
C接口
串６５口００模块板,
角度使用调试助手进行显示.将模块板水平放置在桌面上,采集的零点静态误差,连续采集数据D频率为２０Hz.将采集到的部分数据导入５MMPU６５００~P数据融合后１０分钟,采样成图表,如图３和图４所示.图３中实e线xce代l生
表MP数据融合后的俯仰角.图４中实线代表MP数据融合后的横滚角.
图３ DMP数据融合后的俯仰角
３２８
图４ DMP数据融合后的横滚角
DMP数据融合后的姿态角的标准差如表所示.
１
表１ 姿态角度的标准差
姿态角度平均值标准差俯仰角０－．００８．１３６
２６
０．０１横滚角
P０．０３４８４９
直接调用DM数据融合,
一方面可以将单片机从姿态解算算法处理的压力中出来,将更多的精力用来处理姿态控制和电机转速调节,这样可大大提高系统的实时性.另一方面,解算出的姿态角度在精度方面,静态零点误差保持在但是,这种集成的处理单元,库代码并不开源０．１５°以内.,融合的参数也不能做修改,后期算法优化难度很大,并且在硬件开发上了姿态传感器芯片的选择范围,因此,本文对DMP数据融合算法不做进一步验证.
为了验证飞行控制系统的动态响应能力,购买了慧联科技生产的HEC３９５－九轴姿态航向参考系统作为姿态角度参考.如图系统和上,
手动匀速转动转台HEC３９５放置在,同时采集两个姿态测量系PT５－５所示,将飞行控制
三自由度手动转台统输出的姿态角度.图５ 三轴机械转台上的姿态测量系统
将三轴手动转台保持两个转轴不变,手动匀速转动第三个转轴,并将飞行控制系统采集到的绕第
３D４DD桂林航天工业学院学报２０１９年第３期杨可晗 唐源 /文 ()JOURNALOFGUILINUNIVERSITYOFAEROSPACETECHNOLOGY总第９５期何川 黄杰华(,三个转轴转动的姿态角度和HE动态误差C３９５采集到的角册,HEC３９５的静态误差为０．５°RMS)
().度导入e如图６和图７所示.为１xcel表图中进行比较,°RMS
横坐标表示采样时间,纵坐标表示姿态角度.根据如图６所示,虚线为横滚运动的基准,(,手动转动转台的速度,将采样频率设置为１实线为飞行控制系统使０HzHEC３９５采集的横滚角)
,整个转动过程是从－２转到２其中根据产品手用本文提出的姿态解算算法后输出的横滚角度.０°０°
图６ 横滚角动态跟踪
图７ 俯仰角动态跟踪
图７中虚线采集到的俯仰角度相对于实线采
集到的俯仰角度较为平滑,同时可以发现整个转动过程,以虚线代表的HEC３９５采集到的俯仰角作为基准线,本文设计的飞行控制系统采集到的俯仰(角误差保持在２纵坐标一小格)以内.°
由图６和图７可知,飞行控制系统进行俯仰和
飞行控制系统动态误差较大有可能存在两个方面的原因,一方面是由于飞行控制系统中姿态测量传感器和HEC３９５中传感器在转台上因摆放位置导致水平面内X和Y坐标轴并不重合,从而随着转动角度变大,误差逐渐变大;另一方面有可能由于MEMS传感器的生产工艺不同导致.
横滚运动时,采集到的姿态角度相比于HEC３９５HEC３９５内部采用的ADI公司的微电子机械系统(,成熟的商用航姿参考系统)零点附近误差较小,(iMEMS)惯性传感器,它采用的是一种数据较为准确,但是随着转动的角度变大和时间的I相比与本文使用的廉价nterleaved－CMOS工艺,推移,误差逐渐趋近于线性误差.
３２９
的Invensense公司的MEMS惯性传感器的NF
桂林航天工业学院学报２０１９年第３期杨可晗 唐源 /文 ()JOURNALOFGUILINUNIVERSITYOFAEROSPACETECHNOLOGY总第９５期何川 黄杰华()工艺,Nasiri－FabricationiMEMS惯性传感器可５ 结束语
靠性和线性度更高、性能更加稳定.本文基于DMP数据处理,读取出四元数,然
综上,本文设计的姿态解算算法的飞行控制系
后采用四元数坐标换算,将陀螺仪、加速度计和磁罗盘三者数据进行融合,求解出姿态角度.并搭建了半实物仿真平台进行实验验证.实验结果表明,姿态解算算法较好地解决了多传感器的数据融合的问题,达到了长时间输出准确可靠的姿态数据的要求.
统,姿态角度测量动态响应良好,误差基本保持在左右,静态误差保持在１左右.与国内外成熟的２°°
高精度姿态测量系统误差相比,依旧存在一定差距,但总体达到了基本的动态和静态误差指标要求.
参考文献
[]]():任芊．四旋翼飞行器航姿测量系统的数据融合方法[兵工自动化,１ 张浩,J．２０１３,３２１２８Ｇ３１．[]]李智,王勇军．基于S电子技术应用,２ 何川,TM３２的四旋翼飞行器的姿态最优估计研究[J．２０１５,():４１１２６１Ｇ６４．[]]杨忠,陈喆,等．传感器与微系统,３ 郭晓鸿,EKF和互补滤波器在飞行姿态确定中的应用[J．２０１１,():３０１１１４９Ｇ１５２．[]]贾宏光,陈涛,等．基于四元数法的捷联式惯性导航系统的姿态解算[光学精密工程,４ 张荣辉,J．():２００８,１６１０１９６３Ｇ１９７０．[]]():王强,蓝东浩．基于S科技视界,５ 吴司林,TM３２微处理器的飞行器设计与实现[J．２０１８９９７Ｇ９８．[]王秀萍,王丽霞．捷联惯性导航技术[北京:国防工业出版社,６ 张天光,M]．２００７:１３Ｇ１４．[]]张铁民,廖贻泳．基于模糊－比例积分偏差修正的多旋翼飞行器姿态测算系统[农业工７ 廖懿华,J．
():程学报,２０１４,３０２０１９Ｇ２７．
[]]康利平,余运俊,等．互补滤波算法在四旋翼飞行器姿态解算中的应用[测控技术,８ 万晓凤,J．
():２０１５,３４２８Ｇ１１．[]惯性技术[哈尔滨:哈尔滨工业大学出版社,９ 邓正隆．M]．２００６:１０Ｇ１１．
[]肖治垣,马东立．模型飞机空气动力学[北京:航空工业出版社,１０M]．２００７:８Ｇ１０．[]杨续伟．基于微惯性组合的嵌入式航姿参考系统研究[上海:上海交通大学,１１D]．２０１２．[]秦永元．惯性导航[北京:科技出版社,１２M]．２００６:２８９Ｇ２９２．
９１０Ｇ９１４．
[]李一波,]:宋述锡．基于模糊自整定P控制工程,１３ID四旋翼无人机悬停控制[J．２０１３,２０(５)
(责任编辑 陈葵晞)
３３０
