CN106017497A - 基于地图定位能力的路径规划方法 - Google Patents

Abstract

本发明提供了一种基于地图定位能力的路径规划方法，通过将地图定位能力加入路径节点代价函数，以达到优化规划路径的目的。本发明的规划方法包括以下部分：首先计算概率栅格地图各点的定位能力，其次将概率栅格地图各点加入路径节点代价函数，最后通过路径规划算法进行搜索，计算出从初始点至目标点的移动路径。应用本发明所提出的路径规划方法具有提高移动单位在环境中的定位能力、避免移动单位定位失败等特点，适用于民用、工业、军事等各个领域。

Description

基于地图定位能力的路径规划方法

技术领域

本发明涉及路径规划技术领域，具体地，涉及一种基于地图定位能力的路径规划方法。

背景技术

在机器人领域中，移动机器人所处的环境情况是相对复杂的。移动机器人的基本需求之一为在定位导航过程中规划出一条避开所有障碍物的路径。在环境各点中由于可观测信息的不同，导致机器人在不同点有不同的定位能力(或称定位能力)。在进行路径规划时须考虑该问题，沿着定位能力相对较强的区域进行规划，并避免因为路径节点的定位能力过低而引起机器人定位失败甚至丢失的情况。

Kaelbling[1]等人提出了一种通过使定位过程中的期望熵值最低来提高定位预测信息的准确性的方法，但这种方法在规划路径时存在局部最小点的问题。Fox[2]等人提出了一种主动选择高定位精度的路径的策略，他们在代价函数中引入了表示定位不确定性的熵值。应用熵的策略通常存在计算复杂度过大的问题，该问题在针对较大环境时尤为明显。同时，使用熵的方法还存在不能离线计算环境的不确定信息的问题，以及只能对不确定的观测估计值进行一个整体评估的问题。

参考文献：

[1]L.P.Kaelbling,M.L.Littman,and A.R.Cassandra.Planning and acting in partiallyobservable stochastic domains.Technical report,Brown University,1995.

[2]D.Fox,W.Burgard and S.Thrun.“Active markov localization for mobile robots,”Robotics and Autonomous Systems,1998

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于地图定位能力的路径规划方法。

根据本发明提供的基于地图定位能力的路径规划方法，包括如下步骤：

步骤1：计算概率栅格地图各点的定位能力；

步骤2：将概率栅格地图各点的定位能力加入路径节点代价函数，使用路径规划算法进行搜索，计算出目标移动路径。

优选地，所述步骤1包括：

步骤1.1：获取待进行路径规划的概率栅格地图；具体地，包括采用SLAM算法对待进行路径规划的环境进行建图，得到环境的概率栅格地图；

步骤1.2：针对概率栅格地图上的多个点，分别计算每个点的定位能力。

优选地，所述步骤1.2包括：

步骤1.2.1：将机器人设置在概率栅格地图上，记录机器人的位姿坐标为

以坐标P为球心，且已知机器人的传感器感知半径为r，从而能够确定机器人的最大扫描边界，以Δr为间隔进行扫描，得到n条扫描射线；其中x_p,y_p,z_p为三维空间直角坐标系下的坐标，分别为x_p,y_p,z_p对应坐标轴上的角度，Δr为相邻的两束传感器射线的间隔；

步骤1.2.2：对每条扫描射线i计算τ_i方向上机器人到障碍物栅格的期望距离r_iE；

步骤1.2.3：求取栅格的静态定位能力矩阵，计算公式如下：

式中：Δr_iE表示期望距离的单位偏差，Δx_p表示位姿P及沿x轴方向移动的单位距离，Δy_p表示位姿P及沿y轴方向移动的单位距离，Δz_p表示位姿P及沿z轴方向移动的单位距离，表示位姿P及沿角方向旋转的单位角度，Δθ_p表示位姿P及沿θ角方向旋转的单位角度，Δψ_p表示位姿P及沿ψ角方向旋转的单位角度；表示位姿P及沿x轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿y轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿z轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿角方向旋转单位角度后两者期望距离r_iE的偏差，表示位姿P及沿θ角方向旋转单位角度后两者期望距离r_iE的偏差，表示位姿P及沿ψ角方向旋转单位角度后两者期望距离r_iE的偏差；

