基于卡尔曼滤波的室内车辆定位方法

1.本发明涉及室内定位技术领域，特别是涉及一种基于卡尔曼滤波的室内车辆定位方法。


背景技术：

2.随着科技的进步发展，室内场景下的车辆服务也越来越普及到生产和生活中，例如，厂房中的轮式搬运机器人的调度，以及室内停车场中的车辆控制。为了使得室内场景的车辆更加自动化、智能化，其中非常关键的一环就是室内车辆定位。
3.然而，由于定位场景是在室内，卫星信号由于被墙体遮挡，难以接收，因此传统的高精度卫星定位技术就无法用于车辆定位了。在室内环境无法使用卫星定位时，使用室内定位技术作为卫星定位的辅助定位，解决卫星信号到达地面时较弱、不能穿透建筑物的问题，最终定位物体当前所处的位置。室内定位是指在室内环境中实现位置定位，主要采用无线通讯、基站定位、惯导定位、动作捕捉等多种技术集成形成一套室内位置定位体系，从而实现人员、物体、车辆等在室内空间中的位置监控。
4.对于室内车辆的自动化运行，提高定位精度势在必行。为了实现这一目的，通常在车辆或者定位场景中增加多组不同传感器或者采用高精度的定位传感器，但是，这会导致定位成本急剧升高，难以应用到生产和生活中。


技术实现要素：

