CN112581616B - 基于序贯分块滤波的最近邻ukf-slam方法 - Google Patents

Abstract

本发明公开了一种基于序贯分块滤波的最近邻UKF‑SLAM方法，本发明为了研究基于滤波器SLAM的建图逻辑，在无迹卡尔曼滤波器(UKF)的基础上，区别于传统UKF‑SLAM使用增广矩阵的做法，对量测信息进行最近邻数据关联之后，利用每一个地标量测对机器人依次进行序贯分块滤波，实现了基于序贯分块滤波的最近邻UKF‑SLAM方法。本发明配置结构明了，计算方便，鲁棒性强，可广泛应用于激光SLAM领域。

Description

基于序贯分块滤波的最近邻UKF-SLAM方法

技术领域

本发明涉及SLAM(simultaneous localization and mapping)，也称CML(Concurrent Mapping and Localization)，即时定位与地图构建，或称并发建图与定位领域，涉及一种基于序贯分块滤波的SLAM方法，缓解基于UKF-SLAM的计算复杂问题。

背景技术

SLAM问题最初是由Smith Self和Raja Chatila等科学家在1986年提出的。Durrant提出了关于SLAM问题的基本框架，并证明了基于该框架的SLAM系统时收敛的，这对机器人技术的发展具有重要意义。此后多种解决SLAM问题的方案被提出。现有解决方案中，基于贝叶斯理论的SLAM方法是比较成功且鲁棒性较高的方案。SLAM方法可以分为基于随机向量建模和随机有限集建模两大类。根据贝叶斯估计的原理不同SLAM方法主要分为基于贝叶斯滤波估计和基于贝叶斯平滑估计两种类型。基于贝叶斯滤波估计的SLAM方法用到的滤波技术主要包括高斯滤波器(Gaussian Filter，GF),粒子滤波器(Particle Filter，PF)和概率假设密度滤波器(Probability Hypothesis Density，PHD)。基于贝叶斯平滑估计的SLAM方法主要是用因子图表示某一时间段内的机器人位姿及路标特征，并利用非线性优化的方法来处理。

在传统的基于扩展卡尔曼滤波器的SLAM方法中，当运动模型和观测模型的非线性程度较高时，由一阶泰勒级数展开线性近似引入的累积误差会严重影响系统状态的估计结果。通过改进标准扩展卡尔曼滤波器中线性化点的选择方法，陆续出现了一些基于改进的扩展卡尔曼滤波器的SLAM方法，包括基于迭代扩展卡尔曼滤波器的SLAM方法和基于均值扩展卡尔曼滤波器的SLAM方法。另一方面，在采用一阶泰勒级数展开来近似非线性函数时需要计算函数在线性化点处的雅克比矩阵，而对于一些具有复杂模型的非线性系统而言，雅克比矩阵的计算十分困难。近年来，一类根据特定采样规则选取确定性样本点并采用统计线性化方式来近似非线性系统的滤波器受到了广泛关注。这类被统称为Sigma点卡尔曼滤波器包括：无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)、容积卡尔曼滤波器(Cubature Kalman Filter,CKF)、中心差分卡尔曼滤波器(Central Difference KalmanFilter,CDKF)、以及高斯埃尔米特求积分卡尔曼滤波器(Gauss-Hermite QuadratureKalman Filter,GHQKF)等。由于Sigma点卡尔曼滤波方法中不需要计算雅克比矩阵，并且能够将非线性函数的近似逼近到二阶以上的估计精度，因此各种基于Sigma点卡尔曼滤波器的SLAM方法被陆续提出。此外，为了保持系统状态协方差矩阵在递归估计过程中的对称性与半正定性，相应地出现了基于平方根Sigma点卡尔曼滤波器的SLAM方法。在这些方法中，系统状态协方差的平方根因子被直接通过非线性运动模型和观测模型进行传播和更新，进而大幅度强化了方法的数值稳定性，从而在一定程度上提高了对机器人位姿及路标位置估计的精确性。

发明内容

针对传统的基于无迹卡尔曼滤波器的SLAM采用增广矩阵，随着地标点的增多计算量增大，计算复杂的问题，本发明提出了一种基于序贯分块滤波融合的单传感器最近邻UKF-SLAM方法，该方法可以在避免部分随着地图增加而增加的计算量，且计算模式简介不会随着地标点的增多而复杂。为了达到上述目的，本发明采用技术方案如下：

