一种基于磁带导引AGV的多传感器信息融合定位方法与流程

本发明涉及移动机器人定位领域，尤其是一种基于磁带导引AGV的多传感器信息融合定位方法。

背景技术：

近年来，随着经济社会的发展，市场需求已经开始转向多样化的方向，单品种大批量的生产模式已经日渐显现其弊端，定制化生产越来越受到消费者和生产者的青睐。如何在保持低成本高效率生产的前提下，提高生产的灵活性、多样性；缩短单品生产周期；在一个或多个设备发生故障下仍能维持生产，是急需解决的问题，柔性制造即在此基础上提出来了。

物料运输的时间成本是柔性制造系统效率进一步提高的瓶颈，因此缩短AGV系统运作的时间对整个物流系统起关键性作用，对实际工程应用有着巨大的影响，AGV运作流程主要包含装载、行驶、卸载、行驶四个过程。对于特定的AGV系统，物料的装卸载位置、AGV所能行驶的通道以及物料移载所需的时间是固定的。而单辆AGV在全局地图中的定位对AGV调度管理，系统运作效率及实时监控起着至关重要的作用，因此，AGV的行驶速度、任务调度及路径规划的优劣直接关系到整个AGV系统的运作。

磁带导引AGV以其高性价比、高可靠性和灵活性逐渐占据着低端市场主导地位。目前，大部分磁带导引AGV调度系统应用在低端制造业和应用场景相对固定的环境当中，而AGV的全局定位大多采用射频信号识别或里程计测程来实现，定位精度太过依赖外部信号，且大部分的AGV横向定位精度会随着时间的累积而不断减低，甚至出现定位脱轨的情况，这对于整个调度系统是致命的。为了提高磁带导引AGV的横向定位精度，针对AGV系统配置的磁导引传感器，

技术实现要素：

为了解决磁带导引AGV已有定位方法中局部定位精度低、定位成本高且无法实现AGV的全局可定位及可监控等不足，本发明针对磁带导引AGV系统配置的传感器，并基于数据融合模型，提出了一种采用卡尔曼滤波算法对编码器和磁导引传感器的数据进行信息融合，并结合特殊磁带信号对AGV的定位精度进行进一步修正的定位方法。

本发明解决其技术问题所采用的技术方案是：

一种基于磁带导引AGV的多传感器信息融合定位方法，所述方法包括以下步骤：

步骤1)建立磁带导引AGV自动导引的循迹误差模型，根据AGV小车与导引路径的位置关系，对AGV循迹误差系统的偏差角度和横向偏差距离进行分析，记e_θ为AGV小车行进方向相对于磁带中心线的偏差角度，e_d为AGV质心相对于磁带中心线的横向偏差距离，磁带导引AGV的循迹误差系统表示为：

在AGV正常寻线运行的情况下，车体运行方向与磁带中心线的夹角始终保持小角度，因此，式(1)改写为：

其中，w(t)为AGV移动角速度，v_c(t)为AGV质心移动线速度；

将式(2)改写成离散形式如下：

即

其中，T为系统采样时间，采样计数k∈N；

步骤2)确定AGV小车质心位置与磁导引传感器的安装位置，记磁导引传感器中心点与质心之间的距离为L；

步骤3)确定磁带导引路径以及特殊磁带信号的铺设方式；

步骤4)令AGV的系统状态变量x_k＝[e_θ(k) e_d(k)]^T，控制输入量u_k＝Δv，Δv为AGV左右驱动轮的速度差，则可以采用卡尔曼滤波对AGV左右驱动轮编码器的里程计信息和横向偏差距离信息进行融合，系统状态方程为：

x_k＝Fx_k-1+Bu_k (5)

其中，D为左右驱动轮轮距，F为作用在x_k-1上的状态变换矩阵，B为作用在系统控制变量u_k上的输入控制变换矩阵；

步骤5)由磁导引传感器和左右驱动轮的里程计信息提取的偏差数据作为滤波器的观测值，则得到系统的状态观测方程：

z_k＝Hx_k (8)

其中，分别为带有噪声的实际观测值；

步骤6)由左右驱动轮的里程计信息得到AGV沿磁带循迹移动的距离表示为：

其中，Δs_l,Δs_r分别为左右驱动轮的转动距离；

步骤7)在不同形式导引路径转换的节点处铺设横向磁带，以区别导引磁带路径，来修正AGV行进过程中里程计的累计误差。

进一步，所述步骤2)中，磁导引传感器安装在位于AGV小车行进方向的前端，磁导引传感器的数据值为传感器中心点偏离磁带中心线的距离值。

