惯导机械编排——等效旋转矢量公式推导

最新推荐文章于 2024-11-05 10:04:47 发布

学习代码的小赵

最新推荐文章于 2024-11-05 10:04:47 发布

阅读量892

点赞数 15

分类专栏：惯性导航文章标签：笔记

本文链接：https://blog.csdn.net/JIEAOGUOFEN/article/details/140753694

版权

惯性导航专栏收录该内容

2 篇文章 0 订阅

订阅专栏

特别鸣谢i2nav导航团队，感谢武汉大学牛小骥老师和陈起金老师，下文所述内容均学习自牛小骥老师在2022年秋季惯性导航课程合集：武汉大学惯性导航课程合集【2021年秋】_哔哩哔哩_bilibilihttps://www.bilibili.com/video/BV1nR4y1E7Yj/?spm_id_from=333.337.search-card.all.click 学习课件也来自i2nav团队：

武汉大学多源智能导航实验室 (i2nav.com)http://www.i2nav.com/index/newListDetail_zw.do?newskind_id=13a8654e060c40c69e5f3d4c13069078&newsinfo_id=2d6a2bbbd25640cbb6fbf685554ae939 在此处针对当时学习等效旋转矢量的一些困扰做一个学习总结。

一、旋转的不可交换性

1.1 陀螺的不可交换性与惯性导航之间的联系

一、旋转的不可交换性

1.1 陀螺的不可交换性与惯性导航之间的联系

涉及到陀螺仪的具体设计原理：陀螺在输出角增量时只对各轴的角度变化量做了一个数值累加，但是并没有考虑角速度 $\omega_{ib}$ 在采样间隔之间的方向变化，所以导致陀螺输出的角增量只是单纯的角度变化，没有明确的物理含义。

而单纯的角增量没有办法确定一个物体的最终姿态，因为旋转存在“不可交换性”。

1.2 不可交换性的理论认知

在力学和运动学中，刚体的有限次独立转动会形成独一无二的姿态结果，这种连续转动是不可交换的，表现在互相交换旋转的次序会导致最后的姿态产生较大的差异。旋转的这一特性决定了旋转不是一个单纯的矢量，不能够以矢量的角度去看待物体姿态的变化，两次及两次以上的不同轴的转动不能够简单相加。

举个例子：

通过上图我们可以发现：仅仅是两次旋转过程，从同一个初始状态出发，当调换两次旋转的次序之后最终会导致姿态产生明显差异。

通过以上两个例子，希望大家明白：仅仅根据陀螺输出的角增量来判断实际的姿态变化是不可能的，姿态的变化具有不可交换性（正如上面两个例子的角增量完全一致，但是旋转次序不一样导致最后的姿态不一致）。想要得到最终的姿态，不仅需要获得角度的变化量，还需要知道具体的旋转过程是怎样的，这样才可以确定物体最终的姿态。所以：

对一个空间方向随时间变化的角速度矢量进行积分是没有物理意义的。

那么究竟怎么凭借角增量来建立一个独一无二的限制体系，这是解算姿态必须要考虑的一个问题。基于此需要引入“等效旋转矢量”，等效旋转矢量会对case1和case2进行一个区分，使得每次转动都是确定的。

二、等效旋转矢量

2.1 理论基础

一个坐标系到另一个坐标系的变换可以通过多次定轴转动来完成，也可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现（就是可以绕原定坐标轴x,y,z转多次来实现到目标坐标系的旋转，也可以绕空间中一个特定的向量作为轴转动一次达到相同的旋转效果）。

该矢量称作等效旋转矢量（rotationvector）是一个三元素的向量，旋转矢量的方向给出了转动轴的方向，它的模长为转动角度的大小，又称为轴角（axis-angle）

等效旋转矢量记作： $\boldsymbol{\phi }_{Rb}=\theta u,\textcolor[rgb]{0.00,0.50,1.00}{\phi=||\boldsymbol{\phi }_{Rb}||=\theta }$ 。其中 $\theta$ 表示旋转角度，即轴角； $u$ 表示旋转轴的方向向量。

那究竟如何通过陀螺输出的角增量来构造出等效旋转矢量呢？在这里需要引入Bortz 方程。

2.2 Bortz 方程

