一种足式机器人路径规划方法与流程

本发明属于机器人技术领域，具体涉及一种足式机器人路径规划方法。

背景技术：

足式机器人相比于轮式、履带式等其他类型移动机器人的最大优势在于:其对多种环境类型的良好适应性，与轮式和履带式不同的通过离散落足点与地面相接触的行走方式，使足式机器人既可以轻易的通过较为崎岖复杂的环境，又可以避免对环境造成破坏。由于足式机器人需要在野外环境中完成相关任务，而野外环境常常是十分复杂且不可预期的动态环境，所以机器人要完成任务就必须具备在这样的环境能够规划出可通行路径的能力。路径规划方法的分类较多，大致可归纳为全局和局部规划两大类。全局规划无法适应动态环境，不能满足足式机器人实时避障的要求；局部规划的局部信息造成局部极值问题，且在大多数情况下无法获取最优路径。

技术实现要素：

本发明提供了一种足式机器人路径规划方法，它既解决了全局路径规划方法无法应对动态环境，又解决了局部路径规划因为缺乏全局信息而容易陷入局部极值造成路径反复的问题，而且所得路径的长度和平滑性均较好。

为达到上述目的，本发明所述一种足式机器人路径规划方法，包括以下步骤：

步骤1、利用机器人配置相机采集环境图像；

步骤2、利用采集的环境图像并基于栅格法创建机器人创建环境模型；

步骤3、基于环境模型规划出全局路径，然后对全局路径进行路径优化，得到优化后的路径；

步骤4、以优化后的路径中的每一个路径点作为dwa的目标点来进行局部路径规划，得到最终的规划路径。

进一步的，步骤3中，采用改进的a*算法规划全局路径，规划全局路径包括以下步骤：

step1、确定起始点s和目标点g，创建open表和close表，并将close表初始化为空集，将起始点s存储到open表中；

step2、判断open表是否为空：

如果open表为空，则搜索结束；反之，取open表中评价函数f(n)值最小的结点作为最佳结点，将最佳结点从open表中删除，并加入到close表中；

step3、判断step2选取的最佳结点是否为目标点g，如果是，则流程结束；反之，则以该结点为父结点在其周围扩展子结点，并计算所有子结点的评价函数f(n)值；

step4、判断子结点是否在open表中：

如果子结点在open表中：则判断子结点基于最佳结点产生的机器人从起始点到当前点n已经历的实际代价g(n)值是否小于其原来在open表中该子结点对应的机器人从起始点到当前点n已经历的实际代价g(n)值：如果是，则更新该子结点的机器人从起始点到当前点n已经历的实际代价g(n)值和评价函数f(n)值；如果不是，保留子结点在open表该结点的g(n)值和评价函数f(n)值；

如果不在open表中：则判断该子结点是否在close表中，如果在，则忽略该子结点；

如果该子结点不在close表，则将其添加到open表中，然后跳到步骤step2执行；

step5、继续搜寻最佳结点的邻接子节点，如果存在邻接子节点，则跳到步骤step4执行，如果不存在邻接子节点，则跳到步骤step2执行；

step6、找到目标点g后，逆序遍历父结点得到路径。

进一步的，评价函数f(n)＝g(n)+e^h(n)*h(n)，h(n)为启发函数。

进一步的，步骤3中，路径优化包括以下步骤：

step1、将a*算法规划所得的路径点从起始点到目标点依次排序，得到一组序列：a(1)，a(2)，a(3)….a(n)，其中，a(1)为起始点，a(n)为目标点；

step2、令s＝1，m＝n，b(1)＝a(1)；

step3、连接a(s)与a(m)形成线段l(s，m)；

step4、判断l(s，m)是否与障碍物相交：

若l(s，m)与障碍物相交，则m＝m-1，跳转至step3；

若l(s，m)与障碍物不相交，则连接a(s)与a(s+1)形成的线段l(s，s+1)；

step5、判断线段l(s，s+1)与线段l(s，m)在是否同一直线上：

若线段l(s，s+1)与线段l(s，m)在同一直线上，删除a(s)点，令s＝s+1，m＝n，跳转至step3；

若线段l(s，s+1)与线段l(s，m)不在同一直线上，则保存a(s)为关键路径点b(i)，且i＝i+1；

step6、判断a(s)是否为目标点a(n)：

如果是，则流程结束，连接b(i)各点生成新优化路径；

如果a(s)不是目标点，则重复步骤step3至step4，直至a(s)为目标点。

进一步的，步骤4中，局部路径规划的具体过程为：首先根据机器人自身物理特性在速度空间中按照采样得到多组速度，然后假设机器人在这些速度下进行运动，经过设定时间后得到多组对应的轨迹，再用评估函数g(v，ω)对上述轨迹进行评价，然后选出评估函数值最大的轨迹作为机器人下一时刻的路径。

进一步的，评估函数g(v，ω)＝σ(αheading(v，ω)+βdist(υ，ω)+γvel(v，ω))，式中，v为机器人的线速度，ω为角速度，heading(v，ω)表征机器人朝向目标点的程度，dist(v，ω)为机器人预测轨迹上距离最近障碍物的距离，如果没有障碍物，则根据实际情况将其设定为某一常数；vel(v,ω)为机器人预测轨迹对应的线速度。

进一步的，在评估函数g(v,ω)对上述轨迹进行评价之前，计算机器人某一位置与障碍物距离d，同时设置通过阈值；若d小于阈值，则通过机器人用dwa在某一位置的速度采样，生成预估轨迹，调整α,β,γ系数，增大机器人通过安全性；反之，则选择从障碍物外围绕行，反向调整α,β,γ系数。

与现有技术相比，本发明至少具有以下有益的技术效果：

与传统路径规划方法相比，本发明的方法通过路径优化去掉了路径中较多的冗余点，间接地减少了机器人完成任务的时间；因为去除冗余点，路径长度更短，将e^h(n)作为启示函数乘的系数，当h(n)与实际值相差较大时，e^h(n)也增大，这样可以减轻搜索压力，在当前点n接近目标点时，e^h(n)趋向于1，算法回归到a*算法，从而保证目标点可达，缩短了搜索时间；在处理过程中，改进a*和自适应dwa融合算法不但确定可以避开突然出现的障碍物，还能够保证全局最优性，在障碍物比较复杂的区域也可以顺利通过，环境适应性较好。

改进的a*算法较传统a*算法的拐点数大幅减低，路径拐点越少，机器人需要转向的次数也就越少，而机器人在转向时为了保持机体稳定性一般都会以较低速度通过这些地方，所以拐点越少，对机器人的运动速度的影响也越小，这也间接地减少了机器人完成任务的时间；另外，由于改进算法的扩展结点数较少，耗时短，因此其搜索效率也较高。

另外，融合算法还可以实时地输出机器人的速度和角速度等控制参数，这也为机器人的自动控制提供了较大的便利，因此，融合算法是一种比较适合机器人进行路径规划的实用方法。

附图说明

图1为a*算法的流程图；

图2a为传统的a*算法仿真结果；

图2b为改进a*算法的仿真结果；

图3为路径优化方法流程图；

图4a为a地图在基于改进的a*算法路径优化结果一；

图4b为a地图在基于改进的a*算法路径优化结果二；

图4c为b地图在基于改进的a*算法路径优化结果一；

图4d为b地图在基于改进的a*算法路径优化结果二；

图5为速度采样结果；

图6为机器人航向示意图；

图7为融合算法流程图；

图8a为传统a*算法搜索得到的全局最优路径；

图8b为改进a*算法得到的路径；

图8c为dwa得到的路径；

图8d为融合算法得到的路径；

图9a为加入动态障碍物时，传统a*算法得到的路径；

图9b为加入动态障碍物时，改进a*算法得到的路径；

图9c为加入动态障碍物时，dwa算法得到的路径；

图9d为加入动态障碍物时，的融合算法得到的路径；

图10a为融合算法输出的机器人的线速度和控制节点个数示意图；

图10b为融合算法输出的机器人的线速度和控制节点个数示意图。

具体实施方式

为了使本发明的目的和技术方案更加清晰和便于理解。以下结合附图和实施例，对本发明进行进一步的详细说明，此处所描述的具体实施例仅用于解释本发明，并非用于限定本发明。

一、a*算法的评估函数为：

f(n)＝g(n)+h(n)(1)

式(1)中，f(n)为当前结点n的评价函数值，它由g(n)和h(n)两部分组成，其中，g(n)为机器人从起始点到当前点n已经历的实际代价，h(n)为机器人从当前结点n到目标点的预计代价。a*算法最大的特点就是加入了启发函数h(n)，从而有效地缩小搜索范围，降低了算法计算量，本算法将欧式距离作为启发函数h(n)，h(n)表达式如式(2)所示，由欧式距离的计算方法可知，h(n)一定小于或等于实际代价。在当前点n距离目标点较远时，由于环境中存在障碍物的原因，预计代价会远小于实际代价，且h(n)越小，f(n)就越接近于g(n)，在搜索时，每次搜索下一个结点时，总会选择到起始结点最近的结点作为子结点，直到到达目标点或遍历整个搜索空间，导致搜索范围越大，因此算法耗时也越长，所以在此阶段增大h(n)以提高算法效率；在当前节点n接近目标点时，h(n)也接近当前点与目标点的距离的实际值，到达目标点时，则h(n)等于0。

a*算法的流程图如图1所示，主要包括以下步骤：

step1、确定起始点s和目标点g，创建open表和close表，并将close表初始化为空集，将起始点s存储到open表中；

step2、判断open表是否为空，如果是，则搜索结束；反之，则选取open表中评价函数f(n)值最小的结点作为最佳结点，并将其从open表中删除，加入到close表中；

step3、判断步骤2选取的最佳结点是否为目标点g，如果是，则算法结束；反之，则以该结点为父结点在其周围扩展子结点(不包括障碍物点)，并计算所有子结点的f(n)值；

step4、判断子结点是否在open表中，如果在open表中，则判断子结点基于最佳结点产生的g(n)值是否小于其原来在open表中该结点对应的g(n)值：如果是，则更新该结点的g(n)值和f(n)值；如果不是，保留子结点在open表该结点的g(n)值和f(n)值；

如果不在open表中，则判断该子结点是否在close表中，如果在，则忽略该子结点；

step5、如果该子结点既不在open表也不在close表，则将其添加到open表中，然后跳到步骤step2执行。

step6、继续搜寻最佳结点的邻接子节点，如果存在，则跳到步骤step4执行，如果不存在，则跳到步骤step2执行；

step7、找到目标节点后，逆序遍历父结点得到路径。

二、改进的a*算法

第一处改进：启发函数改进，本发明将欧式距离作为启示函数h(n)，由欧式距离的计算方法可知，h(n)一定小于或等于实际代价。在当前点n距离目标点较远时，由于环境中存在障碍物的原因，预计代价会远小于实际代价，h(n)越小，搜索范围越大，因此算法耗时也越长，所以在此阶段可以增大h(n)以提高算法效率；在当前点n接近目标点时，h(n)也接近实际值，到达目标点时，则h(n)等于0。本发明对a*算法的评价函数进行改进，如式(3)所示，当h(n)与实际值相差较大时，e^h(n)也增大，这样可以减轻搜索压力，在当前点n接近目标点时，e^h(n)趋向于1算法回归到a*算法，从而保证目标点可达。

f(n)＝g(n)+e^h(n)*h(n)(3)

为了验证改进算法的可行性，在matlab2017a软件中进行了实验仿真，结果如图2a和图2b所示，在仿真地图中，圆圈为起始点，菱形为目标点，起始点和目标点之间的实线代表机器人的路径，图2a为传统的a*算法仿真结果，图2b为改进a*算法的仿真结果，传统的a*算法和改进a*算法的仿真结果具体对比见表1。

表1算法仿真对比表

由表1可知，两种算法的路径长度相差不大，但改进算法在耗时、路径拐点数和扩展结点数等方面皆优于传统的a*算法，尤其是拐点数，两者相差较大，路径拐点越少，机器人需要转向的次数也就越少，而机器人在转向时为了保持机体稳定性一般都会以较低速度通过这些地方，所以拐点越少，对机器人的运动速度的影响也越小，这也间接地减少了机器人完成任务的时间，所以拐点数是评价路径优劣的一个重要指标，因此拥有更少拐点数的改进算法在路径可行性方面表现更好。另外，由于改进算法的扩展结点数较少，耗时短，因此其搜索效率也较高，综上，改进的a*算法较之于传统a*算法有一定的优势。

第二处改进：路径优化

a*算法和改进算法都可以找到安全无碰撞的路径，但由图2a和2b可知，在两种方法生成的路径中，存在较多的冗余点和转折点，冗余点会增大路径的长度，而转折点则对于机器人的实际运动不利，为了得到更好的路径，因此由算法规划出来的路径还需要进行相应的路径优化处理。主要分为三步，一是选出关键转折点，二是去除冗余点，三是连接剩余路径点生成新路径。图3为本发明采取的路径优化方法流程图。具体实施步骤如下：

step1、将a*算法规划所得的路径点从起始点到目标点依次排序，如a(1)，a(2)，a(3)….a(n)，a(1)为起始点，a(n)为目标点；

step2、初始化：令s＝1，m＝n，b(1)＝a(1)；

step3、连接a(s)与a(m)形成线段l(s,m)；

step4、判断l(s,m)是否与障碍物相交；

如果l(s,m)与障碍物相交，则m＝m-1，跳转至step3；

如果l(s,m)与障碍物不相交，则连接a(s)与a(s+1)形成的线段l(s,s+1)；

step5、判断线段l(s,s+1)与线段l(s,m)在是否同一直线上：

如果线段l(s,s+1)与线段l(s,m)在同一直线上，删除a(s)点，令s＝s+1，m＝n，跳转至step3；

如果线段l(s,s+1)与线段l(s,m)不在同一直线上，则保存a(s)为关键路径点b(i)，且i＝i+1；

step6、判断a(s)是否为目标点a(n)：

如果是，则流程结束，连接b(i)各点生成新优化路径；

如果a(s)不是目标点，则重复步骤step3至step4，直至a(s)为目标点。

为了验证上述提出的路径优化方法的可行性，进行了仿真实验，并将本发明所述方法与改进a*算法进行比较。在两种环境地图a和b中进行路径规划，实验结果如图4a至图4d所示，图4a和图4b为a地图在基于改进的a*算法路径优化结果，图4c和图4d为b地图在基于改进的a*算法路径优化结果，图中：圆圈为起始点，菱形为目标点，1号实线为改进的a*算法的规划路径，2号实线为优化路径，*号为由路径优化后保留的路径拐点。

根据本发明所研究的机器人运动特点以及为了更好凸显算法的优越性，本发明利用路径长度、运行时间和转折次数等评估指标来评判两种方法分别规划的路径，表2为两种方法在两种地图中的各项实验数据的比较

表2路径优化实验对比表

从实验结果可知，本发明所述的路径优化方法较之于改进的a*算法，在路径长度和路径转折次数(路径减短4％左右，转折次数减少40％左右)等方面有较大的改善，虽然在算法运行时间上稍多于后者(增加2.6％左右)，但相比于前两项指标较大提升，运行时间的损耗在可接受范围之内。因此，由本发明提到的路径优化方法得到的路径相比于改进的a*算法得到的路径更有优势，表明该方法对路径的优化作用较为明显。

三、dwa算法

dwa首先根据机器人自身物理特性在速度空间中按照一定规则采样得到多组速度，如图5所示，然后假设机器人在这些速度下进行运动，经过某一段时间后可以得到多组对应的轨迹，再采取评估函数对这些轨迹进行评价，最后选出最符合机器人运动的轨迹，而该最优轨迹所对应的运动参数将用来控制机器人运动。

图5为机器人在某一位置的速度采样结果，机器人根据当前点速度和环境信息确定下一时刻的采样速度范围，并生成相应的预估轨迹。由于轨迹几乎不唯一，如果要得到最优解，则需要对这些轨迹进行评估。评估标准应根据机器人的实际运行特点来设计，首先要使机器人自身安全性得到保证，其次尽可能使径较短和平滑，综上，本发明选取的评估函数如下：

g(v，ω)＝σ(αheading(v，ω)+βdist(v，ω)+γvel(v，ω))(4)

式中，式中，v为机器人的线速度，ω为角速度。heading(v，w)表征机器人朝向目标点的程度，用180-θ表示，其θ中为机器人朝向和它与目标点之间的夹角，如图6所示。dist(v，ω)为机器人预测轨迹上距离最近障碍物的距离，如果没有障碍物，则根据实际情况将其设定为某一常数；vel(v，ω)为机器人预测轨迹对应的线速度；另外α、β、γ为上述三个因素的加权系数，σ为平滑系数。最后要得到最优轨迹就是利用式(4)对所有预估轨迹进行评估，然后选出评估函数值最大的轨迹作为机器人下一时刻的路径。

四、dwa存在的问题分析

