利用梯度下降法进行姿态解算

js200300953

上次搞姿态解算的时候，没想出高效的长期融合方法。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 descentalgorithm》——　Sebastian O.H. Madgwick, Andrew J.L. Harrison, RaviVaidyanathan
• 为什么需要梯度下降法
  　　我姿态解算的传感器是陀螺仪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。
  实线和矩形是解算出的姿态，也就是解算的结果。
  虚线是下一次加速度和磁场确定的姿态，也就是目标姿态。
  静止时可以看到有“跳动”，那是磁场和加速度的噪声，实线和矩形是稳定的。
  http://v.youku.com/v_show/id_XNDI3NTgwODgw.html
  
  仅进行快速积分，没有用加速度和磁场纠正漂移。
  http://v.youku.com/v_show/id_XNDI3NTgzMTc2.html
  
  剪一段快速运动时的慢放，先是原速播放，然后是慢放0.05倍。
  低速时加速度和磁场确定的姿态比较准确，可以观察到“追逐”现象，因为虚线是“未来”的。
  而高速时加速度和磁场确定的姿态就信不过啦。还好，我的步长小，影响不大，实际使用时要判断。
  http://v.youku.com/v_show/id_XNDI3ODEwNTc2.html
• 总结
  这次梯度下降法的应用，用了差不多一个月，感觉进度越来越慢了。
  　　一开始移植FatFS到NUC140，用了几天来调试，硬件SPI，总线频率上到24MHz。
  　　采集到数据后，又用了几天理解梯度下降法，啃那篇英文论文。
  　　接着买书学Scilab，又用了一个星期。
  　　然后编程，处理数据，又几天。
  　　之后还学了视频录制、视频处理、视频编码转换。
  　　最后写博客。
  进度是慢了点，但这些都是积累的，花点时间还是值的。
  

原文;http://blog.sina.com.cn/s/blog_81f1e268010181v3.html

okARM

分析的真带劲

dxf5200

又出新文章了，学习一下。

nightdemon

分析的很好  学到 了  感谢

jokemcu

非常有卖相嘛，系统周期是多少的

ququ625

看看加入震动干扰会是什么情况。

js200300953

ququ625 发表于 2012-7-16 17:03
看看加入震动干扰会是什么情况。

我还没买电机呢。
很快就买了。
测到数据会第一时间公布出来。

js200300953

jokemcu 发表于 2012-7-16 16:57
非常有卖相嘛，系统周期是多少的

文中有说

陀螺仪快速积分频率为400Hz，没有滤波。
低速融合频率50Hz。加速度采样率为400Hz，8个取平均；罗盘采样率50Hz，没有滤波。

ququ625

气压计可以换了现在都用ms5611。陀螺仪加速度mpu6050.

chliken

好文，学习中

40342zz

楼主进度很快啊~前几天调了一下午的PID参数还是没有让四轴平衡下来，你弄到那一步的时候给介绍介绍~~
我上个月试过一种类似的方法，但是一震动就悲剧了，等你的测试结果~~
向你学习了~~
希望能与你交流~~

js200300953

40342zz 发表于 2012-7-17 11:53
楼主进度很快啊~前几天调了一下午的PID参数还是没有让四轴平衡下来，你弄到那一步的时候给介绍介绍~~
我上 ...

我也很想有人一起聊聊。
准备买电调电机了，有进度就会发出来。
但暑假学校要求实习，只能晚上搞，会慢很多。

godsays

请问LZ,你的磁传感器是怎么校准的?

js200300953

godsays 发表于 2012-7-17 16:44
请问LZ,你的磁传感器是怎么校准的?

跟加速度计一样。
http://www.amobbs.com/thread-5482090-1-1.html

ququ625

LZ有没有关注这个http://autoquad.org/

js200300953

ququ625 发表于 2012-7-17 17:58
LZ有没有关注这个http://autoquad.org/

第一次听说，多谢介绍。
今晚好好学习一下。

Name_006

好好研究一下  。。。。                                                                                         

__@

看了楼主的视频，感觉静态测试还可以，真正上飞机可能会出问题。
飞机本身的震动很大，对加速度计的影响是致命的，楼主梯度下降法加速度部分数据没有做其他处理，很可能电机震动让加速度计找不到重力方向，这样的话姿态就会越来越偏了。

个人意见

ququ625

关键还是看动态的姿态估测效果。静态大家都好。

robocon

厉害啊，，，，学习了

xue_xi_zhong

等你真正开了电机后，就知道还有一大堆的问题等着你了

fsswl

