[发明专利]一种应用于灾后救援环境下的移动消防机器人建图方法

说明书

本发明涉及一种应用于灾后救援环境下的移动消防机器人建图方法。本发明首先进行系统初始化，包括机器人的状态向量；然后将地标状态加入到机器人运动模型中，并建立观测模型；其次获取机器人的最优控制输入，并进行时间更新。如果没有发现新的地标，只进行预测不更新。如果发现新地标进行量测更新，如果该地标已被其他机器人发现，对该地标估计进行凸组和融合。最后判断是否遍历完整个环境，确定地标的位置；输出全部地标，完成建图。本发明通过采用凸组合融合，引入吸引因子，既提升了机器人自身定位建图准确性，又引导探索环境，降低不确定度，其定位建图精度大大提升，可以有效提升灾后救援的效率。

技术领域

本发明属于救灾领域，涉及一种应用于灾后救援环境下的移动消防机器人建图方法。

背景技术

同步定位与地图构建技术广泛应用于移动机器人、无人驾驶汽车、无人机、增强现实、导航或水下无人潜航器等领域。同时，随着科技的进步和发展，在灾后搜索和救援领域，该技术也逐渐得到广泛使用。但是目前已有的建图方法，大多是单机器人系统且沿着已知运动轨迹前进，这种被动式建图与灾难后现场环境未知相矛盾，导致建图效果不佳，影响灾后救援的效率。所以，在此基础上，多机器人建图逐步成为移动消防机器人领域的热门。

在灾后救援领域，多机器人主动建图在探索能力、鲁棒性、容错性等方面比单机器人更具优势，各个机器人通过通信网络进行信息传递与共享，相互协调工作，共享环境信息，极大扩展了机器人系统对环境的感知范围与定位的精度。但是，受限于机器人自身位置和传感器观测的误差，多机器人主动建图的精度仍会受到影响。因此，需要对建图方法进行改进，使其能够更好地适应于同步定位与地图构建问题，同时提高机器人自身位置估计准确性以及指引多机器人进行环境的探索，降低被估计地标的不确定度，提升定位与建图的精度。

发明内容

本发明的目的是针对现有移动消防机器人在观测定位和建图方面的不足，提供了一种应用于灾后救援环境下，基于EKF-SLAM算法的多移动消防机器人主动建图方法。

本发明包括以下步骤：

步骤一：初始化参数。

在多机器人主动建图过程中，初始化机器人的数量n，机器人的状态向量x^r＝[x,y,θ]^T，其中x,y为位置，θ为其方位角，离散控制输入U＝{(v,ω)}，其中v为速度，ω为角速度，以及吸引因子的触发条件e、t₀。

步骤二：考虑一个n个机器人系统，符合以下运动模型。

将地标状态加入到中可以得到x_i,t+1：

其中w_i,t为系统噪声，u_i,t为控制输入，y_t为地标，包含了m个静态地标的位置，f(×)为非线性系统函数，为机器人状态，x_i,t+1为机器人和地标状态。

步骤三：建立观测模型z_i,t。

z_i,t＝h(x_i,t,y_t)+v_i,t,i∈{1,2,...,n} (3)

其中z_i,t为在t时刻第i个机器人的观测值，h(×)是非线性量测函数，v_i,t是量测噪声。

步骤四：获取机器人的最优控制输入。吸引因子出现时机器人基于吸引因子朝向进行最优的控制决策。如果机器人自身的位置估计协方差大于一定阈值ε时，吸引因子会出现，其朝向为定位最精确的地标。如果机器人在t₀步内无新增扩展面积时，则会触发吸引因子，其朝向为最近的虚拟探索地标。其余情况机器人基于公式(4)进行决策。