一种带有运动学的复杂形状目标遗传路径规划方法

摘要

本发明的目的在于提供一种带有运动学的复杂形状目标遗传路径规划方法，包括以下步骤：第一步，采用三维位姿法对路径进行编码；第二步，产生初始种群；第三步，对初始种群进行遗传操作；第四步，采用三段法对路径进行解码并设计适应度函数；第五步，将本次迭代评估后当前代种群路径进行遗传操作，产生下一代种群，将本次迭代第四步中修补后的基因加入到下一代种群中，如此反复进行下去直到所得路径的可行适应度函数的函数值不再发生变化，则迭代结束，收获最优路径解。本发明将机器人的不规则形状及运动学约束考虑均在内，引入基因修补策略和适应度函数惩罚策略，最优解收敛速度得到较大提升。

说明书

技术领域

本发明涉及的是一种运动路径避碰方法。

背景技术

带有运动学约束的不规则形状目标路径规划具有重要的意义，能够在复杂 环境下进行目标的路径规划，是保证机器人有效规避障碍，顺利到达目标点的 重要条件。现有的路径规划方法仅能满足障碍简单环境下的目标路径规划，但 当面对地图大、障碍多、障碍形状复杂的情况时，路径规划问题将变得复杂， 在以往其它的路径规划方法中，均将规划目标简化为栅格点或圆形，并且计算 可行路径时不考虑目标的转弯半径约束(即假设目标可以原地转弯)，直接以折 线路径作为有效路径，而在有些情况下，会出现规划目标形状复杂且存在着转 弯半径约束、障碍环境复杂等情况，这将导致传统的路径规划方法失效。

发明内容

本发明的目的在于提供能够在障碍复杂的环境下进行避碰规划的一种带有 运动学的复杂形状目标遗传路径规划方法。

本发明的目的是这样实现的：

本发明一种带有运动学的复杂形状目标遗传路径规划方法，其特征是：

(1)采用三维位姿法对路径进行编码：

路径编码采用二进制编码方式，给定目标起点位姿(x_st,y_st,φ_st)及终点位姿 (x_end,y_end,φ_end)，将地图分为N×M个栅格，每个栅格位置对应K个角度方向，这 样每个路径中间点就是由(x,y,φ)构成的三维矢量，且在N×M×K个离散空间点 内随机分布，设定路径搜索空间范围为n×m的二维矩形区域，则地图中搜索精 度为(n/N,m/M)；将每个栅格对应的中间点设定为2π的姿态范围，取姿态量 化数量为K，每个姿态角度间隔为2π/K，则整个编码空间变成三维离散位姿空 间；

(2)产生初始种群：

基于步骤(1)的编码方式，采用随机法产生初始种群：动态生成一个随机 数，根据随机数生成一个基因，将其加入到初始种群中，再随机生成下一个基 因，如此循环进行，直至生成的基因数量达到种群规模G为止，完成初始种群 的产生；

(3)对初始种群进行遗传操作：

将步骤(2)所生成的初始种群进行遗传操作，产生新一代种群；

(4)采用三段法对路径进行解码并设计适应度函数：

采用三段法解码方式：针对给定的起点位姿矢量(x_st,y_st,φ_st)和终点位姿矢 量(x_end,y_end,φ_end)，通过起点圆弧—直线路径—终点圆弧连接起点、终点位姿两 矢量，起点圆弧与起点矢量相切、终点圆弧与终点矢量相切，直线路径同时相 切于起点圆弧和终点圆弧，起点圆弧和终点圆弧半径相同均为r，并大于目标 的最小允许用转弯半径r₀，根据几何位置关系求出路径中间节点、中间姿态及 路径长度；

设计适应度函数对解码后的路径进行评价：

若解码后的路径无碰撞则认为解码后的路径是可行的，将解码后的路径总 长度及与障碍最小距离作为路径评价指标，可行路径适应度函数W(p)为：

$W\left(p\right)=w_f·f\left(p\right)+w_{\mathrm{obs}}·\frac{1}{d_{\mathrm{obs}}\left(p\right)+d_0}$

