|
上次搞姿态解算的时候，没想出高效的长期融合方法。WarMonkey建议用梯度下降法，的确好用。梯度下降法思想不难，但还是用了好几天才明白。
- 关于梯度下降法
梯度下降法其实是一种迭代求极值的方法。
首先，什么是梯度呢？简单来说，就是“导数”，用符号“▽”来表示。只考虑值为标量的函数：只有一个变量的函数，梯度就是导数，例如r=f(x)，则f的梯度▽f=df/dx=f'(x)；对于多变量的函数，梯度是函数对各个变量的偏导组成的向量，例如r=f(x,y,z)，则f的梯度▽f=[?f/?x ?f/?y ?f/?z]，是一个向量，维数为函数变量数。把梯度向量点乘微量向量[dx dy dz]，结果就是全微分，所以梯度可以看成是多维的导数，都可以指示函数值增长的方向。
有了梯度，梯度下降法就简单了。既然梯度指示函数值增大的方向，逆着梯度走，就可以走向函数的极小值了。梯度通常可以直接求得，而每次走多大一步就是关键了，太大步会振荡，太小步收敛慢。
参考：
梯度 - 维基百科
梯度下降法 - 维基百科
雅可比矩阵 - 维基百科
《Estimation of IMU and MARG orientation using a gradient descent algorithm》——
Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan
- 为什么需要梯度下降法
我姿态解算的传感器是陀螺仪L3G4200D、加速度计ADXL345、罗盘HMC5883L，和气压计BMP085。姿态解算临时方案为：高速陀螺仪积分，以获得高响应性能，低速加速度计和罗盘融合，以纠正积分漂移。高速陀螺仪积分肯定用四元数乘法的了，毋庸质疑，而低速加速度计和罗盘融合则只想出 上次写的几何旋转法。 几何旋转法有几个缺点：一、没有充分利用重力和地磁场的特性。规定重力反方向为z轴正方向，则重力加速度的x、y分量为0，规定磁北方向为y轴正方向，则地磁场x分量为0。这3个0都没有利用到；二、每次融合，都把完整的姿态计算出来，最后却又与原来的姿态进行插值，因为“不怎么相信”这个结果，明显是浪费嘛。最终导致几何旋转法运算量大，而梯度下降法就刚好解决了这些问题。
- 怎样使用梯度下降法
首先再次明确长期融合的目的：利用加速度和磁场强度，纠正陀螺仪积分的漂移误差。也就是说，要误差趋向0。如果0是误差的一个极值，就可以用梯度下降法来逼近了。
先来几个热身：
定义一下向量的表示方式。
回顾四元数转矩阵的公式，这个是从飞行器坐标系到世界坐标系的转换。
逆过来就是从世界坐标系到飞行器坐标系的转换，旋转矩阵的逆就是其转置。
两个矩阵的用法如下。
好了，热身好了，下面开始讲梯度下降法：
也是从定义符号开始。
取重力反方向为z轴正方向，磁北为y轴正方向，则两个常量取值如下。
可以看到有3个0，1个1，这些都可以大大减少运算量。
然后定义误差，Δa为重力加速度与测量的加速度的偏差，Δh为地磁场与测量的磁场的偏差。
其实由于相减的向量是单位向量，Δ很小时|Δ|就相当于角度啦。
为了利用那3个0和1个1，把常量变换到飞行器坐标系，而不是把测量量变换到世界坐标系。
两个误差要合成一个误差函数，因为我只会“值为标量的函数”的梯度下降法。这里用长度的平方和。
好吧，f总是大于等于0的，0肯定是极小值了，可以用梯度下降法。
先算出梯度。这个表达式很长啊，我已经检查了有3、4遍了，应该没错的。
有了梯度，剩下就是确定步长γ了。
确定步长是梯度下降法的核心。步长未必是也往往不是常数，我取它为梯度长度的0.04倍，大家可以根据个人口味，酌量增减。当然，也可以选择其它形式。
可以看到，如果使用平方根倒数速算法，梯度下降法只有简单的加减乘，甚至math.h库都不需要。我又相信定点数了（可以快一个数量级）。
- 编程
有了公式，编程就相当简单了。
输入a、h和q，算出Δa和Δh，再用Δa、Δh和q算出?f/?q，再把q和?f/?q通过步长融合，最后单位化q。这就是一次迭代，每次长期融合迭代一次就够。
要注意的是：公式中有很多冗余计算，写代码的时候要避免重复计算。
- 晒几个视频
陀螺仪快速积分频率为400Hz，没有滤波。
低速融合频率50Hz。加速度采样率为400Hz，8个取平均；罗盘采样率50Hz，没有滤波。
快速积分+梯度下降法纠正漂移。
红绿蓝表示xyz。
实线和矩形是解算出的姿态，也就是解算的结果。
虚线是下一次加速度和磁场确定的姿态，也就是目标姿态。
静止时可以看到有“跳动”，那是磁场和加速度的噪声，实线和矩形是稳定的。
仅进行快速积分，没有用加速度和磁场纠正漂移。
剪一段快速运动时的慢放，先是原速播放，然后是慢放0.05倍。
低速时加速度和磁场确定的姿态比较准确，可以观察到“追逐”现象，因为虚线是“未来”的。
而高速时加速度和磁场确定的姿态就信不过啦。还好，我的步长小，影响不大，实际使用时要判断。
- 总结
这次梯度下降法的应用，用了差不多一个月，感觉进度越来越慢了。
一开始移植FatFS到NUC140，用了几天来调试，硬件SPI，总线频率上到24MHz。
采集到数据后，又用了几天理解梯度下降法，啃那篇英文论文。
接着买书学Scilab，又用了一个星期。
然后编程，处理数据，又几天。
之后还学了视频录制、视频处理、视频编码转换。
最后写博客。 进度是慢了点，但这些都是积累的，花点时间还是值的。
上次搞姿态解算的时候，没想出高效的长期融合方法。WarMonkey建议用梯度下降法，的确好用。梯度下降法思想不难，但还是用了好几天才明白。
- 关于梯度下降法
梯度下降法其实是一种迭代求极值的方法。
首先，什么是梯度呢？简单来说，就是“导数”，用符号“▽”来表示。只考虑值为标量的函数：只有一个变量的函数，梯度就是导数，例如r=f(x)，则f的梯度▽f=df/dx=f'(x)；对于多变量的函数，梯度是函数对各个变量的偏导组成的向量，例如r=f(x,y,z)，则f的梯度▽f=[?f/?x ?f/?y ?f/?z]，是一个向量，维数为函数变量数。把梯度向量点乘微量向量[dx dy dz]，结果就是全微分，所以梯度可以看成是多维的导数，都可以指示函数值增长的方向。
有了梯度，梯度下降法就简单了。既然梯度指示函数值增大的方向，逆着梯度走，就可以走向函数的极小值了。梯度通常可以直接求得，而每次走多大一步就是关键了，太大步会振荡，太小步收敛慢。
参考：
梯度 - 维基百科
梯度下降法 - 维基百科
雅可比矩阵 - 维基百科
《Estimation of IMU and MARG orientation using a gradient descent algorithm》——
Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan
- 为什么需要梯度下降法
我姿态解算的传感器是陀螺仪L3G4200D、加速度计ADXL345、罗盘HMC5883L，和气压计BMP085。姿态解算临时方案为：高速陀螺仪积分，以获得高响应性能，低速加速度计和罗盘融合，以纠正积分漂移。高速陀螺仪积分肯定用四元数乘法的了，毋庸质疑，而低速加速度计和罗盘融合则只想出 上次写的几何旋转法。 几何旋转法有几个缺点：一、没有充分利用重力和地磁场的特性。规定重力反方向为z轴正方向，则重力加速度的x、y分量为0，规定磁北方向为y轴正方向，则地磁场x分量为0。这3个0都没有利用到；二、每次融合，都把完整的姿态计算出来，最后却又与原来的姿态进行插值，因为“不怎么相信”这个结果，明显是浪费嘛。最终导致几何旋转法运算量大，而梯度下降法就刚好解决了这些问题。
- 怎样使用梯度下降法
首先再次明确长期融合的目的：利用加速度和磁场强度，纠正陀螺仪积分的漂移误差。也就是说，要误差趋向0。如果0是误差的一个极值，就可以用梯度下降法来逼近了。
先来几个热身：
定义一下向量的表示方式。
回顾四元数转矩阵的公式，这个是从飞行器坐标系到世界坐标系的转换。
逆过来就是从世界坐标系到飞行器坐标系的转换，旋转矩阵的逆就是其转置。
两个矩阵的用法如下。
好了，热身好了，下面开始讲梯度下降法：
也是从定义符号开始。
取重力反方向为z轴正方向，磁北为y轴正方向，则两个常量取值如下。
可以看到有3个0，1个1，这些都可以大大减少运算量。
然后定义误差，Δa为重力加速度与测量的加速度的偏差，Δh为地磁场与测量的磁场的偏差。
其实由于相减的向量是单位向量，Δ很小时|Δ|就相当于角度啦。
为了利用那3个0和1个1，把常量变换到飞行器坐标系，而不是把测量量变换到世界坐标系。
两个误差要合成一个误差函数，因为我只会“值为标量的函数”的梯度下降法。这里用长度的平方和。
好吧，f总是大于等于0的，0肯定是极小值了，可以用梯度下降法。
先算出梯度。这个表达式很长啊，我已经检查了有3、4遍了，应该没错的。
有了梯度，剩下就是确定步长γ了。
确定步长是梯度下降法的核心。步长未必是也往往不是常数，我取它为梯度长度的0.04倍，大家可以根据个人口味，酌量增减。当然，也可以选择其它形式。
可以看到，如果使用平方根倒数速算法，梯度下降法只有简单的加减乘，甚至math.h库都不需要。我又相信定点数了（可以快一个数量级）。
- 编程
有了公式，编程就相当简单了。
输入a、h和q，算出Δa和Δh，再用Δa、Δh和q算出?f/?q，再把q和?f/?q通过步长融合，最后单位化q。这就是一次迭代，每次长期融合迭代一次就够。
要注意的是：公式中有很多冗余计算，写代码的时候要避免重复计算。
- 晒几个视频
陀螺仪快速积分频率为400Hz，没有滤波。
低速融合频率50Hz。加速度采样率为400Hz，8个取平均；罗盘采样率50Hz，没有滤波。
快速积分+梯度下降法纠正漂移。
红绿蓝表示xyz。
实线和矩形是解算出的姿态，也就是解算的结果。
虚线是下一次加速度和磁场确定的姿态，也就是目标姿态。
静止时可以看到有“跳动”，那是磁场和加速度的噪声，实线和矩形是稳定的。
仅进行快速积分，没有用加速度和磁场纠正漂移。
剪一段快速运动时的慢放，先是原速播放，然后是慢放0.05倍。
低速时加速度和磁场确定的姿态比较准确，可以观察到“追逐”现象，因为虚线是“未来”的。
而高速时加速度和磁场确定的姿态就信不过啦。还好，我的步长小，影响不大，实际使用时要判断。
- 总结
这次梯度下降法的应用，用了差不多一个月，感觉进度越来越慢了。
一开始移植FatFS到NUC140，用了几天来调试，硬件SPI，总线频率上到24MHz。
采集到数据后，又用了几天理解梯度下降法，啃那篇英文论文。
接着买书学Scilab，又用了一个星期。
然后编程，处理数据，又几天。
之后还学了视频录制、视频处理、视频编码转换。
最后写博客。 进度是慢了点，但这些都是积累的，花点时间还是值的。 |
