CN115729238A - 一种用于移动机器人自主避障的动态路径规划方法 - Google Patents

Abstract

本发明公开了一种用于移动机器人自主避障的动态路径规划方法，包括以下步骤：环境建模；通过改进的A‑Star算法进行移动机器人全局路径的预规划；对预规划的路径中代价函数大的节点通过改进的麻雀搜索算法进行局部的二次规划；基于改进动态窗口法进行移动机器人局部路径规划。本发明基于原有A*算法，首先加入子节点选择规则，再将改进的麻雀搜索算法ISSA与A*算法相结合，利用其能够帮助加速迭代寻优的能力，用来解决传统A*算法中搜索节点太多以及会出现斜切障碍物的问题，从而进行安全、有效的全局参考路径。同时，在局部避障部分，在局部路径规划部分针对DWA算法轨迹评价函数的设计能够有效地对未知动态和静态障碍物进行避让。

Description

一种用于移动机器人自主避障的动态路径规划方法

技术领域

本发明涉及机器人路径规划领域，特别涉及一种用于移动机器人自主避障的动态路径规划方法。

背景技术

随着人类生产生活进入智能化阶段，移动机器人技术在越来越多的领域得到了广泛应用，例如无人作战系统、无人物流配送、无人搜救、医疗服务机器人以及自动驾驶等领域。路径规划作为移动机器人领域复杂且重要的核心问题，成为近几年来机器人领域研究的热点。移动机器人路径规划是在通过传感器进行同步定位与建图(SLAM)获得移动机器人运行环境的栅格地图后，依据一定的算法规则，在环境空间中找到一条从起点状态到终点状态的无碰撞最优路径。

现有技术中路径规划方法可分为需要地图信息作为先验知识的全局路径规划以及利用传感器探测环境避开障碍物的局部路径规划，其中前者对于在移动机器人运动状态下出现的未知障碍物无法进行有效避让，后者由于没有对环境信息进行预处理，对传感器要求较高且在未知环境下的路径搜索时间过长，效率低下。因此采用单一的全局或者局部路径规划算法都无法实现移动机器人在使用过程中的路径规划。

发明内容

为了解决上述技术问题，本发明提供一种算法简单、安全有效的用于移动机器人自主避障的动态路径规划方法。

本发明解决上述技术问题的技术方案是：一种用于移动机器人自主避障的动态路径规划方法，包括以下步骤：

步骤1：基于移动机器人的真实工作环境，进行环境建模获得二维坐标系，并确定起点和终点；

步骤2：通过改进的A-Star算法进行移动机器人全局路径的预规划；

步骤3：判断是否存在预规划路径，如果是，进入步骤4；如果否，则说明该环境地图下起始点到终点没有全局路径；

步骤4：对预规划的路径中代价函数大的节点通过改进的麻雀搜索算法进行局部的二次规划；

步骤5：规划的全局参考路径输入到改进DWA算法的局部路径规划模块，给出实际移动机器人的避障轨迹，并将实时的速度、几何信息、转向信息的输入到移动机器人的底层控制器端进行移动机器人的实时控制。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤1中，采用栅格法来建立移动机器人的环境模型，将环境分割成若干栅格，栅格中的数值表示是否存在障碍物，判断障碍物是否存在的依据就是根据激光雷达以及摄像头的点云分布情况。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤2的具体过程为：

首先引入障碍率P，抽象地表达地图环境的复杂度，定义障碍率P为当前点与目的地组成的矩形局部环境中，障碍物栅格单元的个数N与此局部地图中整个栅格单元的个数之比，即：

P∈(0,1)，x_s为起点位置横坐标，x_g为终点位置横坐标，y_s为起点位置的纵坐标，y_g为终点位置的纵坐标；同时将障碍率P引入评价函数，根据障碍率改变代价函数g_(n)和启发函数h_(n)的权重，实现评价函数f_(n)的自适应调整，由此产生不同的有效搜索空间；改进后的评价函数为：f_(n)＝g_(n)+(1-lnP)h_(n)，其中n为当前栅格节点，f_(n)为节点n的实际评价函数。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤2中，采用优化子节点选择方式，增加子节点选择规则，避免出现风险路径；子节点选择优化方式具体为：

