CN110763224A - 一种自动导引运输车导航方法及导航系统 - Google Patents

Abstract

本发明提供一种自动导引运输车导航方法及导航系统，该导航包括：采集导航的原始数据，并将原始数据去零偏，获取自动导引运输车的当前运行方向；通过预定装备的位置得到自动导引运输车的当前运行方向与预定轨迹方向的偏差角度，校准导航的角度；根据里程数据和校准后的导航的角度，形成导航航迹；基于导航航迹以及控制算法对自动导引运输车进行运动控制。该方法采用惯性导航为主导、磁钉校正的方法提高了精度，而且一些复杂的控制算法，如：模糊控制、模糊PID、自适应控制、鲁棒控制等也能在AGV上实现,提高其循迹精度，还可以比较各种控制算法的控制精度，极大提升了工厂生产的智能化。

Description

一种自动导引运输车导航方法及导航系统

技术领域

本发明涉自动导引运输车技术领域，尤其涉及一种自动导引运输车导航方法及导航系统。

背景技术

AGV为Automated Guided Vehicle的缩写，意即自动导引运输车，是指装备有电磁、光学或陀螺仪等自动导引装置，能够沿着规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。与物料输送中常用的其他设备相比，AGV的活动区域无需铺设轨道、支座架等固定装置，不受场地、道路和空间的限制。因此，在自动化物流系统中，最能充分地体现其自动性和柔性，实现高效、经济、灵活的无人化生产。目前工厂主要的导航方式还是磁导航，近年来比较流行的导航方式还有雷达导航、惯性导航、视觉导航。新兴的导航方式有很大的潜力，由于技术还不是那么的成熟，还有好多不足的地方值得深入研究。

单一磁导航导航精度不高，磁条的维护比较频繁，容易损坏,单一的激光雷达易受天气、光线的影响，单一的惯性导航陀螺仪会积累误差，单一的视觉会面临参考坐标点的损坏,技术也不是很成熟，处于研究开发的阶段。

发明内容

本发明的一个目的是提供一种自动导引运输车导航方法及导航系统，以解决现有的自动导引运输车单一磁导航导航精度不高，磁条的维护比较频繁，单一的惯性导航陀螺仪会积累误差的问题。

根据本发明的第一方面，提供一种自动导引运输车导航方法，其特征在于，包括：

采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向；

通过预定装备的位置得到所述自动导引运输车的当前运行方向与预定轨迹方向的偏差角度，校准导航的角度；

根据里程数据和校准后的导航的角度，形成导航航迹；

基于导航航迹以及控制算法对所述自动导引运输车进行运动控制。

可选的，所述原始数据包括自动导引运输车上陀螺仪的原始数据、加速度计的原始数据和磁力计的原始数据；

相应的，所述采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向，包括：

将所述陀螺仪的去零偏的数据四元数解算出姿态角作为估计量，将所述加速度计和磁力计测量的原始数据中的角度作为观测量进行卡尔曼滤波得到所述自动导引运输车的姿态角。

可选的，所述根据里程数据和校准后的导航的角度，形成导航航迹，包括：

k时刻自动导引运输车的位置和姿态为(x(k),y(k),φ(k))，φ是航向与自己设定的X轴方向的夹角，则K+1时刻的姿态为：

其中Δd(k)为编码器获得的增量里程；Δφ(k)采用陀螺仪测量得增量角度，这样通过迭代推算出所有采样时刻的位置和姿态，形成导航航迹。

可选的，所述基于导航航迹以及控制算法对所述自动导引运输车进行运动控制，包括：

将所述自动导引运输车当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出；

以偏差角度转为运动输入，利用控制算法来调整自动导引运输车的姿态。

可选的，所述控制算法包括：模糊控制算法、模糊PID算法、自适应控制算法和鲁棒控制算法。

可选的，所述将所述陀螺仪的去零偏的数据四元数解算出姿态角作为估计量，将所述加速度计和磁力计测量的原始数据中的角度作为观测量进行卡尔曼滤波得到所述自动导引运输车的姿态角，包括：

其中：x_k为状态向量；y_k为观测向量；A为从k一1时刻到k时刻的状态转移矩阵；u_k为系统输入控制向量；B为输入控制向量的增益矩阵；H为状态量到观测量的增益矩阵；w_k为输入噪声；v_k为观测噪声。输入噪声和观测噪声都是服从正态分布，输入噪声的协方差为Q，观测噪声的协方差为R，则卡尔曼滤波递推公式如下:

状态量的预估计:

误差协方差预估计:

P_k|k-1＝AP_k-1A^T+Q (4)

卡尔曼增益更新：

K_k＝P_k|k-1H^T[HP_k|k-1H^T+R]^-1 (5)

现状态估计值更新:

误差协方差更新：

P_k＝(I-K_kH)P_k|k-1 (7)

其中：

为对现状态真实值X_k的估计；

为基于k-1次对k次的预估计；A为从k-1次到k次的状态转移矩阵；B为输人控制向量的增益矩阵；H为状态量到观测量的增益矩阵；y_k为k次的观测量；K_k为对观测偏差

的修正加权，即卡尔曼增益矩阵是直接影响状态估计准确性的关键因素；P_k为现状态估计值的误差协方差矩阵；P_k|k-1为预估计的误差协方差矩阵；I为单位矩阵。

可选的，在根据里程数据和校准后的导航的角度，形成导航航迹之前，所述方法还包括：

采集经过四元数解算和卡尔曼滤波得到的姿态角，获取磁传感器采集分布在导航路线上的预定装备，获取预定装备的位置，以使编码器根据驱动电机的脉冲数计算里程，获取里程数据。

可选的，所述将所述自动导引运输车当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出，包括：

如果在k时刻给定的坐标为(x_m(k),y_m(k))，而实际的位置和姿态为(x(k),y(k))，此时输出偏差采用极坐标方式为：

Δφ＝atan(y(k)-y_m(k))(x(k)-x_m(k)) (8)

根据本发明的第二方面，提供一种用于执行上述所述的自动导引运输车导航方法的导航系统，包括：

磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块、微控制器模块和运动控制模块；

所述磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块和运动控制模块均与所述微控制器模块相连。

本发明提供的一种自动导引运输车导航方法及导航系统，该方法采用惯性导航为主导、磁钉校正的方法提高了精度，而且一些复杂的控制算法，如：模糊控制、模糊PID、自适应控制、鲁棒控制等也能在AGV上实现,提高其循迹精度，还可以比较各种控制算法的控制精度，极大提升了工厂生产的智能化。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明一实施例提供的一种自动导引运输车导航方法的流程示意图；

图2为本发明一实施例提供的自动导引运输车组合导航系统开发的流程示意图；

图3为本发明一实施例提供的一种磁钉布置示意图；

图4为本发明一实施例提供的磁钉校准原理图；

图5为本发明一实施例提供的航迹推算原理图；

图6为本发明一实施例提供的运动控制原理图；

图7为本发明一实施例提供的自动导引运输车总体控制原理图；

图8为本发明一实施例提供的自动导引运输车系统结构图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

图1为本发明一实施例提供的一种自动导引运输车导航方法的流程示意图，如图1所示，该方法包括：

101、采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向；

102、通过预定装备的位置得到所述自动导引运输车的当前运行方向与预定轨迹方向的偏差角度，校准导航的角度；

本实施例中预定装备为可以标记的设备，本实施例以磁钉为例进行说明。

103、根据里程数据和校准后的导航的角度，形成导航航迹；

104、基于导航航迹以及控制算法对所述自动导引运输车进行运动控制。

上述实施例提供的方法该方法采用惯性导航为主导、磁钉校正的方法提高了精度，而且一些复杂的控制算法，如：模糊控制、模糊PID、自适应控制、鲁棒控制等也能在AGV上实现,提高其循迹精度，还可以比较各种控制算法的控制精度，极大提升了工厂生产的智能化。

在步骤101中所述原始数据包括自动导引运输车上陀螺仪的原始数据、加速度计的原始数据和磁力计的原始数据；

相应的，所述采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向，包括：

将所述陀螺仪的去零偏的数据四元数解算出姿态角作为估计量，将所述加速度计和磁力计测量的原始数据中的角度作为观测量进行卡尔曼滤波得到所述自动导引运输车的姿态角。

在步骤104中基于导航航迹以及控制算法对所述自动导引运输车进行运动控制，包括：

将所述自动导引运输车当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出；

以偏差角度转为运动输入，利用控制算法来调整自动导引运输车的姿态。

在步骤103根据里程数据和校准后的导航的角度，形成导航航迹之前，所述方法还包括：

采集经过四元数解算和卡尔曼滤波得到的姿态角，获取磁传感器采集分布在导航路线上的预定装备，获取预定装备的位置，以使编码器根据驱动电机的脉冲数计算里程，获取里程数据。

