物理学基础概念

左手定则

将左手的食指，中指和拇指伸直，使其在空间内相互垂直。食指方向代表磁场的方向（从N级到S级），中指代表电流的方向（从正极到负极），那拇指所指的方向就是受力的方向。可用于判断安培力（运动导体所受到的力）和洛伦兹力（运动电荷所受的力）。

概述

openshoe是一种通过惯性测量元件(IMU)来对运动进行积分以得到路径的控制算法。算法的核心是ZUPT(零速度更新算法)，在只有IMU惯性测量元件的情况下获得路径只能通过对加速度积分得到速度，再将速度对时间进行积分得到路径，这里如果不能对积分得到的速度进行有效的校正那么这个速度误差会时刻对时间进行积分，导致路径完全失效，因此必须采取有效手段及时对速度误差进行校正以得到较为准确的路径信息，ZUPT就是一种基于检测零速度进行路径积分校正的算法。该算法的关键是对零速度的准确检测。

零速度检测算法

openshoe采用广义似然检测算法对零速度进行检测，以判断IMU是否处于静止状态。

广义似然检测原理

要理解广义似然检测原理需要先熟悉几个概率论中相关的概念，详细如下：

传感器误差模型

对于理想的IMU三轴加速度计两两正交，构成一个正交的三轴直角坐标系，加速度计每一轴单独测量该轴的加速度，而陀螺仪则测量该轴的角速度。在实际的真实IMU中由于制造工艺的误差，三个坐标轴不可能完全两两正交，加速度计与陀螺仪的坐标系也不会完全重合，并且单个传感器也不是完全精确的。在实际器件中将数字输出量转化为实际物理量的scale参数在不同轴上是不同的，但是设备生产商都会提供一个默认的scale参数用于转换所有轴的数据，而且数字量的输出还会受到零偏（传感器在静止情况下也会有微小量的输出）的影响，这些就是造成IMU传感器的系统误差。如下图所示：

我们取实际器件的加速度计坐标系为AF,陀螺仪坐标系为GF，根据AF和GF分别建立对应的正交坐标系AOF和GOF，其建立约束为

• AOF的x轴与AF的x轴重合
• AOF的y轴位于AF的x与y轴的平面中

对于GOF的建立约束与AOF的约束类比建立。最后再建立一个正交机体坐标系BF，BF通常与AF和GF之间有一个小角度的偏差。在非正交坐标系（AF或GF）中测量得到的物理量\(s^S\)可以转换到机体坐标系BF中得到\(s^B\)于是得到下式： \[ s^B = Ts^S, T = \begin{bmatrix} 1 & -\beta_{yz} & \beta_{zy} \\ \beta_{xz} & 1 & -\beta_{zx} \\ -\beta_{xy} & \beta_{yx} & 1 \end{bmatrix} \tag{1} \] \(s^B和s^S\) 表示加速度或角速度在机体坐标系BF和加速度坐标系AF或陀螺仪坐标系GF下测量表示量，\(\beta_{ij}\) 表示加速度或陀螺仪的i轴绕机体坐标系BF的j轴的旋转角度。如图二所示：

imu通过加速度计解算姿态角

通过三角函数可以将加速度计三个轴的角速度解算为姿态角，其中 \(\alpha\) , \(\beta\) 和 \(\gamma\)（ \(\gamma\) 是z轴与重力加速度之间的夹角）与三个轴之间的关系如上图所示：

\[ \begin{aligned} &\beta = arcsin(\frac{a_x}{g}) \\ &\gamma = arcsin(\frac{a_y}{g}) \end{aligned} \]

其中重力加速度g的取值使用三轴角速度的矢量和即：

\[ g = \sqrt{a_x^{2} + a_y^{2} + a_z^{2}} \]

将g的值带入上式可以得到：

\[ \begin{aligned} &\beta = arctan(\frac{a_x}{\sqrt{a_y^{2} + a_z^{2}}}) \\ &\gamma = arctan(\frac{a_y}{\sqrt{a_x^{2} + a_z^{2}}}) \end{aligned} \]

其中α为俯仰角pitch，β为滚转角roll，其中航向角是没有办法通过加速度计来计算的。

优先级调度算法

概述

嵌入式实时操作系统对于task调度有着严格的时间要求，即在不同优先级task之间的调度时间应该保持一致，但是普通查表法会从头开始遍历数组，这样就绪态最高优先级的task会比就绪态最高优先级低的task先被查找到，造成不同优先级task在调度上花费时间不一致的情况。为了可以在相同时间里查找到最高就绪态优先级task，ucos采用了空间换取时间的算法在相同时间里查找到优先级最高task，具体算法如下。