CN110398251B - 一种基于多传感器融合的无轨导航agv定位系统及其定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的无轨导航AGV定位系统及其定位方法，涉及导航与控制技术领域。该系统包括红外图像定位模块、航迹推算定位模块和局部超声网络定位模块，红外图像定位模块测得的数据通过线性预测，对齐航迹推算定位模块推算出的定位频率；同时用局部超声网络定位模块的数据对航迹推算定位的结果进行校准；最终采用径向基函数神经网络模型对统一频率后的两组定位坐标进行数据融合，得到最终的定位结果。本发明克服了各种传感器在单独使用时存在的定位不精确，单个传感器使用成本过高的问题，并且精度高、实时性好、路径变更更加灵活。

Description

一种基于多传感器融合的无轨导航AGV定位系统及其定位 方法

技术领域

本发明涉及导航与控制技术领域，具体是一种基于多传感器融合的无轨导航AGV定位系统及其定位方法。

背景技术

AGV(Automated Guided Vehicle，自动导引运输车)装有自动导引装置，能够沿规定的路径行驶，在车体上有编程和停车选择装置、安全保护装置以及各种具有物料移载功能的装置。其中，定位是AGV研究的核心问题。

常用的引导定位方式有红外图像定位、航迹推算定位以及局部超声网络定位等。红外图像定位得到的数据为实时的、无累计误差的定位数据，虽然精度较高，但是定位频率低，一般只能达到10Hz。航迹推算定位的数据采集频率较高，可达到100Hz左右，但定位随着时间和距离的增长会产生累计误差。局部超声网络定位在局部定位方面效果较好，但不能覆盖全部工作区域。

上述各种定位方式均通过单个定位传感器实现定位，但是当针对精确对接领域时，单个定位传感器通常会因为数据不稳定、精度低或者具有累计误差等因素影响，而不能满足自主导航与对接要求。

现有的研究采用设定优先级的方式融合多种定位传感器，即在某一定位传感器出现故障时，使用另一种定位方式。但是，这种方法仅仅避免了数据采集过程中由于遮挡或故障引起的无效测量导致的外在误差，而对系统本身性质导致的误差未做任何有效处理，无法从根本上提高定位系统的精度和实时性。

发明内容

本发明要解决的技术问题是：通过多传感器融合的算法，改善和修正红外图像定位系统、局部超声网络定位方法和航迹推算定位方法中存在的问题与误差，从而在保证系统实时性的同时，形成一套精度更高的AGV定位系统，具体是一种基于多传感器融合的无轨导航AGV定位系统及其定位方法。

所述的基于多传感器融合的无轨导航AGV定位系统包括红外图像定位模块、航迹推算定位模块和局部超声网络定位模块，三个模块都能独立测得定位数据信息；

首先，红外图像定位模块测得的数据通过线性预测，对齐航迹推算定位模块推算出的定位频率；同时用局部超声网络定位模块的数据对航迹推算定位的结果进行校准；最终采用径向基函数神经网络模型对统一频率后的两组定位坐标进行数据融合，得到最终的定位结果。

红外图像定位模块包括红外传感器，安装在AGV车体外侧上方的头部中间位置，且红外传感器的正方向与车头正方向平行；

航迹推算定位模块安装在AGV车体的几何中心位置，包括一个惯性测量模块IMU和两个辅助编码器；

局部超声网络定位模块包括两个接收端和一个超声波发射端，两个接收端分别为接收端a和接收端b，安装在车体的同一侧无遮挡的位置，超声波发射端安装在工位点O。

优选地，将其中一个接收端安装在车身一侧的头部，另一个安装在同一侧的中心部位。局部超声网络定位模块自带红外同步功能，使得超声波发射端发射超声波的同时，接收端几乎同时开始计时，接收到超声波信号后停止计时，该时间近似为超声波的传播时间。

所述AGV车体的轮系结构包括两个主动轮和四个万向从动轮，其中两个主动轮安装在AGV车体左右两侧的中间位置，两个电机分别安装在两个主动轮上，电机上装有用于反馈的编码器；四个万向从动轮分别安装在AGV车体的四个角上。

基于多传感器融合的无轨导航AGV定位系统进行定位的方法，具体包括以下步骤：

步骤一、以车体中心为原点，初始方向与世界坐标系x轴和y轴正方向一致，建立车载坐标系。

在车载坐标系下，红外传感器的中心位置相对于车体中心的坐标记作(X_s,Y_s)。

步骤二、在AGV运行轨迹范围内水平安装若干电子标签，电子标签安装在红外传感器发出的红外射线扫射的区域内，并保证红外传感器始终扫到至少一个电子标签。

电子标签安装的方向与世界坐标系的x轴正方向一致。

步骤三、从红外传感器识别出的所有电子标签中选取最邻近红外传感器的第i个电子标签作为定位原始数据源，并对原始数据进行滤波处理；

标定第i个电子标签在世界坐标系下的坐标，记作(X_IDi,Y_IDi)。

