PSINS学习笔记---姿态解算(1)---圆锥运动

2023-05-16

PSINS堪称中国导航领域的福音了,计划将工具箱中常用于工程实际中的相关算法根据个人理解做个解读注释,并且利用严老师网站中公开的数据集进行测试,由于个人水平有限,料想会漏洞百出,希望大家发现了不吝赐教，感谢！

1. 加载必需环境

为避免不必要的麻烦，matlab工作环境尽量保证英文目录，运行所有demo第一步都要先运行psins\psinsinit.m将常用的全局变量添加到工作空间中。

2.1 圆锥运动模型

学过导航的肯定都听过圆锥运动了，相比于多项式表示的运动角速度，圆锥运动更接近高振动环境中的陀螺仪输出规律。一般也认为正弦角运动比多项式角运动更恶劣,能激励出更大的不可交换误差，一些论文甚至将圆锥运动产生的误差等效成不可交换误差。运动角速度公式如下：

$\omega \left ( t \right )=\left [ a\Omega sin(\Omega t) \, \, b\Omega cos(\Omega t) \,\, c\right ]$

为了方便使用四元数来研究圆锥运动，假设b系相对于n系旋转对应的变换单位四元数为：

$Q(t)=\begin{bmatrix} cos(\phi /2)\\ sin(\phi /2) cos\Omega t\\ sin(\phi /2) sin\Omega t\\ 0 \end{bmatrix}\rightarrow (1)$

下面验证该单位四元数和圆锥运动角速度表现形式的一致性：

首先最先应该想到的就是四元数微分方程了，这样就能建立角速度和四元数的关系：

$\dot{Q_{b}^{i}}=\frac{1}{2}Q_{b}^{i}\circ \omega _{ib}^{b}\rightarrow (2)$

将(1)对t求导带入到(2)中，可以求出角速度表达式如下：

