基于卡尔曼滤波的动态角度测量系统设计*

角度测量技术可以确定被测物体与水平面的倾斜角或者其运动姿态,在军事、民用领域中都有着重要应用,如在机械加工、建筑中的倾斜角度的测量,无人飞行器的航行姿态角度,智能机器人的运动姿态控制,以及在医疗中的运动康复仪等[1]。

MEMS数字倾角仪主要运用在静态环境下的角度测量,在动态环境下,倾角仪内部的加速度计除受到重力加速度外还有物体运动方向上的加速度影响,此时仅依靠加速度计的输出无法准确解算出被测物体动态角度(运动状态)。而陀螺仪能够测量物体绕轴转动的角速度,通过对角速度积分就可以得到物体旋转的角度,利用陀螺仪和加速度计的互补性,即加速度计可以提供静止时的倾角(准确性较高);陀螺仪能够提供短时间内物体的动态角度,用其进行积分得到这段时间内的运动角度,利用卡尔曼滤波融合加速度计和陀螺数据解算得到较精确的双轴动态下角度[2](俯仰角和横滚角)。

1 整体架构设计

系统整体架构如图1所示,MEMS传感器采用InvenSense公司的MPU9255,其内部集成了三轴加速度计(设定测量范围为±2 gn)、三轴陀螺仪(设定测量范围为±500 dps),和三轴磁力计,与外部以I2C协议通讯,MCU为ST公司的STM32F405RG 32 bit单片机。

A)很高（超过5种学习方式）B)比较高（3-4种方式）C)高（2-3种方式）D一般（1-2种方式）E）很低（0-1种）

采集MPU9255姿态传感器输出的加速度和角速率数值,利用其中三轴加速度计解算出静态初始的俯仰角和滚转角后,再加上旋转轴的角速率利用Kalman滤波后,得到我们预测估计的角度,最后可以通过OLED实现实时显示或串口传给上位机进行控制后进一步处理。

图1 系统整体框架图

图2 旋转坐标系示意图

2 动态测量原理

如图2,当物体的倾角发生变化时,相当于坐标系分别绕相应轴转动3次后,到达新的位置oxyz,即:

视高考如生命的北方人，早在那个年月里就变态地迫切追求升学率了。我初中时便有晚课，分两个班，一个叫补差，一个叫培优，月考分数排名靠前去培优，靠后则去补差。我和朋友的功课都属于中不溜阵营，上下随便一波动就波动去了隔壁班，两人一会儿培优一会儿补差，好不热闹。

得到的方向余弦矩阵C=CγCθCφ[3-4],可表示为:

C=

在这里,我们令:

当SDA处在下降沿时,SCL置高,I2C的开始通讯,在开始信号发出后,主控芯片开始写入7个地址位和1个读写位,读写位决定了主从机的读写状态,然后主机释放SDA线,等待从机的ACK应答信号,每次数据传输后必须跟一位读写位。STM32通过写入地址,读取出MPU9255内部的三轴加速度值和三轴陀螺仪输出的角速率值。

C

设在载体坐标系下的加速度为(aBx,aBy,aBz);参考坐标系下的加速度(aix,aiy,aiz)之间的关系为

当处于静止状态,此时参考系下的加速度等于重力加速度可表示为:

如图3(a)所示系统内传感器跟随系统绕X轴转动产生俯仰角(Pitch)γ,跟随系统绕Y轴转动横滚角(Roll)θ。在处于静止状态时由上述推导可求出此时的俯仰角(Pitch)和横滚角(Roll)[3-4]。

这事儿，我当时最清楚。张麻子说。那时侯，我和村长四细狗住邻居，老鳜鱼提着刀子，进了村长的院子，隔着竹篱笆墙，我看的一清二楚。

图3 系统绕X、Y轴的转动角度

通过采集MPU9255内部的三轴加速计输出:Ax、Ay、Az,分别表示X、Y、Z轴输出的加速度值(m/s2)。

Pitch:γ=arctan(Ax/Az)

图4 MPU9255调理电路