定位原始数据源测得的原始数据包括：红外传感器相对于该最邻近电子标签的相对坐标(x_i0,y_i0)以及偏航角yaw_i。

将原始数据按照红外传感器的测量频率发送给AGV自带的控制器，通过数据接收时间对原始数据进行滤波处理。具体为：判断数据接收时间是否大于设定阈值，如果是，则判定这类原始数据是错误数据，由于场地内红外干扰或自身发射的红外线反射多次返回摄像头的干扰造成的；利用多次定位数据的线性拟合推算出此次测量的估计值，来替代上述错误数据。否则，判定这类原始数据为可用数据。

步骤四、根据初次滤波后的结果，控制器利用红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)，得到车体在世界坐标系中的坐标；

首先，控制器根据偏航角yaw_i和红外传感器相对于车体的坐标(X_s,Y_s)，将红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)进行变换，推出车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)：

其中，yaw为偏航角yaw_i转换为世界坐标系时的偏航角，当电子标签安装方向与世界坐标系的x轴正方向一致时，偏航角无需进行变换，即yaw＝yaw_i；

然后，将车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)，结合第i个电子标签在世界坐标系下的坐标(X_IDi,Y_IDi)，进行世界坐标系的转换；

在世界坐标系下车体的坐标(x^infrared,y^infrared)为：

步骤五、随着车体旋转得到不同偏航角下的平均偏差，用平均偏差正量修正车体的世界坐标。

修正是指：红外传感器安装不绝对水平导致有微小的偏角，或黏贴的标签不绝对水平产生的测量误差；由于标签的正方向已经固定，而传感器会随着车体旋转，误差是和传感器的正方向有关，需要进行标定实验来进行修正。

具体过程如下：

步骤501、在最邻近红外传感器的第i个电子标签辐射范围内，将安装好红外传感器的AGV原地旋转360°，测得不同角度j下的坐标(x_i0j,y_i0j)，并在坐标系中形成图形的几何中心(X_i0,Y_i0)；

步骤502、将不同角度下的各坐标分别和几何中心作差，求得不同角度下的坐标偏差δx_i0j和δy_i0j，经过多次试验，求得不同偏航角下的平均偏差

步骤503、利用平均偏差

修正最终的车体在世界坐标系中的坐标；

修正后的车体坐标(x^infrared',y^infrared')如下：

步骤504、判断是否需要对修正后的车体坐标进行滤波处理。

判断过程为：

设AGV最高行驶速度为V_max，当Dis(P_i,P_i-1)-V_maxΔt＞0时，采用之前多次定位数据的线性拟合推算出的估计值，来替代此次修正后的车体坐标结果。否则，不需要进行滤波处理。

其中，Dis(P_i,P_i-1)表示此次测得的位置和上一次测得的位置的欧氏距离，Δt为两次测量的时间间隔。

步骤六、运用卡尔曼滤波对航迹推算定位模块中获取的数据进行融合，获得AGV的位置向量、速度向量和姿态向量，并将其投影到世界坐标系的XOY平面，获得其二维表示。

航迹推算定位模块获取的数据包括：从IMU获取AGV的统加速度、角速度、磁感应强度和温度等相关数据，从两个辅助编码器获得两驱动轮轴转过的角度和转动的速度。

步骤七、红外图像定位模块采集数据进行线性预测，得到定位结果(x₁,y₁)；对齐航迹推算定位模块的定位频率；

具体方法如下：

红外图像定位模块的定位频率设为f^Infrared，航迹推算定位模块的定位频率设为f^INS，对齐两者的定位频率时，首先两种定位模式同时开始采集数据，在每个间隔时间1/f^INS内，将红外图像定位模块最近采集到的N个数据，使用最小二乘法进行线性拟合，预测出此时的定位坐标，记为

作为定位结果(x₁,y₁)。

步骤八、局部超声网络定位模块利用几何关系确定出车体中心在世界坐标系XOY平面的全局坐标。

首先，根据超声波的传播时间和声速计算出距离，结合偏航角以及AGV姿态向量中的车身姿态分量，求出车体中心相对于工位点O的坐标

然后，结合工位点O的坐标(x_o,y_o)得到车体在世界坐标系的全局坐标(x^ultrasonic,y^ultrasonic)。

坐标变换关系如下：

步骤九、利用车体在世界坐标系的全局坐标(x^ultrasonic,y^ultrasonic)对航迹推算定位模块融合的结果进行校准，校准后的坐标为(x₂,y₂)；

具体为：超声波发射端安装在AGV行驶路径的工位点O处，每到达工位点O时进行一次校准，即使用局部超声波网络定位得到的车体中心的全局坐标替换航迹推算得到的AGV的位置向量，替换后结果(x₂,y₂)。

步骤十、采用径向基函数神经网络模型对定位结果(x₁,y₁)和(x₂,y₂)进行融合，得到最终精确度高且实时性好的定位坐标。

