CN105243263A - 二子样旋转矢量姿态算法ip核 - Google Patents

Abstract

本发明公开了一种二子样旋转矢量姿态算法IP核，包括422接口模块、角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块，其中422接口模块又包括分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块；所述分频模块分别与422接收模块、运算结果分解模块、422输出模块相连；422接收模块通过帧校验模块、FIFO存储发送模块接入角增量模块；角增量模块与等效旋转矢量模块、变换四元数模块、四元数更新模块顺次连接，四元数更新模块分别接入计算姿态角模块和角增量模块；计算姿态角模块依次通过姿态角输出模块、运算结果分解模块接入422输出模块。本发明运行周期短、精度高，为高速姿态测量组件的实现提供可行性。

Description

二子样旋转矢量姿态算法IP核

技术领域

本发明属于姿态测量组件技术领域，特别是一种二子样旋转矢量姿态算法IP核。

背景技术

在研制姿态测量组件原理样机时，对所采用的姿态解算算法通常会考虑其在圆锥运动时产生的误差。对捷联惯导姿态更新算法来说，圆锥运动是最恶劣的环境条件，它会诱发数学平台的严重漂移，所以在旋转矢量优化算法中常以锥运动作为环境条件。如果能确保锥运动环境条件下的算法漂移最小，就一定能确保在其余环境条件下的算法漂移最小。引起圆锥误差的因素很多，对于陀螺带宽、量化误差、安装误差等引起的锥误差，可以通过提高陀螺相应性能来克服。对于姿态更新率过低，理应通过提高更新率来解决，但算法的实时性又制约着更新率的提高，解决这一矛盾的有效途径是采用等效旋转矢量法实现姿态更新。传统的使用DSP作为处理器实现该算法运算时间接近1ms，如处理器为TMS320VC5509A时，算法运行时间为0.77ms。这个速度不能满足对速度要求较高的姿态测量组件的设计。

目前，二子样旋转矢量姿态算法在通用MCU或者DSP中运行周期过长，导致运算时间不能进一步减少，无法实现高速和高精度。

发明内容

本发明的目的在于提供一种速度快、精度高的二子样旋转矢量姿态算法IP核，为高速姿态测量组件的实现提供可行性。

实现本发明目的的技术解决方案为：一种二子样旋转矢量姿态算法IP核，包括422接口模块、角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块，其中422接口模块又包括分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块；所述分频模块分别与422接收模块、运算结果分解模块、422输出模块相连；422接收模块通过帧校验模块与FIFO存储发送模块相连，FIFO存储发送模块接入角增量模块；角增量模块与等效旋转矢量模块、变换四元数模块、四元数更新模块顺次连接，四元数更新模块分别接入计算姿态角模块和角增量模块；计算姿态角模块依次通过姿态角输出模块、运算结果分解模块接入422输出模块；

所述422接口模块负责从外部读取422串口数据及各个时刻的轴向角速度，并将经过二子样旋转矢量姿态算法运算的结果按422串口标准输出到外部存储；

所述角增量模块根据FIFO存储发送模块输入的三组数据格式为IEEE754单精度浮点的三个轴向的角速度Theta_i(i＝1,2,3)，按照积分公式求得两组二子样姿态角增量θ₁,θ₂作为等效旋转矢量模块的输入信号；

所述等效旋转矢量模块将二子样姿态角增量θ₁,θ₂根据优化的二子样旋转矢量公式处理得到等效旋转矢量Φ作为变换四元数模块的输入信号；

所述变换四元数模块将等效旋转矢量Φ按照四元数变换公式进行处理得到四元数作为四元数更新模块的输入信号；

所述四元数更新模块将四元数按照姿态更新方程进行处理得到更新后的四元数作为计算姿态角模块的输入信号；

所述计算姿态角模块根据更新后的四元数构建姿态矩阵，求得姿态角即俯仰角横滚角航向角输出至422接口模块的姿态角输出模块。