开电机后就见招拆招了,在姿态解算方面,楼主的方法不失为一个好方法,顶

zpwkxg

楼主：偏微分方程中每一项都有误差项，误差项怎么来的？

js200300953

zpwkxg 发表于 2012-7-22 21:50
楼主：偏微分方程中每一项都有误差项，误差项怎么来的？

复合函数求导嘛。

xuzc

来学习下

zpwkxg

谢谢楼主回答，学习中!
请教：
1.文中提到："为了利用那3个0和1个1，把常量变换到飞行器坐标系...."，这和一般姿态算法吧飞行坐标转换到地理坐标不同，能进步解释下吗？
2.文中最后视频中提到高速/高频姿态变化时：“高速时加速度和磁场确定的姿态就信不过啦....”，这算法在高速时和扩展卡尔曼算法不知兄弟比较过没有，我觉得kalman姿态融合在高速时也会存在这个问题的，不知这两个算法那个更好一点？

js200300953

zpwkxg 发表于 2012-7-23 07:37
谢谢楼主回答，学习中!
请教：
1.文中提到："为了利用那3个0和1个1，把常量变换到飞行器坐标系...."，这和 ...

1.我们需要的是误差的大小，所以哪个量变换到哪个坐标系无关要紧，只要在同一坐标系下相减就行。如果把测量量变换到地理坐标系，误差将会是这样：
  
  0和1没有参与乘法，浪费了。可以想象最终梯度的表达式会是怎么样。
  但是，鉴于上面的表达式很整齐，也有可能运算量反而更少。这个没多作思考啦。
2.这个抱歉了，我不懂卡尔曼算法。
    其实很久之前就知道卡尔曼滤波是神器，但看过书后就萎了，什么协方差啊，什么状态量啊，都不懂。还望大家教教。
    所以现在计划有变：先不搞硬件，把《矩阵理论与应用》和《自动控制原理》看完再来。

zpwkxg

感谢楼主精心回答，是的，矩阵理论很重要啊，一起学习啊！我好好看看Madgwick S.O.H这篇文章后再和你交流；你的上位机是什么软件做的？上位机编程是搞嵌入式同学的短板啊，感觉没有精力去弄VC这些！还有，每次启动为什么要插入CF卡，ARM没有Flash吗？

js200300953

zpwkxg 发表于 2012-7-23 14:10
感谢楼主精心回答，是的，矩阵理论很重要啊，一起学习啊！我好好看看Madgwick S.O.H这篇文章后再和你交流； ...

先晒晒：我是从纯软件转过来的，VC、C#、Java、HTML、JavaScript都无压力，但这些都没用到。
上位机就是Scilab，相当于开源的Matlab。
其实没有上位下位啦，我不会实时采集数据，都是离线处理的，所以用SD卡。

zpwkxg

谢谢答复，楼主从纯软转到软硬兼修，让人佩服啊！轻轻“拍”一下啊！
1.如果载体一直处于加速运动状态，就可知加速度计和磁强计量测不准，若让姿态误差“最小”，我们使陀螺向积分的姿态值向加计和磁强计的量测姿态值靠近，我们知道，此时是加计和磁强计不准，而“硬”让陀螺计算的姿态向不准方靠近（你文中描述的“追逐”吗），此时姿态误差加大，如果载体继续加速晃动，甚至“走向”更差的地步，我理解是如果载体一直处于加速晃动状态，这个算法最终会发散的，不知理解是否正确？
2.求完偏微分方程后，哪个地方显现出梯度下降（误差最小）算法的含义了，是不是在算法中每步要更新偏微分的最小值呢？

Niandet

强帖留名(1)!

js200300953

zpwkxg 发表于 2012-7-23 22:49
谢谢答复，楼主从纯软转到软硬兼修，让人佩服啊！轻轻“拍”一下啊！
1.如果载体一直处于加速运动状态，就 ...

1.这是大部分用重力和磁场计算姿态的算法的硬伤了。有两个措施应对这个问题：
      一、尽量减小步长和增加长期融合周期。因为陀螺仪漂移是小量，长期融合不需要太大动作。
      二、判断异常数据，必要时取消长期融合。
    其实有更“高级”的方法：预测实际加速度。这要跟控制部分扯上关系（卡尔曼融合好像就有考虑），到时再考虑了。
    那个“追逐”，是显示的效果，因为虚线是下一次重力+磁场计算的姿态（我是离线处理的，所以可以看到未来），所以如果陀螺仪积分的姿态和重力+磁场计算的姿态都正确，实线会向虚线运动，当与虚线重合时，虚线就跳到下一个位置（第3个视频16～20s）。
