CN104050390B - 一种基于可变维粒子群膜算法的移动机器人路径规划方法 - Google Patents

Abstract

本发明属于智能机器人与控制技术领域，尤其涉及一种基于可变维粒子群膜算法的移动机器人路径规划方法。本发明步骤主要为：环境建模、输入路径参数、设置适应度函数、初始化有效路径母粒子和子粒子、路径粒子分类和重归类、对适应度函数进行评价、更新d维粒子的位置、判断当前粒子是否有无效维、对存在无效维的粒子进行修正和去除冗维信息等。本发明能利用复杂环境全局已知或者复杂环境局部位置的信息以及待定移动机器人的状态参数快速规划处适合此类机器人行走的最优路径。

Description

一种基于可变维粒子群膜算法的移动机器人路径规划方法

技术领域

本发明属于智能机器人与控制技术领域，尤其涉及一种基于可变维粒子群膜算法的移动机器人路径规划方法。

背景技术

机器人技术无论是在工业化时代还是在信息化时代都备受关注，无论是在民用工业还是在国防军事都有着十分广泛的应用。移动机器人路径规划问题是智能机器人技术的一个重要分支，其主要任务是在充满着静止障碍物、移动物体以及危险区域的复杂环境中，为移动机器人提供从起始位置到目标位置免碰撞并合理的离线或在线工作方式的全局或局部性规划。随着应用背景的不断复杂化以及应用领域的不断扩大，机器人技术一直向着更高层次的人工智能化目标发展，因此，一直是国内外的热门研究主题。作为移动机器人导航技术重要组成部分,路径规划结果的优劣直接决定着机器人的实用性及总体性能。路径规划问题已经被证明是一种NP难问题，传统的优化方法对这类复杂非线性NP优化问题具有计算耗时、环境自适应性差、缺乏鲁棒性以及可扩展性等诸多弊端，因此，采用现代智能优化方法是解决路径规划问题的趋势。

移动机器人的路径规划问题需要考虑因数众多。环境方面诸如：障碍物之间的距离，障碍物形状，随机出现的危险区域或动态障碍物及其移动方向和移动速度等；机器人的自身特性方面诸如：机器的最小转弯半径，速度及加速度大小，距离传感器范围等；优化目标选择：路径最短、最平滑，安全性最高(尽量远离障碍物或危险区域)，最小耗能或耗时等。因此，在复杂环境中要找到一条适合特定机器人行走的合理路径需要综合分析环境搭建方法、优化目标函数以及相匹配的智能优化方法三大方面，以便规划出来的路径能够兼顾合理性、实时性、实用性、自适应性、可扩展性等多方面的要求。虽然目前用于路径规划的环境建模方法很多，例如：可视图法、单元分解法、构造空间法、势力场法、栅格法等。优化方法也众多，例如：遗传算法、蚁群算法、神经网络、粒子群算法、模拟退火法等。但这些方法并没有有效解决所搭建环境中最大限度兼顾合理性，即最短路径、安全性、平滑性等多目标同时考虑，实时性，即算法要求尽量简单可行计算效率高，实用性，即既能用于静态离线路径规划又能将其扩展用于在线动态路径规划，自适应性，即既能用于简单环境中仅考虑单个优化对象的情况又能用于复杂环境中同时兼顾多个优化对象的情况多方面平衡的问题。因此，发明一种既适用于静态已知环境的移动机器人离线全局路径规划，又可扩展用于静态或动态未知环境中的移动机器人在线实时局部路径规划的高效的基本方法，具有十分重要的实际意义，而且有利于未来智能移动机器人控制***的不断完善。

发明内容

本发明针对现有技术的不足，提供了一种基于可变维粒子群膜算法的移动机器人路径规划方法。该方法能利用复杂环境全局已知或者复杂环境局部位置的信息以及待定移动机器人的状态参数快速规划处适合此类机器人行走的最优路径。

本发明的目的是通过如下步骤实现的：

S1、环境建模，具体为：

S101、将物理环境信息映射为n·n的栅格型虚拟环境，对所述n·n个栅格进行标号，其中，n为不为零的自然数，所述栅格为正方形，所述栅格的边长SoG＝N×机器人直径，N≥2且N为自然数；

S102、对S101所述带有标号的栅格进行分类，分为障碍物栅格和自由栅格；

S103、对S102所述障碍物栅格的外凸顶点***栅格进行标记，其中，外凸顶点***栅格为自由栅格；

S104、对S103所述外凸顶点***栅格进行分类，分为对角顶点***栅格和侧边***栅格，定义所述对角顶点***栅格的随机选择概率系数为γ₁，定义所述侧边***栅格的随机选择概率系数为γ₂，其中，0≤γ₁≤1，0≤γ₂≤1，γ₁+γ₂＝1；

S105、实际环境的物理坐标到虚拟环境栅格序号之间的映射关系表示为p＝u·Noc+v，其中，p为序号数，x,y为物理坐标，SoG为单个栅格的边长，fix函数为低整位取整函数；

S2、输入路径规划参数，具体包括：机器人状态参数，起止位置及安全范围；

S3、设置适应度函数f＝K_d·Dis+K_f·S+K_s·SD，其中，K_d,K_f,K_s为权重系数，K_d用来调节最短距离Dis目标函数在最优路径中所占的比重，K_f用来调节平滑度S目标函数在最优路径中所占的比重，K_s用来调节安全度SD目标函数在最优路径中所占的比重，其中，Dis是从起始点到终止点的h个节点之间的距离之和；

S4、初始化有效路径母粒子和子粒子，具体为：

S401、在表层膜中创建m个子膜，在每个子膜中产生1个有效路径母粒子，共产生m个有效路径母粒子，对m个子膜进行标号，记作M_i,i＝1,2,...,m；

