hx H (q 2 q 2 q 2 q 2 ) H 2(q q q q ) h 0 1 2 3 Z 1 3 0 2 x X h R * H h hy H X 2(q1q2 q0 q3 ) H Z 2(q2 q3 q0 q1 ) hy H 2(q q q q ) H (q 2 q 2 q 2 q 2 ) h h 1 3 0 2 Z 0 1 2 3 z z X 有了梯度,剩下的就是确定步长r 确定步长是梯度下降法的核心。步长未必是一成不变 的。 加速度计和磁力计通过梯度下降法得到姿态误差速率 ,陀螺仪直接通过四元数微分方程得到姿态四元数速 率,两个加起来积分就可以得到相对精确的姿态四元 数了。以下公式就是四元数微分方程 其中w为陀螺仪的测量值。 H H X 0 HZ T 注意:此时的测量值都是经过标准化的 (三)四元数矩阵 从机体坐标系到参考坐标系 2 2 2 q0 q12 q2 q3 R 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 ) 2(q1q2 q0 q3 ) 2 2 2 q0 q12 q2 q3 R * V A V B 说明:A是参考坐标系;B是机体坐标系 向量 V A、 VB 是同一个向量,但分别被定义在坐 标系A中和坐标系B中 二、梯度下降法 梯度下降法是一种求解最优值得方法,就是利用负 梯度方向来决定每次迭代的新的搜索方向,使得每次 迭代能使待优化的目标函数逐步减小。 1、 加速度计和磁力计通过梯度下降法得到姿态误差 速率,陀螺仪直接通过四元数微分方程得到姿态四元 数速率,两个加起来积分就可以得到相对精确的姿态 a 四元数了。 定义: h 为重力加速度与测量的加速度的偏差 Q q0 * q1 q2 q3 T
(二)参数定义、坐标系定义 如果不存在误差那么 a=Q ⊕G ⊕Q* h=Q ⊕H⊕Q* 实际上是不相等的。 H是参考坐标系下地磁场 G是参考坐标系下重力加速动 参考坐标系:X轴对应北;Y轴对应西;Z轴对应天 机体坐标系:初始与参考坐标同向 T 则: G 0 0 1
为地磁场与测量的磁场的偏差 2、由于相减的向量是单位向量,∆很小时,|∆|就相 当于角度。 ax 2(q1q3 q0 q2 ) ax 2(q q q q ) a a R * G a a 2 3 0 1 y y 2 2 2 2 a q q q q 1 2 3 az z 0 2(q1q2 q0 q3 ) 2 2 2 q0 q12 q2 q3 2(q2 q3 q0 q1 ) 2(q1q3 q0 q2 ) 2(q2 q3 q0 q1 ) 2 2 2 q0 q12 q2 q3 两个四元数矩阵的用法 R V B V A 四元数微分方程 1 q t qt 1 wt 2
更新四元数 1 q t qt 1 qt 1 wt t 2 融合计算: 1 f ( a、 h) qt qt 1 ( qt 1 wt r ) t 2 f ( a、 h) 规范会处理 qt q qt 一、姿态求解理论 (一)四元数基本理论 假设现在我们已经得到正确的姿态四元数Q,那么我 们可以利用四元数旋转将参考坐标系和机体坐标系下 的向量互相转换。 定义四元数Q是坐标系A(参考坐标)旋转到坐标系B (机体坐标)所得到的姿态四元数。 T Q q0 q1 q2 q3 并且定义从坐标系B到坐标系A的四元数为Q* 4hy H z q3 4hz H x q0 4hz H z q2 f 4ax q1 4a y q2 4az q3 4hx H x q3 4hx H z q1 4hy H x q0 q3 4hy H z q2 4hz H x q1 4hz H z q3 2(q2 q3 q0 q1 ) 2(q1q3 q0 q2 ) 2(q2 q3 q0 q1 ) 2 2 2 q0 q12 q2 q3 从参考坐标系到机体坐标系 2 2 2 q0 q12 q2 q3 R* 2(q1q2 q0 q3 ) 2(q1q3 q0 q2 ) Leabharlann Baidu f 4ax q3 4a y q0 4az q1 4hx H x q1 4hx H z q3 4hy H x q2 q1 4hy H z q0 4hz H x q3 4hz H z q1 f 4ax q0 4a y q3 4az q2 4hx H x q2 4hx H z q0 4hy H x q1 q2 3、创建梯度函数 两个误差要合并成一个函数,这里我们使用标量函数 的梯度下降法。这里用长度的平方和。 f (a、h) a h 2 2 f ( a、 h) f f ( a、 h) q q0 f q1 f q2 f q3 T 求偏导数得: f 4ax q2 4a y q1 4hx H z q2 4hy H x q3 4hy H z q1 4hz H X q2 q0