以父节点为中心，选择其上、下、左、右、左上、左下、右上、右下8个方向的8个子节点共同组成九宫格，将此8个子节点分类，其中上、下方向的子节点为第①类，左、右方向的子节点的第②类，左上、左下、右上、右下方向的子节点为第③类，则：当障碍物子节点为第①类时，则删除该障碍物节点的左右两个可选子节点；当障碍物子节点为第②类位置时，则删除该障碍子节点的上下两个可选择子节点；当障碍物为第③类位置时，则不做处理。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤4具体过程为：

(4-1)设置参数、利用改进的Logistics混沌映射初始化麻雀种群；

(4-2)根据代价函数确定算法中的麻雀个体的适应度值f_i，通过排序确定最优和最差适应度值f_g、f_w及其位置X_best、X_worst；

(4-3)根据发现者位置更新公式、跟随者位置更新公式、警戒者位置更新公式进行发现者、跟随者、警戒者的位置更新；

(4-4)融合对位差分进化策略和柯西变异算子对当前最优解进行扰动，产生新解；

(4-5)判断是否达到算法设置的最大迭代次数，若是，则输出最优适应度值f_g和麻雀最优个体位置X_best；否则返回(4-3)继续更新迭代。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤(4-3)中，发现者、跟随者和警戒者的位置更新公式如下所示：

发现者位置更新公式：

跟随者位置更新公式：

警戒者位置更新公式：

其中，

为步骤3预规划路径中代价函数大的节点对应的适应度值，即为路径的代价函数；t为当前迭代次数，

表示在t+1次迭代次数时第i只麻雀的适应度值；iter_max是最大迭代次数，ξ∈(0,1)是一个随机数，R₂表示警戒值，ST表示安全阈值，q是一个服从正态分布的随机数，L是一个一行多维的全为一的矩阵；X_P表示目前发现者所处的最优位置，X_worst则表示当前全局最差位置；A表示一个1×d的矩阵，其中每个数随机赋值为1或-1，并且A⁺＝A^T(AA^T)^-1；全局最优位置用

表示，β为服从标准正态分布的随机数，警戒者位置更新中用于控制步长；K∈(0,1)是一个随机数，f_i表示算法过程中求解的当前麻雀个体的适应度值，f_g和f_w分别表示当前算法的最优、最差适应度值，常数ε用于避免出现分母为零的情况。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤(4-4)中，差分进化算法是一类在遗传算法的基础上衍生出的智能优化算法；在差分进化的基础上，将其进行改进得到对位差分进化算法，对位差分进化算法采取如公式：

所示生成对位个体，其中，

为新生成的对位个体，m∈(0,0.8)为随机因子；而后再选择柯西变异算子进行变异交叉操作进一步丰富麻雀种群的多样性，变异后的个体实现为：

其中

为变异后的最优麻雀个体，Chuchy(0,1)为一个服从柯西分布的随机变量，选择操作为：

rand(0,1)为(0,1)中的一个随机数，C_r为交叉概率。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤5具体过程为：

(5-1)基于移动机器人在全局路径规划阶段获得的静态全局路径，建立移动机器人当前的运动模型X_t＝[x_t y_t θ_t ν_t ω_t]^T，确定当前路径的目标节点，其中x_t、y_t、θ_t、ν_t、ω_t分别表示机器人在t时刻的x坐标、y坐标、朝向角、移动速度、旋转速度；

(5-2)机器人进行姿态调整，消除方位角偏差；

(5-3)由速度约束条件生成速度取值空间[ν_min,ν_max]、[ω_min,ω_max]，ν_min,ν_max分别为最小线速度和最大线速度，ω_min,ω_max分别为最小角速度和最大角速度；设置线速度分辨率E_v和角速度分辨率E_ω，即线速度从最小值0每隔E_v取一个值，直到最大值v_max取值结束；角速度从最小值ω_min每隔E_ω取一个值，直到最大值ω_max结束；线速度与角速度两两组合成速度组，形成采样空间；

(5-4)根据每对速度组生成该速度下的模型预测轨迹，即所有速度组成形成多条模拟轨迹；由改进的评价函数选择一条最优轨迹，即选择出该轨迹速度组(ν,ω)进行移动，并设置该速度为当前速度。

上述用于移动机器人自主避障的动态路径规划方法，所述步骤(5-4)中，改进后的评价函数为：

G(ν,ω)＝α*head(ν,ω)+β*vel(ν,ω)+γ*dist_sta(ν,ω)+λ*dist_dyna(ν,ω)