(1)构建单机器人SLAM场景，并对目标的运动模型进行初始化，设置目标运动的相关参数，包括目标运动的过程噪声和传感器的量测噪声；

(2)机器人位姿和地图的一步预测部分。

(3)最近邻数据关联部分。

(4)关联数据预处理部分；

(5)机器人UKF序贯分块滤波更新，地图UKF更新部分；

(6)将步骤(5)最终的结果作为下一时刻的输入，重复步骤(2)到步骤(6)，迭代完所有时刻，得到最终的估计结果。

本发明的有益效果：针对UKF-SLAM随着地图增多，增广矩阵逐渐增大，计算复杂度上升的问题，本发明采用序贯分块滤波的方式，对位姿更新采用序贯分块滤波代替高维增广矩阵，实现了基于序贯分块滤波的单传感器最近邻UKF-SLAM方法。本发明配置结果明了，结构简单，计算方便，可广泛应用于基于滤波器的SLAM领域。

附图说明

图1基于序贯分块滤波最近邻SLAM方法框图；

图2序贯分块滤波方法流程框图；

图3机器人雷达视域图；

图4基于序贯分块滤波最近邻SLAM方法建图结果；

具体实施方式

以下结合技术方案和附图，详细叙述本发明的具体实施方式。

如图1所示，基于序贯分块滤波的单传感器最近邻UKF-SLAM方法该方法，具体包括以下步骤：

(1)构建单机器人密集地图SLAM场景，并对机器人的运动模型进行初始化，设置机器人运动的相关参数，包括机器人运动的过程噪声和量测噪声；其中机器人的量测来自地标；

建立目标的运动模型：x_k+1＝f_k,k+1(x_k)+w_k

式中，k表示离散时间变量，x_k表示机器人在k时刻的状态变量，w_k表示均值为零、方差为Q_k的高斯白噪声，映射f_k|k+1表示机器人从k时刻到k+1时刻状态转移的状态转移方程；机器人在k时刻的状态变量其中，(x_k,y_k)为k时刻机器人在监测空间中的位置分量，/>为k时刻机器人在监测空间中的速度分量；

采用特征地图的形式，假设k时刻特征地图中地标个数用N_k表示，所有地标的集合表示为其中m_i∈M_k是地标i的坐标向量，m_i＝[m_i,x,m_i,y]^T。假设环境中的地图是固定不变的模型，则地图的状态转移方程如下，

M_k+1＝I_k·M_k

其中，I_k是N_k维的单位方阵。

假设传感器位于机器人中心，为激光传感器，激光传感器视域FOV为d，对落入视域FOV范围内的所有地标产生观测数据。假设k时刻得到的观测数据集合为其中c_k是k时刻所接受到的观测数量。对于z_k,i∈Z_k，其数据形式为：/>

假设k时刻机器人的真实位姿为X_k＝[x_k,v_x,k,y_k,v_y,k]^T，地标m_i∈M_k的笛卡尔坐标为：m_i＝[m_i,x,m_i,y]^T，且地标落入探测范围则观测方程如下，

z_k,i＝h(m_i,X_k)+v_k,i

h(m_i,X_k)由下式导出：

其中，v_k,i＝[δ_r,δ_θ]^T为二维随机量测噪声，其中的分量均服从零均值高斯随机过程，其中δ_r表示距离噪声，δ_θ表示角度噪声。

(2)构建序贯分块滤波最近邻UKF-SLAM算法

如图2所示，假设k时刻机器人位姿和地图的后验估计分别为： 机器人位姿的后验协方差为/>地标/>的后验协方差为/>

1)预测逻辑

对于机器人的位姿和地图执行不同的预测逻辑。机器人位姿在k-1时刻后验位姿的基础上进行做UT变换一步外推，地图在k-1时刻估计的后验地图状态的基础上进行继承。

P(x_k,m|Z_0:k-1,U_0:k,x₀)＝∫P(x_k|x_k-1,u_k)×P(x_k-1,m|Z_0:k-1,U_0:k-1,x₀)dx_k-1