径向基函数神经网络模型为单隐层的三层前馈网络，设第一个神经网络的输出为最终的定位坐标；输入为x₁、x₂和影响因素d及t，共4个输入变量；隐含层节点个数为6个。

AGV在大部分路径中为匀速行驶，则航迹推算定位模块行驶中的累计误差与矫正前的累计行驶时间t有关。

红外图像定位模块的定位精度与距离参考标签中心的距离d有关，即距离中心越近得到的数据可信度越高；

本发明的优点及其有益效果在于：

1、本发明一种基于多传感器融合的无轨导航AGV定位系统，融合了红外图像定位模块、航迹推算定位模块和局部超声网络定位模块的多个传感器，克服了各种传感器在单独使用时存在的定位不精确，单个传感器使用成本过高的问题。

2、本发明一种基于多传感器融合的无轨导航AGV定位方法，运用了径向基函数(RBF-Radial Basis Function)神经网络等智能算法，融合了红外图像定位系统、航迹推算定位系统和局部超声波网络的定位方法，精度高、实时性好、路径变更更加灵活。

附图说明

图1为本发明基于多传感器融合的无轨导航AGV定位系统的俯视图；

图2为本发明一种基于多传感器融合的无轨导航AGV定位方法流程图；

图3为本发明红外图像定位模块的原理示意图；

图4为本发明AGV运动模型示意图；

图5为本发明卡尔曼滤波器滤波流程示意图；

图6为本发明局部超声网络定位原理示意图；

图7为本发明RBF神经网络模型图。

图中，1-红外图像定位模块，2-IMU，3-电机，4-接收端a，5-接收端b，6-AGV车体，7-主动轮，8-万向从动轮。

具体实施方式

下面将结合具体实施例和附图对本发明的内容作进一步明确清楚地表述。

如图1所示，所述的基于多传感器融合的无轨导航AGV定位系统包括红外图像定位模块、航迹推算定位模块和局部超声网络定位模块，三个模块都能独立测得定位数据信息；

首先，红外图像定位模块测得的数据通过线性预测，对齐航迹推算定位模块推算出的定位频率；同时用局部超声网络定位模块的数据对航迹推算定位的结果进行校准；最终采用径向基函数神经网络模型对统一频率后的两组定位坐标进行数据融合，得到最终的定位结果。

红外图像定位模块1包括红外传感器，安装在AGV车体6外侧上方的头部中间位置，且红外传感器的正方向与车头正方向平行；

航迹推算定位模块安装在AGV车体6的几何中心位置，包括一个惯性测量模块(IMU)2和两个辅助编码器；

局部超声网络定位模块包括两个接收端和一个超声波发射端，两个接收端分别为接收端a4和接收端b5，安装在车体的同一侧无遮挡的位置，超声波发射端安装在工位点O。

优选地，将其中一个接收端4安装在车身一侧的头部，另一个安装在同一侧的中心部位。局部超声网络定位模块自带红外同步功能，使得超声波发射端发射超声波的同时，接收端几乎同时开始计时，接收到超声波信号后停止计时，该时间近似为超声波的传播时间。

所述AGV车体6的轮系结构包括两个主动轮7和四个万向从动轮8，其中两个主动轮安装在AGV车体6左右两侧的中间位置，两个电机3分别安装在两个主动轮7上，电机上装有用于反馈的编码器；四个万向从动轮分别安装在AGV车体的四个角上。

基于多传感器融合的无轨导航AGV定位系统进行定位的方法，如图2所示，具体包括以下步骤：

步骤一、以车体中心为原点，初始方向与世界坐标系x轴和y轴正方向一致，建立车载坐标系。

将红外传感器正方向与车头正方向平行安装，在车载坐标系下，红外传感器的中心位置相对于车体中心的坐标记作(X_s,Y_s)。

步骤二、在AGV运行轨迹范围内水平安装若干电子标签，电子标签安装在红外传感器发出的红外射线扫射的区域内，并保证红外传感器始终扫到至少一个电子标签。

如图3所示，电子标签安装的方向与世界坐标系的x轴正方向一致，使得测量得到的偏航角无需进行坐标变换，且不可有光线的遮挡。为了保证实时性，电子标签安装距离需要保证安装在AGV上的红外定位模块发出的红外射线可以始终扫到至少一个。

步骤三、从红外传感器识别出的所有电子标签中选取最邻近红外传感器的第i个电子标签作为定位原始数据源，并对原始数据进行滤波处理后对红外传感器的测量数据进行修正。

标定第i个电子标签在世界坐标系下的坐标，记作(X_IDi,Y_IDi)。

定位原始数据源测得的原始数据包括：红外传感器相对于该最邻近电子标签的相对坐标(x_i0,y_i0)以及偏航角yaw_i。

将原始数据按照红外传感器的测量频率发送给AGV自带的控制器，控制器通过数据接收时间对原始数据进行滤波处理。

