基于旋转矢量和对偶四元数的捷联惯导算法比较

Wang Zhenhuan, Chen Xijun, Zeng Qingshuang

速度控制与惯性技术研究中心，哈尔滨工业大学，哈尔滨150001，中国

摘要：对于捷联惯性导航系统的导航算法，通过与双四元数和四元数方程的比较，分析了基于旋转矢量精度和基于双四元数的姿态算法在旋转矢量上的优越性的导航框架。通过比较双四元数解中重力速度的更新算法与传统速度解的有害加速度补偿算法，可以解决基于双四元数的重力速度的精度优势。鉴于基于双四元数的姿态和速度算法的思想，提出了一种改进的导航算法，其与旋转矢量算法一样的计算复杂度。根据该方法，导航框架旋转时姿态四元数不需要补偿。为了验证理论分析的正确性，利用软件进行仿真，仿真结果表明，改进算法的精度近似等于双四元数算法。

关键词：双重四元数 惯性导航系统 导航算法 四元数 旋转矢量

1、介绍

自20世纪50年代末捷联惯性导航系统（SINS）概念出现以来，半个多世纪以来SINS理论和应用的发展取得了长足的进步。 Savage^1.2表示了SINS³的传统导航解决方案的成熟.2006年，Savage⁴描述了传统的陷阱导航算法设计的统一数学框架。 在传统算法中，陀螺仪输出用于维护数字参考系，其中来自加速度计三元组的特定力测量被解析，然后双重积分以获得速度和位置^5-7。车辆的旋转和平移运动被分别描述。 四元和矢量分别被选择来代表旋转和平移^8-11。

双四元数是同时表示旋转和平移的最简洁有效的数学工具^12-15。1992年，双四元数首先引入了捷联惯性导航的解¹⁶。Wu et al¹²证明了双轨惯性导航的原理四元数，并得出结论为双四元数算法在速度精度方面优于传统的导航算法，特别是推力速度。没有有关分析基于双四元数算法的姿态和引力速度解的表现。虽然双四元数算法的解决方案精度高于任何常规算法，但由于双向量和双四元数的计算，双四元数算法的计算复杂度非常大。

在本文中，证明了基于双四元数的姿态和重力速度算法与基于旋转矢量精度的优势。提出了一种改进的导航算法来解决导航精度与计算复杂度之间的冲突。

2、导航算法比较

2.1 姿态算法的比较

对于基于旋转向量的姿态算法，假设b（k）和n（k）表示时刻T（k），b（k 1）和n（k 1） 分别在时刻t（k 1）表征车体框架和导航框架。 令单位四元数q_b（h）表示从帧b（k）到b（k 1）的变换，Q_nb(t_k)从帧n（k）到帧b（k）的变换，Q_nb(t_{k 1})从帧的变换 n（k 1）到帧b（k 1），p（h）从帧n（k）到帧n（k 1）的变换，其中h =t_{k 1}-t_k。 根据四元素的特征，可以得到以下等式：

其中r表示向量，上标表示相应帧中的投影，上标“*”表示共轭，符号表示四元数乘法。

方程式 （1）等价于以下等式：

比较方程 （1）式 （2），可以得到：

在姿态更新周期中，导航帧的变化非常慢，即p（h）asymp;[1 0 0 0]，所以等式 （3）可以简化为

其中q_b(h)= cos（sigma;/ 2） （sigma;/sigma;）sin（rsigma;/ 2），sigma;表示等效旋转矢量，sigma;= |sigma;|。当t_kle;tle;t_{k 1}时，方程式 （4）可以描述为：

根据方程式（5）的四元数微分方程和时间导数，可以获得以下等式：

旋转矢量的微分方程可以通过公式 （6）获得，即Bortz方程^17-18：

