CN114353786A - 一种基于改进卡尔曼滤波器的无人矿卡融合定位方法 - Google Patents

Abstract

本发明公开了一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，包括基于组合导航定位法获取定位数据作为控制量；以及基于环境特征匹配法获取定位数据作为观测量；将控制量和观测量同时导入基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位，得到融合后的定位结果作为当前的定位数据。本发明通过改进现有的卡尔曼滤波的状态模型与测量模型，将传统的组合导航定位作为系统的控制量，环境特征匹配法作为观测量，在一维条件下实现上述两种定位方法的融合。从而能够进行具有更高的定位精度、更低的成本和更高的稳定性的融合定位，对提升无人矿卡的智能驾驶性能具有重要的意义。同时解决了单一定位方法容易受到天气、建筑等外界干扰。

Description

一种基于改进卡尔曼滤波器的无人矿卡融合定位方法

技术领域

本发明涉及自动驾驶和融合定位技术领域，特别涉及一种基于改进卡尔曼滤波器的无人矿卡融合定位方法。

背景技术

随着露天矿山作业方式向着自动化、智能化以及无人化方向发展，为了能在这种环境下执行任务，实现无人矿车的自主导航、路径规划以及运动控制，连续精确的定位显得尤为重要。目前国内外关于无人矿车的定位主要采用全球导航卫星系统(GlobalNavigation Satellite System,GNSS)与惯性导航系统(Inertial Navigation System,INS)的传统组合导航定位方法，由其提供高精度的全局车辆定位。

现有技术的不足之处在于，露天矿山环境GNSS信号容易丢失，基于GNSS/INS组合导航系统的车辆定位性能会迅速下降。因此，缺失GNSS信号条件下的低速无人车定位有问题。同时，环境特征匹配法作为无人驾驶的定位方法，单一传感器的定位精度、适用范围以及可靠性毕竟有限。考虑到多种传感器定位互补性与冗余可靠性提升等因素，基于多传感器环境特征匹配的定位技术受到无人驾驶领域的广泛关注。其中，激光雷达广泛使用于自动驾驶车辆中，测距精度高，但是价格昂贵，且使用寿命尚没有达到量产标准。相反，通过视觉进行定位具有很多优点，是一种低成本高性能的导航方案。近些年，基于摄像头的视觉状态估计技术发展迅速，科研人员提出了多种采用单目与双目相机定位算法。其中单目相机不能直接获得深度信息，需要复杂的初始化过程，双目立体视觉根据图像特征计算视差得到目标的深度信息，需要高性能处理器，实时性较低。然而，由于无人车车载计算平台的计算能力有限。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，以解决上述背景技术中提出的问题。

一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，包括：

基于组合导航定位法获取定位数据作为控制量；以及

基于环境特征匹配法获取定位数据作为观测量；

将控制量和观测量同时导入基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位，得到融合后的定位结果作为当前的定位数据。

作为本发明的进一步的方案：所述基于组合导航定位法获取定位数据作为控制量的具体步骤包括：

根据组合导航定位法采集定位数据u，并对定位数据u进行误差处理，定位数据u满足于：

其中，

式中，U(k)为无人车实际定位值，k为获取次数，E为第k-1次获取时的误差；

根据连续两次定位数据相减可以消除误差E，得到状态模型为：

U(k)＝U(k-1)+Δu(k|k-1)+Q(k)；

式中，Δu(k|k-1)表示无人车定位数据的变化趋势，Q(k)为过程激励噪声协方差。

作为本发明的进一步的方案：所述基于环境特征匹配法获取定位数据作为观测量的具体步骤包括：

根据环境特征匹配法采集定位数据u作为卡尔曼滤波器的测量模型的输入，测量模型表示为：u(k)＝U(k)+R；

式中，R为测量噪声协方差。

作为本发明的进一步的方案：所述基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位的具体步骤包括：

根据状态模型与测量模型，进行状态与测量的迭代更新；

首先将模型代入卡尔曼滤波器的状态更新方程与测量更新方程；

状态更新方程：

测量更新方程：

将卡尔曼滤波器的输出u(k)作为状态模型与测量模型的U(k)输出；

根据状态变量x(k|k)作为第k次融合的结果，得到x(k|k)＝u(k)；

由于状态变量能够直接反映测试结果，则系统参数A，B和H为1；