其中：矩阵的形式能够根据实际使用的传感器的不同转化为三维全景、二维全景、二维非全景中的任一种形式；三维全景是指机器人的位姿坐标P只考虑含x_p,y_p,z_p项，其余项为零；二维全景是指机器人的位姿坐标P只考虑含x_p,y_p项，其余项为零；二维非全景是指机器人的位姿坐标P只考虑含x_p,y_p,θ_p项，其余项为零的形式；n为总射线数，σ²为传感器噪声方差；所述单位距离和单位角度可根据实际需要确定；

步骤1.2.4：计算的行列式，并将结果作为反映栅格定位能力的代价L；对每个栅格计算定位能力矩阵并计算行列式，行列式的计算结果即为该栅格的L值。

优选地，所述步骤2中根据不同的路径规划方法得到相应的不同的路径节点代价函数，所述路径节点代价函数均能够加入定位能力作为规划时的指标；每个栅格的定位能力由每个栅格的L值反映。

优选地，所述步骤2中的路径规划算法包括：A*、Dijkstra、Fallback或者Floyd算法。

优选地，当使用A*算法进行路径规划时，计算公式如下：

G(n)＝G(n-1)+K_C·C(n-1,n)+K_L·L(n)

F(n)＝G(n)+H(n)

其中F(n)为路径节点n的综合代价；G(n)为从初始节点移动至节点n的实际代价；H(n)为从节点n移动至目标点的启发式估计代价；L(n)为路径节点n所在位置栅格的定位能力；C(n-1,n)为机器人由节点n-1移动至节点n的代价；K_C和K_L均为权重系数；当权重系数K_L为零时，代价函数退化为A*算法。

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

本发明提供的基于地图定位能力的路径规划方法，通过在路径节点代价函数中考虑地图各处的定位能力，从而能够有效优化规划路径，使得机器人沿着定位能力相对较强的区域进行规划，并避免因为路径节点的定位能力过低而引起机器人定位失败甚至丢失的情况。本法的定位算法简单，能够离线计算环境中的不确定信息，定位效率高。应用本发明所提出的路径规划方法具有提高移动单位在环境中的定位能力、避免移动单位定位失败等特点，适用于民用、工业、军事等各个领域。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为扫描方程示意图；

图2为传感器观测模型示意图；

图3为仿真环境概率栅格地图；

图4为仿真环境定位能力强度图；

图5为路径规划结果示例图。

图6为本发明的方法流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明的规划方法包括(1)计算概率栅格地图各点的定位能力；(2)使用路径规划算法进行搜索两个部分，由以下步骤组成：

步骤A1，获取待进行路径规划的环境的概率栅格地图：

待进行路径规划的环境的建图工作可通过SLAM等算法完成。建图结束后可以得到环境的概率栅格地图。

步骤A2，计算概率栅格地图各点的定位能力：

对概率栅格地图中各栅格进行如下计算：

步骤A2.1，设机器人位姿坐标为以该坐标为球心，根据实际传感器感知半径r可得机器人最大扫描边界：

(x-x_p)²+(y-y_p)²+(z-z_p)²＝r² (1)

以Δτ为扫描间隔，在式(1)的约束范围内，按照传感器的扫描顺序可以得到n条扫描射线。

步骤A2.2，对每条扫描射线i计算τ_i方向上机器人到障碍物栅格的期望距离r_iE：

图示2为传感器观测模型示意图，其中概率栅格地图的灰度区域为已知障碍物栅格，μ为栅格被占据的概率值。其值越大，表示栅格被障碍物占有的可能性越大。第i条射线到障碍物的期望长度r_iE可表示为：

$r_{iE}=\frac{{\Σ}_j^m{r}_{ij}{\mathrm{\μ}}_{ij}}{{\Σ}_j^m{\mathrm{\μ}}_{ij}}---\left(2\right)$

其中r_ij为机器人沿着第i条扫描射线方向至第j个概率栅格的距离，μ_ij为对应的栅格概率值。由于只需考虑该方向的最近已知障碍物，因此截止栅格序号m需满足条件：μ_im<T且μ_i(m-1)>μ_im，T为概率阈值。当栅格概率值小于T时，可近似认为该栅格没有被占据。

步骤A2.3，求取栅格的静态定位能力矩阵：

其中为机器人位姿坐标；为传感器数据高斯白噪声方差；n为扫描射线的个数；r_iE为第i条射线到障碍物的期望长度。本矩阵根据实际使用的传感器的不同，可以转化为三维全景(只考虑x_p,y_p,z_p信息)、二维全景(只考虑x_p,y_p信息)或二维非全景(只考虑x_p,y_p,θ_p信息)等形式。

步骤A2.4，计算的行列式，并将结果作为反映栅格定位能力的代价L，即

$i\left(P\right)=\det \hat{I}\left(P\right)---\left(4\right)$

步骤A3，将各栅格的定位能力加入路径节点代价函数：

赋予每个路径节点的定位能力即是其所处栅格的定位能力。路径节点代价函数的形式可根据具体路径规划算法的不同而有不同的形式，以下以使用A*算法进行路径规划的情况为例

G(n)＝G(n-1)+K_C·C(n-1,n)+K_L·L(n) (5)

F(n)＝G(n)+H(n) (6)

其中F(n)为路径节点n的综合代价；G(n)为从初始节点移动至节点n的实际代价；H(n)为从节点n移动至目标点的启发式估计代价；L(n)为路径节点n所在位置栅格的定位能力；C(n-1,n)为机器人由节点n-1移动至节点n的代价；K_C和K_L均为权重系数。当权重系数K_L为零时，代价函数退化为A*算法。

步骤A4，使用路径规划算法进行路径搜索，并计算出目标移动路径：

本步骤可使用的路径规划算法包括A*、Dijkstra、Fallback、Floyd算法。

下面结合附图和实施例对本发明作进一步的详细说明。此处以二维全景(只考虑x_p,y_p信息)情况为例加以说明，在此情况公式(3)将退化为以下形式：

$\hat{I}\left(P\right)={\&Sigma;}_{i=1}^n\frac{1}{{\mathrm{\&sigma;}}_i^2}\left[\begin{array}{cc}\frac{{\mathrm{\&Delta;r}}_{iE}^2}{{\mathrm{\&Delta;x}}^2}& \frac{{\mathrm{\&Delta;r}}_{iE}^2}{\mathrm{\&Delta;}x\mathrm{\&Delta;}y}\\ \frac{{\mathrm{\&Delta;r}}_{iE}^2}{\mathrm{\&Delta;}x\mathrm{\&Delta;}y}& \frac{{\mathrm{\&Delta;r}}_{iE}^2}{{\mathrm{\&Delta;y}}^2}\end{array}\right]---\left(7\right)$

本例使用ROS系统仿真环境加以说明。该环境为一室内办公区域，包括两条走廊及若干间办公室。其中一条走廊经过办公室门口，另外一条走廊两侧为墙面。步骤S1，在上述仿真环境中使用激光测距仪作为传感器进行SLAM建图，可以得到该环境的概率栅格地图，见图示3。其中栅格颜色深浅代表其被障碍物占用的概率，颜色越深表示其为障碍物的概率越高。栅格每一格尺寸为0.1×0.1米，激光传感器的感知范围为10米。

对离线建立的概率栅格地图上的各个栅格计算期望感知数据。机器人模型及扫描示意图见图示1。设在位姿P＝[x_p,y_p]下进行扫描，在式(1)的约束范围内，按照传感器的扫描顺序，可以得到n条扫描射线。对于每条激光射线i，在给定概率阈值T并确定截止栅格序号m后，可以根据式(2)计算期望的激光扫描距离r_iE。设定扫描范围为360°，扫描距离r为10米，概率阈值T为0.65。

对概率栅格地图上的各个栅格按照公式(7)计算其静态定位能力矩阵。Δx、Δy均取栅格的分辨率，即0.1米。在计算该矩阵后，计算该矩阵的行列式，并作为该栅格最终的定位能力。图示4为仿真环境的定位能力计算结果。

