CN111174798A - 一种足式机器人路径规划方法 - Google Patents

一种足式机器人路径规划方法 Download PDF

Info

Publication number
CN111174798A
CN111174798A CN202010053856.0A CN202010053856A CN111174798A CN 111174798 A CN111174798 A CN 111174798A CN 202010053856 A CN202010053856 A CN 202010053856A CN 111174798 A CN111174798 A CN 111174798A
Authority
CN
China
Prior art keywords
path
robot
node
algorithm
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010053856.0A
Other languages
English (en)
Inventor
惠记庄
朱雅光
徐子健
史合
明瑞浩
罗丹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changan University
Original Assignee
Changan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Changan University filed Critical Changan University
Priority to CN202010053856.0A priority Critical patent/CN111174798A/zh
Publication of CN111174798A publication Critical patent/CN111174798A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Business, Economics & Management (AREA)
  • Automation & Control Theory (AREA)
  • Human Resources & Organizations (AREA)
  • Strategic Management (AREA)
  • Economics (AREA)
  • Game Theory and Decision Science (AREA)
  • Development Economics (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Marketing (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Theoretical Computer Science (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

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

Description

一种足式机器人路径规划方法
技术领域
本发明属于机器人技术领域，具体涉及一种足式机器人路径规划方法。
背景技术
足式机器人相比于轮式、履带式等其他类型移动机器人的最大优势在于:其对多种环境类型的良好适应性，与轮式和履带式不同的通过离散落足点与地面相接触的行走方式，使足式机器人既可以轻易的通过较为崎岖复杂的环境，又可以避免对环境造成破坏。由于足式机器人需要在野外环境中完成相关任务，而野外环境常常是十分复杂且不可预期的动态环境，所以机器人要完成任务就必须具备在这样的环境能够规划出可通行路径的能力。路径规划方法的分类较多，大致可归纳为全局和局部规划两大类。全局规划无法适应动态环境，不能满足足式机器人实时避障的要求；局部规划的局部信息造成局部极值问题，且在大多数情况下无法获取最优路径。
发明内容
本发明提供了一种足式机器人路径规划方法，它既解决了全局路径规划方法无法应对动态环境，又解决了局部路径规划因为缺乏全局信息而容易陷入局部极值造成路径反复的问题，而且所得路径的长度和平滑性均较好。
为达到上述目的，本发明所述一种足式机器人路径规划方法，包括以下步骤：
步骤1、利用机器人配置相机采集环境图像；
步骤2、利用采集的环境图像并基于栅格法创建机器人创建环境模型；
步骤3、基于环境模型规划出全局路径，然后对全局路径进行路径优化，得到优化后的路径；
步骤4、以优化后的路径中的每一个路径点作为DWA的目标点来进行局部路径规划，得到最终的规划路径。
进一步的，步骤3中，采用改进的A*算法规划全局路径，规划全局路径包括以下步骤：
Step1、确定起始点S和目标点G，创建open表和close表，并将close表初始化为空集，将起始点S存储到open表中；
Step2、判断open表是否为空：
如果open表为空，则搜索结束；反之，取open表中评价函数f(n)值最小的结点作为最佳结点，将最佳结点从open表中删除，并加入到close表中；
Step3、判断Step 2选取的最佳结点是否为目标点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)，
Figure BDA0002372135850000031
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。
Figure BDA0002372135850000061
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)
为了验证改进算法的可行性，在MATLAB 2017a软件中进行了实验仿真，结果如图2a和图2b所示，在仿真地图中，圆圈为起始点，菱形为目标点，起始点和目标点之间的实线代表机器人的路径，图2a为传统的A*算法仿真结果，图2b为改进A*算法的仿真结果，传统的A*算法和改进A*算法的仿真结果具体对比见表1。
表1算法仿真对比表
Figure BDA0002372135850000081
由表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路径优化实验对比表
Figure BDA0002372135850000091
从实验结果可知，本发明所述的路径优化方法较之于改进的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算法
第一步：机器人配置的Kinect v2.0相机采集图像，图像中包括地形和障碍物；
第二步：基于三维点云数据检测法将采集到的图像填充至基于栅格法建立的环境模型，环境模型包括机器人位置和障碍物位置；
第三步：根据式(5)算出机器人所在位置与障碍物距离d，同时设置通过阈值；
第四步：如果d小于阈值，则通过机器人用DWA在某一位置的速度采样，生成预估轨迹，融合自适应思想，调整α,β,γ系数，如减小α和γ或者增大β，增大机器人通过安全性；反之，则选择从障碍物外围绕行，仍融合自适应思想，调整α,β,γ系数，例如增大α或者减小β，保证机器人的路径增幅尽可能小。
Figure BDA0002372135850000111
式中，
Figure BDA0002372135850000112
为相机的发射光与接收光的相位差，Δt对应的飞行时间，f为激光的频率，c为光传播的速度。
改进A*—自适应DWA融合算法思想
改进A*算法的其中一个特点就是只要环境中存在从起始点到达目标点的可通行路径，它总是能够成功搜索得到，这一点相比于其他局部路径算法具有非常突出的优势，本方法采用的融合算法的思想就是利用这一特性进行全局路径规划，然后再结合自适应DWA进行实时的局部规划，从而使最终路径近乎全局最优的同时也具有较好的动态适应能力，图7为融合算法的流程图。
融合算法是先以改进的A*算法规划出全局路径，然后再以该路径中的每一个路径点作为DWA的目标点来进行局部路径规划，因此该算法的局部最优问题也不复存在。具体包括以下步骤：
(1)利用机器人配置的Kinect v2.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所示)，这也为机器人的自动控制提供了较大的便利，因此，融合算法是一种比较适合机器人进行路径规划的实用方法。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (7)