本发明与现有技术相比，其显著优点为：(1)采用等效旋转矢量的二子样优化算法进行算法IP核的设计，减少了姿态解算算法在圆锥运动时的漂移；(2)采用FPGA作为处理器，使用Xilinx提供的算法IP核搭建，全程并行处理，使得实现该算法只需要十几微秒，大大提高姿态测量组件的运行速度，为高速姿态测量组件的实现提供了可能性；(3)采用422进行串口通信，以差动方式发送和接收，并且全双工工作模式使得收发不影响，使得它传输距离远，速度快。

附图说明

图1本发明二子样旋转矢量姿态算法IP核的模块结构示意图。

图2为本发明二子样旋转矢量姿态算法的流程图。

图3为本发明二子样旋转矢量姿态算法IP核中422接收模块状态转换图。

图4为本发明二子样旋转矢量姿态算法IP核中帧校验模块状态转换图。

图5为本发明二子样旋转矢量姿态算法IP核中运算结果分解模块状态转换图。

图6为本发明二子样旋转矢量姿态算法IP核中422输出模块状态转换图。

图7为本发明二子样旋转矢量姿态算法IP核总体模块电路封装符号图。

具体实施方式

结合图1，本发明二子样旋转矢量姿态算法IP核，包括422接口模块、角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块，其中422接口模块又包括分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块；所述分频模块分别与422接收模块、运算结果分解模块、422输出模块相连；422接收模块通过帧校验模块与FIFO存储发送模块相连，FIFO存储发送模块接入角增量模块；角增量模块与等效旋转矢量模块、变换四元数模块、四元数更新模块顺次连接，四元数更新模块分别接入计算姿态角模块和角增量模块；计算姿态角模块依次通过姿态角输出模块、运算结果分解模块接入422输出模块；所述角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块均由Xilinx提供的CORDICIP核、浮点操作器IP核、加/减法器IP核、乘法器IP核和除法器IP核搭建而成，整个算法的流程如图2，各个模块的功能如下：

一、422接口模块

所述422接口模块负责从外部读取422串口数据及各个时刻的轴向角速度，将其进行和校验后，并将经过二子样旋转矢量姿态算法运算的结果按422串口标准输出到外部存储。

所述422接口模块中的分频模块作用是将系统时钟分为422接收模块、运算结果分解模块和422发送模块所需的时钟；422接收模块采用16倍频采样接收，设计采用状态机，分为5个状态：等待起始位、求中点，等待采样，采样，停止位接收，422接收模块的状态转换如图3所示；帧校验模块设计采用状态机，分为4个状态：接收同步字其中字节0xaa、接收同步字其中字节0x55、接收保留字节、接收一组角速度，帧校验模块的状态转换如图4所示；FIFO存储发送模块用于存储转发接收的角速度数据；运算结果分解模块设计采用状态机，分为8个状态：空闲发送同步字字节0xaa、发送同步字字节0x55、等待、422使能信号归’0’、分解航向角、分解横滚角、分解俯仰角、停止发送校验字节，运算结果分解模块状态转换如图5所示；422输出模块设计采用状态机，分为5个状态：空闲、发送起始位、移位等待、移位、发送停止位，422输出模块的状态转移如图6所示。

所述422接口模块中分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块，具体功能如下：

(1)分频模块根据所需波特率将系统时钟进行分频处理获得所需的422接收模块的输入时钟，422接收模块在输入时钟上升沿时接收422接收端传来的比特，接收到一个字节后输入帧校验模块；

(2)帧校验模块接收到同步字0x55aa时开始接收新的一帧数据，该帧的最后一个字节为前面所有数据和较验的低八位数据，通过和校验的一组数据将输出口使能信号cheken置’1’作为FIFO存储发送模块的敏感信号，一帧数据包括该时刻的三个轴向角速度；

(3)FIFO存储发送模块当使能信号cheken上升沿时存入一组数据，在存储数据大于3时同时输出三组数据用于二子样旋转矢量姿态算法IP核的运算，下面每新存入两组新数据，加上之前最近时刻的一组数据为角增量模块提供输入信号；

