[发明专利]一种融合卡尔曼滤波预测思想的适用于移动机器人导航的改进蚁群算法

权利要求书

1.一种融合卡尔曼滤波预测思想的适用于移动机器人导航的改进蚁群算法，其特征在于，包括以下步骤：

步骤一：根据移动机器人使用场景，构建二维栅格地图；同时，设置机器人运动起始点、目的点以及初始化障碍物信息的地图参数；

步骤二：根据步骤一，设置蚁群算法中蚁群大小、迭代次数、信息素蒸发率、启发信息权重因子和信息素权重因子参数数值；同时，根据信息素初始分布规则进行初始蚁群算法信息素分布设定，并开始蚁群算法寻路迭代；

步骤三：在蚂蚁选择下一路径节点时，采用复合型节点选择策略；根据普通节点选择策略，同时融合最优节点预测模型进行路径节点选择；

步骤四：当蚂蚁搜索路径过程完成时，利用融合卡尔曼滤波思想的预测信息素更新模型进行全局信息素浓度最优更新，用于下一次蚂蚁寻路过程；

步骤五：根据蚂蚁寻路结果，采用动态更新方式进行蚁群算法节点概率公式中的信息素权重因子和启发信息权重因子的动态更新；

步骤六：判断蚁群算法迭代次数是否达到算法初始化迭代次数，若是，则进行下一步，否则，进行下一次蚂蚁寻路迭代过程；

步骤七：根据历次迭代数据，输出最优路径长度结果以及最优路径节点数据；

最优节点预测模型是指对于蚂蚁所处当前栅格节点L_Position，计算下一可选节点数组Can＝[f₁,f₂,...,f_n]，其中n为可选节点个数；分别以每一个可选节点f_i,i∈n为起始点，进行下一可选节点数组f＝[X₁,X₂,..,X_m]的计算，其中，m为可选节点个数；

基于上述可选节点定义方式，分别对f_i,i∈m进行适应度计算求得fitness＝[fitness(f₁),fitness(f₂),...,fitness(f_n)]，根据公式(1)对适应度值大小进行归一化处理，求得P_i＝[p₁,p₂,...,p_n]；同时，结合普通蚁群算法对于当前蚂蚁所处栅格节点的下一可选节点数组Can选择概率P_normal＝[p_no1,p_no2,...,p_non]]，通过公式(2)和公式(3)进行下一可选节点概率的最优预测融合，求得预测概率P_final，并通过轮盘赌法进行节点选择，保证算法的多样性；

P_final＝[p_fin1,p_fin2,...,p_finn] (2)

节点适应度计算是指以当前节点信息素浓度Tau_num和距离路径规划目的点的欧几里得距离Dis为参数，构建适应度计算模型，如公式(4)；

其中，Tau(x_i)为栅格节点x_i的信息素浓度数值，a为蚁群算法初始化的信息素权重数值，β为启发信息权重因子数值；

融合卡尔曼滤波思想的预测信息素更新模型，是指基于卡尔曼滤波思想进行信息素更新策略的改进和融合，其中，信息素更新策略公式如公式(5)所示；

其中，Tau_tra为普通蚁群算法信息素浓度值，ρ为信息素蒸发率，Q为常数1，L_z为当前蚂蚁z寻找到的路径长度；

引入改进信息素更新模型公式，如公式(6)所示；

Tau_improve(x_i)＝(1-ρ)×Tau_improve(x_i)+Dis(x_i)  (6)

此外，卡尔曼滤波器要求观测量精确度高于状态量，基于蚁群算法系统，信息素浓度的精确度由该浓度对蚂蚁寻路的启发作用大小代为描述；基于此，融合卡尔曼滤波思想，结合公式(7)，以Tau_improve(x_i)为卡尔曼系统状态变量x_k，Dis(x_i)为系统控制量u_k，Tau_tra(x_i)为卡尔曼系统观测量；为了保证算法的随机性和多样性求解，设定随机噪声Q和R分别为0.01，同时根据公式(5)和公式(6)可以求得矩阵A和矩阵H为1-ρ，矩阵B为1，协方差矩阵P初始为0，公式(7)中的I为单位矩阵；

通过公式(7)对第k次蚂蚁寻得路径上得信息素浓度进行预测更新，求得传统信息素更新策略信息素数值和卡尔曼滤波思想预测信息素数值，获得二者融合预测后的具有较大启发作用的信息素浓度数值Tau_Kalman(x_i)，如公式(8)，其中x_i为当前迭代蚂蚁寻找到的路径节点；

采用数据融合动态更新方式进行蚁群算法节点概率公式中的信息素权重因子a和启发信息权重因子β的动态更新，根据蚂蚁寻找到的路径长度为对比值进行权重因子动态更新，如公式(9)所示；其中，所采用的路径长度主要是当此迭代路径长度和上一次迭代路径长度数据进行后续动态更新调整；

其中，Length_{Path_k}为第k次迭代蚂蚁寻路结果的路径长度，φ为常数0.1；

当蚂蚁搜索路径完成时，动态更新信息素浓度完成时，根据本次路径长度数据和上次路径长度数据根据公式(9)进行权重因子更新，用于下一次寻路搜索过程。