CN104390648A - 一种基于人工势场的煤矿救灾机器人路径规划方法 - Google Patents

Abstract

一种基于人工势场的煤矿救灾机器人路径规划方法，属于机器人路径规划方法。该方法是在机器人工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤：S-1根据先验地图，建立机器人局部工作空间斥力势场；S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；S-3求解该路径方程，得到局部路径；S-4如果检测到算法陷入局部最优，则处理局部最优问题；S-5重复S-1～S-4，直至到达目的点，得到初始路径；S-6对初始路径进行优化，减少路径长度，得到最终路径。该方法能得到较为精确和较短的路径，同时不需要耗费太多的运行时间，提高了救援时间和救援效率。

Description

一种基于人工势场的煤矿救灾机器人路径规划方法

技术领域

本发明涉及一种机器人路径规划方法，特别是一种基于人工势场的煤矿救灾机器人路径规划方法。

背景技术

目前，我国煤矿事故频发，不仅造成了生命和财产的重大损失，而且还影响社会的安定。搜救遇险人员是发生矿难以后的重要工作，传统的搜救设备不能有效地实现受困人员救助，而且二次事故常常会导致救援队员的伤亡。

煤矿井下救灾机器人作为一种智能探测和搜救设备可以获取和发送灾变区域参数和生还者信息，可以替代人从事搜索和救援工作，从而降低了搜救人员的风险。

近些年来，已有的一些救灾机器人大多是采用遥控方式使机器人行进和避障，自主性比较差，在实际应用中还有一定的局限性。受井下恶劣环境的限制，许多地面机器人的自主导航技术不能在井下应用，例如GPS导航无法应用于矿井；无线电导航虽然精度很高，但煤层对电波有较大吸收，因此不适合应用于井下；惯性导航需要安装大量的辅助标示物，一旦发生矿难会损坏大量的标示物影响灾后导航；激光和超声等方式抗干扰较差。

路径规划技术是视觉导航中的重要组成部分，人工势场法是一种简单有效的路径规划方法，应用极其广泛。在传统的人工势场法中，势场不能有效反映障碍物的轮廓，因此得不到精确的路径，而为了获得精确的势场分布，又要耗费大量的运算时间。

发明内容

本发明的目的是要提供一种既能精确反映障碍物轮廓又能减少运算时间的基于人工势场的煤矿救灾机器人路径规划方法，克服传统人工势场的缺点。

本发明的目的是这样实现的：该方法是在机器人工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤：

S-1根据先验地图，建立机器人局部工作空间斥力势场；

S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；

S-3求解该路径方程，得到局部路径；

S-4如果检测到算法陷入局部最优，则处理局部最优问题；

S-5重复S-1～S-4，直至到达目的点，得到初始路径；

S-6对初始路径进行优化，减少路径长度，得到最终路径。

所述的步骤S-1中，斥力势场建立步骤如下：

S-1-1计算机器人当前位置到各障碍物的距离，该步骤按单一障碍物和形状复杂的障碍物两种情况处理：

S-1-1-1单一障碍物：计算机器人当前距离到该障碍物各顶点的距离和各边的垂直距离，得到距离集合其中m，k为该障碍物顶点数和变数，则机器人到该障碍物的距离为

S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离集合 ${\mathrm{\ρ}}_s^i=\{{\mathrm{\ρ}}_{o1}^i,{\mathrm{\ρ}}_{o2}^i,...,{\mathrm{\ρ}}_{\mathrm{om}}^i\},$ 则机器人到该障碍物的距离为 ${\mathrm{\ρ}}_o^i=\min \left({\mathrm{\ρ}}_s^i\right);$

S-1-2计算当前工作空间场景，其中ρ_r为机器人探测半径，n为障碍物数；

S-1-3根据W和式(6)计算当前场景下的势场分布。

所述的步骤S-2中，在步骤S-1中建立的机器人局部工作空间斥力势场，为了减少计算量，工作空间斥力势场采用动态方法建立，在检测到工作空间场景变化后更新工作空间斥力场；空间斥力势场包括以下步骤：