其中，α、β、γ、λ为四项评价的加权系数，设置不同的比重；

head(ν,ω)为方向角的评价函数，其表达式为：

表示在当前速度下，模拟轨迹的终点姿态方向与目的地之间的方向角偏差Δθ，设置π-Δθ的方式来评价，即前进方向越靠近目的地,Δθ越小，则π-Δθ数值越高，评价函数占比越大；其中，M为该模拟周期内模拟轨迹的总数，δ表示M条模拟轨迹中的第δ个，Δθ_δ表示第δ个模拟轨迹终点与目标点之间方位角的偏差；

vel(ν,ω)为当前速度大小评价函数，表达式为：

ν_δ表示第δ个模拟轨迹终点处线速度的绝对值；移动机器人速度越快，则评价得分越高；

dis(ν,ω)为速度对应轨迹上离障碍物最近间距的评价函数，表达式为：

d_δ表示第δ个模拟轨迹终点处于最近障碍物的间距；若无障碍物或者最近距离d超过设定值D，则将其值等于设定的常量D；即在规定的范围内与障碍物距离越近，d数值越小，则评价得分越低；

在障碍物评价函数部分，提出将评价函数进行已知障碍物、未知障碍物的区分；dist_sta(ν,ω)为全局中已知障碍物最近距离的评价函数；dist_dyna(ν,ω)为局部中出现的未知障碍物最近距离的评价函数。

本发明的有益效果在于：本发明通过对传统A*算法进行改进，基于原有A*算法，首先加入子节点选择规则，再将改进的麻雀搜索算法ISSA与A*算法相结合，利用其能够帮助加速迭代寻优的能力，用来解决传统A*算法中搜索节点太多以及会出现斜切障碍物的问题，从而进行安全、有效的全局参考路径。同时，在局部避障部分，在局部路径规划部分针对DWA算法轨迹评价函数的设计能够有效地对未知动态和静态障碍物进行避让。

附图说明

图1为本发明全局路径规划的流程图。

图2为采用栅格法建立环境模型时原地图与栅格地图的对比图，其中左侧为原地图，右侧为栅格地图。

图3为栅格地图二维坐标系示意图。

图4为风险路径图，其中左侧为规划路径斜过障碍物顶点的示意图，右侧为规划路径斜过两个障碍物相接顶点的示意图。

图5为子节点选择优化方式的示意图。

图6为ISSA算法流程图。

图7为本发明局部路径规划的流程图。

图8为本发明的全局路径规划算法与改进前的A*路径规划算法得到的最优路径对比图，其中左侧为改进前的A*路径规划算法得到的路径，右侧为本发明提出的全局规划算法得到的规划路径。

图9为本发明的动态环境下的移动机器人路径规划仿真实验结果图，其中左侧为传统A*算法加DWA算法进行路径规划的仿真结果，右侧为本发明的仿真实验结果。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

如图1、图7所示，一种用于移动机器人自主避障的动态路径规划方法，包括以下步骤：

步骤1：基于移动机器人的真实工作环境，进行环境建模获得如图3所示的二维坐标系，并确定起点和终点。

采用栅格法来建立移动机器人的环境模型，将环境分割成若干栅格，栅格中的数值表示是否存在障碍物，判断障碍物是否存在的依据就是根据激光雷达以及摄像头的点云分布情况，移动机器人中就是使用如图2所示的二维占据栅格地图的表示方法。

步骤2：通过改进的A-Star算法进行移动机器人全局路径的预规划。

步骤2的具体过程为：

传统的A-Star算法在进行全局路径规划时遍历节点过多，同时受环境地图影响灵活的不高。因此，首先引入障碍率P，抽象地表达地图环境的复杂度，定义障碍率P为当前点与目的地组成的矩形局部环境中，障碍物栅格单元的个数N与此局部地图中整个栅格单元的个数之比，即：

P∈(0,1)，x_s为起点位置横坐标，x_g为终点位置横坐标，y_s为起点位置的纵坐标，y_g为终点位置的纵坐标；同时将障碍率P引入评价函数，由此增加A-Star算法的灵活性。根据障碍率改变代价函数g_(n)和启发函数h_(n)的权重，实现评价函数f_(n)的自适应调整，由此产生不同的有效搜索空间；改进后的评价函数为：f_(n)＝g_(n)+(1-ln P)h_(n)，其中n为当前栅格节点，f_(n)为节点n的实际评价函数。

