amoBBS 阿莫电子论坛

 找回密码
 注册
搜索
bottom↓
查看: 111534|回复: 674
打印 上一主题 下一主题

捷联惯导算法心得

  [复制链接]
跳转到指定楼层
1
发表于 2012-8-16 10:23:30 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
本帖最后由 seanwood 于 2012-8-16 15:33 编辑

1、四个概念:“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。

地理坐标系:东、北、天,以下简称地理。在这个坐标系里有重力永远是(0,0,1g),地磁永远是(0,1,x)(地磁的垂直不关心)两个三维向量。
机体坐标系:以下简称机体,上面有陀螺、加计、电子罗盘传感器,三个三维向量。
换算公式:以下简称公式,公式就是描述机体姿态的表达方法,一般都是用以地理为基准,从地理换算到机体的公式,有四元数、欧拉角、方向余弦矩阵。
换算公式的系数:以下简称系数,四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。

姿态,其实就是公式+系数的组合,一般经常用人容易理解的公式“欧拉角”表示,系数就是横滚xx度俯仰xx度航向xx度。

2、五个数据源:重力、地磁、陀螺、加计、电子罗盘,前两个来自地理,后三个来自机体

3、陀螺向量:基于机体,也在机体上积分,因为地理上无参考数据源,所以很独立,直接在公式的老系数上积分,得到新系数。
狭义上的捷联惯导算法,就是指这个陀螺积分公式,也分为欧拉角、方向余弦矩阵、四元数,他们的积分算法有增量法、数值积分法(X阶龙格-库塔)等等

4、加计向量、重力向量:加计基于机体,重力基于地理,重力向量(0,0,1g)用公式换算到机体,与机体的加计向量算出误差。理论上应该没有误差,这误差逆向思维一下,其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数(横滚、俯仰),也就是姿态。

5、电子罗盘向量、地磁向量:同上,只不过要砍掉地理上的垂直向量,因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数(航向)。

6、就这样,系数不停地被陀螺积分更新,也不停地被误差修正,它和公式所代表的姿态也在不断更新。
如果积分和修正用四元数算法(因为运算量较少、无奇点误差),最后用欧拉角输出控制PID(因为角度比较直观),那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式,它们之间都有转换算法。

再搞个直白一点的例子:
机体好似一条船,地理就是那地图,姿态就是航向(船头在地图上的方位),重力和地磁是地图上的灯塔,陀螺/积分公式是舵手,加计和电子罗盘是瞭望手。
舵手负责估计和把稳航向,他相信自己,本来船向北开的,就一定会一直往北开,觉得转了90度弯,那就会往东开。
当然如果舵手很牛逼,也许能估计很准确,维持很长时间。不过只信任舵手,肯定会迷路,所以一般都有地图和瞭望手来观察误差。
瞭望手根据地图灯塔方位和船的当前航向,算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位,那肯定船的当前航向有偏差了,偏差就是ERR=X-Y。
舵手收到瞭望手给的ERR报告,觉得可靠,那就听个90%*ERR,觉得天气不好、地图误差大,那就听个10%*ERR,根据这个来纠正估算航向。。



------------------------------------------------------
来点干货,注意以下的欧拉角都是这样的顺序:先航向-再俯仰-然后横滚
公式截图来自:袁信、郑锷的《捷联式惯性导航原理》,邓正隆的《惯性技术》。
--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要,因为加计和电子罗盘给出欧拉角的描述方式比较方便。
imu.euler.x = atan2(imu.accel.y, imu.accel.z);
imu.euler.y = -asin(imu.accel.x / ACCEL_1G);
ACCEL_1G 为9.81米/秒^2,accel.xyz的都为这个单位,算出来的euler.xyz单位是弧度
航向imu.euler.z可以用电子罗盘计算
--------------------------------------------------
欧拉角微分方程
如果用欧拉角算法,那么这个公式就够了,不需要来回转换。

矩阵上到下三个角度(希腊字母)是roll pitch和yaw,公式最左边的上面带点的三个是本次更新后的角度,不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度,单位为弧度,计算为间隔时间T*陀螺角速度,比如0.02秒*0.01弧度/秒=0.0002弧度.


--------------------------------------------------
以下是四元数
--------------------------------------------------
四元数初始化
q0-3为四元数四个值,用最上面公式根据加计计算出来的欧拉角来初始化

--------------------------------------------------
四元数微分方程
四元数更新算法,一阶龙库法,同样4个量(入、P1-3)也为四元数的四个值,即上面的q0-3。
Wx,y,z是三个陀螺的这个周期的角速度,比如欧拉角微分方程中的0.01弧度/秒,T为更新周期,比如上面的0.02秒。

再来一张,另外一本书上的,仔细看和上面是一样的delta角度,就是上面的角速度*周期,单位为弧度

--------------------------------------------------
四元数微分方程更新后的规范化
每个周期更新完四元数,需要对四元数做规范化处理。因为四元数本来就定义为四维单位向量。
求q0-3的平方和,再开根号算出的向量长度length。然后每个q0-3除这个length。

