一种同步定位与地图构建和路径规划方法技术

本发明专利技术提供了一种同步定位与地图构建和路径规划方法；包括：步骤1，同步定位与地图构建；利用改进的EKF SLAM算法对物流仓进行环境地图构建与同步定位，获取物流仓的现场物理数据；步骤2，利用改进的路径规划算法，对巡检目标进行路径规划，提供最优路径，使得巡检机器人能够在最短时间内到达目标点；步骤3，在巡检过程中，判断现场环境是否存在温度异常物品；步骤4，进行检测，判断物流仓是否有可燃性气体，是否存在安全隐患；步骤5，进行信息采集。本发明专利技术一种16邻域路径规划算法，引入了16邻域的搜索方法，解决了搜索维度少而造成的路径次优问题，该方法更加平滑，更加接近实测最优路径。更加接近实测最优路径。更加接近实测最优路径。

【技术实现步骤摘要】
一种同步定位与地图构建和路径规划方法


[0001]本专利技术属于机器人
；尤其涉及一种同步定位与地图构建和路径规划方法。

技术介绍

[0002]目前，随着电子商务的飞速发展，物流仓库的规模和吞吐量在显著增大，为了节约存储空间并进行高效管理，物流仓库需要将货物摆放的密集且规则。如此高密度的货物存储，物流仓安全问题已成为重中之重，对物流仓库管理提出了新挑战。目前包括中国、日本、加拿大等多个国家都在进行针对以物流仓为应用背景的智能巡检机器人研究和推广。物流仓巡检机器人旨在将移动机器人应用于无人值守或少人值守的物流仓场景。以移动机器人为平台，搭载多种智能化检测装备，对仓库现场信息进行及时准确的检测，上传云平台及时感知物流仓的安全隐患，以便于对检测现场下达及时有效的处理指令。保证物流仓储安全稳定的运行。
[0003]物流仓为追求空间的最大化利用，通常会高密度的存储货物，因此仓库安防成为不可忽视的一环。现有的物流仓监测一般通过室内加装多个摄像头和人工巡检的方式组合进行。该方法存在经济开支大、人力资源浪费、检测精度不够及响应不及时等问题。研究设计一款通过携带智能化检测装备对周围环境进行自主感知巡检机器人。该机器人能够应对复杂昏暗现场环境，合理规划路径，不间断地负责对物流仓内可燃性气体检测、红外热图采集、远程视屏监控等进行巡检，基于智能算法自动识别设备的状态信息，从而取代人工对物流仓库进行设备检测和预警分析的功能。
[0004]SLAM的主要任务是对机器人的位置估计信息进行更新。由于仅单一的通过运动信息推算出来的位置信息存在精度不高的问题，通过引入观测信息，滤波的作用是将两者信息结合起来赋给不同的权重来修正预测值。由于扩展卡尔曼滤波在处理非线性问题时，对非线性函数进行近似化处理，即仅保留泰勒展开的一阶项，这就给估计带来了很大的误差。要实现EKF滤波后的结果更加接近真实值，要求系统的状态方程和观测方程都接近线性且连续。
[0005]EKF SLAM算法进行理论推导如下：
[0006](1)定义二维EKF SLAM的状态变量
[0007]状态变量定义为：
[0008]X＝(φ,X
r
,X1,
…
,X
N
)
ꢀꢀꢀ
(3.4)
[0009]式中，φ代表机器人的方向，X
r
＝(x
r
,y
r
)代表机器人当前位置，X1＝(x1,y1),
…
X
N
＝(x
N
,y
N
)代表机器人观测到地图上的地标点位置信息。将机器人的方向φ分开标注的原因是其在收敛性和一致性分析上有着重要的作用。
[0010](2)预测
[0011]①
过程模型：式(3.5)表示机器人的过程模型：
[0012][0013]简写为：
[0014][0015]式中，v，r代表控制输入，δ
γ
，δ
v
是关于v，r均值为零的高斯白噪声，T代表机器人每移动一步的时间间隔，函数f
φ
的显示公式依据于移动机器人。
[0016]以简单离散时间机器人为例进行分析。机器人运动模型如下所示：
[0017][0018]式中，v为速率，γ为转率。
[0019]地标的运动模型：
[0020]X
i
(k+1)＝X
i
(k),i＝1,
…
,N
ꢀꢀꢀ
(3.8)
[0021]系统模型表达公式如下：
[0022]X(k+1)＝F(X(k),γ(k),v(k),δ
γ
,δ
v
)
ꢀꢀꢀ
(3.9)
[0023]式中，F为公式(3.6)和公式(3.8)的结合。
[0024]②
预测：假设k时刻状态变量的最有估计值为：
[0025][0026]矩阵P(k|k)为估计误差的协方差阵，预测过程如下：
[0027][0028]式中，Σ为噪声(δ
γ
,δ
v
)的协方差矩阵，的定义为：
[0029][0030]式中，为当前估计值处机器人位姿(φ,X
r
)，为处控制噪声(δ
γ
,δ
v
)雅可比矩阵。
[0031]根据上式(3.6)所定义的系统，机器人位姿的雅可比矩阵表达式如下：
[0032][0033](3)更新
[0034]①
测量模型：在k+1时刻，机器人的所获取得到的当前时刻地标信息包含γ
i
和θ
i
，γ
i
为测量距离，θ
i
为转向角，具体的表达式如下：
[0035][0036]式中，和为测量噪声。
[0037]观测模型为：
[0038][0039]式中，为高斯白噪声。
[0040]②
更新：对协方差矩阵进行更新：
[0041][0042]式中，Ω(
·
)是信息矩阵，Ω
new
是从观测值获得的信息矩阵：
[0043][0044]式中，是在当前估计值处计算的关于函数H
i
的雅克比矩阵。为噪声的协方差矩阵的逆。
[0045]更新状态变量估计值表达式如下：
[0046][0047][0048][0049][0050]估计误差协方差阵表达式如下：
[0051][0052]以上推导则为典型的EKF更新方式。
[0053]测量函数H的雅克比矩阵为：
[0054][0055]式中，
[0056][0057]从上式推导公式可以得出，雅可比矩阵仅包含当前时刻所观测到的点，观测到的地标点不包含其中。
[0058]由于扩展卡尔曼滤波在处理非线性问题时，对非线性函数进行近似化处理，即仅保留泰勒展开的一阶项，这就给估计带来了很大的误差。要实现EKF滤波后的结果更加接近真实值，要求系统的状态方程和观测方程都接近线性且连续。针对于EKF这一问题，本专利技术提出一种对非线性系统的前二阶矩进行线性近似滤波的方法。在扩展卡尔曼滤波的基础上，对状态函数和观测函数进行泰勒展开时保留二阶项以此来克服原算法只保留一阶项而造成的滤波精度和收敛性问题。

