算法、代码与测试（完）

OK，经过一系列的改代码与测试，这一代的惯性导航系统最终完成了。在继续前，先说几件事。首先jy901b的性能即便榨干了，但是作为低成本MEMS，它的误差还是很大的，但是我和辰星梦人讨论的结果是这一代先用着，对于千米以下的探空火箭是完全可以用的，以后再升级。它的误差很难标定，我只能给几个实验的结果，具体原因后面会说，现在预估在500m级探空火箭上定位会在最高点产生10m左右的位置误差和1m/s左右的速度误差，但是具体要等试验。其次这个专栏类似于记录，所以我会完全开源，但是关于具体的解释就不会说了，只是简单解释一些重要的地方。主要原因是过几天期末，没啥时间。我会在把整个飞控搞完之后完全开源并写一个移植的指南，大概一月底？最后，这个飞控将会被安装再水火箭和辰星梦人的500m级试验探空火箭上测试。

好的，开始说下算法。惯性导航的算法是先解姿态，再解速度，最后解位置。其中姿态的算法是维特智能商业闭源的，我也不知道，所以不详细说了。接下来是速度算法，速度算法参考以下两个文献，一个是秦永元先生的惯性导航，另一个就是杨浩天先生，汪立新先生，沈强先生的论文——一种改进的三子样划桨误差补偿算法。

一种改进的三子样划桨误差补偿算法_杨浩天.pdf990.21KBPDF9次下载预览

这个算法的推导可以看书，我只简单地把公式打上来。

$v_m = v_{m-1}+C_{m-1}\Delta v_{sfm} + \Delta v_{g/corn}$（1）

（1）式左侧是第m次更新后的箭体速度矢量，右侧三项分别是第m-1次更新后箭体的速度矢量，箭体的姿态矩阵（箭体转到地面）左乘根据比力（或者说测得的加速度）算出的速度补偿矢量，重力加速度、惯性离心力、科里奥利力等引起的速度增量。

左侧和右侧第一项好理解，略。

重力加速度、惯性离心力、科里奥利力等引起的速度增量中只考虑了重力，因为在探空火箭速度和射高较小的时候，后二者远不如低端MEMS的噪声，而且毕竟射高小，重力加速度视为恒定。

即：

$\Delta v_{g/corn} = \begin{pmatrix}0\\0\\-g\Delta t\end{pmatrix}$（2）

jy901b会传回归一化四元数和欧拉角，姿态矩阵可以根据欧拉角或四元数来确定，一般用四元数，以下是公式。

$C = \begin{pmatrix}1-2(q_2^2+q_3^2)&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\\2(q_1q_2+q_0q_3)&1-2(q_1^2+q_3^2)&2(q_2q_3-q_0q_1)\\2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&1-2(q_1^2+q_2^2)\end{pmatrix}$（3）

然后重点就是速度补偿矢量。

有：

$\Delta v_{sfm} = \Delta v_m + \Delta v_{rotm}+\Delta v_{sculm}$（4）

右侧三项分别是测得的加速度带来的速度增量（也就是箭体平动的影响），转动补偿（箭体转动的影响），划桨效应补偿（平动和转动的混合效应，就像划桨那样的运动会带来的额外误差）。

前两项较为简单，有以下的式子，推导见书：

$\Delta v_m = \int_{t_{m-1}}^{t_m}adt$（5）（这个a是测得的加速度，书上用比力f，我还是习惯用a）

$\Delta \theta = \int_{t_{m-1}}^{t_m}\omega dt$ （6）

$\Delta v_{rotm} = \frac{1}{2}\Delta \theta \times\Delta v_m$（7）

划桨效应非常复杂，算法也非常多，而且现在的研究也没有非常成熟，不过这个传感器不行，过于精确的算法也没用，所以我就找了个简单且相对精确的，优化的三子样算法。如果图简便的话，可以用书上的三子样算法。

先说下采样问题，这个算法需要在一个更新周期内多次采样，当然由于这个传感器也就200Hz回传，所以没办法，只能把5ms作为一个采样周期，然后选择三子样算法，15ms作为一个更新周期，当然，书上有实验结论，这样比5ms更新的一子样算法还是要精确不少的。然后这个算法太过复杂，我直接截论文里的图。

（8）

括号里是几代表第几次采样，然后后面的格式和本文的不一样后面的vm实际上是这一次速度更新的总速度增量相当于Δvm，而vm-1是上一次更新的总速度增量，vm-2是上上次，角度增量也如此。

然后四个参数的值：

（9）

推导见论文。

那么速度更新完了，接下来是位置更新，这个其实很复杂，还涉及什么经纬度的，但是考虑到应用场景，只需要简单地对速度积分便可。

那么直接上工程。

XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

太大了，就不发上来了，可以直接在gitee上克隆或者下载zip文件。

只说几个关键的地方，首先这块pcb有两个串口接口，靠近电解电容那个是直接连接jy901b的，要先用上位机校准磁场，并设置200hz回传和230400的串口波特率（最大了，其他也能用但是精度会更低），另一个串口是可以接到电脑的，默认是230400波特率下输出几乎全部数据，可以在代码里改，校准加速度计的时候就要在JY901B.c文件中注释掉用加速度校准那里，再解除直接读取的注释，然后再main.c里输出加速度。那个tf卡模块是记录所有串口数据的，tf卡要先以4096B为单元格式化，然后第一次上电会生成一个文件，config.txt，只需要打开波特率改成230400就行了，虽然最好是1152000，但是实测超过230400这个模块就不行了。算法部分就是上面写的，只是写成c语言了。

好的最后说下测试。因为这个MEMS的精度比较低，实在不能用纯惯导的方式校准，而工程上常用MEMS在获取姿态方面，所以也没有后续的流程，也不好标定。

所以我做了一些简单的测试。在校准好的情况下，有一面向上的姿态下漂移是最小的，实际发射的时候请让它向上，因为火箭上升段姿态较为稳定。按照10s来测试（这一次火箭500m射高），静止漂移约5m，保持姿态缓慢地做往复运动漂移约10m，拿着它在房间里走一圈，画八字，自由落体，漂移约15m，较快速度大范围往复运动漂移约25m，剧烈运动且姿态偏移较大漂移约50m。可以参考，但是不同传感器不同校准结果可能不同，具体情况要在参考GPS，安装在水火箭和固体燃料火箭测试之后才能得知。

当然还有些别的办法可以提升精度，比如校准加入二次项，对姿态也进行校准，以及一些歪门邪道，比如发现最后总是往某个方向偏就直接在那个方向减一点。不过太麻烦了，我不打算做，最多是做最后一个邪修（）

最后的最后说下成本，jy901s 103元，stm32f103c8t6 17元，换成8块的c6t6也行，GYopenlog的tf卡模块10元，其他零件加起来不会超过1元但是不能单买，板子白嫖嘉立创的，校准盒委托辰星梦人绘制后用自行购买的耗材白嫖学校的打印机打印。实际成本不会超过140。

（好了这个系列到此结束，接下来是飞控的整体开发和未来的更好更精确的惯性导航，不过明天得复习高数了，考完期末见）