state estimation 学习笔记

Dzxion

已于 2023-04-13 16:25:33 修改

阅读量763

点赞数

分类专栏： algorithm 文章标签：算法

于 2020-10-30 17:52:52 首次发布

本文链接：https://blog.csdn.net/hkkvg/article/details/109389843

版权

algorithm 专栏收录该内容

6 篇文章 1 订阅

订阅专栏

0.状态估计：状态估计目的是估计带未知扰动的确定系统的trajectory

0.1.姿态估计：估计姿态的trajectory

一个比较少见的记法： $w^{B}_{WB}$ 表示B系相对于W系的角速度，投影在B系

因为运动是相对的，总是有一个运动的相对于一个静止的，这里运动的是B系，静止的是W系

1.坐标系：在进行算法设计时，第一步就是确定坐标系，先定义几个坐标系作为基础：

在算法中需要用到的坐标系有：世界坐标系（ENU）、机体坐标系（前左上）、中间坐标系（Bodyheading）

bodyheading坐标系：x为机头朝向（与地面平行），y为朝向机头左方（与地面平行），z为上方

然后在估计算法里面估计的四元数就是 $q_{Body}^{ENU}$

这个四元数可分解为： $q_{Body}^{ENU}=q_{Bodyheading}^{ENU}\otimes q_{Body}^{Bodyheading}$

整个系统讨论的四元数都是单位四元数，只有单位四元数才能正确表示旋转和姿态，所以每次处理完四元数后都要对四元数进行单位化处理

2.旋转表示： $R_{Body}^{ENU}$ 表示Body系在ENU系的坐标表示， $q_{Body}^{ENU}$ 表示ENU系旋转到Body系，二者的关系是 $R_{Body}^{ENU}=R(q_{Body}^{ENU})$

所以 $R_{Body}^{ENU}$ 和 $q_{Body}^{ENU}$ 表示的是同一个姿态

旋转矩阵的另一种表述方式：

$R_{Body}^{World}=[x_{Body}^{World} \ y_{Body}^{World} \ z_{Body}^{World} ]$ ， $R_{Body}^{World}$ 的各列就是Body系各坐标轴在World系中的投影

$R_{World}^{Body}=[x_{World}^{Body} \ y_{World}^{Body} \ z_{World}^{Body} ]$ ， $R_{World}^{Body}$ 的各列就是World系各坐标轴在Body系中的投影

3.矢量观测：通过观测重力向量和地磁向量在机体系的值来确定飞行器的姿态

反重力向量： $a^{ENU}=[0 \ 0 \ 1]^T$ ，地磁向量： $m^{ENU}=[0 \ m_y^{ENU} \ m_z^{ENU}]^T$

这里写一下旋转矩阵的性质：

1.转置=逆一般矩阵不具备这样的特性： $R_A^B=(R_B^A)^T$ 四元数： $q_A^B=(q_B^A)^*$

2.旋转矩阵*旋转矩阵后还是一个旋转矩阵

4.加速度传感器模型： $a^{Body}=R^{Body}_{ENU}(\dot{v}^{ENU}-g)$ ，其中 $\dot{v}$ 表示机体系的速度， $g=[0 \ 0 \ -9.8m/s^2]^T$

单传感器测量方法：如何测量姿态？

5.陀螺仪测量姿态：陀螺仪测量的是机体角速度，他和四元数之间是“积分”的关系，陀螺仪描述的就是“旋转”。

抽象成数学问题，表述如下：

已知 $t_{k-1}$ 时刻的四元数 ${q}_{Body(t_{k-1})}^{ENU}$ ，采样时间 $\Delta t=t_k-t_{k-1}$ ， $t_k$ 时刻的机体角速度 ${\omega}^{Body(t_k)}$ ，现要求 $t_k$ 时刻的四元数 ${q}_{Body(t_k)}^{ENU}$

可知四元数微分方程为： $\dot{q}_{Body(t_k)}^{ENU}=\frac{1}{2} {q}_{Body(t_{k-1})}^{ENU} \otimes {\omega}^{Body(t_k)}$ ，还有其逆 $\dot{q}_{ENU}^{Body(t_k)}=-\frac{1}{2} {\omega}^{Body(t_k)} \otimes q_{ENU}^{Body(t_k)}$