(4)运算结果分解模块将最近更新的姿态角包括俯仰角、横滚角、航向角分解成一个个字节并加上同步字0x55aa以及和校验的低八位按时钟周期输出；

(5)422输出模块按照一定的波特率将运算结果分解模块输出的字节按顺序发送出去，422输出模块的时钟由分频模块分频得到。

二、角增量模块

所述角增量模块根据FIFO存储发送模块输入的三组数据格式为IEEE754单精度浮点的三个轴向的角速度Theta_i(i＝1,2,3)，按照积分公式求得两组二子样姿态角增量θ₁,θ₂作为等效旋转矢量模块的输入信号；所述积分公式如式(1)所示：

${\mathrm{\θ}}_1=(5*{\mathrm{Theta}}_1+8*{\mathrm{Theta}}_2-{\mathrm{Theta}}_3)\frac{h}{24}$

${\mathrm{\θ}}_2=(-{\mathrm{Theta}}_1+8*{\mathrm{Theta}}_2+5*{\mathrm{Theta}}_3)\frac{h}{24}---\left(1\right)$

其中，h为姿态更新周期，在二子样旋转矢量姿态算法中取0.02，Theta_i(i＝1,2,3)为三个轴向角速度。

三、等效旋转矢量模块

所述等效旋转矢量模块将二子样姿态角增量θ₁,θ₂根据优化的二子样旋转矢量公式处理得到等效旋转矢量Φ作为变换四元数模块的输入信号；优化的二子样旋转矢量公式如式(2)所示：

$\mathrm{\Φ}={\mathrm{\θ}}_1+{\mathrm{\θ}}_2+\frac{2}{3}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2)---\left(2\right)$

其中Φ为等效旋转矢量。

四、变换四元数模块

所述变换四元数模块将等效旋转矢量Φ按照四元数变换公式通过CORDICIP核、浮点操作器IP核、加/减法器IP核、乘法器IP核和除法器IP核进行处理得到四元数作为四元数更新模块的输入信号；四元数变换公式如式(3)所示：

$q\left(h\right)={\[cos\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_x}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_y}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_z}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\]}^T---\left(3\right)$

其中，Φ_x,Φ_y,Φ_z分别为等效旋转矢量Φ的三个轴向分量值，为所求四元数。

五、四元数更新模块

所述四元数更新模块将四元数按照姿态更新方程通过浮点操作器IP核进行处理得到更新后的四元数作为计算姿态角模块的输入信号；姿态更新方程如式(4)所示：

$Q(t+h)=Q\left(t\right)\⊗q\left(h\right)---\left(4\right)$

其中Q(t)为当前时刻的姿态四元数，Q(t+h)为下一时刻的姿态四元数。

六、计算姿态角模块

所述计算姿态角模块根据更新后的四元数构建姿态矩阵，求得姿态角即俯仰角横滚角航向角输出至422接口模块的姿态角输出模块。所述计算姿态角模块中，姿态矩阵如式(5)所示，求姿态角的公式如式(6)所示：

${\hat{C}}_b^n\left[\begin{array}{ccc}{q}_0^2+q_1^2+q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(5\right)$

其中，q₀,q₁,q₂,q₃为四元数，并记 ${\hat{C}}_b^n\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right];$

其中，为俯仰角、为横滚角、为航向角。

本发明二子样旋转矢量姿态算法IP核的工作过程为：分频模块将100Mhz(可更换)的系统时钟分成422接收发送所需的时钟，以及运算结果分解所需的时钟；422接收模块通过识别起始位0按字节接收角速度；帧校验模块当接收一组角速度后进行和校验，如通过校验则将使能信号置’1’；FIFO存储发送模块在帧校验使能信号为’1’时，存储一组角速度数据，当存储的角速度组数大于3时，输出3组角速度用于姿态解算，之后每更新两组角速度保留前一次输出角速度的最后一组数据加上更新的两组输出用于姿态解算；运算结果分解模块用于将从姿态解算模块输出的的俯仰角，航向角，横滚角按字节输出另加上其和校验低八位数据，并在每输出一个字节时将一个信号置一个脉冲，用于422输出模块的发送信号。422输出模块根据输出使能信号将每个字节加上起始位和停止位按波特率发送出去。

