一种基于DBN和卡尔曼滤波算法的AGV定位算法的制作方法

本发明涉及机器人定位和深度学习领域，具体地说是一种基于DBN和卡尔曼滤波算法的AGV定位算法。

背景技术：

AGV(Automated Guided Vehicle)是指安装有电磁或光学等自动导引装置，能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。AGV以轮式移动方式为主，快速高效、相对于其他生产设备结构简单、可控性及安全性好等优点。一个完整的AGV涉及到机械设计、电子技术、自动控制、人工智能、信息处理、计算机、传感器、图像处理等多个学科。AGV的广泛应用使它成为现代物流、生产、自动化系统中的关键设备，与常用的其他设备相比，AGV的工作区域受到的场地空间的限制较小，所以工作环境的可选择弹性较大。这些优点在实际应用中，最能充分体现AGV的柔性特点，实现灵活高效的自动化生产。

目前AGV主要用于仓库、航天工程、工厂工业自动化生产、医院、气象及复杂系统安全控制等领域。

对AGV的位置进行估计是完成上述任务的前提，根据AGV工作环境，定位系统所采用的传感器不同，定位方式也不同。近几年来，随着传感器技术的发展，根据AGV工作环境的复杂程度，给AGV可配备不同种类和不同数量的传感器。由于AGV工作环境具有多样性，到目前为止仍没有一套完整的、具有良好环境适应能力的定位方法。所用传感器的特性决定了定位的方法，同时，传感器的性能也影响了AGV的定位精度。按传感器所获得的信息类型来分，定位可分为：惯性定位法、测距定位法和感应定位法等。

惯性定位法也叫航位推算法或计算定位法，根据车载传感器检测AGV的运动状态信息(如运动位移和速度等)来解算出其相对惯性坐标系中的相对位姿。优点是不依赖于任何外部信息，也不向外部辐射能量，独立性强，不受外界工作环境的限制，同时，由于数据更新率高、短期定位精度较高、稳定性较好，因而得到广泛的应用。但由于定位信息是相对量，故定位累积误差会随时间增长而增加，绝对定位精度差。最主要的是，一旦定位失败，系统自恢复能力较差，需定期修正系统的定位误差。

测距定位法测距定位法是通过车载传感器对环境中所设路标的探测来实现定位。它的基础是对环境的几何描述，通过获得环境中的一些物体或标志点相对AGV的距离和方位的量化描述，解释出AGV自身的当前位置。一般有：几何定位法、渡越时间测量法、焦点扫描法、返回信号强度测量法、相移测量法、干涉度量法等，所用的传感器通有摄像机、声纳、激光测距仪、近红外测距仪、超声波定位、雷达和GPS定位等。这种方法具有很好的环境适应能力，对所设置的标识物的安装位姿有一定的要求，如超声波传感器要求标识物的法线与其轴线间的夹角在±5°内时其测量的距离比较准确，激光测距仪要求所设定的标识物必须与所发出的激光束在同一高度内，具定位用的标识物不能少于四个，否则定位失效。

感应定位法感应定位法也是通过获取环境中设置的标志信息如磁性元件产生的磁场等实现对AGV的定位。通常用于感应定位的传感器有霍尔元件、触觉传感器阵列、光电开关、磁栅尺等。这种定位方法对环境中所设置的标识物要求连续，同时，AGV上的车载传感器必须能时时感应到标识物的信息。这种定位方式最大的缺点是标识物必须按AGV所需运动的路径设置，因此，当AGV的运动路径发生改变时，已设置好的标识物应跟着改变。AGV的运动灵活性差。

技术实现要素：

本发明提供一种基于DBN和卡尔曼滤波算法的AGV定位算法，以解决传统卡尔曼滤波算法在AGV定位过程中，由于外部环境多变所导致得滤波发散的问题。

本发明采取的技术方案是：包括下列步骤：

步骤1，采集多组AGV运动过程中的速度和加速度信息，通过卡尔曼滤波算法估计出AGV不同时刻的位置信息；