下面通过具体的步骤对上述实施例的方法进行详细说明。

详细步骤如下：

基于惯性导航和磁钉校正的AGV组合导航系统的搭建，利用铝方管和铁皮搭建建议的车架，安装轮子和电机，电机上已经安装好了编码器，将磁传感器安转在AGV前方中轴线上，RFID读卡器安装在车体的中心，将微控制器和电机的驱动安装在车体内，连接各部分的电路。

STM32采集惯性传感器数据，获取陀螺仪、加速度计、磁力计的原始数据，并将原始数据去零偏。

陀螺仪去零偏的数据四元数解算出姿态角作为估计量，将加速度计和磁力计测量的角度作为观测量进行卡尔曼滤波。

其中：x_k为状态向量；y_k为观测向量；A为从k一1时刻到k时刻的状态转移矩阵；u_k为系统输入控制向量；B为输入控制向量的增益矩阵；H为状态量到观测量的增益矩阵；w_k为输入噪声；v_k为观测噪声。输入噪声和观测噪声都是服从正态分布，输入噪声的协方差为Q，观测噪声的协方差为R，则卡尔曼滤波递推公式可概括为5个核心公式:

状态量的预估计:

误差协方差预估计:

P_k|k-1＝AP_k-1A^T+Q (3)

卡尔曼增益更新：

K_k＝P_k|k-1H^T[HP_k|k-1H^T+R]^-1 (4)

现状态估计值更新:

误差协方差更新：

P_k＝(I-K_kH)P_k|k-1 (6)

其中：

为对现状态真实值X_k的估计；为基于k-1次对k次的预估计；A为从k-1次到k次的状态转移矩阵；B为输人控制向量的增益矩阵；H为状态量到观测量的增益矩阵；y_k为k次的观测量；K_k为对观测偏差

的修正加权，即卡尔曼增益矩阵是直接影响状态估计准确性的关键因素；P_k为现状态估计值的误差协方差矩阵；P_k|k-1为预估计的误差协方差矩阵；I为单位矩阵。

(4)导航数据采集，STM32采集经过四元数解算和卡尔曼滤波得到的姿态角，磁传感器采集分布在导航路线上的磁钉，获取磁钉的位置，编码器根据驱动电机的脉冲数可以计算里程。

编码器的计算公式：编码器发一个脉冲对应的长度＝圆周率*直径/编码器线数

编码器发一个脉冲对应的长度＝轮周长/编码器线数＝脉冲当量

(5)磁钉二次校准，磁传感器根据采集的磁钉的位置得到AGV当前运行方向和预定轨迹方向的偏差角度，校准回预定轨道；如果偏差角度超过设定的范围，则视为AGV脱轨运行。

磁钉的校准原理是：在运行轨迹直线处放置一对磁钉，两磁钉间的距离为D。如图3所示，A1、A2是所布置的磁钉。磁传感器分别检测A1、A2磁钉的位置，所用的磁传感器16个检测点，如图4所示，通过检测一对磁钉相对于磁传感器中心的距离，从而计算出AGV当前运行方向和预定轨迹方向的偏差角度，角度在设定的范围内则将进行校准。一对磁钉的间距越大，测量角度的分辨率越小，磁钉的间距不能太大，否则会出现不能同时读到两个磁钉的现象，如果没有读到第二颗磁钉，则认为校准角度很大，即AGV脱轨。

偏差角度：

φ＝arcsin(|d_A1-d_A2|D) (7)

(6)航迹推算，根据编码器求出的里程数据、校准后的惯导的角度数据形成极坐标关系，推算出所有采样时刻的位置和姿态，形成航迹。

航迹推算如图5，航迹的推算根据编码器的里程数据、校准后的陀螺仪数据形成极坐标关系，假设k时刻AGV的位置和姿态为(x(k),y(k),φ(k))，φ是航向与自己设定的X轴方向的夹角，则K+1时刻的姿态为：

x(k+1)＝x(k)+ΔdcosΔφ(k)

y(k+1)＝y(k)+ΔdsinΔφ(k) (8)

φ(k+1)＝φ(k)+Δφ(k)

其中Δd(k)为编码器获得的增量里程；Δφ(k)采用陀螺仪测量得增量角度，这样通过迭代推算出所有采样时刻的位置和姿态，从而形成航迹。