式中，p代表起点位姿至终点位姿的可行路径，f(p)为解码后的路径总长度代 价值，d_obs(p)表示起、终点轮廓及广义包围盒与障碍线段集的最小距离，用凸 多边轮廓线段集将路径包围起来，称该线段集为广义包围盒，d₀为常值，w_f及 w_obs分别代表解码后的路径长度及安全性权值；

若解码后的路径碰撞，则认为不可行，评估指标为路径总长度与固定惩罚 项的加和，则不可行路径适应度函数W`(p)为：

W`(p)＝w_f·f(p)+w_obs·R

式中，R为一固定惩罚项，R值大于的最大可能值(1/d₀)； 将不可行路径适应度函数的适应度值排在前五位的不可行路径记录到待修补列 表中，在本次遗传操作结束后，找出导致其不可行的基因位，根据可行适应度 函数修正此类基因位，使其变成可行路径；

(5)获取最优路径

将本次迭代评估后当前代种群路径进行遗传操作，产生下一代种群，将本 次迭代步骤(4)中修补后的基因加入到下一代种群中，如此反复进行下去直到 所得路径的可行适应度函数的函数值不再发生变化，则迭代结束，收获最优路 径解。

本发明还可以包括：

1、步骤(1)中中间点个数为O(0<O≤3)。

2、所述的种群规模G的取值范围为0<G≤200。

本发明的优势在于：(1)以往关于路径规划的发明专利，均将机器人简化 为质点或圆形，且未考虑目标的运动学约束(即假设可原地转弯)，而本发明将 机器人的不规则形状及运动学约束考虑均在内，设计了避碰路径规划方法；(2) 本项目在传统遗传算法基础上，引入基因修补策略和适应度函数惩罚策略，与 罗熊等人提出的算法相比较，最优解收敛速度得到较大提升。

附图说明

图1为本发明的流程图；

图2为机器人环境及编码示意图；

图3为基因编码序列示意图；

图4a为三段法路径解码示意图a，图4b为三段法路径解码示意图b；

图5a为最优解收敛曲线对比a，图5b为最优解收敛曲线对比b。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1～5，各步骤具体实现如下。

(1)采用三维位姿法对路径进行编码

路径编码采用二进制编码方式，给定目标起点位姿(x_st,y_st,φ_st)及终点位姿 (x_end,y_end,φ_end)，将地图分为N×M个栅格，每个栅格位置又对应K个角度方向， 这样每个路径中间点就是由(x,y,φ)构成的三维矢量，且在N×M×K个离散空间 点内随机分布。设定路径搜索空间范围为n×m的二维矩形区域，则地图中搜索 精度为(n/N,m/M)；将每个栅格对应的中间点又设定为2π的姿态范围，取姿 态量化数量为K，每个姿态角度间隔为2π/K，如图2所示。这样，遗传算法的 整个编码空间变成三维离散位姿空间，为了避免染色体编码过长，需尽量减小中 间点的个数，但又考虑到算法的避碰能力，本项目算法设定中间点个数为 O(0<O≤3)。例如设中间点位姿为P₁(x₁,y₁,φ₁)、P₂(x₂,y₂,φ₂)，则染色体编码为 六维矢量：(x₁,y₁,φ₁,x₂,y₂,φ₂)。各变量对应二进制位数为log₂N、log₂M和 log₂K，基因编码序列如图3所示。

(2)产生初始种群

基于步骤(1)的编码方式，采用随机法产生初始种群：由系统动态生成一 个随机数，根据随机数大小生成一个基因，由步骤(1)可得，该基因必是可行 路径。将其加入到初始种群中，再随机生成下一个基因，如此循环进行，直至 生成的基因数量达到种群规模G(0<G≤200)为止。至此，完成初始种群的产 生。

(3)对初始种群进行遗传操作

将步骤(2)所生成的初始种群进行遗传操作，则产生的种群为新一代种群。 本发明将选择、交叉、变异算子引入算法中辅助寻优。而经实验得出，进行单 点交叉后的路径不可行概率高达83％，故本项目引入了基因修补策略：将适应 度值排在前五位的不可行路径记录到待修补列表中，在本次遗传操作结束后， 找出导致其不可行的基因位，根据适应度评估方法改变这些基因位的二进制数， 使其变成可行路径，加入到新一代种群中参与下次遗传操作；惩罚策略是指在 适应度函数里加上惩罚项，大幅度降低非法个体的适应度，让非法个体在选择 过程中被淘汰。并加快算法的收敛速度至W代，易得到最优路径个体。

