赞
踩
代码路径ardupolit/modules/PX4Firmware/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
最近结合惯性导航这本书,详细看了四元数姿态解算的代码,然后对这部分代码进行了详细的分析,分享给大家,如果分析有误请大家留言不吝赐教!!
/*根据机体坐标系下实际测量得到磁传感器值左乘四元数旋转矩阵得到地理坐标系下的磁传感器值*/
现在我们假设CbR旋转矩阵是经过加速度计校正后的矩阵,当某个确定的向量(机体系中)经过这个矩阵旋转之后(到地理坐标系),这两个坐标系在XOY平面上重合,只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过CbR旋转之后的机体坐标系b和地理坐标系n的相对关系。可以明显发现加速度计可以把机体坐标系b通过四元数法从任意角度拉到与地理坐标系n水平的位置上,这时,只剩下一个偏航角误差。这也是为什么加速度计误差修正偏航的原因。
hx,hy,hz是地理坐标系下的磁传感器值,可以有机体坐标系下的mx,my,mz左乘CbR得到,假设理想情况下的机体能够和当地的地理坐标系处于同一XOY平面,且机头指北,那么此时的磁传感器值应该为bx,0,bz,此时我们很方便的可以得到bx²=hx²+hy²,bz=hz,当时理想必定是理想,飞机的姿态不可能达到这种状态,所以我们再根据bx,0,bz(地理坐标系)右乘CbR得到估计后的磁传感器值halfwx,halfwy,halfwz(这部分解说间黄色底色部分)
对于重力加速度的补偿相比于磁传感器要简单的多,我们认为理想情况下的飞机状态能够达到和当地地理坐标系XOY水平的状态,那么此时的重力加速度值应该为0,0,1(归一化后),那么根据此地理坐标系下的重力加速度值,右乘CbR即可得到此时机体坐标系下的重力加速度估计值(见红色底色部分)
/*四元数更新完毕*/
/*将更新后的四元数进行归一化,并根据与余弦矩阵的关系可以得到yaw,pitch,roll角度,进而进行下,一次的数据融合
根据余弦矩阵和四元数旋转矩阵的关系可以得到角度关系
(roll,pitch,yaw)
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。