CN109737961A - 一种具有概率完备性的机器人最优化点到区路径规划方法 - Google Patents

Abstract

本发明提出一种具有概率完备性的机器人最优化点到区路径规划方法，在明确机器人路径规划的起点和目标区域，以及不可通行的障碍物区域之后，栅格化表达距离场形式，通过将路径表达为一个协变代价函数在距离场内的寻找最优解的过程，路径就是最优函数的自变量，为了保证在起点和目标区域约束有效，推导出原路径规划问题的对偶形式，对该代价函数进行寻优计算，所得最优值的解就是最终路径。当此最优化求解过程陷入局部极小时，就切换到基于结合模拟退火和协变代价函数的路径规划方法，来保证路径规划的概率完备性，即提高路径规划成功率。本发明解决了传统路径规划方法更多侧重于到点的规划，不能有效解决点到目标区域规划的问题，同时藉由引入模拟退火方法与协变代价函数相结合的具有概率完备性的方法，可以确保路径规划的成功率。

Description

一种具有概率完备性的机器人最优化点到区路径规划方法

技术领域

本发明属于移动机器人路径规划领域，涉及一种具有概率完备性的机器人最优化点到 区路径规划方法。

背景技术

移动机器人的路径规划是指在含有障碍物的工作空间中，按照一定的评价指标，搜索 一条可以完成指定任务的移动路径。移动机器人工作环境的描述方式主要包括栅格地图、 拓扑地图、几何地图与以上几种地图的混合地图。栅格地图一般指以大小相等或不等的方 形栅格将外部环境进行划分，根据栅格区域是否被障碍物占据将栅格进行二值划分。这种 建模方式便于计算机存储与批量处理，因为是目前最为被广泛应用的地图建立方式。

从路径规划目的性来考量，移动机器人的路径规划主要可以分为点到点和点到区路 径规划方法等。

点到点路径规划方法具有较长时间的研究历史，按照方法自身主要特性可以分为单元 分解法如A*和D*等，势场法如人工势场法和流体势场法等，以及图形学法如维诺图法及 其衍生方法。其主要特征是将机器人的运动看作从起点到终点的连续运动，通过某种算法 找到该运动可以顺畅通行的路径。

点到区路径规划方法区别与点到点路径规划方法不同之处在于，起点与后者相同，但 终止的位置不是一个固定的点，而是一个可供选择的区域。其主要方法有基于优化和基于 概率的点到区路径规划方法。这个目标区域是基于某种原则选取的目标点集合区域。目标 区域可以通过提取骨架、框架等特征的方式进行简化以提高机器人到达目标的效率和精度， 也可以在特定目标的条件下按照一定规则进行拓展或者精炼以提高规划成功率和规划效 率。显然，点到区路径规划方法比点到点提供了更多的可选路径。如果路径规划目的地不 是确定的点，显然选定一个目标区域可以更好的解决问题，例如在火星星球车想去科学家 感兴趣的目标区域时，首先应该找到到达该区域的最佳路径，然后再进一步锁定具体的检 查目标，如果选用点到点的路径规划方法，实际上浪费了更多的可行目标，可能引发不必 要的能量和时间浪费，而对于星球探索车辆来说，能量和时间都是有限，显然点到区可以 更好的解决此类问题。

基于优化的路径规划方法由于容易陷入局部极小导致路径并不是最优，即可能规划失 败，因此需要添加基于概率的方法来保证其概率完备性，即提高路径规划成功率。

发明内容

本发明的主要目的找到一条连通起点到目标区域的最优路径，提供给移动机器人作为 前进的依据。本方法将路径规划视为优化理论中的线性规划问题，并将代价函数结合其中， 进一步推导得到在线性目标区域约束下的对偶问题，通过求解对偶问题，可以解出满足目 标区域约束，并且连通起点到目标终点，同时躲避障碍的最优路径。此处的最优路径即为 同时满足路径长度较短而且路径较为平滑以便机器人顺畅跟踪的路径。而确保最优路径的 代价函数，由随着每次迭代生成的路径协变的目标代价函数和平滑代价函数构成，二者都 对路径变化产生影响。当基于优化的路径规划方法陷入局部极小时，就切换到基于概率的 路径规划方法，本发明引入了模拟退火方法结合本发明的优化方法，模拟退火方法中的代 价值求取由本发明中的最优化路径规划方法实现，来保证路径规划的概率完备性。