写成矩阵形式的微分方程为： $\dot{q}_{Body(t_k)}^{ENU}=\frac{1}{2} \left[ \begin{matrix} 0 & -(w^{Body(t_k)})^T \\ w^{Body(t_k)} & -[w^{Body(t_k)}]_{\times} \end{matrix} \right] {q}_{Body(t_{k-1})}^{ENU}$ ，具体展开就是 $\dot{q}_{Body(t_k)}^{ENU}=\frac{1}{2} \left[ \begin{matrix} 0 & -w^{Body(t_k)}_x & -w^{Body(t_k)}_y & -w^{Body(t_k)}_z\\ w^{Body(t_k)}_x & 0 & w^{Body(t_k)}_z & -w^{Body(t_k)}_y\\ w^{Body(t_k)}_y & -w^{Body(t_k)}_z & 0 & w^{Body(t_k)}_x\\ w^{Body(t_k)}_z & w^{Body(t_k)}_y & -w^{Body(t_k)}_x & 0 \end{matrix} \right] {q}_{Body(t_{k-1})}^{ENU}$

离散则有 ${q}_{Body(t_k)}^{ENU}={q}_{Body(t_{k-1})}^{ENU}+{\dot{q}}_{Body(t_k)}^{ENU}\Delta t$

加速度计测量姿态：加速度计测量的是比力，就是合外力减去重力后的整体加速度

这里要注意的是，加速度计只能测量 $q_{Body}^{Bodyheading}$ （roll，pitch分量），要保证飞机不炸鸡就要融合好陀螺仪和加速度计啦

测量原理： $R_{ENU}^{Body}a^{ENU}=a^{Body}$ ，可得 $a^{Body}= \left[ \begin{matrix} -sin\theta \\ cos\theta sin\phi \\ cos\theta cos\phi \end{matrix} \right]$

实际上这里隐藏一个重要的结论：加速度的测量向量等于反重力向量（在小加速度前提下）

则有： $pitch=arcsin(-a^{Body}_x)$ ， $roll=arctan(\frac{a^{Body}_y}{a^{Body}_z})$

6.欧拉角->旋转矩阵：欧拉角的定义是相对旋转的z-y-x，yaw是 $\psi$ ，pitch是 $\theta$ ，roll是 $\phi$

采用相对变换的描述： $R_{Body}^{ENU}=R(\psi)R(\theta)R(\phi)$

各个轴的旋转矩阵： $R(\psi)= \left[ \begin{matrix} cos\psi & - sin\psi & 0 \\ sin\psi & cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right]$ 、 $R(\theta)= \left[ \begin{matrix} cos\theta & 0 & sin\theta \\ 0 & 1 & 0 \\ -sin\theta & 0 & cos\theta \end{matrix} \right]$ 、 $R(\phi)= \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\phi & -sin\phi \\ 0 & sin\phi & cos\phi \end{matrix} \right]$

ENU->Body： $R_{Body}^{ENU}= R(\psi)R(\theta)R(\phi)= \left[ \begin{matrix} cos\theta cos\psi & cos\psi sin\theta sin\phi - sin\psi cos\phi & cos\psi sin\theta cos\phi + sin\psi sin\phi \\ cos\theta sin\psi & sin\psi sin\theta sin\phi + cos\psi cos\phi & sin\psi sin\theta cos\phi - cos\psi sin\phi \\ -sin\theta & sin\phi cos\theta & cos\phi cos\theta \end{matrix} \right]$

Bodyheading->Body： $R_{Body}^{Bodyheading}=R(\theta)R(\phi)= \left[ \begin{matrix} cos\theta & sin\theta sin\phi & cos\phi sin\theta \\ 0 & cos\phi & - sin\phi \\ -sin\theta & sin\phi cos\theta & cos\phi cos\theta \end{matrix} \right]$

7.四元数->旋转矩阵：

$R_{Body}^{ENU}= \left[ \begin{matrix} q_w^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y-q_w q_z) & 2(q_w q_y+q_x q_z) \\ 2(q_w q_z+q_x q_y) & q_w^2-q_x^2+q_y^2-q_z^2 & 2(q_y q_z-q_w q_x) \\ 2(q_x q_z-q_w q_y) & 2(q_w q_x+q_y q_z) & q_w^2-q_x^2-q_y^2+q_z^2 \end{matrix} \right]$

8.四元数->欧拉角：结合上面的公式不难得出

$pitch = asin( 2*(q_w*q_y-q_x*q_z) )$

$roll = atan(\frac{2(q_w*q_x+q_y*q_z)}{1-2*(q_x^2+q_y^2)})$

$yaw = atan(\frac{2(q_w*q_z+q_x*q_y)}{1-2*(q_y^2+q_z^2)})$

9.欧拉角->四元数：

