【运动传感器】Madgwick算法(下)
在上篇中,讲解了分别从角速度计、加速度计、磁力计估计姿态的方法。本篇文章讲解他们的融合,对误差的处理,以及标定实验。
融合
这一部分在文章中称为filter。
在上一篇文章中,我们能够通过角速度计的读数ω\omegaω,加速度计或磁力计的读数dSd_SdS在每个采样间隔更新姿态四元数qqq:
qt+1=qt+12qt×ωtΔtq_{t+1} = q_{t} + \frac{1}{2}q_t \times \omega_t \Delta tqt+1=qt+21qt×ωtΔt
qt+1=qt−μ∇f∣∣∇f∣∣q_{t+1}=q_t - \mu \frac{\nabla f}{||\nabla f||}qt+1=qt−μ∣∣∇f∣∣∇f
其中fff是姿态拟合误差,可以通过当前姿态qqq和读数dSd_SdS计算得到。
两者融合有:
qt+1=qt+12qt×ωtΔt−β∇f∣∣∇f∣∣Δtq_{t+1} = q_{t} + \frac{1}{2}q_t \times \omega_t \Delta t - \beta \frac{\nabla f}{||\nabla f||}\Delta_tqt+1=qt+21qt×ωtΔt−β∣∣∇f∣∣∇fΔt
增量由两部分构成:
第一部分12qt×ωt\frac{1}{2}q_t \times \omega_t21qt×ωt为通过角速度计获取的结果。
如果角速度计是完美的,则只需这部分即可。
第二部分∇f∣∣∇f∣∣\frac{\nabla f}{||\nabla f||}∣∣∇f∣∣∇f为通过加速度计/磁力计获取的结果。其权重β\betaβ表示角速度计的误差。
如果误差为0,则不需要此项;否则,因为角速度计不完美,当前姿态qqq有误差,导致拟合误差fff没有达到极小,∇f\nabla f∇f不为0矢量,需要沿着误差下降方向对姿态qqq进行修正,修正幅度和角速度计误差成正比。
本部分证明原文参见Madgwick Internal Report3.3节。原始证明感觉就是凑的尚存疑,此处根据个人理解书写。
磁力计偏转
根据前文推导,世界坐标系下磁力矢量dE=[0,bx,0,bz]d_E = [0,b_x,0,b_z]dE=[0,bx,0,bz]和传感器坐标系下磁力矢量dS=[0,mx,my,mz]d_S = [0,m_x,m_y,m_z]dS=[0,mx,my,mz]应满足如下关系:
[0,bx,0,bz]T=q×[0,mx,my,mz]T×q−1[0,b_x,0,b_z]^T = q \times [0,m_x,m_y,m_z]^T \times q^{-1}[0,bx,0,bz]T=q×[0,mx,my,mz]T×q−1
但由于误差存在(电子仪器干扰,磁铁干扰,地磁偏转等),变换后的y坐标可能不为零,得到结果为[0,bx′,by′,bz′][0,b_x',b_y',b_z'][0,bx′,by′,bz′]。
[0,bx′,by′,bz′]T=q×[0,mx,my,mz]T×q−1[0,b_x',b_y',b_z']^T = q \times [0,m_x,m_y,m_z]^T \times q^{-1}[0,bx′,by′,bz′]T=q×[0,mx,my,mz]T×q−1
在计算误差eee和Jacobian矩阵JJJ时,可以人工强制消除此误差:
dE=[0,bx′2+by′2,0,bz′]d_E = [0,\sqrt{b_x'^2+b_y'^2},0,b_z']dE=[0,bx′2+by′2,0,bz′]
这样做的另一个好处是,x轴正方向不必强制指北,指向平面任意方向即可。
角速度计误差
存疑:这部分道理不明,作者网站的源码中也没有实现,只简略记下流程。
角速度计误差由两部分组成
漂移(bias drift):ωζ˙\dot{\omega_{\zeta}}ωζ˙,单位rad/s/s。
增益(error):ωβ,\omega_{\beta},ωβ,单位rad/s。
角速度计的三个示数和真实值的关系为:
ωtrue=ωβωread+ωζ˙\omega_{true} = \omega_{\beta} \omega_{read} + \dot{\omega_{\zeta}}ωtrue=ωβωread+ωζ˙
两个传感器参数可以通过后面的标定算法测量而知。
进一步可以得到两个系统参数:
β=∣∣[0,ωβ,ωβ,ωβ]∣∣=34ωβ\beta = ||[0,\omega_{\beta},\omega_{\beta},\omega_{\beta}]|| = \sqrt{\frac{3}{4}}\omega_{\beta}β=∣∣[0,ωβ,ωβ,ωβ]∣∣=43ωβ
ζ=∣∣[0,ωζ˙,ωζ˙,ωζ˙]∣∣=34ωζ˙\zeta = ||[0,\dot{\omega_{\zeta}},\dot{\omega_{\zeta}},\dot{\omega_{\zeta}}]||=\sqrt{\frac{3}{4}}\dot{\omega_{\zeta}}ζ=∣∣[0,ωζ˙,ωζ˙,ωζ˙]∣∣=43ωζ˙
其中β\betaβ就是前节融合算法中,加速度计/磁力计的权重参数。
而ζ\zetaζ则用来直接修正加速度计中的ω\omegaω。
ω′=ω−ζ∫2qt−1×∇f∣∣∇f∣∣dt\omega' = \omega - \zeta \int 2q_t^{-1}\times \frac{\nabla f}{||\nabla f||}dtω′=ω−ζ∫2qt−1×∣∣∇f∣∣∇fdt
其中∇f∣∣∇f∣∣\frac{\nabla f}{||\nabla f||}∣∣∇f∣∣∇f表示qqq的误差随时间变化率。经过反变换,积分号内部分为ω\omegaω的误差随时间变化率。其对时间的积分表示均值不为零的部分。
标定
使用8台高速(120Hz)红外摄像机,拍摄带有红外标记(杆顶端的亮点)的支架,获取标定支架(三条杆)上传感器(橘黄盒子)的真实姿态。
涉及四个坐标系统:世界(E),摄像机(C),支架(M),传感器(S)。各个坐标系统之间的转换按如下方式确定。
世界(E)↔\leftrightarrow↔摄像机©
使用棉线悬挂重物,垂线方向的单位向量cZc_ZcZ记为摄像机坐标系下世界坐标系的z轴。
使用棉线悬挂一根磁铁杆,杆方向的单位向量cXc_XcX记为摄像机坐标系下世界坐标系的x轴。
垂直轴cZc_ZcZ一般较准确,但由于摩擦力等因素,水平轴cXc_XcX可能和z不垂直(不够平)。用以下方法进行修正:
cX=cX−(cXTcZ)cZc_X = c_X - (c_X^Tc_Z) c_ZcX=cX−(cXTcZ)cZ
cX=cX/∣∣cX∣∣c_X = c_X / ||c_X||cX=cX/∣∣cX∣∣
即,将cXc_XcX向z轴垂直方向投影,之后再归一化。
y轴由xz轴叉乘得到。
2×32\times32×3矩阵R=[cX,cY,cZ]R=[c_X,c_Y,c_Z]R=[cX,cY,cZ]即为从摄像机到世界的旋转矩阵,可以将其转换为四元数ECq^C_EqECq。
摄像机©↔\leftrightarrow↔支架(M)
设拍摄图像中,三个杆方向的单位向量(参看前图)为cX,cY,cZc_X,c_Y,c_ZcX,cY,cZ。并排构成如下2×32\times32×3的矩阵:$R=[c_X, c_Y,c_Z]=\left( r_{ij}\right)_{i=1:2,j=1:3} $。
构造如下的4×44\times44×4的矩阵:
该矩阵最大特征值对应的特征向量即为摄像机坐标系下支架的姿态四元数,换言之,得到的是从摄像机到支架的转换MCq^C_MqMCq。
此部分证明超出本文范围,可以参看Bar-Itzhack的论文1。
支架(M)↔\leftrightarrow↔传感器(S)
两者之间的相对关系无法测量,必须借助第三方的姿态估计算法。此处使用Kalman算法2,在静止情况下,获得世界坐标系中传感器的姿态四元数ESqKalman^S_Eq_{Kalman}ESqKalman,将其认为是真实值。从传感器到支架的转换可以通过下式获取:
MSq=MCq×CEq×ESqKalman^S_Mq = {^C_Mq} \times {^E_Cq} \times {^S_Eq_{Kalman}} MSq=MCq×CEq×ESqKalman
综合三个结果,可以通过摄像机获取世界坐标系下的传感器姿态:
ESq=ECq×CMq×MSq^S_Eq = {^C_Eq}\times {^M_Cq}\times {^S_Mq}ESq=ECq×CMq×MSq
结论
比较摄像机获取的姿态和前述算法估计的姿态,可以对算法进行评估。
作者在自己公司的网站上给出了Madgwick算法的源码。网站上还有许多实际应用例子。
Madgwick算法的一个显著优点是通用性:不需要对运动做任何假设,可以直接套用。但是,在实际应用中,还应该尽量利用先验知识,对加速度、速度或者位置进行重置,避免随时间的漂移。
Itzhack Y Bar-Itzhack. New method for extracting the quaternion from a rotation matrix. AIAA Journal of Guidance, Control and Dynamics, 23(6):10851087, Nov.Dec 2000. (Engineering Note). ↩︎
R. E. Kalman. A new approach to linear ltering and prediction problems. Journal of Basic Engineering, 82:35{45, 1960. ↩︎
【运动传感器】Madgwick算法(下)相关推荐
- 应用先进的传感器和算法进行低成本运动跟踪---凯利讯半导体
高性能运动跟踪技术已经从深奥的军事.航空电子设备.海洋和工业应用领域,发展到越来越多的消费者应用领域.然而,将高性能的运动跟踪带到消费者市场往往会迫使设计师们协调一致的目标,特别是在成本.功耗和计算资 ...
- android 加速度传感器测步数,基于加速度传感器的运动步数检测算法研究
基于加速度传感器的运动步数检测算法研究 为解决人体运动过程中手腕摆动对步数检测干扰的影响,提出了一种区域峰值步数检测算法.该算法根据人体运动时的加速度信号特征,使用高斯滤波去除噪声对加速度信号的影响, ...
- Madgwick算法详细解读
Madgwick算法详细解读 极品巧克力 前言 接上一篇文章<Google Cardboard的九轴融合算法>. Madgwick算法是另外一种九轴融合的方法,广泛应用在旋翼飞行器上,效果 ...
- 运动模糊图像复原算法实现及应用
MATLAB图像复原系统 概述 1.1选题背景 从历史上来看,数字图像处理研究有很大部分是在图像恢复方面进行的,包括对算法的研究和针对特定问题的图像处理程序的编写.数字图像处理中很多值得注意的成就就是 ...
- SLAM 03.多传感器融合算法
传感器标定是自动驾驶的基本需求,一个车上装了多个/多种传感器,而它们之间的时间和空间关系需要统一,传感器数据的标定主要为了定位的准确性. 1.传感器分类 机器人有多种传感器,每种数据类型不一样,数据精 ...
- 组合导航+多传感器融合算法
1.INS/GPS的组合导航系统可以输出高频率的导航参数信息(位置.速度.姿态),并且在长.短期的导航过程中均能具备较高精度.(输出数据的意义什么,PDR是步长+航向) 2.误差反馈系统 (1)在开环 ...
- ICM-42670-P 六轴运动传感器 TDK ICP-10740 气压计实现运动监测
ICM-42670-P 六轴运动传感器 & TDK ICP-10740 气压计,配合 CyweeMotion 算法,实现了运动监测.CyweeMotion 算法不仅支持多种运动模式:如走路.跑 ...
- 详解多旋翼无人机的传感器技术(下)
详解多旋翼无人机上的传感器技术(下) 按:本文作者YY硕,来自大疆工程师.本文来自雷锋网,一个很不错的资讯网站 雷锋网连接在此 http://www.leiphone.com/ 强烈推荐雷锋网,本文 ...
- Android系统运动传感器
Android平台提供了多种感应器,让你监控设备的运动. 传感器的架构因传感器类型而异: 重力,线性加速度,旋转矢量,重要运动,计步器和步检测传感器是基于硬件或基于软件的. 加速计和陀螺仪传感器是始终 ...
最新文章
- Java程序员转Android开发必读经验
- PrimeTime指南——合理设置约束
- 数据结构——链式队列解析(C语言版)
- select count(*)底层究竟干了啥么?
- java元注解 @Documented注解使用
- 【java笔记】接口的定义,接口的使用
- JavaScript JSON的key 下划线格式与驼峰格式互相转换
- 终端不需要主机的服务器,要实现云桌面需要哪些硬件?云终端必不可少
- 产品初探(一):面试经验记录
- java mail 20m附件,发送邮件时附件大小不能超过20M,否则无法发送
- tomcat html位置,HTML的Tomcat
- Android 广播(Broadcast)
- UEFI.源码分析.DXE阶段的执行
- 江在川上曰:less样式预编译
- 松下服务器编码器由谁该信号,松下伺服电机编码器判断好坏的方法以及功能和作用...
- Rank-consistent Ordinal Regression for Neural Networks
- Oracle 11g ORA-27101,ORA-00119,ORA-00132 解决方案
- table中cellspacing和cellspadding的用法
- 中移动内部 无线信号测试软件,- 移动网无线信号质量监测系统[图]
- hbuilder常用
热门文章
- clickhouse简单使用及优化
- php 比对两张图片,PHP实现图片相似度对比
- qsettings删除注册表_QSettings读写ini文件和注册表
- 随机性检测之游程总数测试
- Java面向对象系列[v1.0.0][SQL语法之数据库约束]
- SNMPSarpNet通过OID获取计算机或计算机所连的硬件信息
- 5611BH一位数码管及SMA420354L四位数码管引脚图
- 进大厂必看!拼多多大佬总结出来的10万字Springboot经典学习笔记PDF版本,分享给大家!
- Ubuntu虚拟机无法上网
- python 图片验证码识别,较实用