1969年，John E. Bortz在其博士论文中详细推导了等效旋转矢量微分方程（Bortz 方程），该方程是利用陀螺输出求解等效旋转矢量的基本公式，奠定了等效旋转矢量多子样算法的理论基础。

在这里不做推导，咱直接用。Bortz 方程如下：

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}+\frac12(\boldsymbol{\phi}\times\boldsymbol{\omega})+\frac1{\boldsymbol{\phi}^2}\left[1-\frac{\phi\sin\phi}{2(1-\cos\phi)}\right]\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})\\$

$\boldsymbol{\phi}\triangleq\boldsymbol{\phi}_{Rb}, \boldsymbol{\omega}\triangleq\boldsymbol{\omega}_{Rb}, \boldsymbol{\phi}(0)=\boldsymbol{0}$

等效旋转矢量常用于表示姿态的变化量；
Bortz 方程等式右边两项补偿了角速度向量的方向变化。

2.3 等效旋转矢量微分方程的工程近似

在计算机处理时，不能够处理连续数据，必须对Bortz 方程进行一些工程近似的处理。

首先在这里可以利用倍角公式对 $\dfrac{\sin\phi}{(1-\cos\phi)}$ 进行一个形式上的变化：

$\dfrac{\sin\phi}{(1-\cos\phi)}=\dfrac{2\sin\frac{\phi}{2}\cos\frac{\phi}{2}}{2\sin^2\frac{\phi}{2}}=\cot\dfrac{\phi}{2}$

可以将Bortz 方程转变为如下形式：

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}+\frac12(\boldsymbol{\phi}\times\boldsymbol{\omega})+\frac1{\boldsymbol{\phi}^2} \left[1-\dfrac{\phi}{2}\cot\dfrac{\phi}{2}\right] \boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})\\$

根据 $\cot x$ 的泰勒展开形式： $\cot x=\frac1x-\frac x3-\frac{x^3}{45}-\frac{2x^5}{945}-\cdots$

Bortz 函数可以进一步写作：

$\begin{aligned}\dot{\boldsymbol{\phi}}&=\boldsymbol{\omega}+\frac{1}{2}\boldsymbol{\phi}\times\boldsymbol{\omega}+\frac{1}{\phi^{2}}\Big(1-\frac{\phi}{2}\cot\frac{\phi}{2}\Big)\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})\\&=\boldsymbol{\omega}+\frac{1}{2}\boldsymbol{\phi}\times\boldsymbol{\omega}+\frac{1}{\phi^{2}}\bigg[1-\frac{\phi}{2}\bigg(\frac{2}{\phi}+\frac{1}{3}\cdot\frac{\phi}{2}-\cdots\bigg)\bigg]\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})\\&=\boldsymbol{\omega}+\frac12\boldsymbol{\phi}\times\boldsymbol{\omega}+\frac1{12}\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})+\dots\end{aligned}$

接下来进行三步近似处理，进一步简化Bortz 方程：

近似处理1：当旋转矢量为微小量时，bortz 方程可以仅保留至级数第二项；

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}+\dfrac12\boldsymbol{\phi}\times\boldsymbol{\omega}+\dfrac1{12}\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})$

近似处理2：当旋转矢量为微小量时，可以忽略旋转矢量的二阶小量 $\frac1{12}\boldsymbol{\phi}\times(\boldsymbol{\phi}\times\boldsymbol{\omega})$ ；

$\dot{\boldsymbol{\phi}}=\boldsymbol{\omega}+\dfrac12\boldsymbol{\phi}\times\boldsymbol{\omega}$

近似处理3：当旋转矢量为微小量时，其与角度增量的差异会很微小，为二阶小量可以忽略不计,所以可以将等号右侧的等效旋转矢量 $\boldsymbol{\phi}$ 替换为角度增量 $\Delta\boldsymbol{\theta}(t)$ ，这样可以避免未知数 $\boldsymbol{\phi}$ 在等式两侧陷入循环求解的困境。

$\dot{\boldsymbol{\phi}}(t)\approx\boldsymbol{\omega}(t)+\dfrac12\Delta\boldsymbol{\theta}(t)\times\boldsymbol{\omega}(t)$