S402、在S401所述m个有效路径母粒子的基础上利用随机和特殊方向指导相结合的方式在所述m个有效路径母粒子对应的子膜M_i中产生c个有效路径子粒子，共产生m·c个有效路径粒子的初始种群；

S5、路径粒子分类与重归类，具体为：

S501、根据S401所述每个子膜M_i中的粒子维度的不同，将粒子分为k_i组；

S502、构建第i个子膜中的第j个基本膜将S501所述分类后的粒子放入所述基本膜中，其中，中粒子均为同维粒子，j＝1,2,...,k_i；

S503、运用粒子运动方向重调整方法对粒子速度进行重新初始化；

S6、对S502所述基本膜中路径粒子的适应度函数进行评价，具体为：

S601、在S502所述基本膜中根据S3所述适应度函数f＝K_d·Dis+K_f·S+K_s·SD计算基本膜中每个粒子对应的适应度函数值，找到每个粒子的个体最优值

S602、在S502所述基本膜中找到局部最优其中，j＝1,2,...,k_i，t为子粒子代数；

S603、将第i个子膜里的局部最优送出到表层膜；

S604、比较S603所述得出同维的全局最优解其中，d∈{1,2,...,D}；

S605、将S604所述送回维数相匹配的基本膜中；

S7、利用公式计算d维粒子的速度，利用公式X_id(t+1)＝V_id(t+1)+X_id(t)更新d维粒子的位置，其中，V_id(t)为粒子在第t代的速度，V_id(t+1)是粒子在t+1代的速度，是粒子在第t代的个体最优，X_id(t)是粒子在第t代的位置，是第t代同维数粒子的局部最优，是第t代同维粒子的全部最优，是第t代粒子经方向调整方法处理后的速度，δ₁和δ₂是比例系数，ρ是惯性权重系数，r₁,r₂,r₃是0和1间的随机数，c₁,c₂,c₃为加速度系数，X_id(t)是第t代粒子的位置，X_id(t+1)是第t+1代粒子的位置，V_id(t+1)第t+1代粒子的速度；

S8、判断当前粒子是否有无效维，即对S7所述更新位置后的d维粒子进行判断，若所述更新位置后的d维粒子存在无效维则进入S9，若所述更新位置后的d维粒子不存在无效维则进入S10；

S9、对S8所述存在无效维的粒子进行修正，修正后判断其无效性，直至不存在无效维转至S10；

S10、判断当前粒子是否有冗维信息，如存在冗维信息则转入S11，如不存在冗维信息则转入S12；

S11、去除冗维信息：

S1101、对一条有效路径从起点到终点的所有节点依次排序并放入序列表L中，令起点为首节点，终点为尾节点；

S1102、取出S1101所述首节点，从第3个节点开始，判断第3个节点是否与首节点能无障碍连接，如果能链接则依次取下一节点，继续判断此节点与首节点的无障碍连接性，直至找到与首节点不能直接相连接的节点a，如果首节点与第3个节点不能无障碍连接，则说明第2个节点不是冗余维节点，再令第2个为首节点；

S1103、从序列表L中删除首节点和第a-1个节点之间的所有节点；

S1104、令第a-1个节点为首节点，重复S1102依次取其后的节点进行判断并去除冗余信息，直至尾节点出现；

S12、在基本膜中得到不同维数的最优路径粒子：各基本膜中的路径粒子经过S9的无效维路径粒子修正和S11的去冗余维信息处理后，每次得到的新路径粒子向较优或最优路径接近，其维数已经和初始状态的维数不同，同一子膜M_i内各基本膜溶解，去掉基本膜的封装，将新粒子重新释放回各自所属的子膜内，等待下一次迭代的分类；

S13、判断算法满足终止条件：如果循环次数t≥T，则循环结束，输出包含最优路径的解集Φ，否则t＝t+1，算法跳转到S5。

进一步地，S2所述机器人状态参数包括：机器人尺寸大小、机器人行进速度、机器人最小转弯半径、机器人起点位置坐标信息和机器人期望到达的目标终点位置信息。

进一步地，S3所述最短距离Dis目标函数为其中，L(a,a+1)是节点a和节点a+1之间的距离，x_a是节点a的X轴坐标值，x_a+1是节点a+1的X轴坐标值，y_a是节点a的Y轴坐标值，y_a+1是节点a+1的Y轴坐标值；

S3所述平滑度S目标函数为S＝α·S_c+β·S_p，其中，α和β是权重系数，N_f是有效路径的总片段数，DA_l是线段之间夹角大于期望值阈值的数量，所述期望值阀值为机器人最小转弯半径夹角，S_min是最小路径片段数；

S3所述安全度SD目标函数为其中，d_r是第r条路径片段与其最近障碍物之间的最小距离，λ是安全度阀值，C_r为第r条路径片段的安全性罚值。

进一步地，所述安全度阀值λ为机器人与障碍物的最小欧几里得距离。

进一步地，S9所述对S8所述存在无效维的粒子进行修正，具体方法如下：

S901、判断S8所述存在无效维的粒子的情况，若存在无效维的粒子的某一维陷入障碍物中则为类型T1，若存在无效维的粒子的某两维跨越障碍物则为类型T2，其中，类型T1即存在无效维的粒子的节点陷入障碍物中，类型T2即存在无效维的粒子的节点之间的线段跨越障碍物；

S902、对类型T1进行无效维修正，包括：

S9021、计算S901所述陷入障碍物的节点到对应障碍物所有外凸顶点的点到点之间的直线距离；

S9022、对S9021计算所得直线距离进行从大到小排序，选出与陷入障碍物的节点的直线距离最小的外凸顶点；

S9023、将S9022选出的外凸顶点***栅格作为将要修改至的备选栅格，删去无效维节点；

S9024、将新节点***S9023所述被删去无效维节点的位置，其中，所述新节点为S9023所述备选栅格；

S903、对类型T2进行无效维修正，包括：