2.最后两个公式就是“下降”了。文中好像写错了（怎么又不能编辑了）。应该这样：
   
    要减的。
    的确每次都要把梯度算一遍，因为梯度是姿态的函数。

ksh84222

学习了!

fsswl

你好,我尝试使用你的梯度下降法进行数据融合,发现初始化后,啥也不动,3个旋转角会向某个角度靠近,按照你的经验,我大概是哪些地方理解错或者做错?我的快速融合效果与你的差不多,估计是使用的你方法时有些地方理解存在误区,请指正,谢谢

js200300953

fsswl 发表于 2012-7-29 22:22
你好,我尝试使用你的梯度下降法进行数据融合,发现初始化后,啥也不动,3个旋转角会向某个角度靠近,按照你的经 ...

• “啥也不动,3个旋转角会向某个角度靠近”。
  如果把姿态初始化为[1 0 0 0]，这就是要的效果。在另一个贴里有提过，可以看看：《开始研究姿态解算了》，29楼，第2点。
• 你是怎么实现这个算法的？直接在单片机上算，还是像我这样“离线处理”？
  我用Scilab写的，当时调试了几个错误，都是公式输错。
  如果先把公式仔细推导出来，再按照公式写程序，会方便很多。
  为了调试公式是否输错，写几个测试案例，单独测试某个功能（函数）。
  我这些公式都是有“背景”的，例如：四元数表示飞行器坐标系向世界坐标系的转换。如果这些不同，公式就要重新推了，不过也差不多。
  

我把程序附上了。

fsswl

我在弄快速融合时就是参考Health's blog的四元数的算法,直接用单片机算,是STM32F103RBT6,姿态有初始化为[1 0 0 0]，仅进行快速积分，没有用加速度和磁场纠正漂移时,效果和你的差不多,话说传感器也是一样,是环球电子商城买的?不过我是分开弄的,先买陀螺仪,觉得需要修正XY轴,就再买加速度传感器,随着进一步学习,觉得没达到想要的效果,然后又正好看到你的这个帖子,想试试你的方法,于是又搞了个电子罗盘```

xizi

感觉很高深。

fsswl

js200300953 发表于 2012-7-29 23:06

“啥也不动,3个旋转角会向某个角度靠近”。
如果把姿态初始化为[1 0 0 0]，这就是要的效果。在另一个贴 ...

我一直都采用初始化为[1,0,0,0]的做法,可是趋近的角度太离谱了,我都放平了,但X,Y都跑到好几十度```

js200300953

fsswl 发表于 2012-7-29 23:48
我一直都采用初始化为[1,0,0,0]的做法,可是趋近的角度太离谱了,我都放平了,但X,Y都跑到好几十度``` ...

• "是环球电子商城买的?"
  你怎么知道的！
• 如果XY偏得严重，可能是测量不准或公式输错。
  你的传感器有标定过吗？特别是罗盘，偏移特严重，我的X轴都偏到400多了。
  可以用“几何旋转法”校验测量是否出问题，再用梯度下降法提高效率。
  

fsswl

js200300953 发表于 2012-7-30 07:04

"是环球电子商城买的?"
你怎么知道的！

如果XY偏得严重，可能是测量不准或公式输错。

我这个做得比较粗略,加速度传感器是通过平放然后连续取样,再根据应用手册
X_CALIB = –(OUTPUT (X) ÷ 4)
Y_CALIB = –(OUTPUT (Y) ÷ 4)
Z_CALIB = –((OUTPUT (Z) – 256) ÷ 4)    (这里是直接复制手册公式,具体做法有点不同,的方向就是这样)

电子罗盘也是片得比较厉害,也是用HoneyWell的方法,慢慢转一周采样,
Xsf=1 或(Y 最大-Y 最小)/(X 最大-X 最小) (6)
以较大的数值为准
Ysf=1 或(X 最大-Y 最小)/(Y 最大-Y 最小) 以
较大的数值为准
Xoff=[(X 最大-X 最小)/2-X 最大]*Xsf (7)
Yoff=[(Y 最大-Y 最小)/2-Y 最大]*Ysf

X 值=Xsf*X 读数+Xoff
Y 值=Ysf*Y 读数+Yoff
就这样处理了XY轴的数据

不知是不是我理解错,我认为水平旋转时,磁倾角在Z轴向的投影量应该是不变的,Z轴的测量值应该也不怎么变的,但实际上还是有一定的变化,Z轴你是怎么处理的?

js200300953

