STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码

上一篇博客，采用的是DMP解算姿态，且给出了底层硬件配置及驱动方式，此处摒弃DMP直接由单片机自己实现姿态解算，接下来介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法。

我们先来看看如何用欧拉角描述一次平面旋转(坐标变换)：

设坐标系绕旋转α角后得到坐标系，在空间中有一个矢量在坐标系中的投影为，在内的投影为由于旋转绕进行，所以Z坐标未变，即有。

转换成矩阵形式表示为：

整理一下：

所以从旋转到可以写成上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋转要转三次:

上面得到了一个表示旋转的方向余弦矩阵。

不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了：

上式中左侧，是本次更新后的欧拉角，对应row、pit、yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。

虽然欧拉角求解姿态表达清晰，但是欧拉角微分方程中包含了大量的三角运算，不利于实时运算。而且当俯仰角为90度时方程式会万向锁问题。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。

四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。

我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：

四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，因为这就是四元数表示旋转的约束条件。

而刚才用欧拉角描述的方向余弦矩阵用四元数描述则为：

所以在软件解算中，我们要首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模，传入参数是陀螺仪x、y、z值和加速度计x、y、z值：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{ float norm;

float vx, vy, vz; float ex, ey, ez;

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

下面把四元数换算成方向余弦中的第三行的三个元素。刚好vx、vy、vz 。其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。

estimated direction of gravity vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3);

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

axyz是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。

axyz是测量得到的重力向量，vxyz是陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标参照系上的重力向量。

那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。

向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，exyz就是两个重力向量的叉积。

这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

integral error scaled integral gain exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki;

ezInt = ezInt + ez*Ki;

用叉积误差来做PI修正陀螺零偏

integral error scaled integral gain exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki;

ezInt = ezInt + ez*Ki; // adjusted gyroscope measurements

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

四元数微分方程，其中T为测量周期，为陀螺仪角速度，都是已知量，使用了一阶龙哥库塔求解四元数微分方程：

integrate quaternion rate and normalise

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角：

最终：

ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw

ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch

ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

上述过程如下图所示：

接下来采用一节互补滤波求解姿态：

#defineKp 10.0f // 这里的KpKi是用于调整加速度计修正陀螺仪的速度
#defineKi 0.008f
#definehalfT 0.001f // 采样周期的一半，用于求解四元数微分方程时计算角增量
floatq0 = 1, q1 = 0, q2 = 0, q3 = 0; // 初始姿态四元数，由上篇博文提到的变换四元数公式得来
floatexInt = 0, eyInt = 0, ezInt = 0; //当前加计测得的重力加速度在三轴上的分量
//与用当前姿态计算得来的重力在三轴上的分量的误差的积分
voidIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)//g表陀螺仪，a表加计
{

float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用
float norm; //矢量的模或四元数的范数
float vx, vy, vz;//当前姿态计算得来的重力在三轴上的分量
float ex, ey, ez;//当前加计测得的重力加速度在三轴上的分量
//与用当前姿态计算得来的重力在三轴上的分量的误差

//
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
return;
norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
ax = ax /norm;// 归一化了
ay = ay / norm;
az = az / norm;
//用当前姿态计算出重力在三个轴上的分量，
//参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;
//计算测得的重力与计算得重力间的误差，向量外积可以表示这一误差

ex = (ay*vz - az*vy) ;
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ;

exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减。

//下面进行姿态的更新，也就是四元数微分方程的求解
q0temp=q0;//暂存当前值用于计算
q1temp=q1;//网上传的这份算法大多没有注意这个问题，在此更正
q2temp=q2;
q3temp=q3;
//采用龙格库塔法，得到新的四元数
q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;
//单位化四元数在空间旋转时不会拉伸，仅有旋转角度
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//四元数到欧拉角的转换
//其中YAW航向角由于加速度计对其没有修正作用，下次用磁力计整合，此处不管，用陀螺仪积分代替。
Q_ANGLE.Z = GYRO_I.Z; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码相关推荐

【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角
目录源码 MPU6050_Filter.c MPU6050_Filter.h 使用方法测试程序一阶互补滤波效果二阶互补滤波效果卡尔曼滤波效果总结普中51-单核-A2 STC89C5 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...
浅谈陀螺仪和加速度计的互补滤波
结合加速度计和陀螺仪的平衡解决方案首先通过ADC采样,读出陀螺仪和编码器的电压值,并将其转化为可用单位: 1.偏移(offset):让陀螺仪静止,加速度计保持水平并静止,这时候读到电压值,就是偏移值 ...
基于RflySim平台的mahony(含磁罗盘)互补滤波在pixhawk仿真及实物实验（带实验数据）
写在前面本案例实验采用RflySim平台,该平台可以高效快速编写代码,使用simulink模型搭建,可以见代码直接生成对应的C代码,并一键将代码烧录Pixhawk中,是一种快速开发平台,RflySi ...
对比直立车模控制中的互补滤波、Karlman滤波和参考滤波方案
前几天,有参加智能车竞赛的同学在公众号上询问一个关于直立车模控制中角度融合算法的问题.感觉这是所有初次准备智能车模竞赛的同学都会碰到的问题. 卓大大,请问您可以针对卡尔曼滤波.互补滤波.清华滤波的原理 ...
STC51单片机15——MPU6050六轴数据融合，互补滤波，时间常数可调，可稳定运行，串口显示角度值
51单片机用模拟IIC的方式读取MPU6050的原始数据,之后经过换算转成三轴加速度和三轴角速度.设定定时器,以固定的频率采集以上得到的数据,并加入互补滤波,去除加速度的噪声以及陀螺仪的零飘.注意,本 ...
姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
Matlab图像处理创新实践-实验2【图像滤波基础（2）】
Matlab图像处理创新实践-实验1[图像滤波基础(1)] Matlab图像处理创新实践-实验2[图像滤波基础(2)] Matlab图像处理创新实践-实验3[图像锐化] Matlab图像处理创新实践- ...

STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码

STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码相关推荐

最新文章

热门文章