根据控制量u(k-1)由u转化，将状态模型代入状态更新方程，得到u(k-1)＝Δu(k|k-1)；

同时，取观测变量z(k)＝u(k)，并且对状态更新方程与测量更新方程进行迭代，使x(k|k)收敛至最小方差，得到数据融合后的结果u_i+1(k)比u_i(k)和u_i-1(k)更接近当前的实际定位值U(k)。

与现有技术相比，本发明存在以下技术效果：

通过采用上述的技术方案，利用改进现有的卡尔曼滤波的状态模型与测量模型，将传统的组合导航定位法的定位数据作为系统的控制量，环境特征匹配法的定位数据作为观测量，在一维条件下实现上述两种定位方法的融合。从而能够进行具有更高的定位精度、更低的成本和更高的稳定性的融合定位，对提升无人矿卡的智能驾驶性能具有重要的意义。同时解决了单一定位方法容易受到天气、建筑等外界干扰。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的一些实施例的无人矿卡融合定位方法的步骤示意图；

图2为本申请公开的一些实施例的无人矿卡融合定位方法的流程框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1和2，本发明实施例中，一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，包括：

S1、基于组合导航定位法获取定位数据作为控制量，具体步骤包括：

利用组合导航定位系统中的一个观测量去模拟控制量，即该观察量实际并没有参与对测量更新方程的更新，而是直接用于状态更新方程，因此该观测量必须能准确反映真实定位值的变化趋势，同时其测量噪声将作为过程激励噪声影响卡尔曼滤波器更新。因此在二观测量融合时，选择一个变化趋势接近实际定位真实值，同时用噪声较小的观测量来模拟卡尔曼滤波器的控制量，才能达到本文中所设计的改进数据融合模型的最佳效果。

根据GNSS与IMU工作原理，传统组合导航定位法的结果u可用于上述条件。因此，只需对u进行适当变化，从而使其转换为系统的控制量，即可完成滤波融合；

由于无人定位数据，其定位数据U变化曲线是连续的，即U＝U(t)。然而，在车载计算平台获取定位数据时，由于计时器和转换器的采样限制，实际获取的数据是离散的。因此，采用传统组合导航定位法需要满足下述条件；

根据组合导航定位法采集定位数据u，并对定位数据u进行误差处理，定位数据u满足于：

其中，

式中，U(k)为无人车实际定位值，k为获取次数，E为第k-1次获取时的误差；

由于存在较大误差，无法直接使用，但在同一定位测量系统中，短时间内获取定位数据应具有相同的误差，可通过连续两次定位数据相减可以消除误差。

根据连续两次定位数据相减可以消除误差E，得到状态模型为：

U(k)＝U(k-1)+Δu(k|k-1)+Q(k)；

式中，Δu(k|k-1)表示无人车定位数据的变化趋势，Q(k)为过程激励噪声协方差。

具体的，Δu(k|k-1)表示无人车定位数据的变化趋势，根据预测当前的实际定位值U(k)，这样将传统组合导航定位法的结果u从观测量转化为控制量，从而实现对卡尔曼滤波器的降维处理。同时，Q(k)为过程激励噪声协方差，是传统组合导航定位法的测量噪声，但Q(k)对结果影响较小，在车载计算平台系统资源紧张的情况下可以选择忽略。

S2、基于环境特征匹配法获取定位数据作为观测量，具体步骤包括：

本实施例中，卡尔曼滤波融合的测量模型通过参考环境特征匹配法获取结果。测量过程同样是离散的，获取的结果u可直接反映总定位数据；

根据环境特征匹配法采集定位数据u作为卡尔曼滤波器的测量模型的输入，测量模型表示为：u(k)＝U(k)+R；

式中，R为测量噪声协方差。本实施例中，环境特征匹配法噪声较大，R为不可忽略。

S3、将控制量和观测量同时导入基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位，得到融合后的定位结果作为当前的定位数据。

具体实施步骤中，所述基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位的具体步骤包括：

根据状态模型与测量模型，进行状态与测量的迭代更新；

首先将模型代入卡尔曼滤波器的状态更新方程与测量更新方程；

状态更新方程：

测量更新方程：

将卡尔曼滤波器的输出u(k)作为状态模型与测量模型的U(k)输出；

根据状态变量x(k|k)作为第k次融合的结果，得到x(k|k)＝u(k)；