3 系统实现

3.1 硬件平台设计

MPU9255内部集成3个16 bit AD加速度数字输出、3个16 bit AD陀螺仪数字输出和3个16 bit AD磁力计数字输出。通过软件编程写入读取地址,可选择内部三轴加速度计的量程,分别±2 gn,陀螺仪选择±500 dps,所设计的系统不需要磁力计数据。具体MPU9255调理电路如图4所示。

主控芯片采用ST公司Cortex-M4内核32 bit单片机STM32F405RG,其内部采用DSP指令集,168 MHz的高速主频,能满足该系统数据处理和卡尔曼滤波的实现,有方便拓展的外围接口;MPU9255以I2C总线协议与主控芯片进行通讯,时钟线SCL、数据线SDA分别与STM32的I/O口PB8和PB9连接。具体通讯时序如图5所示。

图5 通讯时序图

现在的人会觉得拿部《辞海》来一查不就解决了吗？这当然不错。但那时还没有《辞海》，那是大约1937或1938年的事。中华书局的第一本《辞海》是1947年出版的，这已经是十年以后的事了。我那时僻居农村，只靠死读、死背和苦思苦想，哪里知道有什么工具书？就连注释这些古诗文的书也无处可找。到1947年中华书局的《辞海》出来了，我竭尽全力买到了一部，这就成为我的读书宝钥，凡是不懂的都去问这部书。

3.2 软件部分

软件部分包括下位机程序和上位机软件两个部分组成。如图6所示为下位机程序流程框图。

图6 程序流程框图

STM32单片机以MPU9255自检信号作为判断传感器是否正常工作,通过I2C总线协议获取三轴加计和三轴陀螺的输出数字量,转化成加速度和角速率后进行静态角度解算、卡尔曼滤波后的数据以帧格式进行浮点传输,波特率为115 200 bit/s,帧格式如表1所示。

表1 编帧格式

数据帧头校验码俯仰角横滚角0XAA1Byte4Byte4Byte

上位机软件采用Labview编写,通过VISA控件对串口数据流进行读取,对帧头校验并读取有效角度数据。通过强制类型转换将16进制浮点数据解码处理,再通过波形图控件实时绘制出俯仰角和横滚角的角度值,可以直观的看到角度大小以及变化趋势,点击保存按钮可以将数据存储为.txt文件,方便后续的处理。如图7所示为系统水平静止时,上位机界面显示的俯仰角和横滚角,并用3D控件显示实现动画。

图7 测量上位机

4 卡尔曼滤波融合测量

基于卡尔曼滤波有效的估计出动态下的角度值,具体是将陀螺仪输出角速率进行积分与加计数据同步融合,从而获得当前系统的角度[5-6]。在测量的整个过程中,陀螺仪与加速度计权重不断进行调整。

主控MCU对MPU9255模块的内加速度计和陀螺仪进行采样、滤波、解算。

第1步 进行系统模型的建立,我们将角度测量近似看作线性系统,在卡尔曼滤波中建立状态方程和量测方程[7-8]。其中:

状态方程:

X(k+1)=AX(k)+Bw(k)

(1)

量测方程:

Z(k)=H(k)X(k)+V(k)

(2)

式中:X(k)是k时刻系统的状态;A为状态转移矩阵;B噪声分布矩阵;w(k)是系统过程白噪声;Z(k)是量测量;H(k)是量测系统参数;V(k)为量测系统噪声。

第2步 k时刻的预测角度近似为k-1时刻的静态角度加上陀螺仪测的角速率乘以时间;k时刻陀螺的静态漂移近似与k-1时刻相同,即βk=βk-1。

国内PDA大致有两种形式，一是读者直接通过邮箱、论坛、到馆推荐的方式向图书馆提供所需文献的信息；二是图书馆与原版图书的出版商或是代理商合作，在图书馆举办大型原版书展，邀请读者参与，并根据他们的挑选情况进行现场采买。2014年5月，天津大学图书馆和中国进出口总公司就曾联合举办了这样的原版书展，参展的都是最新出版的原版外文图书，是经有关专家和国外出版社推荐，从剑桥大学出版社等众多国际知名出版社中精选出来的，他们让图书馆的读者现场参与挑选，最后，根据推荐情况进行择优订购。像这样让读者充分参与图书采购决策，能够增加原版外文图书的利用率，将滞架率降低。