S9031、计算S901所述节点之间跨越障碍物的线段到对应障碍物所有外凸顶点的距离，即点到线之间的直线距离；

S9032、对S9031计算所得点到线之间的直线距离进行从大到小排序，选出直线距离最小的所对应的外凸顶点；

S9033、将S9032选出的外凸顶点***栅格作为将要修改至的备选栅格；

S9034、将新节点***S901所述跨越障碍物的线段两头的节点之间，其中，所述新节点为S9033所述备选栅格。

本发明的有益效果是：

规划实时性好。本发明通过引入能自适应环境的可变维粒子多种群共同寻优的思想，优化过程具有并行性、鲁棒性和自组织性等特性，无论是在简单环境还是在复杂环境条件下，其处理算法计算复杂度较小且变化不大，三个目标(最短距离、安全度和平滑性)可以简单而有效的通过随机选择系数γ₁,γ₂控制，因此计算效率高且较恒定，克服了动态变化的复杂环境中拓扑图法建立拓扑图就很复杂的弱点，解决了应用非常广泛的栅格法中计算代价随环境栅格数量增加而成指数增加的固有缺点，规划时效性的大幅提高使得移动机器人在复杂环境行进中动态在线规划路径成为可能。

路径合理性更高。本发明提出的无效维修正算法、去除冗余维信息算法以及粒子运动方向调整方法计算简单，除了能加快寻优过程的收敛外，通过对障碍物凸顶点***栅格的多层次预定义，使得优化出来的路径，在最短距离，安全性最好，平滑度最大之间的权衡可以根据随机选择系数快速决定，规划出来的路径也更合理。另外，本发明与其它很多方法不同，优化过程中一直使用代表有效路径的粒子进行寻优，迭代结束后，和其它利用智能优化方法寻优后结果中存在大量无效解或不能找到最优解不同，本发明算法结束后，优化结果全为有效解，且一定存在大量近似最优解，即使没有找到最优解，很多近似最优解也和最优解相似，因此这些合理的解大量存在也使得进一步将其用于对最优解结果要求不是太严格的动态在线路径规划成为可能。

算法可扩展性强。本发明除具有全局静态路径规划的效率高，合理性好等优点外，利用无效维修正算法的高效和自适应强等优点，在优化过程中，只对搜索起点和终点之间局部范围的障碍物区域进行合理路径寻找，可以快速锁定局部动态路径规划的下一级子目标节点。因此基于算法实时性好且结果中必定存在最优或近似最优的大量合理路径等关键技术优越性，可以将其扩展至动态局部在线路径规划的应用中，利用在全局规划路径节点集上多点滑动快速寻找局部目标位置的思想，利用机器人自带传感器测量外界动态变化的物理环境，再利用机器人自身里程计、GPS等定位设备快速在虚拟环境中定位并动态修改变化的环境信息，使移动机器人能在运动中快速找到合理的路径，满足未来智能机器人发展要求。

附图说明

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

图2是本发明中障碍物外凸顶点及对应***栅格定义图。

图3是本发明中***栅格随机选择系数γ₁和γ₂之间的关系图。

图4是本发明中实现可变维粒子群归类的两种膜结构。

图5是本发明中同一基本膜内母粒子及其产生的子粒子示意图。

图6是本发明中路径粒子维节点运动方向重调整示意图。

图7是本发明中无效维路径及其修正过程示意图。

图8是本发明中无效维路径修正及其去除冗余维信息过程示意图。

图9是本发明中实施例16·16栅格环境(O_s＝9，O_d＝1)优化结果图。

图10是本发明中实施例32·32栅格环境(O_s＝20，O_d＝1)优化结果图。

图11是本发明中实施例64·64栅格环境(O_s＝20)优化结果图。

具体实施方式

本发明的具体的实施方式为：

S1、环境建模，具体为：

S101、将物理环境信息映射为n·n的栅格型虚拟环境，对所述n·n个栅格进行标号，其中，n为不为零的自然数，所述栅格为正方形，所述栅格的边长SoG＝N×机器人直径，N≥2且N为自然数；

S102、对S101所述带有标号的栅格进行分类，分为障碍物栅格和自由栅格；

S103、对S102所述障碍物栅格的外凸顶点***栅格进行标记，其中，外凸顶点***栅格为自由栅格，其中，所述障碍物栅格的外凸顶点如图2中所示的P1、P2和P4，障碍物栅格的内凹顶点如图2中所示的P3，P1周围的栅格(1、2、8)为***栅格，P2周围的栅格(22、29、30)为***栅格，P4周围的栅格(39、46、47)为***栅格；

S104、对S103所述外凸顶点***栅格进行分类，分为对角顶点***栅格和侧边***栅格，定义所述对角顶点***栅格的随机选择概率系数为γ₁，定义所述侧边***栅格的随机选择概率系数为γ₂；

S105、实际环境的物理坐标到虚拟环境栅格序号之间的映射关系表示为p＝u·Noc+v，其中，p为序号数，x,y为物理坐标，SoG为单个栅格的边长，fix函数为低整位取整函数；

S2、输入路径规划参数，具体包括：机器人状态参数，起止位置及安全范围，其中，所述机器人状态参数包括：机器人尺寸大小、机器人行进速度、机器人最小转弯半径、机器人起点位置坐标信息和机器人期望到达的目标终点位置信息；

