您的位置：首页 > 公文范文公文范文
基于MEMS器件的姿态航向系统实现
2025-08-19人已围观
基于MEMS器件的姿态航向系统实现
摘要:针对目前捷联式姿态航向系统低成本、低功耗、高精度、微型化的发展趋势,提出一种基于MEMS器件的姿态航向系统的实现方案,介绍了该系统的方案设计与算法原理,实验证明该系统可以很好的测量出载体的姿态航向,可用于小型无人直升机的导航控制中。
关键词:姿态航向系统;MEMS器件;姿态算法;圆锥运动
中图分类号:U666文献标识码:A文章编号:1009-3044(2009)33-9527-03
Implementation of Attitude Heading System based on MEMS Units
CUI Lu-lu, HAO Ying-guang
(School of Electronic and Information Engineering, Dalian University of Technology, Dalian 116024, China)
Abstract: In view of the current strapdown attitude and heading system"s development trend in low cost, low power, high accuracy and miniaturization, an implementation of attitude and heading system based on MEMS devices is proposed. The system"s design and algorithm theory are introduced. The experiment proves that the system can get a good result on measuring attitude,which can be embedded in navigation of small unmanned helicopter control.
Key words: attitude and heading system; MEMS units; attitude algorithm; coning motion
捷联惯导系统是惯性传感器(主要指陀螺和加速度计)直接固连在运载体上的惯导系统,捷联式姿态航向系统(AHS,Attitude and Heading System)是基于捷联惯导系统原理,通过陀螺仪和加速度计测量载体的角运动和加速度,得到载体姿态和航向信息的系统,目前已成为惯性技术发展的一个重要方向。然而无论是国内还是国外较先进的AHS系统由于传感器因素导致体积和重量都较大,成本昂贵,这在一定程度上限制了航向姿态测量系统的应用。
随着微电子技术的发展而出现的微电子机械系统(MEMS,Micro Electro Mechanical System)由于具有尺寸小、重量轻、成本低、功耗小和动态范围宽等优点[1],被广泛应用于测量领域。本文从工程实践出发,介绍了一种基于微机械惯性传感器的姿态航向系统,该系统以ST公司的32位单片机STM32F103RBT6为核心,以ADIS16355为基本测量元件,同时采用电子罗盘作为辅助测量元件以保证测量精度。
1 系统方案设计
1.1 硬件设计
姿态航向系统的主要任务是:采集各类传感器信号,主要指陀螺仪和加速度计信号,将采集到的角速度和加速度信息进行导航解算,最后,将解算后的姿态信息送到显示终端显示。系统主要由惯性数据采集模块、处理器模块、RS-485电平转换模块以及外围辅助模块四部分组成,图1为系统硬件原理框图。
该文处理器模块由STM32F103RBT6及其外围电路构成,72MHz主频、32位数据宽度能够实现高速、高性能,同时可以方便的與惯性传感器进行数据通信,满足姿态航向系统对运算速度和精度的要求。
惯性数据采集模块由ADIS16355单芯片实现,其六自由度、14位数字输出的高集成度特性,省去了安装和设计数据采集电路的麻烦,标准的SPI接口简化了传感器与处理器的数据通信。使系统无需额外的解码电路和逻辑控制电路便可以实现导航解算。RS-485模块实现LVTTL/LVCMOS电平到RS-485电平之间的转换,使导航信息通过RS-485总线传输到PC。实际应用中,需要用到RS232/RS485转换器将RS-485信号转换成RS-232信号,然后与计算机通信。这里用RS-485传输而不用RS-232直接传输,主要是考虑到RS-485受外界干扰小,传输距离远,适合环境恶劣的场合。
1.2 软件设计
本系统软件设计主要包括传感器数据采集、角度对准和解算以及整个系统的任务调度。软件流程图如图2所示。
系统首先执行初始化操作,包括STM32内核和外设的初始化以及系统任务和变量的初始化。之后通过检查读取数据标志是否置位来确定起止数据读取操作,其中读取数据标志的设置通过微控制器的高级定时器TIM1实现。TIM1每10ms产生一次中断设置读取数据标志,实现微控制器每10ms从ADIS16355中读取一组数据。采样5组数据后,经过平滑滤波取均值用于姿态解算。最后,系统判断数据采样是否完成,进入姿态解算状态。需要注意的是,传感器的片选信号是通过微控制器的通用定时器TIM2控制的。开始读取数据时,系统将传感器的片选信号拉低,使能传感器,同时使能定时器TIM2。当定时时间到产生中断时,系统将片选信号拉低,失能传感器。这样,不仅可以高效的利用传感器,还可以进一步降低功耗。
2 姿态航向系统算法
2.1 坐标系参数说明
本系统选取导航坐标系为东北天地理坐标系Oxtytzt,载体坐标系为机体坐标系Oxbybzb,其中xb指向载体的右侧,yb指向载体的纵轴方向,zb指向载体的竖轴方向。载体坐标系相对地理坐标系的方位为载体的姿态角,包括俯仰角θ、横滚角γ与航向角ψ。
2.2 捷联姿态算法
载体具有静止状态、低速运动状态和高速旋转状态三种工作状态,针对这三种状态采用不同的姿态解算方法,以达到解算精度与计算量的平衡。
1) 加速度计确定姿态算法
在载体处于静止或匀速直线运动时,加速度计可以单独确定载体的横滚和俯仰角[2]:
(1)
航向角ψ可以由电子罗盘得到初始值ψ0,并通过陀螺仪 轴测量值的积分获得,即:
(2)
其中ψk为陀螺仪z轴第k个周期的采样值,dt为采样周期。
2) 四元数姿态算法
四元数算法适用于低速运动下的载体,姿态矩阵可以表示为姿态角θ,γ,ψ的关系:
(3)
设载体坐标系相对地理坐标系的转动四元数为:
(4)
四元数与姿态矩阵间的关系为:
(5)
四元数进行归一化,通过公式(6)即可求出姿态角:
(6)
3) 等效旋转矢量算法
当运载体姿态变化剧烈时,四元数解算将会出现严重的不可交换性误差,1971年JohnE.Bortz提出了等效旋转矢量概念[3],将运载体的姿态四元数更新转化为姿态变化四元数的更新,对不可交换误差作了适当补偿,正好弥补了四元数法的不足。由于本系统采用的陀螺仪输出为角速率,因此考虑角速率输出情况下的等效旋转矢量算法。
① 计算陀螺仪在一个姿态更新周期h内的四次采样值ω1,ω2,ω3,ω4;
② 将陀螺仪一个周期内的输出以三项式拟合,即:ω(T+t)=a+2bt+3ct2+4dt3
③ 计算等效旋转矢量 (7)
其中
锥运动环境下进行优化
(8)
(9)
④ 利用Φ=[Φx Φy, Φz]构造一个变换四元数,即:
(10)
其中
⑤ 利用Q(t+h)=Q(t) ?茚q(h)更新四元数从而得到姿态角。
2.3 姿态融合算法
对载体运动状态的实时判断是进行姿态算法融合的依据,本系统提取两方面信息判断载体运动状态。
1) 匀速运动门限值εa
在静止或匀速运动的情况下,加速度计三轴的测量值应满足下式:
(11)
实际系统中由于加速度计精度与噪声的问题,在判断时不可能完全满足上式,而需要设置一个门限值εa,只要加速度计三轴分量的测量值的平方和减去重力加速度的平方的差小于此门限值,即可近似的认为载体处于静止或匀速状态。
2) 旋转运动门限值εω
旋转运动可通过陀螺仪的角速度输出加以判断:
(12)
式中:ωibbx、ωibby、ωibbz为载体坐标系x、y、z轴的角速度输出。
本系统将载体做高动态运动的角速度门限值设为10°/s。
姿态融合算法是指载体在匀速运动或静止时,利用加速度计和电子罗盘获取姿态信息,同时每周期检测系统的运动状态,当系统检测到载体在进行低速非旋转运动时,就在上周期加速度计解算值的基础上利用四元数算法继续解算,若系统恢复匀速运动,就重新利用加速度计解算,当系统检测到载体做旋转运动时,则直接用等效旋转矢量算法进行解算。
3 系统实验与仿真
3.1 低动态环境下实验
在Keil μVision3集成开发环境下对系统进行调试与实现,姿态算法部分采用C语言编写。通过实时采集低动态环境下传感器输出数据,并分别采用四元数算法和融合算法两种解算模式进行姿态解算,将解算结果输入到matlab中进行评估与分析。
系统解算初始条件设为:经度38.8859°,纬度121.5243°,高度100m,俯仰角0.0°,横滚角0.0°,方位角0.0°,姿态解算周期为0.01s,解算时间为40s。
解算结果如图3、图4所示,误差结果如表1所示。
由表1可以看出, 单纯使用四元数解算,由于陀螺漂移的存在,算法误差会随时间增长而累积,而通过实时判断载体运动状态,利用加速度计对陀螺漂移进行重调补偿,航向角的误差漂移减小了一个数量级。在低动态环境下,系统解算精度俯仰角與横滚角误差<0.8°/s,航向角误差<0.2°/s,满足系统设计要求。
3.2 高动态环境下仿真
由于实验条件有限,无法确定载体的高动态运动状态,因此对于旋转运动状态下的解算采用MATLAB进行仿真,以确定旋转矢量算法的精确性。
以含误差的圆锥运动作为仿真环境,仿真条件设置如下:
考虑陀螺仪的误差为白噪声,其测量误差模型为:
=ε+wg
其中ε为系统随机常值漂移,wg为白噪声,陀螺仪实际输出为:
(14)
锥运动的半锥角α=1°、锥频率为f=15Hz,系统陀螺仪随机常值漂移为0.0015°/s,白噪声密度为0.07°/s,姿态更新周期h=12ms、仿真时间tmax=60s。
仿真结果如图5、6所示。
由仿真结果可以看出,当载体做高速旋转运动时,姿态角误差增大,旋转矢量算法精度高于四元数解算精度。
4 结论
该文以STM32F103RBT6为核心处理器,以ADIS16355为惯性检测传感器,设计实现了一个小型姿态航向系统,实验表明采用嵌入式处理器及基于MEMS的惯性传感器,经过合理的软、硬件设计,可以很好的完成导航任务,实现系统的微型化和低成本,而根据载体运动状态采用不同的姿态解算算法,则可以提高不同环境下导航系统可靠性,满足工程中对系统精度的要求,实现预期的设计目标。
参考文献:
[1] Bernstein J.An overview of MEMS inertial sensing technology[EB/OL]http://www.performandhealth.com/gongwenfanwen/2022/0525/articles/0203/14/.
[2] 沈晓蓉,张海,范耀祖.加速度计辅助模糊混合捷联姿态确定算法研究[J].压电与声光,2008(2):106-108.
[3] Bortz J E. A New Mathematical Formulation for Strapdown Inertial Navigation[J].IEEE Transactions on Aerospace and Electronic Systems,1971,7(1):61-66.
[4] 薛定宇,陈阳泉.基于MATLAB/Simulink的系统仿真技术与应用[M].北京:清华大学出版社,2002:281-296
[5] 黄旭,王常虹.基于微机械惯性器件和磁强计的姿态航向系统[J].传感器技术,2005(8):54-56.
相关热词搜索：
航向
器件
姿态
系统实现
MEMS
2022/0525/56849
摘要:针对目前捷联式姿态航向系统低成本、低功耗、高精度、微型化的发展趋势,提出一种基于MEMS器件的姿态航向系统的实现方案,介绍了该系统的方案设计与算法原理,实验证明该系统可以很好的测量出载体的姿态航向,可用于小型无人直升机的导航控制中。
关键词:姿态航向系统;MEMS器件;姿态算法;圆锥运动
中图分类号:U666文献标识码:A文章编号:1009-3044(2009)33-9527-03
Implementation of Attitude Heading System based on MEMS Units
CUI Lu-lu, HAO Ying-guang
(School of Electronic and Information Engineering, Dalian University of Technology, Dalian 116024, China)
Abstract: In view of the current strapdown attitude and heading system"s development trend in low cost, low power, high accuracy and miniaturization, an implementation of attitude and heading system based on MEMS devices is proposed. The system"s design and algorithm theory are introduced. The experiment proves that the system can get a good result on measuring attitude,which can be embedded in navigation of small unmanned helicopter control.
Key words: attitude and heading system; MEMS units; attitude algorithm; coning motion
捷联惯导系统是惯性传感器(主要指陀螺和加速度计)直接固连在运载体上的惯导系统,捷联式姿态航向系统(AHS,Attitude and Heading System)是基于捷联惯导系统原理,通过陀螺仪和加速度计测量载体的角运动和加速度,得到载体姿态和航向信息的系统,目前已成为惯性技术发展的一个重要方向。然而无论是国内还是国外较先进的AHS系统由于传感器因素导致体积和重量都较大,成本昂贵,这在一定程度上限制了航向姿态测量系统的应用。
随着微电子技术的发展而出现的微电子机械系统(MEMS,Micro Electro Mechanical System)由于具有尺寸小、重量轻、成本低、功耗小和动态范围宽等优点[1],被广泛应用于测量领域。本文从工程实践出发,介绍了一种基于微机械惯性传感器的姿态航向系统,该系统以ST公司的32位单片机STM32F103RBT6为核心,以ADIS16355为基本测量元件,同时采用电子罗盘作为辅助测量元件以保证测量精度。
1 系统方案设计
1.1 硬件设计
姿态航向系统的主要任务是:采集各类传感器信号,主要指陀螺仪和加速度计信号,将采集到的角速度和加速度信息进行导航解算,最后,将解算后的姿态信息送到显示终端显示。系统主要由惯性数据采集模块、处理器模块、RS-485电平转换模块以及外围辅助模块四部分组成,图1为系统硬件原理框图。
该文处理器模块由STM32F103RBT6及其外围电路构成,72MHz主频、32位数据宽度能够实现高速、高性能,同时可以方便的與惯性传感器进行数据通信,满足姿态航向系统对运算速度和精度的要求。
惯性数据采集模块由ADIS16355单芯片实现,其六自由度、14位数字输出的高集成度特性,省去了安装和设计数据采集电路的麻烦,标准的SPI接口简化了传感器与处理器的数据通信。使系统无需额外的解码电路和逻辑控制电路便可以实现导航解算。RS-485模块实现LVTTL/LVCMOS电平到RS-485电平之间的转换,使导航信息通过RS-485总线传输到PC。实际应用中,需要用到RS232/RS485转换器将RS-485信号转换成RS-232信号,然后与计算机通信。这里用RS-485传输而不用RS-232直接传输,主要是考虑到RS-485受外界干扰小,传输距离远,适合环境恶劣的场合。
1.2 软件设计
本系统软件设计主要包括传感器数据采集、角度对准和解算以及整个系统的任务调度。软件流程图如图2所示。
系统首先执行初始化操作,包括STM32内核和外设的初始化以及系统任务和变量的初始化。之后通过检查读取数据标志是否置位来确定起止数据读取操作,其中读取数据标志的设置通过微控制器的高级定时器TIM1实现。TIM1每10ms产生一次中断设置读取数据标志,实现微控制器每10ms从ADIS16355中读取一组数据。采样5组数据后,经过平滑滤波取均值用于姿态解算。最后,系统判断数据采样是否完成,进入姿态解算状态。需要注意的是,传感器的片选信号是通过微控制器的通用定时器TIM2控制的。开始读取数据时,系统将传感器的片选信号拉低,使能传感器,同时使能定时器TIM2。当定时时间到产生中断时,系统将片选信号拉低,失能传感器。这样,不仅可以高效的利用传感器,还可以进一步降低功耗。
2 姿态航向系统算法
2.1 坐标系参数说明
本系统选取导航坐标系为东北天地理坐标系Oxtytzt,载体坐标系为机体坐标系Oxbybzb,其中xb指向载体的右侧,yb指向载体的纵轴方向,zb指向载体的竖轴方向。载体坐标系相对地理坐标系的方位为载体的姿态角,包括俯仰角θ、横滚角γ与航向角ψ。
2.2 捷联姿态算法
载体具有静止状态、低速运动状态和高速旋转状态三种工作状态,针对这三种状态采用不同的姿态解算方法,以达到解算精度与计算量的平衡。
1) 加速度计确定姿态算法
在载体处于静止或匀速直线运动时,加速度计可以单独确定载体的横滚和俯仰角[2]:
(1)
航向角ψ可以由电子罗盘得到初始值ψ0,并通过陀螺仪 轴测量值的积分获得,即:
(2)
其中ψk为陀螺仪z轴第k个周期的采样值,dt为采样周期。
2) 四元数姿态算法
四元数算法适用于低速运动下的载体,姿态矩阵可以表示为姿态角θ,γ,ψ的关系:
(3)
设载体坐标系相对地理坐标系的转动四元数为:
(4)
四元数与姿态矩阵间的关系为:
(5)
四元数进行归一化,通过公式(6)即可求出姿态角:
(6)
3) 等效旋转矢量算法
当运载体姿态变化剧烈时,四元数解算将会出现严重的不可交换性误差,1971年JohnE.Bortz提出了等效旋转矢量概念[3],将运载体的姿态四元数更新转化为姿态变化四元数的更新,对不可交换误差作了适当补偿,正好弥补了四元数法的不足。由于本系统采用的陀螺仪输出为角速率,因此考虑角速率输出情况下的等效旋转矢量算法。
① 计算陀螺仪在一个姿态更新周期h内的四次采样值ω1,ω2,ω3,ω4;
② 将陀螺仪一个周期内的输出以三项式拟合,即:ω(T+t)=a+2bt+3ct2+4dt3
③ 计算等效旋转矢量 (7)
其中
锥运动环境下进行优化
(8)
(9)
④ 利用Φ=[Φx Φy, Φz]构造一个变换四元数,即:
(10)
其中
⑤ 利用Q(t+h)=Q(t) ?茚q(h)更新四元数从而得到姿态角。
2.3 姿态融合算法
对载体运动状态的实时判断是进行姿态算法融合的依据,本系统提取两方面信息判断载体运动状态。
1) 匀速运动门限值εa
在静止或匀速运动的情况下,加速度计三轴的测量值应满足下式:
(11)
实际系统中由于加速度计精度与噪声的问题,在判断时不可能完全满足上式,而需要设置一个门限值εa,只要加速度计三轴分量的测量值的平方和减去重力加速度的平方的差小于此门限值,即可近似的认为载体处于静止或匀速状态。
2) 旋转运动门限值εω
旋转运动可通过陀螺仪的角速度输出加以判断:
(12)
式中:ωibbx、ωibby、ωibbz为载体坐标系x、y、z轴的角速度输出。
本系统将载体做高动态运动的角速度门限值设为10°/s。
姿态融合算法是指载体在匀速运动或静止时,利用加速度计和电子罗盘获取姿态信息,同时每周期检测系统的运动状态,当系统检测到载体在进行低速非旋转运动时,就在上周期加速度计解算值的基础上利用四元数算法继续解算,若系统恢复匀速运动,就重新利用加速度计解算,当系统检测到载体做旋转运动时,则直接用等效旋转矢量算法进行解算。
3 系统实验与仿真
3.1 低动态环境下实验
在Keil μVision3集成开发环境下对系统进行调试与实现,姿态算法部分采用C语言编写。通过实时采集低动态环境下传感器输出数据,并分别采用四元数算法和融合算法两种解算模式进行姿态解算,将解算结果输入到matlab中进行评估与分析。
系统解算初始条件设为:经度38.8859°,纬度121.5243°,高度100m,俯仰角0.0°,横滚角0.0°,方位角0.0°,姿态解算周期为0.01s,解算时间为40s。
解算结果如图3、图4所示,误差结果如表1所示。
由表1可以看出, 单纯使用四元数解算,由于陀螺漂移的存在,算法误差会随时间增长而累积,而通过实时判断载体运动状态,利用加速度计对陀螺漂移进行重调补偿,航向角的误差漂移减小了一个数量级。在低动态环境下,系统解算精度俯仰角與横滚角误差<0.8°/s,航向角误差<0.2°/s,满足系统设计要求。
3.2 高动态环境下仿真
由于实验条件有限,无法确定载体的高动态运动状态,因此对于旋转运动状态下的解算采用MATLAB进行仿真,以确定旋转矢量算法的精确性。
以含误差的圆锥运动作为仿真环境,仿真条件设置如下:
考虑陀螺仪的误差为白噪声,其测量误差模型为:
=ε+wg
其中ε为系统随机常值漂移,wg为白噪声,陀螺仪实际输出为:
(14)
锥运动的半锥角α=1°、锥频率为f=15Hz,系统陀螺仪随机常值漂移为0.0015°/s,白噪声密度为0.07°/s,姿态更新周期h=12ms、仿真时间tmax=60s。
仿真结果如图5、6所示。
由仿真结果可以看出,当载体做高速旋转运动时,姿态角误差增大,旋转矢量算法精度高于四元数解算精度。
4 结论
该文以STM32F103RBT6为核心处理器,以ADIS16355为惯性检测传感器,设计实现了一个小型姿态航向系统,实验表明采用嵌入式处理器及基于MEMS的惯性传感器,经过合理的软、硬件设计,可以很好的完成导航任务,实现系统的微型化和低成本,而根据载体运动状态采用不同的姿态解算算法,则可以提高不同环境下导航系统可靠性,满足工程中对系统精度的要求,实现预期的设计目标。
参考文献:
[1] Bernstein J.An overview of MEMS inertial sensing technology[EB/OL]http://www.performandhealth.com/gongwenfanwen/2022/0525/articles/0203/14/.
[2] 沈晓蓉,张海,范耀祖.加速度计辅助模糊混合捷联姿态确定算法研究[J].压电与声光,2008(2):106-108.
[3] Bortz J E. A New Mathematical Formulation for Strapdown Inertial Navigation[J].IEEE Transactions on Aerospace and Electronic Systems,1971,7(1):61-66.
[4] 薛定宇,陈阳泉.基于MATLAB/Simulink的系统仿真技术与应用[M].北京:清华大学出版社,2002:281-296
[5] 黄旭,王常虹.基于微机械惯性器件和磁强计的姿态航向系统[J].传感器技术,2005(8):54-56.
相关热词搜索：
航向
器件
姿态
系统实现
MEMS
2022/0525/56849
下一篇：坚持党的领导,深入推进依法治国