具体为：判断数据接收时间是否大于设定阈值，如果是，则判定这类原始数据是错误数据，由于场地内红外干扰或自身发射的红外线反射多次返回摄像头的干扰造成的；利用多次定位数据的线性拟合推算出此次测量的估计值，来替代上述错误数据。否则，判定这类原始数据为可用数据。

根据初次滤波后的结果，控制器将红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)转换到世界坐标系中；具体为：

首先，控制器根据偏航角yaw_i和红外传感器的中心位置相对于车体的坐标(X_s,Y_s)，将红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)进行变换，推出车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)：

其中，yaw为偏航角yaw_i转换为世界坐标系时的偏航角，当电子标签安装方向与世界坐标系的x轴正方向一致时，偏航角无需进行变换，即yaw＝yaw_i；

然后，将车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)进行坐标变换，得到车体中心在世界坐标系中的坐标(x^infrared,y^infrared)：

然后，控制器对红外传感器的测量数据进行修正。

修正是指：红外传感器安装不绝对水平导致有微小的偏角，或黏贴的标签不绝对水平产生的测量误差；由于标签的正方向已经固定，而传感器会随着车体旋转，误差是和传感器的正方向有关，需要进行标定实验来进行修正。

标定过程如下：

首先、在最邻近红外传感器的第i个电子标签辐射范围内，将安装好红外传感器的AGV原地旋转360°，测得不同角度yaw_ij下的坐标(x_i0j,y_i0j)并转换为车体中心的坐标x_ij和y_ij，并在坐标系中形成图形的几何中心(X_i0,Y_i0)；

然后、将不同角度下的各坐标分别和几何中心作差，求得不同角度下的坐标偏差δx_i0j和δy_i0j，经过多次试验，求得不同偏航角下的平均偏差

利用平均偏差

修正相对坐标(x_i,y_i)；由于和最终的全局坐标(x^infrared,y^infrared)为线性关系，故可以直接将偏移量加入到最终的坐标中。

即修正后的全局坐标为：

得到全局坐标后，再次判断是否需要进行滤波处理。

判断过程为：设AGV最高行驶速度为V_max，当Dis(P_i,P_i-1)-V_maxΔt＞0时，采用之前多次定位数据的线性拟合推算出的估计值，来替代此次修正后的车体的全局坐标结果。否则，Dis(P_i,P_i-1)-V_max·Δt≤0时，不需要进行滤波处理。

其中，Dis(P_i,P_i-1)表示此次测得的位置和上一次测得的位置的欧氏距离，Δt为两次测量的时间间隔。

步骤四、运用卡尔曼滤波对航迹推算定位模块中获取的数据进行融合，获得AGV的位置向量、速度向量和姿态向量，并将其投影到世界坐标系的XOY平面，获得其二维表示。

航迹推算定位模块获取的数据包括：从IMU获取AGV的统加速度、角速度、磁感应强度和温度等相关数据，从两个辅助编码器获得两驱动轮轴转过的角度和转动的速度。

根据AGV车体上的三个正交加速度计和双重磁性传感器计算车体的初始方向，以起始点作为起点，能够测量在完全停滞的状态下车身框架上的加速度f^b：

其中

是导航框架到车体框架的转换矩阵。fⁿ是导航框架上的加速度，gⁿ是重力加速度，f_x,f_y,f_z分别代表车载坐标系中三轴方向的加速度。φ和θ是倾斜角和俯仰角。

倾斜角和俯仰角可以用公式(5)表示：

静止时的偏航角

可以从地球旋转角速度获得。然而，测量地球旋转角速度很困难，因为测量非常小的角加速度值需要陀螺仪具有非常高的精度。罗盘传感器由两个正交的磁传感器组成，可以用地球磁场的水平分量测量正交的方向。所以可以用罗盘传感器来测量偏航角。双轴向磁场(H_ex,H_ey)可以用双轴向磁传感器测量，得到的磁场的北方角(α)可以用公式(6)表示：

真正的北和磁场的北之间是有区别的，他们角度的偏差记为磁偏角(λ)。磁偏角在世界上大多数人口稠密的地区从零度到三十度变化。偏航角是关于正北方呈右手旋转的，因此偏航角表示为：

当AGV移动时，方向由三陀螺仪测得的角速度积分获得。同时速度和位置由加速度的一次和二次积分获得，所以加速度计和陀螺仪的偏差将会增大位置和方向的误差，因此用双轴倾斜仪测得的倾斜角的信息来消除加速度计在每个轴上的偏差。

当AGV静止时，补偿三加速度计的偏差的算法如下。罗盘传感器测量偏航角，双轴倾斜仪测量倾斜角和俯仰角。假定这些角度是移动机器人的真实方向，那么转移矩阵

如下：

上述运算符c和s为cos运算和sin运算的简写。

车体框架里的加速度和导航框架的加速度的关系如下：

f_Nmeasure，f_Emeasure，f_Dmeasure分别代表世界坐标系三轴方向上的加速度测量值。f_Nbiasf_Ebias，f_Dbias分别表示静止时三轴方向上的加速度测量偏差。