S3、设置适应度函数f＝K_d·Dis+K_f·S+K_s·SD，其中，K_d,K_f,K_s为权重系数，K_d用来调节最短距离Dis目标函数在最优路径中所占的比重，K_f用来调节平滑度S目标函数在最优路径中所占的比重，K_s用来调节安全度SD目标函数在最优路径中所占的比重，其中，Dis是从起始点到终止点的h个节点之间的距离之和，所述最短距离Dis目标函数为其中，L(a,a+1)是节点a和节点a+1之间的距离，x_a是节点a的X轴坐标值，x_a+1是节点a+1的X轴坐标值，y_a是节点a的Y轴坐标值，y_a+1是节点a+1的Y轴坐标值，所述平滑度S目标函数为S＝α·S_c+β·S_p，其中，α和β是权重系数， N_f是有效路径的总片段数，DA_l是线段之间夹角大于期望值阈值的数量，所述期望值阀值为机器人最小转弯半径夹角，S_min是最小路径片段数，对移动机器人而言，路径片段数越少，其转弯次数越少，里程计定位误差也相应越小，所述安全度SD目标函数为其中，d_r是第r条路径片段与其最近障碍物之间的最小距离，λ是安全度阀值，C_r为第r条路径片段的安全性罚值，所述安全度阀值λ为机器人与障碍物的最小欧几里得距离(Euclidean distance)；

S4、初始化有效路径母粒子和子粒子，具体为：

S401、在表层膜中创建m个子膜，在每个子膜中产生1个有效路径母粒子，共产生m个有效路径母粒子，对m个子膜进行标号，记作M_i,i＝1,2,...,m；

S402、在S401所述m个有效路径母粒子的基础上利用随机和特殊方向指导相结合的方式在所述m个有效路径母粒子对应的子膜M_i中产生c个有效路径子粒子，共产生m·c个有效路径粒子的初始种群；

S5、路径粒子分类与重归类，具体为：

S501、根据S401所述每个子膜M_i中的粒子维度的不同，将粒子分为k_i组；

S502、构建第i个子膜中的第j个基本膜将S501所述分类后的粒子放入所述基本膜中，其中，中粒子均为同维粒子，j＝1,2,...,k_i；

S503、运用粒子运动方向重调整方法对粒子速度进行重新初始化；

S6、对S502所述基本膜中路径粒子的适应度函数进行评价，具体为：

S601、在S502所述基本膜中根据S3所述适应度函数f＝K_d·Dis+K_f·S+K_s·SD计算基本膜中每个粒子对应的适应度函数值，找到每个粒子的个体最优值

S602、在S502所述基本膜中找到局部最优其中，t为子粒子代数；

S603、将第i个子膜里的局部最优送出到表层膜；

S604、比较S603所述得出同维的全局最优解其中，d∈{1,2,...,D}；

S605、将S604所述送回维数相匹配的基本膜中；

S7、在所有基本膜中，利用公式计算d维粒子的速度，利用公式X_id(t+1)＝V_id(t+1)+X_id(t)更新d维粒子的位置，其中，V_id(t)为粒子在第t代的速度，V_id(t+1)是粒子在t+1代的速度，是粒子在第t代的个体最优，X_id(t)是粒子在第t代的位置，是第t代同维数粒子的局部最优，是第t代同维粒子的全部最优，是第t代粒子经方向调整方法处理后的速度，δ₁和δ₂是比例系数，ρ是惯性权重系数，r₁,r₂,r₃是0和1间的随机数，c₁,c₂,c₃为加速度系数，X_id(t)是第t代粒子的位置，X_id(t+1)是第t+1代粒子的位置，V_id(t+1)第t+1代粒子的速度；

S8、判断当前粒子是否有无效维，即对S7所述更新位置后的d维粒子进行判断，若所述更新位置后的d维粒子存在无效维则进入S9，若所述更新位置后的d维粒子不存在无效维则进入S10；

S9、对S8所述存在无效维的粒子进行修正，修正后判断其无效性，直至不存在无效维转至S10，所述对S8所述存在无效维的粒子进行修正具体如下：

S901、判断S8所述存在无效维的粒子的情况，若存在无效维的粒子的某一维陷入障碍物中则为类型T1，若存在无效维的粒子的某两维跨越障碍物则为类型T2，其中，类型T1即存在无效维的粒子的节点陷入障碍物中，类型T2即存在无效维的粒子的节点之间的线段跨越障碍物；

S902、对类型T1进行无效维修正，包括：

S9021、计算S901所述陷入障碍物的节点到对应障碍物所有外凸顶点的点到点之间的直线距离；

S9022、对S9021计算所得直线距离进行从大到小排序，选出与陷入障碍物的节点的直线距离最小的外凸顶点；

S9023、将S9022选出的外凸顶点***栅格作为将要修改至的备选栅格，删去无效维节点；

S9024、将新节点***S9023所述被删去无效维节点的位置，其中，所述新节点为S9023所述备选栅格；

S903、对类型T2进行无效维修正，包括：

S9031、计算S901所述节点之间跨越障碍物的线段到对应障碍物所有外凸顶点的距离，即点到线之间的直线距离；

S9032、对S9031计算所得点到线之间的直线距离进行从大到小排序，选出直线距离最小的所对应的外凸顶点；

S9033、将S9032选出的外凸顶点***栅格作为将要修改至的备选栅格；

S9034、将新节点***S901所述跨越障碍物的线段两头的节点之间，其中，所述新节点为S9033所述备选栅格；

S10、判断当前粒子是否有冗维信息，如存在冗维信息则转入S11，如不存在冗维信息则转入S12；

S11、去除冗维信息：

S1101、对一条有效路径从起点到终点的所有节点依次排序并放入序列表L中，令起点为首节点，终点为尾节点，其中，所述序列表L为存储单元，例如链表或者数组；

S1102、取出S1101所述首节点，从第3个节点开始，判断第3个节点是否与首节点能无障碍连接，如果能链接则依次取下一节点，继续判断此节点与首节点的无障碍连接性，直至找到与首节点不能直接相连接的节点a，如果首节点与第3个节点不能无障碍连接，则说明第2个节点不是冗余维节点，再令第2个为首节点；

S1103、从序列表L中删除首节点和第a-1个节点之间的所有节点；

S1104、令第a-1个节点为首节点，重复S1102依次取其后的节点进行判断并去除冗余信息，直至尾节点出现；

