[发明专利]AGV组合导航的多传感器融合定位方法

说明书

本发明公开了一种AGV组合导航的多传感器融合定位方法，所述方法包括：确定坐标：获取AGV机器人的载体坐标系(b)与地理坐标系(n)；数据处理：采用四元数微分方程进行姿态解算；获取数据：通过陀螺仪、角速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度；滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态。本发明提供一种AGV组合导航的多传感器融合定位方法，使线性化程度和精度均有所提高，并且降低了全姿态解算算法的计算量和误差。

技术领域

本发明涉及自动导引机器人技术领域，尤其涉及一种AGV组合导航的多传感器融合定位方法。

背景技术

AGV是(Automated Guided Vehicle)的缩写，即“自动导引运输车”，是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。AGV组合导航技术一般通过两步定位，第一步为粗定位，采用柔性高、精度低的导引方式初步定位，确定AGV的当前位置；第二步为精定位，通过技术成熟、可靠性髙、成本低的导引方式进行误差矫正，提高AGV的定位精度。组合导航作为导航技术发展的重要方向之一，目前在汽车、航海、航空、航天等领域得到广泛运用。

AGV的导航引导技术的核心在于AGV姿态解算，即是确定载体坐标系与地理坐标系之间的方位关系，目前常用的算法分为欧拉角微分方程法、方向余弦微分方程法和四元数微分方程法。其中，欧拉角法虽然较为简单，但三角运算困难，在俯仰角为90度附近时算法将会出现退化失效现象，不能用于全姿态解算；方向余弦法虽可避免欧拉角法产生的方程退化现象，但需解算九个微分方程，计算量大；四元数姿态解算的线性化程度和精度均较高，并可实现全姿态解算。但是，运用四元数姿态解算法，陀螺仪测量角速度会产生累积误差，容易发散，而加速度计和磁力计不易产生累积误差，但实时精度较差。因此，设计一种线性化程度和精度均较高，并可降低计算量的全姿态解算算法是本领域技术人员迫切需要解决的技术问题。

发明内容

本发明的目的在于提供一种AGV组合导航的多传感器融合定位方法，使线性化程度和精度均有所提高，并且降低了全姿态解算算法的计算量和误差。

本发明公开的AGV组合导航的多传感器融合定位方法所采用的技术方案是:

一种AGV组合导航的多传感器融合定位方法，所述方法包括：

确定坐标：获取AGV机器人的载体坐标系(b)与地理坐标系(n)；

数据处理：采用四元数微分方程进行姿态解算；

获取数据：通过陀螺仪、角速度计和磁力计分别获取AGV机器人的角速度、运行的加速度和磁场强度；

滤波去噪：采用互补滤波算法，通过加速度计和磁力计测量值修正四元数来修正姿态。

作为优选方案，所述获取数据步骤中，所述陀螺仪采用三个独立的MEMS检测X、Y、Z轴的电容变化，输出与角速度变化相关的电压值。

作为优选方案，所述滤波去噪步骤中，通过陀螺仪、加速度计和磁力计测得的值对俯仰角、翻滚角、航向角和四元数初始化，计算公式如下：

q_{0_init}＝cos(0.5*roll₀)*cos(0.5*pitch₀)*cos(0.5*yaw₀)-

sin(0.5*roll₀)*sin(0.5*pitch₀)*sin(0.5*yaw₀)