对上式等式两边同时积分，即可以获得等效旋转矢量 $\boldsymbol{\phi}(t)$ 的最终工程化表达形式：

$\begin{aligned} \boldsymbol{\phi}(t)& =\int_{t_{k-1}}^{t}\biggl(\boldsymbol{\omega}\left(t\right)+\dfrac12\Delta\boldsymbol{\theta}\left(t\right)\times\boldsymbol{\omega}\left(t\right)\biggr)dt \\ &=\int_{t_{k-1}}^{t}\boldsymbol{\omega}\left(t\right)+\frac12{\int_{t_{k-1}}^{t}\Delta\boldsymbol{\theta}(t)\times\boldsymbol{\omega}(t)dt} \end{aligned}$

而角度增量 $\Delta\boldsymbol{\theta}(t)$ 可以表示为：

$\\\Delta\boldsymbol{\theta}(t)=\int_{t_{k-1}}^{t}\boldsymbol{\omega}(\tau)d\tau, t\in[ t_{k-1}, t_{k}]$

2.4 等效旋转矢量的双子样算法

首先请大家考虑一个问题，什么叫双子样算法？

双子样算法的本质是对物体在运动过程中的角速度变化的一种假设。由于物体角速度变化的假设会显著影响到整个旋转矩阵的积分过程，所以存在对角速度向量的不同的假设：

假设角速度 $\boldsymbol{\omega}\left(t\right)$ 为关于时间的常值函数——单子样算法；
假设角速度 $\boldsymbol{\omega}\left(t\right)$ 为关于时间的线性函数——双子样算法；
假设角速度 $\boldsymbol{\omega}\left(t\right)$ 为关于时间的二次函数——三子样算法；

双子样算法就是假设角速度向量在时间 $\left[t_{k-2},t_k\right]$ 内随时间线性变化：

$\boldsymbol{\omega}(t)=\boldsymbol{a}+\boldsymbol{b}(t-t_{k-1})$

至于为啥要用 $\left[t_{k-2},t_{k-1}\right],\left[t_{k-1},t_k\right]$ 两个时段的假设，因为线性假设有两个未知数 $\boldsymbol{a}$ 和 $\boldsymbol{b}$ ，需要两组等式关系才可以建立方程求解未知数，进而获得 $\boldsymbol{\omega}(t)$ 的线性表达式。所以接下来可以根据陀螺两个角增量的输出，来求解未知数 $\boldsymbol{a}$ 和 $\boldsymbol{b}$ ： $\left( \Delta t=t_k-t_{k-1}\right)$

$\Delta\boldsymbol{\theta}_{k}=\int_{{t_{k-1}}}^{{t_{k}}}\boldsymbol{\omega}(\tau)d\tau=\int_{{t_{k-1}}}^{{t_{k}}}\boldsymbol{a}+\boldsymbol{b}\left(\tau-t_{k-1}\right)d\tau=\boldsymbol{a}\Delta t+\dfrac{1}{2}\boldsymbol{b}\Delta t^{2}$

$\Delta\boldsymbol{\theta}_{k-1}=\int_{{t_{k-2}}}^{{t_{k-1}}}\boldsymbol{\omega}(\tau)d\tau=\int_{{t_{k-2}}}^{{t_{k-1}}}\boldsymbol{a}+\boldsymbol{b}\left(\tau-t_{k-1}\right)d\tau=\boldsymbol{a}\Delta t-\dfrac{1}{2}\boldsymbol{b}\Delta t^{2}$

联立可以求解得：

$\begin{cases}a=\dfrac{\Delta\boldsymbol{\theta}_{k}+\Delta\boldsymbol{\theta}_{k-1}}{2\Delta t}\\ b=\dfrac{\Delta\boldsymbol{\theta}_{k}-\Delta\boldsymbol{\theta}_{k-1}}{\Delta t^2} \end{cases}$

将解得的未知数 $\boldsymbol{a}$ 和 $\boldsymbol{b}$ 代入角速度向量 $\boldsymbol{\omega}(t)$ 的表达式，可得 $\Delta\boldsymbol{\theta}(t)$ 的表达为：