三维陀螺仪为车身框架提供三维角速度，虽然角速率在长时间内是可靠的，但是必须将其整合在一起才能提供绝对的方位测量。因此，即使在角速度上的小误差也会产生无界的误差。当AGV静止时，车身框架的角速度必须为零。因此，测量的角度是偏差误差。为了补偿偏差，角速率被采样10秒钟；然后对采样数据进行平均如公式(10)所示。

其中，w^b代表测量出得到的待有测量偏差的角速度，

是在车体框架中消除偏差后的角速率矢量，w_xbiasw_ybiasw_zbias分别表示静止状态下的该角速度车载坐标系下三轴方向的测量偏差。

使用加速度的单一积分计算移动机器人的速度。下一时刻的速度将等于当前速度加上由测量周期(Δt)乘以的指令加速度。通过使用速度的单一积分来计算位置。

表示第k次测量时AGV车载坐标系下的加速度；

为第k次测量时AGV在世界坐标系下的速度和位移；

为第k+1次的AGV在世界坐标系下的速度和位移。

角标k,b,n分别代表数值属于第k次测量，车载坐标系和导航坐标系(世界坐标系)。

由安装在AGV车轮上的转子编码器测量出旋转角度η，转子转码器在轮子旋转360度时产生N个脉冲。如果测量的脉冲为M个计数，则每个车轮的旋转角度将变为：

角标l,r分别代表AGV的左、右轮。

用上述车轮的旋转角度来估计AGV的速度，位置和偏航角。如图4所示，即为车轮的旋转角度的AGV的运动预测。

AGV的行驶距离a_k可以用其轮的半径(R_wheel)和每个轮的旋转角度来表示。

a_k,la_k,r分别表示左右轮的行驶距离。

车体的偏转角变化量

是用车体的宽度和各个轮子的行驶距离计算的。公式如下：

d_width表示两轮间再车身宽度方向的距离。

所以，车体的旋转半径r_k为：

根据余弦定理，Δλ_k车体的位置变化量可以由下式求得：

然后将位置变化量转换到导航坐标系下：

ΔP_Nk，ΔP_Ek为第k时刻世界坐标系下除了重力方向外的两个方向的位移变化量，

为k-1时刻的偏航角。(N和E表示的是世界坐标系下除了重力方向外的两个方向，在航迹推算算法中由于存在磁力计，一般角标表示用北方和东方的英文首字母)

位置变化量的补偿如下：

所以AGV的位置和偏航角可以表示为：

P_Nk，P_Ek表示第k时刻AGV的位置在世界坐标系下的除重力方向外的两轴分量；P_Nk+1，P_Ek+1表示第k+1时刻AGV的位置在世界坐标系下的除重力方向外的两轴分量；

分别表示第k、k+1时刻AGV的偏航角。

在这种情况下，就可以得到AGV速度的分量：

将上面得到的AGV移动过程中的中间变量利用卡尔曼滤波器进行综合，如图5所示。在这种情况下卡尔曼滤波器的线性随机差分方程为：

其中

为k时刻的状态向量，

表示世界坐标系中除重力方向外两轴的速度，θ_k，φ_k，

表示AGV世界坐标系中的方位角，

为偏航角的变换量。x_k-1，μ_k-1，w_k-1为k-1时刻的状态向量，输入向量和过程激励噪声，z_k为输出向量，A为状态转移矩阵，B为控制矩阵，H为观测矩阵，γ_k为观测噪声。

卡尔曼滤波过程为：

为第k时刻的预测状态向量，

表示第k-1时刻的状态最优估计值，

表示第k时刻的状态预测误差协方差矩阵，K_k-1表示第k-1时刻的状态估计误差协方差矩阵，K_k第k时刻的状态估计误差协方差矩阵，Q表示过程噪声协方差矩阵，R表示观测噪声协方差矩阵，G_k表示卡尔曼增益。

通过以上过程，就可以得到更新之后的AGV状态估计。

步骤五、将红外图像定位模块的线性预测结果，与航迹推算定位模块的定位频率对齐，得到定位结果(x₁,y₁)；

具体方法如下：

红外图像定位模块的定位频率设为f^Infrared，航迹推算定位模块的定位频率设为f^INS，对齐两者的定位频率时，首先两种定位模式同时开始采集数据，在每个间隔时间1/f^INS内，将红外图像定位模块最近采集到的N个数据，使用最小二乘法进行线性拟合，预测出此时的定位坐标，记为

作为定位结果(x₁,y₁)。

同时，设频率f^Infrared和f^INS的最小公倍数f^Correction为红外图像定位系统的校正频率，每间隔时间1/f^Correction根据接收到的实际定位数据对此预测值进行修正，即使用经过控制器预处理的真实测量值(x^infrared,y^infrared)替换预测值，赋予定位结果(x₁,y₁)。