S12、在基本膜中得到不同维数的最优路径粒子：各基本膜中的路径粒子经过S9的无效维路径粒子修正和S11的去冗余维信息处理后，每次得到的新路径粒子向较优或最优路径接近，其维数已经和初始状态的维数不同，同一子膜M_i内各基本膜溶解，去掉基本膜的封装，将新粒子重新释放回各自所属的子膜内，等待下一次迭代的分类；

S13、判断算法满足终止条件：如果循环次数t≥T＝200，则循环结束，输出包含最优路径的解集Φ，否则t＝t+1，算法跳转到S5。

下面结合附图对本发明的具体实施方式进行介绍。

本发明利用7·7栅格对具体操作步骤进行详细描述。试验采用Matlab作为实现工具，在CPU为AMD Sempron 1.6GHz，内存为1GB，操作***为Windows XP的计算机上进行求解。

如图1所示：

步骤1、环境建模。

将物理环境信息映射为7*7的栅格型虚拟环境，每个栅格为正方形，其尺寸为机器人直径大小的整数倍，对每个栅格标序号{0,1,2,…,48}，实际环境的物理坐标到虚拟环境栅格序号之间的映射关系式为p＝u·Noc+v，其中，p为序号数，x,y为物理坐标，SoG为单个栅格边长，栅格边长为特定移动机器人直径的整数倍，fix函数为底整位取整函数。

栅格分为两种类型：障碍物栅格和自由栅格，标记所有障碍物栅格的外凸顶点***栅格。外凸顶点***栅格由自由栅格构成，分为两类：对角顶点***栅格以及侧边***栅格。如图2中障碍物O的内凹顶点为p₃，而外凸顶点p₁，p₂，p₄分别被三个***栅格{1,2,8}、{29,22,30}和{46,39,47}包围，所有的***栅格有两类受f＝K_d·Dis+K_f·S+K_s·SD中的目标函数权重系数K_d,K_f,K_s控制的随机选择系γ₁和γ₂。γ₁主要受K_d控制，与γ₁有关的边缘栅格是{2,8,22,30,39,47}，γ₂主要受K_s和K_f控制，与γ₂有关的边缘栅格是{1,29,46}，这里K_d+K_f+K_s＝1，0.6≤K_d≤1，0≤γ₁≤1，0≤γ₂≤1，γ₁+γ₂＝1。γ₁和γ₂的关系如图3所示。

步骤2、输入路径规划参数。

路径规划的参数输入包括：机器人状态参数，起止位置及安全范围。其中，机器人状态参数包括：机器人直径大小R_d、机器人速度R_v，机器人最小转弯半径夹角等基本信息；起止位置：机器人起点位置坐标信息(x_s,y_s)，机器人期望到达的目标终点位置信息(x_g,y_g)；机器人允许距离障碍物或危险区域的最小距离

步骤3、

设置适应度函数。

f＝K_d·Dis+K_f·S+K_s·SD，其中，K_d,K_f,K_s为权重系数，用来调节最短距离Dis、平滑度S、安全度SD三个目标函数在最优路径中所占的比重。具体的三个目标函数表述如下：

距离目标函数：

Dis是从起始点到终止点的h个节点之间的距离之和；L(a,a+1)是节点a和a+1之间的距离，且这里x_a和x_a+1是节点a和a+1的X轴坐标值，y_a和y_a+1是节点a和a+1的Y轴坐标值，0＜a＜h-1，h的范围根据有效路径的具体长度来确定。

平滑度目标函数。

S＝α·S_c+β·S_p，其中，α和β是权重系数；N_f是有效路径的总片段数，DA_l是线段之间夹角大于期望值的数量，期望值阀值设为机器人最小转弯半径夹角，S_min是本组中最小路径片段数。对移动机器人而言，路径片段数越少，其转弯次数越少，里程计定位误差也相应越小。

安全度目标函数。

其中，d_r是第r条路径片段和所述第r条路径片段最近障碍物之间的最小距离，λ是安全度阀值，所述安全度阈值为机器人允许距离障碍物的最小欧几里得距离(Euclidean distance)。

步骤4、在表层膜及子膜中初始化有效路径母粒子及子粒子。

如图4的(a)所示，首先在表层膜中创建m＝13个子膜，即i＝1,2,...,13，在每个子膜M_i中通过贪心算法在20·20栅格环境的每一行随机选择自由栅格构成有效路径的方式产生1个有效路径母粒子，共产生13个有效路径母粒子。再利用每个母粒子在各自的子膜M_i中产生10个有效路径子粒子,共产生13·10个有效路径粒子的初始种群。如图5所示为一个基本膜中一个20维的母粒子产生10个6-9维子粒子的结果。

步骤5.、路径粒子分类与重归类。

根据每个子膜M_i中粒子维数的不同分为k_i组，每一子膜M_i内的粒子分类后，构建第i个子膜里的第j个基本膜并将同维子粒子放入同一个基本膜中。各子膜和子膜内的各基本膜逻辑关系图4的(b)所示。

重归类完毕后，运用粒子运动方向重调整方法对归类后的粒子速度进行重新初始化。粒子运动方向重调整方法如图6中所示：T为目标节点，S为起始节点，对S和T之间的3个粒子维节点p₁，p₂和p₃按路径经过顺序排序后，维节点p₃的运动方向指向其目标节点T，维节点p₂的运动方向指p₃，维节点p₁的运动方向指p₂，得到新的粒子速度此思想受钓鱼活动中，鱼线收紧过程的启发，有效路径类似一根鱼线，终止节点是最上级节点，起始节点是最下级节点，从终止节点处开始收紧鱼线(路径)时，除终止节点外的所有节点将要移动的方向是其相邻的上级节点。使用这种方法能使标准粒子的随机运动变成一种快速趋优运动。

步骤6、评价各基本膜中路径粒子的适应度函数。