$\begin{aligned} \Delta\boldsymbol{\theta}(t)&=\int_{t_{k-1}}^{t}\boldsymbol{\omega}(\tau)d\tau\\ &=\int_{t_{k-1}}^{t}\boldsymbol{a}+\boldsymbol{b}(\tau-t_{k-1}) d\tau\\ &=\boldsymbol{a}(t-t_{k-1})+\frac12\boldsymbol{b}(t-t_{k-1})^2\\ &= \dfrac{\Delta\boldsymbol{\theta}_{k}+\Delta\boldsymbol{\theta}_{k-1}}{2\Delta t}(t-t_{k-1})+\dfrac{\Delta\boldsymbol{\theta}_{k}-\Delta\boldsymbol{\theta}_{k-1}}{2\Delta t^2}(t-t_{k-1})^2 \\ \end{aligned}$

当 $t=t_k$ 时， $\Delta\boldsymbol{\theta}(t)= \Delta\boldsymbol{\theta}_k$ 。

再往下计算需要知道向量叉乘的几个关键定理：

$\begin{aligned} \boldsymbol{X}\times\boldsymbol{X}&=0\\ \boldsymbol{X}\times\boldsymbol{Y}&=-\boldsymbol{Y}\times\boldsymbol{X}\\ a\boldsymbol{X}\times\left( b\boldsymbol{Y}+c\boldsymbol{Z}\right )&=ab\boldsymbol{X}\times\boldsymbol{Y}+ac\boldsymbol{X}\times\boldsymbol{Z} \end{aligned}$

综上求解：

$\begin{aligned} \boldsymbol{\phi}_k\equiv\boldsymbol{\phi}(t_k)& =\int_{t_{k-1}}^{t_{k}}\biggl(\boldsymbol{\omega}\left(t\right)+\frac12\Delta\boldsymbol{\theta}\left(t\right)\times\boldsymbol{\omega}\left(t\right)\biggr)dt \\ &=\Delta\boldsymbol{\theta}_k+\frac12{\int_{t_{k-1}}^{t_k}\Delta\boldsymbol{\theta}\left(t\right)\times\boldsymbol{\omega}\left(t\right)dt} \\ &=\Delta\boldsymbol{\theta}_k+\frac12\int_{t_{k-1}}^{t_k} \left[\boldsymbol{a}(t-t_{k-1})+\frac12\boldsymbol{b}(t-t_{k-1})^2\right]\times \left[ \boldsymbol{a}+\boldsymbol{b}(t-t_{k-1}) \right ]dt\\ &=\Delta\boldsymbol{\theta}_k+\frac12\int_{t_{k-1}}^{t_k}\boldsymbol{a}\times\boldsymbol{b}(t-t_{k-1})^2+\frac12\boldsymbol{b}\times\boldsymbol{a}(t-t_{k-1})^2dt\\ &=\Delta\boldsymbol{\theta}_k+\frac12\int_{t_{k-1}}^{t_k}\frac12\boldsymbol{a}\times\boldsymbol{b}(t-t_{k-1})^2dt\\ &=\Delta\boldsymbol{\theta}_k+\frac1{12}\boldsymbol{a}\times\boldsymbol{b}\Delta t^3 \end{aligned}$

将之前求解的未知数 $\boldsymbol{a}$ 和 $\boldsymbol{b}$ 代入上式，可得：

$\begin{aligned} \boldsymbol{\phi}_k&=\Delta\boldsymbol{\theta}_k+\dfrac{1}{12}\dfrac{\Delta\boldsymbol{\theta}_{k}+\Delta\boldsymbol{\theta}_{k-1}}{2\Delta t}\times\dfrac{\Delta\boldsymbol{\theta}_{k}-\Delta\boldsymbol{\theta}_{k-1}}{\Delta t^2}\Delta t^3\\ &=\Delta\boldsymbol{\theta}_k+\dfrac{1}{24}\left( -\Delta\boldsymbol{\theta}_k\times\Delta\boldsymbol{\theta}_{k-1}+\Delta\boldsymbol{\theta}_{k-1}\times\Delta\boldsymbol{\theta}_{k}\right)\\ &=\Delta\boldsymbol{\theta}_k+\dfrac{1}{12}\Delta\boldsymbol{\theta}_{k-1}\times\Delta\boldsymbol{\theta}_{k}\end{aligned}$