fsswl 发表于 2012-7-30 17:03
我这个做得比较粗略,加速度传感器是通过平放然后连续取样,再根据应用手册
X_CALIB = –(OUTPUT (X) ÷ 4) ...

加速度这样校正为什么不是除以2？
罗盘这样校正肯定错了，因为旋转一周，磁场很难保证垂直于转轴。
我的校正方法是：http://www.amobbs.com/thread-5482090-1-1.html
加速度计和罗盘都一样，它们性质几乎一样。

fsswl

js200300953 发表于 2012-7-30 19:05
加速度这样校正为什么不是除以2？
罗盘这样校正肯定错了，因为旋转一周，磁场很难保证垂直于转轴。
我的 ...

加速度计的校正我不是照搬公式,只是参考应用手册的思路,总的来说,校正后,乘上刻度因子,水平放置时,X,Y轴向基本为0了,Z基本为1,换成其他轴向下,效果相同,我觉得问题应该是出在电子罗盘,我参考的是这份应用文档:磁阻传感器在导航系统中应用(结算公式)
貌似等级太低上传不了``````总的来说,只是校正了X,Y轴,对于用的三个轴向的数据是远远不够的,而且效果并不理想.

我看了你贴的方法,虽然一开始做的东西比较复杂,但是具有通用性,思路很好,学习了!我从这方面找找问题,也消化下你的方法

题外话,你懂的东西真多呢,佩服,说来惭愧,眨一下眼就要升大三了,再眨一下眼就差不多该毕业了,各方面的能力还差得远呢,还得加把劲呢,以后有什么不懂还请多多指教!

js200300953

fsswl 发表于 2012-7-30 21:22
加速度计的校正我不是照搬公式,只是参考应用手册的思路,总的来说,校正后,乘上刻度因子,水平放置时,X,Y轴 ...

你抬举我了，我就大三的，专业也跟这个不沾边。
所有东西都自学，需要什么就学什么，所以杂和乱。
你见到我弄出来的东西，已经是我当时的极限了。

fsswl

js200300953 发表于 2012-7-29 23:06

“啥也不动,3个旋转角会向某个角度靠近”。
如果把姿态初始化为[1 0 0 0]，这就是要的效果。在另一个贴 ...

我想问下,如果水平放置不动时,初始姿态为[1,0,0,0],那么,正确的情况下解算出得姿态角是不是为roll约等于0,pitch约等于0,yaw=xxxx(与摆放的方向有关)?

js200300953

fsswl 发表于 2012-8-1 18:10
我想问下,如果水平放置不动时,初始姿态为[1,0,0,0],那么,正确的情况下解算出得姿态角是不是为roll约等于0 ...

对      

fsswl

js200300953 发表于 2012-8-1 21:07
对