1在实际的应用中，机器人的工作环境大都比较复杂多变，充满不确定性，要在这样的环境中完成路径规划任务就必须要求路径规划方法具有较强的抗干扰能力，能够可靠且及时的采取措施来应对意外情况.dwa采集用于路径规划的信息是局部的，它对于整个环境信息无法进行全局把控，所以不可避免的会陷入一些局部陷阱之中而无法逃脱,造成局部最优问题。

2目前的dwa应用过程中，α,β,γ的取值一般还是取某一定值，这一定值的确定则主要是参考相关经验值或者实地测试结果，这种取值方法不仅适应性较差而且效率也较低，非常不利于需要在多种地形和高效率状态下工作的机器人进行路径规划任务。

五、自适应dwa算法

第一步：机器人配置的kinectv2.0相机采集图像，图像中包括地形和障碍物；

第二步：基于三维点云数据检测法将采集到的图像填充至基于栅格法建立的环境模型，环境模型包括机器人位置和障碍物位置；

第三步：根据式(5)算出机器人所在位置与障碍物距离d，同时设置通过阈值；

第四步：如果d小于阈值，则通过机器人用dwa在某一位置的速度采样，生成预估轨迹，融合自适应思想，调整α,β,γ系数，如减小α和γ或者增大β，增大机器人通过安全性；反之，则选择从障碍物外围绕行，仍融合自适应思想，调整α,β,γ系数，例如增大α或者减小β，保证机器人的路径增幅尽可能小。

式中，为相机的发射光与接收光的相位差，δt对应的飞行时间，f为激光的频率，c为光传播的速度。

改进a*—自适应dwa融合算法思想

改进a*算法的其中一个特点就是只要环境中存在从起始点到达目标点的可通行路径，它总是能够成功搜索得到，这一点相比于其他局部路径算法具有非常突出的优势，本方法采用的融合算法的思想就是利用这一特性进行全局路径规划，然后再结合自适应dwa进行实时的局部规划，从而使最终路径近乎全局最优的同时也具有较好的动态适应能力，图7为融合算法的流程图。

融合算法是先以改进的a*算法规划出全局路径，然后再以该路径中的每一个路径点作为dwa的目标点来进行局部路径规划，因此该算法的局部最优问题也不复存在。具体包括以下步骤：

(1)利用机器人配置的kinectv2.0相机采集图像，图像中包含地形和障碍物；

(2)利用采集的环境信息并基于栅格法创建机器人创建环境模型，环境模型包括机器人位置和障碍物位置；

(3)以改进的a*算法规划出全局路径，对该全局路径进行优化；

(4)以该路径中的每一个路径点作为dwa的目标点来进行局部路径规划，得到最终的规划路径。

为了验证融合算法既能够保证路径全局最优又具有较好动态避障性能，设计了如下两组实验，其中实验一为静态环境，实验二是在实验一的基础上加入动态障碍物。另外，为了比较融合算法与其他算法的性能，利用传统a*算法、改进a*算法和dwa在两种实验环境中分别进行路径规划，实验结果如图8a至图8d和图9a至图9d所示。

图8a至图8d为静态环境的实验结果，图8a为传统a*算法搜索得到的全局最优路径，路径长度为256.16；图8b为改进a*算法得到的路径，路径长度为242.32；由于其在启发函数与路径优化两方面都做了改进，所以所得路径与传统a*算法相比，不仅删除了路径中较多的冗余点，路径长度更短，而且算法搜索时间也较短。图8c为dwa得到的路径，路径长度为312.28；与传统a*算法相比，路径平滑性更好，但不是全局最优，而且路径较长。图8d为融合算法得到的路径，路径长度为253.48，与a*算法相比，路径更加平滑，与dwa相比，路径具有全局最优性，路径长度也大幅下降。

图9a至图9d为加入动态障碍物的实验结果，动态障碍物为图中蓝色方块所示。由图9a和图9b可知，传统a*算法和改进a*算法均无法应对动态障碍物，由图9c可知，dwa可以实时避障，但无法通过障碍物密集区域，而且所得路径也不是全局最优，图9d所示的融合算法不仅可以避开突然出现的障碍物，还能够保证全局最优性，在障碍物比较复杂的区域也可以顺利通过，环境适应性较好。另外，融合算法还可以实时地输出机器人的速度和角速度等控制参数(如图10a和图10b所示)，这也为机器人的自动控制提供了较大的便利，因此，融合算法是一种比较适合机器人进行路径规划的实用方法。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。