四元数q_b可以由公式（7）跟新，以便根据方程式(4)更新姿态四元数Q_nb。 由于近似公式（4）中，基于旋转向量的姿态算法不可避免地出现误差，姿态四元数必须补偿几个姿态更新周期后导航框旋转引起的误差。 然而，当车辆高速移动时，误差将被放大，误差补偿的影响将显着降低。

利用他对双四元数的捷联惯性导航的原理由三个连续的运动学方程表示，即推力速度方程，重力速度方程和位置方程，其中车辆姿态的确定来自推力速度运动学方程。

假设T（k）和i（k）表示时间tk，T（k 1）和i（k 1）处的推力帧和惯性帧，表征时刻tk 1的推力框架和惯性框架。令单位双重四元数q_T(h)代表变换从帧T（k）到T（k 1）的关系，Q_iT(t_k)从帧i（k）到帧T（k）的变换，从帧i（k 1）到帧T（k 1）的变换，以及p(h)从帧i（k）到 帧i（k 1），其中h =t_{K 1}-t_k。根据双四元数的特性，可以得到以下等式：

其中r表示双向量，上标表示相应帧中的投影。

由于惯性框架是静止的，即i（k）= i（k 1），可以得到以下等式：

比较方程 （8）和（9），可以得到：

假设q_T(h)=cos(sigma;/2) ( sigma;/sigma;)sin(sigma;/2), 其中sigma;表示位于螺旋轴上的螺旋矢量，sigma;=（sigma;·sigma;）^1/2。当t_kle;tle;t_{k 1}时，可以根据等式（10）得到下面式子：

在将上述等式推导出微积分之后，可以得到以下等式：

把等式 （11）变为双四元数微分方程可以获得如下等式：

比较方程 （12）和（13），可知

方程式 （14）可以进一步简化为

根据Shen¹⁹等人的转移原则，四元数的特征完全由双四元数继承。 使用相同的方法导出旋转矢量的微分方程，螺旋矢量的微分方程可以写为

比较方程 （10）等式 （4）可知（10）是惯性框架静止的精确表达式。根据等式（10） 当车辆高速行驶时，双重四元数Q_iT的更新根据方程式不具有误差。所以基于双四元数的姿态算法的精度高于基于旋转矢量的姿态算法的精度。

2.2 速度算法的比较

传统的速度算法描述如下：

其中和分别表示时刻t_m和t_m-1的车辆速度，表示时刻t_m-1的姿态矩阵，f^b是特定力，gⁿ是重力加速度，是地球的旋转角速度，是导航框架相对于地球框架旋转的角速率。

对于双四元数算法，重力速度的确定在与地球框架对齐的重力速度框架中，重力速度相对于惯性框架。引力速度的更新可以描述为

其中sigma;rsquo;=G.·Delta;t, sigma;=·Delta;t，Delta;t是速度的更新周期，G是重力加速度，可以通过以下公式确定：

其中表示从导航框架到地球框架的方向余弦矩阵，R表示车辆的位置矢量。 由方程式 （18）和（19），可以得到重力速度的更新仅取决于前一时间的车辆位置，即经度，纬度和高度。随着速度变化速率远大于位置变化率，重力速度算法优于传统速度解的有害加速积分算法。

3. 改进的导航算法

假设b（k）和i（k）表示在时刻t_k，b（k 1）和i（k 1）处的体帧和惯性帧，表征车体帧和时刻t_{k 1}的惯性帧. 令单位四元数q_b(h)表示从帧b（k）到帧b（k 1）的变换，Q_ib(t_{k 1})是从帧i（k 1）到帧b（k 1）的变换，以及p(h)从帧i（k）到帧i（k 1）的变换，其中h =t_{k 1}-t_k，根据四元数的特征，可以得到以下等式：

其中r表示向量，上标表示相应帧中的投影。

方程式 （20）相当于以下等式：

比较方程式（20）和（21），可以得到

由于惯性框架是静止的，即p（h）= [1 0 0 0]，可以得到以下等式：

