Pixhawk之姿态解算篇（3）_源码姿态解算算法分析

一、开篇

        终于到ardupilot源代码的姿态解算了，有了前期关于mahony姿态解算算法的基础以后，理解源代码的姿态解算算法就快多了，所有的东西都在脑海中初步有了一个框架；首先要做什么，然后再做什么，再然后捏~~~反正容易上手的。

        2016.04.04日晚，别人都在嗨，而我却在实验室苦逼的工作着，今晚最大的收获就是发现了“新大陆”-----“北航可靠飞行控制研究组”，其喜悦之情绝不亚于哥伦布发现新大陆。他们才是专业的啊，看看他们毕业生的去向，不是研究所就是出国深造，好吧，人家才是专业搞科研的，不食人间烟火，那么问题来了：DJI的员工都是在哪招来的呢？！哎，我是俗人一个，经济基础决定上层建筑，好好学习才能挣大钱。最近他们开设了一门课程《多旋翼飞行器设计与控制》，课程体系安排的非常好，现在更新到第四讲了（听北航一个博士说只有PPT没有视频，感谢Mallin的帮助，成功打入内部），PPT也足够了，相当上档次啊，课程到2016.06.30结束，正好可以把无人机的整个架构理解完，找工作去~~~

        在下面的基础知识部分先分享一部分“北航”的研究成果，特别是气动方面的，以前一点概念都没有，只看着超跑的流线型非常炫酷，不知其原因，特此记录，大家共勉。

三、实验平台

Software Version：PX4Firmware

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、基础知识（均来自北航可靠飞行控制研究组）

1、无人机飞行的气动模型与分析

        1）多旋翼前飞情形：在下图中，因为螺旋桨的柔性，诱导的来流会产生阻力。

        如果多旋翼重心在桨盘平面下方，那么阻力形成的力矩会促使多旋翼俯仰角转向0度方向。

        如果多旋翼重心在桨盘平面上方，那么阻力形成的力矩会促使多旋翼俯仰角朝发散方向发展，直至翻转。因此，当多旋翼前飞时，重心在桨盘平面的下方会使前飞运动稳定。

        2）多旋翼风干扰情形：在下图中，当阵风吹来，因为螺旋桨的柔性，诱导的来流会在产生阻力。

        如果多旋翼重心在下，那么阻力形成的力矩会促使多旋翼俯仰角朝发散的方向发展，直至翻转。

        如果多旋翼重心在上，那么阻力形成的力矩会促使多旋翼俯仰超0度方向发展。因此，当多旋翼受到外界风干扰时，重心在桨盘平面的上方可以抑制扰动。

        3）综上所述：无论重心在桨盘平面的上方或下方都不能使多旋翼稳定。因此需要通过反馈控制将多旋翼平衡。然而，如果重心在桨盘平面很靠上的位置，会使多旋翼某个运动模态很不稳定。因此，实际中建议将重心配置在飞行器桨盘周围，可以稍微靠下。这样控制器控制起来更容易些。

2、气动布局

        对外形进行设计主要是为了降低飞行时的阻力。按其产生的原因不同可分为

（1）摩擦阻力

（2）压差阻力

（3）诱导阻力

（4）干扰阻力

        要减少这些阻力，需要妥善考虑和安排各部件之间的相对位置关系，部件连接处尽量圆滑过渡，减少漩涡产生。

        因此它与物体的迎风面积有很大关系，迎风面积越大，压差阻力也越大。物体的形状也对压差阻力影响很大。如上图所示的三个物体，圆盘的压差阻力最大，球体次之，而流线体的最小，就压差阻力而言可以是平板压差阻力的1/20。

        设计建议：（法拉利、保驰捷等超跑的流线型车就是很好的榜样，宝马 Z4也可以，奔驰Smart就太low了，差点忘记特斯拉了）

（1）需要考虑多旋翼前飞时的倾角，减少最大迎风面积。

（2）并设计流线型机身。

（3）考虑和安排各部件之间的相对位置关系，部件连接处尽量圆滑过渡，飞机表面也要尽量光滑。

（4）通过CFD仿真（Computational Fluid Dynamics：计算流体动力学）计算阻力系数，不断优化。

        昨天见到了零度的四旋翼，外形设计的就是好，可惜只靠外形还是干不过DJI的，加油吧，零度。

五、正文（源代码的姿态解算算法）

1、进程入口：voidAttitudeEstimatorQ::task_main()

        1）订阅所需要的topics，注意sensor_combined，传感器数据都是靠它来的。

        在订阅时使用ORB_ID(sensor_combined)获取ID号，该ID号代表了从topic_name到topicmetadata structure name之间的转换桥梁。在task_main()的初始部分使用uORB模型的orb_subscribe()函数获取在sensors.cpp中通过orb_advertise()函数广播的传感器信息（sensor_combined）。由此说明在使用之前需要通过orb_advertise()函数之后才能在需要其数据的地方使用orb_subscribe()获取。如果没有使用，订阅者可以订阅，但是接收不到有效数据。

        关于uOBR模型的不再赘述，详细介绍参看：http://blog.csdn.net/freeape/article/details/46880637

和http://www.pixhawk.com/start?id=zh%2Fdev%2Fshared_object_communication&go

        px4_poll(fds,1, 1000)：配置阻塞时间，1ms读取一次sensor_combined的数据。