CN112797979B - 一种应用于agv的惯性姿态导航*** - Google Patents
一种应用于agv的惯性姿态导航*** Download PDFInfo
- Publication number
- CN112797979B CN112797979B CN202011635138.0A CN202011635138A CN112797979B CN 112797979 B CN112797979 B CN 112797979B CN 202011635138 A CN202011635138 A CN 202011635138A CN 112797979 B CN112797979 B CN 112797979B
- Authority
- CN
- China
- Prior art keywords
- calibration
- angle
- value
- axis
- flash
- 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.)
- Active
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 (1)
1.一种应用于AGV的惯性姿态导航***，其特征在于，工作过程如下:
step1,惯性姿态导航***随AGV开机，前3秒内完成***初始化，读取flash初始值，获取姿态初始矩阵和静基座对准；
step2,***检查flash中是否存在静止校准参数值，若存在则读出并加入陀螺原始值的修正，若不存在则等待指令，当收到静止校准指令时进行10秒的静止校准；静止校准为将***模块静止放置于水平台上，校准期间***自动求取陀螺和加速度计的零偏并将此数据保存在MCU的flash中；
step3,***以1000HZ频率采集三轴陀螺与三轴加速度计的数据，获得三轴加速度和三轴角速率的数据，毕卡算法更新四元数，并解算欧拉角,其中，毕卡算法更新公式如下式1，其中Qk是当前四元数，Qk-1是前一时刻四元数，Δθ是角增量模值，ΔΘ为角增量Δθ计算中间变量，如下式2，I为4维单位阵，
step4,陀螺Z轴进行低通滤波运算，将运算后的值加入姿态角四元数更新中，对陀螺Z轴数据进行低通滤波的公式如下式3,其中LFn为当前滤波输出值，LFn-1为前时刻滤波输出值，a为滤波系数，该***a取0.3，
LFn＝a*GYROXn+(1-a)*LFn-1 (式3)；
step5,***根据三轴加速度计和当前解算角度进行自适应卡尔曼滤波计算，获得修正的俯仰角和横滚角，其中，卡尔曼滤波将俯仰角、横滚角作为状态量，将三轴加速度计的值作为量测量；
step6,***检查flash中是否存在航向角尺度误差校准值，若存在则读出该数值并加入航向角修正，若不存在则将在获得尺度误差校准指令后进行航向角尺度误差校正，航向角修正的操作步骤为：将***放置在能够水平旋转180°的平台上，平台旋转180°将给***发送尺度误差校准指令，该***接收到指令后进行尺度校准并将校准值保存在MCU的flash中；
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 CN112797979A (zh) | 2021-05-14 |
| CN112797979B true 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) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114485641B (zh) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
| CN115127548A (zh) * | 2022-06-28 | 2022-09-30 | 苏州精源创智能科技有限公司 | 一种惯导与激光点阵融合导航*** |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101929862A (zh) * | 2009-06-26 | 2010-12-29 | 上海市上海中学 | 基于卡尔曼滤波的惯性导航***初始姿态确定方法 |
| US9753144B1 (en) * | 2016-02-12 | 2017-09-05 | GM Global Technology Operations LLC | Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data |
| CN106705968B (zh) * | 2016-12-09 | 2020-11-27 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
| CN110221302B (zh) * | 2019-05-24 | 2023-04-18 | 上海高智科技发展有限公司 | 环境探测装置及其修正方法、***、便携设备及存储介质 |
| CN110954098A (zh) * | 2019-12-18 | 2020-04-03 | 苏州精源创智能科技有限公司 | 一种应用于汽车的惯性导航*** |
-
2020
- 2020-12-31 CN CN202011635138.0A patent/CN112797979B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112797979A (zh) | 2021-05-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108318052B (zh) | 一种基于双轴连续旋转的混合式平台惯导***标定方法 | |
| CN106959110B (zh) | 一种云台姿态检测方法及装置 | |
| KR102017404B1 (ko) | 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법 | |
| US9222801B2 (en) | Apparatus and method for correcting error of gyro sensor in mobile robot | |
| CN112797979B (zh) | 一种应用于agv的惯性姿态导航*** | |
| US20040176882A1 (en) | Attitude sensing apparatus for determining the attitude of a mobile unit | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| CN105352527B (zh) | 一种基于双轴转位机构光纤陀螺标定方法 | |
| CN110515381B (zh) | 用于定位机器人的多传感器融合算法 | |
| CN105910606A (zh) | 一种基于角速度差值的方向修正方法 | |
| CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| CN111896007A (zh) | 一种补偿足地冲击的四足机器人姿态解算方法 | |
| CN116067370B (zh) | 一种imu姿态解算方法及设备、存储介质 | |
| CN110873563B (zh) | 一种云台姿态估计方法及装置 | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| CN110986928A (zh) | 光电吊舱三轴陀螺仪漂移实时修正方法 | |
| CN114964222A (zh) | 一种车载imu姿态初始化方法、安装角估计方法及装置 | |
| CN113203429A (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
| CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
| CN113009816B (zh) | 时间同步误差的确定方法及装置、存储介质及电子装置 | |
| CN111307114A (zh) | 基于运动参考单元的水面舰船水平姿态测量方法 | |
| CN114459466A (zh) | 一种基于模糊控制的mems多传感器数据融合处理方法 | |
| CN113532481A (zh) | 一种mems惯性导航***的零偏补偿方法 | |
| CN110940357B (zh) | 一种用于旋转惯导单轴自对准的内杆臂标定方法 | |
| CN111207734A (zh) | 一种基于ekf的无人机组合导航方法 |
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 |