$\omega (t)=2Q^{_{}^{*}}(t)\circ \dot{Q}(t)=2M_{}^{'}Q^{_{}^{*}}=\Omega sin\phi \begin{pmatrix} -sin\Omega t\\ cos\Omega t\\ -tan(\phi /2) \end{pmatrix}$

可以看出，由单位四元数反推出的角速度和圆锥运动角速度公式表现形式是一致的。

根据单位四元数和旋转矢量的关系式也可以推出等效旋转矢量，其中u为单位长度的旋转轴，phi为旋转角度：

$Q=cos(\phi /2)+\textbf{u}sin(\phi /2)$

值得注意的是：只有在圆锥运动条件下，角速度、四元数、等效旋转矢量的表达式如此简单

下面看一下圆锥运动到底是怎么运动的：

圆锥运动模型位于psins\demos\demo_cone_motion.m下，运行后可以看到三轴陀螺仪各轴的运动情况，PSINS中规定正方向都是东(x)北(y)天(z)，在图中可以看出，z轴在空间中画出圆锥面（圆锥角为phi），而xy轴端点是在球面画八字形，相位差了90°（sin和cos的相位差）。

2.2 代码梳理

公式已经知道了，下面看一下圆锥运动是怎么画出来的呢？

原理很简单，知道了单位四元数表示的坐标系的旋转关系（与t有关），假设原坐标系是单位矩阵，用四元数去旋转矩阵，每个周期变化t即可。

afa = 30.0*glv.deg;     % half-apex angle用弧度表示,matlab和c库中的sin,cos输入参数类型都是弧度
f = 1;  w = 2*pi*f;     % coning frequency振动频率
ts = 0.05;              % sampling interval采样周期
ki = 1;
while 1
    t = ki*ts;
    qnb = [cos(afa/2); sin(afa/2)*cos(w*t); sin(afa/2)*sin(w*t); 0]; % quaternion true value
    qbn = qconj(qnb);%求伴随
    % 旋转单位矩阵
    x = qmulv(qbn,[1;0;0]);    y = qmulv(qbn,[0;1;0]);    z = qmulv(qbn,[0;0;1]);
    if t<2*pi/f % record first cycle
        xk(ki,:) = x'; yk(ki,:) = y'; zk(ki,:) = z';
    end
end

3.1 圆锥误差补偿算法

圆锥运动四元数更新方程如下：

$Q(t_{m})=Q(t_{m-1})\circ Q(T) \qquad T=t_{m}-t_{m-1}$

如此可以反解出四元数增量表达式（换算过程用到了和差化积）：

$Q(T)=Q_{}^{*}(t_{m-1})\circ Q(t_{m})=M_{Q_{}^{*}(t_{m-1})}Q(t_{m})$

$\LARGE =\bigl(\begin{smallmatrix} 1-2(sin\frac{\phi }{2}sin\frac{\Omega T }{2})_{}^{2}\\ -sin\phi sin\frac{\Omega T}{2}sin\Omega (t_{m}-\frac{T}{2})\\ sin\phi sin\frac{\Omega T}{2}cos\Omega (t_{m}-\frac{T}{2})\\ -sin_{}^{2}\frac{\phi }{2}sin\Omega T \end{smallmatrix}\bigr)\rightarrow (3)$

有了四元数增量表达式就可以求得对应的等效旋转矢量增量：

$\large Q(T)=cos\frac{\phi T}{2}+\frac{\boldsymbol{\phi(T)}}{\phi (T)}sin\frac{\phi (T)}{2}\rightarrow (4)$

由于（3）和（4）等价，所以矢量部分也是相等的，

$\large \frac{\boldsymbol{\phi(T)}}{\phi (T)}sin\frac{\phi (T)}{2}=\bigl(\begin{smallmatrix} -sin\phi sin\frac{\Omega T}{2}sin\Omega (t_{m}-\frac{T}{2})\\ sin\phi sin\frac{\Omega T}{2}cos\Omega (t_{m}-\frac{T}{2})\\ -sin_{}^{2}\frac{\phi }{2}sin\Omega T \end{smallmatrix}\bigr)$

将等号两边取模值：

$\large sin\frac{\phi (T)}{2}=\sqrt{sin_{}^{2}\phi sin_{}^{2}\frac{\Omega T}{2}+sin_{}^{4}\frac{\phi}{2}sin_{}^{2}\Omega T}$

下面求等效旋转矢量增量的近似解，当等式右边的半锥角phi和omegaT为小量时：

$\large sin\frac{\phi (T)}{2}\approx \frac{\phi \Omega T}{2}$

右边是小量，那么左边也就是小量，所以等效旋转矢量phi(T)也是小量：

$\large \phi (T)\approx \phi \Omega T$

由于陀螺仪求得的是角增量而不是等效旋转矢量，角增量和等效旋转矢量的误差就是需要补偿的值，所以下面对比一下角增量和等效旋转矢量增量的区别：

$\large \boldsymbol{\phi(T)} =\bigl(\begin{smallmatrix} -sin\phi sin\frac{\Omega T}{2}sin\Omega (t_{m}-\frac{T}{2})\\ sin\phi sin\frac{\Omega T}{2}cos\Omega (t_{m}-\frac{T}{2})\\ -sin_{}^{2}\frac{\phi }{2}sin\Omega T \end{smallmatrix}\bigr)$

$\large \Delta \theta _{m}=\int_{t_{m-1}}^{t_{m}}\omega (t)dt =\bigl(\begin{smallmatrix} -sin\phi sin\frac{\Omega T}{2}sin\Omega (t_{m}-\frac{T}{2})\\ sin\phi sin\frac{\Omega T}{2}cos\Omega (t_{m}-\frac{T}{2})\\ -2sin_{}^{2}\frac{\phi }{2}\cdot \Omega T \end{smallmatrix}\bigr)$

两者只有z轴存在差异，这一差别会随着时间累积。

知道了待补偿量，怎么求解呢，常用的方法是将多子样互相进行叉乘求和。为啥是叉乘？因为当x轴正弦，y轴余弦时，叉乘后z轴正好会有个常数项，该常数项就是补偿算法提供的补偿值。

叉乘值前面的系数k一般是用matlab进行求解的，值得注意的是，当二子样时，系数为2/3与多项式形式一致，二子样算法已经可以满足绝大多数导航要求，三子样剩余误差达到10的-6次方级别，静电陀螺一般精度也就10的-4而已，所以三子样算法已经没有太多工程性的意义。

总结：常用的姿态解算方法是通过陀螺仪获得的角增量（可以是多子样），通过圆锥误差补偿算法得到等效旋转矢量（这个过程有近似），再将等效旋转矢量转换成四元数来更新姿态（这个过程无近似）。

3.2 代码梳理

未完待续

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)