基于惯性传感器的姿态测量系统设计与实现

摘要：针对微机电系统陀螺仪姿态传感器输出速率不高、静态精度低、有静漂等问题，文章设计了一种基于磁力计、陀螺仪、温度传感器和单片机的姿态测量系统。将磁力计、陀螺仪数据融合解算后得到三轴姿态角，使用温度传感器做温度补偿修正。通过角度静态准确性测试及零漂测试结果表明，系统静态精度误差为±0.2°，零漂方差最大值为0.002°，有效提高了姿态测量的精度和稳定性。

关键词： 惯性传感器；扩展卡尔曼滤波；传感器数据融合；温度补偿

中图分类号：TP311 文献标识码：A

文章编号：1009-3044（2025）04-0137-04 开放科学（资源服务） 标识码（OSID） ：

0 引言

随着人工智能技术的快速发展，自动驾驶技术已被广泛应用于无人驾驶汽车和自主移动机器人等领域。自动驾驶技术的实现需要解决未知环境中的即时定位、地图构建与导航问题[1]，其中导航的精准度在很大程度上依赖于姿态检测系统的准确性。然而，惯性传感器的漂移误差以及系统噪声会导致惯性测量单元（IMU） 数据及姿态解算出现偏差[2]，使自主移动机器人平台的坐标变换及导航角出现严重误差，从而影响导航效果。

针对上述问题，本文拟构建一种适用于运动载体姿态测量的惯性检测系统，并设计姿态检测系统的软硬件实施方案。针对惯性传感器存在的漂移误差和系统噪声，选择合适的姿态数据解算方法，结合卡尔曼滤波等技术完成传感器数据融合[3-5]，并对IMU姿态误差进行校正。通过实际系统测试验证，系统采集的姿态测量值的频率最高可达1 kHz，在满足姿态矩阵修正阈值的条件下，姿态测量误差小于1°。

1 系统总体设计

设计一个最高以1 kHz速率输出并具有温度补偿的高精度姿态传感系统，并具备二次开发和扩展性，系统环境适应性强。

系统框图如图1所示，主要由3个核心传感器（磁力计RM3100、陀螺仪ICM-42688-P 和温度传感器LMT70） 、单片机STM32以及两个低压差线性稳压器构成。磁力计RM3100具有较低的噪声密度和较高的输出速率，陀螺仪ICM-42688-P的温漂小，而温度传感器LMT70则具备高精度，这三种传感器能够满足本设计的高速率和低噪声输出要求。

电源部分实现了5 V转3.3 V和3.3 V转1.8 V的功能。3.3 V电源为磁力计、陀螺仪、温度传感器和单片机提供电源，1.8V电源则为陀螺仪的内核和模拟电源供电。陀螺仪向单片机提供采集到的角速度和加速度数据，单片机通过扩展卡尔曼滤波器得到角度数据。磁力计将地磁场数据传送给单片机，单片机将地磁场数据与扩展卡尔曼滤波器得到的角度数据进行进一步融合，以获得更高可靠性的航向角数据。温度传感器将采集的温度数据传递给单片机，单片机经过处理后进行角度的温度补偿。单片机通过外部接口中的串口及通用输入输出接口与外部进行数据交互。

2 硬件设计

2.1 单片机最小系统板设计

单片机最小系统板设计主要包括单片机外围电路、电源电路、串口转换为通用串口总线的电路以及按键等电路。单片机的外围电路主要包括复位电路、外部晶振电路和退耦电容电路[6]，原理图如图2所示。复位电路由电容C6 和按键组成，晶振电路采用16MHz晶振及其配套电容，退耦电容电路由电容C7 和C8构成。电源电路采用低压差线性稳压器AP2112 芯片实现5V转3.3V的功能。串口转通用串口总线电路采用转换芯片CH340x和Type-C接口，可直接使用Type-C数据线进行电脑与STM32芯片之间的通信。

2.2 磁力计测试板设计

磁力计RM3100 的测试板设计主要包括5 V 转3.3 V电源和磁力计外围电路两个部分。测试板的原理图如图3 所示。电源部分在低压差线性稳压器AP2112 芯片的电路中增加了瞬态电压抑制二极管ESD5451N，以吸收电源的高尖峰并提供静电防护。磁力计的外围电路包括三路RL振荡器，其阻值分别为R1、R2、R3、R5、R6和R7。R4为磁力计内部振荡器的外部电阻，C4至C7为磁力计的退耦和滤波电容。

2.3 陀螺仪测试板设计

陀螺仪ICM-42688-P的测试板主要包括5 V转3.3 V电源、3.3 V转1.8 V电源和陀螺仪外围电路3个部分。电源部分分别采用低压差线性稳压器AP2112-3.3和AP2112-1.8，实现5 V转3.3 V及3.3 V 转1.8 V的功能。5 V转3.3 V电源电路的输入端采用肖特基二极管D1和瞬态电压抑制二极管D2，以提高电路的输出稳定性。同时，在输出端设计的钽电容参数为4.7 μF，以改善输出与下一级3.3 V转1.8 V的寄生电感，从而获得更小纹波的1.8 V 电源输出。在3.3 V转1.8 V电源电路中，采用500 Ω的磁珠及电容E2、C5、C6来抑制电源纹波，为陀螺仪提供低于1.5 mA 的模拟电压输入。ICM-42688-P测试板的原理图如图4所示。

3 软件设计

3.1 单片机配置及初始化

本设计使用了两个串口，UART1和UART2，分别用于接收和发送数据，其中UART1用于接收，UART2 用于发送。串口1的接收同时采用DMA接收，配合队列的缓冲接收和空闲中断。在完成单片机初始化（包括UART 和DMA 的初始化） 后，从堆中获取大小为100字节的空间作为串口的DMA接收缓存，然后等待中断。

