文档章节

捷联惯导算法心得

Jr小王子
 Jr小王子
发布于 2016/10/27 18:06
字数 2518
阅读 45
收藏 0
点赞 0
评论 0

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. //----------------------------------------------------------------------------------------------------
  22. // Header files
  23.  
  24. #include "IMU.h"
  25. #include <math.h>
  26.  
  27. //----------------------------------------------------------------------------------------------------
  28. // Definitions
  29.  
  30. #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  31. #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  32. #define halfT 0.5f                // half the sample period
  33.  
  34. //---------------------------------------------------------------------------------------------------
  35. // Variable definitions
  36.  
  37. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  38. float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error
  39.  
  40. //====================================================================================================
  41. // Function
  42. //====================================================================================================
  43.  
  44. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
  45.         float norm;
  46.         float vx, vy, vz;
  47.         float ex, ey, ez;         
  48.        
  49.         // normalise the measurements
  50.         norm = sqrt(ax*ax + ay*ay + az*az);      
  51.         ax = ax / norm;
  52.         ay = ay / norm;
  53.         az = az / norm;      
  54. 把加计的三维向量转成单位向量。
  55.        
  56.  
  57.         // estimated direction of gravity
  58.         vx = 2*(q1*q3 - q0*q2);
  59.         vy = 2*(q0*q1 + q2*q3);
  60.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
  61.  
  62. 这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。
  63. 根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到机体坐标系,正好是这三个元素。
  64. 所以这里的vx\y\z,其实就是当前的欧拉角(即四元数)的机体坐标参照系上,换算出来的重力单位向量。
  65.  
  66.  
  67.         // error is sum of cross product between reference direction of field and direction measured by sensor
  68.         ex = (ay*vz - az*vy);
  69.         ey = (az*vx - ax*vz);
  70.         ez = (ax*vy - ay*vx);
  71.  
  72. axyz是机体坐标参照系上,加速度计测出来的重力向量,也就是实际测出来的重力向量。
  73. axyz是测量得到的重力向量,vxyz是陀螺积分后的姿态来推算出的重力向量,它们都是机体坐标参照系上的重力向量。
  74. 那它们之间的误差向量,就是陀螺积分后的姿态和加计测出来的姿态之间的误差。
  75. 向量间的误差,可以用向量叉积(也叫向量外积、叉乘)来表示,exyz就是两个重力向量的叉积。
  76. 这个叉积向量仍旧是位于机体坐标系上的,而陀螺积分误差也是在机体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺。(你可以自己拿东西想象一下)由于陀螺是对机体直接积分,所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。
  77.  
  78.  
  79.  
  80.         // integral error scaled integral gain
  81.         exInt = exInt + ex*Ki;
  82.         eyInt = eyInt + ey*Ki;
  83.         ezInt = ezInt + ez*Ki;
  84.  
  85.         // adjusted gyroscope measurements
  86.         gx = gx + Kp*ex + exInt;
  87.         gy = gy + Kp*ey + eyInt;
  88.         gz = gz + Kp*ez + ezInt;
  89.  
  90. 用叉积误差来做PI修正陀螺零偏
  91.  
  92.  
  93.  
  94.  
  95.         // integrate quaternion rate and normalise
  96.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  97.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  98.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  99.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  100. 四元数微分方程
  101.  
  102.        
  103.  
  104.         // normalise quaternion
  105.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  106.         q0 = q0 / norm;
  107.         q1 = q1 / norm;
  108.         q2 = q2 / norm;
  109.         q3 = q3 / norm;
  110. 四元数规范化
  111. }
  112.  
  113. //====================================================================================================
  114. // END OF CODE
  115. //====================================================================================================

复制代码

本文转载自:http://www.amobbs.com/thread-5492189-1-1.html

共有 人打赏支持
Jr小王子
粉丝 10
博文 107
码字总数 18368
作品 0
深圳
程序员
游戏开发与程序设计知识总结03——算法

更新日志 每此对思维导图有改动或者在github中有了对应的实现,则增加一条更新日志。 2017.9.2: 确定更新为系列文章并持续维护 前言 这是游戏开发与程序设计知识总结系列文章的第三篇算法,...

kashiwa ⋅ 2017/09/02 ⋅ 0

专访DeepMotion蔡锐:颠覆传统图商,高精地图时代,数据的流通是双向的

高精地图对于自动驾驶车辆的定位、导航与控制至关重要,这项技术已经成为兵家必争之地,但今天高精地图的应用标准、服务模式还在探索当中。 近日,刚刚宣布获得千万美元A轮融资的DeepMotion深...

利荣 ⋅ 03/27 ⋅ 0

VR中的9轴传感器(重力加速度/陀螺仪/磁力计)

转自:https://blog.csdn.net/dabenxiong666/article/details/53836503 前言 传感器的调试过程,一般根据原厂提供demo代码,调试数据接口,将数据流打通即可,在VR中,当带上头显设备,运行应...

williamgavin ⋅ 04/24 ⋅ 0

软银引进爱立信机器学习算法,升级无线电接入网络;国家文物局联合百度启动“AI 博物馆计划” | AI 掘金晚报

软银引进爱立信机器学习算法,升级无线电接入网络 雷锋网消息 据外媒报道,爱立信宣布,它将利用机器学习算法升级日本电信运营商软银集团在东海(日本地名)地区的无线电接入网络。 爱立信表示...

刘伟 ⋅ 05/18 ⋅ 0

锐捷网络:十一年深耕智启医疗 服务上亿病患云联健康

“华山”论剑,锐捷携手用户探索医疗行业的大数据之路 10月14日,以“智启医疗云联健康”为主题的“大数据时代医院信息化平台建设与应用研讨会暨医疗卫生CIO高峰论坛” 在上海开幕,来自全国...

