CN110207647A - 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 - Google Patents
一种基于互补卡尔曼滤波器的臂环姿态角计算方法 Download PDFInfo
- Publication number
- CN110207647A CN110207647A CN201910379845.9A CN201910379845A CN110207647A CN 110207647 A CN110207647 A CN 110207647A CN 201910379845 A CN201910379845 A CN 201910379845A CN 110207647 A CN110207647 A CN 110207647A
- Authority
- CN
- China
- Prior art keywords
- attitude angle
- armlet
- avr
- state
- complementary
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
本发明主要公开了一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其技术方案:包括以下步骤:步骤1:搭建姿态角测量***,***上电启动,陀螺仪、加速度计、磁传感器开始读取数据;步骤2:开始时,手臂平放处于静止水平状态,载体系xyz三轴分别指向水平右前上,采集一段时间序列N长度的传感器数据,计算状态X初始值X0;步骤3:***噪声阵、量测噪声噪初始化;步骤4:状态量X的预测更新;步骤5:量测Z更新;步骤6:滤波增益K计算;步骤7:状态X估计;步骤8:运动结束,反之若运动未结束,则循环执行步骤4~步骤7;对加速度计、陀螺仪、磁传感器测量的角度进行融合,并得到一种更加精确的臂环姿态角计算结果。
Description
技术领域
本发明涉及臂姿态角测量技术领域,特别是一种基于互补卡尔曼滤波器的臂环姿态角计算方法。
背景技术
在GINS中,姿态是通过环架上的角度传感器敏感的,无需计算,而在SINS 中,所有的导航计算(位置、速度、姿态)都要通过计算得出,而且姿态计算(attitude calculation)的频率最高。
通过臂环佩戴于手腕或者小臂处时,现有的技术方案种实现小臂姿态角测量一般有两种方法,一是基于加速度计和磁分别进行姿态角测量,这种方法测量的精度较差,容易受运动情况干扰,二是基于陀螺仪进行测量,这种方法测量的结果容易随时间漂移。其中专利申请号:201610207713.4,公开了一种基于互补卡尔曼滤波算法计算融合姿态,其公开的技术方案是通过螺仪测量数据计算得到第一姿态角度,根据加速度计测量数据计算得到第二姿态角度,根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度,最后根据互补滤波原理,将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。首先其融合方式过程比较粗糙,融合策略不精准,其次其陀螺仪和加速度计只采用六轴融合,不能完全保证3自由度的姿态解算都精确不发散,只能保证2自由度的水平姿态精度。
发明内容
针对现有技术存在的不足,本发明提供一种基于互补卡尔曼滤波器的臂环姿态角计算方法,对加速度计、陀螺仪、磁传感器测量的角度进行融合,并得到一种更加精确的臂环姿态角计算结果,便可实现人体小臂或者手腕的运动估计,继而更进一步实现人体的运动监测、AR/VR用途。
为了达到上述目的,本发明通过以下技术方案来实现:一种基于互补卡尔曼滤波器的臂环姿态角计算方法,包括以下步骤:
步骤1:搭建姿态角测量***,***上电启动,陀螺仪、加速度计、磁传感器开始读取数据;
步骤2:开始时,手臂平放处于静止水平状态,载体系xyz三轴分别指向水平右前上,采集一段时间序列N长度的传感器数据,计算状态X初始值X0;
步骤3:***噪声阵、量测噪声噪初始化;
步骤4:状态量X的预测更新;
步骤5:量测Z更新;
步骤6:滤波增益K计算;
步骤7:状态X估计;
步骤8:运动结束,反之若运动未结束,则循环执行步骤4~步骤7。
本发明进一步:步骤1具体包括:
加速度计三轴数据为fb=[fx_b fy_b fz_b]T,陀螺仪三轴数据为 Gb=[Gx_bGy_b Gz_b]T,磁传感器三轴数据为Mb=[Mx_b My_b Mz_b]T;b表示载体坐标系,原点为臂环中心位置,XYZ三个方向分别指向当前传感器位置的右前上方向。
本发明进一步:步骤2具体包括:
此处的N可以根据实际传感器采样频率取下式中的 fb_avr=[fx_b_avr fy_b_avr fz_b_avr]T、Gb=[Gx_b_avr Gy_b_avr Gz_b_avr]T、Mb_avr=[Mx_b_avr My_b_avrMz_b_avr]T均表示基于这段时间序列N的采样均值,状态X取=[θ γ ψ wx wy wz]T,分别表示小臂的俯仰角,横滚角,航向角,x 轴角速率,y轴角速率,z轴角速率;初始值X0的具体计算公式如下:
其中g表示当地重力加速度,可取值为9.80665m/s2;D表示地球上当地的磁偏角。
本发明进一步:步骤3具体包括:
***噪声驱动阵Γ和***噪声矩阵Q为:
***噪声矩阵Q由陀螺仪的三轴输出噪声方差构成,T为卡尔曼的滤波周期;
量测噪声噪声阵R为:
量测噪声矩阵R由互补滤波输出的噪声方差构成。
本发明进一步:步骤4具体包括:
状态量X的预测更新为:
状态X的一步预测协方差矩阵Pk+1/k为:
上式中Pk表示协方差估计矩阵,其初始值为状态的初始误差,可以根据器件的测量精度给定。
本发明进一步:步骤5具体包括:
量测信息由互补滤波进行计算,将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到,量测矩阵H为:
上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数,这可以根据实际***进行调整,φxk+1、φyk+1、φzk+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角:
与步骤2中计算初始值时类似,不同之处在于,上式中 fb=[fx_b fy_b fz_b]T、Gb=[Gx_b Gy_b Gz_b]T、Mb=[Mx_b My_b Mz_b]T表示传感器k+1时刻的取值。
本发明进一步:步骤6具体包括:
滤波增益K计算具体见下:
上式中的Hk+1表示量测矩阵,其为:
本发明进一步:步骤7具体包括:
状态X估计,具体见下式:
此时的估计结果:便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角,同时可以得到;状态X估计的协方差矩阵Pk+1:
Pk+1=(I-Kk+1Hk+1)Pk+1/k。
本发明具有有益效果为:
1、本方法可基于臂环中的陀螺仪、加速度计、磁传感器进行9轴融合,输出稳定不发散的小臂姿态角信息,保证3自由度的姿态解算都精确。
2、采用本发明步骤2-6中关于互补卡尔曼滤波的状态设定以及初始值计算、***噪声阵、***噪声驱动阵、量测噪声阵、状态预测更新过程、量测更新过程、量测矩阵计算的详细过程和方法,输出的结果误差小、更能反应小臂的实际运动状况。
附图说明
图1为本发明的流程图;
图2为本发明中载体系xyz三轴示意图。
具体实施方式
结合附图,对本发明较佳实施例做进一步详细说明。
如图1所示,一种基于互补卡尔曼滤波器的臂环姿态角计算方法,包括以下步骤:
步骤1:搭建姿态角测量***,***上电启动,陀螺仪、加速度计、磁传感器开始读取数据;加速度计三轴数据为fb=[fx_b fy_b fz_b]T,陀螺仪三轴数据为Gb=[Gx_b Gy_bGz_b]T,磁传感器三轴数据为Mb=[Mx_b My_b Mz_b]T;b 表示载体坐标系,原点为臂环中心位置,XYZ三个方向分别指向当前传感器位置的右前上方向。
步骤2:开始时,手臂平放处于静止水平状态,载体系xyz三轴分别指向水平右前上,采集一段时间序列N长度的传感器数据,此处的N可以根据实际传感器采样频率取下式中的fb_avr=[fx_b_avr fy_b_avr fz_b_avr]T、 Gb=[Gx_b_avr Gy_b_avr Gz_b_avr]T、Mb=[Mx_b_avr My_b_avr GMz_b_avr]T均表示基于这段时间序列N的采样均值,状态X取=[θ γ ψ wx wy wz]T,分别表示小臂的俯仰角,横滚角,航向角,x轴角速率,y轴角速率,z轴角速率;初始值X0的具体计算公式如下:
其中g表示当地重力加速度,可取值为9.80665m/s2;D表示地球上当地的磁偏角,可根据资料查询。
步骤3:***噪声驱动阵Γ和***噪声矩阵Q为:
***噪声矩阵Q由陀螺仪的三轴输出噪声方差构成,T为卡尔曼的滤波周期;
量测噪声噪声阵R为:
量测噪声矩阵R由互补滤波输出的噪声方差构成,实际使用时可以实时根据互补滤波(量测信息)输出进行计算。
步骤4:状态量X的预测更新为:
状态X的一步预测协方差矩阵Pk+1/k为:
上式中Pk表示协方差估计矩阵,其初始值为状态的初始误差,可以根据器件的测量精度给定。
步骤5:量测Z更新,量测信息由互补滤波进行计算,将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到,量测矩阵H为:
上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数,这可以根据实际***进行调整,φxk+1、φyk+1、φzk+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角:
与步骤2中计算初始值时类似,不同之处在于,上式中 fb=[fx_b fy_b fz_b]T、Gb=[Gx_b Gy_b Gz_b]T、Mb=[Mx_b My_b Mz_b]T表示传感器k+1时刻的取值。
步骤6:滤波增益K计算;滤波增益K计算具体见下:
上式中的Hk+1表示量测矩阵,其为:
步骤7:状态X估计,具体见下式:
此时的估计结果:便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角,同时可以得到;状态X估计的协方差矩阵Pk+1:
Pk+1=(I-Kk+1Hk+1)Pk+1/k。
步骤8运动结束,反之若运动未结束,则循环执行步骤4~步骤7
步骤1-7及图1中的基于互补卡尔曼滤波器的数据融合方案,基于臂环中的陀螺仪、加速度计、磁传感器进行9轴融合,输出稳定不发散的小臂姿态角信息,保证3自由度的姿态解算都精确。而采用本发明步骤2-6中关于互补卡尔曼滤波的状态设定以及初始值计算、***噪声阵、***噪声驱动阵、量测噪声阵、状态预测更新过程、量测更新过程、量测矩阵计算的详细过程和方法,输出的结果误差小、更能反应小臂的实际运动状况。
上述实施例仅用于解释说明本发明的发明构思,而非对本发明权利保护的限定,凡利用此构思对本发明进行非实质性的改动,均应落入本发明的保护范围。
Claims (8)
1.一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,包括以下步骤:
步骤1:搭建姿态角测量***,***上电启动,陀螺仪、加速度计、磁传感器开始读取数据;
步骤2:开始时,手臂平放处于静止水平状态,载体系xyz三轴分别指向水平右前上,采集一段时间序列N长度的传感器数据,计算状态X初始值X0;
步骤3:***噪声阵、量测噪声噪初始化;
步骤4:状态量X的预测更新;
步骤5:量测Z更新;
步骤6:滤波增益K计算;
步骤7:状态X估计;
步骤8:运动结束,反之若运动未结束,则循环执行步骤4~步骤7。
2.根据权利要求1所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤1具体包括:
加速度计三轴数据为fb=[fx_b fy_b fz_b]T,陀螺仪三轴数据为
Gb=[Gx_b Gy_b Gz_b]T,磁传感器三轴数据为Mb=[Mx_b My_b Mz_b]T;b表示载体坐标系,原点为臂环中心位置,XYZ三个方向分别指向当前传感器位置的右前上方向。
3.根据权利要求1或2所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤2具体包括:
此处的N可以根据实际传感器采样频率取下式中的fb_avr=[fx_b_avr fy_b_avr fz_b_avr]T、Gb=[Gx_b_avr Gy_b_avr Gz_b_avr]T、Mb_avr=[Mx_b_avr My_b_avr Mzb_avr]T均表示基于这段时间序列N的采样均值,状态X取=[θ γ ψ wx wy wz]T,分别表示小臂的俯仰角,横滚角,航向角,x轴角速率,y轴角速率,z轴角速率;初始值X0的具体计算公式如下:
其中g表示当地重力加速度,可取值为9.80665m/s2;D表示地球上当地的磁偏角。
4.根据权利要求3所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤3具体包括:
***噪声驱动阵Γ和***噪声矩阵Q为:
***噪声矩阵Q由陀螺仪的三轴输出噪声方差构成,T为卡尔曼的滤波周期;
量测噪声噪声阵R为:
量测噪声矩阵R由互补滤波输出的噪声方差构成。
5.根据权利要求4所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤4具体包括:
状态量X的预测更新为:
状态X的一步预测协方差矩阵Pk+1/k为:
上式中Pk表示协方差估计矩阵,其初始值为状态的初始误差,可以根据器件的测量精度给定。
6.根据权利要求5所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤5具体包括:
量测信息由互补滤波进行计算,将陀螺仪积分得到的角度和加速度计、磁传感器计算的角度进行互补融合得到,量测矩阵H为:
上式中的α、β、ζ分别表示互补滤波中陀螺仪测量角度的权重的系数,这可以根据实际***进行调整,φxk+1、φyk+1、φzk+1分别表示当前k+1时刻采用加速度计、磁传感器计算的姿态角:
上式中fb=[fx_b fy_b fz_b]T、Gb=[Gx_b Gy_b Gz_b]T、Mb=[Mx_b My_b Mz_b]T表示传感器k+1时刻的取值。
7.根据权利要求6所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤6具体包括:
滤波增益K计算具体见下:
上式中的Hk+1表示量测矩阵,其为:
8.根据权利要求4-7任意一项所述的一种基于互补卡尔曼滤波器的臂环姿态角计算方法,其特征在于,步骤7具体包括:
状态X估计,具体见下式:
此时的估计结果:便是基于互补卡尔曼滤波器估计的当前时刻的臂环的三轴姿态角,同时可以得到;状态X估计的协方差矩阵Pk+1:
Pk+1=(I-Kk+1Hk+1)Pk+1/k。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910379845.9A CN110207647B (zh) | 2019-05-08 | 2019-05-08 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910379845.9A CN110207647B (zh) | 2019-05-08 | 2019-05-08 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110207647A true CN110207647A (zh) | 2019-09-06 |
CN110207647B CN110207647B (zh) | 2021-11-09 |
Family
ID=67786983
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910379845.9A Active CN110207647B (zh) | 2019-05-08 | 2019-05-08 | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110207647B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111060813A (zh) * | 2019-12-09 | 2020-04-24 | 国网北京市电力公司 | 高压断路器操作机构的故障诊断方法及装置、电子设备 |
CN111169201A (zh) * | 2020-03-04 | 2020-05-19 | 黑龙江大学 | 练字监测器及监测方法 |
CN114459302A (zh) * | 2022-03-10 | 2022-05-10 | 东南大学 | 一种适用于高旋弹丸的滚转角速率测量方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105651242A (zh) * | 2016-04-05 | 2016-06-08 | 清华大学深圳研究生院 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
CN108225308A (zh) * | 2017-11-23 | 2018-06-29 | 东南大学 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
-
2019
- 2019-05-08 CN CN201910379845.9A patent/CN110207647B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105651242A (zh) * | 2016-04-05 | 2016-06-08 | 清华大学深圳研究生院 | 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法 |
CN108225308A (zh) * | 2017-11-23 | 2018-06-29 | 东南大学 | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 |
Non-Patent Citations (2)
Title |
---|
张栋,焦嵩鸣,刘延泉: "互补滤波和卡尔曼滤波的融合姿态解算方法", 《传感器与微***》 * |
张欣: "多旋翼无人机的姿态与导航信息融合算法研究", 《中国博士学位论文全文数据库 工程科技Ⅱ辑》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111060813A (zh) * | 2019-12-09 | 2020-04-24 | 国网北京市电力公司 | 高压断路器操作机构的故障诊断方法及装置、电子设备 |
CN111169201A (zh) * | 2020-03-04 | 2020-05-19 | 黑龙江大学 | 练字监测器及监测方法 |
CN111169201B (zh) * | 2020-03-04 | 2024-03-26 | 黑龙江大学 | 练字监测器及监测方法 |
CN114459302A (zh) * | 2022-03-10 | 2022-05-10 | 东南大学 | 一种适用于高旋弹丸的滚转角速率测量方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110207647B (zh) | 2021-11-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
CN108458714B (zh) | 一种姿态检测***中不含重力加速度的欧拉角求解方法 | |
CN110207647A (zh) | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 | |
CN201561759U (zh) | 惯性姿态方位测量装置 | |
CN107490378B (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
CN105865453B (zh) | 一种位置传感器和姿态传感器的导航***及其融合方法 | |
CN103323625B (zh) | 一种mems-imu中加速度计动态环境下的误差标定补偿方法 | |
CN101290229A (zh) | 硅微航姿***惯性/地磁组合方法 | |
CN106153069B (zh) | 自主导航***中的姿态修正装置和方法 | |
WO2020164206A1 (zh) | 一种旋转加速度计重力梯度仪标定方法 | |
CN106500721B (zh) | 一种水下机器人双冗余姿态检测*** | |
CN103175502A (zh) | 一种基于数据手套低速运动的姿态角检测方法 | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
CN110007354A (zh) | 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法 | |
CN109540135A (zh) | 水田拖拉机位姿检测和偏航角提取的方法及装置 | |
CN106979779A (zh) | 一种无人车实时姿态测量方法 | |
CN112577518A (zh) | 一种惯性测量单元标定方法及装置 | |
CN108871319A (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
CN107576992B (zh) | 一种重力梯度仪自标定方法及离心梯度补偿方法 | |
CN105303201A (zh) | 一种基于动作感应进行手写识别的方法和*** | |
CN110514201B (zh) | 一种惯性导航***及适用于高转速旋转体的导航方法 | |
Li et al. | An efficient method for tri-axis magnetometer calibration | |
CN104596540B (zh) | 一种惯导/里程计组合导航的半实物仿真方法 | |
CN115523919A (zh) | 一种基于陀螺漂移优化的九轴姿态解算方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |