基于四元数法的航向修正算法分析

戚宪东,郭英,孙玉曦,姬现磊

(山东科技大学 测绘科学与 工程学院,山东 青岛 266590)

0 引 言

捷联惯导将陀螺仪和加速度计直接安装在载体上,省掉了机电式导航平台,利用计算机软件建立一个“数学平台”来代替机电平台实体。由于其结构简单且抗干扰能力强,目前已成为导航定位、无人机、智能交通等领域的研究热点之一[1]。

姿态解算是捷联式惯性导航系统的关键技术,通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据,是捷联式惯导算法中的重要工作[1]。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系。用于描述载体坐标系与导航坐标系转换关系的方法主要有欧拉角法、方向余弦法和四元数法等[2]。其中,欧拉角法在求解姿态时存在奇点,不能用于全姿态的解算[3];方向余弦法可用于全姿态的解算但计算量大,不能满足实时性要求[4];四元数法算法简单,易于操作,只需要求解四个参数,计算量小,可以在实现过程中修正漂移,应用比较广泛[5]。而四元数微分方程的解算主要用到龙格-库塔法[6],文献[7]介绍了一阶龙格-库塔法,其存在解算精度和效率较低的问题;文献[8]详细叙述了四阶龙格-库塔法,在解算时收敛性较好,稳定性较高。因此,寻求一种收敛性好和稳定性高的高阶计算方法,能够高效、快速的解算四元数微分方程,保证四元数解算航向的精度。

在姿态解算过程中,除了优化解算方法外,还存在陀螺仪长时间工作的漂移误差。消除由陀螺仪的漂移误差导致的航向误差一直是阻碍室内定位精度提升的难点[9],为了减少航向误差,研究人员提出了几种航向修正的方法:Borenstein等人[10]提出了基于脚踏式惯性测试单元(IMU)的启发式偏移减少算法(HDR)和启发式漂移消除算法(HDE)。楼喜中等人[11]在HDE的基础上,将航向角作为直接修改的对象,消除了部分陀螺仪积分误差。

本文利用行人在室内直线运动情况下的航向角偏差作为误差反馈回系统,来修正陀螺仪的角速度输出,从而抑制陀螺仪的漂移,进而修正四元数法解算的航向,并将其与参考航向对比以验证四元数法的正确性和有效性,并分析不同采样频率下的二阶龙格-库塔法与四阶龙格-库塔法解算航向精度,得到提高采样频率能减少航向解算误差,提高定位精度,对捷联式惯性导航技术有一定的应用参考价值。

1 系统模型

1.1 姿态矩阵的描述

通常假定捷联系统“数学平台”模拟地理坐标系,即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(b系)和导航坐标系(n系)的空间转动关系,一般用载体坐标系相对导航坐标系的三次转动角确定[1]。载体的姿态由横滚角γ,俯仰角θ,航向角φ来描述。

1.2 四元数微分方程

随着捷联式惯性导航技术的发展,为了更简便地描述刚体的角运动,采用了四元数这个数学工具,用它来弥补通常描述刚体角运动的3个欧拉角参数在设计控制系统时的不足[3]。

规定q为一个四元数向量,即:

(1)

则q满足微分方程:

(2)

四元数姿态矩阵的微分方程只要解四个一阶微分方程式即可,计算量小,容易实现,是导航解算中常用的方法。

2 基于四元数的航向解算

常用的姿态描述方法有:欧拉角法、方向余弦法和四元数法。由于相对比其他两个方法,四元数方法具有计算简单、易于操作、且能避免出现欧拉角所产生的“奇点”问题的优点,故本文选择四元数法来解算姿态信息。

2.1 四元数法姿态矩阵计算的步骤如下:

第1步:初始化四元数。初始姿态信息设置:航向角为90°、俯仰角和横滚角都设置为0°.初始化四元数(q0,q1,q2,q3)为

(3)

式中:φ0、θ0、γ0分别为初始的航向角、俯仰角和横滚角。

第2步:四元数微分方程的实时解算。下面简述二阶龙格-库塔法和四阶龙格-库塔法解算四元数微分方程的基本原理:

方法1:利用二阶龙格-库塔法实时更新四元数。

解算公式[6]:

(4)

(5)

根据上述公式即可获得用二阶龙格-库塔法实时更新的四元数。

方法2:利用四阶龙格-库塔法实时更新四元数。

解算公式[8]:

(6)

式中:T为步长;q(t)、q(t+T)分别为t、t+T时刻的四元数。根据上述公式即可获得用四阶龙格-库塔法实时更新的四元数。

第3步:四元数归一化

(7)

第4步:由四元数计算旋转矩阵并计算航向角φ

(8)

(9)

通过以上步骤的解算,可以利用陀螺仪输出的角速度数据得到行人航向,但由于陀螺仪的输出会随着时间的增长而产生漂移,从而影响航向解算精度。

2.2 航向修正的改进HDE算法

基于室内环境下,行人多为直线行走的情况,其航向角变化较小,利用航向角偏差作为误差反馈回系统,来修正陀螺仪的角速度输出,从而抑制陀螺仪的漂移,提高定位精度,具体算法步骤如下。

第1步:判别行人运动状态

通常利用当前航向与历史航向的变化幅值来判断行人的运动轨迹是否为直线:

(10)

如果Δφi<10°时,则表示航向变化不明显,判定行人沿着直线行走。

