买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：杭州电子科技大学

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

主权项：1.基于序贯分块滤波的最近邻UKF-SLAM方法，其特征在于，该方法具体包括以下步骤：1构建单机器人密集地图SLAM场景，并对机器人的运动模型进行初始化，设置机器人运动的相关参数，包括机器人运动的过程噪声和量测噪声；其中机器人的量测来自地标；建立目标的运动模型：xk+1＝fk|k+1xk+wk式中，k表示离散时间变量，xk表示机器人在k时刻的状态变量，wk表示均值为零、方差为Qk的高斯白噪声，映射fk|k+1表示机器人从k时刻到k+1时刻状态转移的状态转移方程；机器人在k时刻的状态变量其中，xk，yk为k时刻机器人在监测空间中的位置分量，为k时刻机器人在监测空间中的速度分量；采用特征地图的形式，假设k时刻特征地图中地标个数用Nk表示，所有地标的集合表示为其中mi∈Mk是地标i的坐标向量，mi＝[mi，x，mi，y]T；假设环境中的地图是固定不变的模型，则地图的状态转移方程如下，Mk+1＝Ik·Mk其中，Ik是Nk维的单位方阵；假设传感器位于机器人中心，为激光传感器，对落入激光传感器视域FOV范围内的所有地标产生观测数据；假设k时刻得到的观测数据集合为其中ck是k时刻所接受到的观测数量；对于zk，i∈Zk，其数据形式为：假设k时刻机器人的真实位姿为Xk＝[xk，vx，k，yk，vy，k]T，地标mi∈Mk的笛卡尔坐标为：mi＝[mi，x，mi，y]T，且地标落入探测范围则观测方程如下，zk，i＝hmi，Xk+vk，ihmi，Xk由下式导出： 其中，vk,i＝[δr,δθ]T为二维随机量测噪声，其中的分量均服从零均值高斯随机过程，其中δr表示距离噪声，δθ表示角度噪声；2构建序贯分块滤波最近邻UKF-SLAM算法假设k时刻机器人位姿和地图的后验估计分别为：机器人位姿的后验协方差为地标的后验协方差为1预测逻辑对于机器人的位姿和地图执行不同的预测逻辑；机器人位姿在k-1时刻后验位姿的基础上进行UT变换,一步外推，地图在k-1时刻估计的后验地图状态的基础上进行继承；Pxk,m|Z0:k-1,U0:k,x0＝∫Pxk|xk-1,uk×Pxk-1,m|Z0:k-1,U0:k-1,x0dxk-1其中：xk为k时刻机器人位姿，m为地图，Z0:k-1为到k-1时刻的量测，U0:k为控制，x0为初始位姿；Pxk,m|Z0:k-1,U0:k,x0表示机器人初始位姿为x0，控制变量为U0:k，得到直到k-1时刻的量测Z0:k-1对k时刻机器人位姿xk以及地图m的预测；地图状态一步预测：地图协方差一步预测：2最近邻数据关联逻辑对k时刻机器人采集到的量测集合以机器人一步预测位姿为中心，解算出对应地图的坐标；以最近邻逻辑对解算出的地标集合与机器人一步预测位姿视域FOV内的地标结合进行关联，对两组地标关联之后，得到相对应的量测数据关联结果；关联之后把真实量测分为与已存在地图伪量测关联上的量测集合Zk1和未能成功关联上的量测集合Zk2；3关联数据预处理逻辑把上一步与已存在地图伪量测关联上的真实量测Zk1视为已存在地图的对应量测Zkold即，Zkold＝Zk1把上一步未能成功关联的真实量测集合视为新生地图量测Zknew即，Zknew＝Zk2对与已存在地图伪量测关联上的真实量测Zkold进行筛选，先求出相匹配的真实量测和伪量测对应的笛卡尔坐标系坐标之间的欧式距离；对所有组欧式距离求取方差，假设量测集合中的各个量测是相互独立的并且只含随机误差，采用3σ原则对量测进行筛选，剔除错误关联的量测数据对机器人位姿更新和地图更新的影响；3σ原则为：误差欧式距离分布在μ-σ,μ+σ中的概率为0.6286；误差欧式距离分布在μ-2σ,μ+2σ中的概率为0.9544；误差欧式距离分布在μ-3σ,μ+3σ中的概率为0.9974；其中：σ为方差，μ为均值；分布在μ-3σ,μ+3σ之间的误差欧式距离所对应的量测组合为正确关联的量测组合；对μ-3σ,μ+3σ之外的量测组合进行剔除处理；4更新逻辑对于关联好的量测数据Zkold、Zknew采用不同的更新逻辑；对于被视为新生地标所对应的量测集合Zknew，进行新生地标处理；以机器人一步预测位姿为中心，用新生地标量测Zknew反解出新生地标的坐标Mnew；对于被视为老地标所对应的量测集合Zkold，用来对关联上的后验地图进行更新；对机器人的位姿和地图进行同步更新，同时更新其联合后验；即： 其中：xk为k时刻机器人位姿，m为地图，Z0:k为到k时刻的量测，U0:k为控制，x0为初始位姿，zk为k时刻量测，Pxk,m|Z0:k,U0:k,x0表示机器人初始位姿为x0，控制变量为U0:k，得到直到k时刻的量测Z0:k对k时刻机器人位姿xk以及地图m的更新；对于地图和机器人位姿同步使用UKF进行更新：继承k时刻机器人的位姿的预测状态向量和协方差估计 以正确关联的量测集合对机器人位姿和协方差进行序贯分块滤波更新：1量测组1对于机器人位姿的更新为： 2剩余正确关联的量测组对机器人位姿的更新为： 其中，是k时刻第i组量测更新后的机器人后验位姿，为第k时刻第i组量测更新后的卡尔曼系数，为第k时刻第i组量测更新后机器人的后验协方差，表示第k时刻第i组量测与机器人位姿的映射关系,表示第k时刻第i组量测的观测误差协方差；表示机器人位姿在k时刻的一步预测；3k时刻最终机器人位姿更新为： 其中，N为机器人位姿序贯分块滤波更新的次数；以正确关联的量测集合对地图和其协方差进行UKF滤波更新，更新后利用k时刻更新得到k时刻机器人视域FOV内已存在地图的后验估计把替换掉原地图中对应的地标特征得到Mold，k时刻最终地图更新为：

全文数据：

权利要求：

百度查询： 杭州电子科技大学 基于序贯分块滤波的最近邻UKF-SLAM方法