(4)采用三段法对路径进行解码并设计适应度函数

在进行基因适应度评估前应将基因进行解码：针对给定的起点位姿矢量 (x_st,y_st,φ_st)和终点位姿矢量(x_end,y_end,φ_end)，通过起点圆弧—直线路径—终点圆 弧三个曲(直)线段连接起点终点位姿两矢量(如图4所示)，并保证起点圆弧 与起点矢量相切、终点圆弧与终点矢量相切，直线路径同时相切于起点圆弧和 终点圆弧，考虑目标运动学约束将起点圆弧和终点圆弧半径相同(设为r)，并 大于目标的最小允许用转弯半径r₀，根据几何位置关系可求出路径中间节点、 中间姿态及路径长度。

通过路径解码可得到每个基因所对应的路径，适应度函数设计将完成对所 得路径好坏的评估：要得到无碰路径，需进行路径的碰撞检测算法设计。在不 损失大量有效空间的前提下，将不规则形状目标以凸多边轮廓线段集包围起来， 将解码后的路径以弧形和矩形包围盒包围起来，组成广义包围盒，后将碰撞检 测算法分为两步计算：1)进行起点位姿及终点位姿轮廓多边线段集与周围障碍 的多边线段集碰撞检测及最小距离计算；2)进行包围盒与周围障碍的碰撞检测 及最小距离计算。

若路径无碰撞则认为路径是可行的，将上述路径总长度及与障碍最小距离 作为路径评价指标，设计的可行路径适应度函数如下所示。

$W\left(p\right)=w_f·f\left(p\right)+w_{\mathrm{obs}}·\frac{1}{d_{\mathrm{obs}}\left(p\right)+d_0}---\left(1\right)$

式中，p代表起点位姿至终点位姿的可行路径，f(p)为路径总长度代价值， d_obs(p)表示起、终点轮廓及轨迹包围盒与障碍线段集的最小距离，d₀为一常值， 用来避免当d_obs(p)值过小时导致W(p)趋于无穷大，w_f及w_obs分别代表路径长 度及安全性权值，可用来调整路径代价和避碰余量间的重要性分配。

若路径碰撞，则认为其不可行，评估指标为路径总长度，此处引入人为设 定固定惩罚项，公式如下。

W(p)＝w_f·f(p)+w_obs·R          (2)

式中，右边第1项与(1)式含义相同，第2项R为一固定惩罚项，R值应大于(1) 式中右边第2项的最大可能值(1/d₀)。之所以在不可行路径中引入路径代价项， 是为了保证碰撞路径中也能使得长度较短的路径具有更优的适应度值，为步骤 (3)基因修补算子的引入提供先决条件。

(5)获取最优路径

循环迭代步骤(3)、(4)反复产生新一代种群，并对每一代种群按步骤(4) 进行适应度计算，完成Q次迭代后路径适应度值不再变化，则收获了最优路径 解。此处判断算法是否收敛的条件为：适应度值不再随迭代次数而减小。

根据以上算法步骤，在320m×72m的地图当中，给定目标的起点(50,5,120°) 和终点位姿(260,68,90°)，选取初始种群规模为30，交叉概率为0.5，变异概率 为0.05，可行路径适应度函数中取w_f为0.75、w_obs为0.25；不可行路径适应度 函数中取w_f为0.4、w_obs为0.6。通过实验对多次实验分析，本文取O＝2G＝30 W＝50Q＝200，可得如图5(a)所示的实验结果。

实验结果分析：如图5(a)所示，得到的最优解路径总长度为123.5m。算法 开始时的收敛速度较快，至50代已经得到较好的路径解，50～200代的适应度 值没有变化，表明算法的收敛效果已经趋于稳定。图(b)为应用传统遗传算法进 行的路径规划仿真收敛曲线图，从图中可以看出算法迭代至132代时得到最优 解路径，结果表明：本发明公开的遗传避碰规划算法具有较快的收敛速度，提 高了路径规划效率；同时，针对不规则形状目标及带有运动学约束的一类路径 规划问题，给出了解决方案。