5.基于此，有必要针对上述技术问题，提供一种定位精度较高、成本较低的基于卡尔曼滤波的室内车辆定位方法。
6.一种卡尔曼滤波的室内车辆定位方法，所述方法包括：
7.获取车辆在室内场景中imu和uwb传感器的量测信息，所述量测信息包括车辆的位置、速度、加速度和朝向角的测量值；
8.根据所述量测信息整定超参数矩阵，所述超参数矩阵包括量测噪声协方差矩阵的值和过程噪声协方差矩阵的值；
9.实时采集所述量测信息，并对所述量测信息中所述imu和uwb传感器的量测数据进行时空同步；
10.基于线性卡尔曼滤波，融合uwb和imu传感器的量测数据，结合所述超参数矩阵进行迭代运算，获得定位结果。
11.进一步的，所述获取车辆在室内场景中imu和uwb传感器的量测信息，之前包括：
12.在室内场景四周部署uwb传感器的anchor，并在车辆上安装uwb传感器的tag和imu传感器。
13.进一步的，所述根据所述量测信息整定超参数矩阵，包括：
14.将量测噪声协方差矩阵r设置为对角阵，在车辆静止时，分别测得uwb传感器沿着x、y轴的位置(x,y)和速度(v
x
，vy)对应的方差)对应的方差并将其填入所述量测
噪声协方差矩阵r主对角线的对应位置。
15.进一步的，所述根据所述量测信息整定超参数矩阵，还包括：
16.将过程噪声协方差矩阵q设置为对角阵，在车辆静止时，运行定位程序，调节主对角线的值，使得所述定位程序在终端显示的均方根误差最小，完成对过程噪声协方差矩阵q的整定。
17.进一步的，所述时空同步包括时间同步和空间同步，所述时间同步是指获取近似同一时刻的uwb和imu传感器的量测数据，并将其封装入同一数据帧中；所述空间同步是指统一uwb和imu传感器的位置坐标系。
18.进一步的，所述基于线性卡尔曼滤波，融合uwb和imu传感器的量测数据，结合所述超参数矩阵进行迭代运算，其迭代算法为：
[0019][0020]
p(k|k-1)＝fp(k-1|k-1)f
t
q
k-1
，
[0021]kk
＝p(k|k-1)h
t
[hp(k|k-1)h
t
rk]-1
，
[0022][0023]
p(k|k)＝(i-kkh)p(k|k-1)，
[0024]
式中，为车辆状态在k时刻的先验估计，车辆状态定义为系统矩阵f、输入矩阵b和输入向量u
k-1
设定为
[0025][0026]
式中，t为两个数据帧之间的时间间隔，分别为imu传感器测得的车辆沿着x、y轴的加速度值；分别为k-1、k时刻对协方差的后验估计，p(k|k-1)为k时刻的协方差先验估计；kk为卡尔曼增益，rk和qk分别为k时刻的超参数量测噪声协方差和过程噪声协方差矩阵，zk为k时刻的量测向量，量测矩阵h设定为单位阵。
[0027]
上述卡尔曼滤波的室内车辆定位方法，获取传感器关于车辆运动状态的量测数据，基于线性卡尔曼滤波原理，对uwb和imu传感器进行信息融合，从而对车辆进行实时的高精度定位。获取同步传感器数据，而后利用融合算法对当前数据帧进行迭代运算，实现对室内车辆的实时定位，有效地减少定位的误差，提高定位的精确度和准确度。无需通过增加多组传感器或者采用昂贵的高精度定位传感器，在控制成本的同时，充分发挥现有的传感器功能，实现“1 1》2”，提高定位精度。
附图说明
[0028]
图1为一个实施例的卡尔曼滤波的室内车辆定位方法流程图；
[0029]
图2为本实施例的方法测试结果图。
具体实施方式
[0030]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0031]
如图1所示，在一个实施例中，一种卡尔曼滤波的室内车辆定位方法，包括以下步骤：
[0032]
步骤s110，获取车辆在室内场景中imu和uwb传感器的量测信息。该量测信息包括车辆位置、速度、加速度和朝向角的测量值。将uwb(ultra wide band，超宽带)的传感器的anchor(锚节点)部署于室内场景中车辆运行区域的房间四角，并进行标定，在车辆上安装uwb传感器的tag(标签)和imu(inertial measurement unit，惯性测量单元)传感器，在车载电脑上安装传感器的驱动。实地测得的uwb和imu传感器的性能指标，获得量测信息。
[0033]
步骤s120，根据量测信息整定超参数矩阵。该超参数矩阵包括量测噪声协方差矩阵的值和过程噪声协方差矩阵的值。根据量测信息确定融合算法超参数量测噪声协方差矩阵r的值，而后整定超参数过程噪声协方差矩阵q的值，使得融合后的定位性能达到最佳。具体的，将量测噪声协方差矩阵r设置为对角阵，在车辆静止时，分别测得uwb传感器沿着x、y轴的位置(x,y)和速度(v
x
，vy)对应的方差并将其填入量测噪声协方差矩阵r主对角线的对应位置。将过程噪声协方差矩阵q设置为对角阵，在车辆静止时，运行定位程序，调节主对角线的值，使得定位程序在终端显示的均方根误差最小，完成对过程噪声协方差矩阵q的整定。
[0034]
步骤s130，实时采集量测信息，并对量测信息中所述imu和uwb传感器的量测数据进行时空同步。通过数据采集程序实时采集量测信息，实现imu和uwb传感器数据同步。时空同步包括时间同步和空间同步，时间同步是指获取近似同一时刻的uwb和imu传感器的量测数据，并将其封装入同一数据帧中。空间同步是指统一uwb和imu传感器的位置坐标系。
[0035]
步骤s140，基于线性卡尔曼滤波，融合uwb和imu传感器的量测数据，结合超参数矩阵进行迭代运算，获得定位结果。使用线性卡尔曼滤波迭代算法结合车辆的运动模型进行数据融合。其迭代算法如下：
[0036][0037]
p(k|k-1)＝fp(k-1|k-1)f
t
q
k-1
，
[0038]kk
＝p(k|k-1)h
t
[hp(k|k-1)h
t
rk]-1
，
[0039][0040]
p(k|k)＝(i-kkh)p(k|k-1)，
[0041]
式中，为车辆状态在k时刻的先验估计，车辆状态定义为系统矩阵f、输入矩阵b和输入向量u
k-1
设定为
[0042][0043]
式中，t为两个数据帧之间的时间间隔，分别为imu传感器测得的车辆沿着x、y轴的加速度值。分别为k-1、k时刻对协方差的后验估计，p(k|k-1)为k时刻的协方差先验估计。kk为卡尔曼增益，rk和qk分别为k时刻的超参数量测噪声协方差和过程噪声协方差矩阵，zk为k时刻的量测向量，量测矩阵h设定为单位阵。
[0044]
本发明获取提前部署标定的传感器对于车辆运动状态的量测数据，基于线性卡尔曼滤波原理，对uwb和imu传感器进行信息融合，从而对车辆进行实时的高精度定位。本发明在完成两步准备工作后，首先获取同步传感器数据，而后利用融合算法对当前数据帧进行迭代运算，实现对室内车辆的实时定位。在实验室环境下，对本发明的定位性能进行测试得出，与单一uwb传感器相比，本发明能够较为有效地减少定位的均方根误差(drms)，提高定位的精确度和准确度，达到厘米级的定位精度。无需通过增加多组传感器或者采用昂贵的高精度定位传感器，在控制成本的同时，充分发挥现有的传感器功能，实现“1 1》2”，提高定位精度，进而提高控制效果和生产的安全性。
[0045]
以下结合具体场景说明：
[0046]
场景1：室内实验室环境下，对微缩车辆进行精确定位。
[0047]
步骤1：在车辆运行场景四周部署四个uwb anchor，并进行标定。在微缩车上部署uwb tag和imu传感器。
[0048]
步骤2：整定超参数。测试传感器对于x,y，v
x
，vy的量测方差，性能指标如下，
[0049][0050]
将其代入超参数r对应的主对角线中，得到而后整定超参数q，将车辆静止，开启本发明的定位程序，调整q的主对角线元素，使得定位
性能指标达到最小，在本发明实验室中，q的设置为(t为两个数据帧之间的时间间隔)。
[0051]
步骤3：在车辆运行时开启本发明的定位程序。该程序实时收集imu和uwb的量测数据(imu对车辆的加速度和朝向角的测量值、uwb对车辆的位置和速度的测量)，并对其进行时间空间同步，将处理后的数据帧发送给滤波程序。
[0052]
步骤4：滤波程序接收步骤3输出的数据，通过卡尔曼滤波的迭代算法实时输出小车的位置、速度、朝向角等后验估计数值，其中沿着x、y轴位置的后验估计值即本发明实时输出的最终定位结果。然后回到步骤3，循环执行步骤3、和步骤4。图2为本发明的测试结果，其中，灰色点线a为小车的运动轨迹，黑点b为传感器测得的小车位置，灰色点线c为经过本发明融合后的小车位置。
[0053]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。