本算法IP核上电烧写前先根据实际所需的发送与接收数据波特率修改分频模块的分频数。总体模块电路封装符号图如图7所示，烧写前的管脚配置为：[CLKIN]接系统时钟管脚；[RESET]接一个基本I/O，做复位开发；[RXDR]接422接收端；[TXD]接422发送端；[TXD_DONE]接一个基本I/O,为发送指示灯；[FIFOEMPTY]和[FIFOFULL]接基本I/O，用于运行状态指示和运行出错指示。

运行时按一定数据格式进行数据发送，每个轴向的角速度采用的格式为IEEE754浮点，采用单精度，数据位为32位。接收到一组三个轴向角速度后进行和校验，通过校验后将这组角速度组合成一个96位的数据存储在FIFO存储器中。采用的FIFO存储深度为6(可以调节)，宽度为96位。当存储地址位置偏移大于3时读出三组数据，并且四元数输出模块输出初始四元数值用于姿态解算，在经历过角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块后输出此次四元数更新值将其缓存在四元数输出模块用于下一次计算，输出的俯仰角，航向角，横滚角至运算结果分解模块；运算结果分解模块的时钟来自于系统时钟分频所得。因为每次运算产生的输出数据量为3个32位的角度数据共24字节，加上发送时添加的同步字0x55aa以及一个帧字节的帧校验和则每计算一次需要产生分解的字节数为27个，所以它的频率要求不少于运算一次的频率的27倍。最后将分解的字节按发送波特率发送出去及完成第一次运算。下面计算频率为每更新两组数据运行一次。

综上所述，本发明解决了二子样旋转矢量姿态算法在通用MCU或者DSP中运行周期过长，导致运算时间不能进一步减少，无法实现高速和高精度的问题。本发明采用IP核方式提供，以VHDL硬件描述语言软核形式实现二子样旋转矢量姿态算法，可在FPGA中实现，典型工作频率为100MHZ，为高速姿态测量组件的实现提供可行性。

Claims (8)

1.一种二子样旋转矢量姿态算法IP核，其特征在于，包括422接口模块、角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块，其中422接口模块又包括分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块；所述分频模块分别与422接收模块、运算结果分解模块、422输出模块相连；422接收模块通过帧校验模块与FIFO存储发送模块相连，FIFO存储发送模块接入角增量模块；角增量模块与等效旋转矢量模块、变换四元数模块、四元数更新模块顺次连接，四元数更新模块分别接入计算姿态角模块和角增量模块；计算姿态角模块依次通过姿态角输出模块、运算结果分解模块接入422输出模块；

所述422接口模块负责从外部读取422串口数据及各个时刻的轴向角速度，并将经过二子样旋转矢量姿态算法运算的结果按422串口标准输出到外部存储；

所述角增量模块根据FIFO存储发送模块输入的三组数据格式为IEEE754单精度浮点的三个轴向的角速度Theta_i(i＝1,2,3)，按照积分公式求得两组二子样姿态角增量θ₁,θ₂作为等效旋转矢量模块的输入信号；

所述等效旋转矢量模块将二子样姿态角增量θ₁,θ₂根据优化的二子样旋转矢量公式处理得到等效旋转矢量Φ作为变换四元数模块的输入信号；

所述变换四元数模块将等效旋转矢量Φ按照四元数变换公式进行处理得到四元数作为四元数更新模块的输入信号；

所述四元数更新模块将四元数按照姿态更新方程进行处理得到更新后的四元数作为计算姿态角模块的输入信号；

所述计算姿态角模块根据更新后的四元数构建姿态矩阵，求得姿态角即俯仰角横滚角航向角输出至422接口模块的姿态角输出模块。

