CN103499348B - Ahrs高精度姿态数据计算方法 - Google Patents
Ahrs高精度姿态数据计算方法 Download PDFInfo
- Publication number
- CN103499348B CN103499348B CN201310440327.6A CN201310440327A CN103499348B CN 103499348 B CN103499348 B CN 103499348B CN 201310440327 A CN201310440327 A CN 201310440327A CN 103499348 B CN103499348 B CN 103499348B
- Authority
- CN
- China
- Prior art keywords
- coordinate
- conversion matrix
- moment
- formula
- sin
- 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.)
- Expired - Fee Related
Links
Classifications
-
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种AHRS高精度姿态数据计算方法,主要解决了现有技术中存在的由于加速度计和陀螺仪的精度有限,且存在累积误差,因而所得到的姿态数据误差较大,不能满足技术需求的问题。其包括初始化传感器和归一化坐标系转换矩阵;计算载体的姿态数据和此时的坐标系转换矩阵R1;计算该时刻的坐标系转换矩阵R2;确定该时刻最终的坐标系转换矩阵,并对最终的坐标系转换矩阵进行归一化,得到该时刻的归一化坐标系转换矩阵;根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据;返回步骤(2),进行下一时刻的姿态数据计算。通过上述方案,本发明达到了便于实现、精度较高的目的,具有很高的实用价值和推广价值。
Description
技术领域
本发明涉及一种导航领域的姿态数据计算方法,具体地说,是涉及一种AHRS高精度姿态数据计算方法。
背景技术
众所周知,惯性导航主要涉及两方面的内容:定位和定姿,定位可通过全球卫星定位***(如GPS、北斗等)完成;定姿主要将加速度计输出的初始角度信息和陀螺仪输出的三轴角速度信息通过积分计算得到当前的姿态数据(主要包括方向角、俯仰角和翻滚角)。由于加速度计和陀螺仪的精度有限,且存在累积误差,因而所得到的姿态数据误差较大,不能满足技术需求。
发明内容
本发明的目的在于提供一种AHRS高精度姿态数据计算方法,主要解决现有技术中存在的由于加速度计和陀螺仪的精度有限,且存在累积误差,因而所得到的姿态数据误差较大,不能满足技术需求问题。
为了实现上述目的,本发明采用的技术方案如下:
AHRS高精度姿态数据计算方法,包括以下步骤:
(1)将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理;
(2)读取AHRS中加速度计和磁力计的数据,计算载体的姿态数据和此时的坐标系转换矩阵R1;
(3)读取AHRS中陀螺仪的数据,根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R2;
(4)根据坐标系转换矩阵R1和坐标系转换矩阵R2确定该时刻最终的坐标系转换矩阵,并对最终的坐标系转换矩阵进行归一化,得到该时刻的归一化坐标系转换矩阵;
(5)根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据;
(6)返回步骤(2),进行下一时刻的姿态数据计算。
所述步骤(1)具体包括:
(1a)配置传感器的各项参数;
(1b)根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵Rnormalized。
所述步骤(1b)包括以下步骤:
(1b1)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z;
(1b2)根据公式计算俯仰角θ;根据公式计算翻滚角φ;根据公式
相应地,所述归一化坐标系转换矩阵Rnormalized通过公式
所述步骤(2)具体包括以下步骤:
(2a)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z;
(2b)根据公式计算当前时刻的俯仰角θ;根据公式计算当前时刻的翻滚角φ;根据公式
根据公式 得出该时刻的坐标系转换矩阵R1,其中,θ为当前时刻的俯仰角;φ为当前时刻的翻滚角;ψ为当前时刻的方向角。
所述步骤(3)具体包括以下步骤:
(3a)从陀螺仪上读取载体坐标系在X轴向的角速度ωx、在Y轴向的角速度ωy、在Z轴向的角速度ωz;
(3b)根据公式 和公式 得出该时刻的坐标系转换矩阵R2。
所述步骤(4)具体包括以下步骤:
(4a)根据计算出的坐标系转换矩阵R1和坐标系转换矩阵R2计算该时刻最终的坐标系转换矩阵R=R1+λ(R2-R1),其中,λ为坐标系转换矩阵校正因子,且0≤λ≤1;
(4b)根据以下公式:
所述步骤(5)中该时刻最终的姿态数据通过该时刻的最终俯仰角θ、翻滚角φ和方向角ψ确定,该时刻的最终俯仰角θ、翻滚角φ和方向角ψ根据公式
以及步骤(4b)计算出的归一化坐标系转换矩Rnormalized阵求得。
与现有技术相比,本发明具有以下有益效果:
(1)本发明以加速度计、磁力计和陀螺仪获取的数据源为基础,分别独立计算载体的坐标系转移矩阵,并以加速度计和磁力计计算出的坐标系转移矩阵为基础,通过陀螺仪计算的坐标系转移矩阵对其进行纠偏,以提升坐标转移矩阵的精度,进而大大提升了最终姿态数据的精度,且只使用由陀螺仪计算的坐标系转移矩阵进行辅助纠偏能够有效降低陀螺仪累积误差等带来的影响,更有助于最终姿态数据精度的提升,设计十分巧妙,符合技术需求。
(2)本发明所实现的新型AHRS高精度姿态数据计算方法能够使惯性导航的定姿精度得到明显的提升,达到更佳的惯性导航效果,对将惯性导航应用到更多需要精确姿态数据的领域中作出了重大贡献,具有突出的实质性特点和显著进步,适合大规模推广应用。
附图说明
图1为本发明的实施例中地面坐标系与载体坐标系角度示意图。
图2为本发明的流程示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步说明,本发明的实施方式包括但不限于下列实施例。
实施例
为了解决现有技术中存在的由于加速度计和陀螺仪的精度有限,且存在累积误差,因而所得到的姿态数据误差较大,不能满足技术需求问题,如图1所示,本发明中采用两个不同的坐标系:地面坐标系(也称东北天坐标系)和载体坐标系。
地面坐标系是指在地球表面距离较近的范围内(如几十公里内),可将大地水平面近似视为一个平面,从而可以以正东方向为x正轴,正北方向为y正半轴,垂直于地面向上为z正半轴,构成地面坐标系,也就是所谓的东北天坐标系;
载体坐标系是基于载体建立的一种符合右手定则的坐标系,载体坐标系以载体质心为原点,x轴与载体纵轴重合,指向载体头部为正;y轴与载体横轴重合,指向载体右侧为正;z轴垂直于载体平面,指向载体腹部为正。
当载体姿态发生变化时,载体坐标系将相对于地面坐标系发生旋转。如图1所示,(xe,ye,ze)表示地面坐标系,(xb,yb,zb)表示载体坐标系,在图1中,地面坐标系在xeOye平面内旋转ψ度,在xbOze平面内旋转θ度,在ybOzb平面内旋转φ度后,即可得到图1中的载体坐标系,此时,称ψ为方向角,θ为俯仰角,φ为翻滚角。本发明使用这3个角度(方向角、俯仰角和翻滚角)来度量载体的当前姿态,并以此作为姿态数据输出。
设QB=[QBxQByQBz]T,QE=[QExQEyQEz]T分别表示载体坐标系和地面坐标系下的坐标向量,则有QB=RQE。其中R称为坐标系转换矩阵,且满足(式1): (式1)
本发明中,[·]T表示矩阵的转置;rxx为矩阵RT第1行第1列的元素,rxy为矩阵RT第2行第1列的元素,以此类推,rzz为矩阵RT第3行第3列的元素;X=[rxxrxyrxz]T,Y=[ryxryyryz]T,Z=[rzxrzyrzz]T;Xorth,Yorth,Zorth分别是X,Y,Z对应的正交向量;Xnormalized,Ynormalized,Znormalized分别是X,Y,Z对应的归一化向量;X·Y表示向量X与向量Y的点乘,X×Y表示向量X与向量Y的叉乘。
如图2所示,本发明公开了一种新型AHRS高精度姿态数据计算方法,整体方案如下:
步骤一、通过配置传感器各项参数,并根据此时加速度计和磁力计的输出数据,初始化归一化坐标系转换矩阵Rnormalized实现传感器及归一化坐标系转换矩阵的初始化;
其中,归一化坐标系转换矩阵Rnormalized初始化通过从加速度计读取载体坐标系x轴、y轴和z轴3个方向的加速度accel_x、accel_y和accel_z,从磁力计读取载体坐标系x轴、y轴和z轴三个方向的磁场强度magnetom_x、magnetom_y和magnetom_z,并根据(式2)、(式3)和(式4)计算俯仰角θ、翻滚角φ和方向角ψ,之后将计算出的俯仰角θ、翻滚角φ和方向角ψ代入(式1)完成。(式2)、(式3)和(式4)如下所示:
步骤二、从加速度计读取载体坐标系x轴、y轴和z轴3个方向的加速度accel_x、accel_y和accel_z;从磁力计读取载体坐标系x轴、y轴和z轴3个方向的磁场强度magnetom_x、magnetom_y和magnetom_z;
根据(式2)、(式3)和(式4)初步计算当前时刻的俯仰角θ、翻滚角φ和方向角ψ。并将计算出的俯仰角θ、翻滚角φ和方向角ψ代入(式1),得到该时刻的坐标系转换矩阵R1。
步骤三、从陀螺仪读取载体坐标系x轴、y轴和z轴3个方向的角速度ωx、ωy和ωz;根据(式5)和(式6)得到该时刻的坐标系转换矩阵R2,(式5)和(式6)如下所示:
步骤四、根据计算得出的坐标系转换矩阵R1和坐标系转换矩阵R2,和(式7)计算该时刻最终的坐标系转换矩阵R,(式7)如下所示:
R=R1+λ(R2-R1)(式7)
其中,λ为坐标系转换矩阵校正因子,且0≤λ≤1。
根据(式8)~(式16)对该时刻最终的坐标系转换矩阵R进行归一化,得到该时刻归一化坐标系转换矩阵Rnormalized,(式8)~(式16)如下所示:
Rnormalized=[XnormalizedYnormalizedZnormalized](式13)
步骤五、根据(式1)对坐标系转换矩阵的定义,及计算出的归一化坐标系转换矩阵Rnormalized计算出该时刻的最终俯仰角θ、翻滚角φ和方向角ψ。
按照上述实施例,便可很好地实现本发明。
Claims (1)
1.AHRS高精度姿态数据计算方法,其特征在于,包括以下步骤:
(1)将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理;
(2)读取AHRS中加速度计和磁力计的数据,计算载体的姿态数据和此时的坐标系转换矩阵R1;
(3)读取AHRS中陀螺仪的数据,根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R2;
(4)根据坐标系转换矩阵R1和坐标系转换矩阵R2确定该时刻最终的坐标系转换矩阵,并对最终的坐标系转换矩阵进行归一化,得到该时刻的归一化坐标系转换矩阵;
(5)根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据;
(6)返回步骤(2),进行下一时刻的姿态数据计算;
所述步骤(1)具体包括:
(1a)配置传感器的各项参数;
(1b)根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵Rnormalized;
所述步骤(1b)包括以下步骤:
(1b1)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z;
(1b2)根据公式计算俯仰角θ;根据公式y计算翻滚角φ;根据公式
mag_x=magnetom_x*cos(θ)+magnetom_y*sin(φ)*sin(θ)
+magnetom_z*cos(φ)*sin(θ)
计算方向角ψ;
相应地,所述归一化坐标系转换矩阵Rnormalized通过公式
完成初始化;
所述步骤(2)具体包括以下步骤:
(2a)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z;
(2b)根据公式计算当前时刻的俯仰角θ;根据公式计算当前时刻的翻滚角φ;根据公式
mag_x=magnetom_x*cos(θ)+magnetom_y*sin(φ)*sin(θ)
+magnetom_z*cos(φ)*sin(θ)
计算当前时刻的方向角ψ;
根据公式得出该时刻的坐标系转换矩阵R1,其中,θ为当前时刻的俯仰角;φ为当前时刻的翻滚角;ψ为当前时刻的方向角;
所述步骤(3)具体包括以下步骤:
(3a)从陀螺仪上读取载体坐标系在X轴向的角速度ωx、在Y轴向的角速度ωy、在Z轴向的角速度ωz;
(3b)根据公式和公式得出该时刻的坐标系转换矩阵R2;
所述步骤(4)具体包括以下步骤:
(4a)根据计算出的坐标系转换矩阵R1和坐标系转换矩阵R2计算该时刻最终的坐标系转换矩阵R=R1+λ(R2-R1),其中,λ为坐标系转换矩阵校正因子,且0≤λ≤1;
(4b)根据以下公式:
对该时刻最终的坐标系转换矩阵R进行归一化,得出该时刻归一化坐标系转换矩阵Rnormalized;
所述步骤(5)中该时刻最终的姿态数据通过该时刻的最终俯仰角θ、翻滚角φ和方向角ψ确定,该时刻的最终俯仰角θ、翻滚角φ和方向角ψ根据公式
以及步骤(4b)计算出的归一化坐标系转换矩Rnormalized阵求得。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310440327.6A CN103499348B (zh) | 2013-09-24 | 2013-09-24 | Ahrs高精度姿态数据计算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310440327.6A CN103499348B (zh) | 2013-09-24 | 2013-09-24 | Ahrs高精度姿态数据计算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103499348A CN103499348A (zh) | 2014-01-08 |
CN103499348B true CN103499348B (zh) | 2016-03-30 |
Family
ID=49864579
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310440327.6A Expired - Fee Related CN103499348B (zh) | 2013-09-24 | 2013-09-24 | Ahrs高精度姿态数据计算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103499348B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104881192B (zh) * | 2015-05-28 | 2018-11-16 | 努比亚技术有限公司 | 操作识别方法和装置以及终端 |
CN104864865B (zh) * | 2015-06-01 | 2017-09-22 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
CN104897157B (zh) * | 2015-06-25 | 2017-09-22 | 济南大学 | 基于足部航姿参考和肩部电子罗盘的个人导航***及方法 |
CN105115507B (zh) * | 2015-08-10 | 2018-01-02 | 济南大学 | 一种基于双imu的双模式室内个人导航***及方法 |
CN106021167B (zh) * | 2016-03-31 | 2018-12-07 | 常州大学 | 一种基于Android平台的单元刚度矩阵计算*** |
CN108571980A (zh) * | 2017-03-07 | 2018-09-25 | 深圳市博安智控科技有限公司 | 一种捷联惯性导航的误差校正方法及装置 |
CN107907129B (zh) * | 2017-09-26 | 2021-11-09 | 广州新维感信息技术有限公司 | Vr手柄姿态初始化方法、vr手柄及存储介质 |
EP3671119B1 (en) * | 2017-09-30 | 2024-04-10 | Huawei Technologies Co., Ltd. | Attitude matrix calculating method and device |
CN108287524B (zh) * | 2018-01-11 | 2021-02-19 | 中国计量大学 | 一种基于mpu6050的平面分区间定位装置的定位方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102654404A (zh) * | 2011-03-02 | 2012-09-05 | 浙江中科无线授时与定位研发中心 | 一种提高航姿参考***解算精度和***抗干扰能力的方法 |
CN102692225A (zh) * | 2011-03-24 | 2012-09-26 | 北京理工大学 | 一种用于低成本小型无人机的姿态航向参考*** |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102997913B (zh) * | 2011-09-14 | 2015-11-25 | 意法半导体(中国)投资有限公司 | 用于确定物体姿态的方法及装置 |
-
2013
- 2013-09-24 CN CN201310440327.6A patent/CN103499348B/zh not_active Expired - Fee Related
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102654404A (zh) * | 2011-03-02 | 2012-09-05 | 浙江中科无线授时与定位研发中心 | 一种提高航姿参考***解算精度和***抗干扰能力的方法 |
CN102692225A (zh) * | 2011-03-24 | 2012-09-26 | 北京理工大学 | 一种用于低成本小型无人机的姿态航向参考*** |
Also Published As
Publication number | Publication date |
---|---|
CN103499348A (zh) | 2014-01-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103499348B (zh) | Ahrs高精度姿态数据计算方法 | |
EP3364153B1 (en) | Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor | |
CN104567930A (zh) | 一种能够估计和补偿机翼挠曲变形的传递对准方法 | |
CN102486377B (zh) | 一种光纤陀螺捷联惯导***初始航向的姿态获取方法 | |
CN103363992B (zh) | 基于梯度下降的四旋翼无人机姿态航向参考***解算方法 | |
CN103630137A (zh) | 一种用于导航***的姿态及航向角的校正方法 | |
CN104880192B (zh) | 一种基于偏振罗盘的载体航向角计算方法 | |
CN103630139B (zh) | 一种基于地磁梯度张量测量的水下载体全姿态确定方法 | |
CN106507913B (zh) | 用于管道测绘的组合定位方法 | |
CN105783919A (zh) | H型标量传感器阵列对磁性目标的追踪定位方法 | |
CN103940446B (zh) | 一种基于横坐标系的极区航行船舶捷联惯性导航***重调方法 | |
CN101915579A (zh) | 一种基于ckf的sins大失准角初始对准新方法 | |
CN104374388A (zh) | 一种基于偏振光传感器的航姿测定方法 | |
CN103217174B (zh) | 一种基于低精度微机电***的捷联惯导***初始对准方法 | |
CN106052716A (zh) | 惯性系下基于星光信息辅助的陀螺误差在线标定方法 | |
CN104655135A (zh) | 一种基于地标识别的飞行器视觉导航方法 | |
CN103900566B (zh) | 一种消除地球自转角速度对旋转调制型捷联惯导***精度影响的方法 | |
CN103900608A (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN102679978A (zh) | 一种旋转式捷联惯性导航***静基座初始对准方法 | |
CN106840154A (zh) | 地下空间惯性测量与无线传感器组合定位***与方法 | |
CN104165642A (zh) | 一种用于导航***航向角直接校正补偿方法 | |
CN104166169A (zh) | 一种捷联于水下潜器的三轴磁场传感器误差补偿方法 | |
CN103954303A (zh) | 一种用于磁力计导航***航向角动态计算及校正方法 | |
CN102435193B (zh) | 一种捷联惯性导航***的高精度初始对准方法 | |
CN103279642A (zh) | 无地面控制点的目标定位精度分析方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160330 Termination date: 20160924 |
|
CF01 | Termination of patent right due to non-payment of annual fee |