确定行人沿直线行走后,采用下式对航向进行修正:

(11)

第2步:设置主导航向

行人初始的主方向为0°,沿着顺时针方向依次为45°、90°、135°、180°、225°、270°、315°,一共8个主方向,角度间隔δ为45°,当前航向与主航向的偏差可用下式计算:

(12)

式中:INT(x)为取整函数;φi为当前航向角。

利用改进的HDE航向修正算法,对行人在室内环境下的直线行走情况进行修正,抑制了陀螺仪的漂移,提高了定位精度。

2.3 行人运动轨迹推算

将经HDE算法修正过的航向角信息与加速度计分析所得的步长信息相结合,获得精确的定位信息。基于惯性传感器的行人室内定位的关键技术是行人航迹推算(PDR)。所谓航迹推算,即由上一位置的坐标结合当前一步的航向角信息与步长信息求解当前位置的坐标,公式为[11]

(13)

式中:x,y为位置坐标; S为步长。

3 实验结果与分析

3.1 数据采集

实验采用低成本的魅蓝NOTE3手机作为行人导航数据采集模块,进行测试实验。本文利用安卓软件“导航项目(AndroSensor)”为工具来获取传感器数据,该软件简单实用,可以满足实验的需求。为了达到较高的测量精度,在采集数据之前要对手机的内置传感器进行校准,加速度计和陀螺仪要进行零偏校准。

实验数据采集频率分别设为100 Hz、200 Hz,实验人员手持智能手机,平放于身前,以正常步速按设计的路线行走。行走路线以主导方向行走为主,实验场地为山东科技大学测绘学院办公楼的回型走廊,行走的矩形路线长为62 m、宽为42 m.

3.2 实验分析

不同采样频率下,利用二阶龙格-库塔法和四阶龙格库塔法解算四元数微分方程,对解算的行人航向进行HDE算法修正后,推算行人航迹如图1所示。

分析实验结果如表1所示。

表1 实验精度分析

注:TTD为路线闭合差与其总路线的比值

由图1和表1分析可知:

1) 本算法的解算结果与参考路径对比,验证了四元数法解算航向的有效性和可行性。

2) 二阶龙格-库塔法与四阶龙格-库塔法解算四元数微分方程,用四元数表示欧拉角,并用HDE算法修正航向,推算行人轨迹,其在100 Hz采样频率下,轨迹闭合差分别为2.847 m和2.813 m;200 Hz采样频率下,轨迹闭合差分别为1.164 m和1.159 m,解算结果相差不大,主要是由于两种方法解算的是更新后的四元数,其在转换为欧拉角以及HDE算法修正航向的过程中,原始数据进行了处理,导致高阶方法解算结果优势并不是很明显。

3) 当提高采样频率至200 Hz后,行人轨迹与参考路线较为吻合,且二阶龙格-库塔法解算行人轨迹闭合差减小了1.683 m,TTD误差大大减小;四阶龙格-库塔法解算行人轨迹闭合差减小了1.654 m,TTD误差也大幅减小。验证了在传感器一定工作范围内,提高采样频率,可以减小航向解算误差,提高定位精度。

4 结束语

本文以低成本智能手机为实验对象,选用四元数法解算载体航向信息、二阶龙格-库塔法和四阶龙格-库塔法分别进行四元数更新,然后把经过四元数解算的航向信息通过改进的HDE算法修正后,与步长信息相结合,实现位置解算。实验分析了不同采样频率下的二阶龙格-库塔法与四阶龙格-库塔法解算的航向精度,结果表明，提高采样频率能减少航向解算误差,提高定位精度。本文工作对实际导航定位工作具有一定的指导意义。

[1] 张荣辉,贾宏光,陈涛,等. 基于四元数法的捷联式惯性导航系统的姿态解算[J]. 光学精密工程, 2008, 16(10):1963-1970.

[2] 王立波. 捷联惯导系统设计与数据处理技术研究[D]. 西安:西安电子科技大学, 2011.

[3] 黄国英. 微分方程初值问题的加权改进欧拉数值解法[J]. 成功:教育版, 2011(8):287-287.

[4] 李连仲,王小虎,蔡述江. 捷联惯性导航、制导系统中方向余弦矩阵的递推算法[J]. 宇航学报, 2006, 27(3):349-353.

[5] 王彤,马建仓,秦涛,等. 基于旋转四元数的姿态解算算法[J]. 弹箭与制导学报, 2014,34(3):15-16.

[6] 杨阳. 龙格库塔法求模糊微分方程的数值解[D]. 哈尔滨:哈尔滨工业大学, 2015.

[7] 张丽娟,张翔,关天冶. 一阶常微分方程初值问题的数值算法[J]. 通化师范学院学报, 2017, 38(8):22-24.

[8] 冯建强,孙诗一. 四阶龙格——库塔法的原理及其应用[J]. 数学学习与研究, 2017(17):3-5.

[9] 曾庆军,刘慧婷,张明. 基于HDR的陀螺仪随机漂移分析与处理[J]. 江苏大学学报(自然科学版), 2016, 37(3): 332-336.

[10] BORENSTEIN J, OJEDA L. Heuristic drift elimination for personnel tracking systems[J]. Journal of Navigation, 2010, 63(4): 591-606.

[11] 楼喜中,周乐宇,叶敏展, 等. 基于角度的HDE算法在室内行人航迹定位中的研究[J]. 传感技术学报, 2015,28(4): 598-602.