--------------------------------------------------
四元数转欧拉角公式
把四元数转成了方向余弦矩阵中的几个元素,再用这几个元素转成了欧拉角
先从四元数q0-3转成方向余弦矩阵:

再从方向余弦矩阵转成欧拉角


代码:
        //更新方向余弦矩阵
        t11=q.q0*q.q0+q.q1*q.q1-q.q2*q.q2-q.q3*q.q3;
        t12=2.0*(q.q1*q.q2+q.q0*q.q3);
        t13=2.0*(q.q1*q.q3-q.q0*q.q2);
        t21=2.0*(q.q1*q.q2-q.q0*q.q3);
        t22=q.q0*q.q0-q.q1*q.q1+q.q2*q.q2-q.q3*q.q3;
        t23=2.0*(q.q2*q.q3+q.q0*q.q1);
        t31=2.0*(q.q1*q.q3+q.q0*q.q2);
        t32=2.0*(q.q2*q.q3-q.q0*q.q1);
        t33=q.q0*q.q0-q.q1*q.q1-q.q2*q.q2+q.q3*q.q3;
        //求出欧拉角
        imu.euler.roll = atan2(t23,t33);
        imu.euler.pitch = -asin(t13);
        imu.euler.yaw = atan2(t12,t11);
        if (imu.euler.yaw < 0){
                imu.euler.yaw += ToRad(360);
        }
----------------------------------------------------
以下代码摘自网上,很巧妙,附上注释,有四元数微分,有加计耦合。
没电子罗盘,其实耦合原理也一样。
  1. //=====================================================================================================
  2. // IMU.c
  3. // S.O.H. Madgwick
  4. // 25th September 2010
  5. //=====================================================================================================
  6. // Description:
  7. //
  8. // Quaternion implementation of the 'DCM filter' [Mayhony et al].
  9. //
  10. // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
  11. //
  12. // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
  13. // orientation.  See my report for an overview of the use of quaternions in this application.
  14. //
  15. // User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
  16. // and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer
  17. // units are irrelevant as the vector is normalised.
  18. //
  19. //=====================================================================================================

  20. //----------------------------------------------------------------------------------------------------
  21. // Header files

  22. #include "IMU.h"
  23. #include <math.h>

  24. //----------------------------------------------------------------------------------------------------
  25. // Definitions

  26. #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  27. #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  28. #define halfT 0.5f                // half the sample period

  29. //---------------------------------------------------------------------------------------------------
  30. // Variable definitions

  31. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  32. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  33. //====================================================================================================
  34. // Function
  35. //====================================================================================================

  36. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
  37.         float norm;
  38.         float vx, vy, vz;
  39.         float ex, ey, ez;         
  40.       
  41.         // normalise the measurements
  42.         norm = sqrt(ax*ax + ay*ay + az*az);      
  43.         ax = ax / norm;
  44.         ay = ay / norm;
  45.         az = az / norm;      
  46. 把加计的三维向量转成单位向量。
  47.       

  48.         // estimated direction of gravity
  49.         vx = 2*(q1*q3 - q0*q2);
  50.         vy = 2*(q0*q1 + q2*q3);
  51.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

  52. 这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
  53. 根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到机体坐标系,正好是这三个元素。
  54. 所以这里的vx\y\z,其实就是当前的欧拉角(即四元数)的机体坐标参照系上,换算出来的重力单位向量。


  55.         // error is sum of cross product between reference direction of field and direction measured by sensor
  56.         ex = (ay*vz - az*vy);
  57.         ey = (az*vx - ax*vz);
  58.         ez = (ax*vy - ay*vx);

  59. axyz是机体坐标参照系上,加速度计测出来的重力向量,也就是实际测出来的重力向量。
  60. axyz是测量得到的重力向量,vxyz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
  61. 那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
  62. 向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示,exyz就是两个重力向量的叉积。
  63. 这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。



  64.         // integral error scaled integral gain
  65.         exInt = exInt + ex*Ki;
  66.         eyInt = eyInt + ey*Ki;
  67.         ezInt = ezInt + ez*Ki;

  68.         // adjusted gyroscope measurements
  69.         gx = gx + Kp*ex + exInt;
  70.         gy = gy + Kp*ey + eyInt;
  71.         gz = gz + Kp*ez + ezInt;

  72. 用叉积误差来做PI修正陀螺零偏




  73.         // integrate quaternion rate and normalise
  74.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  75.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  76.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  77.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  78. 四元数微分方程

  79.       

  80.         // normalise quaternion
  81.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  82.         q0 = q0 / norm;
  83.         q1 = q1 / norm;
  84.         q2 = q2 / norm;
  85.         q3 = q3 / norm;
  86. 四元数规范化
  87. }

  88. //====================================================================================================
  89. // END OF CODE
  90. //====================================================================================================
复制代码

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
来自 2楼
 楼主| 发表于 2012-9-13 09:34:39 | 只看该作者
werren 发表于 2012-9-9 20:14
樓主,我按照你的代碼使用陀螺儀和加速度計輸出俯仰和橫滾角都是很穩定的,但是航向角每隔一段時間就增加 ...