S-2-1计算机器人当前位置到各障碍物的距离；

S-2-2选取距离小于等于机器人感知距离的障碍物构成当前工作空间；

S-2-3与之前的工作空间场景比较，如果有变化则更新工作空间场景；

建立机器人局部工作空间斥力势场采用Cauchy分布来建立工作空间势场，得到的斥力势场能较为精确反映障碍物轮廓，从而可以获得较精确的路径，具体是：

采用Cauchy分布的累积分布函数来建立斥力势场，其表达式如式(1)所示；

$U(x,y)=\underset{i=1}{\overset{n}{\mathrm{\Π}}}f\left(d_i(x,y)\right)---\left(1\right)$

其中为柯西分布的累积分布函数，d_i(x,y)为工作空间内各点到边i的距离，n为组成多边形的边数；

对于障碍物的每一条边由p_i和p_i+1两个点确定，则边可用式(2)表示

Δy·x-Δx·y+c＝0         (2)

其中 $\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]=\left[\begin{array}{c}{p}_{i+1}\left(x\right)-p_i\left(x\right)\\ p_{i+1}\left(y\right)-p_i\left(y\right)\end{array}\right];$ 要保证前面所述的环境，则要保证在编的内法线侧的点到边的距离为正；因此d_i(x,y)可用式(3)表示

$d(x,y)={(-1)}^{\mathrm{\α}}\frac{\mathrm{\Δy}\·x-\mathrm{\Δx}\·y+c}{\sqrt{{\mathrm{\Δy}}^2+{\mathrm{\Δx}}^2}}---\left(3\right)$

其中α＝0,1；由前面所述的环境的障碍物轮廓由若干条边顺时针形成，因此内法线方向为

$N=\frac{\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]}{\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2}}---\left(4\right)$

在p_i+1处沿N方向取一点p_t带入式(3)，按式(5)来判断α的值

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}0& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c>0\\ 1& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c<0\end{array}\right.---\left(5\right)$

利用式(1)～(5)可以建立单障碍物斥力势场，对于多障碍物斥力势场，则可用下式建立

$U_{\mathrm{ws}}(x,y)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{U}_s(x,y)---\left(6\right)$

对于形状复杂障碍物如凹障碍物，可由多个单障碍组合得到，因此式(6)也适合用来建立复杂形状的障碍物斥力势场；

由于井下下救援，工作空间多为狭小的巷道，因此对式(6)做改进下改进使之适用于巷道空间

U_T(x,y)＝-U_WS(x,y)            (7)；

所述的步骤S-3中，路径偏微分方程建立包含以下流程：

S-3-1如果工作空间场景更新，则在本场景中将开始点变更为前一场景中路径的最后一个点，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程；

S-3-2如果工作空间场景未更新，则开始点不变，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程。

所述的步骤S-4中，处理局部最优问题，采用了一种基于梯度信息的双向路径探测方法克服局部最优的缺点，包括以下步骤：

S-4-1检测到方法陷入局部最优后，根据机器人到当前工作空间各障碍物距离，判断发生局部最优的障碍物；当满足如下两式中的任意一个时，可以判断方法陷入局部最优，

$\frac{\mathrm{var}\left(X(1,(i-10):i)\right)}{\mathrm{var}\left(X(1,:)\right)}<c_x$

$\frac{\mathrm{var}\left(X(2,(i-10):i)\right)}{\mathrm{var}\left(X(2,:)\right)}<c_y$

其中var(X(1,:))为当前路径上所有点x坐标的方差，var(X(1，(i-10):i)))为最新的10个点x坐标的方差，c_x为阈值，通过实验本文取3×10^-6。y方向与x方向类似；

S-4-2从当前点沿等势线顺时针和逆时针方向建立试探路径，当其中一条试探路径上的点与目标点的连线与发生局部最优的障碍物不相交时，选取该方向作为跳出局部最优路径加入到整体路径中，结束跳出局部最优步骤；

