CN114910045B - 一种基于六轴获取偏航角的方法 - Google Patents
一种基于六轴获取偏航角的方法 Download PDFInfo
- Publication number
- CN114910045B CN114910045B CN202111506996.XA CN202111506996A CN114910045B CN 114910045 B CN114910045 B CN 114910045B CN 202111506996 A CN202111506996 A CN 202111506996A CN 114910045 B CN114910045 B CN 114910045B
- Authority
- CN
- China
- Prior art keywords
- yaw angle
- axis
- angle
- rough
- accelerometer
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000007500 overflow downdraw method Methods 0.000 claims abstract description 6
- 230000001133 acceleration Effects 0.000 claims description 26
- 238000005070 sampling Methods 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 11
- 230000005484 gravity Effects 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 7
- 230000004927 fusion Effects 0.000 description 4
- 230000015572 biosynthetic process Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 238000005259 measurement Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 239000000126 substance Substances 0.000 description 3
- 238000003786 synthesis reaction Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Gyroscopes (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本发明公开了一种基于六轴获取偏航角的方法,一种基于六轴获取偏航角的方法,包括以下步骤:S1,通过加速度计推算出粗略的偏航角;S2,通过陀螺仪获得角速度;S3,将粗略的偏航角和角速度通过六轴融合方法进行运算得到最终的偏航角。本发明的方法首先通过加速度计粗略推算得到偏航角,再通过陀螺仪得到角速度,最后将两个数值通过六轴融合方法准确地获得偏航角,并且可以消除温漂和零漂产生的影响,抗干扰性强。
Description
技术领域
本发明涉及门锁设备技术领域,尤其是一种基于六轴获取偏航角的方法。
背景技术
三维空间中经常用到俯仰角(pitch)、偏航角(yaw)和翻滚角(roll),俯仰角是围绕x轴旋转,偏航角是围绕y轴旋转,翻滚角是围绕z轴旋转。其中俯仰角和翻滚角通常采用陀螺仪+加速度计的方法来测量,通过陀螺仪积分得到的角度和加速度计与重力加速度的夹角转换得到的角度进行整合计算出实际的俯仰角和翻滚角。由于偏航角所在平面一直与重力加速度方向垂直,导致加速度计无法通过重力加速度转换成方位角移动相对角度。因此上述的陀螺仪+加速度计测量方法不能用来测量偏航角。测量偏航角目前常用的方法有两种:
1、陀螺仪检测;陀螺仪可以测量当前设备在相对惯性空间转角或角速度。通过陀螺仪积分得到角度,推算出偏航角;
2、磁力计检测;磁力计可以测量出当前设备与东南西北四个方向上的夹角。通过磁力计可以推算出偏航角。
只有陀螺仪单独测量角度时,由于没有加速度计进行补偿,陀螺仪采样角速度数据时会受到外部作用力的影响。但陀螺仪测量精度的问题,推算值会发生偏移。而且陀螺仪存在温漂和零漂的问题,会导致角度失真。使用磁力计测量角度时,虽然能较为准确地推算出偏航角。但是磁力计受外界磁性物质干扰较大,当磁性物质的磁场较大时,可能会出现误判的情况。并且磁力计也会存在零漂的问题,导致角度失真。
在中国专利文献上公开的“一种基于毫米波雷达的车辆偏航角计算方法”,其公开号为CN108909721B,该发明涉及一种基于毫米波雷达的车辆偏航角计算方法,通过采用毫米波雷达来检测高速公路旁的静止物体,如护栏、树木,并进行直线、曲线拟合获得道路边界,利用该边界来代替车道的几何特性;采用长距毫米波雷达获得前方道路边界来预估道路为直道或弯道,采用中距毫米波雷达拟合两侧道路边界,进而计算车辆偏航角。相比于基于机器视觉的车辆偏航角计算,本方法对于天气、光照、阴影、摄像头抖动及交通标志线等因素的抗干扰能力更强,有助于车道偏离预警系统的准确决策。但是该发明需要毫米波雷达的检测,计算过程繁琐。
发明内容
本发明解决了现有技术中使用磁力计测量角度时由于受外界磁性物质干扰较大导致出现误判的问题,提出一种基于六轴获取偏航角的方法,首先通过加速度计粗略推算得到偏航角,再通过陀螺仪得到角速度,最后将两个数值通过六轴融合方法准确地获得偏航角,并且可以消除温漂和零漂产生的影响,抗干扰性强。
为了实现上述目的,本发明采用以下技术方案:一种基于六轴获取偏航角的方法,包括以下步骤:
S1,通过加速度计推算出粗略的偏航角;
S2,通过陀螺仪获得角速度;
S3,将粗略的偏航角和角速度通过六轴融合方法进行运算得到最终的偏航角。
本发明中,由于在非匀加速转动改变偏航角时,偏航角所在平面与重力方向垂直,所以加速度计无法通过重力获得偏航角,因此传统的用卡尔曼滤波将陀螺仪获得的角度作为系统的估计值,和将加速度计获得的角度作为状态的测量值进行融合的方法在此情况下不适用,虽然加速度计无法通过重力获得偏航角,但是在外力作用下可以获得偏航角所在水平面的瞬间加速度。本发明的步骤S1正是依据上述原理推算出粗略的偏航角,又用传统的陀螺仪获得角速度,最后用卡尔曼滤波进行融合,能准确的测量偏航角,并且抗干扰强。
作为优选,所述步骤S1具体包括以下步骤:
S11,用近似求解的方法求出作用力F1;
S12,经过采样时间t后,求出作用力F2,求解方法与求解作用力F1的方法相同;
S13,求出a1和a2并作差得到粗略的偏航角。
本发明中,加速度计获得的瞬间加速度在采样时间一定的情况下,采样时间可以作为常量不计,瞬间加速度的大小能间接反应出偏航角的大小;综合牛顿第二定律和力的合成原则,计算出总的作用力大小,最后转化为加速度进行作差,计算方便。
作为优选,所述近似求解的方法具体为由于加速度计只能测量x轴、y轴或者z轴上的加速度,作用力F1施加到加速度计上时,测量得到三个x轴、y轴和z轴上的分加速度分别为ax,ay和az,故可得在x轴、y轴和z轴上三个分力分别是Fx、Fy和Fz,根据力的合成,计算出:
式中,F1”为三个分力的合力;用合力F1”来代替并求出作用力F1。
本发明中,求解粗略偏航角的过程忽略外界因素干扰,将三个分加速度转化成作用力后进行合成,无需传感器即可求出作用力。
作为优选,所述步骤S13具体为利用质量m一定,根据作用力F1和F2求出合成加速度a1和a2,两者作差的值即为粗略的偏航角。
本发明中,a1和a2的差近似等同于偏航角的前提是采样时间t非常小的时候。
作为优选,所述步骤S2具体还包括通过陀螺仪的角速度积分推算出角度。
本发明中,陀螺仪能测量当前设备在相对惯性空间的角速度,并通过积分计算出角度。
作为优选,所述步骤S3具体包括通过加速度计采样时间内获取的粗略偏航角作为对系统的估计值,将角速度积分得到的角度值作为状态的测量值,通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。
本发明中,平常实用的卡尔曼滤波融合获得俯仰角和翻滚角时,是将角速度采样时间内得到的角度值Δθ=ωΔt作为对系统的估计值,将基于加速度计获得的加速度a作为状态的测量值,然后进行融合得到最终的角度,而本发明则不同,本发明的方法能有效融合得到准确的偏航角,另外采样时间内的偏航角是通过作差得到的,若当前采样出现干扰会在下一次采样时作差抵消,这样的话可以去除温漂和零漂带来的影响,确保使用的过程中不会对偏航角带来误差。
本发明的有益效果是:
1、本发明直接通过六轴获取偏航角,不需要额外增加传感器获取偏航角;
2、本发明中的六轴融合方法抗干扰性强,可以有效的去除温漂和零漂;
3、本发明不受硬件的限制,只要是带有相关功能的硬件都可以使用,替换性强。
附图说明
图1是本发明的流程图;
图2是本发明求解F1过程中的受力分析图。
具体实施方式
实施例:
本实施例提出一种基于六轴获取偏航角的方法,参考图1,首先,由加速度计算出粗略的偏航角;其次通过陀螺仪获取角速度并计算出角度,具体通过角速度积分的方法计算出;最后将偏航角和角速度这两个数据通过六轴融合方法准确获取偏航角。
具体的,参考图2,粗略计算偏航角大致的过程为:首先,计算出作用力F1,由于加速度计直接测量得到的数据为沿x轴、y轴或者z轴上的加速度,故使用近似求解的方法,当作用力F1作用到加速度计上的时候,可检测得到三个轴向方向的分加速度,分别为ax,ay和az,由牛顿第二定律可求出三个轴向方向对应的分作用力,分别为Fx、Fy和Fz,最后对力进行合成,先算出F':
其中,F1'为x轴和y轴方向的合力,之后又和z轴方向的力进行合成,求得:
其中,F1”为三个分力的合力,综合可得:
虽然有外界因素存在,但合力F1”和作用力F1还是近乎相等的。
经过采样时间t后,求出作用力F2,求解过程完全与作用力F1的求解过程相同。最后,由F1和F2再次求出对应的合成加速度,分别为a1和a2,a2-a1得到的值就可以直接作为每次采样后偏航角的变化值,即为粗略偏航角。
最后,通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合。具体的,本方法通过加速度计采样时间内获取的粗略偏航角即a2-a1作为对系统的估计值,将角速度积分得到的角度值作为状态的测量值,角度值为
θ(k)=θ(k-1)+ωΔt
其中θ(k)为当前累加角度值,θ(k-1)为上次累加角度值;最后通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。
本发明中,由于在非匀加速转动改变偏航角时,偏航角所在平面与重力方向垂直,所以加速度计无法通过重力获得偏航角,因此传统的用卡尔曼滤波将陀螺仪获得的角度作为系统的估计值,和将加速度计获得的角度作为状态的测量值进行融合的方法在此情况下不适用,虽然加速度计无法通过重力获得偏航角,但是在外力作用下可以获得偏航角所在水平面的瞬间加速度。本发明的步骤S1正是依据上述原理推算出粗略的偏航角,又用传统的陀螺仪获得角速度,最后用卡尔曼滤波进行融合,能准确的测量偏航角,并且抗干扰强。
本发明中,加速度计获得的瞬间加速度在采样时间一定的情况下,采样时间可以作为常量不计,瞬间加速度的大小能间接反应出偏航角的大小;综合牛顿第二定律和力的合成原则,计算出总的作用力大小,最后转化为加速度进行作差,计算方便。
本发明中,平常实用的卡尔曼滤波融合获得俯仰角和翻滚角时,是将角速度采样时间内得到的角度值Δθ=ωΔt作为对系统的估计值,将基于加速度计获得的加速度a作为状态的测量值,然后进行融合得到最终的角度,而本发明则不同,本发明的方法能有效融合得到准确的偏航角,另外采样时间内的偏航角是通过作差得到的,若当前采样出现干扰会在下一次采样时作差抵消,这样的话可以去除温漂和零漂带来的影响,确保使用的过程中不会对偏航角带来误差。
本发明的卡尔曼滤波方法具体如下:
X(k|k-1)=A X(k-1|k-1)+B U(k)
其中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,A和B是系统参数;
P(k|k-1)=A P(k-1|k-1)A’+Q
其中,P(k|k-1)是X(k|k-1)对应的协方差,P(k-1|k-1)是X(k-1|k-1)对应的协方差,A’表示A的转置矩阵,Q是系统过程的协方差;
X(k|k)=X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))
其中,Kg(k)为卡尔曼增益,Z(k)是k时刻的测量值,H是测量系统的参数;
Kg(k)=P(k|k-1)H’/(H P(k|k-1)H’+R)
其中,R是系统过程的协方差;
P(k|k)=(I-Kg(k)H)P(k|k-1)
其中,I为1的矩阵,对于单模型单测量,I=1;当系统进入k+1状态时,P(k|k)就是本段第二个式子中的P(k-1|k-1)。
上述实施例是对本发明的进一步阐述和说明,以便于理解,并不是对本发明的任何限制,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (2)
1.一种基于六轴获取偏航角的方法,其特征在于,包括以下步骤:
S1,通过加速度计推算出粗略的偏航角;包括:
S11,用近似求解的方法求出作用力F1;近似求解的方法为:加速度计只能测量x轴、y轴或者z轴上的加速度,作用力F1施加到加速度计上时,测量得到三个x轴、y轴和z轴上的分加速度分别为ax,ay和az,可得在x轴、y轴和z轴上三个分力分别是Fx、Fy和Fz,根据力的合成,计算出:
式中,F1”为三个分力的合力;用合力F1”来代替并求出作用力F1;
S12,经过采样时间t后,求出作用力F2,求解方法与求解作用力F1的方法相同;
S13,求出a1和a2并作差得到粗略的偏航角;具体为:利用质量m一定,根据作用力F1和F2求出合成加速度a1和a2,两者作差的值即为粗略的偏航角;
S2,通过陀螺仪获得角速度;
S3,将粗略的偏航角和角速度通过六轴融合方法进行运算得到最终的偏航角;S3包括:
通过加速度计采样时间内获取的粗略偏航角作为对系统的估计值,将角速度积分得到的角度值作为状态的测量值,通过卡尔曼滤波将加速度计推算出的偏航角和陀螺仪得到的角度进行融合得到最终的偏航角。
2.根据权利要求1所述的一种基于六轴获取偏航角的方法,其特征在于,所述步骤S2具体还包括通过陀螺仪的角速度积分推算出角度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111506996.XA CN114910045B (zh) | 2021-12-10 | 2021-12-10 | 一种基于六轴获取偏航角的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111506996.XA CN114910045B (zh) | 2021-12-10 | 2021-12-10 | 一种基于六轴获取偏航角的方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114910045A CN114910045A (zh) | 2022-08-16 |
CN114910045B true CN114910045B (zh) | 2024-04-19 |
Family
ID=82763033
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111506996.XA Active CN114910045B (zh) | 2021-12-10 | 2021-12-10 | 一种基于六轴获取偏航角的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114910045B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101603825A (zh) * | 2008-06-11 | 2009-12-16 | 特林布尔导航有限公司 | 推算高度计和方法 |
KR101250215B1 (ko) * | 2012-05-31 | 2013-04-03 | 삼성탈레스 주식회사 | 칼만 필터와 보행 상태 추정 알고리즘을 이용한 보행자 관성항법 시스템 및 보행자 관성항법 시스템의 높이 추정 방법 |
WO2017063387A1 (zh) * | 2015-10-13 | 2017-04-20 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN108762528A (zh) * | 2018-08-15 | 2018-11-06 | 苏州大学 | 适用于空中飞鼠的姿态解算方法 |
CN109443349A (zh) * | 2018-11-14 | 2019-03-08 | 广州中海达定位技术有限公司 | 一种姿态航向测量系统及其融合方法、存储介质 |
CN109540135A (zh) * | 2018-11-09 | 2019-03-29 | 华南农业大学 | 水田拖拉机位姿检测和偏航角提取的方法及装置 |
CN113674327A (zh) * | 2021-07-01 | 2021-11-19 | 北京航空航天大学 | 一种适用于无人机空中防撞的入侵机航迹融合跟踪方法 |
-
2021
- 2021-12-10 CN CN202111506996.XA patent/CN114910045B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101603825A (zh) * | 2008-06-11 | 2009-12-16 | 特林布尔导航有限公司 | 推算高度计和方法 |
KR101250215B1 (ko) * | 2012-05-31 | 2013-04-03 | 삼성탈레스 주식회사 | 칼만 필터와 보행 상태 추정 알고리즘을 이용한 보행자 관성항법 시스템 및 보행자 관성항법 시스템의 높이 추정 방법 |
WO2017063387A1 (zh) * | 2015-10-13 | 2017-04-20 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN108762528A (zh) * | 2018-08-15 | 2018-11-06 | 苏州大学 | 适用于空中飞鼠的姿态解算方法 |
CN109540135A (zh) * | 2018-11-09 | 2019-03-29 | 华南农业大学 | 水田拖拉机位姿检测和偏航角提取的方法及装置 |
CN109443349A (zh) * | 2018-11-14 | 2019-03-08 | 广州中海达定位技术有限公司 | 一种姿态航向测量系统及其融合方法、存储介质 |
CN113674327A (zh) * | 2021-07-01 | 2021-11-19 | 北京航空航天大学 | 一种适用于无人机空中防撞的入侵机航迹融合跟踪方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114910045A (zh) | 2022-08-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111595592B (zh) | 一种自适应巡航控制系统性能测评方法 | |
EP3364155A1 (en) | A method for initial alignment of an inertial navigation apparatus | |
KR100898169B1 (ko) | 관성항법시스템의 초기정렬 방법 | |
CN105698765A (zh) | 双imu单目视觉组合测量非惯性系下目标物位姿方法 | |
CN104880189B (zh) | 一种动中通天线低成本跟踪抗干扰方法 | |
CN109764870B (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
CN110702113B (zh) | 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法 | |
CN105300381A (zh) | 一种基于改进互补滤波的自平衡移动机器人姿态快速收敛方法 | |
CN110631579A (zh) | 一种用于农业机械导航的组合定位方法 | |
CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 | |
CN112567097A (zh) | 用于确定机器的作业设备的角度的方法 | |
CN111504314B (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
CN110672103B (zh) | 一种多传感器目标跟踪滤波方法及系统 | |
CN110487273B (zh) | 一种水平仪辅助的室内行人轨迹推算方法 | |
CN110007318B (zh) | 风场干扰下基于卡尔曼滤波的单无人机判断gps欺骗的方法 | |
CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN114910045B (zh) | 一种基于六轴获取偏航角的方法 | |
CN109945785A (zh) | 一种平台倾角和高度实时测量方法及系统 | |
CN111811421B (zh) | 一种高速实时形变监测方法及系统 | |
CN113048987A (zh) | 一种车载导航系统定位方法 | |
CN114674345B (zh) | 一种惯导/相机/激光测速仪在线联合标定方法 | |
CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
CN104655132A (zh) | 一种基于加速度计的机体弹性变形角估计方法 | |
Carratù et al. | Self-alignment procedure for IMU in automotive context | |
Le Sage et al. | Kalman filter design for application to an INS analysing swimmer performance |
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 |