是的,陀螺一般有零点漂移、非线性、加速度影响等等多种误差。
其中零点漂移影响最大,但在初始化(保持静止读取一段时间平均值作为零点)后,漂移还可以忍受。不过温度变化大,漂移也比较厉害。
没有磁力计,是无法对航向角做纠正融合的。

以下代码是上面那段代码的磁力计+加计+陀螺版,没仔细研究过,粗看看像是把磁阻mxyz的向量转到地理坐标系,然后用地理坐标系的正北向标准磁场向量取代变成bxyz?又转回机体坐标系变成wxyz,最后和原始磁阻测量值mxyz做向量叉积来修正陀螺。
想学习的就研究研究吧

我自己做的磁阻耦合很简单很粗暴。把磁阻方向角解出来,和四元数解除的欧拉角航向角做个差,再用k滤波器把差值融合到欧拉角航向角上去,然后直接把欧拉角转成四元数。

还有23楼卖传感器的,这不是算法的实时问题,八成是你没用心研究,拷来代码就想直接用。
halfT 0.5f需要根据具体更新周期来调整,T是更新周期,T*角速度=微分角度,能在帖子里容易找到的东西我真不想多说。
  1. //=====================================================================================================
  2. // AHRS.c
  3. // S.O.H. Madgwick
  4. // 25th August 2010
  5. //=====================================================================================================
  6. // Description:
  7. //
  8. // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
  9. // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
  10. // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
  11. // axis only.
  12. //
  13. // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
  14. //
  15. // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
  16. // orientation.  See my report for an overview of the use of quaternions in this application.
  17. //
  18. // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
  19. // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
  20. // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
  21. //
  22. //=====================================================================================================

  23. //----------------------------------------------------------------------------------------------------
  24. // Header files

  25. #include "AHRS.h"
  26. #include <math.h>

  27. //----------------------------------------------------------------------------------------------------
  28. // Definitions

  29. #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  30. #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  31. #define halfT 0.5f                // half the sample period

  32. //---------------------------------------------------------------------------------------------------
  33. // Variable definitions

  34. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  35. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  36. //====================================================================================================
  37. // Function
  38. //====================================================================================================

  39. void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  40.         float norm;
  41.         float hx, hy, hz, bx, bz;
  42.         float vx, vy, vz, wx, wy, wz;
  43.         float ex, ey, ez;

  44.         // auxiliary variables to reduce number of repeated operations
  45.         float q0q0 = q0*q0;
  46.         float q0q1 = q0*q1;
  47.         float q0q2 = q0*q2;
  48.         float q0q3 = q0*q3;
  49.         float q1q1 = q1*q1;
  50.         float q1q2 = q1*q2;
  51.         float q1q3 = q1*q3;
  52.         float q2q2 = q2*q2;   
  53.         float q2q3 = q2*q3;
  54.         float q3q3 = q3*q3;         
  55.        
  56.         // normalise the measurements
  57.         norm = sqrt(ax*ax + ay*ay + az*az);      
  58.         ax = ax / norm;
  59.         ay = ay / norm;
  60.         az = az / norm;
  61.         norm = sqrt(mx*mx + my*my + mz*mz);         
  62.         mx = mx / norm;
  63.         my = my / norm;
  64.         mz = mz / norm;         
  65.        
  66.         // compute reference direction of flux
  67.         hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  68.         hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  69.         hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
  70.         bx = sqrt((hx*hx) + (hy*hy));
  71.         bz = hz;        
  72.        
  73.         // estimated direction of gravity and flux (v and w)
  74.         vx = 2*(q1q3 - q0q2);
  75.         vy = 2*(q0q1 + q2q3);
  76.         vz = q0q0 - q1q1 - q2q2 + q3q3;
  77.         wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  78.         wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  79.         wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  80.        
  81.         // error is sum of cross product between reference direction of fields and direction measured by sensors
  82.         ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  83.         ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  84.         ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
  85.        
  86.         // integral error scaled integral gain
  87.         exInt = exInt + ex*Ki;
  88.         eyInt = eyInt + ey*Ki;
  89.         ezInt = ezInt + ez*Ki;
  90.        
  91.         // adjusted gyroscope measurements
  92.         gx = gx + Kp*ex + exInt;
  93.         gy = gy + Kp*ey + eyInt;
  94.         gz = gz + Kp*ez + ezInt;
  95.        
  96.         // integrate quaternion rate and normalise
  97.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  98.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  99.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  100.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  101.        
  102.         // normalise quaternion
  103.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  104.         q0 = q0 / norm;
  105.         q1 = q1 / norm;
  106.         q2 = q2 / norm;
  107.         q3 = q3 / norm;
  108. }

  109. //====================================================================================================
  110. // END OF CODE
  111. //====================================================================================================
复制代码
3
发表于 2012-8-16 10:29:02 | 只看该作者
精辟,接收学习
4
发表于 2012-8-16 11:28:04 | 只看该作者
学习了
5
发表于 2012-8-17 10:40:02 | 只看该作者
谢谢分享~请问用PI参数估计零偏能补偿加速度测得的外力加速度吗?实际效果怎么样
6
发表于 2012-8-17 10:51:05 | 只看该作者
牛人,顶一个,慢慢看
7
 楼主| 发表于 2012-8-17 12:37:12 | 只看该作者
