基于Q-IGA动态拟合贝塞尔曲线的路径规划

徐岩，崔媛媛

（天津大学电气自动化与信息工程学院，天津 300072）

随着计算机技术、控制理论、传感器技术和人工智能等众多交叉学科的发展成熟[1]，自导航机器人技术得到快速发展，各式各样的机器人被应用于科学研究、工业生产、家居生活等众多领域.自导航机器人技术不仅是移动机器人完成高难度任务的基石，也是机器人智能化成熟的重要标志[2].控制自导航机器人运动的算法[3]主要由定位算法、导航控制算法以及路径规划算法3 部分组成.在路径搜索过程中安全避障是机器人顺利完成任务的前提，因此路径规划方法的优劣对机器人作业具有重要影响，并且在无人机[4]、无人车等领域也具有广泛的应用.路径规划的目的是寻找一条从起始点到目标点的无碰撞最优路径.目前，已有很多研究者致力于路径规划算法研究，从早期的栅格法、可视图法、人工势场法[5]等传统算法到遗传算法、粒子群算法等智能算法以及衍生出的改进算法等[6]，研究不断深入并取得丰硕成果.然而传统路径规划算法依旧存在很大的局限性，因此智能算法凭借其独特的优良性能成为研究重点.由于遗传算法的进化算子具有各态历经性，使其能够有效处理具有概率意义的全局搜索，并且搜索过程不需要限制问题的内在性质，因而在解决路径规划问题方面脱颖而出.

Tuncer 和Yildirim 提出一种改进的遗传算法，通过建立含有障碍物信息的代价矩阵，将地图特征信息引入初始种群中，并采用动态参数设置策略，在变异算子中加入预测过程，有效提高了算法的搜索效率和计算精度[7]；Tsai 等人提出将两种精英遗传算法并行执行，并在其中加入迁移算子，使种群保持较好的多样性，抑制其过早收敛[8].为了解决恶劣环境下的全局路径规划问题，Hu 等人提出了一种基于知识的遗传算法，将不同环境区域的先验知识整合到特定的遗传算子中，并结合局部搜索技术，使其能在复杂的静态和动态环境中找到最优或接近最优的机器人路径[9]；Alajlan 等人提出将遗传算法与路径最小化原则结合，在执行选择算子过程中生成最小化原则，在变异阶段使用此规则，有效缩短了路径长度[10]；Lau 等人提出一种利用模糊逻辑引导遗传算法进行随机搜索的方法[11]，模糊逻辑的作用是在连续迭代10 次后动态调整交叉概率和变异概率.该方法使遗传算法的计算效率有效提高，在不同环境下的适应能力有效增强；邓乾旺等人提出多目标优化操作的遗传算法，添加记忆算子为插入算子和变异算子选取合适的方向和步长[12].

虽然以上各种优化方法克服了遗传算法固有的一些缺点，例如：提升了对新空间的探索能力以防止过早收敛到局部最优解；降低复杂度以防止在个体数量较多时计算效率过低.然而，对于应用在实际生产生活环境中的机器人路径导航系统，很多时候所需最优结果不一定是上述方法中所描述的最短路径.相对地，更希望得到一条“合理”的路径.在这里，合理的路径被定义为这样一条路径，即：路径既没有穿过障碍物，也不会紧紧贴着障碍物行进，并且尽可能少的做出大角度的转弯，在满足上述条件的前提下，使得路程尽可能最短.

因此，针对上述问题与需求，提出一种利用QIGA 算法协同搜索贝塞尔曲线控制点的方法.该方法避免了传统算法中先生成最优路径再利用贝塞尔曲线拟合出平滑路径造成的步骤繁琐，使路径的平滑与搜索过程并行执行.并且在遗传算法的选择算子中利用Q 值检验法的思想判断种群内解方案的多样性，去掉相似度较高的解，增强其勘探能力，使算法搜索效率提高.同时该算法通过修正适应度函数公式，加入除路径长度外的其他代价因素，使生成的路径满足前文所述对路径合理性的要求.仿真结果表明：Q-IGA 算法相较于改进人工势场法和混合遗传算法，可以得到与障碍物保持一定安全距离的平滑路径且计算效率更高.在机器人应用中，该方法为避免能量耗竭提供了一种有效途径，特别是在动态资源有限的机器人应用中.