假设q_b(h)=cos(sigma;/2) ( sigma;/sigma;)sin(sigma;/2), 其中sigma;表示等效旋转矢量，sigma;= |sigma;|，当t_kle;tle;t_{k 1}时，可以根据等式（23）得到下面式子：

推导微积分到上述方程之后，可以得到以下等式：

比较方程 （25）和（26），可以得出：

方程式 （27）可以进一步简化为

使用相同的方法推导出旋转矢量的微分方程，可以写为：

根据方程式（23）和（29），可以更新四元数Q_ib。由于态度决定通常在地理框架中，因此需要获取四元数Q_gb，其中Q_gb表示从地理框架到身体框架的转换。根据四元数的特征，可以得到以下等式：

其中Q_ie表示从惯性框架到地球框架的变换，Q_eg从地球框架到地理框架的转换。

根据四元数的乘法特性，方程式（30）可以转换成以下表达式：

假设地球框架在初始时与惯性框架重合，四元数Q_ie可以改为如下：

其中W_ie= |W_ie|，而t是导航时间。

地理框架定义为x指向东，y指向北，z向上垂直。根据地球帧与地理帧之间的关系，四元数Q_eg可以描述为:

其中lambda;和psi;分别代表经度和纬度。

取等式 （32）和（33）代入（31），可以获取姿态四元数Q_gb，以便确定车辆姿态。与传统姿态算法相比，改进的姿态算法不需要补偿当车辆移动时导航框架旋转引起的误差。

如上所述，基于双四元数的速度算法在精度上是优越的，重力速度的确定描述如下：

推力速度由以下等式确定：

基于双四元数的速度计算相对于惯性框架并投影到其中。 然而，车辆的速度通常在相对于地球框架的地理框架中表示。 它可以转换为以下等式：

其中是从地球帧到地理帧的方向余弦矩阵，是惯性的变换框架到地球框架。

对于车辆的位置确定，可以根据改进的姿态算法的进行更新，即通过来更新Q_ig相应地获得Q_eg。

如上所述，可以获得相对于惯性框架的车速。 假设局部地理帧用作导航帧，则可以通过以下等式获得地理帧相对于惯性帧的角速度，即W_ig：

由于惯性框架是静止的，即p（h）= [1 0 0 0]，所以可以得到以下等式：

假设q_g(h)=cos(sigma;/2) ( sigma;/sigma;)sin(sigma;/2), 其中sigma;表示等效旋转矢量。模仿改进姿态算法的推导，可得到：

根据四元数的特征，可以得到以下等式：

其中Q_ie可以通过等式（32）获取。根据方程式（41）中，可以获得方向余弦矩阵，以便确定车辆的位置。

改进的导航算法的框图如图1所示。 改进的导航算法基于四元数，其结构与传统的导航算法相似， 因此，基于改进算法的计算复杂度与基于传统算法的计算复杂度相同。然而，对于基于双四元数的导航算法，需要计算双向量和双四元数，与改进的导航算法相比，计算复杂度是两倍。特别地，双四元数算法中的位置确定需要进行迭代计算²⁰，所以改进算法的计算远远小于基于计算复杂度的双四元数的算法。

比较方程（23）等式（4）和（23）是惯性框架静止的精确表达式，并且当车辆高速移动时，改进的导航算法中的姿态解决方案不会受到导航框架的旋转的影响，从而使改进算法在精度方面的态度优越性。如上所述，基于改进算法的速度计算分别在体框架和地球框架中，其中重力速度仅取决于前一时间的车辆位置，而不是基于传统算法，其不仅依赖于以前的时间位置也是以前的时间速度。因此，当车辆速度急剧变化时，基于改进的导航算法的速度计算优于基于传统导航算法的精度的速度计算。总而言之，当车辆高速移动时，改进的导航算法具有更高的精度速度。

4.导

剩余内容已隐藏，支付完成后下载完整资料

---

资料编号：[138038]，资料为PDF文档或Word文档，PDF文档可免费转换为Word