还是存在不少问题,先上个代码``````
void Gradient_Descent()
{
        float Normalize;
        float X_Delta_a,Y_Delta_a,Z_Delta_a;
        float X_Delta_h,Y_Delta_h,Z_Delta_h;
        float txz,twy,tyz,twx,tx2,ty2,txy,twz,tz2;
        float df_dw,df_dx,df_dy,df_dz;
        float yHw,yHx,yHy,yHz,zHw,zHx,zHy,zHz;

        txy = 2*Quaternion.Ax*Quaternion.Ay;
        twz = 2*Quaternion.Aw*Quaternion.Az;
        txz = 2*Quaternion.Ax*Quaternion.Az;
        twy = 2*Quaternion.Aw*Quaternion.Ay;
        tyz = 2*Quaternion.Ay*Quaternion.Az;
        twx = 2*Quaternion.Aw*Quaternion.Ax;
        tx2 = 2*Quaternion.Ax*Quaternion.Ax;
        ty2 = 2*Quaternion.Ay*Quaternion.Ay;
        tz2 = 2*Quaternion.Az*Quaternion.Az;

        yHw = HMC_Y_W*Quaternion.Aw;
        yHx = HMC_Y_W*Quaternion.Ax;
        yHy = HMC_Y_W*Quaternion.Ay;
        yHz = HMC_Y_W*Quaternion.Az;
        zHw = HMC_Z_W*Quaternion.Aw;
        zHx = HMC_Z_W*Quaternion.Ax;
        zHy = HMC_Z_W*Quaternion.Ay;
        zHz = HMC_Z_W*Quaternion.Az;

        //Δa                                                            Gravity_X为测量值
        X_Delta_a = txz-twy-Gravity_X;
        Y_Delta_a = tyz+twx-Gravity_Y;
        Z_Delta_a = 1-tx2-ty2-Gravity_Z;

        //Δh
        X_Delta_h = HMC_Y_W*(txy+twz)+HMC_Z_W*(txz-twy)-HMC_X;
        Y_Delta_h = HMC_Y_W*(1-tx2-tz2)+HMC_Z_W*(tyz+twx)-HMC_Y;
        Z_Delta_h = HMC_Y_W*(tyz-twx)+HMC_Z_W*(1-tx2-ty2)-HMC_Z;

        df_dw = -4*X_Delta_a*Quaternion.Ay+4*Y_Delta_a*Quaternion.Ax+4*X_Delta_h*(yHz-zHy)+4*Y_Delta_h*zHx-4*Z_Delta_h*yHx;
        df_dx = 4*X_Delta_a*Quaternion.Az+4*Y_Delta_a*Quaternion.Aw-4*Z_Delta_a*Quaternion.Ax+4*X_Delta_h*(yHy-zHz)+4*Y_Delta_h*(zHw-yHx)-4*Z_Delta_h*(yHw-zHx);
        df_dy = -4*X_Delta_a*Quaternion.Aw+4*Y_Delta_a*Quaternion.Az-4*Z_Delta_a*Quaternion.Ay+4*X_Delta_h*(yHx-zHw)+4*Y_Delta_h*zHz+4*Z_Delta_h*(yHz-zHy);
        df_dz = 4*X_Delta_a*Quaternion.Ax+4*Y_Delta_a*Quaternion.Ay+4*X_Delta_h*(yHw+zHx)+4*Y_Delta_h*(zHy-yHz)+4*Z_Delta_h*yHy;
       
        Quaternion.Aw+=r*df_dw;
        Quaternion.Ax+=r*df_dx;
        Quaternion.Ay+=r*df_dy;
        Quaternion.Az+=r*df_dz;
       
        Normalize = Q_rsqrt(Quaternion.Aw*Quaternion.Aw+Quaternion.Ax*Quaternion.Ax+Quaternion.Ay*Quaternion.Ay+Quaternion.Az*Quaternion.Az);
        Quaternion.Aw = Quaternion.Aw*Normalize;
        Quaternion.Ax = Quaternion.Ax*Normalize;
        Quaternion.Ay = Quaternion.Ay*Normalize;
        Quaternion.Az = Quaternion.Az*Normalize;
}

我觉得我对这个方法的理解上还是存在一定的误区,我先说说目前我自己的想法,如果我想先不理电子罗盘的数据,先用加速度传感器修正X,Y轴向的飘移,那么,最终得到的偏导公式是否为
//        df_dw = -4*X_Delta_a*Quaternion.Ay+4*Y_Delta_a*Quaternion.Ax;
//        df_dx = 4*X_Delta_a*Quaternion.Az+4*Y_Delta_a*Quaternion.Aw-4*Z_Delta_a*Quaternion.Ax;
//        df_dy = -4*X_Delta_a*Quaternion.Aw+4*Y_Delta_a*Quaternion.Az-4*Z_Delta_a*Quaternion.Ay;
//        df_dz = 4*X_Delta_a*Quaternion.Ax+4*Y_Delta_a*Quaternion.Ay;                                                                 这样子???
这样子理解有没有问题?

js200300953

