PX4姿态解算源码原理理解

PX4源码原理理解一．主要参考资料链接：1.1 取PX4源码一小部分姿态解算来进行讲解姿态解算源码中文注释：https://blog.csdn.net/zouxu634866/article/details/79806948(但是并不全面，下面有我添加注释的较为完整的版本1.2 姿态解算文件层级 Cmakelists是编译脚本为之后使用命令刷入固件提供方便 attitude_estimator_q_params是默认参数文件即在地面站可以看到的参数，调参调的就是这些attitude_estimator_q_main是功能代码所在的文件（源码最后） 二．涉及到的部分传感器以及数学知识2.1传感器介绍（imu单元，陀螺仪，加速度计，磁力计）数据来源2.1.1加速度计三轴加速度计是用来推出角度的（主要是横滚角和俯仰角）2.1.2陀螺仪（测量角速度）（主要是横滚角和俯仰角和偏航角）加速度计不会影响w的测量2.1.3磁力计（主要是偏航角）磁场的变化会导致磁阻传感器电阻值发生变化，很容易受到干扰到，得到的是数据当前磁场 三个传感器获取的数据进行整合即可得到准确姿态信息 2.2姿态的三种表达方式以及两种坐标系2.2.1坐标系 PX4机体坐标系（向前为x轴，右为y轴，向下为z轴）：飞机上采集的数据基于机体坐标系地理坐标系（ned坐标系）（向北为x轴，东为y轴，向下为z轴）你在地面观察飞机的数据2.3.1欧拉角：Yaw偏航角（绕z轴）Pitch俯仰角（绕y轴）Roll横滚（绕x轴）通过一次绕不同坐标轴的三次连续转动来实现一个坐标系到另一个坐标系的转换（航空航天领域规定zyx为欧拉角的旋转顺序） 2.3.2旋转矩阵2维平面上旋转一个角度 拓展到三维： 2.3.3四元数（现在的位置到期待的位置，找一个旋转足一次旋转到位）（图中的θ为旋转角度，u为旋转轴） 2.4.1三种方法的对比： 三．姿态解算原理3.1姿态解算简化流程：Subscribe→姿态解算→publishing（sensor_conbine) (算法) (vehicle_attitude)（556- 709） （437-457） 3.2完整流程3.2.1 q0（四元数）初始化获取（498-555行）3.2.2 误差来源（635-645行）重力通过旋转矩阵到机体坐标系中（635-640）拿结果与加速度计的（ax，ay，az）比较，通过叉乘的结果是否等于0判断是否相等（前提加速度计仅仅测重力，因为vx，vy，vz是重力在机体上的分解，只有当加速度计测的也是重力在机体上的分解，实际上可能包含重力加速度与运动加速度，而运动加速度是我们不需要的，那么就减去运动加速度（运动加速度的值由gps给出。）即可得到重力的误差。 另一个误差——磁力计的误差（581磁力计得到的数据通过旋转矩阵回机体，与gps工作良好的时候得到的经纬度查表得到的磁偏角数据进行比较，将误差累积。3.2.3 将得到的误差弥补到陀螺仪之上，用弥补之后的陀螺仪数据（即w）重新进行四元数的求解（代码为_q += _q.derivative(corr) * dt;即一阶的龙格—库塔法求解，得到更新后的四元数）得到这个四元数，姿态解算就完成了，最后进行数据的填充与发布。至此姿态解算完成 四．姿态解算完整源码注释分析：1. #include <drivers/drv_hrt.h> //高精度的定时器。 在控制过程中多数环节都是使用经典的PID控制算法为 2. //了获取较为实时的响应最重要的时间变量，这就涉及如何获取高精度的时间问题。pixhawk中就有高精度的RTC（实时时钟），这个 3. //头文件就做了介绍 4. 5. 6. 7. #include <lib/geo/geo.h> 8. #include <mathlib/math/filter/LowPassFilter2p.hpp> /滤波器/ 9. #include <mathlib/mathlib.h> 10. #include <px4_config.h> 11. #include <px4_posix.h> 12. #include <px4_tasks.h> 13. #include <systemlib/err.h> /包含一些错误警告的功能/ 14. #include <systemlib/param/param.h> 15. #include <systemlib/perf_counter.h> /性能评估/ 16. #include <uORB/topics/att_pos_mocap.h> 17. #include <uORB/topics/parameter_update.h> 18. #include <uORB/topics/sensor_combined.h> /通过uorb获取传感器数据，进行下一步姿态解算19. #include <uORB/topics/vehicle_attitude.h>/我们在这个程序中解算的姿态就要发布进这个主要是/ 20. 21. #include <uORB/topics/vehicle_global_position.h> /去看看有什么，后面讲位置估计时要用到/ 22. 23. extern “C” __EXPORT int attitude_estimator_q_main(int argc, char *argv[]); 24. 25. using math::Vector; /使用向量/ 26. using math::Matrix; /使用矩阵/ 27. using math::Quaternion; /使用四元数/ 28. 29. class AttitudeEstimatorQ; 30. 31. namespace attitude_estimator_q 32. { 33. AttitudeEstimatorQ instance; 34. } // namespace attitude_estimator_q attitude_estimator_q 35. 36. class AttitudeEstimatorQ 37. { 38. public: 39. / 40. * Constructor 41. / 42. AttitudeEstimatorQ(); 43. 44. / 45. * Destructor, also kills task. 46. / 47. ~AttitudeEstimatorQ(); 48. 49. / 50. * Start task. 51. * 52. * @return OK on success. 53. */ 54. int start(); //创建一个新的任务 55. 56. static void task_main_trampoline(int argc, char *argv[]); 57. 58. void task_main(); 59. 60. private: 61. const float _dt_min = 0.00001f; /dt是指更新数据的时间间隔，也就是离散pid公式中的T/ 62. const float _dt_max = 0.02f; 63. 64. bool _task_should_exit = false; /< if true, task should exit / 65. int _control_task = -1; /< task handle for task */ 66. 67. int _params_sub = -1; /与parameter_update有关/ 68. int _sensors_sub = -1; /这个变量是订阅传感器数据时用的句柄/ 69. int _global_pos_sub = -1; /这个变量是订阅地理位置数据时用的句柄/ 70. int _vision_sub = -1; /这个变量是视觉信息时用的句柄/ 71. int _mocap_sub = -1; /mocap=motion capture，动作捕捉/ 72. 73. orb_advert_t _att_pub = nullptr; /nullptr是c++11标准引入的更严谨的空指针，这里的att_pub是用于发布姿态信息的handle/ 74. 75. struct { 76. 77. /前面带w都是权重的意思/ 78. param_t w_acc; /这里的acc为加速度计的权重，用于后面互补滤波/ 79. param_t w_mag; /mag为磁力计/ 80. param_t w_ext_hdg; 81. param_t w_gyro_bias; /陀螺仪偏差/ 82. param_t mag_decl; /磁方位角/ 83. param_t mag_decl_auto; /利用gps自动获得磁方位角/ 84. param_t acc_comp; /加速度的补偿/ 85. param_t bias_max; /最大偏差/ 86. param_t ext_hdg_mode; /外部的航向使用模式，=1表示来自于视觉，=2表示来自mocap/ 87. } _params_handles{}; /< handles for interesting parameters 这个结构体里面全是一些系统参数*/ 88. /在px4的程序中获取系统参数的方法是用param_get和param_find去获取，而不是通过uorb/ 89. 90. 91. 92. /下面这些初始化的数据是用来存放从系统参数get过来的数据*/ 93. float _w_accel = 0.0f; 94. float _w_mag = 0.0f; 95. float _w_ext_hdg = 0.0f; 96. float _w_gyro_bias = 0.0f; 97. float _mag_decl = 0.0f; 98. bool _mag_decl_auto = false; 99. bool _acc_comp = false; 100. float _bias_max = 0.0f; 101. int32_t _ext_hdg_mode = 0; 102. 103. Vector<3> _gyro; /通过传感器获取的三轴的角速度/ 104. Vector<3> _accel; /通过加速度计获取的三轴的加速度/ 105. Vector<3> _mag; /通过磁力计获取的磁航向/ 106. 107. Vector<3> _vision_hdg; /通过视觉获取的handing/ 108. Vector<3&g