CN111474938A - 一种惯性导航自动导引小车及其航迹确定方法 - Google Patents

一种惯性导航自动导引小车及其航迹确定方法 Download PDF

Info

Publication number
CN111474938A
CN111474938A CN202010364424.1A CN202010364424A CN111474938A CN 111474938 A CN111474938 A CN 111474938A CN 202010364424 A CN202010364424 A CN 202010364424A CN 111474938 A CN111474938 A CN 111474938A
Authority
CN
China
Prior art keywords
guided vehicle
automatic guided
inertial navigation
module
magnetic
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.)
Pending
Application number
CN202010364424.1A
Other languages
English (en)
Inventor
董朝轶
樊志强
陈晓艳
齐咏生
张丽杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Inner Mongolia University of Technology
Original Assignee
Inner Mongolia University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Inner Mongolia University of Technology filed Critical Inner Mongolia University of Technology
Priority to CN202010364424.1A priority Critical patent/CN111474938A/zh
Publication of CN111474938A publication Critical patent/CN111474938A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means
    • G05D1/0261—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means using magnetic plots
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及惯性导航自动导引小车及其航迹确定方法。上述惯性导航自动导引小车包括：读取地面磁条上各磁钉间的中线的距离的磁导航模块、用于获取自动导引小车的行走路线的惯性导航模块、用于提供与自动导引小车作动相对应的操控界面，并用于输入自动导引小车的作动指的人机交互模块和用于根据中线的距离校正自动导引小车的行走路线，并根据操作界面输入的作动指令生成控制指令的控制模块。本发明通过采用磁导航模块来读取地面磁条上各磁钉间的中线的距离，然后采用控制模块根据中线的距离校正自动导引小车的行走路线，以减小惯性导航自动导引小车的导航偏差值，进而提高对惯性导航自动导引小车的控制精度。

Description

