目录
七、如果我的文章能帮到你，请点个赞鼓励一下再走吧ヾ(^▽^*)))
一、前言
数据融合进行位姿估计的算法有很多可供选择，常见的就是卡尔曼滤波算法和互补滤波算法等，之前在网上看过一些相关文章，看完感觉云里雾里，说不到点上，于是尝试着自己设计了一版数据融合算法，这一版算法没有复杂的公式推导，浅显易懂，但原理和卡尔曼滤波、互补滤波近似，希望对一些还在迷茫的同学有所帮助。
在设备高速运动时，陀螺仪的数据相较于加速度计更加准确，相反，在设备低速运动时，加速度计的数据比陀螺仪更加可靠，两者融合在一起，可以起到一个互补的作用，但是，由于加速度计无法测量设备的偏航角，而陀螺仪输出的角速度积分后得到的偏航角的数据又存在累计误差无法消除，所以单靠加速度计和陀螺仪的融合，是无法准确测量偏航角的。
话说在前面，使用数据融合算法得到的欧拉角数据不准确哦（欧拉角包括：1、偏航角Yaw；2、俯仰角Pitch；3、翻滚角Roll）。除非有厉害的数据融合算法加持，例如MPU6050里的六轴数据融合算法，LSM6DV16X里的LPSF算法，可以将偏航角误差限制在一个可接受的范围内，但这些厉害的算法都是不开源的。所以当前市面上的数据融合算法输出的数据都是不准确的（我尝试了大概四五种算法，效果差不多），这点必须要提前告知正在努力看文章的你，避免竹篮打水一场空。
如果你不关心算法的准确度，想了解一下传感器融合的思想，请继续看下去，就到正文啦
二、机体坐标系和世界坐标系
对加速度计和陀螺仪的传感器数据进行融合前，首先需要了解什么是机体坐标系和世界坐标系，因为加速度计输出的加速度数据计算得到的欧拉角是以世界坐标系为参考的，而陀螺仪输出的角速度积分得到的欧拉角是以机体坐标系为参考的，这是两者的不同之处，不能一概而论。
世界坐标系是绝对的，静止的，用来描述任一物体在三维空间中的确切位置，比方说东南西北四个方向，不会因为你是躺着坐着而改变。
机体坐标系是相对的，随你的姿态而变化，比方说前后左右，就是机体坐标系的思想，你躺着坐着，起床翻个身，前后左右的指向都会随你自身的姿态而改变。
下图说明了世界坐标系和机体坐标系的区别：
Xb、Yb、Zb为机体坐标系
三、传感器融合数据分析
加速度计输出的数据是三个轴的加速度数据（实际上是重力加速度在xyz三个轴上的分量），进行三角函数计算后可以得到世界坐标系下设备的俯仰角和翻滚角，但无法计算偏航角。陀螺仪输出的数据是绕三个轴的角速度，积分后可以得到机身坐标系下某一段时间内设备绕三个轴转过的角度，存在累计误差。随着使用时间的增加，积分误差会越来越大。
加速度计和陀螺仪输出的数据类型不同，一个是以世界坐标系为参考，一个是以机体坐标系为参考，要想实现数据融合，必须先统一数据类型。
机体坐标系是和陀螺仪的三轴坐标系是完全对应的，六轴传感器的姿态会随着设备姿态一起变动。陀螺仪计算的是每个旋转轴的实时角速度，角速度乘以时间间隔后，得到的是某段时间内设备绕机体坐标系的三个坐标轴转过的角度。
那么问题来了。加速度计可不可以表示某段时间内设备绕机体坐标系的三个轴转过的角度呢？
当然可以！只需要将此刻加速度计得到的世界坐标系下的欧拉角，减去上一时刻测得的世界坐标系下的欧拉角，得到的不就是这段时间内设备绕三个轴转过的角度吗？
如此一来，便完成了加速度计和陀螺仪数据融合的准备工作——数据统一。
四、三维空间旋转向量如何表示
在上一章节中，我们完成了加速度计和陀螺仪的数据类型统一问题，让它们都表示一定时间间隔内设备绕机体坐标系的x、y、z轴转过的角度，在这一个前提条件的基础上，我们就可以开始讨论三维向量旋转的问题。
理想状态下，假设有一个初始单位向量，与设备的x轴重合，在世界坐标系下可以表示为(1，0，0)，在1秒之后，它绕y轴旋转了90°，此时我们发现，陀螺仪的三轴角速度读数为（0，90，0），时间间隔为1s（1s是假设，理论上时间间隔越小，加速度计积分得到的角度结果就越准确），使用 角速度90°/s ✖ 时间间隔1s = 这段时间向量转过的角度90°。（加速度计同理）
知道了向量转动的角度，我们就可以对原向量进行状态更新，得到一个新向量(0,1,0)，计算结果与向量的实际旋转情况符合，新向量仍然与设备机体坐标系的x轴重合。
旧向量代表设备最初姿态，新向量代表设备当前姿态，计算两个向量的夹角，便可知设备的姿态信息。
五、算法流程图
Tips：
在选取数据融合系数时，可以使用自己的思路设计，本例程中是基于陀螺仪的角速度数据来判断设备的运动情况，进而反映陀螺仪和加速度计的输出数据谁更可靠，但还不够好，同学们可以尝试一下自己的方法，针对融合系数计算方程的修改也很简单。但数据融合的思路不变。
数据融合思路如下：
因为设备在高速运动时，陀螺仪和加速度计的输出数据会变大，此时陀螺仪数据相较于加速度计更加可靠，所以增大陀螺仪的数据融合系数。
而设备在低速运动和相对静止时，陀螺仪和加速度计输出的数据会变小，加速度计重力分量明显，此时加速度计的数据相较于陀螺仪更加可靠，所以增大加速度计的数据融合系数。
可以多多尝试一下自己的方案，学以致用，融会贯通。
六、三维空间旋转向量算法代码实现
/*****************************************************************************
[函数名称]Run_LSM6D
[函数功能]定时运行函数，使用定时器每隔15ms调用一次，对陀螺仪的角速度进行积分运算
[备 注] 参考坐标系方向 —— x：正前；y：右； z：上
实际坐标系方向 —— y：正前；x：右；-z：上
*****************************************************************************/
#define TIME_15ms 0.015f //时间间隔
static BOOL IsInit = true; //向量初始化标志位
static float pitch = 0; //记录设备俯仰角
static float Yaw = 0; //记录设备偏航角
static float Roll = 0; //记录设备翻滚角
struct vector3d{
float x,y,z;
}firData, secData; //记录两个三维向量的缓存
void Run_LSM6D(void)
{
const f32 c = 180 / 3.1415926; //将弧度转换为角度的系数
IIC_Read(STATUS_REG, &status.m_Byte, 1); //读状态寄存器的值
if(status.m_Bit.GDA == true) //如果角速度数据准备就绪
{
IIC_Read(0x22, AngleSpeed.m_Byte, 6); //读取三轴方向上角速度的原始数据
//转换为 °/s，注意传感器实际方向
data.gyro.x = -AngleSpeed.m_Data.Y * 17.5f / 1000.0f;
data.gyro.y = AngleSpeed.m_Data.X * 17.5f / 1000.0f;
data.gyro.z = -AngleSpeed.m_Data.Z * 17.5f / 1000.0f;
}
if(status.m_Bit.XLDA == true) //如果加速度计数据准备就绪
{
IIC_Read(0x28, Acceleration.m_Byte, 6); //读取三轴方向上加速度的原始数据
//加速度转换为 g/m2，注意传感器实际方向
data.accel.x = -Acceleration.m_Data.Y * 0.122f / 1000.0f;
data.accel.y = Acceleration.m_Data.X * 0.122f / 1000.0f;
data.accel.z = -Acceleration.m_Data.Z * 0.122f / 1000.0f;
if(IsInit == true) //如果是第一次初始化，记录初始向量数据
{
IsInit = false; //关闭初始化标志位
//使用加速度计计算第一个向量的欧拉角弧度
pitch = atan2(data.accel.x, -data.accel.z);
roll = atan2(data.accel.y, -data.accel.z);
yaw = 0; //加速度计无法计算偏航角，默认为0
firData.x = cosf(pitch); //设初始化向量与x轴重合，得到初始化单位方向向量
firData.y = 0;
firData.z = sinf(pitch);
secData = firData; //后续向量以初始向量为基础，进行角度更新
return ; //返回
}
}
//初始化之后的持续更新
//减小陀螺仪在设备低速运动时的抖动影响
// if(fabs(data.gyro.x)<8 && fabs(data.gyro.y)<8 && fabs(data.gyro.z)<8)
// return ;
//加速度计和陀螺仪数据统一
f32 delta_ax = atan2(data.accel.y, -data.accel.z) - roll; //加速度x轴roll的变化弧度
f32 delta_ay = atan2(data.accel.x, -data.accel.z) - pitch;//加速度计y轴pitch的变化弧度
f32 delta_gx = data.gyro.x * TIME_15ms / c; //陀螺仪x轴变化弧度
f32 delta_gy = data.gyro.y * TIME_15ms / c; //陀螺仪y轴变化弧度
f32 delta_gz = data.gyro.z * TIME_15ms / c; //陀螺仪z轴变化弧度
//当设备稳定时，加速度计数据更准确，为其分配更高的权重
//陀螺仪角速度越小，反应设备越稳定
f32 weight = data.gyro.x; //取陀螺仪三个分量中的最大值
if(delta_gy > weight)
weight = data.gyro.y;
if(delta_gz > weight)
weight = data.gyro.z;
f32 alpha = 1.0f; //创建传感器融合系数变量
//根据陀螺仪角速度数据，计算数据融合系数大小，选择更相信陀螺仪还是加速度计
if(weight < 10) //如果角速度非常小，说明设备在低速运动
{
alpha = 0.01; //选择更相信加速度计数据
delta_gz = 0; //陀螺仪偏航角变化量赋零，减小陀螺仪在低速运动下的抖动情况
}
else if(weight > 100) //高速运动下，加速度计不再可靠，陀螺仪更加精确
{
alpha = 0.99; //为陀螺仪分配更高的权重
}
else //根据方程计算系数，仅供参考，也可以自己拟一个方程
{
alpha = 0.010526 * weight - 0.052632; //alpha是陀螺仪系数
}
//加速度计和陀螺仪数据融合之后
f32 theta_x = alpha*delta_gx + (1-alpha)*delta_ax; //计算x轴角度变化值
f32 theta_y = alpha*delta_gy + (1-alpha)*delta_ay; //计算y轴角度变化值
f32 theta_z = delta_gz; //计算z轴角度变化值
pitch += theta_y; //更新世界坐标系下的欧拉角（弧度表示），加速度计更新要用
roll += theta_x;
yaw += theta_z;
// f32 theta_x = data.gyro.x * TIME_15ms / c; //只使用陀螺仪角速度积分计算向量旋转弧度
// f32 theta_y = data.gyro.y * TIME_15ms / c;
// f32 theta_z = data.gyro.z * TIME_15ms / c;
float x = secData.x;
float y = secData.y;
float z = secData.z;
float sinx = sinf(theta_x);
float cosx = cosf(theta_x);
float siny = sinf(theta_y);
float cosy = cosf(theta_y);
float sinz = sinf(theta_z);
float cosz = cosf(theta_z);
//将欧拉角转换为旋转矩阵，计算第二个更新向量的偏转
secData.x = x*cosz*cosy + y*(cosz*siny*sinx - sinz*cosx) \
+ z*(cosz*siny*cosx + sinz*sinx);
secData.y = x*sinz*cosy + y*(sinz*siny*sinx + cosz*cosx) \
+ z*(sinz*siny*cosx - cosz*sinx);
secData.z = -x*siny + y*cosy*sinx + z*cosx*cosy;
//计算两个向量之间的夹角
//这一个函数没有写，但是很简单，两个单位向量的内积就是夹角的余弦值
f32 angle= calculate_angle(&firData, &secData) * 57.29;
}
七、如果我的文章能帮到你，请点个赞鼓励一下再走吧ヾ(^▽^*)))
后续有时间，会继续更新使用卡尔曼滤波的传感器数据融合，计算量大一些，难度更高，但更加精准，不知道会不会有更好的效果。
扫一扫
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