红外图像定位模块的定位精度与距离参考标签中心的距离d有关，即距离中心越近得到的数据可信度越高；

步骤六、局部超声网络定位模块利用几何关系确定出车体中心在导航坐标系XOY平面的全局坐标。

首先，根据超声波的传播时间和声速计算出距离，结合步骤六中得到的偏航角和车身姿态，求出车体中心相对于工位点O的坐标

超声波传感器定位原理简图如图6所示，中点O为超声波发射器，点F/B为超声波接收器，点C为AGV小车中心，点N为AGV小车前进方向与地面坐标系Y轴方向的交点，点A为直线CB与Y轴方向的交点。超声波接收器F与超声波接收器B之间的间距为250mm，小车中心C与超声波接收器B之间的间距为250mm，直线CB垂直于直线FB，线段FO、线段BO为超声波传播距离。

为确定小车中心C的绝对坐标，定位过程中需要利用IMU惯性传感器或者红外定位传感器提供的偏航角，综合已知距离信息以及超声波传播距离，利用三角公式可以确定小车中心C相对于点O的坐标，实际应用中超声波发射器的坐标为确定的绝对坐标，则小车中心C的绝对坐标也能求出。计算过程中由于小车偏航角的变化，需要分多种情况具体分析中心点C坐标。

下面以偏航角为锐角为例，对小车中心点C相对于超声波发射点O的坐标值进行求解。

∠FON＝∠BFO-∠FNO(25)

∠AOB＝π-∠FON-∠BOF(27)

上述公式中，公式(31)计算结果

为小车中心点C相对于超声波发射器点O的y轴坐标值，公式(33)计算结果

为小车中心点C相对于超声波发射器点O的x轴坐标值。

然后，结合工位点O的坐标(x_o,y_o)得到车体中心的全局坐标(x^ultrasonic,y^ultrasonic)。

坐标变换关系如下：

步骤七、利用车体中心的全局坐标(x^ultrasonic,y^ultrasonic)对航迹推算定位模块融合的结果进行校准，校准后的坐标为(x₂,y₂)；

具体为：超声波发射端安装在AGV行驶路径的工位点O处，每到达工位点O时进行一次校准，即使用局部超声波网络定位得到的车体中心的全局坐标替换航迹推算得到的定位坐标，航迹推算和局部超声波网络定位融合得到的坐标记为(x₂,y₂)。定位精度与工位校正前的累计误差有关。

设AGV在大部分路径中为匀速行驶，则行驶中航迹推算定位模块的累计误差与矫正前的累计行驶时间t有关，当进行超声波网络定位校正后，对累计时间t进行清零并重新计时。

步骤八、采用径向基函数神经网络模型对定位结果(x₁,y₁)和(x₂,y₂)进行融合，得到最终精确度高且实时性好的定位坐标。

本发明综合考虑红外图像定位、航迹推算定位以及局部超声网络定位三种定位方法的优点，每间隔一定频率对红外图像定位系统进行线性预测，对齐航迹推算定位的定位频率；用局部超声波网络对航迹推算进行校准，修正其累计误差；最后采用径向基函数(RBF-RadialBasis Function)神经网络模型，综合考虑多种影响因素得到最终定位。

使用神经网络对统一频率后的(x₁,y₁)和(x₂,y₂)进行数据融合，得到最终此多传感器融合定位系统的定位坐标(x,y)。

具体方法如下：

1、建立AGV定位系统多传感器融合的神经网络模型：

由于坐标x和y为相对独立的变量，故选用两个同模型的网络分别进行训练和拟合。为了收敛速度快，此处优选地选择径向基函数(RBF-Radial Basis Function)神经网络模型。该模型为单隐层的三层前馈网络，已经证明该网络可以任意精度逼近任意连续函数。设第一个神经网络的输出为最终的定位坐标x，在网络中用b表示；输入为红外定位系统得到的x₁、航迹推算融合局部超声波网络定位系统得到的x₂和经过分析得到的两个系统的影响因素d及t，共4个输入变量，网络中用a_i表示：(i＝1,2,…,4)；隐含层节点个数为6个，用h_j表示(j＝1,2,……,6)。网络模型如图7所示，其中隐含层的激活函数为径向基函数，表示为：

上式中，

表示网络输入向量；

表示网络隐含层第j个节点的中心矢量，j＝1,2,…，6，相应的节点基宽参数用δ_j来表示。

网络的权向量为

网络的输出为：

2、神经网络的训练：

网络的性能指标函数表示为：

这里，b_d(k)表示期望输出，b(k)为实际输出，此处为AGV行驶过程中测定的实际坐标，测量方法可以使用激光跟踪仪或摄像头进行标定。优选地，使用梯度下降法对权向量、节点中心向量和节点基宽参数进行调整。迭代算法如下：

其中，ξ为学习速率，ψ为动量因子。

3、同理可以训练第二个以最终坐标y为输出的RBF神经网络。最终可以得到两个传感器融合网络的输入输出关系，表示为：

