Open imuncle opened 5 years ago
太强啦!
附上最后的效果:
红线是电机码盘,绿线是陀螺仪数据(yaw轴),几乎完全重合,并且无漂移
谢老板🐮b
Thanks
博主你好,我想参考一下您关于磁力计数据读取函数的代码可以吗,提前谢谢啦。
前辈你好,如果可以的话可以分享一下您完整的工程吗。
@zcf0619 在这里: https://github.com/imuncle/Embedded_Peripheral_Libs
你好前辈,非常感谢!
在2021年1月25日 11:40,big_unclenotifications@github.com 写道:
@zcf0619 在这里: https://github.com/imuncle/Embedded_Peripheral_Libs
— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.
前辈你好, 冒昧致函。想向您请教在AHRS解算中,Kp和Ki参数的大致范围应该如何确定呢。 望不吝赐教。
赵存飞
北京航空航天大学宇航学院 图像处理中心 通讯地址:北京市昌平区沙河高教园南三街9号 邮政编码:102206 在2021年1月25日 11:42,赵存飞17732726639@163.com 写道:
你好前辈,非常感谢!
在2021年1月25日 11:40,big_unclenotifications@github.com 写道:
@zcf0619 在这里: https://github.com/imuncle/Embedded_Peripheral_Libs
— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.
前段时间尝试使用陀螺仪作为机器人云台的位置环反馈值,在底盘跟随的大前提下,这样做是更符合第一视角的操作的。
我去年比赛的时候买了几个维特智能的型号为WT901的九轴,当时时间紧没有用上,结果不用不知道,一用吓一跳,陀螺仪延迟高的吓人,使用J-Scope看到的图像如下:![image](https://user-images.githubusercontent.com/35989223/54519278-1ad15700-49a1-11e9-9007-17d4b6983d16.png)
从图中可以看出延迟大约有200ms,这种延时已经不再具备方位参考值了。
我在仓库里东翻西找,找到了两个三十多块的MPU9250,相比于我买的一百来块的陀螺仪来说便宜了很多,于是我开始尝试手动解析数据。
读取原始数据
MPU9250内部其实是由6轴陀螺仪MPU6500和磁力计AK8963组成,可以测量三轴加速度,三轴角速度和三轴磁场强度,此外还可以测量温度,但这里我没有用到温度的数据。
数据解析的第一步就是读取原始数据。MPU6500支持IIC通信和SPI通信,AK8963只支持IIC通信,但可以通过MPU9250为中介使用SPI通信,只是比较麻烦,所以这里我选择了IIC通信。
IIC地址
MPU9250的IIC地址取决于AD0口的电平高低,如果AD0接GND,则地址为0x68,如果接VCC,则地址为0x69,。
这里说一下一个坑:
最开始我不知道这个,一直把地址写成0x68,结果死活读不到数据,后来才发现要写成0xD0。
MPU9250的其他地址可以查看它的寄存器数据手册,我这里列出了我用到的一部分寄存器:
配置MPU9250
读取数据之前要对MPU9250进行一系列的配置,比如设置采样频率,设置量程等等,具体的如下:
读取数据
配置好了之后就可以读取数据了,这就很简单了,直接从指定寄存器读取就可以了。这里要注意的是磁力计不能读取太频繁,AK8963本身的数据采样就有一个硬件上限,大约200Hz左右,但实测只能以一百多赫兹的频率读取。且每次读取都需要往磁力计的CNTL1寄存器写1,否则会读不到数据。
读取数据的具体步骤如下:
这个函数的执行为1kHz,也就是加速度和陀螺仪数据采样频率是1kHz,磁力计采样频率为100Hz。
姿态解析
光拿到这九轴的数据是不行的,对我们有用的是欧拉方位角,那么拿到数据之后怎么才能得到欧拉角呢?使用四元数就能解决这个问题。
四元数
说实话四元数真不太容易理解,我现在也还是一脸懵逼,但不用慌,我们只需要掌握四元数与欧拉角之间的转换就行了,公式如下:
对四元数本身感兴趣的可以看这个视频(地址),看过之后我还是不太懂。。。
那么怎么根据我们现有的九轴数据求解四元数呢?
其实都用不到九轴,我们只需要用到陀螺仪的数据,也就是角速度就可以求解到四元数,计算公式如下:
(这里利用一阶龙格库塔公式进行积分求解四元数)
也就是说姿态解析中陀螺仪才是主角,我们通过陀螺仪积分得到角度,但是时间久了陀螺仪会产生误差,也就是漂移,时间短还可以忍受,时间一长误差就很大了,所以需要加速度和磁力计来纠正陀螺仪的数据。
互补滤波
加速度计主要测量的是地球的重力加速度,根据重力加速度在三个轴方向的分量可以推算出当前物体的姿态。磁力计也是相同的道理。
加速度计可以纠正pitch轴和roll轴的角度误差,因为航向角不管指向何方,加速度的读数都是一样的,所以需要磁力计来纠正yaw轴的角度误差。
那么怎么纠正呢?我这里使用的是互补滤波。
因为我们要求的是物体坐标相对于地面坐标的三轴偏转角度,所以要把加速度的各个分量(在物体的坐标系下)转换为在地面坐标的分量。进而得到物体的方位,因为陀螺仪本身有误差,所以由陀螺仪计算出来的方位肯定与加速度计算出来的方位有误差,这个误差用向量的叉乘表示,将这个误差叠加在陀螺仪计算出来的结果上就可以抵消误差,进而达到纠正角度的作用。磁力计同上。
这里使用互补滤波来进行纠正。公式如下:
这里构造了一个比例-积分控制器,有一个比例系数和一个积分系数。比例系数越大,表示越信任加速度计和磁力计,积分系数越大,表示越信任陀螺仪。
把误差叠加上去就行了。
上述步骤不断循环,最后使误差越来越小,角度越来越接近真实值。
代码参考
最后附上我的姿态解析代码:
参考