一种应用于AGV的惯性姿态导航系统技术方案

一种应用于AGV的惯性姿态导航系统的工作过程如下：step1,惯性姿态导航系统随AGV开机，读取flash初始值，获取姿态初始矩阵和静基座对准；step2,系统检查flash中是否存在静止校准参数值；step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据，获得三轴加速度和三轴角速率的数据，毕卡算法更新四元数，并解算欧拉角；step4,陀螺Z轴进行低通滤波运算，将运算后的值加入姿态角四元数更新中；step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算；step6,系统检查flash中是否存在航向角尺度误差校准值；step7,系统等待用户发送当前环境校准指令。户发送当前环境校准指令。户发送当前环境校准指令。

【技术实现步骤摘要】
一种应用于AGV的惯性姿态导航系统


[0001]本专利技术涉及到惯性导航领域，较为具体的，涉及到一种应用于AGV的惯性姿态导航系统。

技术介绍

[0002]AGV(Automated Guided Vehicle)即自动导引运输车，也是我们常说的仓储机器人。AGV种类繁多，但除磁导航之外，其他所有导航模式都需要惯性姿态导航系统进行辅助导航，随着近年来AGV设备逐渐取代人力而成为工厂流水生产线和大型港口、仓库管理的主力军，适用于AGV的惯性姿态导航系统也就成为目前需求最大的惯导系统之一。
[0003]AGV领域使用惯性姿态导航系统(以下简称：惯导)主要是使用其航向角、俯仰角、横滚角数据，当下AGV需要精准的航向角数据，俯仰角和横滚角数据主要是用来防止AGV倾翻，然而航向角数据只来源于陀螺，对陀螺的校准精度决定了航向角的精度。

技术实现思路