2.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述422接口模块又包括分频模块、422接收模块、帧校验模块、FIFO存储发送模块、姿态角输出模块、运算结果分解模块、422输出模块，具体功能如下：

(1)分频模块根据所需波特率将系统时钟进行分频处理获得所需的422接收模块的输入时钟，422接收模块在输入时钟上升沿时接收422接收端传来的比特，接收到一个字节后输入帧校验模块；

(2)帧校验模块接收到同步字0x55aa时开始接收新的一帧数据，该帧的最后一个字节为前面所有数据和较验的低八位数据，通过和校验的一组数据将输出口使能信号cheken置’1’作为FIFO存储发送模块的敏感信号，一帧数据包括该时刻的三个轴向角速度；

(3)FIFO存储发送模块当使能信号cheken上升沿时存入一组数据，在存储数据大于3时同时输出三组数据用于二子样旋转矢量姿态算法IP核的运算，下面每新存入两组新数据，加上之前最近时刻的一组数据为角增量模块提供输入信号；

(4)运算结果分解模块将最近更新的姿态角包括俯仰角、横滚角、航向角分解成一个个字节并加上同步字0x55aa以及和校验的低八位按时钟周期输出；

(5)422输出模块按照一定的波特率将运算结果分解模块输出的字节按顺序发送出去，422输出模块的时钟由分频模块分频得到。

3.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述角增量模块、等效旋转矢量模块、变换四元数模块、四元数更新模块、计算姿态角模块均由Xilinx提供的CORDICIP核、浮点操作器IP核、加/减法器IP核、乘法器IP核和除法器IP核搭建而成。

4.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述角增量模块中，求得两组二子样姿态角增量θ₁,θ₂所使用的积分公式如式(1)所示：

$\begin{array}{c}{\mathrm{\θ}}_1=(5*{\mathrm{Theta}}_1+8*{\mathrm{Theta}}_2-{\mathrm{Theta}}_3)\frac{h}{24}\\ {\mathrm{\θ}}_2=(-{\mathrm{Theta}}_1+8*{\mathrm{Theta}}_2+5*{\mathrm{Theta}}_3)\frac{h}{24}\end{array}---\left(1\right)$

其中，h为姿态更新周期，Theta_i(i＝1,2,3)为三个轴向角速度。

5.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述等效旋转矢量模块中，优化的二子样旋转矢量公式如式(2)所示：

$\mathrm{\Φ}={\mathrm{\θ}}_1+{\mathrm{\θ}}_2+\frac{2}{3}({\mathrm{\θ}}_1\×{\mathrm{\θ}}_2)---\left(2\right)$

其中Φ为等效旋转矢量。

6.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述变换四元数模块中，四元数变换公式如式(3)所示：

$q\left(h\right)={\[cos\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_x}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_y}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\frac{{\mathrm{\Φ}}_z}{{\mathrm{\Φ}}_0}sin\frac{{\mathrm{\Φ}}_0}{2}\]}^T---\left(3\right)$

其中，Φ_x,Φ_y,Φ_z分别为等效旋转矢量Φ的三个轴向分量值，q(h)为所求四元数。

7.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述四元数更新模块中，姿态更新方程如式(4)所示：

$Q(t+h)=Q\left(t\right)\⊗q\left(h\right)---\left(4\right)$

其中Q(t)为当前时刻的姿态四元数，Q(t+h)为下一时刻的姿态四元数。

8.根据权利要求1所述的二子样旋转矢量姿态算法IP核，其特征在于，所述计算姿态角模块中，姿态矩阵如式(5)所示，求姿态角的公式如式(6)所示：

${\hat{C}}_b^n\left[\begin{array}{ccc}{q}_0^2+q_1^2+q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(5\right)$

其中，q₀,q₁,q₂,q₃为四元数，并记 ${\hat{C}}_b^n\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right];$

$\widehat{\mathrm{\θ}}=\arcsin \left(T_{32}\right)=\arctan \[T_{32}/\sqrt{T_{21}^2+T_{22}^2}\],$

其中，为俯仰角、为横滚角、为航向角。