一种基于协变代价函数的机器人点到区路径规划方法与流程

本发明属于移动机器人路径规划领域，涉及一种基于协变代价函数的机器人点到区路径规划方法。

背景技术

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

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

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

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

技术实现要素：

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

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

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

一种基于协变代价函数的机器人点到区路径规划方法，该方法包括以下步骤：

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

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

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

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

附图说明

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

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

图3为ngpa算法中dk计算示意图；

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

图5为完整路径规划算法的流程；

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

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

图8为图6中所有搜索的历史代价汇总；

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

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

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

图12为路径规划整体流程图；

其中，红色线段所围的区域就是目标区域，起点用圆圈表示，最终实际到达目标点用五角星表示，障碍物所在栅格用黑色填充，距离场的强弱用灰度表示。

具体实施方式

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

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

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

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

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

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

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

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

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

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

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

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

其中：

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

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

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

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

；

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

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

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

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

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

ngpa算法的基本流程如下：

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

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

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

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

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

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

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

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

具体实施方式六：本实施是对一种基于协变代价函数的机器人点到区路径规划方法作进一步说明，结合具体实施方式四和具体实施方式五，可以得到完整的路径规划算法：

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

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

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

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

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

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

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

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

具体实施方式七：本实施是对一种基于协变代价函数的机器人点到区路径规划方法作进一步说明，本实施方式中为了测试本方法的实用性，构造了一个100乘100的栅格地图进行测试。仿真验证在matlab中进行，栅格地图中放置不同形状的障碍，并针对不同的起点、目标区域和障碍分布进行了仿真演示。关键参数设置如下：具体实施方式二中的ε＝0.15；具体实施方式三中权值调整系数为λ＝50；具体实施方式五中的j＝100，δ＝0.2，δ＝0.2；具体实施方式六中的u0＝0，εe＝0.001。图6为距离场栅格地图中所有历史搜索路径的展示，距离场的强弱用灰度表示，其中红色线段所围的区域就是目标区域。图7为最终生成的路径展示，其中圆点为机器人的起点，五角星为终点。图8为所有搜索的历史代价汇总。图9为另一个与图6不同的距离场地图，目标区域也不同。图10为最终生成的路径展示。图11为所有的搜索历史汇总。

可以看出，所规划的路径连通了起点和目标区域且较为平滑；最终路径搜索过程中一直向着代价较低的方向前进并调整，直到符合终止条件。