技术实现思路

[0059]本专利技术的目的是提供了一种同步定位与地图构建和路径规划方法。
[0060]本专利技术是通过以下技术方案实现的：
[0061]本专利技术涉及一种同步定位与地图构建和路径规划方法，包括以下步骤：
[0062]步骤1，利用改进的EKF SLAM算法对物流仓进行环境地图构建与实时定位，获取物流仓的现场物理数据；
[0063]步骤2，利用改进的路径规划算法，对巡检目标进行路径规划，提供最优路径，使本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种同步定位与地图构建和路径规划方法，其特征在于，包括以下步骤：步骤1，同步定位与地图构建；利用改进的EKF SLAM算法对物流仓进行环境地图构建与同步定位，获取物流仓的现场物理数据；步骤2，利用改进的路径规划算法，对巡检目标进行路径规划，提供最优路径，使得巡检机器人能够在最短时间内到达目标点；步骤3，在巡检过程中，判断现场环境是否存在温度异常物品；步骤4，进行检测，判断物流仓是否有可燃性气体，是否存在安全隐患；步骤5，进行信息采集。2.如权利要求1所述的同步定位与地图构建和路径规划方法，其特征在于，步骤1中，所述改进的EKF SLAM算法是一种对非线性系统的前二阶矩进行线性近似滤波的计算方法。3.如权利要求1所述的同步定位与地图构建和路径规划方法，其特征在于，步骤2中，所述改进的路径规划算法是在传统的搜索基础上，引入了16邻域的搜索方法的改进算法。4.如权利要求1所述的同步定位与地图构建和路径规划方法，其特征在于，步骤3中，所述判断现场环境是否存在温度异常物品的方法具体为：利用红外热成像仪采集现场热力云图，根据设定的温度阈值判断现场环境是否存在温度异常物品。5.如权利要求...

【专利技术属性】
技术研发人员：汪梅，李远成，黄子洋，王将，刘驰，刘赟超，
申请(专利权)人：西安科技大学，
类型：发明
国别省市：