其中：x_k为k时刻机器人位姿，m为地图，Z_0:k-1为到k-1时刻的量测，U_0:k为控制，x₀为初始位姿，z_k为k时刻量测；P(x_k,m|Z_0:k-1,U_0:k,x₀)表示机器人初始位姿为x₀，控制变量为U_0:k，得到直到k-1时刻的量测Z_0:k-1对k时刻机器人位姿x_k以及地图m的预测；

地图状态一步预测：地图协方差一步预测：/>

2)最近邻数据关联逻辑

对k时刻机器人采集到的量测集合以机器人一步预测位姿为中心，解算出对应地图的坐标。以最近邻逻辑对解算出的地标集合与机器人一步预测位姿/>视域FOV内的地标结合进行关联，对两组地标关联之后，得到相对应的量测数据关联结果。关联之后把真实量测分为与已存在地图伪量测关联上的量测集合Z_k1和未能成功关联上的量测集合Z_k2。

3)关联数据预处理逻辑

把上一步与已存在地图伪量测关联上的真实量测Z_k1视为已存在地图的对应量测Z_kold即，

Z_kold＝Z_k1

把上一步未能成功关联的真实量测集合视为新生地图量测Z_knew即，

Z_knew＝Z_k2

对与已存在地图伪量测关联上的真实量测Z_kold进行筛选，先求出相匹配的真实量测和伪量测对应的笛卡尔坐标系坐标之间的欧式距离。对所有组欧式距离求取方差，假设量测集合中的各个量测是相互独立的并且只含随机误差，采用3σ原则对量测进行筛选，剔除错误关联的量测数据对机器人位姿更新和地图更新的影响。

3σ原则为：

误差欧式距离分布在(μ-σ,μ+σ)中的概率为0.6286；

误差欧式距离分布在(μ-2σ,μ+2σ)中的概率为0.9544；

误差欧式距离分布在(μ-3σ,μ+3σ)中的概率为0.9974。

其中：σ为方差，μ为均值。

所以可以认为，分布在(μ-3σ,μ+3σ)之间的误差欧式距离所对应的量测组合为正确关联的量测组合。对(μ-3σ,μ+3σ)之外的量测组合进行剔除处理。

4)更新逻辑

对于关联好的量测数据Z_kold、Z_knew采用不同的更新逻辑。对于被视为新生地标所对应的量测集合Z_knew，进行新生地标处理。以机器人一步预测位姿为中心，用新生地标量测Z_knew反解出新生地标的坐标M_new。

对于被视为老地标所对应的量测集合Z_kold，用来对关联上的后验地图进行更新。

对机器人的位姿和地图进行同步更新，同时更新其联合后验。即：

其中：x_k为k时刻机器人位姿，m为地图，Z_0:k为到k时刻的量测，U_0:k为控制，x₀为初始位姿，z_k为k时刻量测，P(x_k,m|Z_0:k,U_0:k,x₀)表示机器人初始位姿为x₀，控制变量为U_0:k，得到直到k时刻的量测Z_0:k对k时刻机器人位姿x_k以及地图m的更新。

对于地图和机器人位姿同步使用UKF进行更新：

继承k时刻机器人的位姿的预测状态向量和协方差估计/>

以正确关联的量测集合对机器人位姿和协方差进行序贯分块滤波更新：

1)量测组1对于机器人位姿的更新为：

2)剩余正确关联的量测组对机器人位姿的更新为：

其中，是k时刻第i组量测更新后的机器人后验位姿，/>为第k时刻第i组量测更新后的卡尔曼系数，/>为第k时刻第i组量测更新后机器人的后验协方差，/>表示第k时刻第i组量测与机器人位姿的映射关系,/>表示第k时刻第i组量测的观测误差协方差；/>表示机器人位姿在k时刻的一步预测；

3)k时刻最终机器人位姿更新为：

其中，N为机器人位姿序贯分块滤波更新的次数。

以正确关联的量测集合对地图和其协方差进行UKF滤波更新，更新后利用k时刻更新得到k时刻机器人视域FOV内已存在地图的后验估计把/>替换掉原地图中对应的地标特征得到M_old，k时刻最终地图更新为：

本发明方法激光雷达视域如图3，建图结果如图4所示。

所述的无迹卡尔曼滤波UKF算法的具体过程如下：

1)确定权值

λ＝α²(n+κ)-n