1.一种足式机器人路径规划方法，其特征在于，包括以下步骤：
步骤1、利用机器人配置相机采集环境图像；
步骤2、利用采集的环境图像并基于栅格法创建机器人创建环境模型；
步骤3、基于环境模型规划出全局路径，然后对全局路径进行路径优化，得到优化后的路径；
步骤4、以优化后的路径中的每一个路径点作为DWA的目标点来进行局部路径规划，得到最终的规划路径。
2.根据权利要求1所述的一种足式机器人路径规划方法，其特征在于，所述步骤3中，采用改进的A*算法规划全局路径，规划全局路径包括以下步骤：
S1、确定起始点S和目标点G，创建open表和close表，并将close表初始化为空集，将起始点S存储到open表中；
S2、判断open表是否为空：
如果open表为空，则搜索结束；反之，取open表中评价函数f(n)值最小的结点作为最佳结点，将最佳结点从open表中删除，并加入到close表中；
S3、判断S2选取的最佳结点是否为目标点G，如果是，则流程结束；反之，则以该结点为父结点在其周围扩展子结点，并计算所有子结点的评价函数f(n)值；
S4、判断子结点是否在open表中：
如果子结点在open表中：则判断子结点基于最佳结点产生的机器人从起始点到当前点n已经历的实际代价g(n)值是否小于其原来在open表中该子结点对应的机器人从起始点到当前点n已经历的实际代价g(n)值：如果是，则更新该子结点的机器人从起始点到当前点n已经历的实际代价g(n)值和评价函数f(n)值；如果不是，保留子结点在open表该结点的g(n)值和评价函数f(n)值；
如果不在open表中：则判断该子结点是否在close表中，如果在，则忽略该子结点；
如果该子结点不在close表，则将其添加到open表中，然后跳到步骤Step2执行；
S5、继续搜寻最佳结点的邻接子节点，如果存在邻接子节点，则跳到步骤Step4执行，如果不存在邻接子节点，则跳到步骤S2执行；
S6、找到目标点G后，逆序遍历父结点得到路径。
3.根据权利要求2所述的一种足式机器人路径规划方法，其特征在于，所述S3中，评价函数f(n)＝g(n)+e^h(n)*h(n)，
Figure FDA0002372135840000021
h(n)为启发函数。
4.根据权利要求1所述的一种足式机器人路径规划方法，其特征在于，所述步骤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)为目标点。
5.根据权利要求1所述的一种足式机器人路径规划方法，其特征在于，所述步骤4中，局部路径规划的具体过程为：首先根据机器人自身物理特性在速度空间中按照采样得到多组速度，然后假设机器人在这些速度下进行运动，经过设定时间后得到多组对应的轨迹，再用评估函数G(v,ω)对上述轨迹进行评价，然后选出评估函数值最大的轨迹作为机器人下一时刻的路径。
6.根据权利要求5所述的一种足式机器人路径规划方法，其特征在于，评估函数G(v，ω)＝σ(αheading(v，ω)+βdist(v，ω)+γvel(v，ω))，式中，v为机器人的线速度，ω为角速度，heading(v,ω)表征机器人朝向目标点的程度，dist(v,ω)为机器人预测轨迹上距离最近障碍物的距离，如果没有障碍物，则根据实际情况将其设定为某一常数；vel(v,ω)为机器人预测轨迹对应的线速度。
7.根据权利要求5所述的一种足式机器人路径规划方法，其特征在于，在评估函数G(v,ω)对上述轨迹进行评价之前，计算机器人某一位置与障碍物距离d，同时设置通过阈值；若d小于阈值，则通过机器人用DWA在某一位置的速度采样，生成预估轨迹，调整α,β,γ系数，增大机器人通过安全性；反之，则选择从障碍物外围绕行，反向调整α,β,γ系数。
CN202010053856.0A 2020-01-17 2020-01-17 一种足式机器人路径规划方法 Pending CN111174798A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010053856.0A CN111174798A (zh) 2020-01-17 2020-01-17 一种足式机器人路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010053856.0A CN111174798A (zh) 2020-01-17 2020-01-17 一种足式机器人路径规划方法

