一种基于改进型人工势场法的机器人路径规划方法及系统的制作方法
【技术领域】
[0001] 本发明设及机器人及智能车辆的局部导航领域，特别是一种基于改进型人工势场 法的机器人路径规划方法。
【背景技术】
[0002] 移动机器人实时路径规划和导航是反映机器人自主能力的关键要素之一，也是较 难解决的问题之一。机器人路径规划主要分为环境信息已知的规划和环境信息未知的规 划。对于前者多采用离线规划，得到的路径较优，后者多采用在线规划，体现了路径规划的 实时性。
[0003] 近年来许多移动机器人路径规划的方法被人们所研究。主要的路径规划的方法可 W分为两类:人工智能的方法(AI)和人工势场法(APF)。前者主要运用的方法有遗传算法 (GA)、模糊逻辑控制(FLC)和人工神经网络(ANN)，运些方法往往较为复杂运算速度也较为 缓慢。而后者由于其简洁性和快速性在机器人路径规划中得到广泛应用，其基本思想是环 境中的目标点对其的吸引力W及障碍物的对其的排斥力构成一种势场环境。然而人工势场 法在避障应用的过程中，常常会遇到局部最小的问题。
[0004] 如何避免人工势场法在避障路径规划中出现局部最小的问题，是移动机器人路径 规划的关键。通过查阅专利和论文，主要有沿墙跟踪方法、极限环法、虚拟水流法及引入内 部主体状态法等算法。
[0005] 运些方法虽然在一定程度上缓解了局部最小问题，但都有其各自的缺陷。其中沿 墙跟踪法和极限环法存在规划速度慢的问题。虚拟水流法在解决环境已经情况下的局部极 小点问题有一定的效果，但算法效率不高。引入内部主体状态的方法成功的解决了复杂环 境中的局部最小值问题，但是不能解决通常情况下的静态势场问题，通用性不强。
[0006] 由于上述算法存在实时性不好、效率不高、会引入如环境适应能力差等一些新的 问题等缺点。
[0007] 因此，急需一种既具有高效率，又能保证实时性且通用性强的改进的人工势场法。
【发明内容】
[000引本发明的目的就是提供一种基于改进型人工势场法的机器人路径规划方法及系 统，适用于局部导航中，采用基于势场法的运动规划及规避障碍物。
[0009] 本发明的目的是通过运样的技术方案实现的：
[0010] 本发明提供了一种基于改进型人工势场法的机器人路径规划方法，包括W下步 骤：
[0011] S1:获取机器人的初始化状态参数、环境信息和最终目标点；
[0012] S2:获取机器人当前坐标位置和局部目标点；
[0013] S3:建立基于时间虚拟驱动力的人工势场法生成机器人当前坐标位置和局部目标 点之间的可达路径；
[0014] S4:控制机器人沿可达路径行进；
[0015] S5:在激光雷达可视范围内检测机器人当前坐标位置是否达到局部目标点，如果 没有达到，则返回步骤S4继续控制驱动机器人行进；
[0016] S6:如果达到局部目标点，则检测机器人是否达到最终目标点，如果没有达到，贝U 返回步骤S2;
[0017] S7:如果达到最终目标点，则结束机器人的行进。
[0018] 进一步，所述局部目标点的确定，具体包括W下步骤：
[0019] S21:检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果 没有障碍物，则设置最终目标点作为局部目标点；
[0020] S22:如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍 物充分安全距离的任一点最为新坐标点作为局部目标点；
[0021 ] S23:如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试 探点寻找斥力场合力零的坐标作为局部目标点。
[0022] 进一步，所述试探点寻找斥力场合力零的坐标具体按照W下步骤来实现：
[0023] S231:按照W下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；
[0024]
[0025] 式中；
[0026] δ :相应的正比例位置增益系数；
[0027] Ρ0:正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；
[002引 p(q):某一障碍物区域Cobs到位置q的最小距离，对于所有的q/ eC0bs，P(q)=min| q-q' Μ;
[0029] S232:根据斥力势函数按照W下公式确定机器人所受的排斥力：
[0030]
[00川式中，用qc表示障碍物区域Cobs上距离q最近的位置点；
[003^ p(q)= I |q-qc| I ;
[0033] ^是由qc指向q的单位向量，
[0034] S233:选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥 力势场，
[0035] S234:根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。
[0036] 进一步，所述机器人在达到局部目标点之前还包括W下步骤来实现：
[0037] S31:建立机器人与障碍物之间的斥力势函数并计算斥力：
[003引S32:建立机器人与局部目标点之间的引力势函数；
[0039]
[0040] 式中；
[0041] ε:人工势场法引力势场增益参数；
[0042] 化(q):机器人当前位置距离局部目标点的欧氏距离；
[0043] S33:按照W下公式计算局部目标点对机器人的引力：
[0044]
[0045] S34:通过W下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；
[0046]
[0047] S35:按照W下公式计算时间虚拟驱动力的大小：
[004引
[0049] 式中；
[0050] 丫 ：调节Ftime的常数。
[0051] 本发明提供了一种基于改进型人工势场法的机器人路径规划系统，包括机器人参 数信息采集单元、机器人位置采集单元、局部目标点生成单元、可达路径生成单元、进行控 制单元、局部目标点判断单元和最终目标点判断单元；
[0052] 所述机器人参数信息采集单元，用于获取机器人的初始化状态参数、环境信息和 最终目标点；
[0053] 所述机器人位置采集单元，用于获取机器人当前坐标位置；
[0054] 所述局部目标点生成单元，用于确定机器人行进中的局部目标点；
[0055] 所述可达路径生成单元，用于计算机器人当前位置和局部目标点之间的可达路 径；
[0056] 所述进行控制单元，用于控制、驱动机器人沿可达路径行进；
[0057] 所述局部目标点判断单元，用于计算并判断机器人当前坐标位置是否达到局部目 标点；
[0058] 所述最终目标点判断单元，用于计算并判断机器人是否达到最终目标点；
[0059] 所述机器人参数信息采集单元和机器人位置采集单元分别与局部目标点生成单 元连接;所述机器人位置采集单元与可达路径生成单元连接;所述可达路径生成单元与进 行控制单元连接;所述局部目标点判断单元和最终目标点判断单元分别与行进控制单元连 接。
[0060] 进一步，所述局部目标点生成单元是按照W下步骤来进行的：
[0061] S11:检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果 没有障碍物，则设置最终目标点作为局部目标点；
[0062] S12:如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍 物充分安全距离的任一点最为新坐标点作为局部目标点；
[0063] S13:如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试 探点寻找斥力场合力零的坐标作为局部目标点；
[0064] 所述试探点寻找斥力场合力零的坐标具体按照W下步骤来实现：
[0065] S14:按照W下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；
[0066]
[0067]式中；
[006引δ :相应的正比例位置增益系数；
[0069] Ρ0:正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；
[0070] P(q):某一障碍物区域Cobs到位置q的最小距离，对于所有的q/ eC〇bs，P(q)=min| q-q' Μ;
[0071] S15:根据斥力势函数按照w下公式确定机器人所受的排斥力：
[0072]
[007引式中，用qc表示障碍物区域Cobs上距离q最近的位置点；
[0074] p(q)=||q-qc||;
[0075] 贏是由qc指向q的单位向量，
[0076] S16:选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力 势场，
[0077] S17:根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。
[0078] 进一步，还包括与进行控制单元连接的时间虚拟驱动力单元;所述时间虚拟驱动 力单元机是通过W下步骤来实现：
[0079] S41:建立机器人与障碍物之间的斥力势函数并计算斥力：
[0080] S42:建立机器人与局部目标点之间的引力势函数；
[0081]
[0082] 式中；
[0083] ε:人工势场法引力势场增益参数；
[0084] 化(q):机器人当前位置距离局部目标点的欧氏距离；
[0085] S43:按照W下公式计算局部目标点对机器人的引力：
[0086]
[0087] S44:通过W下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；
[00则玲￡￡ (的=-马巧(机
[0089] S45:按照W下公式计算时间虚拟驱动力的大小：
[0090] 、
/
[0091] 式中：
[0092] 丫 ：调节Ftime的常数。
[0093] 由于采用了上述技术方案，本发明具有如下的优点：
[0094] 本发明是基于人工势场法来规划机器人路径规划，解决了传统势场法对机器人进 行路径规划出现的局部极小点问题，提高了路径规划的实时性、环境适应性效率。本发明对 传统的人工势场法进行了改进，即改进引力势函数，同时将整个任务划分为许多局部目标 点，从而达到最优的路径。
[0095] 引力势函数把机器人到达目标的预计时间考虑进去，并将该时间转化为虚拟的引 力，其一，保证目标点为整个势场的全局