由于传统A-Star全局路径规划算法的存储空间由所有当前节点的周围相邻的子节点组成，每次会选择评价函数最低的节点作为下一路径节点。选择子节点的方式为判断子节点是否是障碍栅格，若是则不生成该位置的子节点。因此，会出现如图4所示的规划路径斜过障碍物顶点的现象，甚至是斜过两个障碍物相接顶点，故采用图5所示优化子节点选择方式，增加子节点选择规则，避免出现风险路径。

子节点选择优化方式具体为：

以父节点为中心，选择其上、下、左、右、左上、左下、右上、右下8个方向的8个子节点共同组成九宫格，将此8个子节点分类，其中上、下方向的子节点为第①类，左、右方向的子节点的第②类，左上、左下、右上、右下方向的子节点为第③类，则：当障碍物子节点为第①类时，则删除该障碍物节点的左右两个可选子节点(如子节点1,3或者子节点5,7)；当障碍物子节点为第②类位置时，则删除该障碍子节点的上下两个可选择子节点(如子节点1,7或者子节点3,5)；当障碍物为第③类位置时，则不做处理。

步骤3：判断是否存在预规划路径，如果是，进入步骤4；如果否，则说明该环境地图下起始点到终点没有全局路径。

步骤4：对预规划的路径中代价函数大的节点通过改进的麻雀搜索算法进行局部的二次规划。

如图6所示，步骤4具体过程为：

(4-1)设置参数、利用改进的Logistics混沌映射初始化麻雀种群。

(4-2)根据代价函数确定算法中的麻雀个体的适应度值f_i，通过排序确定最优和最差适应度值f_g、f_w及其位置X_best、X_worst。

(4-3)根据发现者位置更新公式、跟随者位置更新公式、警戒者位置更新公式进行发现者、跟随者、警戒者的位置更新。

发现者、跟随者和警戒者的位置更新公式如下所示：

发现者位置更新公式：

跟随者位置更新公式：

警戒者位置更新公式：

其中，

为步骤3预规划路径中代价函数大的节点对应的适应度值，即为路径的代价函数；t为当前迭代次数，

表示在t+1次迭代次数时第i只麻雀的适应度值；iter_max是最大迭代次数，ξ∈(0,1)是一个随机数，R₂表示警戒值，ST表示安全阈值，q是一个服从正态分布的随机数，L是一个一行多维的全为一的矩阵；X_P表示目前发现者所处的最优位置，X_worst则表示当前全局最差位置；A表示一个1×d的矩阵，其中每个数随机赋值为1或-1，并且A⁺＝A^T(AA^T)^-1；全局最优位置用

表示，β为服从标准正态分布的随机数，警戒者位置更新中用于控制步长；K∈(0,1)是一个随机数，f_i表示算法过程中求解的当前麻雀个体的适应度值，f_g和f_w分别表示当前算法的最优、最差适应度值，常数ε用于避免出现分母为零的情况，ε设置为10E-6。

(4-4)融合对位差分进化策略和柯西变异算子对当前最优解进行扰动，产生新解。

差分进化算法是一类在遗传算法的基础上衍生出的智能优化算法；在差分进化的基础上，将其进行改进得到对位差分进化算法，对位差分进化算法采取如公式：

所示生成对位个体，其中，

为新生成的对位个体，m∈(0,0.8)为随机因子；而后再选择柯西变异算子进行变异交叉操作进一步丰富麻雀种群的多样性，变异后的个体实现为：

其中

为变异后的最优麻雀个体，Chuchy(0,1)为一个服从柯西分布的随机变量，选择操作为：

rand(0,1)为(0,1)中的一个随机数，C_r为交叉概率；通过本步骤不断对局部最优解进行扰动和最优解的选择操作，能够有效避免算法搜索后期由于种群多样性骤减导致的算法停滞以及陷入局部最优解的情况。

(4-5)判断是否达到算法设置的最大迭代次数，若是，则输出最优适应度值f_g和麻雀最优个体位置X_best；否则返回(4-3)继续更新迭代。

步骤5：规划的全局参考路径输入到改进DWA算法的局部路径规划模块，给出实际移动机器人的避障轨迹，并将实时的速度、几何信息、转向信息的输入到移动机器人的底层控制器端进行移动机器人的实时控制。