一种惯性导航自动导引小车及其航迹确定方法
技术领域
本发明涉及自动导引装置领域，特别是涉及一种惯性导航自动导引小车及其航迹确定方法。
背景技术
自动导引运输车(Automated Guided Vehicle，AGV)是指装备有电磁、光学或陀螺仪等自动导引装置，能够沿着规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。与物料输送中常用的其他设备相比，AGV的活动区域无需铺设轨道、支座架等固定装置，不受场地、道路和空间的限制。因此，在自动化物流系统中，最能充分地体现其自动性和柔性，实现高效、经济、灵活的无人化生产。目前工厂主要的导航方式还是磁导航，近年来比较流行的导航方式还有雷达导航、惯性导航、视觉导航。新兴的导航方式有很大的潜力，由于技术还不是那么的成熟，还有好多不足的地方值得深入研究。
其中，肖献强等人提出的基于二维码和惯性传感器的复合导航方法，主要研究二维码与惯性传感器相结合的组合导航方式，在AGV工作区域内等间距设置有n个二维码标签，当AGV在二维码站点上时采用二维码导航模式，获取二维码图像，并利用图像识别技术估算纵向、侧向和横向三个偏差信息，以进行姿态调整。具体为，将通过二维码获取的偏差信息作为AGV在站点间行驶惯性导航的初始偏差，当AGV在两二维码站点间行驶时采用惯性导航模式，实时推算AGV在行走过程中的偏差，实现AGV在相邻二维码间的运动。
但是该种方法存在以下缺陷：二维码导航与惯性导航组合导航的方式需要在地面铺设二维码阵列，地面铺设的二维码极易被污染，从而导致站点信息无法识别，造成系统运行故障。
林志赟提出的基于T型RFID信标的AGV惯性导航系统，主要采用视觉相机、RFID读卡器和陀螺仪。具体导航过程为，地面铺设T型的RFID信标网格，AGV在经过T型RFID信标上方时，RFID读卡器读取RFID中包含的坐标编码信息，视觉相机通过T型标识计算导航偏差以及姿态角偏差，以有效去除陀螺仪固定漂移和随机漂移，提高导航的精度。
但是该种方法存在的缺陷有：T型RFID信标与惯性导航组合导航的方式需要在地面铺设T型RFID信标，地面铺设的T型RFID信标极易被损坏，尤其在工厂的环境中，导致坐标信息无法识别，容易造成系统运行故障，降低控制精度。
包壁祯提出的一种AGV惯性导航方法，包括S1：导航系统的搭建；S2：数据采集；S3：采集陀螺仪数据；S4：固定漂移处理；S5：改进递归状态滤波处理；S6：角度获取；S7：磁钉校准；S8：航迹推算；S9：PID调节；S10：运动控制。该方法通过查询方式获取陀螺仪采集的数据，同时能有效滤除陀螺仪采集的数据的固定漂移和随机误差，以获得陀螺仪准确的角度数据，进而准确推算出所有采样时刻的位置和姿态。
但是该种方法没有读卡器，存在检测不到磁钉，无法校准的问题。且所用的滤波和控制方法也会导致出现导航定位精度低的问题。
因此，基于现有技术中存在的问题，开发一种具有导航偏差小、精度高的AGV是本领域亟待解决的一个技术难题。
发明内容
本发明的目的是提供一种惯性导航自动导引小车及其航迹确定方法，以解决现有技术的自动导引运输车存在的导航偏差大、精度低的问题。
为实现上述目的，本发明提供了如下方案：
一种惯性导航自动导引小车，包括：
磁导航模块，用于读取地面磁条上各磁钉间的中线的距离；
惯性导航模块，用于获取自动导引小车的行走路线；
人机交互模块，用于提供与所述自动导引小车作动相对应的操控界面，并用于输入所述自动导引小车的作动指令；
控制模块，分别与所述磁导航模块、所述惯性导航模块和所述人机交互模块连接，用于根据所述中线的距离校正所述自动导引小车的行走路线，并根据所述操作界面输入的作动指令生成控制指令；所述控制指令包括：启停控制指令、前进/后退控制指令、搬运货物控制指令、转弯控制指令和车速控制指令。
优选的，所述惯性导航自动导引小车还包括：
站点识别模块，与所述控制模块连接，用于获取所述行走路线上的站点信息；所述控制模块根据所述站点信息生成所述自动导引小车启停或搬运货物的控制指令。
优选的，所述人机交互模块包括：
触屏单元，与所述控制模块连接，用于调节所述自动导引小车的车速和/或输入所述作动指令；所述作动指令包括：启动/停止指令、前进/后退指令、加/减速指令和搬/卸货物指令。
优选的，所述触屏单元包括：
按键子单元，用于键入所述自动导引小车的作动指令；
所述按键子单元包括：启动/停止按键、前进/后退按键、加/减速按键和搬/卸货物按键。
优选的，所述惯性导航自动导引小车还包括：
避障模块，与所述控制模块连接，用于检测所述自动导引小车前方是否存在障碍物；所述控制模块根据所述避障模块的检测结果判断是否发出停止控制指令。
优选的，所述惯性导航自动导引小车还包括：
无线通讯模块，用于将所述惯性导航自动导引小车的信息进行无线发送；所述惯性导航自动导引小车的信息包括：位置和速度。
优选的，所述惯性导航模块包括：
陀螺仪，用于检测所述自动导引小车的姿态角；
加速度计，用于检测所述自动导引小车的加速度；
磁力计，用于检测所述地面磁条上磁钉的位置。
优选的，所述惯性导航自动导引小车还包括：
驱动模块，与所述控制模块连接，用于根据所述控制指令驱动所述自动导引小车。
一种惯性导航自动导引小车的航迹确定方法，应用于上述惯性导航自动导引小车；所述方法包括：
获取地面磁带上各磁钉间的中线的距离；
根据所述中线的距离确定所述自动导引小车的偏差角度；
获取预设偏差角度值和预设偏差范围；
根据所述偏差角度值和所述预设偏差角度值确定所述自动导引小车的偏差范围；
判断所述偏差范围是否包含在所述预设偏差范围内；
若所述偏差范围包含在所述预设偏差范围内，则采用所述偏差角度对所述自动导引小车的姿态角进行校准；
获取所述自动导引小车的行驶里程；
根据所述行驶里程和校准后的姿态角确定所述自动导引小车的位置和所述自动导引小车的姿态；
根据所述自动导引小车的位置和所述自动导引小车的姿态确定所述自动导引小车的航迹；所述航迹即为所述自动导引小车的实际行驶路线；
若所述偏差范围不包含在所述预设偏差范围内，则视为所述自动导引小车脱轨。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明提供的惯性导航自动导引小车通过采用磁导航模块来读取地面磁条上各磁钉间的中线的距离，然后采用控制模块根据中线的距离校正自动导引小车的行走路线，以减小惯性导航自动导引小车的导航偏差值，进而提高对惯性导航自动导引小车的控制精度。
此外，在本发明公开的惯性导航自动导引小车中还对应设置有用于获取所述行走路线上的站点信息的站点识别模块，以便控制模块根据站点信息生成自动导引小车启停或搬运货物的控制指令，以能够达到减小导航偏差值、提高对惯性导航自动导引小车的控制精度的目的。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明提供的惯性导航自动导引小车的结构示意图；
图2为本发明提供的惯性导航自动导引小车的整体控制原理图；
图3为本发明开发惯性导航自动导引小车系统的流程图；
图4为本发明实施例中改进状态滤波的原理流程图；
图5为本发明实施例中磁钉布置示意图；
图6为本发明实施例中磁钉校准原理图；
图7为本发明惯性导航自动导引小车的航迹确定方法的流程图；
图8为本发明实施例中航迹确定过程的原理图；
图9为本发明实施例中惯性导航自动导引小车的运动控制原理图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种惯性导航自动导引小车及其航迹确定方法，以解决现有技术的自动导引运输车存在的导航偏差大、精度低的问题。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
图1为本发明提供的惯性导航自动导引小车的结构示意图，如图1所示，本发明提供的惯性导航自动导引小车，包括：磁导航模块1、惯性导航模块2、人机交互模块3和控制模块4。
磁导航模块1用于读取地面磁条上各磁钉间的中线的距离。
每隔5米放置一对磁钉，磁钉间的距离为1米，以构成本发明中的地面磁条。在自动导引小车行驶的过程中依次读取到地面磁条上两个磁钉间中线的距离，已知到中线的距离和磁钉间的距离可以求出偏航的角度，由于长时间的误差积累造成角度漂移，把通过磁钉求出的偏航角补偿到航向上，实现磁钉的二次精确定位。
惯性导航模块2用于获取自动导引小车的行走路线。
具体的该模块为AGV的导航提供测量航向，在控制模块4中按照设定的路线行驶，以通过控制预定航向与测量航向的偏差来控制AGV前进。
上述惯性导航模块2可以进一步包括：用于检测自动导引小车的姿态角的陀螺仪，用于检测自动导引小车加速度的加速度计，用于检测地面磁条上磁钉位置的磁力计。
人机交互模块3用于提供与自动导引小车作动相对应的操控界面，并用于输入自动导引小车的作动指令。
人机交互模块3包括：触屏单元。
触屏单元与控制模块4连接，该触屏单元用于调节自动导引小车的车速和/或输入作动指令。作动指令包括：启动/停止指令、前进/后退指令、加/减速指令和搬/卸货物指令。
在本发明中，车速等级为：低速、中速和高速。且每个车速等级间还可以依据设定速度值进行调节。
上述触屏单元还可以包括有：用于键入自动导引小车的作动指令的按键子单元。
按键子单元包括：启动/停止按键、前进/后退按键、加/减速按键和搬/卸货物按键。
控制模块4，分别与磁导航模块1、惯性导航模块2和人机交互模块3连接，用于根据中线的距离校正自动导引小车的行走路线，并根据操作界面输入的作动指令生成控制指令。控制指令包括：启停控制指令、前进/后退控制指令、搬运货物控制指令、转弯控制指令和车速控制指令。
为了避免造成系统运行故障，提高控制精度，作为本发明的另一实施例，上述惯性导航自动导引小车还可以包括：用于获取行走路线上的站点信息的站点识别模块5。该站点识别模块5与控制模块4连接。在本发明中，站点识别模块5优选为RFID读卡器模块(射频识别，Radio Frequency Identification，RFID)。
其中，站点识别模块5提供站点功能，以通过读取地上和/或立体面上的卡片来了解AGV到达的站点，之后控制模块4根据站点信息进行启停、搬运货物等操作。
进一步为了提高运行安全，作为本发明的另一实施例，上述惯性导航自动导引小车还包括：避障模块6。
避障模块6与控制模块4连接，以用于检测自动导引小车前方是否存在障碍物。控制模块4根据避障模块6的检测结果判断是否发出停止控制指令。
其中，避障模块6给AGV小车提供安全防护。如果AGV小车的前方出现障碍物能及时的避障停车，等到障碍物移走，AGV小车可以继续行驶。
作为本发明的另一实施例，上述惯性导航自动导引小车还包括：用于将惯性导航自动导引小车的信息进行无线发送的无线通讯模块8。上位机在接收到无线通讯模块8发送的信息后，可以根据实际需要人为设定自动导引小车的速度和行驶路线。
其中，惯性导航自动导引小车的信息包括：位置和速度。
为了实现对自动导引小车的精确控制，作为本发明的另一实施例，上述惯性导航自动导引小车还包括：用于根据控制指令驱动自动导引小车的驱动模块7。该驱动模块7与控制模块4连接。
其中，驱动电机模块由驱动器、直流无刷电机、减速机以及编码器组成，是AGV行驶的驱动系统。
本发明提供的上述惯性导航自动导引小车，是以陀螺仪为基础，通过结合陀螺仪定向、编码器反馈以及磁传感器和磁钉二次精确定位的的AGV。
为了校准惯性导航模块和编码器的积累误差，并标记AGV的移动路径，在路径上每隔一段距离设置一个磁钉。在两个磁钉之间根据惯性导航模块提供的航向控制AGV直行，在磁钉位置，AGV小车根据磁钉信息校准惯性导航模块误差及AGV位置误差，并调整AGV航向。站点识别模块实现读取站点的功能，进而通过控制模块完成对AGV的控制功能，例如启停、上下货物、转弯等功能。
其中，如图2所示，控制模块4与其他各模块通过485通讯获取信息，包括获取航向信息、磁钉信息、编码器的里程信息、站点信息等。陀螺仪为AGV的导航提供测量航向，在控制模块4里已设定好AGV路线，通过控制预定航向与测量航向的偏差来控制AGV前进，通过PID控制给驱动模块7不同的脉冲信号，实现驱动直流电机的差速，完成AGV继续沿预定路线行驶。编码器给AGV提供里程信息，可以判断小车的位置。
如图3所示，上述惯性导航自动导引小车的开发流程具体包括：
1)、基于惯性导航模块和磁钉校正的AGV组合导航系统的搭建：
利用铝方管和铁皮搭建建议的车架，安装轮子和电机，电机上已经安装好了编码器，将磁传感器(磁导航模块1)安转在AGV前方中轴线上，RFID读卡器(站点识别模块5)安装在车体的中心，将微控制器(控制模块4)和电机驱动(驱动模块7)安装在车体内，连接各部分的电路。其中，微控制器(控制模块4)在本实施例中的型号优选为STM32。
2)、STM32(控制模块4)采集惯性传感器(惯性导航模块2)的数据，以获取陀螺仪、加速度计、磁力计的原始数据，并将原始数据去零偏。而陀螺仪的测量值为ωx,ωy,ωz；加速度计的测量值为
Figure BDA0002476243250000081
磁力计的测量值为
Figure BDA0002476243250000082
3)、基于四元数的改进递归状态滤波：
设相对于地面参考坐标系，运载体的旋转角速度为ω，这个角速度的获得是利用固定在运载体坐标系上的陀螺仪。
可以将ω用四元数的形式表示出来：ω＝0+ωxi+ωyj+ωzk
其中，ωx,ωy,ωz表示各坐标轴上的角速度分量。
再定义一个四元数：q＝q0+q1i+q2j+q3k且满足：
Figure BDA0002476243250000091
四元数的微分方程可以表示为：
Figure BDA0002476243250000092
按照四元数乘法，将微分方程展开：
Figure BDA0002476243250000093
上式可简写为：
Figure BDA0002476243250000094
式中Ωb为：
Figure BDA0002476243250000095
微分方程的求解是使用一阶龙格-库塔法(Runge-Kutta)法，可得其离散模型为：
Figure BDA0002476243250000096
其中，T表示系统采样时间间隔；q(k)表示k时刻四元数；q(k-1)表示k-1时刻四元数。
设系统状态量为：
X(k)＝[q0(k) q1(k) q2(k) q3(k) bωx(k) bωy(k) bωz(k)]T(5)
其中：q0(k),q1(k),q2(k),q3(k)为姿态四元数，bωx(k)为绕滚转轴陀螺仪随机漂移向量，bωy(k)为俯仰轴陀螺仪随机漂移向量，bωz(k)为航向轴的陀螺仪随机漂移向量。
系统的状态方程为：
Figure BDA0002476243250000101
式中，T表示系统采样时间间隔，q(k)表示k时刻四元数，q(k-1)表示k-1时刻四元数。
函数f(X(k-1),k-1)为非线性函数，通过泰勒展开将其线性化，可得：
X(k)＝Φ(k,k-1)X(k-1)+φ(k-1)+W(k-1) (7)
上式中：
Figure BDA0002476243250000102
其中，
Figure BDA0002476243250000103
Figure BDA0002476243250000104
表示陀螺仪的估计值，ωx,ωy,ωz表示陀螺仪的估计值。
Figure BDA0002476243250000105
W(k)是过程噪声，是均值为0、方差为Q(k-1)的高斯白噪声，Q(k-1)是常值矩阵，可以根据具体寻优来确定。
取加速度计的测量值和磁强计的测量值，建立观测方程，观测量为：
Figure BDA0002476243250000111
其中，
Figure BDA0002476243250000112
表示机体坐标系中重力加速度方向分量；
Figure BDA0002476243250000113
表示机体坐标系中磁场强度方向分量。
系统的观测方程为：
Figure BDA0002476243250000114
函数h(X(k),k)为非线性函数，通过泰勒展开将其线性化，
可得：Z(k)＝H(x(k))X(k)+y(k-1)+V(k-1) (10)
上式中：
Figure BDA0002476243250000115
Figure BDA0002476243250000116
其中，V(k)是过程噪声，是均值为0、方差为R(k)高斯白噪声，R(k)是常值矩阵，可以根据具体寻优来确定。
获得了观测方程(10)之后，如何在过程扰动和传感器噪声存在的背景下，利用加速度计的测量值和磁强计的测量值(两者构成了观测向量Z(k))准确地将状态变量X(k)估计出来，成为算法的核心任务。
这里改进递归状态滤波算法用来解决非线性系统中状态估计问题，卡尔曼滤波算法主要是用于线性系统的状态估计问题。因此，改进递归状态滤波是卡尔曼滤波算法的基础上发展而来。状态方程和观测方程建立后，就可以使用观测向量Z(k)“反向”估计出状态变量X(k)。
改进递归状态滤波的递推过程如下：
A、初始值。
给定初始状态X(0)的值以及相关的噪声协方差的初值P(0)。
B、时间更新。
由状态转移函数可得状态的一步预测值：
X(k,k-1)＝f(X(k-1),k-1) (11)
计算一步预测误差协方差：
P(k.k-1)＝Φ(k,k-1)P(k-1)ΦT(k,k-1)+Q(k-1) (12)
C、量测更新。
计算扩展卡尔曼增益：
Kg(k)＝P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+R(k)]-1 (13)
由观测变量更新状态估计值：
X(k)＝X(k,k-1)+Kg(k)[Z(k)-h[X(k,k-1),k]] (14)
更新状态误差协方差：
P(k)＝[I-Kg(k)H(k)]P(k,k-1) (15)
D、令k＝k+1，重复步骤A-C，重新递归计算，就可以得到状态X(k)的最优估计值。该最优估计值即为经过递推得到的最优姿态角。
其中，改进递归状态滤波的原理流程图如图4所示。
4)、体轴姿态角、磁钉位置数据和编码器数据采集，STM32采集经过基于四元数的改进递归状态滤波得到的姿态角，磁传感器采集分布在导航路线上的磁钉，获取磁钉的位置，编码器根据驱动电机的脉冲数可以计算里程。
编码器的计算公式：
编码器发一个脉冲对应的长度＝圆周率*直径/编码器线数
编码器发一个脉冲对应的长度＝轮周长/编码器线数＝脉冲当量
速度＝一秒的脉冲数*编码器发一个脉冲对应的长度
里程＝每秒的速度的累加求和。
5)、磁钉二次校准，控制器在磁钉和磁钉之间使用前面滤波得到的姿态角(ωz积分以后得到姿态角φ)，在磁钉处采用磁钉所测量的姿态角偏差，修正下一时刻测量的姿态角。磁传感器根据采集的磁钉的位置得到AGV当前运行方向和预定轨迹方向的偏差角度，校准回预定轨道；如果偏差角度超过设定的范围，则视为AGV脱轨运行。
磁钉的校准原理是：在运行轨迹直线处放置一对磁钉，两磁钉间的距离为D。如图5所示，A1、A2是所布置的磁钉。磁传感器分别检测A1、A2磁钉的位置，所用的磁传感器16个检测点，如图6所示，通过检测一对磁钉相对于磁传感器中心的距离，从而计算出AGV当前运行方向和预定轨迹方向的偏差角度，角度在设定的范围内则将进行校准。一对磁钉的间距越大，测量角度的分辨率越小，磁钉的间距不能太大，否则会出现不能同时读到两个磁钉的现象，如果没有读到第二颗磁钉，则认为校准角度很大，即AGV脱轨。
其中，偏差角度为：φ＝arcsin(|dA1-dA2|/D) (16)
6)、航迹推算，根据编码器求出的里程数据、校准后的角度数据(姿态角或航向角)形成极坐标关系，推算出所有采样时刻的位置和姿态，形成航迹。
如图7所示，航迹的推算根据编码器的里程数据、校准后的陀螺仪数据形成极坐标关系，假设k时刻AGV的位置和姿态为(s1(k),s2(k),φ(k))，φ是航向与自己设定的X轴方向的夹角，则k+1时刻的姿态为：
Figure BDA0002476243250000141
其中，Δd(k)为编码器获得的增量里程，Δφ(k)采用陀螺仪测量得增量角度，这样通过迭代推算出所有采样时刻的位置和姿态，从而形成航迹。
7)、控制器调节，将当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出。
控制调节中如果在k时刻给定的坐标为(s1m(k),s2m(k))，而实际的位置和姿态为(s1(k),s2(k))，此时输出偏差采用极坐标方式表达
Figure BDA0002476243250000142
无偏差输入则指AGV实际轨迹和给定的轨迹重合，也即Δφ＝0；反之，只要Δφ不为零，则存在偏差，纠偏算法采用控制算法，如果调整后AGV的角度一致，但坐标不一致，这样AGV运行一段时间必然和给定的坐标重合；但是如果AGV在当前周期调整还未达到上一时刻的给定值时，AGV的偏差输入以当前时刻的给定值为准。
8)、运动控制，以偏差角度转为运动输入，利用PID控制算法来调整AGV的姿态，控制算法也可以是模糊控制、模糊PID控制、自适应控制、鲁棒控制等。图8为AGV控制原理图。运动控制需根据AGV运动模型控制，控制方式有两种：一、AGV为差动模型则根据角度偏差控制左右电机差速；二、AGV为舵轮，则直接控制舵轮角度。两种方式都可以选择，这取决于AGV底盘机械结构的具体设计。
基于上述惯性导航自动导引小车的开发流程中可进一步得到本发明提供的一种惯性导航自动导引小车的航迹确定方法，如图9所示，该惯性导航自动导引小车的航迹确定方法，包括：
步骤100：获取地面磁带上各磁钉间的中线的距离。
步骤101：根据所述中线的距离确定所述自动导引小车的偏差角度。
步骤102：获取预设偏差角度值和预设偏差范围。
步骤103：根据所述偏差角度值和所述预设偏差角度值确定所述自动导引小车的偏差范围。
步骤104：判断所述偏差范围是否包含在所述预设偏差范围内。
步骤105：若所述偏差范围包含在所述预设偏差范围内，则采用所述偏差角度对所述自动导引小车的姿态角进行校准。
步骤106：获取所述自动导引小车的行驶里程。
步骤107：根据所述行驶里程和校准后的姿态角确定所述自动导引小车的位置和所述自动导引小车的姿态。
步骤108：根据所述自动导引小车的位置和所述自动导引小车的姿态确定所述自动导引小车的航迹；所述航迹即为所述自动导引小车的实际行驶路线。
步骤109：若所述偏差范围不包含在所述预设偏差范围内，则视为所述自动导引小车脱轨。
本发明基于惯性导航和磁钉校正的AGV，利用惯性导航的相关算法，设计一款低成本的惯性导航自动导引小车，在算法实现的基础上，可以用精度更高的陀螺仪，提高导航的精度，使其具有更加好的性能，并且惯性导航不依赖于外部信号，可以实时、高精度地输出姿态和位置信息，能够自主地进行定位、导航，不受地域的限制。
而且车体设计六轮差速驱动的小车，两个驱动轮四个从动轮。控制模块以STM32为核心，通过控制直流无刷电机和处理陀螺仪信号，实现惯性导航自动导引小车的驱动转向。对惯性导航自动导引小车的结构分析，建立了数学运动模型，给出了航位计算公式，形成运行路线；通过直线行走纠偏算法控制惯性导航自动导引小车的运动，通过陀螺仪信号反馈的角速率计算运动方向角，得到惯性导航自动导引小车的行驶方向角，从而纠正小车的行驶方向。针对陀螺仪和编码器组成的导航系统的积累误差，采取了二次精确定位方式，即在陀螺仪定向、编码器反馈的一次定位基础上，采用磁钉和磁传感器，对惯性导航自动导引小车进行二次定位。通过在行驶路径上每隔一段距离安置一对磁钉，由磁传感器检测并进行位姿校正，该方法安装磁钉快捷方便，可以克服现有AGV的误差积累，提高控制精度。
最主要优点是定位精度高、灵活，并且容易和其他导航方式组合，惯导和磁钉的组合导航的方式有着良好的应用前景。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种惯性导航自动导引小车，其特征在于，包括：
磁导航模块，用于读取地面磁条上各磁钉间的中线的距离；
惯性导航模块，用于获取自动导引小车的行走路线；
人机交互模块，用于提供与所述自动导引小车作动相对应的操控界面，并用于输入所述自动导引小车的作动指令；
控制模块，分别与所述磁导航模块、所述惯性导航模块和所述人机交互模块连接，用于根据所述中线的距离校正所述自动导引小车的行走路线，并根据所述操作界面输入的作动指令生成控制指令；所述控制指令包括：启停控制指令、前进/后退控制指令、搬运货物控制指令、转弯控制指令和车速控制指令。
2.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述惯性导航自动导引小车还包括：
站点识别模块，与所述控制模块连接，用于获取所述行走路线上的站点信息；所述控制模块根据所述站点信息生成所述自动导引小车启停或搬运货物的控制指令。
3.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述人机交互模块包括：
触屏单元，与所述控制模块连接，用于调节所述自动导引小车的车速和/或输入所述作动指令；所述作动指令包括：启动/停止指令、前进/后退指令、加/减速指令和搬/卸货物指令。
4.根据权利要求3所述的惯性导航自动导引小车，其特征在于，所述触屏单元包括：
按键子单元，用于键入所述自动导引小车的作动指令；
所述按键子单元包括：启动/停止按键、前进/后退按键、加/减速按键和搬/卸货物按键。
5.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述惯性导航自动导引小车还包括：
避障模块，与所述控制模块连接，用于检测所述自动导引小车前方是否存在障碍物；所述控制模块根据所述避障模块的检测结果判断是否发出停止控制指令。
6.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述惯性导航自动导引小车还包括：
无线通讯模块，用于将所述惯性导航自动导引小车的信息进行无线发送；所述惯性导航自动导引小车的信息包括：位置和速度。
7.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述惯性导航模块包括：
陀螺仪，用于检测所述自动导引小车的姿态角；
加速度计，用于检测所述自动导引小车的加速度；
磁力计，用于检测所述地面磁条上磁钉的位置。
8.根据权利要求1所述的惯性导航自动导引小车，其特征在于，所述惯性导航自动导引小车还包括：
驱动模块，与所述控制模块连接，用于根据所述控制指令驱动所述自动导引小车。
9.一种惯性导航自动导引小车的航迹确定方法，其特征在于，应用于上述权利要求1-8任意一项所述的惯性导航自动导引小车；所述方法包括：
获取地面磁带上各磁钉间的中线的距离；
根据所述中线的距离确定所述自动导引小车的偏差角度；
获取预设偏差角度值和预设偏差范围；
根据所述偏差角度值和所述预设偏差角度值确定所述自动导引小车的偏差范围；
判断所述偏差范围是否包含在所述预设偏差范围内；
若所述偏差范围包含在所述预设偏差范围内，则采用所述偏差角度对所述自动导引小车的姿态角进行校准；
获取所述自动导引小车的行驶里程；
根据所述行驶里程和校准后的姿态角确定所述自动导引小车的位置和所述自动导引小车的姿态；
根据所述自动导引小车的位置和所述自动导引小车的姿态确定所述自动导引小车的航迹；所述航迹即为所述自动导引小车的实际行驶路线；
若所述偏差范围不包含在所述预设偏差范围内，则视为所述自动导引小车脱轨。
CN202010364424.1A 2020-04-30 2020-04-30 一种惯性导航自动导引小车及其航迹确定方法 Pending CN111474938A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010364424.1A CN111474938A (zh) 2020-04-30 2020-04-30 一种惯性导航自动导引小车及其航迹确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010364424.1A CN111474938A (zh) 2020-04-30 2020-04-30 一种惯性导航自动导引小车及其航迹确定方法