本帖最后由 seanwood 于 2012-8-17 12:50 编辑
40342zz 发表于 2012-8-17 10:40
谢谢分享~请问用PI参数估计零偏能补偿加速度测得的外力加速度吗?实际效果怎么样 ...


补偿,得有观测者才能对测量者做纠正。
加计本来是观测者,对陀螺补偿。
对外力加速度来说,加计属于测量者,没有外力的观测者,无法补偿加计。
如果是震动,可以调小PI参数,靠长时间的数值平均,减小震动引起的加速度误差。
而机体转弯的离心力可以用 GPS地速×陀螺三维角速度,根据离心力公式算出三维离心加速度,去减掉转弯带来的加速度影响。
但是机体的纵向加速度就没法补偿了,不过一般情况下长时间单方向加速的情况较少,误差可以容忍。

如果要高精度的姿态测算,那除非再引入观测者,比如GPS的坐标定位信息。也就是所谓的GPS/INS组合导航,一般是卡尔曼算法。
举个例子,比如惯导中当前估算速度为匀速xx米/秒,移动方向为北向,所以估算1秒后坐标会向北移动XX米。
1秒后GPS给出信息,和惯导估算的相比,坐标向北多移动了YY米。,变成了XX+YY米。
那么应该是有外力加速度,导致机体北向速度变快,本周期平均值为YY-XX米/秒^2(只是举个例子,数值不做准)系统就会这外力加速度补偿到加计的测量值里去。

GPS/INS组合导航,系统就会运算出姿态和坐标定位信息,是融合过的,都非常准。卡尔曼算法很难,我不懂,只知道个大概原理。

一言蔽之,这些算法都是以测量者的数值为基础,用观测者观察到的其他方面的数值,用一个转换算法去推断测量者的数值误差,然后纠正之。而这就是卡尔曼算法的流程。
8
发表于 2012-8-19 10:56:22 | 只看该作者
真是好文章,超级喜欢这个帖子,感谢楼主~!!!!
9
发表于 2012-8-19 11:20:47 | 只看该作者
多谢分享  再看一下                                                                                                   
10
发表于 2012-8-19 11:37:47 来自手机 | 只看该作者
很好的帖子,感谢楼主
来自:amoBBS 阿莫电子论坛 Android客户端
11
发表于 2012-8-19 12:04:33 | 只看该作者
计算初始欧拉角   重力被分到3个轴上了   LZ的计算初始角公式不该那样吧
12
发表于 2012-8-19 12:20:22 | 只看该作者
支持原创,留个位置慢慢研究
13
发表于 2012-8-19 12:49:22 | 只看该作者
支持一下
14
 楼主| 发表于 2012-8-20 08:35:08 | 只看该作者
蓝海de梦 发表于 2012-8-19 12:04
计算初始欧拉角   重力被分到3个轴上了   LZ的计算初始角公式不该那样吧

请注意初始角是欧拉角,所以要遵循欧拉角的定义来计算。
通常思维计算出来的倾角不知道数学上是什么名字,我称之为平面夹角,反正与欧拉角不一样。
15
发表于 2012-8-20 12:42:44 | 只看该作者
解算方法直接用fpga的时序和组合逻辑,不知可否实现?
16
 楼主| 发表于 2012-8-20 16:38:26 | 只看该作者
四元数或欧拉角都要用到浮点和三角函数,我没做过fpga,不过想想应该搞不定。
17
发表于 2012-8-22 08:49:48 | 只看该作者
seanwood 发表于 2012-8-20 16:38
四元数或欧拉角都要用到浮点和三角函数,我没做过fpga,不过想想应该搞不定。 ...

在实际应用中,传感器数据在输入捷联系统前,是否要经过预处理,去除器件抖动和干扰(比如机架抖动)?
18
发表于 2012-8-22 10:59:42 来自手机 | 只看该作者
验证过此算法,简单可用…
来自:amoBBS 阿莫电子论坛 Android客户端
19
发表于 2012-8-22 12:57:12 | 只看该作者
努力分析中……
20
发表于 2012-8-22 16:59:03 | 只看该作者
高手,学习中……
21
发表于 2012-8-23 13:28:31 | 只看该作者
四元数更新陀螺仪的数据,加速度计和磁罗盘修正陀螺仪的数据,这些都是基本的,关键是如何做的准确,特别是在盘旋的的时候和机动性强的时候。
22
发表于 2012-8-23 20:31:34 | 只看该作者
高科技,大学时候有个老师的课题就是这个,一连串公司看死哥了
23
 楼主| 发表于 2012-8-24 08:50:26 | 只看该作者
本帖最后由 seanwood 于 2012-8-24 08:52 编辑
wjhltk 发表于 2012-8-23 13:28
四元数更新陀螺仪的数据,加速度计和磁罗盘修正陀螺仪的数据,这些都是基本的,关键是如何做的准确,特别是 ...


