Ahrs高精度姿态数据计算方法
【专利摘要】本发明公开了一种AHRS高精度姿态数据计算方法，主要解决了现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求的问题。其包括初始化传感器和归一化坐标系转换矩阵；计算载体的姿态数据和此时的坐标系转换矩阵R1；计算该时刻的坐标系转换矩阵R2；确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；根据步骤（4）中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；返回步骤（2），进行下一时刻的姿态数据计算。通过上述方案，本发明达到了便于实现、精度较高的目的，具有很高的实用价值和推广价值。
【专利说明】AHRS高精度姿态数据计算方法
【技术领域】
[0001]本发明涉及一种导航领域的姿态数据计算方法，具体地说，是涉及一种AHRS高精度姿态数据计算方法。
【背景技术】
[0002]众所周知，惯性导航主要涉及两方面的内容:定位和定姿，定位可通过全球卫星定位系统(如GPS、北斗等)完成；定姿主要将加速度计输出的初始角度信息和陀螺仪输出的三轴角速度信息通过积分计算得到当前的姿态数据(主要包括方向角、俯仰角和翻滚角)。由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求。
【发明内容】
[0003]本发明的目的在于提供一种AHRS高精度姿态数据计算方法，主要解决现有技术中存在的由于加速度计和陀螺仪的精度有限，且存在累积误差，因而所得到的姿态数据误差较大，不能满足技术需求问题。
[0004]为了实现上述目的，本发明采用的技术方案如下:
[0005]AHRS高精度姿态数据计算方法，包括以下步骤:
[0006](I)将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理；
[0007](2)读取AHRS中加速度计和磁力计的数据，计算载体的姿态数据和此时的坐标系转换矩阵R1 ;
[0008](3)读取AHRS中陀螺仪的数据，根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R2 ；
[0009](4)根据坐标系转换矩阵R1和坐标系转换矩阵R2确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵；
[0010](5)根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据；
[0011](6)返回步骤(2)，进行下一时刻的姿态数据计算。
[0012]所述步骤(I)具体包括:
[0013](Ia)配置传感器的各项参数；
[0014](Ib)根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵
D
normalized°
[0015]所述步骤(Ib)包括以下步骤:
[0016](Ibl)从加速度计上读取载体坐标系在X轴向的加速度accel_x、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z ;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z ；[0017](lb2)根据公式
【权利要求】
1.AHRS高精度姿态数据计算方法，其特征在于，包括以下步骤: (1)将AHRS中的传感器和归一化坐标系转换矩阵进行初始化处理； (2)读取AHRS中加速度计和磁力计的数据，计算载体的姿态数据和此时的坐标系转换矩阵R1 ; (3)读取AHRS中陀螺仪的数据，根据上一时刻的归一化坐标系转换矩阵计算该时刻的坐标系转换矩阵R2 ； (4)根据坐标系转换矩阵R1和坐标系转换矩阵R2确定该时刻最终的坐标系转换矩阵，并对最终的坐标系转换矩阵进行归一化，得到该时刻的归一化坐标系转换矩阵； (5)根据步骤(4)中得出的归一化坐标系转换矩阵求解出该时刻最终的姿态数据； (6)返回步骤(2)，进行下一时刻的姿态数据计算。
2.根据权利要求1所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(1)具体包括: (Ia)配置传感器的各项参数； (Ib)根据此时加速度计和磁力计的输出数据初始化归一化坐标系转换矩阵RnOTmalized。
3.根据权利要求2所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(Ib)包括以下步骤: (Ibl)从加速度计上读取载体坐标系在X轴向的加速度accel_X、在Y轴向的加速度accel_y、在Z轴向的加速度acce`l_z ;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z ；ctcccl X (lb2)根据公式tan(妁=—1.2 ~.2计算俯仰角Θ ;根据公式
^jaccelj + accel_z
QCCCl Vtan(妁=-^计算翻滚角Φ ;根据公式
accel_z
magjK - magnetonijK * cos(沒)+ magnctom^y * sin(^) * sin(沒)
+ magnetotn_z * cos(^) * sin(i?)
mag_\' — magnetom_y * cos(^) - magnetom—z * sin(參)计算方向角 ψ ;.、 mag V
tan(i//) =---
mag^x 相应地，所述归一化坐标系转换矩阵Rnmializral通过公式
cos^cosi// sin ^sin ^cosi// - cos ^ si η ψ cos ^ sin ^cos/// + sin ^sin y/
R = cos^sin/// sin^sin^sin*// + cos^cos;// cos^sin^sin/// - sin^cos?// 完成丰刀始
-sinsin 彳 cos Θcos φ cos Θ化。
4.根据权利要求3所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(2)具体包括以下步骤: (2a)从加速度计上读取载体坐标系在X轴向的加速度accel_X、在Y轴向的加速度accel_y、在Z轴向的加速度accel_z ;从磁力计上读取载体坐标系在X轴向的磁场强度magnetom_x、在Y轴向的磁场强度magnetom_y、在Z轴向的磁场强度magnetom_z ；
(2b)根据公式
5.根据权利要求4所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(3)具体包括以下步骤: (3a)从陀螺仪上读取载体坐标系在X轴向的角速度ωχ、在Y轴向的角速度coy、在Z轴向的角速度ωζ ；
(3b)根据公式声和公式
6.根据权利要求5所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(4)具体包括以下步骤: (4a)根据计算出的坐标系转换矩阵R1和坐标系转换矩阵R2计算该时刻最终的坐标系转换矩阵R=R^X (R2-R1),其中，λ为坐标系转换矩阵校正因子，且O≤λ ≤ 1 ； (4b)根据以下公式:
7.根据权利要求6所述的AHRS高精度姿态数据计算方法，其特征在于，所述步骤(5)中该时刻最终的姿态数据通过该时刻的最终俯仰角Θ、翻滚角Φ和方向角Ψ确定，该时刻的最终俯仰角Θ、翻滚角Φ和方向角Ψ根据公式
【文档编号】G01C21/20GK103499348SQ201310440327
【公开日】2014年1月8日 申请日期:2013年9月24日 优先权日:2013年9月24日
【发明者】李玉柏, 王坚, 李桓 申请人:成都市星达通科技有限公司