一种基于人工势场的煤矿救灾机器人路径规划方法
【专利摘要】一种基于人工势场的煤矿救灾机器人路径规划方法，属于机器人路径规划方法。该方法是在机器人工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤：S-1根据先验地图，建立机器人局部工作空间斥力势�。籗-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；S-3求解该路径方程，得到局部路径；S-4如果检测到算法陷入局部最优，则处理局部最优问题；S-5重复S-1～S-4，直至到达目的点，得到初始路径；S-6对初始路径进行优化，减少路径长度，得到最终路径。该方法能得到较为精确和较短的路径，同时不需要耗费太多的运行时间，提高了救援时间和救援效率。
【专利说明】一种基于人工势场的煤矿救灾机器人路径规划方法
【技术领域】
[0001] 本发明涉及一种机器人路径规划方法，特别是一种基于人工势场的煤矿救灾机器 人路径规划方法。
【背景技术】
[0002] 目前，我国煤矿事故频发，不仅造成了生命和财产的重大损失，而且还影响社会的 安定。搜救遇险人员是发生矿难以后的重要工作，传统的搜救设备不能有效地实现受困人 员救助，而且二次事故常常会导致救援队员的伤亡。
[0003] 煤矿井下救灾机器人作为一种智能探测和搜救设备可以获取和发送灾变区域参 数和生还者信息，可以替代人从事搜索和救援工作，从而降低了搜救人员的风险。
[0004] 近些年来，已有的一些救灾机器人大多是采用�？胤绞绞够魅诵薪捅苷希� 主性比较差，在实际应用中还有一定的局限性。受井下恶劣环境的限制，许多地面机器人的 自主导航技术不能在井下应用，例如GPS导航无法应用于矿井；无线电导航虽然精度很高， 但煤层对电波有较大吸收，因此不适合应用于井下；惯性导航需要安装大量的辅助标示物， 一旦发生矿难会损坏大量的标示物影响灾后导航；激光和超声等方式抗干扰较差。
[0005] 路径规划技术是视觉导航中的重要组成部分，人工势场法是一种简单有效的路径 规划方法，应用极其广泛。在传统的人工势场法中，势场不能有效反映障碍物的轮廓，因此 得不到精确的路径，而为了获得精确的势场分布，又要耗费大量的运算时间。
【发明内容】
[0006] 本发明的目的是要提供一种既能精确反映障碍物轮廓又能减少运算时间的基于 人工势场的煤矿救灾机器人路径规划方法，克服传统人工势场的缺点。
[0007] 本发明的目的是这样实现的：该方法是在机器人工作空间已知、工作空间障碍物 用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境 下进行的，所述的路径规划方法包括以下步骤：
[0008] S-I根据先验地图，建立机器人局部工作空间斥力势�。�
[0009] S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；
[0010] S-3求解该路径方程，得到局部路径；
[0011] S-4如果检测到算法陷入局部最优，则处理局部最优问题；
[0012] S-5重复S-I?S-4,直至到达目的点，得到初始路径；
[0013] S-6对初始路径进行优化，减少路径长度，得到最终路径。
[0014] 所述的步骤S-I中，斥力势场建立步骤如下：
[0015] S-1-1计算机器人当前位置到各障碍物的距离，该步骤按单一障碍物和形状复杂 的障碍物两种情况处理：
[0016] S-1-1-1单一障碍物：计算机器人当前距离到该障碍物各顶点的距离和各边的垂 直距离，得到距离集合K = {/^，/4，··_，/4，/4/4，···，/4}，其中m，k为该障碍物顶点数和 变数，则机器人到该障碍物的距离为忒=min?
[0017] S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离 集合K ={ρΚ，···，/0，则机器人到该障碍物的距离为汶;
[0018] S-1-2计算当前工作空间场景，W= !/| 〇SM"!，其中Pi为机器人探测半 径，η为障碍物数；
[0019] S-1-3根据W和式（6)计算当前场景下的势场分布。
[0020] 所述的步骤S-2中，在步骤S-I中建立的机器人局部工作空间斥力势�。思跎� 计算量，工作空间斥力势场采用动态方法建立，在检测到工作空间场景变化后更新工作空 间斥力�。豢占涑饬κ瞥“ㄒ韵虏街�：
[0021] S-2-1计算机器人当前位置到各障碍物的距离；
[0022] S-2-2选取距离小于等于机器人感知距离的障碍物构成当前工作空间；
[0023] S-2-3与之前的工作空间场景比较，如果有变化则更新工作空间场景；
[0024] 建立机器人局部工作空间斥力势场采用Cauchy分布来建立工作空间势�。玫� 的斥力势场能较为精确反映障碍物轮廓，从而可以获得较精确的路径，具体是：
[0025] 采用Cauchy分布的累积分布函数来建立斥力势�。浔泶锸饺缡剑�1)所示；
[0026]
【权利要求】
1. 一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：该方法是在机器人 工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任 意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤： S-I根据先验地图，建立机器人局部工作空间斥力势�。� S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程； S-3求解该路径方程，得到局部路径； S-4如果检测到算法陷入局部最优，则处理局部最优问题； S-5重复S-I?S-4,直至到达目的点，得到初始路径； S-6对初始路径进行优化，减少路径长度，得到最终路径。
2. 根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征 是：所述的步骤S-I中，斥力势场建立步骤如下： s-1-ι计算机器人当前位置到各障碍物的距离，该步骤按单一障碍物和形状复杂的障 碍物两种情况处理： S-1-1-1单一障碍物：计算机器人当前距离到该障碍物各顶点的距离和各边的垂直距 离，得到距离集合/^={/^,/4,.",/4，/^,/4,".,/4}，其中111，1^为该障碍物顶点数和变数， 则机器人到该障碍物的距离为次=; S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离集合 A: =W1W2,...,/0 ,则机器人到该障碍物的距离为汶=minW); S-1-2计算当前工作空间场景，fF = {/1 ^ < A,0 U < π}，其中ρ ^为机器人探测半径， η为障碍物数； S-1-3根据W和式（6)计算当前场景下的势场分布。
3. 根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征 是：所述的步骤S-2中，在步骤S-I中建立的机器人局部工作空间斥力势�。思跎偌扑� 量，工作空间斥力势场采用动态方法建立，在检测到工作空间场景变化后更新工作空间斥 力�。豢占涑饬κ瞥“ㄒ韵虏街瑁� S-2-1计算机器人当前位置到各障碍物的距离； S-2-2选取距离小于等于机器人感知距离的障碍物构成当前工作空间； S-2-3与之前的工作空间场景比较，如果有变化则更新工作空间场景； 建立机器人局部工作空间斥力势场采用Cauchy分布来建立工作空间势�。玫降某� 力势场能较为精确反映障碍物轮廓，从而可以获得较精确的路径，具体是： 采用Cauchy分布的累积分布函数来建立斥力势�。浔泶锸饺缡剑�1)所示；
(1) 其中.
为柯西分布的累积分布函数，（IiUy)为工作空间内各点 到边i的距离，η为组成多边形的边数； 对于障碍物的每一条边由Pi和Pi+1两个点确定，则边可用式（2)表示 Δ y · χ- Δ X · y+c = O (2) 其中
：要保证前面所述的环境，则要保证在编的内法线侧的点 到边的距离为正；因此Cli (x，y)可用式（3)表示
(3) 其中α =〇，1;由前面所述的环境的障碍物轮廓由若干条边顺时针形成，因此内法线 方向为
(4) 在Pi+1处沿N方向取一点pt带入式（3)，按式（5)来判断α的值
(5) 利用式（1)?（5)可以建立单障碍物斥力势场，对于多障碍物斥力势�。蚩捎孟率浇�
(6) 对于形状复杂障碍物如凹障碍物，可由多个单障碍组合得到，因此式（6)也适合用来 建立复杂形状的障碍物斥力势�。� 井下救援，工作空间多为狭小的巷道，因此对式（6)做改进下改进使之适用于巷道空 间 Ut (x, y) = -Uffs (x, y) (7)。
4. 根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征 是：所述的步骤S-3中，路径偏微分方程建立包含以下流程： S-3-1如果工作空间场景更新，则在本场景中将开始点变更为前一场景中路径的最后 一个点，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程； S-3-2如果工作空间场景未更新，则开始点不变，根据开始点和目标点利用 Quasi-Geodesic法建立路径偏微分方程。
5. 根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征 是：所述的步骤S-4中，处理局部最优问题，采用了一种基于梯度信息的双向路径探测方法 克服局部最优的缺点，包括以下步骤： S-4-1检测到方法陷入局部最优后，根据机器人到当前工作空间各障碍物距离，判断发 生局部最优的障碍物；当满足如下两式中的任意一个时，可以判断方法陷入局部最优，
其中var(X(l，：））为当前路径上所有点X坐标的方差，var(X(l，（i-10) :i)))为最新 的10个点x坐标的方差，cx为阈值，通过实验本文取3X10_6。y方向与x方向类似； S-4-2从当前点沿等势线顺时针和逆时针方向建立试探路径，当其中一条试探路径上 的点与目标点的连线与发生局部最优的障碍物不相交时，选取该方向作为跳出局部最优路 径加入到整体路径中，结束跳出局部最优步骤； 对于每一个障碍物如果满足Pi^ Pth，则该障碍物为造成局部最优问题的障碍物，力口 入问题障碍物表中； S-4-3按下式更新探测路径Ctestl和Ctest2 xi+1 - Xi+r2Dsin θ yi+i = yi+r2Dcos Θ
机器人顺时针绕障碍物运动时r2D = -h反之;r2D = h ; SHpcmratl和Pcmrant2为探测路径当前点，当PcmratlD与问题障碍物不相交时，将C testl 加入到路径中，反之将Ctest2加入到路径中； 当检测到发生局部最优后，从当前位置沿式（8)和（9)所示的Dtl和Dt2两个方向进行 探测，当任一探测路径上的点与目的点之间的连线不与当前障碍物相交，则将该探测路径 加入到路径中；
(8) (9)
6.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征 在于：所述的步骤S-6中，对初始路径进行优化，步骤如下： S-6-1置路径优化阶段初值Ctl为前述路径偏微分方程的得到的路径； S-6-2如果没有到达最大循环次数，则按式（11)更新路径； S-6-3如果达到最大循环次数，则得到最终路径值； 采用活动轮廓法来对路径进行优化，从而减少路径长度，用（10)来描述：
(10) 其中C为路径，根据式（10)，可以得到路径的迭代方程如下： cn+1 = Cn-Cn" +VU(Cn) (11) 利用式（11)对路径优化。
【文档编号】G01C21/20GK104390648SQ201410809844
【公开日】2015年3月4日 申请日期:2014年12月22日 优先权日:2014年12月22日
【发明者】张晓光, 李辉, 刘小平, 姜玉叶, 赵志科, 刘超, 于立波, 户硕, 张春梅 申请人:中国矿业大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