具体的模型可表示为:

αk=αk-1+(ωk-1+βk-1+Wk-1)dt

(3)

式中:αk为k时刻的预测角度;ωk-1、βk-1、Wk-1分别为k-1时刻的陀螺仪的角速度、静态漂移和测量噪声;dt是测量时间。

综上所述，DN 患者血清 Cys-C、HCY、HbA1c和UmALB表达水平显著升高，联合检测血清Cys-C、HCY、HbA1c和UmALB表达水平对DN的早期诊断优于单项检测，对评价DN的发生发展以及预后有重要的临床意义。

第3步 建立状态更新方程:

1.2.2.1 气相色谱条件。色谱柱为HP-5MS毛细管柱(30 m×0.25 mm × 0.25 μm)；程序升温：从60 ℃开始,以10 ℃/min升到120 ℃,然后再以20 ℃/min升到220 ℃,最后以10 ℃/min升到280 ℃；进样口温度为270 ℃；载气为He；柱流量：0.8 mL/min；进样量：0.1 μL,分流比：50∶1。

(4)

系统的量测方程:

(5)

式中:Uk是陀螺仪输出;Ts是采样时间;Zk是加速度计测的角度;加速度计测的角度和陀螺仪的漂移βk不相关,所以测量系统参数是[1 0];Vk是加速度测量噪声。

由式(3),可得到系统状态预测方程:

从国际范围来看，由于自由贸易区建设带来了更多的经济和外贸刺激作用，因此，世界范围内自贸区的数量越来越多，覆盖的国家及地区越来越多；且随着自贸区内出口贸易企业产业链的完善，自贸区的功能也开始有了综合性的发展；其次，由于世界经济发展在各区域的差异较大，因此，因地制宜和特色化管理制度的自贸区也开始广泛化发展[2]。

Xk=Xk|k-1+Kk(Zk-HXk|k-1)

(6)

一步预测协方差阵:

Pk|k-1=APk-1AT+Q

(7)

式中:滤波增益Kk=Pk|k-1HT(HPk|k-1HT+R)-1;Zk为利用加计输出计算的角度;Q是系统过程噪声协方差矩阵是量测误差的协方差阵

最后 更新协方差:

Pk=(I-KkH)Pk|k-1

(8)

图8 验证实验

5 系统测试

具体步骤如下:

具体验证步骤:

（5）DI数据仓库。主要构建面向主题级别的数据仓库，将业务数据打散为元数据，去除数据的冗余，使数据更规整，方便进行多维度的透视，生成相应的数据集市。

(1)如图8所示,中框转动90°,将系统安装放置在三轴转台上,将系统水平静止5 min。

(2)此时的俯仰角在零度附近,控制转台的中框分别转动30°、90°,此时,输出纯加速度计输出的曲线;重复上述实验,输出卡尔曼滤波后的曲线。

中框的转动为加速到匀速再到减速的过程,加速与减速时加速度计会受到影响,从而导致测量结果的不准确[9]。

行政管理工作人员的工作执行能力直接决定了行政管理的实效性，在行政管理的整个过程中执行能力贯彻了全过程，换言之，执行能力是一种能够实现管理目标的方式和手段，因此，高校的领导层和决策层必须要强化行政管理工作人员的执行能力，从而提高行政管理工作的效率和质量，让行政管理工作能够落到实处。

图9和图10分别为倾角仪转动30°和90°的过程中,卡尔曼滤波前后的对比图,可以看出转动会使纯加速度计倾角仪在运动过程中测量的角度不准,通过滤波后可利用上一个时刻的角度和陀螺输出估计下一刻的角度得到滤波后的角度,使得测量的角度更加精准。