对于每一个障碍物如果满足ρ_i≤ρ_th，则该障碍物为造成局部最优问题的障碍物，加入问题障碍物表中；

S-4-3按下式更新探测路径C_test1和C_test2

x_i+1＝x_i+r_2D sinθ

y_i+1＝y_i+r_2D cosθ

其中 $\begin{array}{c}\sin \mathrm{\&theta;}=\frac{\frac{\&PartialD;U}{\&PartialD;y}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\\ \cos \mathrm{\&theta;}=\frac{\frac{\&PartialD;Q}{\&PartialD;x}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\end{array}$

机器人顺时针绕障碍物运动时r_2D＝-h反之r_2D＝h；

S-4-4p_current1和p_current2为探测路径当前点，当p_current1与问题障碍物不相交时，将C_test1加入到路径中，反之将C_test2加入到路径中；

当检测到发生局部最优后，从当前位置沿式(8)和(9)所示的D_t1和D_t2两个方向进行探测，当任一探测路径上的点与目的点之间的连线不与当前障碍物相交，则将该探测路径加入到路径中；

$D_{t1}=\left[\begin{array}{c}\frac{\&PartialD;U}{\&PartialD;y}\\ -\frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(8\right)$

$D_{t2}=\left[\begin{array}{c}-\frac{\&PartialD;U}{\&PartialD;y}\\ \frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(9\right);$

所述的步骤S-5中，重复S-1～S-4，直至到达目的点，得到初始路径；

所述的步骤S-6中，对初始路径进行优化，步骤如下：

S-6-1置路径优化阶段初值C₀为前述路径偏微分方程的得到的路径；

S-6-2如果没有到达最大循环次数，则按式(11)更新路径；

S-6-3如果达到最大循环次数，则得到最终路径值；

采用活动轮廓法来对路径进行优化，从而减少路径长度，可用公式(10)来描述：

$\frac{\&PartialD;C}{\&PartialD;t}=-{\left(C^{\&prime;}\right)}^{\&prime;}+\&dtri;U\left(C\right)---\left(10\right)$

其中C为路径，根据式(10)，可以得到路径的迭代方程如下：

C_n+1＝C_n-C_n″+▽U(C_n)         (11)

利用式(11)对路径优化。

有益效果，由于采用了上述方案，解决了传统势场不能有效反映障碍物的轮廓，得不到精确的路径，而为了获得精确的势场分布，又要耗费大量的运算时间的问题。为了解决这一问题，该方法能得到较为精确和较短的路径，同时不需要耗费太多的运行时间，提高了救援时间和救援效率。优点是具有抗干扰强、导航精确、观察范围大的导航技术应用在煤矿救灾领域，视觉导航非常适合应用于井下救灾机器人导航。

附图说明：

图1是本发明的工作流程图。

图2是障碍物构成示意图。

图3是斥力势场建立流程图。

图4是双向试探法流程图。

图5是路径优化流程图。

图6是一个巷道场景中得到的初始路径。

图7是优化后的最终路径。

具体实施方式

该方法是在机器人工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤：

S-1根据先验地图，建立机器人局部工作空间斥力势场；

S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；

S-3求解该路径方程，得到局部路径；

S-4如果检测到算法陷入局部最优，则处理局部最优问题；

S-5重复S-1～S-4，直至到达目的点，得到初始路径；

S-6对初始路径进行优化，减少路径长度，得到最终路径。

所述的步骤S-1中，斥力势场建立步骤如下：

S-1-1计算机器人当前位置到各障碍物的距离，该步骤按单一障碍物和形状复杂的障碍物两种情况处理：

S-1-1-1单一障碍物：计算机器人当前距离到该障碍物各顶点的距离和各边的垂直距离，得到距离集合其中m，k为该障碍物顶点数和变数，则机器人到该障碍物的距离为

S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离集合 ${\mathrm{\&rho;}}_s^i=\{{\mathrm{\&rho;}}_{o1}^i,{\mathrm{\&rho;}}_{o2}^i,...,{\mathrm{\&rho;}}_{\mathrm{om}}^i\},$ 则机器人到该障碍物的距离为 ${\mathrm{\&rho;}}_o^i=\min \left({\mathrm{\&rho;}}_s^i\right);$

S-1-2计算当前工作空间场景，其中ρ_r为机器人探测半径，n为障碍物数；

S-1-3根据W和式(6)计算当前场景下的势场分布。

所述的步骤S-2中，在步骤S-1中建立的机器人局部工作空间斥力势场，为了减少计算量，工作空间斥力势场采用动态方法建立，在检测到工作空间场景变化后更新工作空间斥力场；空间斥力势场包括以下步骤：

S-2-1计算机器人当前位置到各障碍物的距离；

S-2-2选取距离小于等于机器人感知距离的障碍物构成当前工作空间；

S-2-3与之前的工作空间场景比较，如果有变化则更新工作空间场景；

建立机器人局部工作空间斥力势场采用Cauchy分布来建立工作空间势场，得到的斥力势场能较为精确反映障碍物轮廓，从而可以获得较精确的路径，具体是：

采用Cauchy分布的累积分布函数来建立斥力势场，其表达式如式(1)所示；

$U(x,y)=\underset{i=1}{\overset{n}{\mathrm{\&Pi;}}}f\left(d_i(x,y)\right)---\left(1\right)$

其中为柯西分布的累积分布函数，d_i(x,y)为工作空间内各点到边i的距离，n为组成多边形的边数；

对于障碍物的每一条边由p_i和p_i+1两个点确定，则边可用式(2)表示

Δy·x-Δx·y+c＝0          (2)

其中 $\left[\begin{array}{c}\mathrm{\&Delta;x}\\ \mathrm{\&Delta;y}\end{array}\right]=\left[\begin{array}{c}{p}_{i+1}\left(x\right)-p_i\left(x\right)\\ p_{i+1}\left(y\right)-p_i\left(y\right)\end{array}\right];$ 要保证前面所述的环境，则要保证在编的内法线侧的点到边的距离为正；因此d_i(x,y)可用式(3)表示

$d(x,y)={(-1)}^{\mathrm{\&alpha;}}\frac{\mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c}{\sqrt{{\mathrm{\&Delta;y}}^2+{\mathrm{\&Delta;x}}^2}}---\left(3\right)$

其中α＝0,1；由前面所述的环境的障碍物轮廓由若干条边顺时针形成，因此内法线方向为

$N=\frac{\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;x}\\ \mathrm{\&Delta;y}\end{array}\right]}{\sqrt{{\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;y}}^2}}---\left(4\right)$

在p_i+1处沿N方向取一点p_t带入式(3)，按式(5)来判断α的值

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}0& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c>0\\ 1& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c<0\end{array}\right.---\left(5\right)$