在基本膜中根据f＝K_d·Dis+K_f·S+K_s·SD计算每个粒子对应的适应度函数值，找到每个粒子的个体最优值在基本膜中找到局部最优将第i个子膜里的局部最优送出至表层膜。比较得到同维的全局最优解将表层膜里的全局最优解送回至子膜中与其维数匹配的基本膜中。其中，i＝1,2,...,m,j＝1,2,...,k_i，d∈{1,2,...,D}。

步骤7、更新各基本膜中路径粒子的速度和位置。

利用公式计算d维粒子的速度，其中，V_id(t)和V_id(t+1)是粒子分别在第t和t+1代的速度，是粒子在第t代的个体最优，X_id(t)是粒子在第t代的位置，是第t代同维数粒子的局部最优，是第t代同维粒子的全部最优，是第t代粒子经方向调整方法处理后的速度，δ₁和δ₂是比例系数，ρ是惯性权重系数，r₁,r₂和r₃是0和1间的随机数，c₁,c₂和c₃为加速度系数。

利用X_id(t+1)＝V_id(t+1)+X_id(t)更新d维粒子的位置。其中，X_id(t)和X_id(t+1)是第t和t+1代粒子的位置，V_id(t+1)第t+1代粒子的速度。

步骤8、判断当前的新粒子是否有无效维路径。

判断当前的新粒子是否有无效维路径片段，如果有进入下一步，既步骤9进行无效维修正，否则转入步骤10。

步骤9、修正路径粒子无效维。

经过步骤7位置更新后的新路径粒子可能出现某些节点维无效的情况，两类无效路径分别在图7的(a)-(b)中。d₁是节点2和p₁之间的点间距，d₂是节点2和p₂之间的点间距，d₃是线段L和p₁之间的点到线间距，d₄是线段L和p₂之间的点到线间距。图7的(a)中，{1，2，3，4}(类型T1)表示的无效路径中的节点2必须被修正；图7的(b)中，跨越障碍物的路径片段L(类型T2)必须被分段。

无效维路径修正算法具体步骤如下：

步骤91.判断路径有效性，如果有效则略过，否则执行修正算法。

步骤92.判断无效路径类型T1或类型T2。

步骤93.如果无效路径为类型T2，跳转到步骤94,否则执行下面步骤：

(I)计算无效点p₀和障碍物外凸顶点p_i之间的点间距d_i并按距离大小升序排序；

(II)利用最小点间距d_i选择未被使用的p_i，并得到***栅格

(III)利用γ₁和γ₂随机选择***栅格并得到其中心坐标P_{xy_i}；

(IV)利用P_{xy_i}代替无效路径节点；

(V)再次判断路径有效性，有效则终止算法，否则跳转到步骤92；

步骤94.类型T2根据以下步骤修正：

(A)计算无效路径片段L和障碍物外凸顶点p_i之间的点间距d′_i并按距离大小升序排序；

(B)利用最小点间距d′_i选择未被使用的p_i,并得到***栅格

(C)利用γ₁和γ₂随机选择***栅格并得到其中心坐标P′_{xy_i}；

(D)在代表无效路径片段的两节点之间***新点P′_{xy_i}，分别得到新的两路径片段path_{s_1}和path_{s_2}；

(E)分别判断path_{s_1}和path_{s_2}的有效性，如果无效则跳转到步骤94。否则终止算法。

图7的(c)-(d)分别对应图7的(a)-(b)修复后的有效路径。

步骤10、判断当前新路径粒子是否有冗余维信息。

判断当前新路径粒子是否有冗余维信息，如果有,则进入下一步，即步骤11，进行去除冗余信息处理；否则转入步骤12。

步骤11、去除路径粒子冗余维信息。

经过步骤9处理后，各路径粒子维节点全部有效，但含有大量冗余维信息，对其进行冗余维信息去除处理。其具体算法步骤描述如下：

AA、从起点到终点对一条有效路径的所有维节点依次排序,得到序列n_i,i＝1,2,...,d,这里d是粒子的维数，n₁和n_d分别对应起始节点和终止节点；

BB、判断n_i和n_j之间的路径片段L_ij(开始时,i＝1,j＝3)有效性。如果无效，则将节点i和j-1***光顺路径集合P_f中，此时P_f＝{i,j-1}，并令i＝j-1,j＝j+1后，继续判断路径片段L_ij的可行性。如果有效，则令j＝j+1后，继续判断路径片段L_ij的可行性，直至L_ij为无效路径后停止，将i和j-1***P_f，并令i＝j-1，重复此步，直至j＝d。

图8给出无效维路径粒子修正后再去掉冗余维信息的实例。路径节点{1，2，3，4，5，6}中{3，4}为无效路径片段，需修正。γ₁＞＞γ₂，则{3，4}被{3，8}和{8，4}代替。{3，8}仍然为无效路径，进一步修正为{3，7}和{7，8}片段，得到修正后有效路径{1，2，3，7，8，4，5，6}。再经过去冗余维信息算法,得到最终有效路径{1，8，6}。

步骤12、各基本膜中得到不同维数的最优路径粒子。

在本此迭代中，各基本膜中的路径粒子经过标准粒子群速度和位置更新公式更新后存在大量无效路径片段,经步骤9的无效维路径粒子修正后其维数有可能增加且存在冗余维信息再经步骤11的去除冗余维信息处理后，维数减少的同时也去除了全部的冗余信息，每次得到的新路径粒子都会向较优或最优路径靠近，同时其维数也已经和迭代开始前的维数不同，同一子膜M_i内各基本膜溶解，去掉基本膜的封装,将新粒子重新释放回各自所属的子膜内,等待下一次迭代的重新分类。

步骤13、判断该算法是否满足终止条件。

若该算法满足终止条件,即如果循环次数t≥100,则循环结束,输出包含最优路径的解集Φ，否则，t＝t+1，算法跳转到步骤5。