图9 卡尔曼滤波前后俯仰角输出曲线

图10 卡尔曼滤波前后俯仰角输出曲线

6 结论

参考文献:

通过提出并设计了一种动态角度测量系统,利用卡尔曼滤波对加速度计和陀螺仪的数据进行融合,解决了加速度计在动态下无法准确测量角度和陀螺仪由于静态漂移测量导致的测量精度不高的问题[10]。实际测试表明,该测量系统能够测量动态角度,卡尔曼滤波数据融合方法适用于对信号的实时处理,具有较好的实际应用价值。

中学地理课堂教学以及学生学习的成败，在于地理课堂教学的活力，活力十足的教学课堂，直接影响着学生地理知识思维能力成果的优劣，在地理教学中，结合学生原有的地理水平和生活实际，不仅可以有利于降低许多地理问题的难度，使学生能充分理解，而且还可以避免许多学生在地理学习中的误区，使学生形成正确的地理思维。还可以降低他们对地理恐惧的心理，增强学生学好地理的自信心，真正的达到了地理课堂教学中学习活力的提升的目的。

[1] 马洪连,郑保重,王伟. 基于MEMS 技术倾角测量系统的设计与实现[J]. 仪器仪表学报,2005,26(8):185-187.

[2] 张杰. 基于MEMS陀螺仪和加速度计的动态倾角传感器[J]. 机械设计与制造,2012(9):141-143.

[3] 刘俊,石云波,李杰. 微惯性技术[M]. 北京:电子工业出版社,2005.

一是护理人员自身的因素。护理人员自身的临床经验与专业知识上的欠缺是影响护理工作效果的重要因素之一，心血管疾病的特性决定了护理人员需要有充足的专业知识储备，丰富的临床经验积累。部分刚毕业的护理人员工作经验欠缺，对于专业知识的理解不深入，工作经验不足，在工作中可能会出现疏忽大意、警觉性低以及用药和操作不规范等，导致在护理工作中对患者的安全带来威胁。另外，部分护理人员的责任感不高也是一个重要的不安全因素。部分护理人员的岗位责任意识不强，工作不积极甚至是拖拉敷衍，只会机械完成医嘱上的要求，对于患者关心不够，容易导致突发情况的产生，影响到心血管内科护理工作的正常进行。

[4] 李杰,洪惠惠,张文栋. MEMS微惯性测量组合标定技术研究[J]. 传感技术学报,2008,21(7):1169-1173.

[5] 冯智勇,曾瀚,张力,等. 基于陀螺仪及加速度汁信号融合的姿态角度测量[J]. 西南师范大学学报,2011,36(4):137-141.

[6] 苑艳华,李四海,南江. 基于卡尔曼滤波器的航姿系统测姿算法研究术[J]. 传感技术学报,2011,24(12):1718-1722.

[7] 秦永元,张洪钺,汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安:西北工业大学出版社,1998.

[8] 王晓宇,闫继宏,秦勇,等. 基于扩展卡尔曼滤波的两轮机器人姿态估计[J]哈尔滨工业大学学报,2007,39(12):1920-1924.

阿东这天还是迟到了。他跟处长解释迟到的原因。处长说：“是呀，我也听说了。附近的人都叫那条路‘官道’。网上还编了歌骂哩。”

[9] 崔建伟,廖启征,郭磊. 基于ARM微控制器的动态倾角传感器设计与制作[J]. 仪表技术与传感器,2007(9):63-65.

[10] 付勇杰,丁艳红,梁义维,等. 动态倾角传感器及其传递特性的研究[J]. 仪表技术与传感器,2012(9):6-8.

张 波(1992-),男,汉族,河南开封人,现中北大学在读硕士研究生,主要从事微系统集成、惯性测量方向的研究,562043762@qq.com;

李 杰(1976-),男,汉族,山西省岚县人,教授,博士生导师,目前的主要研究方向为微系统集成理论与技术、惯性感知与控制技术、组合导航理论、计算几何及智能信息处理等。