再进一步，所述步骤3)中，磁带导引路径的铺设方式采用直线和一定曲率的圆周形式，直线路径引导AGV小车进行距离，圆周路径引导AGV小车的方向转变。

更进一步，所述步骤3)中，特殊磁带信号采取垂直于导引路径的铺设方式，即在直线与圆周导引磁带的转换点处，导引磁带的左边或右边铺设垂直磁带，以区别磁导引传感器导引时的信号值。

进一步，所述步骤4)中，系统状态方程包含过程噪声W_k，符合均值为零，协方差矩阵为Q_k的多元正态分布，W_k～N(0,Q_k)；

所述步骤4)和所述步骤5)中，在k时刻，对系统状态进行观测，系统的状态观测方程包含观测噪声V_k，符合均值为零，协方差矩阵为R_k的多元正态分布，V_k～N(0,R_k)；

所述步骤4)和步骤5中，卡尔曼滤波的操作过程分为两个阶段：预测，利用上一阶段的状态估计值，作出对当前状态的估计；更新，使用当前状态的观测值对预测阶段的预测值进行优化，获得一个对系统当前状态更加精确的估计；

预测系统状态：

式(11)中，是根据上一阶段的最优估计结果以及最近一次的控制输入u_k-1对现在的系统状态的估计；

预测协方差矩阵：

式(12)中，P_k-1|k-1是对应的协方差矩阵，P_k|k-1是对应的协方差矩阵，Q_k为系统动态噪声的协方差矩阵；

以上为卡尔曼滤波器对系统状态的预测阶段，下面为滤波器根据实际观测值对预测数据进行更新的阶段：

式(13)中，为测量余量，是真实观测值z_k与预测系统状态估计观测的差值；

式(14)中，S_k为测量余量的协方差矩阵，包含了预测阶段误差的传递值和观测噪声协方差矩阵R_k；

式(15)中，K_k是对测量余量的权重估计，即卡尔曼增益；

式(16)中，I为单位矩阵，P_k|k为系统协方差矩阵P_k|k-1的更新，以便在下一次状态估计中使用；

以上即为卡尔曼滤波算法的基本流程，根据公式(11)～(16)采用计算机容易实现滤波器的自回归计算。

更进一步，步骤4)和步骤5)中，采用的编码器是AGV的内部传感器，其工作稳定抗干扰，数据采集精度高，在AGV稳定运行车轮无滑移的前提下，得到车体精确的位移数据，但其误差会随时间积累；磁导引传感器属于外部传感器，直接得到其中心点与导引磁带中心线的偏差距离信息，其精度受采样频率和外界干扰磁场的限制。

所述步骤6)中，两轮差速移动机器人的里程计模型可以得到AGV行进里程信息。

进一步，所述步骤7)中，由于里程计信息的累积误差，故必须定时修正其误差，磁导引传感器的信号采集特点决定了可以铺设横向磁带来作定点标识。

本发明的技术构思为：采用传感器信息融合技术，设计了卡尔曼滤波器的定位方法，并且在综合比较了磁导引传感器、编码器各自的数据特点后，对滤波结果进行了进一步的融合处理，提高了AGV相对于磁带的横向定位精度，并且在此基础上，采用布置特殊磁带信号点的方法，对AGV的纵向定位精度进行修正，综合以上方法，实现了AGV的全局可定位和可监控。

本发明的有益效果主要表现在：针对固定导引路径的AGV定位问题，由于过于依靠外部传感器而导致定位精度易受外界环境影响，且成本相对较高，以及无法准确定位AGV的站点、节点等位置信息等不足，针对以上存在的问题，本发明提供一种对AGV内部传感器信息和导引传感器信息进行数据融合，且利用特殊磁带信号修正里程累计误差的方法，解决了固定路径导引AGV的相对定位问题。本专利提供的定位方法将导引传感器用于定位和识别站点等信息，有效且充分地扩展了已有传感器的用途，降低了系统的硬件成本。相比传统的利用外界的射频信号、行程开关、视觉等方法来定位，本发明采用数据融合的方法解决AGV质心相对于磁带中心线的定位偏差问题，利用特殊磁带信号解决AGV的运行里程累计误差问题，提高了相对定位的精度，且大大降低了系统的硬件成本。

附图说明：

图1是磁带导引AGV的循迹示意图，便于得到小车的循迹误差模型；

图2是磁带导引AGV系统状态量与磁导引传感器偏差关系示意图，便于刻画系统输出变量；