利用式(1)～(5)可以建立单障碍物斥力势场，对于多障碍物斥力势场，则可用下式建立

$U_{\mathrm{ws}}(x,y)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{U}_s(x,y)---\left(6\right)$

对于形状复杂障碍物如凹障碍物，可由多个单障碍组合得到，因此式(6)也适合用来建立复杂形状的障碍物斥力势场；

由于井下救援，工作空间多为狭小的巷道，因此对式(6)做改进下改进使之适用于巷道空间

U_T(x,y)＝-U_WS(x,y)             (7)；

所述的步骤S-3中，路径偏微分方程建立包含以下流程：

S-3-1如果工作空间场景更新，则在本场景中将开始点变更为前一场景中路径的最后一个点，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程；

S-3-2如果工作空间场景未更新，则开始点不变，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程。

所述的步骤S-4中，处理局部最优问题，采用了一种基于梯度信息的双向路径探测方法克服局部最优的缺点，包括以下步骤：

S-4-1检测到方法陷入局部最优后，根据机器人到当前工作空间各障碍物距离，判断发生局部最优的障碍物；当满足如下两式中的任意一个时，可以判断方法陷入局部最优，

$\frac{\mathrm{var}\left(X(1,(i-10):i)\right)}{\mathrm{var}\left(X(1,:)\right)}<c_x$