我怎么没看到四轴论坛里有这些知识贴?这些关键公式都是我啃书啃出来的。
有货就拿出来分享一下,想藏着掖着就别来说风凉话,一句基本的,难道大家就学会如何做了?
24
发表于 2012-8-28 10:20:28 | 只看该作者
分享的精神最伟大!顶楼主
25
发表于 2012-8-28 11:03:11 | 只看该作者
刚刚买了本 《捷联惯性导航技术》 准备研究一下。
26
发表于 2012-8-28 21:11:34 | 只看该作者
如何理解“向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示”?两个向量直接做差不就是误差吗?
27
发表于 2012-9-1 19:06:37 | 只看该作者
学习了。。。。。。。。。。。。
28
发表于 2012-9-4 22:15:21 | 只看该作者
高手,写得不错,正在学习这块。
29
 楼主| 发表于 2012-9-5 08:01:00 | 只看该作者
本帖最后由 seanwood 于 2012-9-5 08:03 编辑
QuadRotor 发表于 2012-8-28 21:11
如何理解“向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示”?两个向量直接做差不就是误差吗? ...


这里误差没说清楚,不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与重力向量重合,需要绕什么轴,转多少角度
逆向推理一下,这个叉积在机体三轴上的投影,就是加计和重力之间的角度误差。
也就是说,如果陀螺按这个叉积误差的轴,转动叉积误差的角度(也就是转动三轴投影的角度)那就能把加计和重力向量的误差消除掉。(具体可看向量叉积的定义)
如果完全按叉积误差转过去,那就是完全信任加计。如果一点也不转,那就是完全信任陀螺。
那么把这个叉积的三轴乘以X%,加到陀螺的积分角度上去,就是这个x%互补系数的互补算法了。

ps:实际上叉积的length是两向量夹角的正弦,而且必须在±90度以内,并不完全与误差角度成线性正比。
如果转成三轴夹角,按欧拉角的转动顺序分解到三轴上去,会很麻烦。
这里的叉积算法把sin误差当成角度误差,并无视欧拉角的转动顺序,在误差较小的时候,并不会有影响。
因为这个修正对误差来说,是收敛的,正常情况下误差只会越来越小。
为了解释方便,所以上面就把叉积等同于角度误差了。
30
发表于 2012-9-5 22:57:03 | 只看该作者
楼主总结的很精僻,把我这些天来看的资料理顺了,终于有了个思路。
31
发表于 2012-9-5 23:27:18 | 只看该作者
非常好的资料啊!
32
发表于 2012-9-5 23:51:50 | 只看该作者
精辟!收藏了
33
发表于 2012-9-6 21:26:00 | 只看该作者
我试验了楼主贴的代码。发现在静止的情况下计算的欧拉角比较正确,但是稍微转动一下角度,计算额结果就会有很大突变,知道再次静止很久后数值才能恢复稳定。请问需要调节哪个参数可以改善这个算法实时性能?
34
发表于 2012-9-8 12:49:16 | 只看该作者
精辟,学习了~
35
发表于 2012-9-8 23:25:28 | 只看该作者
学习了。
36
发表于 2012-9-8 23:52:04 | 只看该作者
seanwood 发表于 2012-8-24 08:50
我怎么没看到四轴论坛里有这些知识贴?这些关键公式都是我啃书啃出来的。
有货就拿出来分享一下,想藏着 ...

这里顶楼主
37
发表于 2012-9-9 01:31:50 | 只看该作者
好贴, 学习
38
发表于 2012-9-9 20:14:17 | 只看该作者
seanwood 发表于 2012-9-5 08:01
这里误差没说清楚,不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与重力向量重合,需要绕什 ...