路径规划在经过处理的栅格地图当中进行，栅格地图标识了障碍物占据空间，通过求 取每个栅格与障碍物的占据空间的距离，得到一个距离代价场空间填充栅格地图。该距离 代价场作为寻优路径的数值参考，即越靠近障碍，距离代价越高，反之越低。这样找到的 路径中所有被路径占据的栅格距离代价之和较小就是较短的路径。同时通过添加对沿路径 变化的斜率进行表达的平滑函数，优化迭代的过程中对斜率进行最小值搜索，可以进一步 保证路径的平滑性。而通过基于模拟退火方法的改造，在基于优化方法失败的时候，可以 通过牺牲一部分路径平滑性，来确保规划成功率。

本发明中的路径规划目标区域是一个方形区域(长方形或正方形)，以便对所规划的 路径施加线性约束。

一种具有概率完备性的机器人最优化点到区路径规划方法，该方法包括以下步骤：

步骤一：通过栅格地图中的障碍物分布构建距离场分布栅格地图；

步骤二：给定移动机器人的起点和拟到达的目标区域范围；

步骤三：连接机器人的起点和方形目标区域内的任一点，作为初始路径；

步骤四：针对初始路径进行满足目标区域约束的优化求解，直到找到符合终止条件的 路径停止优化。

步骤五：当步骤四无法找到最优化路径或者陷入局部极小时，切换到基于模拟退火的 最优化路径规划方法，在到达新的终止条件时停止规划。

步骤五中所述局部极小，实质上就是步骤四计算得到的最终路径实际上经过了高代价 区域，也就是障碍区域，即规划失败。

附图说明

图1为障碍物分布栅格地图；

图2为按照图1障碍物生成的距离场；

图3为NGPA算法中d_k计算示意图；

图4 NGPA算法的单步完整流程图；

图5为完整的基于优化的路径规划算法的流程；

图6为基于模拟退火的路径规划算法的流程；

图7为距离场栅格地图中所有历史搜索路径的展示；

图8为图7中最终生成的路径展示；

图9为图7中所有搜索的历史代价汇总；

图10为与图7不同的距离场栅格地图中所有历史搜索路径的展示；

图11为图10中最终生成的路径展示；；

图12图10中所有搜索的历史代价汇总；

图13为基于优化方法搜索失败的情况；

图14为在图13搜索失败时切换到基于模拟退火的路径规划成功的情况，其中，线段 所围的区域就是目标区域，起点用圆圈表示，最终实际到达目标点用五角星表示，障碍物 所在栅格用黑色填充，距离场的强弱用灰度表示；

图15为完整路径规划算法的流程图。

具体实施方式一：明确机器人路径规划的起点和目标区域，以及不可通行的障碍物区 域，并栅格化表达距离场形式之后，将路径规划表达为一个协变代价函数在距离场内的寻 找最优解的过程，路径就是最优函数的自变量。为了保证起点和目标区域的约束效果，推 导出原路径规划问题的对偶形式，对该代价函数进行寻优计算，所得最优值的解就是最终 路径。当最优化过程陷入局部极小时，切换到基于模拟退火的规划方法，找到全局最优解， 即新的最优路径。

具体实施方式二：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，栅格地图中包含机器人的可通行区域和不可通行区域，分别对每一个栅 格，也就是机器人可能处于的任一位置，分别与障碍物的边缘按照以下公式求取距离场：

其中x机器人可能处于的任一位置，为任一栅格点与障碍区域边缘的距离，ε为临界 值。公式含义为：当机器人可能处于障碍物内部时，也就是负距离时产生一个的较大的场； 当机器人处于障碍物附近时，产生一个较小的场；当位于障碍物外部且超过临界距离时， 不产生场。这样当寻找路径所走过的栅格代价和的最小值的时候，路径的代价越小，机器 人就离障碍越远直到不受障碍影响。

障碍物分布栅格地图如图1所示，处理之后得到的距离场如图2所示。

具体实施方式三：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，本实施方式中，将路径表达为协变代价函数的形式，该路径代价函数由 目标代价函数和平滑代价函数构成：

其中ξ为整条路径的抽象化表达，为目标代价函数，为平滑代价函数，λ为权重调节值，表示对平滑函数的强调。