经过实验，得到的坐标(x,y)相比步骤1,2的定位结果精度更高，且具有更好的实时性。

Claims (9)

1.一种基于多传感器融合的无轨导航AGV定位系统，其特征在于，包括红外图像定位模块、航迹推算定位模块和局部超声网络定位模块，三个模块都能独立测得定位数据信息；

首先，红外图像定位模块测得的数据通过线性预测，对齐航迹推算定位模块推算出的定位频率；

具体为：红外图像定位模块的定位频率设为f^Infrared，航迹推算定位模块的定位频率设为f^INS，对齐两者的定位频率时，首先两种定位模式同时开始采集数据，在每个间隔时间1/f^INS内，将红外图像定位模块最近采集到的N个数据，使用最小二乘法进行线性拟合，预测出此时的定位坐标，记为

作为定位结果(x₁,y₁)；

同时用局部超声网络定位模块的数据对航迹推算定位的结果进行校准；

具体为：首先，根据超声波的传播时间和声速计算出距离，结合偏航角以及AGV姿态向量中的车身姿态分量，求出车体中心相对于工位点O的坐标

然后，结合工位点O的坐标(x_o,y_o)得到车体在世界坐标系的全局坐标(x^ultrasonic,y^ultrasonic)；

坐标变换关系如下：

利用车体在世界坐标系的全局坐标对航迹推算定位模块融合的结果进行校准，得到校准后的坐标(x₂,y₂)；

最终采用径向基函数神经网络模型对统一频率后的两组定位坐标进行数据融合，得到最终的定位结果；

红外图像定位模块包括红外传感器，安装在AGV车体外侧上方的头部中间位置，且红外传感器的正方向与车头正方向平行；

航迹推算定位模块安装在AGV车体的几何中心位置，包括一个惯性测量模块IMU和两个辅助编码器；

局部超声网络定位模块包括两个接收端和一个超声波发射端，两个接收端分别为接收端a和接收端b，安装在车体的同一侧无遮挡的位置，超声波发射端安装在工位点O。

2.如权利要求1所述的一种基于多传感器融合的无轨导航AGV定位系统，其特征在于，所述的两个接收端中一个接收端安装在车身一侧的头部，另一个安装在同一侧的中心部位。

3.如权利要求1所述的一种基于多传感器融合的无轨导航AGV定位系统，其特征在于，局部超声网络定位模块自带红外同步功能，使得超声波发射端发射超声波的同时，接收端几乎同时开始计时，接收到超声波信号后停止计时，该计时时间近似为超声波的传播时间。

4.如权利要求1所述的一种基于多传感器融合的无轨导航AGV定位系统，其特征在于，所述AGV车体的轮系结构包括两个主动轮和四个万向从动轮，其中两个主动轮安装在AGV车体左右两侧的中间位置，两个电机分别安装在两个主动轮上，电机上装有用于反馈的编码器；四个万向从动轮分别安装在AGV车体的四个角上。

5.基于权利要求1所述的一种基于多传感器融合的无轨导航AGV定位系统进行定位的方法，其特征在于，具体包括以下步骤：

步骤一、以车体中心为原点，初始方向与世界坐标系x轴和y轴正方向一致，建立车载坐标系；

在车载坐标系下，红外传感器的中心位置相对于车体中心的坐标记作(X_s,Y_s)；

步骤二、在AGV运行轨迹范围内水平安装若干电子标签，电子标签安装在红外传感器发出的红外射线扫射的区域内，并保证红外传感器始终扫到至少一个电子标签；

电子标签安装的方向与世界坐标系的x轴正方向一致；

步骤三、从红外传感器识别出的所有电子标签中选取最邻近红外传感器的第i个电子标签作为定位原始数据源，并对原始数据进行滤波处理；

标定第i个电子标签在世界坐标系下的坐标，记作(X_IDi,Y_IDi)；

定位原始数据源测得的原始数据包括：红外传感器相对于该最邻近电子标签的相对坐标(x_i0,y_i0)以及偏航角yaw_i；

将原始数据按照红外传感器的测量频率发送给AGV自带的控制器，通过数据接收时间对原始数据进行滤波处理；

步骤四、根据初次滤波后的结果，控制器利用红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)，得到车体在世界坐标系中的坐标；

首先，控制器根据偏航角yaw_i和红外传感器相对于车体的坐标(X_s,Y_s)，将红外传感器相对于最邻近电子标签的原始坐标(x_i0,y_i0)进行变换，推出车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)：

其中，yaw为偏航角yaw_i转换为世界坐标系时的偏航角，当电子标签安装方向与世界坐标系的x轴正方向一致时，偏航角无需进行变换，即yaw＝yaw_i；

然后，将车体中心相对于最邻近第i个电子标签的相对坐标(x_i,y_i)，结合第i个电子标签在世界坐标系下的坐标(X_IDi,Y_IDi)，进行世界坐标系的转换；

在世界坐标系下车体的坐标(x^infrared,y^infrared)为：

