第九章 PX4-pixhawk-姿态估计解析

版权声明：本文为博主原创文章，未经博主允许不得转载。

第九章 PX4-pixhawk-姿态估计解析 
这一章节我们对姿态估计进行解析，这一章节涉及到算法，主要涉及到的还是DCM(方向余弦)算法。首先我们还从启动文件开始进行讲解。 
我们找到rc.mc_apps中，我们可以找到SYS_MC_EST_GROUP的参数为0，所以我们对attitude_estimator_q.cpp进行讲解 。 
回到attitude_estimator_q.cpp文件中，这里提一下这个文件应该知道是怎么编译的吧，在cmake文件中找到的。找到入口函数attitude_estimator_q_main中。我们可以知道参数只有start，这个怎么解析的不用说了吧，前几章节都说到过了。 
这里面没有什么好说的，new这个就不用说了吧，我们找到start函数 这里就创建了任务task_main_trampoline 再找到task_main函数。 
首先还是先订阅相关数据，包括传感器、视觉、空速计、参数、定位信息并且更新参数 ，这里的更新各个参数的函数就不用多说了吧，主要是滤波权重、偏移量和模式选择等等。 
接着到wihile中，首先还是poll函数来防止阻塞，这个用法直接去度一下poll就有说明。首先进行参数更新检测 ，这个没什么好说的，如果不懂那我也没办法了哦。然后就是更新传感器数据 ，这个更新里面有加速度计和加速度计的更新，而且这里还对它们进行了一个二阶低通滤波，这种滤波器比较普遍也比较容易，有兴趣的同学去研究一下，不懂得邮件我也行，如果大家有更高明的滤波算法也可以分享一下，我也可以代劳来编写一下。接着对磁力计更新。接下来的几个更新我们不做细去说明。视觉更新动作捕捉更新 空速计数据更新 GPS数据更新 这里的GPS的更新数据主要还是计算磁偏角用来修正电子罗盘用的，这个的计算主要是利用经纬度通过查表发进行查找磁偏角。接着就是利用从GPS中获取的NED坐标系的速度来计算加速度 这里有个利用四元素来进行坐标系的转换_q.conjugate_inversed这个就是将数据转换成机体坐标系中，至于这个函数的如何去解析，我们待会去一起解析。 
然后就是进入到最重要的函数update，这个函数就涉及到姿态解算了。我们进入到这个函数中 。函数开始会对是否初始化进行判断，如果是第一次运行就会运行init函数，运行一次后就不会运行了。这里的初始化主要是构建旋转矩阵，然后通过旋转矩阵转换为四元素。这里主要是运用了较多的矢量运算和旋转矩阵的一些知识。 
首先要做的是加速率的偏移调整，这里我们不用视觉和动作捕捉，所以不对它们进行讲解，这些主要是对航向角进行修正。接下来是利用电子罗盘数据进行航向修正，这里的到的修正量是yaw。 
 
这里讲一下_q.conjugate函数这里是坐标系的转换也就是b系转R系也就是机体坐标系转大地坐标系，这里的_q是四元素，首先我们看看这个系的转换公式（四元素法） ，这里利用的是C++中的vector这里我们去看看conjugate函数 这里的理解一下其实就date数据就是_q.conjugate中的_q赋过来的，而v.date就是conjugate后面的数据这个我不知道讲清楚了没，我们看到conjugate后面的参数是_mag，其实这个参数也是有mag(0)、mag(1)、mag(2)所以这个就不难理解了吧！然后就是利用转换后的X、Y进行转换得到mag_err,最后通过conjugate_inversed函数再次转换到机体坐标，转换公式贴出来自己去看。 
 
然后进行四元素的归一化处理，这个normalize函数不难理解吧，平方求根后作商。接着利用accl进行陀螺仪数据修正这个貌似不难，跟inint立面差不多。然后进行PI调节 这个理论性很强哦。主要是利用PI调节对姿态进行估计，大家去看看姿态解算的论文就会明白的。同样最后对四元素进行归一化处理并且对四元素进行数据判别。那么这个update就基本讲完了。 
我们回到att_estimator_q_main中，接着就是利用四元素转换为欧拉角。 这个的理论公式如下 自己对照看吧！然后对这些数据进行赋值到att结构体中接着利用四元素转换为方向余弦矩阵中最后赋值到date中。然后就是发布att的数据 同时也将数据赋值到控制状态中再进行发布数据。说到这里姿态估计这章节就讲完了。又不懂的还是那句话大家互相交流可以邮件给我或直接qq给我都行。

http://blog.csdn.net/qq_18112493/article/details/73822871