[0004]本专利技术专利的应用于AGV的惯性姿态导航系统专为汽车设计，专为AGV 设计，基于32位MCU和三轴陀螺、三轴加速度计数据进行解算，通过将系统分为两部分，第一部分为数据解算和滤波，第二部分为校准策略组，系统在这两部分算法策略的融合下，能够输出精准的航向角、俯仰角和横滚角。
[0005]本系统由如下两部分组成：
[0006]第一部分：数据解算和滤波，数据解算和滤波包括：姿态角解算与更新、自适应卡尔曼滤波和离散低通滤波，通过采集三轴陀螺和三轴加速度计的值进行惯性姿态四元数解算，系统使用毕卡算法进行更新，更新公式如下式1，其中Qk是当前四元数，Qk
‑
1是前一时刻四元数，Δθ是角增量模值，ΔΘ为角增量Δθ计算中间变量，如下式2，I为4维单位阵。
[0007][0008][0009]将俯仰角、横滚角作为状态量，将三轴加速度计的值作为量测量，从而进行自适应卡尔曼滤波，由此可实时修正俯仰角和横滚角；由于航向角数据主要来源于陀螺Z轴，故对陀螺Z轴数据进行低通滤波，公式如下式3,其中LFn 为当前滤波输出值，LFn
‑
1为前时刻滤波输出值，a为滤波系数，该系统a取 0.3。
[0010]LFn＝a*GYROXn+(1
‑
a)*LFn
‑1ꢀꢀ
(式3)
[0011]第二部分：校准策略组，校准策略组包括：静止校准、当前环境校准和航向角尺度误差校准，将系统模块静止放置于水平台上，系统接收到静止校准指令后将进行10秒的校准，期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序；系统接收到当前环境校准指令后进行3秒的校准，期间系统自动求取陀螺Z轴的零偏，用户在使用过程中将能够根据需要多次进行该校准；将系统放置在能够水平旋转180
°
的平台上，平台旋转180
°
将给系统发送尺度误差校准指令，该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
[0012]所述的应用于AGV的惯性姿态导航系统的工作过程如下:
[0013]step1,惯性姿态导航系统随AGV开机，前3秒内完成系统初始化，读取flash初始值，获取姿态初始矩阵和静基座对准；
[0014]step2,系统检查flash中是否存在静止校准参数值，若存在则读出并加入陀螺原始值的修正，若不存在则等待指令，当收到静止校准指令时进行10 秒的静止校准；
[0015]step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据，获得三轴加速度和三轴角速率的数据，毕卡算法更新四元数，并解算欧拉角；
[0016]step4,陀螺Z轴进行低通滤波运算，将运算后的值加入姿态角四元数更新中；
[0017]step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算，获得修正的俯仰角和横滚角；
[0018]step6,系统检查flash中是否存在航向角尺度误差校准值，若存在则读出该数值并加入航向角修正，若不存在则将在获得尺度误差校准指令后进行航向角尺度误差校正；
[0019]step7,系统等待用户发送当前环境校准指令，当接收到该指令后，立刻进行3秒的当前环境校准，得到的值加入航向角误差校正。
[0020]进一步的，步骤step2中，静止校准为将系统模块静止放置于水平台上，系统接收到静止校准指令后将进行10秒的校准，期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序。
[0021]进一步的，步骤step3中，毕卡算法更新公式如下式1，其中Qk是当前四元数，Qk
‑
1是前一时刻四元数，Δθ是角增量模值，ΔΘ为角增量Δθ计算中间变量，如下式2，I为4维单位阵。
[0022][0023][0024]进一步的，步骤step4中，对陀螺Z轴数据进行低通滤波的公式如下式 3,其中LFn
为当前滤波输出值，LFn
‑
1为前时刻滤波输出值，a为滤波系数，该系统a取0.3，
[0025]LFn＝a*GYROXn+(1
‑
a)*LFn
‑1ꢀꢀ
(式 3)。
[0026]进一步的，步骤step5中，卡尔曼滤波C将俯仰角、横滚角作为状态量，将三轴加速度计的值作为量测量。
[0027]进一步的，步骤step6中，航向角修正的操作步骤为：将系统放置在能够水平旋转180
°
的平台上，平台旋转180
°
将给系统发送尺度误差校准指令，该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
[0028]进一步的，步骤step7中，系统接收到当前环境校准指令后进行3秒的校准，期间系统自动求取陀螺Z轴的零偏，用户在使用过程中将能够根据需要多次进行该校准。
附图说明
[0029]图1为本专利技术的应用于AGV的惯性姿态导航系统的运行流程图。
[0030]图2为本专利技术的应用于AGV的惯性姿态导航系统的系统连接框图。
[0031]如下具体实施方式将结合上述附图进一步说明本专利技术。
具体实施方式
[0032]具体实施案例1：
[0033]如图1所示，为本专利技术的应用于AGV的惯性姿态导航系统的运行流程图；如图2所示，为本专利技术的应用于AGV的惯性姿态导航系统的系统连接框图。
[0034]应用于AGV的惯性姿态导航系统的工作过程如下:
[0035]step1,惯性姿态导航系统随AGV开机，前3秒内完成系统初始化，读取flash初始值，获取姿态初始矩阵和静基座对准；
[0036]step2,系统检查flash中是否存在静止校准参数值，若存在则读出并加入陀螺原始值的修正，若不存在则等待指令，当收到静止校准指令时进行10 秒的静止校准；
[0037]step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据，获得三轴加速度和三轴角本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种应用于AGV的惯性姿态导航系统的工作过程如下:step1,惯性姿态导航系统随AGV开机，前3秒内完成系统初始化，读取flash初始值，获取姿态初始矩阵和静基座对准；step2,系统检查flash中是否存在静止校准参数值，若存在则读出并加入陀螺原始值的修正，若不存在则等待指令，当收到静止校准指令时进行10秒的静止校准；step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据，获得三轴加速度和三轴角速率的数据，毕卡算法更新四元数，并解算欧拉角；step4,陀螺Z轴进行低通滤波运算，将运算后的值加入姿态角四元数更新中；step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算，获得修正的俯仰角和横滚角；step6,系统检查flash中是否存在航向角尺度误差校准值，若存在则读出该数值并加入航向角修正，若不存在则将在获得尺度误差校准指令后进行航向角尺度误差校正；step7,系统等待用户发送当前环境校准指令，当接收到该指令后，立刻进行3秒的当前环境校准，得到的值加入航向角误差校正。2.如权利要求1所述的应用于AGV的惯性姿态导航系统，其特征在于：步骤step2中，静止校准为将系统模块静止放置于水平台上，系统接收到静止校准指令后将进行10秒的校准，期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序。3.如权利要求1所述的应用于AGV的惯性姿态导航系统，其特征在于：步骤step3中，毕卡算法...

【专利技术属性】
技术研发人员：刘鹏，江冠华，张亚洲，
申请(专利权)人：苏州精源创智能科技有限公司，
类型：发明
国别省市：