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

本发明专利技术提出一种基于协变代价函数的机器人点到区路径规划方法，在明确机器人路径规划的起点和目标区域，以及不可通行的障碍物区域之后，栅格化表达距离场形式，通过将路径表达为一个协变代价函数在距离场内的寻找最优解的过程，路径就是最优函数的自变量，为了保证在起点和目标区域约束有效，推导出原路径规划问题的对偶形式，对该代价函数进行寻优计算，所得最优值的解就是最终路径。本发明专利技术解决了传统路径规划方法更多侧重于到点的规划，不能有效解决点到目标区域规划的问题，同时本发明专利技术提出的方法可以兼顾路径的平滑性。

【技术实现步骤摘要】
一种基于协变代价函数的机器人点到区路径规划方法
本专利技术属于移动机器人路径规划领域，涉及一种基于协变代价函数的机器人点到区路径规划方法。
技术介绍
移动机器人的路径规划是指在含有障碍物的工作空间中，按照一定的评价指标，搜索一条可以完成指定任务的移动路径。移动机器人工作环境的描述方式主要包括栅格地图、拓扑地图、几何地图与以上几种地图的混合地图。栅格地图一般指以大小相等或不等的方形栅格将外部环境进行划分，根据栅格区域是否被障碍物占据将栅格进行二值划分。这种建模方式便于计算机存储与批量处理，因为是目前最为被广泛应用的地图建立方式。从路径规划目的性来考量，移动机器人的路径规划主要可以分为点到点和点到区路径规划方法等。点到点路径规划方法具有较长时间的研究历史，按照方法自身主要特性可以分为单元分解法如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代表目标区域的线性化表达参数，则路径代价函数的迭代公式可以写为：；为了有效施加此区域约束于具体实施方式三中的协变代价函数，需要使用求取对偶函数的方法对此问题求解，令对偶因子为uT，则拉格朗日乘子法的表达式为：如果将拉格朗日乘子表达式中对偶因子u视为变量，则得到对偶因子的二次规划迭代公式：如果将路径ξ视为变量进一步化简拉格朗日乘子表达式，就得到最终路本文档来自技高网...

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

【技术特征摘要】
1.一种基于协变代价函数的机器人点到区路径规划方法，其特征在于，明确机器人路径规划的起点和目标区域，以及不可通行的障碍物区域，并栅格化表达距离场形式之后，将路径规划表达为一个协变代价函数在距离场内的寻找最优解的过程，路径就是最优函数的自变量,为了保证起点和目标区域的约束效果，推导出原路径规划问题的对偶形式，对该代价函数进行寻优计算，所得最优值的解就是最终路径。2.根据权利要求1所述的一种基于协变代价函数的机器人点到区路径规划方法，其特征在于，该路径规划方法的距离场栅格地图构造使用以下公式：其中x机器人可能处于的任一位置，为任一栅格点与障碍区域边缘的距离，ε为临界值。公式含义为：当机器人可能处于障碍物内部时，也就是负距离时产生一个的较大的场；当机器人处于障碍物附近时，产生一个较小的场；当位于障碍物外部且超过临界距离时，不产生场。这样当寻找路径所走过的栅格代价和的最小值的时候，路径的代价越小，机器人就离障碍越远直到不受障碍影响。3.根据权利要求1所述的一种基于协变代价函数的机器人点到区路径规划方法，其特征在于，将路径表达为协变代价函数的形式，该路径代价函数由目标代价函数和平滑代价函数构成：；其中ξ为整条路径的抽象化表达，为目标代价函数，为平滑代价函数，λ为权重调节值，表示对平滑函数的强调，将第k次迭代中路径离散化后，路径的初始点表示为q0＝ξk(0)，终点表示为q1＝ξk(1)，则优化迭代中的目标代价函数即从起点到终点的代价和表示为：其中C(ξk)为某个路径点所位于栅格的距离场代价，第k次迭代中目标函数中的平滑函数表示为：如果将一个有N个路径点组成的路径视为...

【专利技术属性】
技术研发人员：尤波，李智，丁亮，高海波，许家忠，陈潇磊，
申请(专利权)人：哈尔滨理工大学，
类型：发明
国别省市：黑龙江,23