Publications (1)

Publication Number Publication Date
CN111174798A true CN111174798A (zh) 2020-05-19

Family

ID=70623897

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010053856.0A Pending CN111174798A (zh) 2020-01-17 2020-01-17 一种足式机器人路径规划方法

Country Status (1)

Country Link
CN (1) CN111174798A (zh)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112013841A (zh) * 2020-08-26 2020-12-01 南京工业大学 一种室内动态环境下的语义slam服务机器人导航方法
CN112114584A (zh) * 2020-08-14 2020-12-22 天津理工大学 一种球形两栖机器人的全局路径规划方法
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
CN112526988A (zh) * 2020-10-30 2021-03-19 西安交通大学 一种自主移动机器人及其路径导航和路径规划方法、系统
CN112650234A (zh) * 2020-12-16 2021-04-13 浙江大学 一种双足机器人的路径规划方法
CN112650242A (zh) * 2020-12-22 2021-04-13 天津理工大学 一种基于混合算法的移动机器人路径规划方法
CN112710313A (zh) * 2020-12-31 2021-04-27 广州极飞科技股份有限公司 覆盖路径生成方法、装置、电子设备和存储介质
CN112731941A (zh) * 2020-12-29 2021-04-30 深圳市优必选科技股份有限公司 双足机器人路径规划方法、装置和双足机器人
CN112741688A (zh) * 2020-12-11 2021-05-04 北京信息科技大学 一种用于微创手术软体操作器的路径规划方法
CN112882468A (zh) * 2021-01-13 2021-06-01 安徽理工大学 一种改进a*算法的药房机器人路径规划方法
CN112987721A (zh) * 2021-02-01 2021-06-18 哈尔滨工业大学 一种多agv调度装及其全局规划与局部规划融合方法
CN113137969A (zh) * 2021-05-13 2021-07-20 洛阳尚奇机器人科技有限公司 一种移动机器人的局部路径优化方法
CN113254562A (zh) * 2021-06-18 2021-08-13 长安大学 一种高效gps轨迹地图匹配方法
CN113341991A (zh) * 2021-06-18 2021-09-03 重庆大学 一种基于动态窗口和冗余节点过滤的路径优化方法
CN113359718A (zh) * 2021-05-26 2021-09-07 西安理工大学 移动机器人全局路径规划与局部路径规划融合方法及设备
CN113467445A (zh) * 2021-06-17 2021-10-01 南京蔚蓝智能科技有限公司 一种基于视觉与路径规划的四足机器人摆动腿避障方法
CN114371713A (zh) * 2022-01-12 2022-04-19 深圳鹏行智能研究有限公司 足式机器人的路径规划方法、电子设备及存储介质
CN114428499A (zh) * 2021-12-16 2022-05-03 哈尔滨理工大学 一种融合Astar与DWA算法的移动小车路径规划方法
CN114489040A (zh) * 2021-12-13 2022-05-13 中煤科工集团信息技术有限公司 基于改进a*算法与人工势场算法的混合路径规划方法
CN114779772A (zh) * 2022-04-13 2022-07-22 泉州装备制造研究所 一种融合全局及局部算法的路径规划方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
CN105955280A (zh) * 2016-07-19 2016-09-21 Tcl集团股份有限公司 移动机器人路径规划和避障方法及系统
CN107491070A (zh) * 2017-08-31 2017-12-19 成都通甲优博科技有限责任公司 一种移动机器人路径规划方法及装置
CN108253987A (zh) * 2017-12-29 2018-07-06 中国地质大学（武汉） 一种基于a*算法的无人机轨迹规划方法、设备及存储设备
CN110567477A (zh) * 2019-09-27 2019-12-13 五邑大学 基于改进a*算法的路径规划方法、装置及机器人

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
CN105955280A (zh) * 2016-07-19 2016-09-21 Tcl集团股份有限公司 移动机器人路径规划和避障方法及系统
CN107491070A (zh) * 2017-08-31 2017-12-19 成都通甲优博科技有限责任公司 一种移动机器人路径规划方法及装置
CN108253987A (zh) * 2017-12-29 2018-07-06 中国地质大学（武汉） 一种基于a*算法的无人机轨迹规划方法、设备及存储设备
CN110567477A (zh) * 2019-09-27 2019-12-13 五邑大学 基于改进a*算法的路径规划方法、装置及机器人

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
刘生伟等: "改进A*算法的AGV路径规划", 《计算机应用》 *
史辉等: "A~*算法的改进及其在路径规划中的应用", 《测绘与空间地理信息》 *
秦锋等: "基于A*的双向预处理改进搜索算法", 《计算机系统应用》 *
程传奇等: "融合改进A*算法和动态窗口法的全局动态路径规划", 《西安交通大学学报》 *
陈慧岩等: "《无人驾驶汽车概论》", 31 July 2014, 北京理工大学出版社 *