在局部路径规划方面，传统的动态窗口法(DWA)在路径规划的起始位置由于机器人姿态方向与当前目标点方向不一致，机器人不得不进行圆弧转圈，同时传统DWA算法未对障碍物进行区分导致机器人在沿全局规划路径行驶过程中对动态障碍物未能够进行及时避让。为达到上述目的，本发明提供一种基于改进动态窗口法(DWA)的移动机器人局部路径规划算法，具体过程为：

(5-1)基于移动机器人在全局路径规划阶段获得的静态全局路径，建立移动机器人当前的运动模型X_t＝[x_t y_t θ_t ν_t ω_t]^T，确定当前路径的目标节点，其中x_t、y_t、θ_t、ν_t、ω_t分别表示机器人在t时刻的x坐标、y坐标、朝向角、移动速度、旋转速度。

(5-2)机器人进行姿态调整，消除方位角偏差。

(5-3)由速度约束条件生成速度取值空间[ν_min,ν_max]、[ω_min,ω_max]，ν_min,ν_max分别为最小线速度和最大线速度，ω_min,ω_max分别为最小角速度和最大角速度；设置线速度分辨率E_v和角速度分辨率E_ω，即线速度从最小值0每隔E_v取一个值，直到最大值v_max取值结束；角速度从最小值ω_min每隔E_ω取一个值，直到最大值ω_max结束；线速度与角速度两两组合成速度组，形成采样空间；

(5-4)根据每对速度组生成该速度下的模型预测轨迹，即所有速度组成形成多条模拟轨迹；由改进的评价函数选择一条最优轨迹，即选择出该轨迹速度组(ν,ω)进行移动，并设置该速度为当前速度。

改进后的评价函数为：

G(ν,ω)＝α*head(ν,ω)+β*vel(ν,ω)+γ*dist_sta(ν,ω)+λ*dist_dyna(ν,ω)

其中，α、β、γ、λ为四项评价的加权系数，设置不同的比重；

head(ν,ω)为方向角的评价函数，其表达式为：

表示在当前速度下，模拟轨迹的终点姿态方向与目的地之间的方向角偏差Δθ，设置π-Δθ的方式来评价，即前进方向越靠近目的地，Δθ越小，则π-Δθ数值越高，评价函数占比越大；其中，M为该模拟周期内模拟轨迹的总数，δ表示M条模拟轨迹中的第δ个，Δθ_δ表示第δ个模拟轨迹终点与目标点之间方位角的偏差。

vel(ν,ω)为当前速度大小评价函数，表达式为：

ν_δ表示第δ个模拟轨迹终点处线速度的绝对值；移动机器人速度越快，则评价得分越高。

dis(ν,ω)为速度对应轨迹上离障碍物最近间距的评价函数，表达式为：

d_δ表示第δ个模拟轨迹终点处于最近障碍物的间距；若无障碍物或者最近距离d超过设定值D，则将其值等于设定的常量D；即在规定的范围内与障碍物距离越近，d数值越小，则评价得分越低。

在障碍物评价函数部分，提出将评价函数进行已知障碍物、未知障碍物的区分；dist_sta(ν,ω)为全局中已知障碍物最近距离的评价函数，控制已知障碍物对动态规划的干扰，避免局部规划路径为了远离障碍物，偏离全局最优路径，增大全局的移动距离。dist_dyna(ν,ω)为局部中出现的未知障碍物最近距离的评价函数，可增大其系数，增强对未知障碍物的避障能力。对障碍物进行区别后，可分别调节加权系数β和λ，既可以减少已知障碍物对规划轨迹的干扰，又增强了对未知障碍物及时避障的灵敏度。

为了验证本发明所提供的用于移动机器人自主避障的动态路径规划方法的可行性和有效性，如图8所示，本发明设置30×30的栅格环境，三角形位置为起点，圆圈位置为终点位置，黑色部分为环境中的障碍物栅格，虚线分别为算法规划的起点到终点的全局路径。改进前的A*路径规划算法最优路径如图8左图所示，出现了斜切障碍物的危险路径情况。本发明改进后的算法规划的全局路径如图8右侧所示，能够有效避免危险路径情况。