将第k次迭代中路径离散化后，路径的初始点表示为q₀＝ξ_k(0)，终点表示为q₁＝ξ_k(1)， 则优化迭代中的目标代价函数即从起点到终点的代价和表示为：

其中C(ξ_k)为某个路径点所位于栅格的距离场代价。

第k次迭代中目标函数中的平滑函数表示为：

如果将一个有N个路径点组成的路径视为一个N维空间的点，就可以将路径的代价函数视为一个关于N维空间变量ξ的泛函

将路径的泛函代价函数表达进行一阶泰勒展开：

其中是将代价函数泛函梯度，其中：

其中代表了路径ξ_k的曲率，ξ′_k和ξ″_k为路径的一次微分和二次 微分，代表了一次微分的模。

将一阶泰勒展开式进一步化简，就得到关于路径ξ迭代公式，即类似一次线性规划的 表达式：

其中是对泰勒展开余项的表达，η_k为调整余项在表达式中所占权重的参数。

具体实施方式四：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，本实施方式中，在得到路径代价函数的线性规划形式基础上，增加一般 线性约束Cξ≤d，其中C和d代表目标区域的线性化表达参数，则路径代价函数的迭代公 式可以写为：

为了有效施加此区域约束于具体实施方式三中的协变代价函数，需要使用求取对偶 函数的方法对此问题求解，令对偶因子为u^T，则拉格朗日乘子法的表达式为：

如果将拉格朗日乘子表达式中对偶因子u视为变量，则得到对偶因子的二次规划迭代 公式：

如果将路径ξ视为变量进一步化简拉格朗日乘子表达式，就得到最终路径的线性规划 迭代公式：

此时，整个路径的迭代由两个大步骤组成，即先通过上一次迭代中的已知量，利用二 次规划求解方法求取在对偶因子u_k+1，然后将求得的对偶因子代入最终路径迭代公式，通过 一般的线性规划迭代得到新路径ξ_k+1。反复执行这两步，直到算法收敛，得到最终的优化路 径ξ。

具体实施方式五：本实施是对具体实施方式四作进一步说明，本实施方式中为了求取 对偶因子u_k+1，将目标区域的线性约束视为框式约束，进而将对偶因子u_k+1求解问题视为框 式约束下的二次规划问题，采用了NGPA算法(Nonmonotone Gradient ProjectionAlgorithm， 非单调梯度投影算法)进行求解，并对该算法进行了简化。

NGPA算法的基本流程如下：

步骤一：将NGPA的框式约束区间设置为具体实施方式三中的线性约束框式区域；

步骤二：计算其中u_k为上一次迭代得到的对偶因子，为临时迭代步长，g_k等于具体实施方式三中的

步骤三：选择每次迭代中的参考函数f(u_k)＝G[u_k]；

步骤四：判断是否为真，如果为真，则令最终迭代步长α_k＝1， 跳转到步骤六；如果为否，继续进行步骤五；

步骤五：根据Armijo-Goldstein准则搜索迭代步长，判断是 否为真，如果为真，令j＝j-1，继续执行步骤五；如果为否，令最终迭代步长α_k＝η^j+1并 继续执行步骤六。

步骤六：计算u_k+1＝u_k+α_kd_k，得到本次循环的对偶因子，并跳回到步骤六。

步骤二中的d_k计算如图3所示，其中Ω代表了整个框式目标区域的集合。

NGPA算法的单步流程图如图4所示。

具体实施方式六：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，结合具体实施方式四和具体实施方式五，可以得到完整的最优化路径规 划算法：

步骤一：建立可行空间和不可行空间的栅格地图，并按照具体实施方式二建立距离场 空间；

步骤二：给定初始路径ξ₀，该路径连接机器人的起点和方形目标区域的任一点，和对偶因子初始值u₀；

步骤三：根据具体实施方式五中的NGPA算法，将第k次迭代得到的对偶因子值u_k代入NGPA算法，求取对偶因子值u_k+1；

步骤四：将第k次迭代得到的路径ξ_k代入具体实施方式四中的最终迭代公式：

其中ξ_k+1即为本次迭代得到路径输出值；

步骤五：将ξ_k+1作为路径输入值给入步骤三，进行新的循环，直到满足终止条件，算法结束。

