姿态解算 (MPU9250 & Motion Driver 6)

2019-8-10 15:31 [复制链接] 1 384

本帖最后由 sogw 于 2019-8-10 15:31 编辑 0 N' b: }+ f: q( c

" ^1 n0 G3 F: r3 \( ?( |; X! V* B本文的主要目的是讲解如何使用 MPU9250 和官方motion driver 6.12 库实现姿态解算。
. a( C3 m+ |9 z  _( |( {2 P% I% h$ V* i$ g* C6 R) m! B1 D
基础知识:2 l7 o! Y8 U  X- ]$ S
4 c% @% g5 h! L0 \3 d2 W  A
坐标系
3 [8 w! E$ X3 G(1)地心惯性坐标系(简称 i系)
" B& p/ Y  i% E0 S* Q/ C

坐标系的原点O为地球的中心, 且与地球的自转无关。z轴与地球的自转轴重合,其正向指向北极。x轴、y轴在地球的赤道平面内,Ox指向春分点,y轴由右手定则确定。三个坐标轴指向惯性空间固定不动,这个坐标系是惯性仪器测量的参考标准。


, @5 r3 x  z8 I  s! Z/ W6 g& ^* ?(2)地球坐标系(简称 e系)
, M4 n: J. ^: C$ v' q

坐标系的原点O位于地心,z轴为地球自转轴,x轴、y轴在地球的赤道平面内,x轴指向零度子午线,y轴则指向东经90°方向。 e系与i系的相对运动为地球的自转。


" _# R% S/ U3 e3 w: [5 p(3)地理坐标系(简称 g系), Q! ?# ]9 d5 p7 ?1 ?

地理坐标系是指原点位于运载体所在的地球表面,其中一轴与地理垂线重合的右手直角坐标系。地理坐标系的原点O选取在载体重心处,对于地理坐标系的坐标轴有不同的取法,如东北天、北西天、北东地等,这些只是坐标轴的正向方向不同,不影响导航解算。

dili.png
6 Q# A$ D" y7 |/ v$ c( C( j1 c) J# N
(4)载体坐标系(简称 b系)
$ {' r* j1 Y  K

与载体固连在一起,坐标系原点O位于载体的质心,Ox沿载体横轴向右,Oz沿载体垂直轴向上,Oy符合右手定则沿纵轴向前。载体的姿态角信息可由地理坐标系与载体坐标系的角度确定。

zaiti.png
  Y' E4 V& `9 B* i5 i5 M5 F$ n
(5)导航坐标系(简称 n系)" w5 S  Z( z4 U' y! P+ g- E+ Q* |3 P

惯导系统中求解导航参数时所采用的坐标系。

/ K9 J3 o% I7 H- j: e* I
欧拉角7 l5 F6 o" l/ f  W* A# I, u

莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。参考系又称为实验室参考系,是静止不动的。而坐标系则固定于刚体,随着刚体的旋转而旋转。

200px-Euler2a.gif

摘自维基百科-欧拉角


% a/ }1 y4 {. s: t3 c

姿态解算的目的即是为了获取姿态信息,姿态由多种表示方式,常见的是四元数,欧拉角,矩阵和轴角。他们各自有其自身的优点,在不同的领域使用不同的表示方式。通常我们使用欧拉角表述姿态。 在姿态解算前,一般需要进行传感器标定等,此不是本文的目的,不再赘述。

7 H; Y4 t7 G* B  N) f

姿态解算需要一些算法,常见的有 非线性互补滤波算法、非线性互补滤波算法、Mahony互补滤波算法。算法的比较可以参考这里:http://www.olliw.eu/2013/imu-data-fusing/


/ j6 Z1 c. z7 g

鉴于使用的是MPU9250模块,内含DMP,且其官方 InvenSense 的 Motion Driver 6.12(内含姿态解算算法等)支持九轴数据融合。所以直接使用 MD6 库获取姿态信息。不再自行解算。


8 z5 M* ~8 T6 [; l1 i" U) z' mMD6
! ~: G1 ^# \9 L$ k7 p(1)获取
2 H7 \& ]7 p1 w6 S

MD6可以从x893大神的github获取:https://github.com/x893/MotionDriver6。MD6对arm支持很好,文档也很详细, 移植相对简单,基本看着文档就可已完成移植和使用。

(2)移植! ?9 M' ~! ]1 C5 b" T

参考文档 “App Note 3- Motion Driver 6.12 Porting Guide”。

& o1 j) |) C9 H: g3 j

首先将源代码添加进工程。源代码即为 “core” 下的所有文件,如果想使用针对性优化过的二进制库,需要替换掉 “core” 的二进制库。

然后实现类似如下的底层接口(注意返回值):

port.png

最后添加预编译选项:

CPPDEFINES.png

编译成功即可


; X  I& Q: G" c0 ^6 d. I3 N(3)使用

添加 MD6 中的 “main.c” 到工程中,删减掉不需要的输入输出等,解决编译错误。然后可以创建线程来运行其中的 “main” ,就可以获得欧拉角了。(注意定点数的转化)

1 e; F) {4 f4 ~7 f) x8 q# x
app.png

) x: S  B& D+ ^# z7 B7 A/ l( {8 p% H: h+ `$ P) ^; d
- e3 t6 b0 \* `1 t  _9 Z  m; }
% Y/ `5 l! G) ?4 G; X
, g% V  o7 h$ Y& l2 G: [7 }
使用道具 举报 显示全部楼层 回复
最新评论 | 正序浏览
显示全部楼层 |楼层直达:
发表于 2019-8-12 11:46:57 | 显示全部楼层
这个是不是可以发布一个软件包
使用道具 举报 回复
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|RT-Thread开发者社区 ( 沪ICP备13014002号-1

有害信息举报电话:021-31165890 手机:18930558079

© 2006-2019 上海睿赛德电子科技有限公司

Powered by RT-Thread

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