6DoF运动跟踪实战：从MPU6050到四元数姿态解算

1. 从一颗陀螺仪说起6DoF运动跟踪到底在解决什么问题如果你拆开过任何一台VR头显、无人机飞控、或者最近很火的人形机器人大概率会在主板中央看到一颗指甲盖大小的芯片上面印着MPU6050或者ICM42688之类的丝印。这颗芯片就是整个运动跟踪系统的起点。但很多人第一次把MPU6050接到开发板上兴冲冲地读出六个数据——三轴加速度、三轴角速度——然后发现这数据根本没法直接用。加速度计在静止时能告诉你重力方向但一振动就满屏噪声陀螺仪积分出来的角度短时间内很准几分钟后漂到姥姥家。这就是6DoF运动跟踪要解决的核心问题如何把多个不完美的传感器数据融合成一个稳定、准确、实时的姿态估计。6DoF即Six Degrees of Freedom六个自由度指的是刚体在三维空间中的运动描述——三个平移自由度沿X、Y、Z轴的位移和三个旋转自由度绕X、Y、Z轴的旋转通常用俯仰Pitch、横滚Roll、偏航Yaw表示。注意很多入门教程会把6DoF和9DoF搞混。9DoF是在加速度计和陀螺仪基础上加了磁力计多了三轴磁场测量能提供绝对的航向参考。但磁力计在室内、金属附近极易受干扰所以实际工程中6DoF方案反而更常见——用加速度计修正俯仰和横滚用陀螺仪积分维持短时航向航向的长期漂移则靠外部视觉或UWB等手段来校正。这套技术栈的应用场景远比大多数人想象的广泛。VR/AR头显需要它来追踪头部转动无人机飞控需要它来维持机身稳定手机屏幕旋转需要它来判断横竖屏扫地机器人需要它来构建房间地图甚至你手里的游戏手柄、智能手表里的计步和睡眠监测底层都在跑姿态解算算法。可以说只要涉及“物体在三维空间中怎么动”这个问题6DoF运动跟踪就是绕不开的基础设施。这篇文章面向的是想真正搞懂6DoF运动跟踪底层原理并动手实现的开发者。不管你是刚接触MPU6050的嵌入式新手还是已经在做无人机飞控但姿态环总是调不好的老手我都会从传感器特性讲起一步步拆到四元数姿态解算的代码实现把传感器融合的每个关键决策背后的逻辑讲清楚。我不会只给你一堆公式而是告诉你为什么选这个方案、参数怎么定、坑在哪里、怎么排查。读完你应该能自己搭出一套可用的6DoF姿态跟踪系统并且知道每一行代码在干什么。2. 传感器选型与数据特性为什么不能只用一种传感器2.1 加速度计、陀螺仪、磁力计各自的能力边界要理解传感器融合的必要性得先搞清楚每种传感器单独用会出什么问题。我拿最常见的MPU6050来举例它内部集成了三轴MEMS加速度计和三轴MEMS陀螺仪通过I2C接口输出原始数据。先看加速度计它的原理是检测质量块在加速度作用下的位移静止时输出的是重力加速度在三个轴上的分量。假设传感器水平放置Z轴朝上那么理论输出是(0, 0, 1g)。根据这个分量你可以反推出俯仰角和横滚角Pitch arctan2(-ax, sqrt(ay² az²))Roll arctan2(ay, az)这两个公式在静止或低速运动时非常准而且没有累积误差——因为重力是绝对参考。但问题在于一旦物体有运动加速度比如无人机加速前飞加速度计读到的就不只是重力了姿态解算立刻被污染。更麻烦的是振动电机转动带来的高频振动会让加速度计输出剧烈波动直接解算出来的角度根本不能用。再看陀螺仪它测量的是角速度单位通常是度每秒dps或弧度每秒rad/s。对角速度做一次积分就得到角度变化。陀螺仪的优点是动态响应极好短时间内的角度变化非常精确而且不受线性加速度影响。但它的致命伤是零偏bias——即使静止不动输出也不是严格的零而是有一个微小的直流偏置。这个偏置随温度变化随时间缓慢漂移。积分会把零偏误差不断累加假设零偏是0.1度每秒积分60秒后角度误差就是6度10分钟后误差60度完全不可用。磁力计的情况更复杂。它测量的是地磁场在三个轴上的分量理论上可以解算出绝对的航向角Yaw而且没有累积误差。但实际环境中电机、扬声器、金属结构、甚至手机壳里的磁铁都会产生干扰磁场强度可能远超地磁场。室内环境下磁力计的数据基本不可信所以很多6DoF方案干脆不用磁力计航向靠陀螺仪积分定期用其他手段校正。2.2 传感器融合的核心逻辑互补而非叠加理解了三种传感器的特性融合的思路就清晰了加速度计和磁力计提供长期稳定的绝对参考但动态响应差、噪声大陀螺仪提供短期精确的动态响应但存在累积漂移。融合的本质就是取长补短——用加速度计的低频信息去校正陀螺仪的高频积分漂移用陀螺仪的高频信息去平滑加速度计的噪声和运动干扰。这个思路在频域上看得最清楚。加速度计解算出的角度低频段比如0.1Hz以下是可信的因为重力方向不变高频段比如1Hz以上被运动加速度和振动污染。陀螺仪积分出的角度高频段可信低频段因为零偏积分而漂移。所以理想的做法是设计一个互补滤波器对加速度计角度做低通滤波对陀螺仪积分角度做高通滤波然后相加。这就是最经典的互补滤波姿态解算。但互补滤波有个前提你得先有加速度计解算出的角度。而加速度计解算角度需要用到反三角函数计算量不小而且在某些姿态下比如俯仰角接近±90度时会出现万向节死锁。所以更优雅的方案是直接在四元数空间做融合用梯度下降或卡尔曼滤波来更新四元数。这就是Mahony滤波和Madgwick滤波的核心思想后面会详细展开。2.3 MPU6050的实操配置要点在动手写融合算法之前MPU6050的初始化和配置有几个关键点必须处理好否则后面算法再牛也白搭。首先是量程选择。MPU6050的陀螺仪可配置为±250、±500、±1000、±2000 dps加速度计可配置为±2、±4、±8、±16 g。量程越小分辨率越高但容易饱和。对于大多数姿态跟踪场景陀螺仪选±500 dps或±1000 dps加速度计选±4g或±8g比较合适。如果你做的是无人机飞控电机振动大加速度计量程要选大一点避免削顶。其次是采样率。MPU6050内部有一个可配置的低通滤波器DLPF截止频率从5Hz到260Hz可选。采样率建议至少是信号带宽的2倍实际工程中常用1kHz采样、42Hz或98Hz的DLPF带宽。采样率越高积分越精确但I2C读取压力也越大。如果你用STM32的硬件I2C400kHz速率下读14字节加速度6陀螺仪6温度2大约需要0.3ms1kHz采样完全没问题。第三是零偏校准。这是最容易被忽视但影响最大的一步。MPU6050出厂时的零偏可能高达±20 dps不校准的话积分几秒就飘了。校准方法很简单把传感器静止水平放置采集几百到几千个样本求平均值作为零偏后续读数减去这个零偏。注意校准时要确保传感器真正静止桌面振动、风扇吹风都会影响校准精度。我一般会采集2000个样本去掉前100个等待传感器稳定然后求均值。注意MPU6050的零偏随温度变化明显从冷启动到热稳定零偏可能漂移几度每秒。如果应用场景温度变化大建议做温度补偿或者定期在静止时重新校准零偏。还有一个坑是I2C通信的稳定性。MPU6050在高速读取时偶尔会出现数据错位或NACK尤其是在长排线或电机干扰环境下。我的经验是I2C速率不要超过400kHzSDA和SCL线上拉电阻用4.7kΩ排线尽量短必要时加屏蔽。如果数据偶尔跳变可以在软件里加一个简单的异常值剔除——连续读两次如果差异超过阈值就丢弃。3. 姿态解算的数学基础从欧拉角到四元数3.1 欧拉角的直观与局限描述三维旋转最直观的方式是欧拉角绕X轴转Roll绕Y轴转Pitch绕Z轴转Yaw。人类很容易理解“先抬头30度再左转45度”这样的描述。欧拉角在游戏UI、飞行仪表上很常见但用在姿态解算里有三个致命问题。第一是万向节死锁。当Pitch角接近±90度时Roll轴和Yaw轴重合失去一个自由度解算会出现奇异点。你让一个物体先抬头90度然后无论怎么转Roll和Yaw效果是一样的数学上无法区分。这在无人机做特技动作或VR头显用户仰头看天时都会遇到。第二是旋转顺序依赖。欧拉角的三个角度值依赖于旋转顺序是先转X还是先转Y结果完全不同。不同厂商、不同软件可能用不同的顺序导致数据交换时出现混乱。第三是插值困难。两个欧拉角之间的平滑过渡需要处理角度环绕比如从179度到-179度插值算法复杂且容易出错。所以姿态解算内部几乎不用欧拉角只在最后输出给用户显示时才转成欧拉角。内部表示和运算用四元数或旋转矩阵。3.2 四元数的几何直觉与运算规则四元数听起来很玄其实可以这样理解一个四元数q [w, x, y, z]表示绕单位轴(x, y, z)旋转角度θ其中w cos(θ/2)x sin(θ/2)·axis_xy和z同理。为什么是θ/2而不是θ这是四元数的一个数学特性简单说是因为四元数旋转需要左右各乘一次所以角度减半。四元数的核心优势在于表示旋转没有奇异点运算效率高比旋转矩阵少很多乘法插值平滑球面线性插值SLERP。对于姿态解算我们关心的是四元数的微分方程dq/dt 0.5 * q ⊗ ω其中ω是角速度四元数[0, ωx, ωy, ωz]⊗是四元数乘法。这个方程告诉我们如果知道当前姿态四元数和角速度就能推下一时刻的姿态。离散化后就是q(tΔt) q(t) 0.5 * q(t) ⊗ ω * Δt然后归一化。这就是陀螺仪积分更新四元数的基本公式。但如前所述纯积分会漂移所以需要用加速度计数据来校正。四元数乘法展开写出来是q ⊗ p [ qw*pw - qx*px - qy*py - qz*pz, qw*px qx*pw qy*pz - qz*py, qw*py - qx*pz qy*pw qz*px, qw*pz qx*py - qy*px qz*pw ]这个乘法在代码里就是一个函数调用频率很高要尽量优化。我一般用宏或者内联函数避免函数调用开销。3.3 从四元数到欧拉角的转换解算出的四元数最终要转成欧拉角给用户看。转换公式如下假设旋转顺序为Z-Y-X即先Yaw再Pitch再RollRoll arctan2(2*(qwqx qyqz), 1 - 2*(qx² qy²))Pitch arcsin(2*(qwqy - qzqx))Yaw arctan2(2*(qwqz qxqy), 1 - 2*(qy² qz²))注意Pitch用arcsin因为它的范围是-90到90度正好对应sin函数的单调区间。Roll和Yaw用arctan2范围-180到180度。这些公式在代码里就是几行三角函数但要注意浮点精度和除零保护。实操心得转换出来的欧拉角在Pitch接近±90度时Roll和Yaw会剧烈跳动这是正常的数学现象不是算法bug。如果应用需要显示欧拉角建议在UI层做平滑处理或者直接显示四元数。4. 传感器融合算法实战互补滤波、Mahony与Madgwick4.1 互补滤波最简单可用的入门方案互补滤波是理解传感器融合的最佳起点。它的核心思想用一句话概括对加速度计解算的角度做低通滤波对陀螺仪积分角度做高通滤波然后相加。用公式表示angle α * (angle gyro * dt) (1 - α) * accel_angle其中α是滤波系数通常取0.95到0.99之间。α越大越信任陀螺仪动态响应好但漂移校正慢α越小越信任加速度计静态准但动态噪声大。这个系数需要根据你的采样率和应用场景调。采样率1kHz时α0.98对应的时间常数大约是1/(1-0.98)/1000 0.05秒意味着加速度计大约50ms能校正一次陀螺仪漂移。互补滤波的优点是计算量极小一个浮点乘加就能更新在8位单片机上都能跑。缺点是只对俯仰和横滚有效航向角没有绝对参考除非加磁力计而且滤波系数固定无法自适应调整。另外它是在欧拉角空间做融合仍然有万向节死锁的风险。我早期做平衡车时用的就是互补滤波代码不到20行效果对于慢速倾斜场景完全够用。但如果你要做无人机特技飞行或者VR头显的快速转动互补滤波的相位滞后会让你感觉“手感发虚”。4.2 Mahony滤波PI控制器校正的优雅实现Mahony滤波的核心创新在于它不是在欧拉角空间做融合而是在四元数空间用PI控制器来校正陀螺仪的漂移。具体来说它把加速度计测量的重力方向与当前四元数估计的重力方向做叉积得到误差向量然后用这个误差去修正陀螺仪的角速度。误差向量的计算假设当前四元数估计的重力方向是v_est [2*(qxqz - qwqy), 2*(qyqz qwqx), qw² - qx² - qy² qz²]加速度计测量归一化后是v_meas [ax, ay, az]那么误差e v_meas × v_est。这个叉积的物理意义是如果估计姿态准确两个向量应该重合叉积为零如果有偏差叉积方向指示了需要旋转的方向大小与偏差角度成正比。然后把这个误差送入PI控制器比例项Kp * e直接修正角速度积分项Ki * ∫e dt消除稳态误差修正后的角速度ω_corrected ω_gyro Kpe Ki∫e dt再用这个角速度去更新四元数。Kp和Ki的调参是关键。Kp决定收敛速度太大容易振荡太小收敛慢。Ki消除稳态误差但太大会引入低频振荡。根据Madgwick的论文和我的实测经验采样率1kHz时Kp取0.5到2.0Ki取0.001到0.01比较合适。如果加速度计噪声大Kp要取小一点如果陀螺仪零偏大Ki要取大一点。Mahony滤波的优点是计算量适中收敛性好而且PI控制器的物理意义明确调参有方向感。缺点是仍然假设加速度计只测量重力在有持续线性加速度的场景比如无人机长时间加速下会引入误差。4.3 Madgwick滤波梯度下降的工程化方案Madgwick滤波是Sebastian Madgwick在2010年提出的它的思路和Mahony不同不是用PI控制器修正而是把姿态估计问题转化成一个优化问题——找到一个四元数使得它估计的重力方向与加速度计测量方向的误差最小。然后用梯度下降法迭代求解。具体来说定义目标函数f(q) q* ⊗ g ⊗ q - a其中g是重力向量[0,0,1]a是归一化的加速度计测量值q*是四元数共轭。这个函数表示的是用当前四元数把重力向量旋转到机体坐标系与加速度计测量值的差异。我们要找q使得f(q)最小。梯度下降的更新公式q(tΔt) q(t) (dq_gyro - β * ∇f/|∇f|) * Δt其中dq_gyro是陀螺仪积分项∇f是目标函数的梯度β是梯度下降的步长控制加速度计校正的权重。β的物理意义是陀螺仪测量误差的估计值。如果陀螺仪很准β取小一点如果陀螺仪漂移大β取大一点。典型值在0.01到0.1之间。Madgwick论文里推荐β sqrt(3/4) * ω_β其中ω_β是陀螺仪测量误差的均方根值。Madgwick滤波的优点是收敛速度快计算量比卡尔曼滤波小很多而且有开源实现可以直接用。缺点是梯度下降的步长固定在动态加速度大的场景下仍然会受影响。另外它的推导涉及较多数学第一次看容易懵。4.4 三种算法的对比与选型建议算法计算量收敛速度动态性能调参难度适用场景互补滤波极低慢一般低慢速倾斜、平衡车、入门学习Mahony中等中等好中等无人机、VR头显、机器人Madgwick中等快好中等同上尤其适合陀螺仪漂移大的场景卡尔曼滤波高快优秀高高精度导航、多传感器融合选型建议如果你是第一次做姿态解算从互补滤波开始理解融合的基本逻辑。然后上Mahony或Madgwick两者性能接近Mahony的PI控制器更直观Madgwick的梯度下降收敛更快。卡尔曼滤波除非你有很强的数学背景或者需要融合多种传感器比如加GPS、视觉否则不建议一上来就搞。实操心得我实测下来MPU6050在1kHz采样、42Hz DLPF下Mahony滤波Kp1.0、Ki0.005静态角度误差小于0.5度动态响应延迟约10ms完全满足无人机和VR的基本需求。Madgwick滤波β0.05时性能相当但代码更简洁。5. 完整实现流程从I2C读取到姿态输出5.1 硬件连接与初始化配置先讲硬件。MPU6050的引脚不多VCC接3.3V注意不要接5V会烧GND接地SCL和SDA接单片机的I2C引脚另外INT引脚可以接中断用于数据就绪通知。如果你用的是模块通常还带一个XDA和XCL用于连接外部磁力计6DoF场景不用管。I2C地址AD0接地时是0x68接VCC时是0x69。大多数模块默认AD0接地地址0x68。初始化流程复位设备向PWR_MGMT_1寄存器0x6B写0x80等待100ms。唤醒向PWR_MGMT_1写0x00退出睡眠模式。设置采样率向SMPLRT_DIV寄存器0x19写分频值。采样率 陀螺仪输出率 / (1 分频值)。陀螺仪输出率默认1kHz所以写0x00得到1kHz写0x09得到100Hz。设置DLPF向CONFIG寄存器0x1A写带宽值。0x03对应42Hz0x04对应20Hz0x05对应10Hz。带宽越低噪声越小但延迟越大。设置陀螺仪量程向GYRO_CONFIG寄存器0x1B写0x08对应±500 dps0x10对应±1000 dps。设置加速度计量程向ACCEL_CONFIG寄存器0x1C写0x08对应±4g0x10对应±8g。初始化完成后读取WHO_AM_I寄存器0x75应该返回0x68确认通信正常。5.2 原始数据读取与单位转换MPU6050的数据寄存器从0x3B开始连续14个字节加速度X高、低Y高、低Z高、低温度高、低陀螺仪X高、低Y高、低Z高、低。一次I2C连续读14字节比分开读效率高很多。单位转换加速度原始值 / 灵敏度。±4g时灵敏度是8192 LSB/g所以加速度(g) raw / 8192.0。陀螺仪原始值 / 灵敏度。±500 dps时灵敏度是65.5 LSB/dps所以角速度(dps) raw / 65.5。转弧度要乘以π/180。温度raw / 340.0 36.53单位摄氏度。温度可以用来做零偏补偿但MPU6050的温度传感器精度一般只能粗略参考。5.3 零偏校准的实操步骤校准代码逻辑#define CALIB_SAMPLES 2000 float gyro_bias[3] {0, 0, 0}; void calibrate_gyro() { float sum[3] {0, 0, 0}; for (int i 0; i CALIB_SAMPLES; i) { read_mpu6050(); if (i 100) { // 跳过前100个样本 sum[0] gyro_raw[0]; sum[1] gyro_raw[1]; sum[2] gyro_raw[2]; } delay_ms(1); } gyro_bias[0] sum[0] / (CALIB_SAMPLES - 100); gyro_bias[1] sum[1] / (CALIB_SAMPLES - 100); gyro_bias[2] sum[2] / (CALIB_SAMPLES - 100); }校准完成后每次读取陀螺仪数据都要减去零偏。注意零偏是原始值不是转换后的物理量所以要在转换前减。注意校准期间传感器必须完全静止。我试过在桌上校准结果同事走过带来的振动让零偏偏了0.5 dps积分几分钟后角度就飘了。最好放在水泥地面或者减震台上校准。5.4 Mahony滤波的代码实现与参数调试下面是一个精简的Mahony滤波实现跑在STM32F103上1kHz采样CPU占用不到5%。float q0 1.0f, q1 0.0f, q2 0.0f, q3 0.0f; // 四元数 float Kp 1.0f, Ki 0.005f; float exInt 0, eyInt 0, ezInt 0; void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; // 归一化加速度计数据 norm sqrt(ax*ax ay*ay az*az); if (norm 0) return; ax / norm; ay / norm; az / norm; // 估计重力方向 vx 2*(q1*q3 - q0*q2); vy 2*(q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 误差叉积 ex ay*vz - az*vy; ey az*vx - ax*vz; ez ax*vy - ay*vx; // PI控制器 exInt ex * Ki * dt; eyInt ey * Ki * dt; ezInt ez * Ki * dt; // 修正角速度 gx Kp*ex exInt; gy Kp*ey eyInt; gz Kp*ez ezInt; // 四元数积分 float halfT 0.5f * dt; float q0_last q0, q1_last q1, q2_last q2, q3_last q3; q0 (-q1_last*gx - q2_last*gy - q3_last*gz) * halfT; q1 ( q0_last*gx q2_last*gz - q3_last*gy) * halfT; q2 ( q0_last*gy - q1_last*gz q3_last*gx) * halfT; q3 ( q0_last*gz q1_last*gy - q2_last*gx) * halfT; // 归一化四元数 norm sqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; }调参步骤先把Ki设为0只调Kp。从0.5开始逐渐增大到2.0观察静态时角度是否稳定、动态时是否跟得上。如果角度振荡Kp太大如果收敛慢Kp太小。Kp调好后加Ki。从0.001开始逐渐增大到0.01。Ki的作用是消除稳态误差但如果太大会在低频段引入振荡。用串口打印欧拉角手动转动传感器观察角度变化是否平滑、有无跳变。我一般用Kp1.0、Ki0.005作为起点大多数场景下微调即可。5.5 欧拉角输出与数据可视化解算出的四元数转欧拉角float roll atan2(2*(q0*q1 q2*q3), 1 - 2*(q1*q1 q2*q2)) * 57.29578f; float pitch asin(2*(q0*q2 - q3*q1)) * 57.29578f; float yaw atan2(2*(q0*q3 q1*q2), 1 - 2*(q2*q2 q3*q3)) * 57.29578f;57.29578是180/π把弧度转成度。数据可视化推荐用匿名上位机或者VOFA通过串口发送欧拉角实时看波形。调参时看波形比看数字直观得多。我习惯把Roll、Pitch、Yaw三条曲线放在同一个窗口转动传感器时观察三条曲线的响应和耦合情况。实操心得如果发现Yaw角缓慢漂移这是正常的——6DoF没有绝对航向参考陀螺仪零偏残余会导致Yaw积分漂移。漂移速率取决于零偏校准精度一般每小时几度到几十度。如果需要绝对航向加磁力计做9DoF融合或者用视觉/UWB做外部校正。6. 常见问题与排查技巧实录6.1 姿态解算问题速查表现象可能原因排查方法解决方案角度完全不动I2C通信失败读WHO_AM_I是否为0x68检查接线、上拉电阻、I2C地址角度剧烈跳动加速度计噪声大示波器看原始数据降低DLPF带宽、增加滤波、减震角度缓慢漂移陀螺仪零偏未校准静止时看角速度输出重新校准零偏、加温度补偿动态时角度滞后滤波系数太保守对比陀螺仪积分和融合输出增大Kp或β、提高采样率Pitch接近90度时Roll跳变万向节死锁数学现象正常UI层做平滑或显示四元数Yaw持续漂移无绝对航向参考静止时看Yaw变化加磁力计或外部校正数据偶尔错位I2C干扰连续读两次对比缩短排线、加屏蔽、软件剔除异常值6.2 三个我踩过的坑第一个坑是I2C时钟拉伸。MPU6050在某些情况下会拉伸SCL时钟如果单片机的I2C外设不支持时钟拉伸通信会失败。我用的STM32F103硬件I2C就遇到过这个问题后来换成软件I2C才稳定。如果你也遇到I2C随机NACK先试试降低速率到100kHz或者换软件I2C。第二个坑是加速度计的量程选择。我做第一版无人机飞控时加速度计量程选了±2g结果电机一启动加速度计就饱和了姿态直接飞掉。后来换成±8g问题解决。经验是只要场景有振动或冲击加速度计量程至少±8g陀螺仪至少±1000 dps。第三个坑是四元数归一化频率。我一开始每100次更新才归一化一次四元数结果发现角度慢慢偏了。后来改成每次更新都归一化问题消失。四元数归一化的计算量不大一次sqrt和四次除法1kHz下完全不是瓶颈没必要省。6.3 进阶优化方向如果你已经跑通了基本的6DoF姿态解算想进一步提升精度和鲁棒性可以考虑这几个方向自适应滤波根据加速度计的置信度动态调整Kp或β。当加速度计测量值接近1g时认为可信增大校正权重当偏离1g时认为有线性加速度减小校正权重。温度补偿MPU6050内置温度传感器可以建立零偏-温度查找表实时补偿。多传感器融合加磁力计做9DoF加气压计做高度加GPS做位置用扩展卡尔曼滤波统一融合。视觉惯性融合用摄像头做视觉里程计与IMU数据融合实现高精度的6DoF跟踪。这是目前VR/AR和机器人领域的主流方案。我个人在实际操作中的体会是6DoF姿态解算的难点不在算法本身而在工程细节——传感器校准、数据同步、异常处理、参数调试。算法网上有大量开源实现但每个硬件平台、每个应用场景的坑都不一样。最好的学习方式就是动手搭一套从MPU6050开始把原始数据读出来把四元数跑起来把欧拉角显示出来然后一点点调、一点点优化。踩过的坑越多理解越深。