下面结合附图对本发明的具体例进行介绍。

采用16·16，32·32和64·64的环境栅格求解移动机器人的路径规划问题。试验采用Matlab作为实现工具，在CPU为AMD Sempron 1.6GHz，内存为1GB，操作***为WindowsXP的计算机上进行求解。

实施例、

针对本发明，设计几类环境模型检验本发明算法的效果,分别使用如图9所示的16*16，32*32，64*64几种栅格数成指数增长的环境模型来对算法进行验证。其中O_s为静态障碍物，O_d为动态障碍物，其中动态障碍物为算法迭代开始运行后利用功能函数形参的方式传入主程序中改变环境状态变量表得到，栅格尺寸大小等于轮式移动机器人直径R_d，对静态障碍物做一个栅格单位的膨化处理，考虑的目标函数关系分三种情况：

Case1:K_d＝1，K_f＝K_s＝0，γ₁＝1,γ₂＝0；

Case2:K_d＝0.6，K_f＝K_s＝0.2，γ₁＝0,γ₂＝1；

Case3:K_d＝0.8，K_f+K_s＝0.2，γ₁+γ₂＝1。

对16·16，32·32的栅格环境采用为子膜个数m＝13，种群大小130。对16·16，64·64的栅格环境采用为子膜个数m＝15，种群大小150。分别进行100次独立实验，找到的部分最优和较优路径分别如图9，10和11所示，实验结果如表1所示。

表1

从表1可见,障碍物从9增长到20，栅格从16·16到64·64以4ⁿ指数倍方式增长，而时间消耗并未如其它基于栅格的算法成***式增长,而是从0.84线性增长到2.68。此外，有动态障碍物出现时，时间消耗也增加很小。本发明算法搜索最优解和近似最优解的能力都很强，和其它基于智能优化算法的路径规划算法结束后不一定能找到最优解不同，本发明的算法无论是简单环境还是复杂环境都能找到最优解和大量的近似最优解，效率快且路径合理。

综上,本发明能利用无效维路径修正算法和去冗余维信息算法实时快速的将无效路径修复为有效路径，并去掉冗余信息，加快了优化算法的收敛过程；受钓鱼活动启发的粒子速度调整技术也进一步加快收敛，同时也进一步提高了路径的合理性；动态膜结构的思想将标准粒子群的只有一个种群寻优分化为适应问题驱动而产生的多个子种群共同寻优的过程，也进一步提高了粒子群算法的性能；解决了路径规划问题中应用非常广泛的栅格类建模环境固有的缺点。本发明的核心算法除了可以解决静态已知环境下的全局移动机器人路径规划问题，同时可以将其扩展为动态在线的路径规划(利用机器人的传感器信息并结合滑动窗口的思想可以进一步解决动态未知条件下的局部路径规划问题)。本发明平衡了合理性、实时性、实用性、自适应性、可扩展性之间的关系，可以作为解决静态已知或动态未知环境下的基础算法。

Claims (5)

1.一种基于可变维粒子群膜算法的移动机器人路径规划方法，其特征在于，包括以下步骤：

S1、环境建模，具体为：

S101、将物理环境信息映射为n·n的栅格型虚拟环境，对所述n·n个栅格进行标号，其中，n为不为零的自然数，所述栅格为正方形，所述栅格的边长SoG＝N×机器人直径，N≥2且N为自然数；

S102、对S101所述带有标号的栅格进行分类，分为障碍物栅格和自由栅格；

S103、对S102所述障碍物栅格的外凸顶点***栅格进行标记，其中，外凸顶点***栅格为自由栅格；

S104、对S103所述外凸顶点***栅格进行分类，分为对角顶点***栅格和侧边***栅格，定义所述对角顶点***栅格的随机选择概率系数为γ₁，定义所述侧边***栅格的随机选择概率系数为γ₂，其中，0≤γ₁≤1，0≤γ₂≤1，γ₁+γ₂＝1；

S105、实际环境的物理坐标到虚拟环境栅格序号之间的映射关系表示为p＝u·Noc+v，其中，p为序号数，x,y为物理坐标，SoG为单个栅格的边长，fix函数为低整位取整函数，Noc为列数；

S2、输入路径规划参数，具体包括：机器人状态参数，起止位置及安全范围；

S3、设置适应度函数f＝K_d·Dis+K_f·S+K_s·SD，其中，K_d,K_f,K_s为权重系数，K_d用来调节最短距离Dis目标函数在最优路径中所占的比重，K_f用来调节平滑度S目标函数在最优路径中所占的比重，K_s用来调节安全度SD目标函数在最优路径中所占的比重，其中，Dis是从起始点到终止点的h个节点之间的距离之和；

S4、初始化有效路径母粒子和子粒子，具体为：

S401、在表层膜中创建m个子膜，在每个子膜中产生1个有效路径母粒子，共产生m个有效路径母粒子，对m个子膜进行标号，记作M_i,i＝1,2,...,m；

S402、在S401所述m个有效路径母粒子的基础上利用随机和特殊方向指导相结合的方式在所述m个有效路径母粒子对应的子膜M_i中产生c个有效路径子粒子，共产生m·c个有效路径粒子的初始种群；

S5、路径粒子分类与重归类，具体为：

S501、根据S401所述每个子膜M_i中的粒子维度的不同，将粒子分为k_i组；

S502、构建第i个子膜中的第j个基本膜将S501所述分类后的粒子放入所述基本膜中，其中，中粒子均为同维粒子，j＝1,2,...,k_i；

S503、运用粒子运动方向重调整方法对粒子速度进行重新初始化；

S6、对S502所述基本膜中路径粒子的适应度函数进行评价，具体为：

S601、在S502所述基本膜中根据S3所述适应度函数f＝K_d·Dis+K_f·S+K_s·SD计算基本膜中每个粒子对应的适应度函数值，找到每个粒子的个体最优值