图3是特殊磁带信号的集中铺设方式，便于得到AGV的站点信息；

图4是AGV的数据融合模型。

具体实施方式

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

参照图1～图4，一种基于磁带导引AGV的多传感器信息融合定位方法，

所述方法包括以下步骤：

步骤1)建立磁带导引AGV自动导引的循迹误差模型，根据AGV小车与导引路径的位置关系，对AGV循迹误差系统的偏差角度和横向偏差距离进行分析，记e_θ为AGV小车行进方向相对于磁带中心线的偏差角度，e_d为AGV质心相对于磁带中心线的横向偏差距离，磁带导引AGV的循迹误差系统表示为：

在AGV正常寻线运行的情况下，车体运行方向与磁带中心线的夹角始终保持小角度，因此，式(1)可改写为：

其中，w(t)为AGV移动角速度，v_c(t)为AGV质心移动线速度；

将式(2)改写成离散形式如下：

即

其中，T为系统采样时间，采样计数k∈N；

步骤2)确定AGV小车质心位置与磁导引传感器的安装位置，记磁导引传感器中心点与质心之间的距离为L；

步骤3)确定磁带导引路径以及特殊磁带信号的铺设方式；

步骤4)令AGV的系统状态变量x_k＝[e_θ(k)e_d(k)]^T，控制输入量u_k＝Δv，Δv为AGV左右驱动轮的速度差，则可以采用卡尔曼滤波对AGV左右驱动轮编码器的里程计信息和横向偏差距离信息进行融合，系统状态方程为：

x_k＝Fx_k-1+Bu_k (5)

其中，D为左右驱动轮轮距，F为作用在x_k-1上的状态变换矩阵，B为作用在系统控制变量u_k上的输入控制变换矩阵；

步骤5)由磁导引传感器和左右驱动轮的里程计信息提取的偏差数据作为滤波器的观测值，则得到系统的状态观测方程：

z_k＝Hx_k (8)

其中，分别为带有噪声的实际观测值；

步骤6)由左右驱动轮的里程计信息得到AGV沿磁带循迹移动的距离表示为：

其中，Δs_l,Δs_r分别为左右驱动轮的转动距离；

步骤7)在不同形式导引路径转换的节点处铺设横向磁带，以区别导引磁带路径，来修正AGV行进过程中里程计的累计误差；

进一步，所述步骤2)中，磁导引传感器一般安装在位于AGV小车行进方向的前端，磁导引传感器的数据值为传感器中心点偏离磁带中心线的距离值；

进一步，所述步骤3)中，磁带导引路径的铺设方式采用直线和一定曲率的圆周形式，直线路径引导AGV小车进行距离，圆周路径引导AGV小车的方向转变；

更进一步，所述步骤3)中，特殊磁带信号采取垂直于导引路径的铺设方式，即在直线与圆周导引磁带的转换点处，导引磁带的左边或右边铺设垂直磁带，以区别磁导引传感器导引时的信号值；

进一步，所述步骤4)中，系统状态方程包含过程噪声W_k，符合均值为零，协方差矩阵为Q_k的多元正态分布，W_k～N(0,Q_k)；

结合步骤4)，所述步骤5)中，在k时刻，对系统状态进行观测，系统的状态观测方程包含观测噪声V_k，符合均值为零，协方差矩阵为R_k的多元正态分布，V_k～N(0,R_k)；

针对步骤4)和步骤5)，卡尔曼滤波的操作过程分为两个阶段：预测，利用上一阶段的状态估计值，作出对当前状态的估计；更新，使用当前状态的观测值对预测阶段的预测值进行优化，获得一个对系统当前状态更加精确的估计；

预测系统状态：

式(11)中，是根据上一阶段的最优估计结果以及最近一次的控制输入u_k-1对现在的系统状态的估计；

预测协方差矩阵：

式(12)中，P_k-1|k-1是对应的协方差矩阵，P_k|k-1是对应的协方差矩阵，Q_k为系统动态噪声的协方差矩阵；

以上为卡尔曼滤波器对系统状态的预测阶段，下面为滤波器根据实际观测值对预测数据进行更新的阶段：

式(13)中，为测量余量，是真实观测值z_k与预测系统状态估计观测的差值；

式(14)中，S_k为测量余量的协方差矩阵，包含了预测阶段误差的传递值和观测噪声协方差矩阵R_k；

式(15)中，K_k是对测量余量的权重估计，即卡尔曼增益；