1 数学模型

1.1 遗传算法

利用遗传算法求解路径规划问题包含以下几个基本步骤：首先随机产生一个初始种群，然后计算种群内每一个个体的适应度值并利用选择算子筛选出适应度值较高的个体参与迭代，在迭代中按照一定的概率进行交叉和变异操作积累优良基因，然后再次计算种群内新个体的适应度值，反复迭代直至输出最佳个体及最优解.

1.2 贝塞尔曲线

贝塞尔曲线具有良好的几何特性，在计算机图形学中得到广泛的应用.贝塞尔曲线的最大优点之一是，如果控制点构成的多边形是凸多边形，即其特征多边形是凸的，则贝塞尔曲线也是凸的.此特点使得贝塞尔曲线具有良好的拟合特性.

给定R3：B：[0，1]→R3，它代表n+1 个控制点，i=0，1，…，n.n 次贝塞尔曲线可由n 次Bernestein 基多项式组成的参数曲线表示，定义为[13]：

式中：t 表示归一化时间变量；Bi，n（t）是Bernestein 基多项式，代表贝塞尔曲线中的基函数，其表达式定义如下：

贝塞尔曲线的导数由控制点决定，其一阶导数可以表示为：

在二维空间中，贝塞尔曲线相对于t 的曲率可以表示为：

2 基于Q-IGA 的动态搜索策略

2.1 环境建模

机器人行进的环境为一个二维平面空间，平面中的障碍物设置为静态、随机和已知的任意不规则多边形，其顶点由环形列表表示.与栅格法相比，该方法易于解决复杂的环境信息问题，且占用的资源较少.

2.2 机器人运动学模型

考虑到机器人自身的位姿以及运动状态，首先建立运动学模型对它进行描述.如图1 所示：圆矩形表示机器人，在t 时刻其位姿可用xt，yt，θt3 个参量表示.其中（xt，yt）表示机器人在二维平面内的位置；θ代表机器人运动方向与x 轴的夹角，初始为0；φ 代表机器人的行驶方向和当前位置与目标点连线的夹角；机器人运动的角速度为ω；质心的线速度为v，由于线速度垂直于两轮之间的连线，因此它在x 轴和y轴的分量分别为：

由式（5）（6）消去v 可得：

设某一时刻机器人运动状态为q=[Vx，Vy，θ]，其中，Vx，Vy分别为考虑了角速度影响的机器人等效线速度在x 轴和y 轴的分量.类比式（7）可得，在笛卡尔坐标系下，移动机器人的约束条件方程为：

则由单位时间内θ=ω，结合式（5）（6）可得机器人位于某点的运动模型为：

机器人的线速度和角速度分别为：

式中：vL和vR分别为机器人左右两轮的线速度；R 为运动半径.将机器人的线速度和角速度以及运动半径代入运动模型中可得机器人位于某一点的运动方程为：

将运动学模型引入路径规划过程后，即为路径规划算法增加一个约束条件：通过不断减小φ，使机器人朝向目标点方向持续运动.

图1 机器人运动模型示意图Fig.1 Schematic diagram of robot motion model

2.3 动态并行拟合策略

在以往的研究中，利用贝塞尔曲线平滑路径的过程是一个独立的过程，即在产生最优路径后再用贝塞尔曲线进行静态拟合，这样会增加计算步骤使算法繁琐.因此利用Q-IGA 算法在搜索路径点的同时协同搜索最合适的控制点P 作为贝塞尔曲线的控制点，利用选出的控制点可生成较短的最佳路径.

本文建立的环境模型为多面障碍物平面模型，所以首先将环境地图以图片格式输入并对其进行二值化处理，使其相当于具有多个像素点的网格地图.每一个网格相当于一个基因，其值可以为0 或1，当该网格是贝塞尔曲线的控制点时，其值取为1，当该网格不是贝塞尔曲线的控制点时，其值取为0.若该网格被障碍物覆盖，则其值设为-1，不可作为控制点.