(7)控制器调节，将当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出。

控制调节中如果在k时刻给定的坐标为(x_m(k),y_m(k))，而实际的位置和姿态为(x(k),y(k))，此时输出偏差采用极坐标方式表达

Δφ＝atan(y(k)-y_m(k))(x(k)-x_m(k)) (9)

无偏差输入则指AGV实际轨迹和给定的轨迹重合，也即Δφ＝0；反之，只要Δθ不为零，则存在偏差，纠偏算法采用控制算法，如果调整后AGV的角度一致，但坐标不一致，即x不一致，这样AGV运行一段时间必然和给定的坐标重合；但是如果AGV在当前周期调整还未达到上一时刻的给定值时，AGV的偏差输入以当前时刻的给定值为准。

(8)运动控制，以偏差角度转为运动输入，利用PID控制算法来调整AGV的姿态，控制算法也可以是模糊控制、模糊PID控制、自适应控制、鲁棒控制。图6就是AGV控制原理图。运动控制需根据AGV运动模型控制，控制方式有两种：一、AGV为差动模型则根据角度偏差控制左右电机差速，二、AGV为舵轮则直接控制舵轮角度。两种方式都可以选择，这取决于AGV底盘机械结构的设计。

基于惯性导航和磁钉校正的AGV，利用惯性导航的相关算法，设计一款低成本惯性元件AGV，在算法实现的基础上，可以用精度更高的陀螺仪，提高导航的精度，使其具有更加好的性能，并且惯性导航不依赖于外部信号，可以实时、高精度地输出姿态和位置信息，能够自主地进行定位、导航，不受地域的限制。而且车体设计六轮差速驱动的自动导引运输车，两个驱动轮四个从动轮。控制系统以STM32为核心，通过控制直流无刷电机和处理陀螺仪信号，实现AGV的驱动转向。对AGV的结构分析，建立了数学运动模型，给出了航位计算公式，形成运行路线；通过直线行走纠偏算法控制AGV的运动，通过陀螺仪信号反馈的角速率计算运动方向角，得到AGV的行驶方向角，从而纠正自动导引运输车的行驶方向。针对陀螺仪和编码器组成的导航系统的积累误差，采取了二次精确定位方式，即在陀螺仪定向、编码器反馈的一次定位基础上，采用磁钉和磁传感器，对AGV进行二次定位。通过在行驶路径上每隔一段距离安置一对磁钉，由磁传感器检测并进行位姿校正，该方法安装磁钉快捷方便，可以克服AGV误差积累。

最主要优点是定位精度高、灵活，并且容易和其他导航方式组合，惯导和磁钉的组合导航的方式有着良好的应用前景。由于AGV具有自动化集成度高、柔性好、可靠性高、自动导航运行、安装便利及使用方便等特点，AGV在欧美等发达国家己被频繁运用于烟草、汽车制造、仓储物流、机场，码头等物资运输频繁的场所,所以本文研究的惯性导航AGV具有一定工程应用价值，特别是一些工业现场更加容易实现。

一种以陀螺仪为基础，通过结合陀螺仪定向、编码器反馈以及磁传感器和磁钉二次精确定位的的自动导引AGV小车。为了校准惯导系统和编码器的积累误差，并标记机器人移动路径，在路径上每隔一段距离设置一个磁钉。在两个磁钉之间根据惯导系统提供的航向控制机器人直行，在磁钉位置，机器人根据磁钉信息校准惯导航向误差及机器人位置误差，并调整机器人航向。读卡器模块实现站点的功能，例如启停、上下货物、转弯等功能。图7控制原理图详细的介绍了底层的控制器通过读取磁传感器和惯导传感器的数据，得到偏差信号之后驱动电机来进行导航，还是就是一部分上位机的功能，在此不做详细说明。

图8示出了一种用于执行上述的自动导引运输车导航方法的导航系统的示意图，包括：

磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块、微控制器模块和运动控制模块；

所述磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块和运动控制模块均与所述微控制器模块相连。

本系统中的所有模块的实现方式如上述自动导引运输车导航方法中的模块一致，本实施例不再进行详细说明。

需要注意的是，本发明可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一些实施例中，本发明的软件程序可以通过处理器执行以实现上文步骤或功能。同样地，本发明的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本发明的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (9)

1.一种自动导引运输车导航方法，其特征在于，包括：

采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向；