步骤五中的终止条件为：相邻两次得到的路径值所对应的的目标代价函数值小于设定 值ε_e，即：|F_obs[ξ_k+1]-F_obs[ξ_k]|＜ε_e，且路径的终点落在目标区域内。

完整最优化路径规划算法的流程图如图5所示。

具体实施方式七：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，当具体实施方式六陷入局部极小，也就是计算得到的最终路径经过了障 碍区域，即规划失败时，切换到模拟退火改进的最优化路径算法，来提高路径规划的成功 率，完整流程如下：

步骤一：执行具体实施方式六的最优化路径规划算法，如规划成功，跳转到步骤100， 如陷入局部极小即规划失败，继续执行步骤二；

步骤二：设定初始温度t₀，终止温度t_e和温度下降率α；

步骤三：对当前路径施加扰动γ，作为新的初始路径；

步骤四：使用新的初始路径执行具体实施方式六中的最优化路径规划算法；

步骤五：判断本次路径规划结果与上一次路径规划结果的路径总代价之差ΔC是否小 于ε_t，如果是，则跳转到步骤八，如果否，则继续执行步骤六；

步骤六：计算接受概率p＝exp{-ΔC/t}，其中t为当前温度，随机选取0～1内任一值， 判断接受概率p是否小于它，如果是，跳转到步骤八，如果否，则继续执行步骤七；

步骤七：判断是否到达拒绝次数上限，如果是，则跳转到步骤九，如果否，则跳转回步骤三；

步骤八：接受并保留、记录此次路径规划结果，并判断是否到达接受次数上限，如是 果是，则继续执行步骤九，如果否，则跳转回步骤三；

步骤九：降低温度并判断是否达到终止温度，如果是，则继续执行步骤十，如果否则 跳转回步骤三；

步骤十：输出所有规划中代价最低的路径结果，即为全局最优解。

步骤三中的扰动γ是一个上下限适中，且符合正态分布的向量，其向量维数与路径ξ 的维数相同，其实际含义是，将路径的每一个节点随机偏移一定值，作为一条全新的临时 路径。

完整最优化路径规划算法的流程图如图6所示。

具体实施方式八：本实施是对一种具有概率完备性的机器人最优化点到区路径规划方 法作进一步说明，本实施方式中为了测试本方法的实用性，构造了一个100乘100的栅格 地图进行测试。仿真验证在MATLAB中进行，栅格地图中放置不同形状的障碍，并针对不同的起点、目标区域和障碍分布进行了仿真演示。关键参数设置如下：具体实施方式二中的ε＝0.15；具体实施方式三中权值调整系数为λ＝50；具体实施方式五中的j＝100， δ＝0.2，δ＝0.2；具体实施方式六中的u₀＝0，ε_e＝0.001；具体实施方式八中的t₀＝10，t_e＝1， α＝0.5。图7为距离场栅格地图中所有历史搜索路径的展示，距离场的强弱用灰度表示， 其中线段所围的区域就是目标区域。图8为最终生成的路径展示，其中圆点为机器人的起 点，五角星为终点。图9为所有搜索的历史代价汇总。图10为另一个与图7不同的距离场 地图，目标区域也不同。图11为最终生成的路径展示。图12为所有的搜索历史汇总。图13为基于优化方法搜索失败的情况；图14为在图13失败时切换到基于模拟退火的路径规划成功的情况，两种方法的起点和目标区域都一致。

可以看出，所规划的路径连通了起点和目标区域且较为平滑；最终路径搜索过程中一 直向着代价较低的方向前进并调整，直到符合终止条件。当基于优化的路径规划失败的时 候，切换到基于模拟退火的路径规划，可以以牺牲平滑性为代价找到可行路径，理论上只 要模拟退火的参数选择合适，在足够多次的迭代后都可以找到路径。

Claims (1)

1.一种具有概率完备性的机器人最优化点到区路径规划方法，其特征在于，机器人在距离代价地图中进行从起点到目标区域的基于最优化方法的路径规划，优化过程中以路径为自变量的代价函数是由随着每次迭代生成的路径协变的目标代价函数和平滑代价函数构成，目标代价函数和平滑代价函数都对路径变化产生影响，当基于优化的路径规划方法陷入局部极小时，就切换到基于结合模拟退火和协变代价函数的路径规划方法，来保证路径规划的概率完备性，即提高路径规划成功率。