步骤2，根据步骤1中采集到的信息，建立卡尔曼滤波过程中新息方差理论值的和实际测量值间的匹配度变量In_k，训练DBN；

步骤3，将训练好的DBN和卡尔曼滤波算法相结合，当匹配度偏离1时，通过DBN产生的自适应调节因子，校准匹配度变量，保证卡尔曼滤波算法对AGV位置和速度的精准估计；

本发明所述步骤3的训练过程按下列步骤进行：

1)预训练：

分别单独无监督地训练每一层RBM(restricted Boltzmann machine)网络，确保特征向量映射到不同特征空间时，都尽可能多地保留特征信息，RBM的结构模型如图1所示。

2)微调：

在DBN的最后一层设置BP网络，接收RBM的输出特征向量作为它的输入特征向量，有监督地训练实体关系分类器，而且每一层RBM网络只能确保自身层内的权值对该层特征向量映射达到最优，并不是对整个DBN的特征向量映射达到最优，所以反向传播网络还将错误信息自顶向下传播至每一层RBM，微调整个DBN网络，RBM网络训练模型的过程可以看作对一个深层BP网络权值参数的初始化，使DBN克服了BP网络因随机初始化权值参数而容易陷入局部最优和训练时间长的缺点，DBN的结构模型如图2所示。

本专利的有益效果在于：在卡尔曼滤波算法中，噪声系统的统计特性极易随外部环境的改变而发生变化。当噪声系统统计特性发生变化时，会带来滤波效果降低甚至发散。对于此类问题，本发明提出了一种基于深度信念网络的自适应卡尔曼滤波算法。该算法根据卡尔曼滤波算法中新息序列理论方差和实际方差的一致程度，利用DBN获取自适应调节因子，对卡尔曼滤波器的噪声方差矩阵进行修正，保证了AGV运行过程中对位置的精确估计。

附图说明

图1是本发明RBM的结构模型图；

图2是本发明DBN的结构模型图。

具体实施方式

在AGV工作的过程中，常常将AGV上布置各种的传感器(加速度计、陀螺仪和电子罗盘等)，根据传感器获得的数据，通过使用卡尔曼滤波算法可以获得任意时刻的位置、速度和加速度等相关信息，卡尔曼是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式(只需要当前的一个测量值和前一个采样周期的预测值)，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。

本发明包括下列步骤：

1.根据AGV上传感器采集的数据，通过卡尔曼滤波算法估计出AGV不同时刻的位置信息；

首先，设AGV的动态系统和量测系统的方程如下：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1 (1)

Z_k＝H_kX_k+V_k (2)

式中：X_k是系统的n维状态向量，W_k是p维系统的动态噪声，Z_k是m维系统的动态噪声，Φ_k,k-1是系统的n×n维状态转移矩阵，Γ_k,k-1是系统的n×p维噪声输入矩阵，H_k是系统的m×n维观测矩阵，V_k是m维系统的量测噪声。其中，W_k和V_k是互不相关的零均值白噪声。

卡尔曼滤波器的基本方程主要由时间更新和状态更新两个过程组成.具体的过程如下：

时间更新过程：

状态更新过程：

P_k＝[I-K_kH_k]P_k,k-1 (7)

式中：是k时刻系统状态的估计值，是一步状态的预测值，P_k是k时刻系统状态的估计误差方差矩阵，P_k,k-1是一步状态的预测误差方差矩阵，K_k是滤波增益，Q是系统噪声的协方差矩阵，R是观测噪声的协方差矩阵；

从动态系统方程、量测系统方程以及卡尔曼滤波的基本方程可以看出，若要使用卡尔曼滤波器完成最优估计的相关工作，需要预先精准地确定动态系统的数学模型、系统噪声的统计特性和观测噪声的统计特性，并且在滤波过程中保持上述的参量保持不变，但是，在实际AGV的定位过程中，运动状态的突变和外界环境的干扰都会影响卡尔曼滤波的精度，甚至导致滤波算法的发散；

2.根据新息序列的一致程度训练DBN；

卡尔曼滤波算法中，新息定义如下：

