本发明公开一种机器人路径规划方法，在人工鱼群算法基础上引入方向算子来提升聚群、追尾、甚至是觅食等鱼群行为的准确度和成功率，增加免疫记忆操作来提高算法的全局搜索能力并减少局部极值出现的概率。在两种典型栅格地图环境下的仿真实验表明，这一免疫‑方向性人工鱼群算法与快速遗传算法和常规人工鱼群算法相比，具有更好的结果稳定性、更短的计算时间和更接近最优路径的可行解。
1.一种机器人路径规划方法，其特征是，包括如下步骤：
步骤1，将移动机器人的行走区域分割成规则且均匀的栅格，每个栅格只有占据或自由两种状态，占据状态的栅格表示障碍物，自由状态的栅格表示可行点；在可行点中决定好机器人的起点与终点；
步骤2，随机生成N条初始的路径，这些路径中的每一条路径的第一个路径点为起点，最后一个路径点为终点；
步骤3，每条路径随机选择鱼群的聚群行为或者追尾行为，根据聚群行为描述或追尾行为描述来改变该条路径中除起点终点之外的其他路径点的位置，以生成一条新的路径；
步骤4，经过步骤3的处理后，每条路径都会得到一条新的路径，由此得到2N条路径，其中N条为原路径，N条为新路径；
步骤4.1，找出这2N条路径中路径长度最小的一条路径，并记录该条最小路径的路径长度及其路径点；
步骤4.2，计算这2N条路径的适应度函数值，并从2N条原路径中选出X条适应度函数值大的路径；
步骤4.3，分别对这X条路径的适应度函数值进行矢量距运算，得到每条路径的矢量距,并将每条路径的矢量距除以X条路径的总的矢量距，得到每条路径的选择概率；
步骤5，将步骤4.1所记录的最小路径的路径长度和上一次循环迭代所选定的最小路径的路径长度进行比较，选定两者中路径长度相对小的路径作为本次循环迭代所选定的最小路径；同时从X条路径中选择N条选择概率相对大的路径作为下一次迭代的初始的路径；
步骤6，循环执行步骤3-5，直至选定的最小路径保持不变的次数达到预设的次数不变阈值或循环次数达到预设的循环次数阈值时，则将该选定的最小路径作为最优路径，将最优路径的路径点依次连起来便为机器人的行走路径；
2.根据权利要求1所述的一种机器人路径规划方法，其特征是，步骤3中，所述的聚群行为描述具体为：步骤3.1.1，遍历N条路径；
步骤3.1.2，对于当前路径D，其中D∈N，计算其余N-1条路径的中心位置之间与当前路径的中心位置的中心距离，选出中心距离在预设中心距离范围visual内的路径作为选中路径；
步骤3.1.3，判定是否需要进行聚群学习，即若当前路径D的路径长度乘以预设的拥挤因子δ乘以选中路径的条数小于所有选中路径的路径长度平均值，则认为当前路径D需要向选中路径学习，并进入步骤3.1.4；否则，进入步骤4；
步骤3.1.4，遍历当前路径D中除了起点跟终点的其他路径点；对每个路径点来说，做两条矢量，其中一条矢量方向是路径点到所有选中路径的平均中心位置方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向；
步骤3.1.5，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤
3.根据权利要求1所述的一种机器人路径规划方法，其特征是，步骤3中，所述的追尾行为描述具体为：步骤3.2.1，遍历N条路径；
步骤3.2.2，对于当前路径D，其中D∈N，计算其余N-1条路径的中心位置之间与当前路径的中心位置的中心距离，选出中心距离在预设中心距离范围visual内的路径作为选中路径，并从选中路径中选出路径长度最小的路径Y；
步骤3.2.3，判定是否需要进行追尾学习，即若当前路径D的路径长度乘以预设的拥挤因子δ乘以选中路径的条数小于路径Y的路径长度平均值，则认为当前路径D需要向路径Y学习，并进入步骤3.2.4；否则，进入步骤4；
步骤3.2.4，遍历当前路径D中除了起点跟终点的其他路径点；对每个路径点来说，做两条矢量，其中一条矢量方向是路径点到路径Y的中心位置方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向；
步骤3.2.5，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤
4.根据权利要求1所述的一种机器人路径规划方法，其特征是，步骤3还进一步包括：觅食行为，该觅食行为描述具体为：步骤3.3.1，遍历当前执行觅食行为的路径中除了起点跟终点的每一个路径点；
步骤3.3.2，对于当前路径点，首先随机选取一个方向，然后做两条矢量，其中一条矢量方向是路径点到随机选取的方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向F；
步骤3.3.3，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤
4；否则，返回步骤3.3.2，下一个路径点尝试移动，直至当前执行觅食行为的路径的所有路径点均移动完成。
5.根据权利要求2、3或4所述的一种机器人路径规划方法，其特征是，当前路径点D再向前进方向移动的随机路径长度需要小于预设移动路径长度范围step。
6.根据权利要求1所述的一种机器人路径规划方法，其特征是，步骤4.2和4.3中，X条路径包括从N条原路径中选出的p条适应度函数值大的路径和从N条新路径中选出的w条适应度函数值大的路径；其中N≤X≤2N，0≤p≤N，0≤w≤N。
7.根据权利要求1或6所述的一种机器人路径规划方法，其特征是，
步骤4.3之后还进一步包括，步骤4.4，按选择概率的大小，对X条路径进行排序的步骤，其中N≤X≤2N。
8.根据权利要求1所述的一种机器人路径规划方法，其特征是，步骤5中，在进行第一次循环迭代时，用于与记录的最小路径的路径长度进行比较的上一次循环迭代所选定的最小路径的路径长度为设定的初始值0。
一种机器人路径规划方法
技术领域
[0001] 本发明属于机器人人工智能技术领域，具体涉及一种机器人路径规划方法。
背景技术
[0002] 路径规划作为移动机器人导航关键技术之一受到广泛重视。机器人路径规划是在一定环境下，满足一定的优化准则，如工作代价最小、行走路线最短、行走时间最少或能量消耗最少等标准，在运动空间中找到一条从起始状态到目标状态、可以避开障碍物的最优或者接近最优的路径。传统优化方法，如人工势场法、可视图法和栅格法等，在机器人路径规划这类复杂非线性优化问题中缺乏足够的鲁棒性。
[0003] 随着人工智能技术的不断发展，神经网络算法、遗传算法、粒子群算法和人工鱼群算法等智能仿生路径规划算法得到应用，已取得一系列研究成果，形成一系列典型规划算法。J.Lee引入遗传算法，在自然选择过程中采用两种评估函数，得到了一种简单环境下的快速遗传算法(Fast Genetic Algorithm，FGA)。实验表明该算法能满足实时性的要求，但是路径长度偏长。莫宏伟在粒子群优化算法(Particle Swarm Optimization,PSO)基础上，引入生物地理中迁移算子之间的信息共享解决方案，将仿生优化(Biogeography-based Optimization，BBO)思想与之结合，形成一种新的仿生粒子群算法BPSO，优点是编程易于实现，算法运算速度快，缺点是路径长度过大，难以准确得到路径规划的最优解。Joon-Woo Lee等人针对蚁群算法(Ant Colony Optimization,ACO)会导致局部最优的问题，提出异构蚁群算法(Heterogeneous ACO，HACO)，但是该算法运行时间长，实时性能不高。Z.-H.Yao等人将人工鱼群算法(Artificial Fish Swarm Algorithm，AFSA)应用于机器人路径规划，具有鲁棒性强等优点，但也存在后期收敛速度低、易陷入局部最优解，导致最终结果精度下降等缺点。
发明内容
[0004] 本发明所要解决的是传统人工鱼群算法应用于机器人路径规划存在后期收敛速度低和易陷入局部最优解的问题，提供一种机器人路径规划方法。
[0005] 为解决上述问题，本发明是通过以下技术方案实现的：
[0006] 一种机器人路径规划方法，包括如下步骤：
[0007] 步骤1，将移动机器人的行走区域分割成规则且均匀的栅格，每个栅格只有占据或自由两种状态，占据状态的栅格表示障碍物，自由状态的栅格表示可行点；在可行点中决定好机器人的起点与终点；
[0008] 步骤2，随机生成N条初始的路径，这些路径中的每一条路径的第一个路径点为起点，最后一个路径点为终点；
[0009] 步骤3，每条路径随机选择鱼群的聚群行为或者追尾行为，根据聚群行为描述或追尾行为描述来改变该条路径中除起点终点之外的其他路径点的位置，以生成一条新的路径；
[0010] 步骤4，经过步骤3的处理后，每条路径都会得到一条新的路径，由此得到2N条路径，其中N条为原路径，N条为新路径；
[0011] 步骤4.1，找出这2N条路径中路径长度最小的一条路径，并记录该条最小路径的路径长度及其路径点；
[0012] 步骤4.2，计算这2N条路径的适应度函数值，并从2N条原路径中选出X条适应度函数值大的路径；
[0013] 步骤4.3，分别对这X条路径的适应度函数值进行矢量距运算，得到每条路径的矢量距,并将每条路径的矢量距除以X条路径的总的矢量距，得到每条路径的选择概率；
[0014] 步骤5，将步骤4.1所记录的最小路径的路径长度和上一次循环迭代所选定的最小路径的路径长度进行比较，选定两者中路径长度相对小的路径作为本次循环迭代所选定的最小路径；同时从X条路径中选择N条选择概率相对大的路径作为下一次迭代的初始的路径；
[0015] 步骤6，循环执行步骤3-5，直至选定的最小路径保持不变的次数达到预设的次数不变阈值或循环次数达到预设的循环次数阈值时，则将该选定的最小路径作为最优路径，将最优路径的路径点依次连起来便为机器人的行走路径；
[0016] 其中N≥1,N≤X≤2N。
[0017] 上述步骤3中，所述的聚群行为描述具体为：
[0018] 步骤3.1.1，遍历N条路径；
[0019] 步骤3.1.2，对于当前路径D，其中D∈N，计算其余N-1条路径的中心位置之间与当前路径的中心位置的中心距离，选出中心距离在预设中心距离范围visual内的路径作为选中路径；
[0020] 步骤3.1.3，判定是否需要进行聚群学习，即若当前路径D的路径长度乘以预设的拥挤因子δ乘以选中路径的条数小于所有选中路径的路径长度平均值，则认为当前路径D需要向选中路径学习，并进入步骤3.1.4；否则，进入步骤4；
[0021] 步骤3.1.4，遍历当前路径D中除了起点跟终点的其他路径点；对每个路径点来说，做两条矢量，其中一条矢量方向是路径点到所有选中路径的平均中心位置方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向；
[0022] 步骤3.1.5，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤4；若路径点能顺利前进，则直接进入步骤4；
[0023] 其中0＜δ＜1。
[0024] 上述步骤3中，所述的追尾行为描述具体为：
[0025] 步骤3.2.1，遍历N条路径；
[0026] 步骤3.2.2，对于当前路径D，其中D∈N，计算其余N-1条路径的中心位置之间与当前路径的中心位置的中心距离，选出中心距离在预设中心距离范围visual内的路径作为选中路径，并从选中路径中选出路径长度最小的路径Y；
[0027] 步骤3.2.3，判定是否需要进行追尾学习，即若当前路径D的路径长度乘以预设的拥挤因子δ乘以选中路径的条数小于路径Y的路径长度平均值，则认为当前路径D需要向路径Y学习，并进入步骤3.2.4；否则，进入步骤4；
[0028] 步骤3.2.4，遍历当前路径D中除了起点跟终点的其他路径点；对每个路径点来说，做两条矢量，其中一条矢量方向是路径点到路径Y的中心位置方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向；
[0029] 步骤3.2.5，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤4；若路径点能顺利前进，则直接进入步骤4；
[0030] 其中0＜δ＜1。
[0031] 上述步骤3还进一步包括：每条路径在执行完聚群或者追尾行为后，如果该条路径的路径长度不能变得更小，则需要进一步执行觅食行为。
[0032] 上述步骤3中，所述的觅食行为描述具体为：
[0033] 步骤3.3.1，遍历当前执行觅食行为的路径中除了起点跟终点的每一个路径点；
[0034] 步骤3.3.2，对于当前路径点，首先随机选取一个方向，然后做两条矢量，其中一条矢量方向是路径点到随机选取的方向，另一条矢量方向是路径点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向F；
[0035] 步骤3.3.3，当前路径点再向前进方向移动随机路径长度；若路径点不能顺利前进，即超出行走区域边界、或者前进到障碍点上、或者移动前当前路径D的路径长度比移动后当前路径D的路径长度小，则此路径点不执行前进操作，并将失败次数加1，并进入下一个路径点；当失败次数达到预设的失败次数阈值时，则视为此路径前进失败，放弃前进，进入步骤4；否则，返回步骤3.3.2，下一个路径点尝试移动，直至当前执行觅食行为的路径的所有路径点均移动完成。
[0036] 上述方案中，当前路径点D再向前进方向移动的随机路径长度需要小于预设移动路径长度范围step。
[0037] 上述步骤4.2和4.3中，X条路径包括从N条原路径中选出的p条适应度函数值大的路径和从N条新路径中选出的w条适应度函数值大的路径；其中N≤X≤2N，0≤p≤N，0≤w≤N。
[0038] 上述步骤4.3之后还进一步包括，步骤4.4，按选择概率的大小，对X条路径进行排序的步骤，其中N≤X≤2N。
[0039] 上述步骤5中，在进行第一次循环迭代时，用于与记录的最小路径的路径长度进行比较的上一次循环迭代所选定的最小路径的路径长度为设定的初始值0。
[0040] 与现有技术相比，本发明为免疫-方向性人工鱼群算法IDAFSA(Immune-Directional Artificial Fish Swarm Algorithm，其在人工鱼群算法基础上引入方向算子来提升聚群、追尾、以及觅食鱼群行为的准确度和成功率，增加免疫记忆操作来提高算法的全局搜索能力并减少局部极值出现的概率。在两种典型栅格地图环境下的仿真实验表明，这一免疫-方向性人工鱼群算法IDAFSA算法与快速遗传算法(FGA)和常规人工鱼群算法(AFSA)相比，具有更好的结果稳定性、更短的计算时间和更接近最优路径的可行解。
附图说明
[0041] 图1为一种机器人路径规划方法的流程图。
[0042] 图2为一种机器人路径规划方法的仿真路径结果图。
具体实施方式
[0043] 针对传统AFSA算法后期收敛速度降低的情况，本发明对鱼群中单条鱼的状态增加方向算子来提升鱼群觅食、聚群、追尾等行为能力与成功率，有利于加快收敛速度。同时将免疫记忆特性引入到人工鱼群算法中来提高算法的全局搜索能力并增强避免限于局部解的能力，我们将这一改进算法称为免疫-方向性人工鱼群算法(Immune-Directional Artificial Fish Swarm Algorithm,IDAFSA)算法。
[0044] 基于免疫-方向性人工鱼群算法的一种机器人路径规划方法，如图1所示，具体包括如下步骤：首先确定整个环境模型，包括起点、终点、障碍物以及可行点，精确描述适应度函数。然后将免疫特性引入人工鱼群算法的优化，求解出最优的可行解。最后对两个栅格地图的仿真实验获得的数据进行分析。
[0045] IDAFSA算法开始优先选择聚群或追尾行为，如果不能变得更好则选择觅食行为。然后对原来的数据进行修改，然后进行免疫选择。没有满足结束条件则重新选择聚群或追尾行为。
[0046] 1基础定义
[0047] 1.1对路径进行定义
[0048] 利用Dijkstra算法的路径编码方法，将第k条鱼游动的路径点从起点S到终点T逐一连接，生成路径结果Pk，连接的过程中不会触碰到障碍物。每个路径点都有一个唯一的坐标，设第i个路径点的坐标为Ai＝(xi,yi)，则第k条鱼的路径结果Pk为
[0049] Pk＝{S,A1,…,Ai,…An,T}
[0050] (1)
[0051] ＝{(Sx,Sy),(x1,y1),…,(xi,yi),…(xn,yn),T}
[0052] 其中n为路径点的总数。这里需要注意起点S＝(Sx,Sy)和终点T＝(Tx,Ty)对所有人工鱼都是相同的。优化工作就是调整Ai(i＝1,…,n)的位置，寻找具有最短路径的人工鱼，从而得到机器人在规划空间的最优(或近似最优)移动路径。
[0053] 1.2路径长度(即路径值)的定义
[0054] 路径长度就是将路径上的各个路径点依次连起来的之后，计算每条连线的长度总和。
[0055] 假设一个路径的路径点坐标分别是{(x1,y1),(x2,y2),……,(xn,yn)}，则该条路径的路径长度为：
[0056] 1.3平均路径长度的定义
[0057] 假设总共有n条路径，则这n个路径的路径长度的平均值就是平均路径长度。
[0058] 1.4中心位置的定义
[0059] 每条路径都有一个中心位置，其具体体现为一个坐标值，该坐标值是该条路径的所有路径点的坐标值加起来的平均值。
[0060] 假设一个路径的路径点坐标分别是{(x1,y1),(x2,y2),……,(xn,yn)}，则该条路径的中心位置为：
[0061] 1.5平均中心位置的定义
[0062] 假设总共有n条路径，则这n个路径的中心位置的平均值就是平均中心位置。
[0063] 1.6对适应度函数进行改进定义
[0064] 路径规划要求满足两个条件：免碰撞和路径最优。免碰撞是路径规划的基本条件，是机器安全通过工作空间的保证。免碰撞条件是任意路径点不在障碍物集合内，同时前后路径点的连线与各障碍物区域不相交；路径最优是指路径长度最小。因此路径的全局适应度函数应能有效区分碰撞可能并能体现出每条路径的优劣性，这里全局适应度函数由免碰撞适应度和路径适应度两部分构成。
[0065] 1.6.1免碰撞适应度函数
[0066] 任意路径点不在障碍物集合B内。对第k条鱼的某个路径点Ai的局部适应度函数可表示为
[0067]
[0068] 即路径点Ai若落在障碍物集合B区域内，则其局部适应度为0，反之为1。第k条鱼的路径点全局适应度函数为
[0069]
[0070] 前后路径点的连线与各障碍物区域不相交。第k条鱼的当前路径点Ai与下一路径点Ai+1的连线为AiAi+1(1≤i≤n)，若与障碍物集合B区域不相交则相应的局部适应度为1，相交则局部适应度为0，即：
[0071]
[0072] 考虑第k条鱼整个路径连线的全局适应度函数为
[0073]
[0074] 1.6.2路径长度适应度函数
[0075] 路径最优是指路径越短越优，路径长度适应度函数fit3可表示为：
[0076]
[0077] fit3取值在0-1之间，路径越优，fit3越小，说明机器人走过的路径越长。
[0078] 1.6.3平滑度适应度函数
[0079] 在路径优化过程中，不仅考虑路径长度还考虑路径线段的平滑度。设有向线段AiAi-1、AiAi+1间的夹角为αi(0≤αi≤π)，则定义对应的平滑度适应度函数为：
[0080]
[0081] fit4取值在0-1之间，当αi＝0，相当于下一路径段折返，fit4＝0，为最差情况；当αi＝π时，相当于下一路径段保持原方向前进，fit4＝1，为最优情况。第k条鱼整个路径的平滑度适应度函数为
[0082]
[0083] 1.6.4总全局适应度函数
[0084] 将以上约束条件有机结合在一起,得到总全局适应度函数Fit
[0085] Fit＝fit1*fit2*fit3*fit4 (9)
[0086] 总全局适应度越高，路径越优。
[0087] 2进行初始化
[0088] 2.1对地图进行初始化
[0089] 用栅格法对地图进行二维空间建模，如图2所示将移动机器人所在环境分割成规则且均匀的栅格，每个栅格只有占据或自由两种状态。占据状态的栅格表示障碍物，自由状态的栅格表示可行点。然后在可行点中决定好机器人的起点与终点。例如图2的起点坐标为(2,4)，终点坐标为(47,46)。
[0090] 2.2对机器人的行走路径进行初始化
[0091] 首先随机生成100个可行路径，这100个路径中的每一条路径的第一个点为起点，最后一个点为终点，并且要求每一条路径的每两个连续的点之间的连线不能覆盖障碍物。
[0092] 2.3对于参数进行初始化
[0093] 预定预设中心距离范围为visual，预设移动路径长度范围为step，表示路径密集程度的拥挤度因子为δ(0<δ<1)，总路径数量为N个，当次迭代中执行到的当前路径为D。初始失败次数设为tryNumber。
[0094] 2.3对于失败的认定
[0095] 对于失败的认定，可以采用以下两种方式：方式一：递增方式，此时初始失败次数设为0，每次失败一次，失败次数加1，当失败次数达到预设的失败次数阈值时，则视为前进失败。方式二：递减方式，此时初始失败次数设为非零的数值，每次失败一次，失败次数减1，当失败次数由初始失败次数减为0时，则视为前进失败。以上两种方式仅仅是描述上的不同而已，但对整个算法的实质上没有任何影响。权利要求采用方式一进行描述，本实施例中采用方式二进行描述。
[0096] 3改进的鱼群算法推演
[0097] 3.1聚群与追尾行为
[0098] 每条路径随机选择鱼群的聚群行为或者追尾行为，根据各自行为描述来改变除起点终点之外，其他路径可行点的位置。
[0099] 3.1.1改进的聚群行为描述
[0100] 1)路径D搜索除了D自身之外的路径，也就是搜索N-1个路径。找到这N-1个路径中中心距离距离路径D在0～visual范围的这些路径，这些路径个数为L。计算这L条路径的平均路径长度以及平均中心位置。平均路径长度取L个路径长度的平均值，平均中心位置取L个路径包含的所有路径点的平均值。
[0101] 2)如果(D的路径长度×δ×L)小于L个路径的路径长度的平均值，则会认为D可以向这L个路径学习，则执行3)。不然直接执行5)。
[0102] 3)然后D中遍历除了起点跟终点的其他路径点们。对这些路径点中的每个路径点来说，做两条矢量，一条矢量方向是点到L个路径的中心位置方向，一条矢量方向是点到D的终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向F。
[0103] 4)路径点再向F移动0至step的随机路径长度，如果路径点不能顺利移动，即超出边界，或者前进到障碍点上,或者移动之后D的路径长度大于移动前D的路径长度，则此路径点不执行前进操作，但是tryNuLber减1。当tryNuLber到0时，则视为此路径前进失败，进入觅食行为。若路径点能顺利移动则进入下一步，免疫选择。
[0104] 5)如果上面的2)中的条件不能满足，则不执行聚群行为，则当前路径不会变得更好，则进入下一步觅食行为。
[0105] 3.1.2改进的追尾行为描述
[0106] 1)D搜索除了D自身的路径，也就是搜索N-1个。找到这N-1个路径中中心距离距离路径D在0至visual范围的这些路径，这些路径个数为M。通过比较M个路径的路径长度，得到M个路径里面路径值最小的路径Y，路径Y即为M个路径中的最优路径。
[0107] 2)如果(D的路径长度×δ×M)小于Y的路径长度，则会认为D可以向Y学习，则执行3)。否则执行5)。
[0108] 3)然后D中遍历除了起点跟终点的其他路径点们。对这些路径点中的每个路径点来说，做两条矢量，一条矢量方向是点到X中心位置的方向，一条矢量方向是点到D的终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向F。
[0109] 4)路径点再向F移动0至step的随机路径长度，如果路径点不能顺利移动，即超出边界，或者前进到障碍点上,或者移动之后D的路径长度大于移动前D的路径长度，则此路径点不执行前进操作，但是tryNuLber减1。当tryNuLber到0时，则视为此路径前进失败，进入觅食行为。若路径点能顺利移动则进入下一步，免疫选择。
[0110] 5)如果上面的2)中的条件不能满足，则不执行追尾行为，则当前路径不会变得更好，则进入下一步觅食行为。
[0111] 3.2觅食行为
[0112] 当路径D执行完聚群或者追尾行为后，如果路径D的路径值不能变得更小，则需要执行觅食行为。
[0113] 3.2.1改进的觅食行为描述
[0114] 1)D遍历路径中除了起点跟终点的每一个路径点。对每个路径点来说，随机选取一个方向，然后做两条矢量，一条矢量方向是点到随机选取的方向，一条矢量方向是点到终点位置，得到的这两个矢量方向进行一个矢量运算，得到一个最终的前进方向F。
[0115] 2)路径点再向F移动0至step的随机路径长度。如果路径点不能顺利移动(超出边界或者前进到障碍点上)，或者移动之后D的路径长度大于移动前D的路径长度，则此路径点不执行前进操作，但是tryNuLber减1。当tryNuLber到0时，则视为此路径移动失败，放弃前进，进入下一步免疫选择。若能顺利前进则进入下一步，免疫选择。
[0116] 3.3免疫选择
[0117] 1)预设原来有100个路径，在经过了聚群行为、追尾行为以及觅食行为的过程后，每个路径都会得到一个新路径。这样我们会得到新的100个路径，也就是总数是200个路径。找到这200里面中的路径最小值以及其路径点，并记录下来。
[0118] 2)然后对这200的路径的适应度函数值进行计算，并记录。
[0119] 3)从旧的100个路径中选出适应度函数值大的p个路径，从新的100个路径中选出w个路径，满足(100
[0120] 4)按选择概率的大小，从大到小排序p+w个路径。进入下一步，修改数据。
[0121] 3.4修改数据
[0122] 1)当执行完免疫选择之后，免疫选择得到的200路径中的最小值跟之前的最小值比较，如果新得到的比之前的路径值要小，则取代。如果比之前的大，则不取代(程序执行的第一次迭代必然取代)。
[0123] 2)免疫选择中已排序好的p+w个路径，选择前100个，取代原来的100个路径成为下一次遍历的初始路径。然后进入下一步，判断程序是否结束。
[0124] 3.5判断程序是否结束
[0125] 当修改完数据后，我们得到了新的100个路径以及新的最小值路径。此时需要判断程序结束还是进入下一次的遍历。程序结束需要满足以下其中一个条件，否则进入下一次循环。最后得出的结果就是最后一次修改数据中的最小值数值，也就是程序的最小值。
[0126] 1)判断程序是否已经执行了100次，如果达到100次，则允许程序结束。
[0127] 2)判断程序的最小值路径是否连续50次不变，如果连续50次不变，则允许程序结束。
[0128] 3.6结果
[0129] 程序结束时得到的路径最小值以及其相应的路径点，则为这次程序的最优路径。如结构图中把最优路径的路径点依次连起来，则得到了机器人的行走路径。行走路径与最小的路径值，这就是得到的最后结果。
[0130] 为了分析IDAFSA全局搜索路径的性能，我们设计了两个网格地图环境，跟普通的FGA以及AFSA在相同情况下进行算法性能上的比较。在本文中，三个算法都是用C++编程语言，在Visual Studio 2012中模拟仿真，计算机配置为Intel(R)Core(TL)i7-4700LP CPU@2.40GHz，内存(RAL)8GB，64位Window7操作系统。采用栅格法对机器人路径规划进行建模，如图2，黑色方块为地图中的障碍物，构成障碍物集合B＝{B1，B2，...，BL}，其它区域表示了机器人能安全通过的区域。机器人行走的路径点Ai，将路径点相连形成一条完整的机器人行走路径，即人工鱼群中的单条鱼，不同的路径点相连就生成不同的单条鱼，这样就形成了人工鱼群。αi为有向线段的夹角。这次实验选择了一个网格地图环境用以检测算法的可行性。机器人行走路径的起始结点为(2,4)，目标结点为(47,46)。IDAFSA的拥挤因子设为δ＝
0.618，允许失败次数Try_nuLber＝10。实验中算法在图2地形下运行100次，每次100代，最终的结果都是取这100次的平均值。终止条件：I.如果算法在一次中连续50代最优解不变，则停止运行。II.算法运行了100代，则停止运行。仿真实验结果表明，IDAFSA算法和普通的AFSA相比，改进的算法具有较好的全局寻优能力，实时性能好，算法稳定，并且能在可接受的时间内产生很好的路径。
[0131] 在人工鱼群机器人路径规划算法中加入方向算子，可有效地提高觅食、聚群和追尾三种鱼群行为的效率。在人工鱼群算法中增加免疫算法操作可有效地提高算法的全局搜索能力并减少局部极值出现的概率。仿真实验结果很好地证明了所得改进人工鱼群机器人路径规划算法IDAFSA在机器人路径规划中具有更好的结果稳定性、更短的计算时间和更接近最优路径的可行解，意味着IDAFSA在机器人路径规划中有很好的应用前景。
