角速率输入条件下单子样旋转矢量姿态方法

摘要

本发明公开了一种角速率输入条件下单子样旋转矢量姿态方法，该方法基于角速率下多子样旋转矢量算法和角增量下单子样旋转矢量算法，将单子样算法应用到基于角速率的圆锥误差补偿算法中，具体利用当前时刻及前N个时刻的角速率来拟合圆锥误差补偿项，最优补偿项的系数由解线性方程组得到，可以在陀螺仪只提供角速率的情况下，利用角速率输入完成姿态解算并且保持解算频率与采样频率一致。本发明的方法既没有引入由角速率提取角增量带来的误差，又没有降低系统的姿态更新频率。

主权项

1.一种角速率输入条件下的单子样旋转矢量姿态方法，具体包括如下步骤：步骤S1.获取设当前k时刻的姿态角为att_k=[θγψ]^T，θ、γ和ψ分别为俯仰角、横滚角和偏航角；步骤S2、计算旋转矢量步骤S3、计算当前k时刻的姿态四元数Q(t_k)，具体过程为：姿态角与坐标转换矩阵的关系为：$C_n^b=\left[\begin{array}{ccc}\cos \gamma \cos \Psi -\sin \gamma \sin \psi \sin \theta & \cos \gamma \sin \Psi +\sin \gamma \cos \Psi \sin \theta & -\sin \gamma \cos \theta \\ -\sin \Psi \cos \theta & \cos \Psi \cos \theta & \sin \theta \\ \sin \gamma \cos \Psi +\cos \gamma \sin \psi \sin \theta & \sin \gamma \sin \psi -\cos \gamma \cos \psi \sin \theta & \cos \gamma \cos \theta \end{array}\right]$设：$C_n^b=\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{23}\\ T_{13}& T_{23}& T_{33}\end{array}\right]$则坐标转换矩阵与姿态四元数Q(t_k)的关系为：$\left\{\begin{array}{c}\left|q_1\right|=\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}\\ \left|q_2\right|=\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}\\ \left|q_3\right|=\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}\\ \left|q_4\right|=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\end{array}\right.$其中，Q(t_k)=[q₀  q₁  q₂  q₃]^T，sign(q₀)的符号可任选，q₁,q₂,q₃的符号按下式确定：$\left\{\begin{array}{c}\mathrm{sign}\left(q_1\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{32}-T_{23})\right]\\ \mathrm{sign}\left(q_2\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{13}-T_{31})\right]\\ \mathrm{sign}\left(q_3\right)=\mathrm{sign}\left(q_0\right)\left[\mathrm{sign}(T_{21}-T_{12})\right]\end{array}\right.$由此可以由当前的姿态角att_k计算当前的姿态四元数Q(t_k)；步骤S4、姿态变化四元数q与旋转矢量的关系和计算步骤S2中得到的旋转矢量对应的姿态变化四元数q，其中，表示转过的角度，u表示旋转瞬轴和方向；步骤S5、根据更新得到k+1时刻的姿态四元数Q(t_k+h)，其中，q(h)为(t_k)时刻至(t_k+h)时刻对应的坐标系姿态变化四元数，即步骤S4得到的姿态变化四元数q；步骤S6、根据步骤S3中姿态变化四元数与坐标转换矩阵的关系以及坐标转换矩阵与姿态角的关系，计算出k+1时刻的姿态角att_k，完成一次姿态更新；步骤S7、重复步骤S1至步骤S6即实现了姿态更新。