通过预定装备的位置得到所述自动导引运输车的当前运行方向与预定轨迹方向的偏差角度，校准导航的角度；

根据里程数据和校准后的导航的角度，形成导航航迹；

基于导航航迹以及控制算法对所述自动导引运输车进行运动控制。

2.根据权利要求1所述的方法，其特征在于，所述原始数据包括自动导引运输车上陀螺仪的原始数据、加速度计的原始数据和磁力计的原始数据；

相应的，所述采集导航的原始数据，并将所述原始数据去零偏，获取自动导引运输车的当前运行方向，包括：

将所述陀螺仪的去零偏的数据四元数解算出姿态角作为估计量，将所述加速度计和磁力计测量的原始数据中的角度作为观测量进行卡尔曼滤波得到所述自动导引运输车的姿态角。

3.根据权利要求1所述的方法，其特征在于，所述根据里程数据和校准后的导航的角度，形成导航航迹，包括：

k时刻自动导引运输车的位置和姿态为(x(k),y(k),φ(k))，φ是航向与自己设定的X轴方向的夹角，则K+1时刻的姿态为：

其中Δd(k)为编码器获得的增量里程；Δφ(k)采用陀螺仪测量得增量角度，这样通过迭代推算出所有采样时刻的位置和姿态，形成导航航迹。

4.根据权利要求1所述的方法，其特征在于，所述基于导航航迹以及控制算法对所述自动导引运输车进行运动控制，包括：

将所述自动导引运输车当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出；

以偏差角度转为运动输入，利用控制算法来调整自动导引运输车的姿态。

5.根据权利要求4所述的方法，其特征在于，所述控制算法包括：模糊控制算法、模糊PID算法、自适应控制算法和鲁棒控制算法。

6.根据权利要求2所述的方法，其特征在于，所述将所述陀螺仪的去零偏的数据四元数解算出姿态角作为估计量，将所述加速度计和磁力计测量的原始数据中的角度作为观测量进行卡尔曼滤波得到所述自动导引运输车的姿态角，包括：

其中：x_k为状态向量；y_k为观测向量；A为从k一1时刻到k时刻的状态转移矩阵；u_k为系统输入控制向量；B为输入控制向量的增益矩阵；H为状态量到观测量的增益矩阵；w_k为输入噪声；v_k为观测噪声。输入噪声和观测噪声都是服从正态分布，输入噪声的协方差为Q，观测噪声的协方差为R，则卡尔曼滤波递推公式如下:

状态量的预估计:

误差协方差预估计:

P_k|k-1＝AP_k-1A^T+Q (4)

卡尔曼增益更新：

K_k＝P_k|k-1H^T[HP_k|k-1H^T+R]^-1 (5)

现状态估计值更新:

误差协方差更新：

P_k＝(I-K_kH)P_k|k-1 (7)

其中：

为对现状态真实值X_k的估计；

为基于k-1次对k次的预估计；A为从k-1次到k次的状态转移矩阵；B为输人控制向量的增益矩阵；H为状态量到观测量的增益矩阵；y_k为k次的观测量；K_k为对观测偏差

的修正加权，即卡尔曼增益矩阵是直接影响状态估计准确性的关键因素；P_k为现状态估计值的误差协方差矩阵；P_k|k-1为预估计的误差协方差矩阵；I为单位矩阵。

7.根据权利要求2所述的方法，其特征在于，在根据里程数据和校准后的导航的角度，形成导航航迹之前，所述方法还包括：

采集经过四元数解算和卡尔曼滤波得到的姿态角，获取磁传感器采集分布在导航路线上的预定装备，获取预定装备的位置，以使编码器根据驱动电机的脉冲数计算里程，获取里程数据。

8.根据权利要求4所述的方法，其特征在于，所述将所述自动导引运输车当前时刻的位置点和给定位置点作为控制调节器的输入，得出增量角度作为输出，包括：

如果在k时刻给定的坐标为(x_m(k),y_m(k))，而实际的位置和姿态为(x(k),y(k))，此时输出偏差采用极坐标方式为：

Δφ＝a tan(y(k)-y_m(k))/(x(k)-x_m(k)) (8)

9.一种用于执行上述权利要求1-8中任一项所述的自动导引运输车导航方法的导航系统，其特征在于，包括：

磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块、微控制器模块和运动控制模块；

所述磁导航模块、惯性导航模块、读卡器模块、避障模块、人机交互模块和运动控制模块均与所述微控制器模块相连。

Images