$\frac{\mathrm{var}\left(X(2,(i-10):i)\right)}{\mathrm{var}\left(X(2,:)\right)}<c_y$

其中var(X(1,:))为当前路径上所有点x坐标的方差，var(X(1，(i-10):i)))为最新的10个点x坐标的方差，c_x为阈值，通过实验本文取3×10^-6。y方向与x方向类似；

S-4-2从当前点沿等势线顺时针和逆时针方向建立试探路径，当其中一条试探路径上的点与目标点的连线与发生局部最优的障碍物不相交时，选取该方向作为跳出局部最优路径加入到整体路径中，结束跳出局部最优步骤；

对于每一个障碍物如果满足ρ_i≤ρ_th，则该障碍物为造成局部最优问题的障碍物，加入问题障碍物表中；

S-4-3按下式更新探测路径C_test1和C_test2

x_i+1＝x_i+r_2D sinθ

y_i+1＝y_i+r_2D cosθ

其中 $\begin{array}{c}\sin \mathrm{\&theta;}=\frac{\frac{\&PartialD;U}{\&PartialD;y}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\\ \cos \mathrm{\&theta;}=\frac{\frac{\&PartialD;Q}{\&PartialD;x}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\end{array}$

机器人顺时针绕障碍物运动时r_2D＝-h反之r_2D＝h；

S-4-4p_current1和p_current2为探测路径当前点，当p_current1与问题障碍物不相交时，将C_test1加入到路径中，反之将C_test2加入到路径中；

当检测到发生局部最优后，从当前位置沿式(8)和(9)所示的D_t1和D_t2两个方向进行探测，当任一探测路径上的点与目的点之间的连线不与当前障碍物相交，则将该探测路径加入到路径中；

$D_{t1}=\left[\begin{array}{c}\frac{\&PartialD;U}{\&PartialD;y}\\ -\frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(8\right)$

$D_{t2}=\left[\begin{array}{c}-\frac{\&PartialD;U}{\&PartialD;y}\\ \frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(9\right);$

所述的步骤S-5中，重复S-1～S-4，直至到达目的点，得到初始路径；

所述的步骤S-6中，对初始路径进行优化，步骤如下：

S-6-1置路径优化阶段初值C₀为前述路径偏微分方程的得到的路径；

S-6-2如果没有到达最大循环次数，则按式(11)更新路径；

S-6-3如果达到最大循环次数，则得到最终路径值；

采用活动轮廓法来对路径进行优化，从而减少路径长度，可用公式(10)来描述：

$\frac{\&PartialD;C}{\&PartialD;t}=-{\left(C^{\&prime;}\right)}^{\&prime;}+\&dtri;U\left(C\right)---\left(10\right)$

其中C为路径，根据式(10)，可以得到路径的迭代方程如下：

C_n+1＝C_n-C_n″+▽U(C_n)           (11)

利用式(11)对路径优化。

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图进一步详细说明本发明的实施方案。

实施例1：图1为本发明工作流程图。

S-1根据先验地图，建立机器人局部工作空间斥力势场；

S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；

S-3求解该路径方程，得到局部路径；

S-4如果检测到算法陷入局部最优，则处理局部最优问题；

S-5重复S-1～S-4，直至到达目的点，得到初始路径；

S-6对初始路径进行优化，减少路径长度，得到最终路径。

图3为本发明斥力势场建立流程图。

S-1-1计算机器人当前位置到各障碍物的距离，该步骤可按两种情况处理：

S-1-1-1单一障碍物。计算机器人当前距离到该障碍物各顶点的距离和各边的垂直距离，得到距离集合其中m，k为该障碍物顶点数和变数，则机器人到该障碍物的距离为

S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离集合 ${\mathrm{\&rho;}}_s^i=\{{\mathrm{\&rho;}}_{o1}^i,{\mathrm{\&rho;}}_{o2}^i,...,{\mathrm{\&rho;}}_{\mathrm{om}}^i\},$ 则机器人到该障碍物的距离为 ${\mathrm{\&rho;}}_o^i=\min \left({\mathrm{\&rho;}}_s^i\right).$

S-1-2计算当前工作空间场景，其中ρ_r为机器人探测半径，n为障碍物数。

S-1-3根据W和式(6)计算当前场景下的势场分布。

本发明中路径偏微分方程建立包含以下流程：

S-3-1如果工作空间场景更新，则在本场景中将开始点变更为前一场景中路径的最后一个点，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程；

S-3-2如果工作空间场景未更新，则开始点不变，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程。

图4为本发明中双向试探法流程图。

S-4-1当满足如下两式中的任意一个时，可以判断方法陷入局部最优，

$\frac{\mathrm{var}\left(X(1,(i-10):i)\right)}{\mathrm{var}\left(X(1,:)\right)}<c_x$

$\frac{\mathrm{var}\left(X(2,(i-10):i)\right)}{\mathrm{var}\left(X(2,:)\right)}<c_y$

其中var(X(1,:))为当前路径上所有点x坐标的方差，var(X(1，(i-10):i)))为最新的10个点x坐标的方差，c_x为阈值，通过实验本文取3×10^-6。y方向与x方向类似。

S-4-2对于每一个障碍物如果满足ρ_i≤ρ_th，则该障碍物为造成局部最优问题的障碍物，加入问题障碍物表中。

S-4-3按下式更新探测路径C_test1和C_test2

x_i+1＝x_i+r_2D sinθ

y_i+1＝y_i+r_2D cosθ

其中 $\begin{array}{c}\sin \mathrm{\&theta;}=\frac{\frac{\&PartialD;U}{\&PartialD;y}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\\ \cos \mathrm{\&theta;}=\frac{\frac{\&PartialD;Q}{\&PartialD;x}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}\end{array}$

机器人顺时针绕障碍物运动时r_2D＝-h反之r_2D＝h。

S-4-4p_current1和p_current2为探测路径当前点，当p_current1与问题障碍物不相交时，将C_test1加入到路径中，反之将C_test2加入到路径中。

图5为路径优化流程图。

S-6-1置路径优化阶段初值C₀为前述路径偏微分方程的得到的路径；

S-6-2如果没有到达最大循环次数，则按式(11)更新路径；

S-6-3如果达到最大循环次数，则得到最终路径值。

图6为执行优化前得到的路径，图7为执行优化后得到的路径。

综上，通过该方法煤矿救灾机器人能得到较为精确和较短的路径，同时不需要耗费太多的运行时间，提高了救援时间和救援效率。

Claims (6)

1.一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：该方法是在机器人工作空间已知、工作空间障碍物用多个线段围成的封闭区域表示、工作空间是联通的即任意两点间都存在可行路径的环境下进行的，所述的路径规划方法包括以下步骤：

S-1根据先验地图，建立机器人局部工作空间斥力势场；

S-2根据斥力势场的分布，利用Quasi-Geodesic方法建立路径偏微分方程；

S-3求解该路径方程，得到局部路径；

S-4如果检测到算法陷入局部最优，则处理局部最优问题；

S-5重复S-1～S-4，直至到达目的点，得到初始路径；

S-6对初始路径进行优化，减少路径长度，得到最终路径。

2.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：所述的步骤S-1中，斥力势场建立步骤如下：

S-1-1计算机器人当前位置到各障碍物的距离，该步骤按单一障碍物和形状复杂的障碍物两种情况处理：

S-1-1-1单一障碍物：计算机器人当前距离到该障碍物各顶点的距离和各边的垂直距离，得到距离集合其中m，k为该障碍物顶点数和变数，则机器人到该障碍物的距离为

S-1-1-2形状复杂的障碍物。计算机器人到该障碍物各组成部分的距离，得到距离集合 ${\mathrm{\&rho;}}_s^i=\{{\mathrm{\&rho;}}_{o1}^i,{\mathrm{\&rho;}}_{o2}^i,...,{\mathrm{\&rho;}}_{\mathrm{om}}^i\},$ 则机器人到该障碍物的距离为 ${\mathrm{\&rho;}}_o^i=\min \left({\mathrm{\&rho;}}_s^i\right);$

S-1-2计算当前工作空间场景，其中ρ_r为机器人探测半径，n为障碍物数；

S-1-3根据W和式(6)计算当前场景下的势场分布。

3.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：所述的步骤S-2中，在步骤S-1中建立的机器人局部工作空间斥力势场，为了减少计算量，工作空间斥力势场采用动态方法建立，在检测到工作空间场景变化后更新工作空间斥力场；空间斥力势场包括以下步骤：

S-2-1计算机器人当前位置到各障碍物的距离；

S-2-2选取距离小于等于机器人感知距离的障碍物构成当前工作空间；

S-2-3与之前的工作空间场景比较，如果有变化则更新工作空间场景；

建立机器人局部工作空间斥力势场采用Cauchy分布来建立工作空间势场，得到的斥力势场能较为精确反映障碍物轮廓，从而可以获得较精确的路径，具体是：

采用Cauchy分布的累积分布函数来建立斥力势场，其表达式如式(1)所示；

$U(x,y)=\underset{i=1}{\overset{n}{\mathrm{\&Pi;}}}f\left(d_i(x,y)\right)---\left(1\right)$

其中为柯西分布的累积分布函数，d_i(x,y)为工作空间内各点到边i的距离，n为组成多边形的边数；

对于障碍物的每一条边由p_i和p_i+1两个点确定，则边可用式(2)表示

Δy·x-Δx·y+c＝0             (2)

其中 $\left[\begin{array}{c}\mathrm{\&Delta;x}\\ \mathrm{\&Delta;y}\end{array}\right]=\left[\begin{array}{c}{p}_{i+1}\left(x\right)-p_i\left(x\right)\\ p_{i+1}\left(y\right)-p_i\left(y\right)\end{array}\right];$ 要保证前面所述的环境，则要保证在编的内法线侧的点到边的距离为正；因此d_i(x,y)可用式(3)表示

$d(x,y)={(-1)}^{\mathrm{\&alpha;}}\frac{\mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c}{\sqrt{\mathrm{\&Delta;}{y}^2+\mathrm{\&Delta;}{x}^2}}---\left(3\right)$

其中α＝0,1；由前面所述的环境的障碍物轮廓由若干条边顺时针形成，因此内法线方向为

$N=\frac{\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\&Delta;x}\\ \mathrm{\&Delta;y}\end{array}\right]}{\sqrt{{\mathrm{\&Delta;x}}^2+{\mathrm{\&Delta;y}}^2}}---\left(4\right)$

在p_i+1处沿N方向取一点p_t带入式(3)，按式(5)来判断α的值

$\mathrm{\&alpha;}=\left\{\begin{array}{cc}0& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c>0\\ 1& \mathrm{\&Delta;y}\&CenterDot;x-\mathrm{\&Delta;x}\&CenterDot;y+c<0\end{array}\right.---\left(5\right)$

利用式(1)～(5)可以建立单障碍物斥力势场，对于多障碍物斥力势场，则可用下式建立

$U_{\mathrm{ws}}(x,y)=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{U}_s(x,y)---\left(6\right)$

对于形状复杂障碍物如凹障碍物，可由多个单障碍组合得到，因此式(6)也适合用来建立复杂形状的障碍物斥力势场；

井下救援，工作空间多为狭小的巷道，因此对式(6)做改进下改进使之适用于巷道空间

U_T(x,y)＝-U_WS(x,y)            (7)。

4.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：所述的步骤S-3中，路径偏微分方程建立包含以下流程：

S-3-1如果工作空间场景更新，则在本场景中将开始点变更为前一场景中路径的最后一个点，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程；

S-3-2如果工作空间场景未更新，则开始点不变，根据开始点和目标点利用Quasi-Geodesic法建立路径偏微分方程。

5.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征是：所述的步骤S-4中，处理局部最优问题，采用了一种基于梯度信息的双向路径探测方法克服局部最优的缺点，包括以下步骤：

S-4-1检测到方法陷入局部最优后，根据机器人到当前工作空间各障碍物距离，判断发生局部最优的障碍物；当满足如下两式中的任意一个时，可以判断方法陷入局部最优，

$\frac{\mathrm{var}\left(X(1,(i-10):i)\right)}{\mathrm{var}\left(X(1,:)\right)}<c_x$

$\frac{\mathrm{var}\left(X(2,(i-10):i)\right)}{\mathrm{var}\left(X(2,:)\right)}<c_y$

其中var(X(1,:))为当前路径上所有点x坐标的方差，var(X(1，(i-10)：i)))为最新的10个点x坐标的方差，c_x为阈值，通过实验本文取3×10^-6。y方向与x方向类似；

S-4-2从当前点沿等势线顺时针和逆时针方向建立试探路径，当其中一条试探路径上的点与目标点的连线与发生局部最优的障碍物不相交时，选取该方向作为跳出局部最优路径加入到整体路径中，结束跳出局部最优步骤；

对于每一个障碍物如果满足ρ_i≤ρ_th，则该障碍物为造成局部最优问题的障碍物，加入问题障碍物表中；

S-4-3按下式更新探测路径C_test1和C_test2

x_i+1＝x_i+r_2Dsinθ

y_i+1＝y_i+r_2Dcosθ

其中 $\sin \mathrm{\&theta;}=\frac{\frac{\&PartialD;U}{\&PartialD;y}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}$

$\cos \mathrm{\&theta;}=\frac{\frac{\&PartialD;Q}{\&PartialD;x}}{\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}}$

机器人顺时针绕障碍物运动时r_2D＝-h反之r_2D＝h；

S-4-4p_current1和p_current2为探测路径当前点，当p_current1D与问题障碍物不相交时，将C_test1加入到路径中，反之将C_test2加入到路径中；

当检测到发生局部最优后，从当前位置沿式(8)和(9)所示的D_t1和D_t2两个方向进行探测，当任一探测路径上的点与目的点之间的连线不与当前障碍物相交，则将该探测路径加入到路径中；

$D_{t1}=\left[\begin{array}{c}\frac{\&PartialD;U}{\&PartialD;y}\\ -\frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(8\right)$

$D_{t2}=\left[\begin{array}{c}-\frac{\&PartialD;U}{\&PartialD;y}\\ \frac{\&PartialD;U}{\&PartialD;x}\end{array}\right]/\sqrt{{\left(\frac{\&PartialD;U}{\&PartialD;y}\right)}^2+{\left(\frac{\&PartialD;U}{\&PartialD;x}\right)}^2}---\left(9\right).$

6.根据权利要求1所述的一种基于人工势场的煤矿救灾机器人路径规划方法，其特征在于：所述的步骤S-6中，对初始路径进行优化，步骤如下：

S-6-1置路径优化阶段初值C₀为前述路径偏微分方程的得到的路径；

S-6-2如果没有到达最大循环次数，则按式(11)更新路径；

S-6-3如果达到最大循环次数，则得到最终路径值；

采用活动轮廓法来对路径进行优化，从而减少路径长度，用(10)来描述：

$\frac{\&PartialD;C}{\&PartialD;t}=-{\left(C^{\&prime;}\right)}^{\&prime;}+\&dtri;U\left(C\right)---\left(10\right)$

其中C为路径，根据式(10)，可以得到路径的迭代方程如下：

C_n+1＝C_n-C_n″+▽U(C_n)            (11)

利用式(11)对路径优化。