Publications (1)

Publication Number Publication Date
CN111474938A true CN111474938A (zh) 2020-07-31

Family

ID=71764251

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010364424.1A Pending CN111474938A (zh) 2020-04-30 2020-04-30 一种惯性导航自动导引小车及其航迹确定方法

Country Status (1)

Country Link
CN (1) CN111474938A (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111930126A (zh) * 2020-08-20 2020-11-13 北京特种机械研究所 一种基于差速轮组agv的导航纠偏方法
CN111976719A (zh) * 2020-08-03 2020-11-24 长沙理工大学 一种车辆入库系统及方法
CN113189994A (zh) * 2021-04-25 2021-07-30 上海德衡数据科技有限公司 基于工业互联网的自动导引方法、系统、设备及存储介质
CN113415266A (zh) * 2021-07-15 2021-09-21 上海电气集团智能交通科技有限公司 一种车辆磁钉循迹运行的轨迹偏离防护方法
CN114545944A (zh) * 2022-02-24 2022-05-27 合肥工业大学 一种基于磁钉磁场强度纠正的agv航向定位导航方法
CN114935342A (zh) * 2022-04-28 2022-08-23 共享智能装备有限公司 一种agv及其导航方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101713666A (zh) * 2009-11-20 2010-05-26 哈尔滨工程大学 一种基于单轴转停方案的系泊估漂方法
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN103135548A (zh) * 2012-05-02 2013-06-05 广州市远能物流自动化设备科技有限公司 一种无人搬运小车及其驱动控制系统
CN106525034A (zh) * 2016-10-26 2017-03-22 郑州轻工业学院 一种基于对偶四元数的惯导系统传递对准建模方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN107479548A (zh) * 2017-08-14 2017-12-15 安徽信息工程学院 一种agv小车控制系统
CN108132053A (zh) * 2017-11-24 2018-06-08 北京工商大学 一种行人轨迹构建方法、系统及惯性测量装置
CN110231031A (zh) * 2018-03-05 2019-09-13 高德信息技术有限公司 一种姿态角确定方法、装置及系统
CN110763224A (zh) * 2019-11-13 2020-02-07 内蒙古工业大学 一种自动导引运输车导航方法及导航系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101713666A (zh) * 2009-11-20 2010-05-26 哈尔滨工程大学 一种基于单轴转停方案的系泊估漂方法
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN103135548A (zh) * 2012-05-02 2013-06-05 广州市远能物流自动化设备科技有限公司 一种无人搬运小车及其驱动控制系统
CN106525034A (zh) * 2016-10-26 2017-03-22 郑州轻工业学院 一种基于对偶四元数的惯导系统传递对准建模方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN107479548A (zh) * 2017-08-14 2017-12-15 安徽信息工程学院 一种agv小车控制系统
CN108132053A (zh) * 2017-11-24 2018-06-08 北京工商大学 一种行人轨迹构建方法、系统及惯性测量装置
CN110231031A (zh) * 2018-03-05 2019-09-13 高德信息技术有限公司 一种姿态角确定方法、装置及系统
CN110763224A (zh) * 2019-11-13 2020-02-07 内蒙古工业大学 一种自动导引运输车导航方法及导航系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
高建东: "SG2300巷道式仓储中心智能搬运AGV小车研制" *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111976719A (zh) * 2020-08-03 2020-11-24 长沙理工大学 一种车辆入库系统及方法
CN111930126A (zh) * 2020-08-20 2020-11-13 北京特种机械研究所 一种基于差速轮组agv的导航纠偏方法
CN111930126B (zh) * 2020-08-20 2021-06-01 北京特种机械研究所 一种基于差速轮组agv的导航纠偏方法
CN113189994A (zh) * 2021-04-25 2021-07-30 上海德衡数据科技有限公司 基于工业互联网的自动导引方法、系统、设备及存储介质
CN113189994B (zh) * 2021-04-25 2023-03-03 上海德衡数据科技有限公司 基于工业互联网的自动导引方法、系统、设备及存储介质
CN113415266A (zh) * 2021-07-15 2021-09-21 上海电气集团智能交通科技有限公司 一种车辆磁钉循迹运行的轨迹偏离防护方法
CN113415266B (zh) * 2021-07-15 2022-10-18 上海电气集团智能交通科技有限公司 一种车辆磁钉循迹运行的轨迹偏离防护方法
CN114545944A (zh) * 2022-02-24 2022-05-27 合肥工业大学 一种基于磁钉磁场强度纠正的agv航向定位导航方法
CN114545944B (zh) * 2022-02-24 2024-04-16 合肥工业大学 一种基于磁钉磁场强度纠正的agv航向定位导航方法
CN114935342A (zh) * 2022-04-28 2022-08-23 共享智能装备有限公司 一种agv及其导航方法

Similar Documents

Publication Publication Date Title
CN111474938A (zh) 一种惯性导航自动导引小车及其航迹确定方法
CN108592906B (zh) 基于二维码和惯性传感器的agv复合导航方法
CN109521768B (zh) 一种基于双pid控制的agv小车的路径纠偏方法
KR102502219B1 (ko) 로봇 운동시 카펫으로 인한 드리프트 제어방법, 칩 및 청소 로봇
CN106843214B (zh) 一种基于自抗扰控制的磁带导引agv循迹控制方法
CN109933056B (zh) 一种基于slam的机器人导航方法以及机器人
CN110763224A (zh) 一种自动导引运输车导航方法及导航系统
EP2450763A1 (en) Global position and orientation estimation system for a vehicle in a passageway environment
CN107063305B (zh) 用惯导、压力传感器修正下坡悬空后轮里程计误差的方法
CN109813305B (zh) 基于激光slam的无人叉车
CN106338991A (zh) 一种基于惯性导航和二维码的机器人及定位导航方法
CN112183133A (zh) 一种基于ArUco码引导的移动机器人自主充电方法
CN205950750U (zh) 基于惯性导航的变电站巡检机器人导航控制系统
CN105737853B (zh) 一种机器人惯性导航系统的漂移校准方法
CN108036792A (zh) 一种用于移动机器人的里程计与测量位姿的数据融合方法
JP5561730B2 (ja) 移動体の誘導制御システム及び誘導制御方法
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
Song et al. A new method of AGV navigation based on Kalman Filter and a magnetic nail localization
CN112433531A (zh) 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
Xu et al. Magnetic locating AGV navigation based on Kalman filter and PID control
Maeyama et al. Rule based filtering and fusion of odometry and gyroscope for a fail safe dead reckoning system of a mobile robot
CN112697153A (zh) 自主移动设备的定位方法、电子设备及存储介质
Ye et al. A vision-based guidance method for autonomous guided vehicles
CN114993310B (zh) 一种智能打桩机自动定位的方法
Etzion et al. MoRPI: Mobile Robot Pure Inertial Navigation

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20200731

RJ01 Rejection of invention patent application after publication