因此遗传算法初始种群内的每个染色体编码方式为二进制编码，基因值为0 或1 即代表路径点是不是待选控制点.若路径穿过障碍物，即穿过值为-1的网格点，则此路径点会在后续的遗传操作中通过避障操作移除.

在路径规划过程中保证路径的连续性十分重要，在此定义低阶连续性准则如下[14]：

1）连接起始点的一条线段具有0 阶连续性.

2）在两条线段的连接处，用等效切线保证一阶连续性.

3）三阶以上贝塞尔曲线由曲率连续性保证其连续性.

考虑到计算复杂度，在满足连续性的条件下，贝塞尔曲线的阶次越低越好.本文采用三阶贝塞尔曲线生成平滑路径，如图2（a）所示.三阶贝塞尔曲线有4 个控制点N0，N1，N2，N3.α 和γ 分别表示向量N0N1，N1N2，N2N3之间的夹角.三阶贝塞尔曲线定义如下：

图2 三阶贝塞尔曲线Fig.2 Third order Bezier curve

图2（b）为根据搜索得到的控制点拟合出的一组三阶贝塞尔曲线，（控制点分别为N0，N1，N2，N3和M0，M1，M2，M3）.三阶贝塞尔曲线成对出现能够保证曲率的连续性（从0 单调递增到kmax，再单调递减到0）.

2.4 优化选择算子增加种群适应性

在Q-IGA 算法中，依据遗传算法基本步骤，首先随机产生一个种群X={x0，x2，…，xn}，它包含S 条染色体，每条染色体为一个路径规划方案，染色体的结构可以表示为xs={u1，u2，…，up}.优化算法的问题之一是求解过程具有很强的随机性，这导致迭代结果易损失多样性.为提高遗传算法生成解的多样性，对选择算子进行优化.通过计算每次迭代种群内不同路径方案之间的差异度，为选择算子增加一个判断准则，保证种群内可行解的多样性.算法设置的判断准则为：在每次迭代中，只有种群的相似性指标低于阈值η 时，这些方案才可保留进入下一次迭代.η的初始值为（0，1）中的一个随机值.在迭代过程中，η的值从初始值线性下降到0.在算法模型中，使用衰减因子a=0.99 代表衰减率，即η 的衰减公式为：ηt+1=aηt，当t 趋近于无穷时，η 趋近于0.因此，优化的选择过程拒绝相同和过于相近的解决方案同时存在，有助于遗传算法避免局部最优问题，增强其勘探能力.在每一次迭代中，η 都会减小，这意味着在搜索后期趋近于最优解时迭代得到的路径方案越来越相近，即在搜索开始和结束之间得到了平衡.

算法借鉴Q 值检验法思想判断种群多样性.根据路径模型，定义两个解决方案x1，x2之间的Q 值为：

式中：N11代表两染色体中对应位置基因值都为1 的数量；N00代表两染色体中对应位置基因值都为0 的数量；N01代表x1染色体基因值为0 的位置对应x2基因值为1 的数量；N10代表x1染色体基因值为1 的位置对应x2基因值为0 的数量.

若两个染色体，x1=[1 1 0 0 1 1 0 0 1 0]，x2=[1 0 1 0 1 0 1 0 1 1]，则二者之间的Q 值为：

综上，种群相似性q 为计算得到的所有个体两两Q 值的平均值，即：

2.5 优化适应度函数

遗传算法的另一重要步骤是计算个体适应度值.从常识可以得知，在相似的运动环境中，机器人的运动能耗与运动距离成正相关，即运动距离越长，耗能越多.传统的遗传算法通过适应度函数判断路径长短，可以保证机器人在完成运动目标的前提下尽可能缩短机器人的运动距离.然而如前文所述，这样得到的路径有可能与障碍物距离过近、或出现耗能较大的大角度转弯.因此在本文提出的算法中，适应度函数将转弯角度、机器人体积也作为评价因素，优化目标为使重新定义的适应度函数值最大.

2.5.1 距离因素

在遗传算法完成控制点搜索后，路径长度D 即可表示为：

