CN112797979A - 一种应用于agv的惯性姿态导航系统 - Google Patents
一种应用于agv的惯性姿态导航系统 Download PDFInfo
- Publication number
- CN112797979A CN112797979A CN202011635138.0A CN202011635138A CN112797979A CN 112797979 A CN112797979 A CN 112797979A CN 202011635138 A CN202011635138 A CN 202011635138A CN 112797979 A CN112797979 A CN 112797979A
- Authority
- CN
- China
- Prior art keywords
- calibration
- value
- axis
- angle
- agv
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
一种应用于AGV的惯性姿态导航系统的工作过程如下:step1,惯性姿态导航系统随AGV开机,读取flash初始值,获取姿态初始矩阵和静基座对准;step2,系统检查flash中是否存在静止校准参数值;step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据,获得三轴加速度和三轴角速率的数据,毕卡算法更新四元数,并解算欧拉角;step4,陀螺Z轴进行低通滤波运算,将运算后的值加入姿态角四元数更新中;step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算;step6,系统检查flash中是否存在航向角尺度误差校准值;step7,系统等待用户发送当前环境校准指令。
Description
技术领域
本发明涉及到惯性导航领域,较为具体的,涉及到一种应用于AGV的惯性姿态导航系统。
背景技术
AGV(Automated Guided Vehicle)即自动导引运输车,也是我们常说的仓储机器人。AGV种类繁多,但除磁导航之外,其他所有导航模式都需要惯性姿态导航系统进行辅助导航,随着近年来AGV设备逐渐取代人力而成为工厂流水生产线和大型港口、仓库管理的主力军,适用于AGV的惯性姿态导航系统也就成为目前需求最大的惯导系统之一。
AGV领域使用惯性姿态导航系统(以下简称:惯导)主要是使用其航向角、俯仰角、横滚角数据,当下AGV需要精准的航向角数据,俯仰角和横滚角数据主要是用来防止AGV倾翻,然而航向角数据只来源于陀螺,对陀螺的校准精度决定了航向角的精度。
发明内容
本发明专利的应用于AGV的惯性姿态导航系统专为汽车设计,专为AGV 设计,基于32位MCU和三轴陀螺、三轴加速度计数据进行解算,通过将系统分为两部分,第一部分为数据解算和滤波,第二部分为校准策略组,系统在这两部分算法策略的融合下,能够输出精准的航向角、俯仰角和横滚角。
本系统由如下两部分组成:
第一部分:数据解算和滤波,数据解算和滤波包括:姿态角解算与更新、自适应卡尔曼滤波和离散低通滤波,通过采集三轴陀螺和三轴加速度计的值进行惯性姿态四元数解算,系统使用毕卡算法进行更新,更新公式如下式1,其中Qk是当前四元数,Qk-1是前一时刻四元数,Δθ是角增量模值,ΔΘ为角增量Δθ计算中间变量,如下式2,I为4维单位阵。
将俯仰角、横滚角作为状态量,将三轴加速度计的值作为量测量,从而进行自适应卡尔曼滤波,由此可实时修正俯仰角和横滚角;由于航向角数据主要来源于陀螺Z轴,故对陀螺Z轴数据进行低通滤波,公式如下式3,其中LFn 为当前滤波输出值,LFn-1为前时刻滤波输出值,a为滤波系数,该系统a取 0.3。
LFn=a*GYROXn+(1-a)*LFn-1 (式3)
第二部分:校准策略组,校准策略组包括:静止校准、当前环境校准和航向角尺度误差校准,将系统模块静止放置于水平台上,系统接收到静止校准指令后将进行10秒的校准,期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序;系统接收到当前环境校准指令后进行3秒的校准,期间系统自动求取陀螺Z轴的零偏,用户在使用过程中将能够根据需要多次进行该校准;将系统放置在能够水平旋转180°的平台上,平台旋转180°将给系统发送尺度误差校准指令,该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
所述的应用于AGV的惯性姿态导航系统的工作过程如下:
step1,惯性姿态导航系统随AGV开机,前3秒内完成系统初始化,读取flash初始值,获取姿态初始矩阵和静基座对准;
step2,系统检查flash中是否存在静止校准参数值,若存在则读出并加入陀螺原始值的修正,若不存在则等待指令,当收到静止校准指令时进行10 秒的静止校准;
step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据,获得三轴加速度和三轴角速率的数据,毕卡算法更新四元数,并解算欧拉角;
step4,陀螺Z轴进行低通滤波运算,将运算后的值加入姿态角四元数更新中;
step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算,获得修正的俯仰角和横滚角;
step6,系统检查flash中是否存在航向角尺度误差校准值,若存在则读出该数值并加入航向角修正,若不存在则将在获得尺度误差校准指令后进行航向角尺度误差校正;
step7,系统等待用户发送当前环境校准指令,当接收到该指令后,立刻进行3秒的当前环境校准,得到的值加入航向角误差校正。
进一步的,步骤step2中,静止校准为将系统模块静止放置于水平台上,系统接收到静止校准指令后将进行10秒的校准,期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序。
进一步的,步骤step3中,毕卡算法更新公式如下式1,其中Qk是当前四元数,Qk-1是前一时刻四元数,Δθ是角增量模值,ΔΘ为角增量Δθ计算中间变量,如下式2,I为4维单位阵。
进一步的,步骤step4中,对陀螺Z轴数据进行低通滤波的公式如下式 3,其中LFn为当前滤波输出值,LFn-1为前时刻滤波输出值,a为滤波系数,该系统a取0.3,
LFn=a*GYROXn+(1-a)*LFn-1 (式 3)。
进一步的,步骤step5中,卡尔曼滤波C将俯仰角、横滚角作为状态量,将三轴加速度计的值作为量测量。
进一步的,步骤step6中,航向角修正的操作步骤为:将系统放置在能够水平旋转180°的平台上,平台旋转180°将给系统发送尺度误差校准指令,该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
进一步的,步骤step7中,系统接收到当前环境校准指令后进行3秒的校准,期间系统自动求取陀螺Z轴的零偏,用户在使用过程中将能够根据需要多次进行该校准。
附图说明
图1为本发明的应用于AGV的惯性姿态导航系统的运行流程图。
图2为本发明的应用于AGV的惯性姿态导航系统的系统连接框图。
如下具体实施方式将结合上述附图进一步说明本发明。
具体实施方式
具体实施案例1:
如图1所示,为本发明的应用于AGV的惯性姿态导航系统的运行流程图;如图2所示,为本发明的应用于AGV的惯性姿态导航系统的系统连接框图。
应用于AGV的惯性姿态导航系统的工作过程如下:
step1,惯性姿态导航系统随AGV开机,前3秒内完成系统初始化,读取flash初始值,获取姿态初始矩阵和静基座对准;
step2,系统检查flash中是否存在静止校准参数值,若存在则读出并加入陀螺原始值的修正,若不存在则等待指令,当收到静止校准指令时进行10 秒的静止校准;
step3,系统以1000HZ频率采集三轴陀螺与三轴加速度计的数据,获得三轴加速度和三轴角速率的数据,毕卡算法更新四元数,并解算欧拉角;
step4,陀螺Z轴进行低通滤波运算,将运算后的值加入姿态角四元数更新中;
step5,系统根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算,获得修正的俯仰角和横滚角;
step6,系统检查flash中是否存在航向角尺度误差校准值,若存在则读出该数值并加入航向角修正,若不存在则将在获得尺度误差校准指令后进行航向角尺度误差校正;
step7,系统等待用户发送当前环境校准指令,当接收到该指令后,立刻进行3秒的当前环境校准,得到的值加入航向角误差校正。
进一步的,步骤step2中,静止校准为将系统模块静止放置于水平台上,系统接收到静止校准指令后将进行10秒的校准,期间系统自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中。该校准是出厂设置的一道工序。
进一步的,步骤step3中,毕卡算法更新公式如下式1,其中Qk是当前四元数,Qk-1是前一时刻四元数,Δθ是角增量模值,ΔΘ为角增量Δθ计算中间变量,如下式2,I为4维单位阵。
进一步的,步骤step4中,对陀螺Z轴数据进行低通滤波的公式如下式 3,其中LFn为当前滤波输出值,LFn-1为前时刻滤波输出值,a为滤波系数,该系统a取0.3,
LFn=a*GYROXn+(1-a)*LFn-1 (式 3)。
进一步的,步骤step5中,卡尔曼滤波C将俯仰角、横滚角作为状态量,将三轴加速度计的值作为量测量。
进一步的,步骤step6中,航向角修正的操作步骤为:将系统放置在能够水平旋转180°的平台上,平台旋转180°将给系统发送尺度误差校准指令,该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
进一步的,步骤step7中,系统接收到当前环境校准指令后进行3秒的校准,期间系统自动求取陀螺Z轴的零偏,用户在使用过程中将能够根据需要多次进行该校准。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以所附权利要求为准。
Claims (7)
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中。该校准是出厂设置的一道工序。
4.如权利要求1所述的应用于AGV的惯性姿态导航系统,其特征在于:步骤step4中,对陀螺Z轴数据进行低通滤波的公式如下式3,其中LFn为当前滤波输出值,LFn-1为前时刻滤波输出值,a为滤波系数,该系统a取0.3,LFn=a*GYROXn+(1-a)*LFn-1 (式3)。
5.如权利要求1所述的应用于AGV的惯性姿态导航系统,其特征在于:步骤step5中,卡尔曼滤波C将俯仰角、横滚角作为状态量,将三轴加速度计的值作为量测量。
6.如权利要求1所述的应用于AGV的惯性姿态导航系统,其特征在于:步骤step6中,航向角修正的操作步骤为:将系统放置在能够水平旋转180°的平台上,平台旋转180°将给系统发送尺度误差校准指令,该系统接收到指令后进行尺度校准并将校准值保存在MCU的flash中。
7.如权利要求1所述的应用于AGV的惯性姿态导航系统,其特征在于:步骤step7中,系统接收到当前环境校准指令后进行3秒的校准,期间系统自动求取陀螺Z轴的零偏,用户在使用过程中将能够根据需要多次进行该校准。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011635138.0A CN112797979B (zh) | 2020-12-31 | 2020-12-31 | 一种应用于agv的惯性姿态导航系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011635138.0A CN112797979B (zh) | 2020-12-31 | 2020-12-31 | 一种应用于agv的惯性姿态导航系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112797979A true CN112797979A (zh) | 2021-05-14 |
CN112797979B CN112797979B (zh) | 2022-07-01 |
Family
ID=75808612
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011635138.0A Active CN112797979B (zh) | 2020-12-31 | 2020-12-31 | 一种应用于agv的惯性姿态导航系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112797979B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114485641A (zh) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
CN115127548A (zh) * | 2022-06-28 | 2022-09-30 | 苏州精源创智能科技有限公司 | 一种惯导与激光点阵融合导航系统 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101929862A (zh) * | 2009-06-26 | 2010-12-29 | 上海市上海中学 | 基于卡尔曼滤波的惯性导航系统初始姿态确定方法 |
CN106705968A (zh) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
US20170234988A1 (en) * | 2016-02-12 | 2017-08-17 | GM Global Technology Operations LLC | Bias and misalignment compensation for 6-dof imu using gnss/ins data |
CN110221302A (zh) * | 2019-05-24 | 2019-09-10 | 上海高智科技发展有限公司 | 环境探测装置及其修正方法、系统、便携设备及存储介质 |
CN110954098A (zh) * | 2019-12-18 | 2020-04-03 | 苏州精源创智能科技有限公司 | 一种应用于汽车的惯性导航系统 |
-
2020
- 2020-12-31 CN CN202011635138.0A patent/CN112797979B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101929862A (zh) * | 2009-06-26 | 2010-12-29 | 上海市上海中学 | 基于卡尔曼滤波的惯性导航系统初始姿态确定方法 |
US20170234988A1 (en) * | 2016-02-12 | 2017-08-17 | GM Global Technology Operations LLC | Bias and misalignment compensation for 6-dof imu using gnss/ins data |
CN106705968A (zh) * | 2016-12-09 | 2017-05-24 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
CN110221302A (zh) * | 2019-05-24 | 2019-09-10 | 上海高智科技发展有限公司 | 环境探测装置及其修正方法、系统、便携设备及存储介质 |
CN110954098A (zh) * | 2019-12-18 | 2020-04-03 | 苏州精源创智能科技有限公司 | 一种应用于汽车的惯性导航系统 |
Non-Patent Citations (9)
Title |
---|
WU, YUANXIN 等: ""Attitude Reconstruction From Inertial Measurements: QuatFIter and Its Comparison with RodFIter"", 《IEEE TRANSACTIONS ON AEROSPACE & ELECTRONIC SYSTEMS》 * |
WU, YUANXIN 等: ""Attitude Reconstruction From Inertial Measurements: QuatFIter and Its Comparison with RodFIter"", 《IEEE TRANSACTIONS ON AEROSPACE & ELECTRONIC SYSTEMS》, vol. 55, no. 6, 30 December 2019 (2019-12-30), pages 3629 - 3639, XP011753608, DOI: 10.1109/TAES.2019.2910360 * |
埃博尔梅德·纽诺丁 等: "《惯性导航、卫星定位及其组合的基本原理》", 30 March 2017, pages: 117 - 121 * |
尹剑等: "基于角速率与角增量的捷联惯导姿态算法", 《四川兵工学报》 * |
尹剑等: "基于角速率与角增量的捷联惯导姿态算法", 《四川兵工学报》, no. 11, 25 November 2015 (2015-11-25), pages 101 * |
王晓宇等: "基于扩展卡尔曼滤波的两轮机器人姿态估计", 《哈尔滨工业大学学报》 * |
王晓宇等: "基于扩展卡尔曼滤波的两轮机器人姿态估计", 《哈尔滨工业大学学报》, no. 12, 15 December 2007 (2007-12-15), pages 1920 - 1923 * |
邓洪洁: ""基于角速度的捷联惯导系统姿态算法研究"", 《科技风》 * |
邓洪洁: ""基于角速度的捷联惯导系统姿态算法研究"", 《科技风》, no. 1, 30 December 2012 (2012-12-30), pages 43 - 44 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114485641A (zh) * | 2022-01-24 | 2022-05-13 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
CN114485641B (zh) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
CN115127548A (zh) * | 2022-06-28 | 2022-09-30 | 苏州精源创智能科技有限公司 | 一种惯导与激光点阵融合导航系统 |
Also Published As
Publication number | Publication date |
---|---|
CN112797979B (zh) | 2022-07-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108318052B (zh) | 一种基于双轴连续旋转的混合式平台惯导系统标定方法 | |
CN106647791B (zh) | 三维姿态测控装置、机械设备及三维姿态的测控方法 | |
CN107270893B (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
US9222801B2 (en) | Apparatus and method for correcting error of gyro sensor in mobile robot | |
CN112797979B (zh) | 一种应用于agv的惯性姿态导航系统 | |
CN105910606B (zh) | 一种基于角速度差值的方向修正方法 | |
CN105352527B (zh) | 一种基于双轴转位机构光纤陀螺标定方法 | |
CN111896007B (zh) | 一种补偿足地冲击的四足机器人姿态解算方法 | |
CN110515381B (zh) | 用于定位机器人的多传感器融合算法 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
CN108132060A (zh) | 一种捷联惯导系统无基准的系统级标定方法 | |
CN113203429B (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
CN114216456A (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
CN111307114B (zh) | 基于运动参考单元的水面舰船水平姿态测量方法 | |
CN108592902B (zh) | 一种基于多传感器的定位设备及定位方法、系统和机械臂 | |
CN114964222A (zh) | 一种车载imu姿态初始化方法、安装角估计方法及装置 | |
CN108416387B (zh) | 基于gps与气压计融合数据的高度滤波方法 | |
CN110940357B (zh) | 一种用于旋转惯导单轴自对准的内杆臂标定方法 | |
CN116380119A (zh) | 组合导航的校准方法、装置和系统 | |
Zhang et al. | Automatic kinematic calibration of multiaxis gimbal systems | |
CN111089606B (zh) | 一种三自激光惯组关键参数快速自标定方法 | |
CN114763994B (zh) | 一种应用于扫地机器人的惯性姿态导航系统 | |
CN110779552B (zh) | 一种地球固联坐标系下的自适应对准方法 | |
CN114966115B (zh) | 一种基于弹载惯性/星光组合导航的加速度标定方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |