基于单目相机测距不确定的机器人路径稳健性规划方法与流程

本发明涉及一种机器人路径规划方法，特别涉及一种基于单目相机测距不确定的机器人路径稳健性规划方法。

背景技术：

路径规划是指在分布有障碍物的环境中，找到从起始点到目标点的最合理、高效的避障路径，路径规划是移动机器人导航中的关键技术之一。现有的路径规划方法分为经典路径规划和智能路径规划，经典路径规划如：人工势场法、概率地图法和快速搜索随机树法等，经典路径规划方法有时不能产生最优路径，而且往往被锁定在局部极小值中。在存在多个障碍物或动态环境下，其中一些可能无法提供合适的解决方案。

因此，经典路径规划已经逐渐被智能路径规划所取代。现有的智能路径规划算法包括粒子群算法、蚁群算法、遗传算法及萤火虫算法等。其中萤火虫算法因其算法结构简单，求解能力强适用于优化复杂问题等特点，受到了许多学者关注和研究。基于萤火虫智能方法的路径规划方法实质是在避开障碍物的前提下，根据路径规划的目标函数，按照萤火中的寻优规则，更新机器人的运动方向。

在机器人每一步动态路径规划过程中，都必须知道机器人与障碍物的距离，已有的路径规划方法都将此变量在数学模型上处理成一个确定变量。其实由于光线或测量方法、仪器精度等变量，该变量具有一定的不确定性。目前，针对路径规划中不确定的研究，学者们常常关注于不确定未知环境和不确定动态环境的情况，包括目标点处于动态不确定的情况、障碍物处于动态不确定或者未知情况，例如申请号为201210376358.5的发明专利，提供了一种面向混合地形区域不确定环境的机器人全局路径规划方法；申请号为200910226707.3的发明专利，提供了一种水下清洗作业机器人未知环境路径规划方法；申请号为201310195535.4的发明专利，提供了一种动态环境下移动机器人路径规划方法。

学者们少有研究在路径规划过程中考虑移动机器人对外界环境感知的不确定性对于路径规划的影响。路径规划时忽视测量不确定的影响会增加机器人在实际移动过程中与障碍物发生碰撞的风险。因此，在路径规划中研究机器人环境感知的不确定性有着非常重要的意义。

申请号为201710019066.9的发明专利，公开了一种存在测量不确定性下的反馈路径规划方法，其通过考虑测量不确定性对于反馈吸引域的影响，采用庞德里亚金集差缩小了吸引域的大小，重新评估路径规划过程中的安全避障问题，并采用gp算法在线估计吸引域大小从而克服吸引域计算缓慢的缺陷。

技术实现要素：

为了解决上述技术问题，本发明提供一种算法简单、准确度高的基于单目相机测距不确定的机器人路径稳健性规划方法。

本发明解决上述问题的技术方案是：一种基于单目相机测距不确定的机器人路径稳健性规划方法，包括以下步骤：

第一步，建立地图模型：在已有的像素点环境模型的基础上，连接路径起点与目标点，以路径起点与目标点连线方向等间距划分n条垂直于连线的垂线，在每条垂线上各取样一个点，得到一个n维点集，将n维点集中相邻两点用直线连接起来即得到一条完整的路径，以路径起点为原点，路径起点与目标点连线方向为横坐标，垂线方向为纵坐标建立坐标轴；重复第一步，得到n条路径；

第二步，构建不确定性优化模型：以路径长度最短为目标函数，以不与障碍物发生碰撞为约束条件，构建不确定性优化模型；

第三步，求解不确定性优化模型：针对路径规划不确定优化模型建立亮度函数，基于亮度函数利用萤火虫智能算法求解路径规划问题，输出最优路径。

上述基于单目相机测距不确定的机器人路径稳健性规划方法，所述第一步中，记等间距划分的n条垂直于连线的垂线为(l1,l2,…,ln)，得到的n维点集为(ph1,ph2,…,phn)，得到完整的路径为path＝(st,ph1,ph2,…,phn,ta)，其中st为起点，ta为终点。

上述基于单目相机测距不确定的机器人路径稳健性规划方法，所述第二步中，不确定性优化模型具体表达式为：

min

s.t.

h^l≤h≤h^u,

其中l(path)为路径长度，phi,phi+1分别为第i条垂线、第i+1条垂线上的纵坐标值，为第i条垂线与第i+1条垂线之间路径的长度，l0为相邻两点的固定水平间隔；dis为路径与障碍物之间的距离，f为单目相机焦距，h为相机固定高度，ypix为图像中障碍物底部与图像水平中心线的像素距离。

上述基于单目相机测距不确定的机器人路径稳健性规划方法，第三步中的亮度函数为：

其中α1为长度系数，α2为风险系数，且α1+α2＝1，light(pathj)为第j条路径的亮度函数，j＝1，2…n，l(pathj)为第j条路径的路径长度，d(pathj)为第j条路径的路径风险，路径长度的计算公式为路径风险的计算公式为其中dismin(phi,phi+1)为第i条垂线与第i+1条垂线之间路径与障碍物之间最短距离。

上述基于单目相机测距不确定的机器人路径稳健性规划方法，第三步中利用萤火虫智能算法求解路径规划问题的步骤为：

s1：初始化萤火虫算法参数；

s2：随机初始化路径并计算路径亮度；

s3：比较路径亮度，根据路径亮度更新路径位置；

s4：采用蒙特卡罗方法评估路径与障碍物之间距离并更新路径亮度；

s5：重复s2-s4直至达到迭代次数后输出最优路径。

上述基于单目相机测距不确定的机器人路径稳健性规划方法，步骤s4中采用蒙特卡罗方法评估路径与障碍物之间距离的过程为：单目相机测距公式：焦距测量随机变量f～n(μ,σ²)，其中u为期望值，σ为标准差，高度测量随机变量h～u(h^l,h^u)，其中h^l为高度测量值下限，其中h^u为高度测量值上限，像素距离随机变量其中为像素距离值下限，其中为像素距离值上限，从标准正态分布中随机抽样m次得到焦距抽样合集fm(fm1,fm2,…,fmm)，fmm为第m次抽样得到的焦距测量值，从标准均匀分布中随机抽样m次分别得到高度抽样合集hm(hm1,hm2,…,hmm)和像素距离抽样合集pixm(pixm1,pixm2,…,pixmm)，hmm为第m次抽样得到的高度测量值，pixmm为第m次抽样得到的像素距离测量值；分别计算fm＝μ+σfm，hm＝h^l+(h^u-h^l)hm和其中fm为焦距不确定度分量，hm为高度不确定度分量，pixm为像素距离不确定度分量；依据单目相机测距公式计算mean(dism)为路径与障碍物的距离测量的评估值。

本发明的有益效果在于：本发明首先建立地图模型，在已有的像素点环境模型的基础上，对以路径起点与目标点的连线方向等间距切分路径，从而保证每条路径的维数一致；然后以路径长度最短为目标函数，以不与障碍物发生碰撞为约束条件，构建不确定性优化模型；最后求解不确定性优化模型：针对路径规划不确定优化模型建立亮度函数，考虑路径长度和路径安全度两个指标，使得该方法不仅有很好的避障功能且能规划出较短的路径，在路径规划过程中采用蒙特卡罗方法评估路径与障碍物之间的距离，利用萤火虫智能算法求解路径规划问题，输出最优路径，采用蒙特卡罗方法对单目相机测距的测量值进行评估，评估的结果考虑了测量的不确定性，减小了测量的不确定性对路径规划的影响，在机器人实际移动过程中具有更强的可行性。

附图说明

图1为本发明的流程图。

图2为本发明的萤火虫智能算法的流程图。

图3为本发明实施例中建立的地图模型的示意图。

图4为使用本发明方法的路径规划效果图。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

如图2、图3所示，路径规划的环境模型为二维平面的正方形，障碍物为静态障碍物，可以根据实际场景描述；路径规划的总体流程图如图1所示。

一种基于单目相机测距不确定的机器人路径稳健性规划方法，包括以下步骤：