在动态系统的数学模型确定时，则新息应该是零均值的白噪声，并且新息方差的实际测量值和理论值的比值等于1。

新息方差的理论值可以从卡尔曼滤波的基本方程中获得，具体如下：

由于新息的均值为零，可以得到新息方差的实际测量值如下：

式中，由最新的M个新息方差的算术平均值得到。M由经验根据具体情况而定，在滤波过程中主要起平滑作用。

为了更好的描述新息方差的理论值和实际测量值之间的一致程度，定义变量匹配度如下：

将In_k作为输入，训练DBN，DBN是是一个高度复杂的有向无环图，从结构上看，DBN由多层无监督的RBM网络和一层有监督的BP(back-propagation)网络组成，具体如图2所示。

其中，RBM是一种典型的神经网络，具体如图1所示，RBM由可视层V和隐含层H组成，W表示两层间的连接权值，各层内神经元节点没有连接，可视层和隐含层间神经元节点完全连接，RBM的训练过程相对比较容易，它通过无监督贪婪逐层方式去预训练获得生成模型的权值，首先，将可见层的特征值映射到隐含层，可见层通过隐含层进行重建，然后，将新构建的可见层的特征值再次映射到隐含层，隐含层重建为可见层。最后，重复上述步骤，直到训练结束；

DBN训练的过程包含预训练和微调两步，具体如下：

1)预训练

分别单独无监督地训练每一层RBM网络,确保特征向量映射到不同特征空间时，都尽可能多地保留特征信息；

2)微调

在DBN的最后一层设置BP网络，接收RBM的输出特征向量作为它的输入特征向量，有监督地训练实体关系分类器.而且每一层RBM网络只能确保自身层内的权值对该层特征向量映射达到最优，并不是对整个DBN的特征向量映射达到最优，所以反向传播网络还将错误信息自顶向下传播至每一层RBM，微调整个DBN网络；

3.将训练好的DBN和卡尔曼滤波算法相结合，当匹配度偏离1时，通过DBN产生的自适应调节因子，校准匹配度变量，保证卡尔曼滤波算法对AGV位置和速度的精准估计；

具体采用在线调整模型系统的噪声的协方差矩阵来改变卡尔曼的滤波增益，从而提升滤波器的性能。调整的方式如下：

式中，和是Q和R的自适应调节因子，Q_k和R_k是调节后的系统噪声和观测噪声的协方差矩阵.

本发明的性能可以通过如下的仿真进行说明：

根据实际情况下AGV的运动状况，建立AGV的运动系统方程和量测系统方程如下：

Z(k)＝X(k)+V(k) (16)

其中，T是采样周期，W(k)～N(0,0.01)是零均值的高斯白噪声；

V(k)＝[V₁(k),V₂(k)] (17)

其中，V₁(k)～N(0,0.09)和V₂(k)～N(0,0.04)是零均值的高斯白噪声；

X(k)＝[X₁(k),X₂(k)]^T (18)

其中，X₁(k)是位置值，X₂(k)是速度值。Z(k)是X(k)的测量值；

在使用卡尔曼滤波算法进行定位之前，首先必须对其进行初始化，初始化结果如下：

在生成自适应调节因子之前，需要收集大量的训练样本对深度信念网络进行训练，在本文中，T＝1s，训练样本数1000，DBN由两层的RBM和一层的BP网络组成；

为了验证DBNAKF的稳定性，在100秒后，V₁(k)被改变为0.64，V₂(k)被改变为0.25；在200秒后，V₁(k)被改变为2.25，V₂(k)被改变为1；

由于系统噪声的统计特性可以通过多次的测量获得并且不易改变，所以可以假定系统噪声的统计特性是恒定。所以，

在100秒和200秒后，测量噪声的统计特性发生了改变。使用KF将导致滤波发散，降低对AGV速度和位置估计的精确度。但是使用DBNAKF却依然可以对AGV速度和位置做出精确的估计。

位置和速度的估计误差的均值和方差如表1所示。

表1位置和速度估计误差的均值和方差统计表