式中n 为控制点的数量，路径长度可以由贝塞尔曲线的积分来计算，计算公式为：

2.5.2 机器人体积

在理想环境中，移动机器人被视为一个粒子，并不考虑其体积对行进过程的影响.然而，在实际应用中，机器人在运动过程中不仅要避开障碍物，而且应与障碍物保持一定的安全距离.在经典的路径规划算法中，为保证最优路径为最短路径，常常会导致机器人紧靠障碍物或墙壁等限制情况出现.

因此算法对经典适应度函数计算方法做出优化.对于无法穿过的障碍物，用+∞来描述其代价.这样，障碍物附近某个点对于机器人运动的代价定义为：

式中：k1为归一化系数；d 是机器人中心距离障碍物的距离；d0是机器人的外形尺寸，如图3 所示.当机器人与障碍物之间的距离小于机器人的尺寸时，其代价为+∞，此时适应度函数值为0，即认为机器人在这种条件下会与障碍物发生碰撞；当其与障碍物的距离大于机器人的尺寸时，其代价值呈高斯函数衰减，即希望在一定范围内机器人尽可能远离障碍物.

图3 机器人体积与障碍物距离示意图Fig.3 diagram of distance between robot volume and obstacle

2.5.3 机器人转弯角度

路径转角过大或过于频繁不仅影响机器人的工作效率，在运动距离同等的条件下，还会消耗更多的能量.虽然随着锂电池等可充电电池技术近年来的快速发展和成熟，机器人的续航时间越来越长，但是充电时间长等缺点仍然限制着机器人的循环使用效率.因此，算法将转弯角度因素引入适应度函数，将每个拐点因转弯所带来的代价定义为：

式中：α 为转弯角度，包括路径方向改变引起的角度变化以及引入机器人运动学模型后其自身旋转时的角速度带来的转弯角度；k2为归一化系数.

经过转弯优化后的效果如图4 所示，虚线路径代表的是转弯优化前的路径，实线路径代表的是转弯优化后的路径.可以看出，优化后的路径较优化前的路径转弯次数更少，且转弯角度更加平滑，更适合于实际的工业应用.

图4 优化转弯角度效果图Fig.4 Optimized turning angle effect diagram

2.5.4 最终的适应度函数公式

综合路径长度因素、机器人自身体积和机器人转弯角度带来的代价，适应度函数最终被修正为：

式中：D 为路径长度；R1（n）为机器人体积代价；R2（n）为机器人转弯角度代价.算法的迭代目标为使新定义的适应度函数值达到最大.

综上，基于Q-IGA 动态拟合贝塞尔曲线的路径规划算法流程如图5 所示.

图5 路径规划流程图Fig.5 Flow chart of path planning

3 仿真实验与分析

3.1 参数设置

算法的仿真实验环境为MATLAB2016，系统环境为Windows 7，运行内存为2GB，处理器为Intel（R）Celeron CPU 1000 @1.80GHz.在进行仿真实验前，首先需要设置合理的参数使算法得到最优解.

3.1.1 遗传算法参数设置

如图6（a）所示，随着迭代次数和种群数量的增加，每次迭代所得最优解的性能得到改善，路径长度减小，计算时间增加，经过150 次迭代后路径长度趋于稳定；而种群规模对算法性能的影响如图6（d）所示，在规模超过110 以后，路径长度趋于稳定.综合考虑路径长度和计算时间两个因素，算法将迭代次数设为160 次，种群规模设为110，在保证计算时间不会过长的前提下，还能获得较短的路径.在此仅展示了第一幅不规则障碍物地图参数选择实验，其余两个地图环境下的参数选择方法与此类似.

图6 参数性能图Fig.6 Parameter performance diagram

3.1.2 网格粒度大小设置

由经验可知，地图不同分辨率对路径规划算法性能会造成一定影响，因此针对Q-IGA 算法在第一幅地图中设置不同的网格粒度探讨其对算法性能的影响.由表1 可知，随着网格粒度的增加，算法的执行时间越来越短，但是路径规划成功率受到的影响有所降低.因此为在较短的时间内保证较高的路径规划成功率，实验中将网格粒度设置为0.5，其余两幅地图情况同理.