第一步，建立地图模型：路径规划计算域为300*500的像素点矩形，相邻两个像素点的距离代表1cm物理距离，因此环境地图的区域为300cm*500cm，在已有的像素点环境模型的基础上，连接路径起点与目标点，以路径起点与目标点连线方向以40cm为间距等间距划分11条垂直于连线的垂线并记为(l1,l2,…,l11)，在每条垂线上各取样一个点，得到一个11维点集(ph1,ph2,…,ph11)，将11维点集中相邻两点用直线连接起来即得到一条完整的路径，path＝(st,ph1,ph2,…,ph11,ta)，其中st为起点，坐标为(10,150)，ta为终点,坐标为(490,150),因此路径规划问题转化为11维点集(ph1,ph2,…,ph11)的优化问题。以路径起点为原点，路径起点与目标点连线方向为横坐标，垂线方向为纵坐标建立坐标轴；重复第一步，得到n条路径；

第二步，构建不确定性优化模型：以路径长度最短为目标函数，以不与障碍物发生碰撞为约束条件，构建不确定性优化模型；

不确定性优化模型具体表达式为：

min

s.t.

h^l≤h≤h^u,

其中l(path)为路径长度，phi,phi+1分别为第i条垂线、第i+1条垂线上的纵坐标值，为第i条垂线与第i+1条垂线之间路径的长度，l0为相邻两点的固定水平间隔，dis为路径与障碍物之间的距离，f为单目相机焦距，f＝50mm，h为相机固定高度，h＝100mm，ypix为图像中障碍物底部与图像水平中心线的像素距离。

第三步，求解不确定性优化模型：针对路径规划不确定优化模型建立亮度函数，基于亮度函数利用萤火虫智能算法求解路径规划问题，输出最优路径。

亮度函数为：

其中α1为长度系数，α2为风险系数，且α1+α2＝1，α1＝0.55,α2＝0.45，light(pathj)为第j条路径的亮度函数，j＝1，2…n，l(pathj)为第j条路径的路径长度，d(pathj)为第j条路径的路径风险，路径长度的计算公式为其中phi,phi+1分别为第i条垂线、第i+1条垂线上的纵坐标值，路径风险的计算公式为其中dismin(phi,phi+1)为第i条垂线与第i+1条垂线之间路径与障碍物之间最短距离。

利用萤火虫智能算法求解路径规划问题的步骤为：

s1：初始化萤火虫算法参数；

s2：随机初始化路径并计算路径亮度；

s3：比较路径亮度，根据路径亮度更新路径位置；

s4：采用蒙特卡罗方法评估路径与障碍物之间距离并更新路径亮度；

s5：重复s2-s4直至达到迭代次数后输出最优路径。

采用蒙特卡罗方法评估路径与障碍物之间距离的过程为：单目相机测距公式：焦距测量随机变量f～n(μ,σ²)，其中u为期望值，σ为标准差，高度测量随机变量h～u(h^l,h^u)，其中h^l为高度测量值下限，其中h^u为高度测量值上限，像素距离随机变量其中为像素距离值下限，其中为像素距离值上限，从标准正态分布中随机抽样m次得到焦距抽样合集fm(fm1,fm2,…,fmm)，fmm为第m次抽样得到的焦距测量值，从标准均匀分布中随机抽样m次分别得到高度抽样合集hm(hm1,hm2,…,hmm)和像素距离抽样合集pixm(pixm1,pixm2,…,pixmm)，hmm为第m次抽样得到的高度测量值，pixmm为第m次抽样得到的像素距离测量值；分别计算fm＝μ+σfm，hm＝h^l+(h^u-h^l)hm和其中fm为焦距不确定度分量，hm为高度不确定度分量，pixm为像素距离不确定度分量；依据单目相机测距公式计算mean(dism)为路径与障碍物的距离测量的评估值。

与现有技术相比，本发明的优点在于：本发明涉及一种基于单目相机测距不确定的机器人路径稳健性规划方法，构建了一种新的萤火虫算法亮度函数，考虑路径长度和路径安全度两个指标，使得该方法不仅有很好的避障功能且能规划出较短的路径。采用蒙特卡罗方法对单目相机测距的测量值进行评估，评估的结果考虑了测量的不确定性，在机器人实际移动过程中具有更强的可行性。图3为仿真实验结果。