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

权利要求书

1.一种应用于灾后救援环境下的移动消防机器人建图方法，其特征在于包括以下步骤：

步骤一：初始化参数；

在多机器人主动建图过程中，初始化机器人的数量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)进行决策；

其中det(P_i,t+1)为估计误差协方差的行列式，α，β为权重参数，C为归一化符号；

步骤五：时间更新：

其中F_i,t和G_i,t分别为f(·)对机器人状态向量与系统噪声w_i,t的雅可比矩阵，Q_i,t为系统噪声方差，和为一步预测状态和估计误差协方差；

步骤六：1)如果没有发现新的地标，只进行预测不更新；2)如果发现新地标进行量测更新：

其中H_i,t+1是h(×)对一步预测状态的雅可比矩阵，R_i,t+1为观测噪声方差，K_i,t+1为增益，为更新后的状态，z_t+1为观测值；

3)如果地标已被其他机器人发现，对地标估计进行凸组和融合；

其中P′_i,t+1∈P_i,t+1、P′_j,t+1∈P_j,t+1，P′_i,t+1和P′_j,t+1为第i和j个地标的估计和协方差；

步骤七：判断是否遍历完整个环境，确定地标的位置；不满足，则转到步骤四继续执行；

步骤八：输出全部地标，完成建图。