玄学酱 ⋅ 05/11 ⋅ 0

pyparsing语法解析心得

一直想总结一年来开发维护导表工具的心得,却因为懒或者项目紧而长期搁置着。最近一个milestone结束之后,有了短暂的空闲调整期,正好趁着这段时间系统得整理一下,也算是一种备份,方便以后...

窗台跳舞的猫 ⋅ 2012/12/04 ⋅ 0

大一ACM心得总结

看了我的大学长@侯盛栋学长 对他的ACM之路进行了深刻的总结 ,我觉得是时候应该对我的2016做个小结吧. 说实在话, 从开学起,我先后加入了学务中心,各种社团,原本想在里面一展风采,和志趣相投的...

angel_kitty ⋅ 2017/02/04 ⋅ 0

智慧教室解决方案,让“智慧”贯穿教室

教室作为教师和学生的教学和学习的主要场地,可以说是人们多年来关注的重点,而随着“智慧”为主题的技术变革,智慧教室解决方案开始应时代步伐而生。智慧教室具有智慧技术、智慧应用、智慧管...

你好啊、 ⋅ 06/12 ⋅ 0

央行公示百行征信:互金协会持股36%,8家个人征信试点机构8%

雷锋网(公众号:雷锋网)报道,1月4日,央行官网称,正在受理了百行征信有限公司(筹)的个人征信业务申请,并对百行征信有限公司(筹)相关情况进行公示。 最后的情况与多家主流媒体的披露基...

伊莉 ⋅ 01/04 ⋅ 0

快速设置XMind中的设置联系

 我们在XMind思维导图中绘制导图时,每个主题间或多或少都存在着一定的关联,这也是我们对导图进行构想的关键,那么我们又需要运用什么来表明这些内部存在的关联呢,这就是今天要讲的如何使...

xmind ⋅ 2015/10/19 ⋅ 0

没有更多内容

加载失败,请刷新页面

加载更多

下一页

C++内存映射文件居然是这样?!

内存映射文件大家都时不时听过,但它到底是个什么?赶紧来看看吧 内存映射文件到底是干嘛的呢?让我们先来思考下面几个问题: 如果您想读的内容大于系统分配的内存块怎么办?如果您想搜索的字...

柳猫 ⋅ 23分钟前 ⋅ 0

MySQL 数据库设计总结

规则1:一般情况可以选择MyISAM存储引擎,如果需要事务支持必须使用InnoDB存储引擎。 注意:MyISAM存储引擎 B-tree索引有一个很大的限制:参与一个索引的所有字段的长度之和不能超过1000字节...

OSC_cnhwTY ⋅ 今天 ⋅ 0

多线程(四)

线程池和Exector框架 什么是线程池? 降低资源的消耗 提高响应速度,任务:T1创建线程时间,T2任务执行时间,T3线程销毁时间,线程池没有或者减少T1和T3 提高线程的可管理性。 线程池要做些什...

这很耳东先生 ⋅ 今天 ⋅ 0

使用SpringMVC的@Validated注解验证

1、SpringMVC验证@Validated的使用 第一步:编写国际化消息资源文件 编写国际化消息资源ValidatedMessage.properties文件主要是用来显示错误的消息定制 [java] view plain copy edit.userna...

瑟青豆 ⋅ 今天 ⋅ 0

19.压缩工具gzip bzip2 xz

6月22日任务 6.1 压缩打包介绍 6.2 gzip压缩工具 6.3 bzip2压缩工具 6.4 xz压缩工具 6.1 压缩打包介绍: linux中常见的一些压缩文件 .zip .gz .bz2 .xz .tar .gz .tar .bz2 .tar.xz 建立一些文...

王鑫linux ⋅ 今天 ⋅ 0

6. Shell 函数 和 定向输出

Shell 常用函数 简洁:目前没怎么在Shell 脚本中使用过函数,哈哈,不过,以后可能会用。就像java8的函数式编程,以后获取会用吧,行吧,那咱们简单的看一下具体的使用 Shell函数格式 linux ...

AHUSKY ⋅ 今天 ⋅ 0

单片机软件定时器

之前写了一个软件定时器,发现不够优化,和友好,现在重写了 soft_timer.h #ifndef _SOFT_TIMER_H_#define _SOFT_TIMER_H_#include "sys.h"typedef void (*timer_callback_function)(vo...

猎人嘻嘻哈哈的 ⋅ 今天 ⋅ 0

好的资料搜说引擎

鸠摩搜书 简介:鸠摩搜书是一个电子书搜索引擎。它汇集了多个网盘和电子书平台的资源,真所谓大而全。而且它还支持筛选txt,pdf,mobi,epub、azw3格式文件。还显示来自不同网站的资源。对了,...

乔三爷 ⋅ 今天 ⋅ 0

Debian下安装PostgreSQL的表分区插件pg_pathman

先安装基础的编译环境 apt-get install build-essential libssl1.0-dev libkrb5-dev 将pg的bin目录加入环境变量,主要是要使用 pg_config export PATH=$PATH:/usr/lib/postgresql/10/bin 进......

玛雅牛 ⋅ 今天 ⋅ 0

inno安装

#define MyAppName "HoldChipEngin" #define MyAppVersion "1.0" #define MyAppPublisher "Hold Chip, Inc." #define MyAppURL "http://www.holdchip.com/" #define MyAppExeName "HoldChipE......

backtrackx ⋅ 今天 ⋅ 0

没有更多内容

加载失败,请刷新页面

加载更多

下一页

返回顶部
顶部