由于状态变量能够直接反映测试结果，则系统参数A，B和H为1；

根据控制量u(k-1)由u转化，将状态模型代入状态更新方程，得到u(k-1)＝Δu(k|k-1)；

同时，取观测变量z(k)＝u(k)，并且对状态更新方程与测量更新方程进行迭代，使x(k|k)收敛至最小方差，得到数据融合后的结果u_i+1(k)比u_i(k)和u_i-1(k)更接近当前的实际定位值U(k)。

本实施例中，根据上述状态模型与测量模型，即可进行状态与测量的迭代更新。将模型代入卡尔曼滤波器状态更新方程与测量更新方程，对于状态模型与测量模型公式中的U(k)应用滤波器输出u(k)替代。状态变量x(k|k)为第k次融合的结果，即x(k|k)＝u(k)。

由于测量系统状态变量直接反映测试结果，所以系统参数A，B和H为1。控制量u(k-1)参考前面所述由u进行转化，将状态模型公式代入状态更新方程可得u(k-1)＝Δu(k|k-1))。观测变量z(k)＝u(k)。对状态更新方程与测量更新方程进行迭代，使x(k|k)快速收敛至最小方差，这样数据融合后的结果u_i+1(k)比u_i(k)和u_i-1(k)将更接近当前实际定位值U(k)。

最后通过上述模型，在系统存在两个或者多个定位值是，通过将传统组合导航定位法的结果转化为控制量，环境特征匹配法的结果作为观测量，能够实现一维卡尔曼滤波器对两个或者多个定位值的融合。

有益效果：

通过对两种无人车定位方法进行融合，解决了单一定位数据的误差和噪声的问题。与与现有技术相比，在GNSS信号丢失时能够有效地实时估计无人矿车位姿与定位信息。同时，通过精简数据融合时的复杂计算。利用改进的卡尔曼滤波的状态模型与测量模型，将传统组合导航定位法的定位数据作为系统的控制量，环境特征匹配法的定位数据作为观测量，在一维条件下实现上述两种定位方法的融合。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，其特征在于，包括：

基于组合导航定位法获取定位数据作为控制量；以及

基于环境特征匹配法获取定位数据作为观测量；

将控制量和观测量同时导入基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位，得到融合后的定位结果作为当前的定位数据。

2.根据权利要求1所述一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，其特征在于，所述基于组合导航定位法获取定位数据作为控制量的具体步骤包括：

根据组合导航定位法采集定位数据u，并对定位数据u进行误差处理，定位数据u满足于：

其中，

式中，U(k)为无人车实际定位值，k为获取次数，E为第k-1次获取时的误差；

根据连续两次定位数据相减可以消除误差E，得到状态模型为：

U(k)＝U(k-1)+Δu(k|k-1)+Q(k)；

式中，Δu(k|k-1)表示无人车定位数据的变化趋势，Q(k)为过程激励噪声协方差。

3.根据权利要求1所述一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，其特征在于，所述基于环境特征匹配法获取定位数据作为观测量的具体步骤包括：

根据环境特征匹配法采集定位数据u作为卡尔曼滤波器的测量模型的输入，测量模型表示为：u(k)＝U(k)+R；

式中，R为测量噪声协方差。

4.根据权利要求1至3任一项所述一种基于改进卡尔曼滤波器的无人矿卡融合定位方法，其特征在于，所述基于改进卡尔曼滤波器的状态模型和测量模型进行融合定位的具体步骤包括：

根据状态模型与测量模型，进行状态与测量的迭代更新；

首先将模型代入卡尔曼滤波器的状态更新方程与测量更新方程；

状态更新方程：

测量更新方程：

将卡尔曼滤波器的输出u(k)作为状态模型与测量模型的U(k)输出；

根据状态变量x(k|k)作为第k次融合的结果，得到x(k|k)＝u(k)；

由于状态变量能够直接反映测试结果，则系统参数A，B和H为1；

根据控制量u(k-1)由u转化，将状态模型代入状态更新方程，得到u(k-1)＝Δu(k|k-1)；

同时，取观测变量z(k)＝u(k)，并且对状态更新方程与测量更新方程进行迭代，使x(k|k)收敛至最小方差，得到数据融合后的结果u_i+1(k)比u_i(k)和u_i-1(k)更接近当前的实际定位值U(k)。

Images