表1 网格粒度参数表Tab.1 Grid granularity parameter table

3.2 未考虑运动模型的路径结果分析

在仿真实验中从简单到复杂设置了3 种地图环境，如图7（a）（b）（c）所示，分别为不规则障碍物地图（大小为20×20），窄带障碍物地图（大小为30×30）和迷宫图（大小为45×45）.每种地图环境下都验证了3种算法的性能，其中路径1 是改进人工势场法[15]产生的路径，路径2 是混合遗传算法[16]（IHGA）产生的路径，路径3 是Q-IGA 算法产生的路径.

3.2.1 路径结果分析

结合图7 中不同环境下的路径结果特点和表2中不同路径的长度可知，虽然路径3 的长度不是最短的（较人工势场法略长，较混合遗传算法较短），但路径3 的性能优于路径1 和路径2，其转弯更少且平滑度更高，与障碍物也保持一定的安全距离，可避免机器人与障碍物发生摩擦碰撞的情况，更适合移动机器人的实际应用.

3.2.2 时间性能分析

本文提出的算法旨在得到合理路径的同时提高算法的搜索效率，因此对混合遗传算法、Q-IGA 算法和改进人工势场法3 种算法的时间性能进行了考察.由表3 数据可知，3 种算法在第2 幅地图中的执行时间较长，结合表2 中的路径长度可知，这是因为第2幅地图中迂回转弯较多，路径搜索方向变化较大，路径较长，因此路径规划算法执行时间较长.3 种算法中，Q-IGA 算法在时间上比改进人工势场法略长，这是由于遗传算法有编解码过程，因此时间略长是合理的.但Q-IGA 算法比混合遗传算法执行时间更短，这说明其在获得最优路径的同时有效提高了算法的时间效率.

图7 不同环境下的仿真结果图Fig.7 Simulation results diagram in different environments

表2 路径长度表Tab.2 Path length table

表3 时间性能表Tab.3 Time performance table

3.3 考虑运动模型的路径结果分析

为验证运动学模型对路径规划结果的影响，在ROS 环境中，结合机器人运动学模型，对路径规划结果进行了进一步的仿真，并利用RVIZ 工具将路径可视化，其结果如图8 所示.实验中，利用ROS系统的导航功能包设置机器人初始状态参数，令其线速度为2 m/s，角速度为0.05 rad/s，当前运动方向与目标点连线方向的夹角φ 为75°，运动半径R为2.图中路径1 为未加入运动学模型、改进人工势场法产生的路径，其为折线；路径2 和路径3 分别为加入运动学模型后、混合遗传算法和Q-IGA 算法产生的路径，其为曲线.由图可知，3 种算法均产生了无碰撞路径，与3.2.1 节未加入运动学模型得到的路径2相比，在加入运动学模型后，原本为折线的路径成为曲线，这说明在实际应用中，由于机器人自身位姿和运动速度的影响，它并不会按照既定的折线型路径行走，这就使实际路径和规划路径产生了偏差.而与3.2.1 节未加入运动学模型得到的路径3 相比，Q-IGA 算法在未使用运动学模型时就是平滑的，与结合运动学模型的路径效果一致，并且在平滑的基础上可与障碍物保持一定的安全距离，因此更符合机器人的实际应用场景，路径更加合理.

图8 结合运动学模型的路径规划结果Fig.8 Path planning based on the kinematics model

4 结论

本文提出一种基于Q-IGA 算法动态拟合贝塞尔曲线的路径规划算法.通过优化遗传算法的选择算子，使种群多样性增加，避免算法陷入局部最优，加快收敛速度.通过修正适应度函数公式，使路径不会紧贴障碍物行进.通过并行搜索路径点和控制点，使路径搜索与平滑过程有机结合，增强了路径的自适应性且提高了算法效率.与改进人工势场法[15]和混合遗传算法[16]相比，Q-IGA 算法不仅保证路径长度较短，而且使机器人旋转更加灵活，可与障碍物保持更为安全的距离，满足对路径合理性的要求，因此可以降低能耗，更适合于实际应用.