一. 简介
在之前的文章中(很久之前了(CSND中)),已经通过FPGA获取到了MPU6050的六轴数据: 三轴加速 和 三轴角速度,但是没有对它进行然后处理。那么在本篇文章中,将利用Cordic算法来进行姿态解算。
二. 踩坑分享
在进行姿态解算分享之前,先分享一个踩坑经历。一般来说MPU6050的ID读出为0x68,淘宝上买到的模块,基本上都是这个。但是我使用的是自己画的PCB,手动焊接的,在读取ID的时候,一直为0x98,但是认知中要为0x68才是对的,这个时候就会怀疑是不是自己的程序或者焊接的问题了。但好在后面读取六轴数据,姿态解算后得到的角度基本是正确的(折腾了一天了,才发现)。这是个坑,大家可以注意一下。
三. 姿态解算
所谓姿态解算就是通过六轴的数据,来求解物体的三个角度: roll , pitch , yaw。
1. 通过加速度求解
先看一下加速度求解角度的表达式(通过加速度是无法求解yaw的)。atan(acc_y / acc_x)和sqrt(acc_y*acc_y + acc_z * acc_z)不就是上篇中Cordic算法求解的值吗,都不需要使用的其他的计算。
roll = atan(acc_y / acc_x);
pitch = atan(acc_x / (sqrt(acc_y*acc_y + acc_z * acc_z)));
2. 通过角速度求解
通过角速度的求解就更简单了,只需要将当前角度加上(角速度×dt)就可以。角速度求解的时候会有些问题,在静态的时候,角速度会有零漂,这个时候角度误差会越来越大。
3. 融合
可以看到有上面的两种方法求解角度,可以单独使用,但是可能会不太准确,精度要求不高的场合可以只使用加速度求解。在精度要求比较高的场合下,需要使用这两种方法求解,然后再将求得的结果进行融合。常用的方法有: 卡尔曼滤波、一阶互补滤波、二阶互补滤波。
一阶互补滤波,如下,简单粗暴。要想滤波效果好的话,可以试试卡尔曼滤波。
roll = a * acc_roll + (1 - a) *gyro_roll;
以上只是一种比较常规的求解方法,追求高精度的话,可以使用四元数的方法进行求解(复杂度大大增加)。
四. 代码实现
代码都是现成的,在之前的文章中已经写好了,这里做的工作就是将这些模块组合在一起。
1. 模块接口
输入请求,输出应答和三个角度,角度值扩大了2^16倍。
module IMU(
input clk, //27M
input rst_n,
input imu_req,
output imu_ack,
output signed[31:0] roll,
output signed[31:0] pitch,
output signed[31:0] yaw,
output IICSCL, /*IIC 时钟输出*/
inout IICSDA /*IIC 数据线*/
);
2. 状态机
这里使用到了两个Cordic模块,第一个模块先计算出roll和sqrt(acc_yacc_y + acc_z * acc_z)的值,然后第二个模块通过acc_x和sqrt(acc_yacc_y + acc_z * acc_z)的值 计算出 pitch的角度。最后对数据经过了一个简单的FIR滤波。
always@(*) begin
case(state)
S_IDLE:
if( imu_req == 1'b1)
next_state <= S_READ_MPU6050;
else
next_state <= S_IDLE;
S_READ_MPU6050:
if( mpu6050_ack == 1'b1 )
next_state <= S_Cordic;
else
next_state <= S_READ_MPU6050;
S_Cordic:
if( cordic_ack == 1'b1)
next_state <= S_Cordic2;
else
next_state <= S_Cordic;
S_Cordic2:
if( cordic2_ack == 1'b1)
next_state <= S_FILTER;
else
next_state <= S_Cordic2;
S_FILTER:
if( fir_filter_ack == 1'b1)
next_state <= S_ACK;
else
next_state <= S_FILTER;
S_ACK:
next_state <= S_IDLE;
default: next_state <= S_IDLE;
endcase
end
3. 融合
这里的融合,暂时没有做,只对加速度求解的角度进行了一个滤波处理,后面会根据需要再进行更新。
FIR_Filter FIR_Filter_HP(
.clk ( clk ),
.rst_n ( rst_n ),
.fir_filter_req ( fir_filter_req ),
.fir_filter_ack ( fir_filter_ack ),
.filter_data_in ( theta ),
.filter_data_out ( acc_roll )
);
FIR_Filter FIR_Filter_HP2(
.clk ( clk ),
.rst_n ( rst_n ),
.fir_filter_req ( fir_filter_req ),
.fir_filter_ack ( ),
.filter_data_in ( theta2 ),
.filter_data_out ( acc_pitch )
);
模块已上板测试,解算出来的角度没有问题(可能精度不是那么完美,滤波与融合那里需要下点功夫)。
欢迎交流学习!!!文章来源:https://www.toymoban.com/news/detail-806085.html
需要完整代码的 可以关注微信公众号回复 FPGA_MPU6050姿态解算 获取。文章来源地址https://www.toymoban.com/news/detail-806085.html
到了这里,关于FPGA实现MPU6050姿态解算---附完整代码的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!