如图9所示，进行动态环境下的移动机器人路径规划仿真实验。实验中三角形表示移动机器人的起点，圆形表示移动机器人的目的地。其中1为实验中的全局规划路径，2为未知移动障碍物的移动路线，3为未知移动障碍物，4为未知静止障碍物，5为移动机器人实际行驶路径。从实验可以看出，本发明提出的用于移动机器人自主避障的动态路径规划方法能够有效规划出与环境中的已知障碍物无交集的全局参考路径，同时在局部避障部分，设计了一种改进的DWA方法用于局部路径规划，使得移动机器人能够对静止、移动的未知障碍物进行有效避让。

Claims (9)

1.一种用于移动机器人自主避障的动态路径规划方法，其特征在于，包括以下步骤：

步骤1：基于移动机器人的真实工作环境，进行环境建模获得二维坐标系，并确定起点和终点；

步骤2：通过改进的A-Star算法进行移动机器人全局路径的预规划；

步骤3：判断是否存在预规划路径，如果是，进入步骤4；如果否，则说明该环境地图下起始点到终点没有全局路径；

步骤4：对预规划的路径中代价函数大的节点通过改进的麻雀搜索算法进行局部的二次规划；

步骤5：规划的全局参考路径输入到改进DWA算法的局部路径规划模块，给出实际移动机器人的避障轨迹，并将实时的速度、几何信息、转向信息的输入到移动机器人的底层控制器端进行移动机器人的实时控制。

2.根据权利要求1所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤1中，采用栅格法来建立移动机器人的环境模型，将环境分割成若干栅格，栅格中的数值表示是否存在障碍物，判断障碍物是否存在的依据就是根据激光雷达以及摄像头的点云分布情况。

3.根据权利要求1所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤2的具体过程为：

首先引入障碍率P，抽象地表达地图环境的复杂度，定义障碍率P为当前点与目的地组成的矩形局部环境中，障碍物栅格单元的个数N与此局部地图中整个栅格单元的个数之比，即：

P∈(0,1)，x_s为起点位置横坐标，x_g为终点位置横坐标，y_s为起点位置的纵坐标，y_g为终点位置的纵坐标；同时将障碍率P引入评价函数，根据障碍率改变代价函数g_(n)和启发函数h_(n)的权重，实现评价函数f_(n)的自适应调整，由此产生不同的有效搜索空间；改进后的评价函数为：f_(n)＝g_(n)+(1-lnP)h_(n)，其中n为当前栅格节点，f_(n)为节点n的实际评价函数。

4.根据权利要求3所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤2中，采用优化子节点选择方式，增加子节点选择规则，避免出现风险路径；子节点选择优化方式具体为：

以父节点为中心，选择其上、下、左、右、左上、左下、右上、右下8个方向的8个子节点共同组成九宫格，将此8个子节点分类，其中上、下方向的子节点为第①类，左、右方向的子节点的第②类，左上、左下、右上、右下方向的子节点为第③类，则：当障碍物子节点为第①类时，则删除该障碍物节点的左右两个可选子节点；当障碍物子节点为第②类位置时，则删除该障碍子节点的上下两个可选择子节点；当障碍物为第③类位置时，则不做处理。

5.根据权利要求4所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤4具体过程为：

(4-1)设置参数、利用改进的Logistics混沌映射初始化麻雀种群；

(4-2)根据代价函数确定算法中的麻雀个体的适应度值f_i，通过排序确定最优和最差适应度值f_g、f_w及其位置X_best、X_worst；

(4-3)根据发现者位置更新公式、跟随者位置更新公式、警戒者位置更新公式进行发现者、跟随者、警戒者的位置更新；

(4-4)融合对位差分进化策略和柯西变异算子对当前最优解进行扰动，产生新解；

(4-5)判断是否达到算法设置的最大迭代次数，若是，则输出最优适应度值f_g和麻雀最优个体位置X_best；否则返回(4-3)继续更新迭代。

6.根据权利要求5所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤(4-3)中，发现者、跟随者和警戒者的位置更新公式如下所示：

发现者位置更新公式：

跟随者位置更新公式：

警戒者位置更新公式：

其中，

为步骤3预规划路径中代价函数大的节点对应的适应度值，即为路径的代价函数；t为当前迭代次数，

