【运动传感器】Madgwick算法(下)
来源:互联网 发布:linux uniq命令详解 编辑:程序博客网 时间:2024/05/17 21:46
在上篇中,讲解了分别从角速度计、加速度计、磁力计估计姿态的方法。本篇文章讲解他们的融合,对误差的处理,以及标定实验。
融合
这一部分在文章中称为filter。
在上一篇文章中,我们能够通过角速度计的读数
其中
两者融合有:
增量由两部分构成:
第一部分
如果角速度计是完美的,则只需这部分即可。
第二部分
如果误差为0,则不需要此项;否则,因为角速度计不完美,当前姿态
本部分证明原文参见Madgwick Internal Report3.3节。原始证明感觉就是凑的尚存疑,此处根据个人理解书写。
磁力计偏转
根据前文推导,世界坐标系下磁力矢量
但由于误差存在(电子仪器干扰,磁铁干扰,地磁偏转等),变换后的y坐标可能不为零,得到结果为
在计算误差
这样做的另一个好处是,x轴正方向不必强制指北,指向平面任意方向即可。
角速度计误差
存疑:这部分道理不明,作者网站的源码中也没有实现,只简略记下流程。
角速度计误差由两部分组成
漂移(bias drift):
增益(error):
角速度计的三个示数和真实值的关系为:
两个传感器参数可以通过后面的标定算法测量而知。
进一步可以得到两个系统参数:
其中
而
其中
标定
使用8台高速(120Hz)红外摄像机,拍摄带有红外标记(杆顶端的亮点)的支架,获取标定支架(三条杆)上传感器(橘黄盒子)的真实姿态。
涉及四个坐标系统:世界(E),摄像机(C),支架(M),传感器(S)。各个坐标系统之间的转换按如下方式确定。
世界(E)↔ 摄像机(C)
使用棉线悬挂重物,垂线方向的单位向量
使用棉线悬挂一根磁铁杆,杆方向的单位向量
垂直轴
即,将
y轴由xz轴叉乘得到。
摄像机(C)↔ 支架(M)
设拍摄图像中,三个杆方向的单位向量(参看前图)为
构造如下的
该矩阵最大特征值对应的特征向量即为摄像机坐标系下支架的姿态四元数,换言之,得到的是从摄像机到支架的转换
此部分证明超出本文范围,可以参看Bar-Itzhack的论文1。
支架(M)↔ 传感器(S)
两者之间的相对关系无法测量,必须借助第三方的姿态估计算法。此处使用Kalman算法2,在静止情况下,获得世界坐标系中传感器的姿态四元数
综合三个结果,可以通过摄像机获取世界坐标系下的传感器姿态:
结论
比较摄像机获取的姿态和前述算法估计的姿态,可以对算法进行评估。
作者在自己公司的网站上给出了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算法(下)
- 【运动传感器】Madgwick算法(上)
- Android 传感器 II-运动传感器
- 位置和传感器:运动传感器
- Android系统运动传感器
- Android开发者指南-运动传感器!!!!!!!!!
- android 传感器必知必会(下)
- 机器蛇运动算法(一)--蜿蜒运动
- 机器蛇运动算法(二)--蠕动运动
- Android开发者指南-运动传感器Motion Sensor
- Android开发者指南-运动传感器Motion Sensor
- Android开发者指南-运动传感器Motion Sensor
- 基于 PIR 的运动检测:传感器解决方案
- Android开发者指南-运动传感器Motion Sensor
- VIBE运动检测算法实现(二)
- Madgwick解算小结
- 无线传感器网络MAC协议(下)
- 无线传感器网络MAC协议(下)
- 发送cookie
- 【菜鸟学Java】11:Session技术
- Java代码规范、格式化和checkstyle检查配置文档
- jdbc编程步骤
- CocoaPods的安装使用和常见问题
- 【运动传感器】Madgwick算法(下)
- windows 使用Node.js安装【IONIC】
- eclipse快捷键
- eclipse插件之Findbugs、Checkstyle、PMD安装及使用
- 二级域名
- 运行 Docker 容器时的安全风险:别丢了你的套接字
- 解决Android 应用方法数不能超过65K的问题
- C++ string TBD
- Linux下Maven的安装与使用