式中，n为状态的维数，α决定了σ点的散布程度，β来描述状态的分布情况，κ通常取0。

2)预测

对于给定的后验状态协方差P_k-1|k-1进行一步预测

其中：为σ点。

计算

其中：Q_k-1为过程误差协方差。为状态的一步预测，P_k|k-1为协方差一步预测。

用UT变换求σ点P_k|k-1通过量测方程的传播，即

其中为σ点。

计算输出的一步提前预测，即

其中为σ点，/>为伪量测，/>为伪量测协方差，/>为伪量测和估计状态的互协方差，R_k为量测噪声协方差。

3)获得量测z_k后滤波更新

其中，为k时刻状态后验估计，K_k是k时刻卡尔曼增益，P_k|k为k时刻后验协方差。

Claims (2)

1.基于序贯分块滤波的最近邻UKF-SLAM方法，其特征在于，该方法具体包括以下步骤：

(1)构建单机器人密集地图SLAM场景，并对机器人的运动模型进行初始化，设置机器人运动的相关参数，包括机器人运动的过程噪声和量测噪声；其中机器人的量测来自地标；

建立目标的运动模型：x_k+1＝f_k|k+1(x_k)+w_k

式中，k表示离散时间变量，x_k表示机器人在k时刻的状态变量，w_k表示均值为零、方差为Q_k的高斯白噪声，映射f_k|k+1表示机器人从k时刻到k+1时刻状态转移的状态转移方程；机器人在k时刻的状态变量其中，(x_k，y_k)为k时刻机器人在监测空间中的位置分量，/>为k时刻机器人在监测空间中的速度分量；

采用特征地图的形式，假设k时刻特征地图中地标个数用N_k表示，所有地标的集合表示为其中m_i∈M_k是地标i的坐标向量，m_i＝[m_i，x，m_i，y]^T；假设环境中的地图是固定不变的模型，则地图的状态转移方程如下，

M_k+1＝I_k·M_k

其中，I_k是N_k维的单位方阵；

假设传感器位于机器人中心，为激光传感器，对落入激光传感器视域FOV范围内的所有地标产生观测数据；假设k时刻得到的观测数据集合为其中c_k是k时刻所接受到的观测数量；对于z_k，i∈Z_k，其数据形式为：/>

假设k时刻机器人的真实位姿为X_k＝[x_k，v_x，k，y_k，v_y，k]^T，地标m_i∈M_k的笛卡尔坐标为：m_i＝[m_i，x，m_i，y]^T，且地标落入探测范围则观测方程如下，

z_k，i＝h(m_i，X_k)+v_k，i

h(m_i，X_k)由下式导出：

其中，v_k,i＝[δ_r,δ_θ]^T为二维随机量测噪声，其中的分量均服从零均值高斯随机过程，其中δ_r表示距离噪声，δ_θ表示角度噪声；

(2)构建序贯分块滤波最近邻UKF-SLAM算法

假设k时刻机器人位姿和地图的后验估计分别为： 机器人位姿的后验协方差为/>地标/>的后验协方差为/>1)预测逻辑

对于机器人的位姿和地图执行不同的预测逻辑；机器人位姿在k-1时刻后验位姿的基础上进行UT变换,一步外推，地图在k-1时刻估计的后验地图状态的基础上进行继承；

P(x_k,m|Z_0:k-1,U_0:k,x₀)＝∫P(x_k|x_k-1,u_k)×P(x_k-1,m|Z_0:k-1,U_0:k-1,x₀)dx_k-1

其中：x_k为k时刻机器人位姿，m为地图，Z_0:k-1为到k-1时刻的量测，U_0:k为控制，x₀为初始位姿；P(x_k,m|Z_0:k-1,U_0:k,x₀)表示机器人初始位姿为x₀，控制变量为U_0:k，得到直到k-1时刻的量测Z_0:k-1对k时刻机器人位姿x_k以及地图m的预测；

地图状态一步预测：地图协方差一步预测：/>

2)最近邻数据关联逻辑

对k时刻机器人采集到的量测集合以机器人一步预测位姿为中心，解算出对应地图的坐标；以最近邻逻辑对解算出的地标集合与机器人一步预测位姿视域FOV内的地标结合进行关联，对两组地标关联之后，得到相对应的量测数据关联结果；关联之后把真实量测分为与已存在地图伪量测关联上的量测集合Z_k1和未能成功关联上的量测集合Z_k2；

