摘要

摘要：针对单一的MEMS陀螺仪无法解决本身的易发散和磁力计易受磁场干扰导致引入新的噪声，而带来的姿态估计不精确的问题，提出了一种基于全向AGV的优化互补滤波的姿态解算方法。偏航角不参与四元数解算，在水平姿态角四元数解算的基础上，利用共轭梯度法减小陀螺仪的漂移误差。再引进经过椭球修正后的磁力计数据作为观测量，与偏航角进行一阶互补滤波融合,并确定加权因子。搭建了基于STM32和MEMS传感器为核心的全向AGV实验平台，实验结果表明，该方法能有效抑制陀螺仪易发散和磁力计易受干扰的问题，提高姿态解算的精确性，使姿态解算具有良好的动态和静态性能，同时保证了系统的稳定性。

随着“中国制造 2025”战略的实施，以及近年来无人化工厂物流业和仓储业的蓬勃发展，使得自动导引车(Automated Guided Vehicles，AGV)的市场需求量急剧上升。在现代制造加工系统中,物流传输时间占整个产品生产总时间的 80%～90%，生产物料的传输与存储费用，占整个零部件加工费用的30%～40% [1-2]。缩短AGV在物流上传输时间，在降低整个物流过程传输和储存的费用上，起到了重要的作用。由于全向AGV在工厂和仓库中，实际用于物流运输，则必须保证AGV小车能够在各个工位之间线性行驶，通常是由上位机中的ROS系统中的导航功能包完成基于AGV小车自身位置到目标点的直线最优路径规划，将直线路径解析成一系列速度指令，由AGV中的下位机接收，并控制小车底盘完成规划的路径行驶，同时底盘传回由姿态传感器采集的姿态数据，形成闭环姿态控制。则必须研究基于全向AGV的姿态解算问题，得到精确的AGV小车底盘姿态解算数据，完成对整个小车的姿态控制，从而实现基于 SLAM 导航的四轮独立驱动的Mecanum轮的AGV线性行驶。

AGV 小车的运行姿态是小车进行 SLAM 建图和自动导航所需要获取的传感器信息中最重要的部分，其姿态数据的精确性直接影响姿态控制的精度和定位精度。姿态描述参数主要包括余弦矩阵、欧拉角和姿态四元数[3]。姿态四元数具有计算量小、运算快、全姿态、同时能避开“万向节死锁问题”等优点,在姿态控制系统中得到了广泛的应用。但是四元数属于旋转矢量，且四个变量不独立。文献[4]将四元数用于和卡尔曼滤波算法融合，但是系统为线性，将测量噪声和系统噪声引入状态方程，提高了姿态解算精度，但是在观测线性方程时引入了误差。文献[5]和文献[6]将四元数用于和无迹卡尔曼滤波算法，解决了线性化带来的误差，相比扩展卡尔曼滤波算法精度更高。文献[7]采用一种加速度计与陀螺仪融合的互补滤波算法，根据加速度计检测外力设计自适应算法修正融合权重,在一定程度上减小了系统非线性带来的测量不精确的问题，但是，由于陀螺仪积分漂移的问题，偏航角始终存在漂移。文献[8]采用基于四元数方法，将导航系中的地磁场旋转到载体系后，与三轴磁力计叉乘，得到误差向量来修正陀螺仪漂移，但是，由于磁力计直接参与了四元数的更新，姿态角易受磁场干扰，对水平姿态角的估计又引入新的干扰误差。

基于上述研究的不足，针对陀螺仪的积分漂移误差，本文在水平姿态角四元数更新方程的基础上，融合陀螺仪信息和加速度计信息，通过共轭梯度修正法进行优化后，对水平姿态角进行实时估计。针对磁力计易受干扰的情况下，使陀螺仪和磁力计通过一阶加权互补滤波，单独对AGV的偏航角进行姿态解算，让偏航角不参与四元数解算，避免磁力计引起水平姿态角的解算误差。

1.AGV小车的运动学模型以及姿态描述

本文采用的是基于激光 SLAM 导航的四轮独立驱动的 Mecanum AGV 小车，其灵活性高，可实现任意平面内的全方位移动,采用微机电系统实现姿态解算，对 AGV 小车进行系统的姿态描述[9-10]。

1.1 AGV 运动学分析

全 向 AGV 底 盘 采 用 左 右 轮 成 对 称 分 布 的Mecanum 轮，同时保证底盘对角线上的 Mecanum 轮的辊子的轴向相同，通过底层嵌入式主控板控制四个轮子的不同转向，实现在 AGV 小车在平面内的全向移动[11]。全向 AGV 采用直流电机四轮驱动，在平面上具有 3 个自由度，机动性能好且加速度性能在各个方向上比较均匀[12]。

图 1 全向 AGV 运动学模型