表示在t+1次迭代次数时第i只麻雀的适应度值；iter_max是最大迭代次数，ξ∈(0,1)是一个随机数，R₂表示警戒值，ST表示安全阈值，q是一个服从正态分布的随机数，L是一个一行多维的全为一的矩阵；X_P表示目前发现者所处的最优位置，X_worst则表示当前全局最差位置；A表示一个1×d的矩阵，其中每个数随机赋值为1或-1，并且A⁺＝A^T(AA^T)^-1；全局最优位置用

表示，β为服从标准正态分布的随机数，警戒者位置更新中用于控制步长；K∈(0,1)是一个随机数，f_i表示算法过程中求解的当前麻雀个体的适应度值，f_g和f_w分别表示当前算法的最优、最差适应度值，常数ε用于避免出现分母为零的情况。

7.根据权利要求6所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤(4-4)中，差分进化算法是一类在遗传算法的基础上衍生出的智能优化算法；在差分进化的基础上，将其进行改进得到对位差分进化算法，对位差分进化算法采取如公式：

所示生成对位个体，其中，

为新生成的对位个体，m∈(0,0.8)为随机因子；而后再选择柯西变异算子进行变异交叉操作进一步丰富麻雀种群的多样性，变异后的个体实现为：

其中

为变异后的最优麻雀个体，Chuchy(0,1)为一个服从柯西分布的随机变量，选择操作为：

rand(0,1)为(0,1)中的一个随机数，C_r为交叉概率。

8.根据权利要求7所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤5具体过程为：

(5-1)基于移动机器人在全局路径规划阶段获得的静态全局路径，建立移动机器人当前的运动模型X_t＝[x_t y_t θ_t ν_t ω_t]^T，确定当前路径的目标节点，其中x_t、y_t、θ_t、ν_t、ω_t分别表示机器人在t时刻的x坐标、y坐标、朝向角、移动速度、旋转速度；

(5-2)机器人进行姿态调整，消除方位角偏差；

(5-3)由速度约束条件生成速度取值空间[ν_min,ν_max]、[ω_min,ω_max]，ν_min,ν_max分别为最小线速度和最大线速度，ω_min,ω_max分别为最小角速度和最大角速度；设置线速度分辨率E_v和角速度分辨率E_ω，即线速度从最小值0每隔E_v取一个值，直到最大值v_max取值结束；角速度从最小值ω_min每隔E_ω取一个值，直到最大值ω_max结束；线速度与角速度两两组合成速度组，形成采样空间；

(5-4)根据每对速度组生成该速度下的模型预测轨迹，即所有速度组成形成多条模拟轨迹；由改进的评价函数选择一条最优轨迹，即选择出该轨迹速度组(ν,ω)进行移动，并设置该速度为当前速度。

9.根据权利要求8所述的用于移动机器人自主避障的动态路径规划方法，其特征在于，所述步骤(5-4)中，改进后的评价函数为：

G(ν,ω)＝α*head(ν,ω)+β*vel(ν,ω)+γ*dist_sta(ν,ω)+λ*dist_dyna(ν,ω)

其中，α、β、γ、λ为四项评价的加权系数，设置不同的比重；

head(ν,ω)为方向角的评价函数，其表达式为：

表示在当前速度下，模拟轨迹的终点姿态方向与目的地之间的方向角偏差Δθ，设置π-Δθ的方式来评价，即前进方向越靠近目的地，Δθ越小，则π-Δθ数值越高，评价函数占比越大；其中，M为该模拟周期内模拟轨迹的总数，δ表示M条模拟轨迹中的第δ个，Δθ_δ表示第δ个模拟轨迹终点与目标点之间方位角的偏差；

vel(ν,ω)为当前速度大小评价函数，表达式为：

ν_δ表示第δ个模拟轨迹终点处线速度的绝对值；移动机器人速度越快，则评价得分越高；

dis(ν,ω)为速度对应轨迹上离障碍物最近间距的评价函数，表达式为：

d_δ表示第δ个模拟轨迹终点处于最近障碍物的间距；若无障碍物或者最近距离d超过设定值D，则将其值等于设定的常量D；即在规定的范围内与障碍物距离越近，d数值越小，则评价得分越低；

在障碍物评价函数部分，提出将评价函数进行已知障碍物、未知障碍物的区分；dist_sta(ν,ω)为全局中已知障碍物最近距离的评价函数；dist_dyna(ν,ω)为局部中出现的未知障碍物最近距离的评价函数。

Images