步骤五、随着车体旋转得到不同偏航角下的平均偏差，用平均偏差正量修正车体的世界坐标；

修正是指：红外传感器安装不绝对水平导致有微小的偏角，或黏贴的标签不绝对水平产生的测量误差；由于标签的正方向已经固定，而传感器会随着车体旋转，误差是和传感器的正方向有关，需要进行标定实验来进行修正；

步骤六、运用卡尔曼滤波对航迹推算定位模块中获取的数据进行融合，获得AGV的位置向量、速度向量和姿态向量，并将其投影到世界坐标系的XOY平面，获得其二维表示；

步骤七、红外图像定位模块采集数据进行线性预测，得到定位结果(x₁,y₁)；对齐航迹推算定位模块的定位频率；

具体方法如下：

红外图像定位模块的定位频率设为f^Infrared，航迹推算定位模块的定位频率设为f^INS，对齐两者的定位频率时，首先两种定位模式同时开始采集数据，在每个间隔时间1/f^INS内，将红外图像定位模块最近采集到的N个数据，使用最小二乘法进行线性拟合，预测出此时的定位坐标，记为

作为定位结果(x₁,y₁)；

步骤八、局部超声网络定位模块利用几何关系确定出车体中心在世界坐标系XOY平面的全局坐标；

首先，根据超声波的传播时间和声速计算出距离，结合偏航角以及AGV姿态向量中的车身姿态分量，求出车体中心相对于工位点O的坐标

然后，结合工位点O的坐标(x_o,y_o)得到车体在世界坐标系的全局坐标(x^ultrasonic,y^ultrasonic)；

坐标变换关系如下：

步骤九、利用车体在世界坐标系的全局坐标(x^ultrasonic,y^ultrasonic)对航迹推算定位模块融合的结果进行校准，校准后的坐标为(x₂,y₂)；

步骤十、采用径向基函数神经网络模型对定位结果(x₁,y₁)和(x₂,y₂)进行融合，得到最终精确度高且实时性好的定位坐标；

径向基函数神经网络模型为单隐层的三层前馈网络，设第一个神经网络的输出为最终的定位坐标；输入为x₁、x₂和影响因素d及t，共4个输入变量；隐含层节点个数为6个；

AGV在大部分路径中为匀速行驶，则航迹推算定位模块行驶中的累计误差与矫正前的累计行驶时间t有关；

红外图像定位模块的定位精度与距离参考标签中心的距离d有关，即距离中心越近得到的数据可信度越高；

6.如权利要求5所述的一种基于多传感器融合的无轨导航AGV定位方法，其特征在于，所述的步骤三中，对原始数据进行滤波处理；具体为：

判断数据接收时间是否大于设定阈值，如果是，则判定这类原始数据是错误数据，由于场地内红外干扰或自身发射的红外线反射多次返回摄像头的干扰造成的；利用多次定位数据的线性拟合推算出此次测量的估计值，来替代上述错误数据；否则，判定这类原始数据为可用数据。

7.如权利要求5所述的一种基于多传感器融合的无轨导航AGV定位方法，其特征在于，步骤五中所述的修正的具体过程如下：

步骤501、在最邻近红外传感器的第i个电子标签辐射范围内，将安装好红外传感器的AGV原地旋转360°，测得不同角度j下的坐标(x_i0j,y_i0j)，并在坐标系中形成图形的几何中心(X_i0,Y_i0)；

步骤502、将不同角度下的各坐标分别和几何中心作差，求得不同角度下的坐标偏差δx_i0j和δy_i0j，经过多次试验，求得不同偏航角下的平均偏差

步骤503、利用平均偏差

修正最终的车体在世界坐标系中的坐标；

修正后的车体坐标(x^infrared',y^infrared')如下：

步骤504、判断是否需要对修正后的车体坐标进行滤波处理；

判断过程为：

设AGV最高行驶速度为V_max，当Dis(P_i,P_i-1)-V_maxgΔt＞0时，采用之前多次定位数据的线性拟合推算出的估计值，来替代此次修正后的车体坐标结果；否则，不需要进行滤波处理；

其中，Dis(P_i,P_i-1)表示此次测得的位置和上一次测得的位置的欧氏距离，Δt为两次测量的时间间隔。

8.如权利要求5所述的一种基于多传感器融合的无轨导航AGV定位方法，其特征在于，步骤六中所述的航迹推算定位模块获取的数据包括：从IMU获取AGV的统加速度、角速度、磁感应强度和温度，从两个辅助编码器获得两驱动轮轴转过的角度和转动的速度。

9.如权利要求5所述的一种基于多传感器融合的无轨导航AGV定位方法，其特征在于，所述的步骤九具体为：

超声波发射端安装在AGV行驶路径的工位点O处，每到达工位点O时进行一次校准，即使用局部超声波网络定位得到的车体中心的全局坐标替换航迹推算得到的AGV的位置向量，替换后结果(x₂,y₂)。

Images