在栅格地图上设定初始点和目标点后使用路径规划算法进行搜索，并计算出机器人的目标移动路径。示例规划结果如图示5所示，其中绿色路径为仅使用A*算法规划的结果，新规划路径为加入栅格定位能力后的规划结果。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (6)

1.一种基于地图定位能力的路径规划方法，其特征在于，包括如下步骤：

步骤1：计算概率栅格地图各点的定位能力；

步骤2：将概率栅格地图各点的定位能力加入路径节点代价函数，使用路径规划算法进行搜索，计算出目标移动路径。

2.根据权利要求1所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤1包括：

步骤1.1：获取待进行路径规划的概率栅格地图；具体地，包括采用SLAM算法对待进行路径规划的环境进行建图，得到环境的概率栅格地图；

步骤1.2：针对概率栅格地图上的多个点，分别计算每个点的定位能力。

3.根据权利要求1所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤2中根据不同的路径规划方法得到相应的不同的路径节点代价函数，所述路径节点代价函数均能够加入定位能力作为规划时的指标；每个栅格的定位能力由每个栅格的L值反映。

4.根据权利要求1或3所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤2中的路径规划算法包括：A*、Dijkstra、Fallback或者Floyd算法。

5.根据权利要求2所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤1.2包括：

步骤1.2.1：将机器人设置在概率栅格地图上，记录机器人的位姿坐标为

以坐标P为球心，且已知机器人的传感器感知半径为r，从而能够确定机器人的最大扫描边界，以Δr为间隔进行扫描，得到n条扫描射线；其中x_p,y_p,z_p为三维空间直角坐标系下的坐标，分别为x_p,y_p,z_p对应坐标轴上的角度，Δr为相邻的两束传感器射线的间隔；

步骤1.2.2：对每条扫描射线i计算τ_i方向上机器人到障碍物栅格的期望距离r_iE；

步骤1.2.3：求取栅格的静态定位能力矩阵，计算公式如下：

式中：Δr_iE表示期望距离的单位偏差，Δx_p表示位姿P及沿x轴方向移动的单位距离，Δy_p表示位姿P及沿y轴方向移动的单位距离，Δz_p表示位姿P及沿z轴方向移动的单位距离，表示位姿P及沿角方向旋转的单位角度，Δθ_p表示位姿P及沿θ角方向旋转的单位角度，Δψ_p表示位姿P及沿ψ角方向旋转的单位角度；表示位姿P及沿x轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿y轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿z轴方向移动单位距离处两者期望距离r_iE的偏差，表示位姿P及沿角方向旋转单位角度后两者期望距离r_iE的偏差，表示位姿P及沿θ角方向旋转单位角度后两者期望距离r_iE的偏差，表示位姿P及沿ψ角方向旋转单位角度后两者期望距离r_iE的偏差；

其中：矩阵的形式能够根据实际使用的传感器的不同转化为三维全景、二维全景、二维非全景中的任一种形式；三维全景是指机器人的位姿坐标P只考虑含x_p,y_p,z_p项，其余项为零；二维全景是指机器人的位姿坐标P只考虑含x_p,y_p项，其余项为零；二维非全景是指机器人的位姿坐标P只考虑含x_p,y_p,θ_p项，其余项为零的形式；n为总射线数，σ²为传感器噪声方差；所述单位距离和单位角度可根据实际需要确定；

步骤1.2.4：计算的行列式，并将结果作为反映栅格定位能力的代价L；对每个栅格计算定位能力矩阵并计算行列式，行列式的计算结果即为该栅格的L值。

6.根据权利要求4所述的基于地图定位能力的路径规划方法，其特征在于，当使用A*算法进行路径规划时，计算公式如下：

G(n)＝G(n-1)+K_C·C(n-1,n)+K_L·L(n)

F(n)＝G(n)+H(n)

其中F(n)为路径节点n的综合代价；G(n)为从初始节点移动至节点n的实际代价；H(n)为从节点n移动至目标点的启发式估计代价；L(n)为路径节点n所在位置栅格的定位能力；C(n-1,n)为机器人由节点n-1移动至节点n的代价；K_C和K_L均为权重系数；当权重系数K_L为零时，代价函数退化为A*算法。