$q^{ENU}_{Body}=q_z(\psi)\otimes q_y(\theta)\otimes q_x(\phi)$ ，其中 $q_x(\phi)=[cos\frac{\phi}{2} \ sin\frac{\phi}{2} \ 0 \0]^T$ ， $q_y(\theta)=[cos\frac{\theta}{2} \ 0 \ sin\frac{\theta}{2} \0]^T$ ， $q_z(\psi)=[cos\frac{\psi}{2} \ 0 \ 0 \sin\frac{\psi}{2}]^T$

$q_{Body}^{ENU}= \left[ \begin{matrix} cos\frac{\phi}{2} cos\frac{\theta}{2} cos\frac{\psi}{2} + sin\frac{\phi}{2} sin\frac{\theta}{2} sin\frac{\psi}{2} \\ sin\frac{\phi}{2} cos\frac{\theta}{2} cos\frac{\psi}{2} - cos\frac{\phi}{2} sin\frac{\theta}{2} sin\frac{\psi}{2} \\ cos\frac{\phi}{2} sin\frac{\theta}{2} cos\frac{\psi}{2} + sin\frac{\phi}{2} cos\frac{\theta}{2} sin\frac{\psi}{2} \\ cos\frac{\phi}{2} cos\frac{\theta}{2} sin\frac{\psi}{2} - sin\frac{\phi}{2} sin\frac{\theta}{2} cos\frac{\psi}{2} \end{matrix} \right]$

10.轴角->四元数：

设轴角 $u=\theta v$ ，其中 $\theta$ 是旋转角， $v$ 是单位旋转轴，对应四元数 $q=[cos\frac{\theta}{2} \ \sin\frac{\theta}{2}v^T]^T$

11.磁力计测量原理：磁力计零偏超级超级大，不校准原始数据根本不能用

磁力计测量的机体系向量： $m^{Body}=[m^{Body}_x \ m^{Body}_y \ m^{Body}_z]^T$

将地磁向量从机体系转换到Bodyheading： $m^{Bodyheading}=R_{Body}^{Bodyheading}m^{Body}$

可求出Bodyheading系的地磁向量为： $m^{Bodyheading}= \left[ \begin{matrix} m^b_x cos\theta + m^b_y sin\phi sin\theta + m^b_z cos\phi sin\theta \\ m^b_y cos\phi - m^b_z sin\phi \\ -m^b_x sin\theta + m^b_y sin\phi cos\theta + m^b_z cos\phi cos\theta \end{matrix} \right]$

可求出heading与磁北的夹角为： $\psi_{mag}=arctan(\frac{m^{Bodyheading}_y}{m^{Bodyheading}_x})$

12.反对称矩阵： $[a]_{\times}= \left[ \begin{matrix} 0 & - a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{matrix} \right]$

13.小角度的旋转矩阵： $R_{Body}^{ENU}= I+\left[ \begin{matrix} 0 & - a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{matrix} \right]$

其中 $a_x$ 、 $a_y$ 、 $a_z$ 分别是roll、pitch、yaw的小角度

14.欧拉角微分方程： $\left[ \begin{matrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{matrix} \right]= \left[ \begin{matrix} 1 & tan\theta sin\phi & tan\theta cos\phi \\ 0 & cos\phi & -sin\phi \\ 0 & sin\phi / cos\theta & cos\phi / cos\theta \end{matrix} \right] \left[ \begin{matrix} w_{x_b} \\ w_{y_b} \\ w_{z_b} \end{matrix} \right]$

15.欧拉角取值范围：

roll：-180-180；pitch：-90-90；yaw：-180-180

实际飞行当中，roll和pitch的范围都是30度之内，yaw是全范围都有覆盖

16.当旋转顺序是z-y-x的时候，欧拉角和飞机的姿态角定义重合

17.四元数每次操作后都要单位化

18.旋转的积分是乘法

19.叉乘： $a \times b = \left[ \begin{matrix} a_2*b_3 - a_3*b_2 \\ a_3*b_1 - a_1*b_3 \\ a_1*b_2 - a_2*b_1 \end{matrix} \right]$

Dzxion

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
state estimation 学习笔记

1.坐标系：在进行算法设计时，第一步就是确定坐标系，一般来说，要确定的有两个坐标系，一个是世界坐标系，一个是机体坐标系。在读shen的博士论文时，发现论文里面并没有明确说明世界坐标系具体的朝向，只是具体“静止”的特性，以及z轴和地面垂直这一约束。在看北航ppt时，ppt里对世界坐标系的定义是这样子的：地球固联坐标系用于研究多旋翼飞行器相对于地面的运动状态，确定机体的空间位置坐标。它忽略地球曲率，即将地球表面假设成一张平面。通常以多旋翼起飞位置或者地心作为坐标原点。先让x轴在水平面内指向某一方向
复制链接

扫一扫