S602、在S502所述基本膜中找到局部最优其中，j＝1,2,...,k_i，t为子粒子代数；

S603、将第i个子膜里的局部最优送出到表层膜；

S604、比较S603所述得出同维的全局最优解其中，d表示粒子的维度大小，是不为零的自然数；

S605、将S604所述送回维数相匹配的基本膜中；

S7、利用公式计算d维粒子的速度，利用公式X_id(t+1)＝V_id(t+1)+X_id(t)更新d维粒子的位置，其中，V_id(t)为粒子在第t代的速度，V_id(t+1)是粒子在t+1代的速度，是粒子在第t代的个体最优，X_id(t)是粒子在第t代的位置，是第t代同维数粒子的局部最优，是第t代同维粒子的全部最优，是第t代粒子经方向调整方法处理后的速度，δ₁和δ₂是比例系数，ρ是惯性权重系数，r₁,r₂,r₃是0和1间的随机数，c₁,c₂,c₃为加速度系数，X_id(t+1)是第t+1代粒子的位置，V_id(t+1)第t+1代粒子的速度；

S8、判断当前粒子是否有无效维，即对S7所述更新位置后的d维粒子进行判断，若所述更新位置后的d维粒子存在无效维则进入S9，若所述更新位置后的d维粒子不存在无效维则进入S10；

S9、对S8所述存在无效维的粒子进行修正，修正后判断其无效性，直至不存在无效维转至S10；

S10、判断当前粒子是否有冗维信息，如存在冗维信息则转入S11，如不存在冗维信息则转入S12；

S11、去除冗维信息：

S1101、对一条有效路径从起点到终点的所有节点依次排序并放入序列表L中，令起点为首节点，终点为尾节点；

S1102、取出S1101所述首节点，从第3个节点开始，判断第3个节点是否与首节点能无障碍连接，如果能链接则依次取下一节点，继续判断此节点与首节点的无障碍连接性，直至找到与首节点不能直接相连接的节点a，如果首节点与第3个节点不能无障碍连接，则说明第2个节点不是冗余维节点，再令第2个为首节点；

S1103、从序列表L中删除首节点和第a-1个节点之间的所有节点；

S1104、令第a-1个节点为首节点，重复S1102依次取其后的节点进行判断并去除冗余信息，直至尾节点出现；

S12、在基本膜中得到不同维数的最优路径粒子：各基本膜中的路径粒子经过S9的无效维路径粒子修正和S11的去冗余维信息处理后，每次得到的新路径粒子向较优或最优路径接近，其维数已经和初始状态的维数不同，同一子膜M_i内各基本膜溶解，去掉基本膜的封装，将新粒子重新释放回各自所属的子膜内，等待下一次迭代的分类；

S13、判断算法满足终止条件：如果循环次数t≥T，则循环结束，输出包含最优路径的解集Φ，否则t＝t+1，算法跳转到S5，其中，T＝100。

2.根据权利要求1所述的一种基于可变维粒子群膜算法的移动机器人路径规划方法，其特征在于：S2所述机器人状态参数包括：机器人尺寸大小、机器人行进速度、机器人最小转弯半径、机器人起点位置坐标信息和机器人期望到达的目标终点位置信息。

3.根据权利要求1所述的一种基于可变维粒子群膜算法的移动机器人路径规划方法，其特征在于：

S3所述最短距离Dis目标函数为其中，L(a,a+1)是节点a和节点a+1之间的距离，x_a是节点a的X轴坐标值，x_a+1是节点a+1的X轴坐标值，y_a是节点a的Y轴坐标值，y_a+1是节点a+1的Y轴坐标值；

S3所述平滑度S目标函数为S＝α·S_c+β·S_p，其中，α和β是权重系数，N_f是有效路径的总片段数，DA_l是线段之间夹角大于期望值阈值的数量，所述期望值阀值为机器人最小转弯半径夹角，S_min是最小路径片段数；

S3所述安全度SD目标函数为其中，d_r是第r条路径片段与其最近障碍物之间的最小距离，λ是安全度阀值，C_r为第r条路径片段的安全性罚值。

4.根据权利要求3所述的一种基于可变维粒子群膜算法的移动机器人路径规划方法，其特征在于：所述安全度阀值λ为机器人与障碍物的最小欧几里得距离。

5.根据权利要求1所述的一种基于可变维粒子群膜算法的移动机器人路径规划方法，其特征在于：S9所述对S8所述存在无效维的粒子进行修正，具体方法如下：

S901、判断S8所述存在无效维的粒子的情况，若存在无效维的粒子的某一维陷入障碍物中则为类型T1，若存在无效维的粒子的某两维跨越障碍物则为类型T2，其中，类型T1即存在无效维的粒子的节点陷入障碍物中，类型T2即存在无效维的粒子的节点之间的线段跨越障碍物；

S902、对类型T1进行无效维修正，包括：

S9021、计算S901所述陷入障碍物的节点到对应障碍物所有外凸顶点的点到点之间的直线距离；

S9022、对S9021计算所得直线距离进行从大到小排序，选出与陷入障碍物的节点的直线距离最小的外凸顶点；

S9023、将S9022选出的外凸顶点***栅格作为将要修改至的备选栅格，删去无效维节点；

S9024、将新节点***S9023所述被删去无效维节点的位置，其中，所述新节点为S9023所述备选栅格；

S903、对类型T2进行无效维修正，包括：

S9031、计算S901所述节点之间跨越障碍物的线段到对应障碍物所有外凸顶点的距离，即点到线之间的直线距离；

S9032、对S9031计算所得点到线之间的直线距离进行从大到小排序，选出直线距离最小的所对应的外凸顶点；

S9033、将S9032选出的外凸顶点***栅格作为将要修改至的备选栅格；

S9034、将新节点***S901所述跨越障碍物的线段两头的节点之间，其中，所述新节点为S9033所述备选栅格。