3)关联数据预处理逻辑

把上一步与已存在地图伪量测关联上的真实量测Z_k1视为已存在地图的对应量测Z_kold即，

Z_kold＝Z_k1

把上一步未能成功关联的真实量测集合视为新生地图量测Z_knew即，

Z_knew＝Z_k2

对与已存在地图伪量测关联上的真实量测Z_kold进行筛选，先求出相匹配的真实量测和伪量测对应的笛卡尔坐标系坐标之间的欧式距离；对所有组欧式距离求取方差，假设量测集合中的各个量测是相互独立的并且只含随机误差，采用3σ原则对量测进行筛选，剔除错误关联的量测数据对机器人位姿更新和地图更新的影响；

3σ原则为：

误差欧式距离分布在(μ-σ,μ+σ)中的概率为0.6286；

误差欧式距离分布在(μ-2σ,μ+2σ)中的概率为0.9544；

误差欧式距离分布在(μ-3σ,μ+3σ)中的概率为0.9974；

其中：σ为方差，μ为均值；

分布在(μ-3σ,μ+3σ)之间的误差欧式距离所对应的量测组合为正确关联的量测组合；对(μ-3σ,μ+3σ)之外的量测组合进行剔除处理；

4)更新逻辑

对于关联好的量测数据Z_kold、Z_knew采用不同的更新逻辑；对于被视为新生地标所对应的量测集合Z_knew，进行新生地标处理；以机器人一步预测位姿为中心，用新生地标量测Z_knew反解出新生地标的坐标M_new；

对于被视为老地标所对应的量测集合Z_kold，用来对关联上的后验地图进行更新；

对机器人的位姿和地图进行同步更新，同时更新其联合后验；即：

其中：x_k为k时刻机器人位姿，m为地图，Z_0:k为到k时刻的量测，U_0:k为控制，x₀为初始位姿，z_k为k时刻量测，P(x_k,m|Z_0:k,U_0:k,x₀)表示机器人初始位姿为x₀，控制变量为U_0:k，得到直到k时刻的量测Z_0:k对k时刻机器人位姿x_k以及地图m的更新；

对于地图和机器人位姿同步使用UKF进行更新：

继承k时刻机器人的位姿的预测状态向量和协方差估计/>

以正确关联的量测集合对机器人位姿和协方差进行序贯分块滤波更新：

1)量测组1对于机器人位姿的更新为：

2)剩余正确关联的量测组对机器人位姿的更新为：

其中，是k时刻第i组量测更新后的机器人后验位姿，/>为第k时刻第i组量测更新后的卡尔曼系数，/>为第k时刻第i组量测更新后机器人的后验协方差，/>表示第k时刻第i组量测与机器人位姿的映射关系,/>表示第k时刻第i组量测的观测误差协方差；/>表示机器人位姿在k时刻的一步预测；

3)k时刻最终机器人位姿更新为：

其中，N为机器人位姿序贯分块滤波更新的次数；

以正确关联的量测集合对地图和其协方差进行UKF滤波更新，更新后利用k时刻更新得到k时刻机器人视域FOV内已存在地图的后验估计把/>替换掉原地图中对应的地标特征得到M_old，k时刻最终地图更新为：

2.根据权利要求1所述的基于序贯分块滤波的最近邻UKF-SLAM方法，其特征在于：UKF算法的具体过程如下：

1)确定权值

λ＝α²(n+κ)-n

式中，n为状态的维数，α决定了σ点的散布程度，β来描述状态的分布情况，κ取0；

2)预测

对于给定的后验状态协方差P_k-1|k-1进行一步预测

其中：为σ点；

计算

其中：Q_k-1为过程误差协方差；为状态的一步预测，P_k|k-1为协方差一步预测；

用UT变换求σ点P_k|k-1通过量测方程的传播，即

其中为σ点；

计算输出的一步提前预测，即

其中为σ点，/>为伪量测，/>为伪量测协方差，/>为伪量测和估计状态的互协方差，R_k为量测噪声协方差；

3)获得量测z_k后滤波更新

其中，为k时刻状态后验估计，K_k是k时刻卡尔曼增益，P_k|k为k时刻后验协方差。