fsswl 发表于 2012-8-2 00:00
还是存在不少问题,先上个代码``````
void Gradient_Descent()
{

[Gravity_X Gravity_Y Gravity_Z]和[HMC_X HMC_Y HMC_Z]有单位化吗？
那个仅用加速度计的公式对。
（那些4都可以提出来的，每次看到CPU被这样折磨，心里都痒。）

fsswl

js200300953 发表于 2012-8-2 16:06
[Gravity_X Gravity_Y Gravity_Z]和[HMC_X HMC_Y HMC_Z]有单位化吗？
那个仅用加速度计的公式对。
（那些 ...

一下子还真没看到4可以提出来``````数据都单位化了,为了找出问题,我目前只用加速度计,一开始还是先水平放置,发现出现偏差后,不但不会向[0,0,1]T收敛,反而越偏越厉害,于是,我把
     Quaternion.Aw+=r*df_dw;
        Quaternion.Ax+=r*df_dx;
        Quaternion.Ay+=r*df_dy;
        Quaternion.Az+=r*df_dz;
改为
     Quaternion.Aw-=r*df_dw;
        Quaternion.Ax-=r*df_dx;
        Quaternion.Ay-=r*df_dy;
        Quaternion.Az-=r*df_dz;
改为这样之后,总算会向着使Δ收敛为0的方向走,但是这样又与推导的公式不符合``````,长期融合后解算出来的姿态角也与单纯快速融合得到的相去甚远``````

js200300953

fsswl 发表于 2012-8-2 16:27
一下子还真没看到4可以提出来``````数据都单位化了,为了找出问题,我目前只用加速度计,一开始还是先水平放 ...

是减的，原文不能编辑了，前面的评论里有提过。
“长期融合后解算出来的姿态角也与单纯快速融合得到的相去甚远”，单纯快速融合需要初值，如果初值不正确，两个比较没有意义。

fsswl

js200300953 发表于 2012-8-2 16:44
是减的，原文不能编辑了，前面的评论里有提过。
“长期融合后解算出来的姿态角也与单纯快速融合得到的相 ...

我突然明白了自己犯了一个很二的错,偏差是由把常量变换到飞行器坐标系再减去测量值而得到的,但是如果抛开电子罗盘,只想修正X,Y轴,那么Z轴向的偏移就不能修正,随着误差的积累,加速度常量通过R-1(Q)并不能正确地把加速度常量变换到飞行器坐标系,于是,就越偏越离谱了

Casey_zjx

楼主，请问那个Aa,加速度测量值，是指直接从加速度计测量出的值，还是说要减掉重力在各轴的分量后的值？

js200300953

Casey_zjx 发表于 2012-8-25 14:47
楼主，请问那个Aa,加速度测量值，是指直接从加速度计测量出的值，还是说要减掉重力在各轴的分量后的值？ ...

我这里处理加速度的过程：
寄存器读出的原始值→偏移校正→单位化→计算梯度

Freezing_

js200300953 发表于 2012-7-30 22:57
你抬举我了，我就大三的，专业也跟这个不沾边。
所有东西都自学，需要什么就学什么，所以杂和乱。
你见到 ...

楼主很厉害啊，你学什么专业的，我学电子的也和这些不沾边，都自学导航算法，费劲死了
最近要解个微分方程，用到了牛顿-高斯迭代，你接触过么，好像和梯度下降类似
还有，楼主要解的方程是什么？是加速度和磁传感器得到四元数的那几个公式么？

Casey_zjx

js200300953 发表于 2012-8-25 21:29
我这里处理加速度的过程：
寄存器读出的原始值→偏移校正→单位化→计算梯度 ...

噢是我理解有误，这里求的是误差，感谢楼主回答。
梯度下降是把求出来的四元数是去更新之前快速融合出来的四元数，并且直接作为下一次融合的四元数，还是说要和前一个点的四元数相乘后得到的才作为下次的四元数呢？

js200300953

Freezing_ 发表于 2012-8-26 08:48
楼主很厉害啊，你学什么专业的，我学电子的也和这些不沾边，都自学导航算法，费劲死了
最近要解个微分方 ...

我是地理信息系统的。惯性导航什么的，也看过，不懂。太复杂了，做四轴用不到吧。


我没学过数值处理，不懂牛顿-高斯迭代。


这里的方程是是下面这个：

变量是四元数、加速度和磁场方向，值是误差的平方和。
每测量得一组加速度和磁场方向，就进行一次迭代。
这样加速度和磁场方向就作为常数，变量只剩四元数。
于是对四元数迭代，使误差趋向0。

js200300953

Casey_zjx 发表于 2012-8-26 09:52
噢是我理解有误，这里求的是误差，感谢楼主回答。
梯度下降是把求出来的四元数是去更新之前快速融合出来 ...

因为求得的是四元数的偏导，所以直接加，然后单位化，就可以。
而四元数相乘的情况，是组合旋转。例如快速融合：陀螺仪测出的角速度乘以积分时间，得到一个“微旋转”，每个积分周期就有一个“微旋转”，要得到最终的姿态，就要把全部微旋转“串起来”，就用四元数乘法了。

苦行僧

要好好补数学

snowblow

非常不错，值得学习倡导。

honphylin

LZ能提供 Estimation of IMU and MARG orientation using a gradient descentalgorithm.pdf 的下载么？

cai_mouse

请看以下文件：

AkeBest

楼主是高手，膜拜一下

qiangnubing

mark                                               

xaper

请问下楼主实时显示姿态的软件是什么，很厉害的样子

xaper

xaper 发表于 2012-10-26 15:43
请问下楼主实时显示姿态的软件是什么，很厉害的样子

看了LZ的帖子后知道了楼主用的SCILAB ，还是学软件的，怪不得高等数学得很好的样子，帖子也有条理。多多交流

js200300953

xaper 发表于 2012-10-26 16:21
看了LZ的帖子后知道了楼主用的SCILAB ，还是学软件的，怪不得高等数学得很好的样子，帖子也有条理。多多 ...

软件也是自己学的啦。我是学地理的。

qiangnubing

LZ 上位机能否开源呢

js200300953

qiangnubing 发表于 2012-10-27 21:18
LZ 上位机能否开源呢

这个上位机是Scilab，只不过我自己写了个脚本，写得很乱，使用还是挺麻烦的，你有兴趣就看一下。
Qt那个还不是东西，都放出来啦。
（只是泄露出源代码，不是传统意义的“开源”）

qiangnubing

js200300953 发表于 2012-10-28 09:23
这个上位机是Scilab，只不过我自己写了个脚本，写得很乱，使用还是挺麻烦的，你有兴趣就看一下。
Qt那个 ...

非常感谢

reAper07

js200300953 发表于 2012-7-30 22:57
你抬举我了，我就大三的，专业也跟这个不沾边。
所有东西都自学，需要什么就学什么，所以杂和乱。
你见到 ...

你好 我也是大三
想自己做个四轴
能不能把你的资料给点我啊
我学的也跟这个不沾边
自学感觉入门好难
看了好久的帖子 依旧毫无头绪啊
QQ519741894
谢谢了

XYM741

牛！

huangrb

错了吧？重力向量的世界坐标按你的定义应该是0，0，-1。之后的deltaA向量也应该随之都乘以-1。我拿你的模拟了一下，飞机上来翻了个底朝天

js200300953

huangrb 发表于 2012-12-3 19:14
错了吧？重力向量的世界坐标按你的定义应该是0，0，-1。之后的deltaA向量也应该随之都乘以-1。我拿你的模拟 ...

加速度计测的是比力，运动加速度-重力加速度。
z轴向上的话，是大于0的。

玉米

楼主是否方便留个QQ交流？？

redwolf310

mark         

kmani

晚上看了一晚上梯度法解线性方程组，回来后看到坛子里有梯度法 。。。

asha

这个方法见过N次了，也点评过N次了。我认为其本质是反馈控制，本来这个梯度法式解非线性方程组的，一般和牛顿法结合可以得到精确解。但是在这个程序里面，用的还不是梯度法，因为梯度法式个递推法，需要递推很多次，才能接近真实值。 因此，这个不是很好的方法，不可能稳定住姿态，还会漂移，可能就是慢点，并且还需要人为调节增益因子K，调节的依据在哪？没有依据，就是看着漂的慢了就OK了。 如果要做还是最优估计的卡尔曼来的好。

liurangzhou

必须得mark

老衲人称秃驴

数学看不懂

js200300953

asha 发表于 2012-12-14 12:52
这个方法见过N次了，也点评过N次了。我认为其本质是反馈控制，本来这个梯度法式解非线性方程组的，一般和牛 ...

哦        

chenghailin66

fsswl 发表于 2012-7-30 17:03
我这个做得比较粗略,加速度传感器是通过平放然后连续取样,再根据应用手册
X_CALIB = –(OUTPUT (X) ÷ 4) ...

你这方法在哪有呢？可以推荐下吗？谢谢

chenghailin66

从你的代码里可以看出，陀螺仪也进行了标定，难道也是采用标定加速计的方法吗？

js200300953

chenghailin66 发表于 2013-2-26 09:20
从你的代码里可以看出，陀螺仪也进行了标定，难道也是采用标定加速计的方法吗？ ...

陀螺的校正跟加速度计不同，零点校正比较简单，减去静止时的均值就可，比例的校正很麻烦，直接采用datasheet上的值了。

caopeng32

js200300953 发表于 2012-12-18 13:17
哦

看到asha点评说梯度下降法没有卡尔曼滤波的方法好，我觉得是没有道理的，从我看的姿态解算来看，目前还没有好的能融合加速度计与罗盘计算姿态角的方法（当然，也可能我看的文献不够全面），而采用一步梯度下降法的融合，从微观来看其实效果也不好，但是后面与陀螺仪的互补融合，效果就体现出来了，另外，利用梯度下降法计算的结果，其准确性完全依赖加速度计，罗盘的准确性太差了，因此在机动飞行时，就是鸡肋了

js200300953

caopeng32 发表于 2013-2-26 20:34
看到asha点评说梯度下降法没有卡尔曼滤波的方法好，我觉得是没有道理的，从我看的姿态解算来看，目前还没 ...

首先，我对asha的评论也是不完全赞同的。
其次，梯度下降法可以分开加速度和磁场，分别设置权重，或者去掉其中一个。
不过，有了“叉积法”，梯度下降法就不需要了。

近段时间开始玩小四轴，罗盘与电机靠得太近，基本没办法用，校正参数不稳定。

yat

mark 利用梯度下降法进行姿态解算

reAper07

mark   值得好好研究研究

jayzah

楼主我在用你的上位机，发送到串口的数据的协议是什么样的，比如用printf怎么发送呢。。。

js200300953

jayzah 发表于 2013-3-29 20:38
楼主我在用你的上位机，发送到串口的数据的协议是什么样的，比如用printf怎么发送呢。。。 ...

不能用printf输出
用的是更高效更方便的二进制格式。
具体格式未整理，你可以参考一下我的代码。

jayzah

楼主好人，thank u

z_jx_1990

在用楼主的加速度标定软件，其中数据格式说明每一帧格式为： 帧头+协议版本+帧类型+XYZ值。 帧头2字节，由前到后为0x55、0xAA。协议版本1字节，为0x00。帧类型1字节，为0x02。XYZ值为32位float浮点数，低字节在前。
其中这个“XYZ值为32位float浮点数，低字节在前。”不是很理解。也看了楼主Scilab的脚本程序，表示也看不懂，还是楼主给指示下。
我发的如下
uint8_t send[20];
send[0]=0x55;
send[1]=0xAA;
send[2]=0x00;
send[3]=0x02;
send[4]=(uint8_t)acc[0];

send[5]=(uint8_t)acc[1];

send[6]=(uint8_t)acc[2];
USART1_send(USART1,7,send);
acc[0]为X轴数据，
acc[1]为Y轴数据，
acc[2]为Z轴数据，
这样有问题吗？
楼主能否给个实例呢。

js200300953

z_jx_1990 发表于 2013-4-3 22:33
在用楼主的加速度标定软件，其中数据格式说明每一帧格式为： 帧头+协议版本+帧类型+XYZ值。 帧头2字节，由 ...

1. send[4]=(uint8_t)acc[0];
   
2. send[5]=(uint8_t)acc[1];
   
3. send[6]=(uint8_t)acc[2];

复制代码

错了，应该这样：

1. for(int i=0;i<3*4;i++)
   
2. {
   
3.     send[4+i] = ((uint8_t *)acc)[i];
   
4. }

复制代码

z_jx_1990

js200300953 发表于 2013-4-4 14:48
错了，应该这样：

太谢谢了。

actshuishan

好文  学习

chenghailin66

js200300953 发表于 2012-7-17 16:54
跟加速度计一样。
http://www.amobbs.com/thread-5482090-1-1.html

磁阻校正和加速计方法一样，地球重力加速度取了9.7883m/s²，那么当地磁场强度如何得到呢？请教贴主

js200300953

chenghailin66 发表于 2013-4-15 11:06
磁阻校正和加速计方法一样，地球重力加速度取了9.7883m/s²，那么当地磁场强度如何得到呢？请教贴主 ...

取1，这个没关系的，只要方向。

chenghailin66

js200300953 发表于 2013-4-15 11:09
取1，这个没关系的，只要方向。

在最小二乘法求a,b,c,d,e,f这些系数的时候也取1吗？帖子里G也是1？不是9.7883吗？

js200300953

chenghailin66 发表于 2013-4-15 11:14
在最小二乘法求a,b,c,d,e,f这些系数的时候也取1吗？帖子里G也是1？不是9.7883吗？ ...

G也可以设成1，之前用比较准确的重力加速度值，出于两个考虑：
1.以后可能需要积分出速度和位移，要带单位。
2.个人习惯把所有量都转到标准单位，加速度用m/s²，角速度用rad/s，只是磁场没办法了，干脆取1规范化了。

chenghailin66

js200300953 发表于 2013-4-15 11:25
G也可以设成1，之前用比较准确的重力加速度值，出于两个考虑：
1.以后可能需要积分出速度和位移，要带单 ...

只是我还不太明白，如果磁阻归一化，那怎么求得系数？难道也把大量采集的数据进行归一化？那样岂不是不需要a,b,c...这些系数了。估计是我数学不太好，O(∩_∩)O哈哈~

js200300953

chenghailin66 发表于 2013-4-15 11:41
只是我还不太明白，如果磁阻归一化，那怎么求得系数？难道也把大量采集的数据进行归一化？那样岂不是不需 ...

过程跟加速度一模一样，只不过9.7883改为1。
不改直接一样，也完全没问题。

chenghailin66

js200300953 发表于 2013-4-15 12:34
过程跟加速度一模一样，只不过9.7883改为1。
不改直接一样，也完全没问题。 ...

理解了，多谢。