樓主,我按照你的代碼使用陀螺儀和加速度計輸出俯仰和橫滾角都是很穩定的,但是航向角每隔一段時間就增加0.1度,這個是不是所謂的“漂移”,如果沒有磁力計修正的話是否無法避免。能否做到我隨便一個初始姿態之後亂轉N圈后又回到初始狀態它的輸出值沒有漂移?
39
发表于 2012-9-9 20:26:39 | 只看该作者
哇...居然有这么好的文章,留名,以后会用到的
40
发表于 2012-9-12 16:30:08 | 只看该作者
mark!
41
发表于 2012-9-13 10:03:41 | 只看该作者
seanwood 发表于 2012-9-13 09:34
是的,陀螺一般有零点漂移、非线性、加速度影响等等多种误差。
其中零点漂移影响最大,但在初始化(保持 ...

多謝耐心回覆!
       現在我是想把這個算出來的角度用作小車導航用,之前聽別人說起磁力計比較容易受干擾,特別是小車直流馬達轉動的時候容易不准確(自己沒有試過,姑且信了)。所以現在我想只用一顆MPU6050(帶加計和陀螺的)不加磁力計來進行角度的計算。如果這樣的漂移(不管靜態還是動態)不能減小到很小的範圍,恐怕小車沒轉幾個彎就已經“找不著北”了。
      目前的狀況是俯仰和橫滾角基本上不會動,但是水平的航向角(沒有地磁方向)在靜止的生活大概沒5秒增加0.1度,不知道能不能用每計時5S減去0.1度來補償?俯仰和橫滾應該是加速度計的一個準確值吧,所以不怎麼動?
      
      有沒有辦法可以消除這些偏差?我只需要用到水平旋轉角度。靜態和動態最好通通無線趨近與0漂移。
42
 楼主| 发表于 2012-9-13 10:07:35 | 只看该作者
仔细看了一会,上面的磁力计修正流程已经想通了,看看大家想没想通吧。

陀螺的零点漂移可以靠加计和磁力计来补偿,不过加计和磁力计的零点漂移、椭圆修正就没法动态补偿了。因为没有观测者。
只能在初始化的时候修正,但是温度的影响就不太好办。
gps/ins是王道,可惜高阶卡尔曼实在不好弄,谁有心得分享一下吧。

不过一般玩玩,这些算法够了。
43
 楼主| 发表于 2012-9-13 10:22:26 | 只看该作者
werren 发表于 2012-9-13 10:03
多謝耐心回覆!
       現在我是想把這個算出來的角度用作小車導航用,之前聽別人說起磁力計比較容易受干 ...

你說的5秒0.1度,就是靜態零點偏移,可以依靠初始化來避免,就是靜止,取一段時間陀螺輸出做平均,作為固定誤差。
但隨著溫度、加速度(具體名字忘記了,陀螺手冊上有,外力加速度會對角速度造成干擾)、量程非線性誤差、算法誤差、計算誤差等等影響,航向早晚會漂移的。
沒有觀測者,測量者無法得知自身的偏差。無論白線軌跡、紅外、GPS、磁力計,必須有一個參考源,才能修正航向。
44
 楼主| 发表于 2012-9-13 10:27:50 | 只看该作者
磁力計離電機多遠能避免干擾我沒試過,不過我試過離DC-DC電感5-7cm后,干擾就微乎其微了。
另外據我所知電機的磁力線應該是比較封閉的,這樣電機效率才高。
另外通過磁力計橢圓修正,可以去除靜態磁力干擾。動態的可以通過多次測量平均濾波消除一些。
我相信離開電機一定距離,干擾可以忽略。
但是室內小車估計受周圍物體影響較大。
45
发表于 2012-9-16 09:12:19 | 只看该作者
seanwood 发表于 2012-9-13 10:27
磁力計離電機多遠能避免干擾我沒試過,不過我試過離DC-DC電感5-7cm后,干擾就微乎其微了。
另外據我所知電 ...

   陀螺仪给出的欧拉角,俯仰和翻滚都能由加速度修正,偏航怎么修正?静置的时候偏航角度会朝着一个方向递增(也有可能是减,我的系统是递增)
46
发表于 2012-9-16 09:51:49 | 只看该作者
seanwood 发表于 2012-9-13 10:22
你說的5秒0.1度,就是靜態零點偏移,可以依靠初始化來避免,就是靜止,取一段時間陀螺輸出做平均,作為固 ...

初始化偏置之后改善很多,水平放置的时候开始启动时候会偏一点,之后就基本稳定了,很久(几分钟)才会动0.1度。也就说仍然存在误差,不知道该误差可否完全消除?
我的小车在转悠了N圈之后得到的相对航向能否还是准确的?
47
 楼主| 发表于 2012-9-17 11:54:34 | 只看该作者
seanwood 发表于 2012-9-13 10:22
你說的5秒0.1度,就是靜態零點偏移,可以依靠初始化來避免,就是靜止,取一段時間陀螺輸出做平均,作為固 ...

你說的5秒0.1度,就是靜態零點偏移,可以依靠初始化來避免,就是靜止,取一段時間陀螺輸出做平均,作為固定誤差。
但隨著溫度、加速度(具體名字忘記了,陀螺手冊上有,外力加速度會對角速度造成干擾)、量程非線性誤差、算法誤差、計算誤差等等影響,航向早晚會漂移的。
沒有觀測者,測量者無法得知自身的偏差。無論白線軌跡、紅外、GPS、磁力計,必須有一個參考源,才能修正航向
48
发表于 2012-9-17 12:41:04 | 只看该作者
好帖子要mark
49
发表于 2012-9-17 13:09:58 | 只看该作者
向牛人学习
50
发表于 2012-9-17 13:48:17 | 只看该作者
perfect
51
发表于 2012-9-19 00:23:58 | 只看该作者
在看书中……
52
发表于 2012-9-19 00:48:01 | 只看该作者
本帖最后由 aishadow 于 2012-9-19 02:28 编辑

楼主专研精神很强大,费心了,很感谢楼主的分享,我现在对姿态算法又有了深刻的了解,有些地方还不是很清楚  四元素float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   这四个参数的  初始值代表什么意义。值是怎么来的?算法是不是容忍陀螺仪漂移存在,而是和加计比较的误差通过PI修正陀螺仪每次的测量值,而不是重新再去修改原来的参考坐标点,是吗。说的有点乱,类似说漂移使中立点变了,一般我们是从新修正一个中立点,但我看这个没有。
53
 楼主| 发表于 2012-9-19 14:09:07 | 只看该作者
本帖最后由 seanwood 于 2012-9-19 14:13 编辑
aishadow 发表于 2012-9-19 00:48
楼主专研精神很强大,费心了,很感谢楼主的分享,我现在对姿态算法又有了深刻的了解,有些地方还不是很清楚 ...


四元数q的初始值等同于是欧拉角为0,0,0,根据东北天的坐标定义,也就是机体完全水平朝北。前面不是有四元数初始化公式吗?欧拉角三个0度算出来就是1,0,0,0。
代码里的初始值并没有意义,那段代码只是原理性的,离实用还有些距离。
实际需要在初始化时根据加计、磁阻计算欧拉角,并初始化四元数。

误差靠什么修正?就是把原来的四元数(姿态)旋转这个误差角度。
下个周期四元数靠什么更新?也是靠陀螺的旋转积分角度去更新。
都是旋转某个角度,把这两者分开、合并没什么多少区别。所以这代码里是把两者合并到下个更新周期里一起旋转。

也许你会说两个旋转有先后顺序,是有的。
但本身四元数微分方程(欧拉角微分方程)就是把同时转动的三个角度(一个旋转)当做有先后顺序的三个欧拉旋转来处理。所以这两个转动叠在一起计算也没关系。就因为是有了微分的大前提。
象这代码用的一阶龙格库塔算法,把sin(x)等同于x、陀螺的旋转当做三个欧拉旋转、积分周期内的角速度当做匀速运动,都是微分简化的方式,有兴趣可以去研究研究锥形运动误差、转动向量法。

陀螺零点漂移修正,就是PI中的I。
PI的概念还用我说吗?每个更新周期都会使用I(exyzInt)来抑制每个更新周期都会出现的零点偏移。
54
发表于 2012-9-19 15:06:49 | 只看该作者
mark~~~~~~传感器到手后研究研究
55
发表于 2012-9-19 16:13:05 | 只看该作者
seanwood 发表于 2012-9-19 14:09
四元数q的初始值等同于是欧拉角为0,0,0,根据东北天的坐标定义,也就是机体完全水平朝北。前面不是有四元 ...


3ks very maqi
56
发表于 2012-9-26 07:49:02 | 只看该作者
mark........
57
发表于 2012-9-26 11:02:35 | 只看该作者
本帖最后由 aishadow 于 2012-9-26 11:04 编辑

楼主 假设我四轴初始状态是水平状态 硬件只只用到了加速度陀螺仪  初始化的我把四轴放在地上  我初始四元素的时候认为加速度其它无分量。偏航角因为没有磁阻参考也认为和世界坐标系的一致。换句话说就拿四轴的此时状态来当世界坐标系。那么我初始的时候就可以直接用初始值1,0 ,0,0. 或者另一种算法 假设偏航并未偏移,而我放在地上(有可能地面并不水平),那么我只用加速度算出横滚角r 俯仰角(这个符号不会打)。偏航角W=0,那么我是不是可用这个方式把得到的值带入四元素初始公式进行运算从而得到q0 q1  q2 q3?然后再更新姿态。? 这两钟方式是否具有可行性?谢谢
58
发表于 2012-9-26 11:51:41 | 只看该作者
mark                  
59
 楼主| 发表于 2012-9-26 13:22:00 | 只看该作者
aishadow 发表于 2012-9-26 11:02
楼主 假设我四轴初始状态是水平状态 硬件只只用到了加速度陀螺仪  初始化的我把四轴放在地上  我初始四元素 ...

四元数初始化,顶楼有公式,知道三个欧拉角就可以求出q0-3。
欧拉角可以根据加计数值计算,顶楼也有公式。根据加计计算初始欧拉角
没磁阻,你就按当前偏航为基准0度,只要求出俯仰和横滚欧拉角,偏航为0,代入四元数初始化公式,就可以求出q0-3来了。
那两种方式本来就是同一个方式,或者根本不能算是方式,而是本来就是该这么算的。
60
发表于 2012-9-26 19:09:43 | 只看该作者
mark 慢慢看
61
发表于 2012-9-27 14:14:15 | 只看该作者
好贴!
62
发表于 2012-9-27 15:10:58 | 只看该作者
太深奥了
63
发表于 2012-9-27 19:43:45 | 只看该作者
seanwood 发表于 2012-9-26 13:22
四元数初始化,顶楼有公式,知道三个欧拉角就可以求出q0-3。
欧拉角可以根据加计数值计算,顶楼也有公式 ...

热心肠的技术牛人,不说了 眼泪哗哗的
64
发表于 2012-10-29 15:44:39 | 只看该作者
学习了,楼主精神可嘉!
65
发表于 2012-10-29 20:32:50 | 只看该作者
很好的东西,MARK 下。
66
发表于 2012-10-29 21:08:14 | 只看该作者
太感谢楼主的分享精神!
67
发表于 2012-10-30 11:36:15 | 只看该作者
seanwood 发表于 2012-9-13 10:07
仔细看了一会,上面的磁力计修正流程已经想通了,看看大家想没想通吧。

陀螺的零点漂移可以靠加计和磁力计 ...

楼主能讲下磁具体是怎么用的吗?

是不是通过磁算出一个偏航角,然后修正陀螺?
68
发表于 2012-10-30 12:51:18 | 只看该作者
mark!!
69
发表于 2012-10-30 12:54:24 | 只看该作者
好东西,mark
70
发表于 2012-10-30 13:11:13 | 只看该作者
htjgdw 发表于 2012-9-6 21:26
我试验了楼主贴的代码。发现在静止的情况下计算的欧拉角比较正确,但是稍微转动一下角度,计算额结果就会有 ...

可能是解算的周期没定准吧??
71
发表于 2012-10-30 13:15:38 | 只看该作者
seanwood 发表于 2012-9-17 11:54
你說的5秒0.1度,就是靜態零點偏移,可以依靠初始化來避免,就是靜止,取一段時間陀螺輸出做平均,作為固 ...

问一下,我用两套一样的陀螺加计组合,安在不同位置,它们互为对方的观测者行吗?
72
发表于 2012-10-30 20:11:07 | 只看该作者
学习啊  惯性导航
73
发表于 2012-11-3 22:15:20 | 只看该作者
mark
74
发表于 2012-11-4 08:50:11 | 只看该作者
mark!!!!!!!
75
发表于 2012-11-4 11:09:06 | 只看该作者
学习
76
发表于 2012-11-5 12:33:02 | 只看该作者
感谢楼主,学习了!
77
发表于 2012-11-29 06:23:27 | 只看该作者
Mark.            
78
发表于 2012-11-29 11:23:59 | 只看该作者
这篇文章绝对是精品中的精品。
79
发表于 2012-11-29 12:38:42 | 只看该作者
好东西,mark一下,以后需要用的时候慢慢研究
80
发表于 2012-12-1 00:00:34 | 只看该作者
努力学习了~~学基础
81
发表于 2012-12-1 00:28:05 | 只看该作者
看懂了就可以玩四轴了,真好!
82
发表于 2012-12-1 02:06:38 来自手机 | 只看该作者
好好的帖子啊!。
83
发表于 2012-12-1 08:50:26 | 只看该作者
不懂的,留个脚印!
84
发表于 2012-12-1 12:18:01 | 只看该作者
这是我看懂的第一个四元数程序,感谢楼主!
85
发表于 2012-12-1 21:59:05 | 只看该作者
向LZ学习!!
86
发表于 2012-12-1 23:28:31 | 只看该作者
谢谢 楼主      
87
发表于 2012-12-3 16:57:57 | 只看该作者
mark!!!
88
发表于 2012-12-5 01:44:12 | 只看该作者
automaticdai 发表于 2012-11-29 11:23
这篇文章绝对是精品中的精品。

完全同意
89
发表于 2012-12-5 07:58:04 | 只看该作者
好好学习一下,感谢楼主!
90
发表于 2012-12-5 10:36:16 | 只看该作者
不知道上述代码能否直接拿来放在stm32上做四轴,
还是运算需要一定的简化?
91
发表于 2012-12-11 13:57:52 | 只看该作者
mark!!!!
92
发表于 2012-12-11 21:22:18 | 只看该作者
我看到你的捷联惯导算法心得真的很有帮助 我现在卡住了。
1.我现在把加速度和陀螺仪的roll和pitch用卡尔曼算出来了,滤波很好。
2我现在的问题就是如何利用地磁传感器去去把yaw给算出来,然后和陀螺仪的Z去卡尔曼出来准确的YAW,这样我就得到3个轴的准确数值。
93
发表于 2012-12-12 00:11:42 | 只看该作者
学习了,谢谢楼主
94
发表于 2012-12-12 12:37:17 | 只看该作者
Freezing_ 发表于 2012-8-22 08:49
在实际应用中,传感器数据在输入捷联系统前,是否要经过预处理,去除器件抖动和干扰(比如机架抖动)? ...

带通滤波。
95
发表于 2013-1-12 15:40:59 | 只看该作者
学习了,不错!看了几篇关于IMU的资料这篇是最好的.
96
发表于 2013-1-12 18:14:17 | 只看该作者
顶,记下先.
97
发表于 2013-1-27 08:02:55 | 只看该作者
百年好贴,马克
98
发表于 2013-1-27 08:47:46 | 只看该作者
这个一定要学习啊,捷联惯性导航系统,改天搞个导弹出来
99
发表于 2013-2-5 11:14:48 | 只看该作者
seanwood 发表于 2012-8-24 08:50
我怎么没看到四轴论坛里有这些知识贴?这些关键公式都是我啃书啃出来的。
有货就拿出来分享一下,想藏着 ...

没错,说风凉话的人就该骂!好像在他面前什么都简单,可真让做就傻眼了。
100
发表于 2013-2-10 17:19:51 | 只看该作者
很好,看完又明了不少
友情提示:标题不合格、重复发帖,将会被封锁ID。详情请参考:论坛通告:封锁ID、获得注册邀请码、恢复被封ID、投诉必读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|阿莫电子论坛(原ourAVR/ourDEV) ( 粤ICP备09047143号 )

GMT+8, 2017-8-18 14:49

阿莫电子论坛, 原"中国电子开发网"

© 2004-2016 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表