Cited By (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112114584A (zh) * 2020-08-14 2020-12-22 天津理工大学 一种球形两栖机器人的全局路径规划方法
CN112013841A (zh) * 2020-08-26 2020-12-01 南京工业大学 一种室内动态环境下的语义slam服务机器人导航方法
CN112526988A (zh) * 2020-10-30 2021-03-19 西安交通大学 一种自主移动机器人及其路径导航和路径规划方法、系统
CN112304318B (zh) * 2020-11-10 2022-07-29 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
CN112304318A (zh) * 2020-11-10 2021-02-02 河北工业大学 一种虚实耦合约束环境下的机器人自主导航方法
CN112741688B (zh) * 2020-12-11 2023-09-29 北京信息科技大学 一种用于微创手术软体操作器的路径规划方法
CN112741688A (zh) * 2020-12-11 2021-05-04 北京信息科技大学 一种用于微创手术软体操作器的路径规划方法
CN112650234A (zh) * 2020-12-16 2021-04-13 浙江大学 一种双足机器人的路径规划方法
CN112650242A (zh) * 2020-12-22 2021-04-13 天津理工大学 一种基于混合算法的移动机器人路径规划方法
CN112731941A (zh) * 2020-12-29 2021-04-30 深圳市优必选科技股份有限公司 双足机器人路径规划方法、装置和双足机器人
CN112731941B (zh) * 2020-12-29 2024-03-05 深圳市优必选科技股份有限公司 双足机器人路径规划方法、装置和双足机器人
CN112710313A (zh) * 2020-12-31 2021-04-27 广州极飞科技股份有限公司 覆盖路径生成方法、装置、电子设备和存储介质
CN112882468A (zh) * 2021-01-13 2021-06-01 安徽理工大学 一种改进a*算法的药房机器人路径规划方法
CN112987721B (zh) * 2021-02-01 2022-12-13 哈尔滨工业大学 一种多agv调度装置及其全局规划与局部规划融合方法
CN112987721A (zh) * 2021-02-01 2021-06-18 哈尔滨工业大学 一种多agv调度装及其全局规划与局部规划融合方法
CN113137969A (zh) * 2021-05-13 2021-07-20 洛阳尚奇机器人科技有限公司 一种移动机器人的局部路径优化方法
CN113137969B (zh) * 2021-05-13 2023-08-04 洛阳尚奇机器人科技有限公司 一种移动机器人的局部路径优化方法
CN113359718A (zh) * 2021-05-26 2021-09-07 西安理工大学 移动机器人全局路径规划与局部路径规划融合方法及设备
CN113467445A (zh) * 2021-06-17 2021-10-01 南京蔚蓝智能科技有限公司 一种基于视觉与路径规划的四足机器人摆动腿避障方法
CN113341991B (zh) * 2021-06-18 2022-08-09 重庆大学 一种基于动态窗口和冗余节点过滤的路径优化方法
CN113254562B (zh) * 2021-06-18 2021-09-28 长安大学 一种高效gps轨迹地图匹配方法
CN113341991A (zh) * 2021-06-18 2021-09-03 重庆大学 一种基于动态窗口和冗余节点过滤的路径优化方法
CN113254562A (zh) * 2021-06-18 2021-08-13 长安大学 一种高效gps轨迹地图匹配方法
CN114489040A (zh) * 2021-12-13 2022-05-13 中煤科工集团信息技术有限公司 基于改进a*算法与人工势场算法的混合路径规划方法
CN114428499A (zh) * 2021-12-16 2022-05-03 哈尔滨理工大学 一种融合Astar与DWA算法的移动小车路径规划方法
CN114371713A (zh) * 2022-01-12 2022-04-19 深圳鹏行智能研究有限公司 足式机器人的路径规划方法、电子设备及存储介质
CN114779772A (zh) * 2022-04-13 2022-07-22 泉州装备制造研究所 一种融合全局及局部算法的路径规划方法及装置
CN114779772B (zh) * 2022-04-13 2023-08-08 泉州装备制造研究所 一种融合全局及局部算法的路径规划方法及装置

Similar Documents

Publication Publication Date Title
CN111174798A (zh) 一种足式机器人路径规划方法
CN109764886B (zh) 一种路径规划方法
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
CN110262518A (zh) 基于轨迹拓扑地图和避障的车辆导航方法、系统及介质
CN112650242A (zh) 一种基于混合算法的移动机器人路径规划方法
CN114510057A (zh) 一种室内环境中基于ros的移动机器人自主导航方法
CN114779772B (zh) 一种融合全局及局部算法的路径规划方法及装置
CN110954124A (zh) 一种基于a*-pso算法的自适应路径规划方法及系统
CN116501064A (zh) 一种光伏电站清扫机器人的路径规划及避障方法
CN116734877A (zh) 基于改进a*算法与动态窗口法的机器人动态避障方法
CN116360457A (zh) 一种基于自适应栅格和改进a*-dwa融合算法的路径规划方法
Haddeler et al. Explore bravely: Wheeled-legged robots traverse in unknown rough environment
CN110849385A (zh) 基于双层启发搜索共轭梯度下降的轨迹规划方法及系统
CN117249842A (zh) 一种基于轨迹平滑优化的无人车混合轨迹规划方法
Ma et al. Robot path planning using fusion algorithm of ant colony optimization and genetic algorithm
CN115690343A (zh) 一种基于视觉跟随的机器人激光雷达扫描建图方法
CN115046557A (zh) 一种结合b样条曲线和a星算法的agv路径规划方法
Hong et al. Path optimization for a wheel loader considering construction site terrain
Yan et al. A study of improved global path planning algorithm for parking robot based on Ros
Li et al. DF-FS based path planning algorithm with sparse waypoints in unstructured terrain
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
Wu et al. Smooth path planning method of agricultural vehicles based on improved Hybrid A
Yi et al. Global dynamic path planning based on fusion of improved a* algorithm and morphin algorithm
Lei et al. An Improved Multi-objective Path Planning Algorithm and its Application to Tennis Ball Collection Robots
CN114578824B (zh) 一种适用于空地两用机器人的未知环境自主探索方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20200519

RJ01 Rejection of invention patent application after publication