3.2 磁力计配置

磁力计RM3100的初始化时先进行自检。首先向相应的寄存器写入控制指令以执行传感器自检，判断标志位DRDY是否变高。如果在超出等待时间后标志位无反应，则自检失败，可以读取自检寄存器以判断哪个轴未通过自检。如果标志位变高且读取自检寄存器的值为0xfa，则表示自检成功。自检成功后，设置三轴循环计数寄存器为155，使磁力计在连续模式下的输出频率为220 Hz。同时，设置数据更新速率为600 Hz，启用传感器的三个轴并设置为连续模式。最后，启用单片机的外部中断，完成磁力计的初始化。初始化流程图如图5所示。

3.3 陀螺仪配置及零偏矫正

陀螺仪ICM-42688-P的初始化过程首先进行自检，以确保传感器正常工作。完成自检后，配置寄存器以输出信号，开启三轴陀螺仪和三轴加速度计的电源，并设置SPI模式以及旁路FIFO。接着，设置温度输出的内部数字滤波器截止频率为40 Hz，并在配置陀螺仪和加速度输出速率后完成初始化。

初始化完成后，芯片可以输出三轴角速度和三轴加速度数据。然而，角速度的输出可能存在零偏、高频噪声及加速度输出不对称等误差。零偏校准的方法是以1 kHz的频率连续读取1 000个角速度数据，并计算出平均值作为偏置数据。为了保证零偏校准的精确性，将计算出的偏置数据与预设值进行比较。如果偏差过大，则须重新进行零偏矫正。

3.4 温度传感器配置及拟合

温度传感器的测试配置流程图如图6所示。在单片机外设完成初始化后，开启DMA传输，使能定时器4通道4，然后开始采集温度传感器的输出。当定时器4通道4发出上升沿信号后，ADC开始采集数据。采集完成后，由DMA自动将数据传输到设定的内存中，并触发DMA的传输完成中断。

初始化完成后，设计了温度传感器的IIR低通滤波器并对其进行了数据拟合。由于测试环境受限，所使用的温度计在显示实时性方面与输出的精准度有限。但在实际系统应用中，陀螺仪温漂矫正时会对温度进行一定的修正。

4 实验结果与分析

4.1 角度静态准确性测试

为评估系统的静态精度，使用可调式角度规铣床垫块作为角度标准器进行静态准确度测试。具体测试方案为：使用一个固定板作为支架，将垫块的可移动测量面伸出，并在固定板上打上螺丝孔，以将整个系统固定于垫块上。由于垫块的测量范围限制（以45°为零点，单边75°） ，因此需要改变4次系统的结构才能完成一次完整的测量，实物图如图7所示。在角度测量时，以5°为步进值，先将垫块带动陀螺仪旋转后固定，随后读取姿态测量系统输出的数据并记录。

俯仰角测试数据和标准值如图8和图9所示，横滚角测试数据和标准值如图10和图11所示。图中横坐标为标准角度，纵坐标为测量角度。圆点代表姿态测量系统的测量值，星号代表角度标准值，直线为拟合后的测量值直线。从图中可以看出，拟合后的直线基本穿过标准值。通过计算，将拟合后的测量值与标准值进行比较，结果显示两轴角度测量误差均小于0.2°。

4.2 零漂测试

在俯仰角、横滚角和航向角的零漂测试中，系统的输出频率设置为0.00333Hz（每5分钟输出一次） ，静置放置5个半小时。三个轴的角度变化如图12～图14 所示。从图中可以看出，输出角度基本无零漂现象，偏移值较小。三轴角度测量的误差方差均小于0.002，俯仰角和横滚角的最大偏差均小于0.08°，航向角的偏差为0.1°，达到了设计预期。然而，由于测试条件的限制和机械结构的安装误差，三轴角度静态输出的初始值并不为零。

5 结论

本文设计并实现了一种基于惯性传感器的姿态测量系统，该系统结合了磁力计、陀螺仪和温度传感器。通过数据融合和姿态解算算法，有效提高了姿态测量的精度和稳定性。经测试，该系统的输出速率最高可达1 kHz，静态精度误差为±0.2°，零漂方差的最大值为0.002°。然而，由于测试时机械结构安装存在误差，无法进行精确的角度校准和修正。同时，缺乏可调的恒温环境使得温度传感器的拟合效果有所欠缺。在后续研究中，需要探索更有效的温度补偿方法，以增强系统在复杂环境下的适应性。

参考文献：

[1] 景元泉，王跃辉，韩伟，等.无人驾驶车辆与移动机器人SLAM 方法综述[J].电子世界，2021（13）：4-5.

[2] 贾浩男.室内行人自主定位算法的研究与实现[D].哈尔滨： 哈尔滨工程大学，2017.

[3] 樊志强，董朝轶，王启来，等.基于多传感器数据融合的巡检机器人测姿系统研究[J].自动化与仪器仪表，2021（3）：77-82，86.

[4] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].2 版.西安：西北工业大学出版社，2012.

[5] 宋国东，姜守达，林连雷.复杂有色噪声广义系统信息融合Kalman滤波器[J].仪器仪表学报，2013，34（5）：1195-1200.

[6] 周润景，张丽娜，丁莉.基于PROTEUS的电路及单片机设计与仿真[M].2版.北京：北京航空航天大学出版社，2010.

【通联编辑：梁书】

基金项目：基于SLAM 技术的仓储运输小车的路径规划及调度（项目编号：2022ZR033）