式(16)中，I为单位矩阵，P_k|k为系统协方差矩阵P_k|k-1的更新，以便在下一次状态估计中使用；

以上即为卡尔曼滤波算法的基本流程，根据公式(11)～(16)采用计算机容易实现滤波器的自回归计算；

更进一步，步骤4)和步骤5)中，采用的编码器是AGV的内部传感器，其工作稳定抗干扰，数据采集精度高，在AGV稳定运行车轮无滑移的前提下，可以得到车体精确的位移数据，但其误差会随时间积累；磁导引传感器属于外部传感器，可直接得到其中心点与导引磁带中心线的偏差距离信息，其精度受采样频率和外界干扰磁场的限制；

所述步骤6)中，两轮差速移动机器人的里程计模型可以得到AGV行进里程信息；

进一步，所述步骤7)中，由于里程计信息的累积误差，故必须定时修正其误差，磁导引传感器的信号采集特点决定了可以铺设横向磁带来作定点标识。

本实施例的基于磁带导引AGV的多传感器信息融合定位方法，所述方法包括以下步骤：

步骤1)以四轮小车为研究对象，两前轮作为驱动轮，实现差速转向，两后轮作为从动轮，起支撑作用。从图1中得到磁带导引AGV的循迹误差模型，为选择器控制方法以及具体设计其控制器提供了依据，同时也为后续的信息融合提供了数学依据。

步骤2)图2给出了磁带导引AGV循迹的最一般姿态，磁导引传感器得出的物理量为磁导引传感器中心点与磁带中心线的偏差距离E，由此，可以计算出AGV质心相对于磁带中心线的偏差距离为：

e_d＝E cos e_θ-L sin e_θ (17)

其中，θ₀为AGV的初始偏差角度；

步骤3)对于特殊磁带信号的铺设方式有三种情形，如图3所示，第一种是铺设在AGV行进方向上的左侧，第二种是右侧，第三种是左右两侧都铺设，这样就有三种特殊磁带信号，有利于在AGV循迹过程中不同更多站点信号的检测与里程误差的修正；

步骤4)对左右驱动轮编码器信号进行变换补偿后得到驱动轮的线速度，磁导引传感器信号经过处理后得到车体与磁带导引线的偏差以及到达站点的位置信息，通过采用卡尔曼滤波融合的方法得到AGV车体方位与预定线路偏差的最优估计；而根据特殊磁带信号的信息以及预先存储的路径数据库预知了站点信息，结合特殊磁带信息和磁导引传感器提取到的站点位置，进而对AGV当前的进行累计里程误差进行进一步的整合修正。

如图1所示，磁带导引AGV的各参数表示清晰，并且在全局坐标系中给定AGV的最一般姿态，对其进行动态分析，得到磁带导引AGV的循迹误差模型，图中各符号表示为：O′为AGV两驱动轮轮轴中心即质心；D为AGV两驱动轮轮距；ν_l为AGV左驱动轮移动线速度；ν_r为AGV右驱动轮移动线速度；ν_c为AGV质心移动线速度；e_d为AGV质心与磁带轨迹中心线的横向偏差距离；e_θ为AGV质心行进方向与磁带轨迹中心线的偏差角度；1为铺设磁带轨迹，箭头表示小车行进方向；

如图2所示，为移动机器人在循迹过程中如何通过磁导引传感器采集的信息来刻画控制系统的输出变量，且输出变量e_d只与当前时刻的E和e_θ相关。图中各符号表示为：O′为AGV两驱动轮轮轴中心即质心；D为AGV两驱动轮轮距；1为铺设磁带轨迹，箭头表示小车行进方向；2为磁导引传感器；L为磁导引传感器安装位置相对于AGV质心的垂直距离；E为磁导引传感器中心点相对于磁带中心线的偏差距离；M为磁导引传感器的中心点；e_d为AGV质心与磁带轨迹中心线的横向偏差距离；e_θ为AGV质心行进方向与磁带轨迹中心线的偏差角度；

如图3所示，为磁带导引系统中特殊磁带信号的铺设方式，充分扩展了磁带的作用。图中各符号表示为：4为铺设磁带轨迹，箭头表示小车行进方向；1为特殊磁带铺设在AGV行进方向上的右侧；2为特殊磁带铺设在AGV行进方向上的左侧；3为特殊磁带铺设在AGV行进方向上的左右两侧；

如图3所示，为AGV数据融合模型，给出了磁带导引AGV在整个循迹过程中定位方法。图中各符号表示为：1为信号源；2为数据融合定位与定位修正；3为决策控制。