基于地图定位能力的路径规划方法
【专利摘要】本发明提供了一种基于地图定位能力的路径规划方法，通过将地图定位能力加入路径节点代价函数，以达到优化规划路径的目的。本发明的规划方法包括以下部分：首先计算概率栅格地图各点的定位能力，其次将概率栅格地图各点加入路径节点代价函数，最后通过路径规划算法进行搜索，计算出从初始点至目标点的移动路径。应用本发明所提出的路径规划方法具有提高移动单位在环境中的定位能力、避免移动单位定位失败等特点，适用于民用、工业、军事等各个领域。
【专利说明】
基于地图定位能力的路径规划方法
技术领域
[0001] 本发明涉及路径规划技术领域，具体地，涉及一种基于地图定位能力的路径规划 方法。
【背景技术】
[0002] 在机器人领域中，移动机器人所处的环境情况是相对复杂的。移动机器人的基本 需求之一为在定位导航过程中规划出一条避开所有障碍物的路径。在环境各点中由于可观 测信息的不同，导致机器人在不同点有不同的定位能力（或称定位能力）。在进行路径规划 时须考虑该问题，沿着定位能力相对较强的区域进行规划，并避免因为路径节点的定位能 力过低而引起机器人定位失败甚至丢失的情况。
[0003] Kaelbling[l]等人提出了一种通过使定位过程中的期望熵值最低来提高定位预 测信息的准确性的方法，但这种方法在规划路径时存在局部最小点的问题。Fox[2]等人提 出了一种主动选择高定位精度的路径的策略，他们在代价函数中引入了表示定位不确定性 的熵值。应用熵的策略通常存在计算复杂度过大的问题，该问题在针对较大环境时尤为明 显。同时，使用熵的方法还存在不能离线计算环境的不确定信息的问题，以及只能对不确定 的观测估计值进行一个整体评估的问题。
[0004] 参考文献：
[0005] [l]L.P.Kaelbling,M.L.Littman,and A.R.Cassandra.Planning and acting in partially observable stochastic domains . Technical report,Brown University, 1995.
[0006] [2]D.Fox,ff.Burgard and S.Thrun."Active markov localization for mobile robots，"Robotics and Autonomous Systems,1998
【发明内容】
[0007] 针对现有技术中的缺陷，本发明的目的是提供一种基于地图定位能力的路径规划 方法。
[0008] 根据本发明提供的基于地图定位能力的路径规划方法，包括如下步骤：
[0009] 步骤1:计算概率栅格地图各点的定位能力；
[0010]步骤2:将概率栅格地图各点的定位能力加入路径节点代价函数，使用路径规划算 法进行搜索，计算出目标移动路径。
[0011]优选地，所述步骤1包括：
[0012] 步骤1.1:获取待进行路径规划的概率栅格地图；具体地，包括采用SLAM算法对待 进行路径规划的环境进行建图，得到环境的概率栅格地图；
[0013] 步骤1.2:针对概率栅格地图上的多个点，分别计算每个点的定位能力。
[0014] 优选地，所述步骤1.2包括：
[0015] 步骤1.2.1:将机器人设置在概率栅格地图上，记录机器人的位姿坐标为
[0016] P =〖气》,外七,外,％為/]，以坐标？为球心，且已知机器人的传感器感知半径为1， 从而能够确定机器人的最大扫描边界，以A r为间隔进行扫描，得到n条扫描射线；其中Xp， yP，zP为三维空间直角坐标系下的坐标，％為,％分别为xP，y P，zP对应坐标轴上的角度，Ar 为相邻的两束传感器射线的间隔；
[0017] 步骤1.2.2:对每条扫描射线i计算τι方向上机器人到障碍物栅格的期望距离rlE;
[0018] 步骤1.2.3:求取栅格的静态定位能力矩阵，计算公式如下：
[0020] 式中：ΔΓιΕ表示期望距离的单位偏差，八知表示位姿P及沿X轴方向移动的单位距 离，Α ^表示位姿Ρ及沿y轴方向移动的单位距离，△ 2[)表示位姿Ρ及沿ζ轴方向移动的单位距 离，伞Pp表示位姿P及沿炉角方向旋转的单位角度，Α θρ表示位姿P及沿Θ角方向旋转的单位 角度，Α %表示位姿Ρ及沿φ角方向旋转的单位角度；
·表示位姿P及沿X轴方向移动单位距 离处两者期望距离rlE的偏差
表示位姿P及沿y轴方向移动单位距离处两者期望距离rlE 的偏差
表示位姿P及沿z轴方向移动单位距离处两者期望距离rlE的偏差
表示位姿 P及沿？^角方向旋转单位角度后两者期望距离rlE的偏差：
=表示位姿P及沿Θ角方向旋转单 位角度后两者期望距离rlE的偏差
表示位姿P及沿Φ角方向旋转单位角度后两者期望距 呙1^的偏差；
[0021] 其中：矩阵/:(7)的形式能够根据实际使用的传感器的不同转化为三维全景、二维 全景、二维非全景中的任一种形式;三维全景是指机器人的位姿坐标P只考虑含 Xp，yP，Zp项， 其余项为零;二维全景是指机器人的位姿坐标P只考虑含&，^项，其余项为零;二维非全景 是指机器人的位姿坐标P只考虑含&，7 [)，9[)项，其余项为零的形式;11为总射线数，0 2为传感 器噪声方差;所述单位距离和单位角度可根据实际需要确定；
[0022] 步骤1.2.4:计算?(Ρ)的行列式，并将结果作为反映栅格定位能力的代价L;对每个 栅格计算定位能力矩阵并计算行列式，行列式的计算结果即为该栅格的L值。
[0023] 优选地，所述步骤2中根据不同的路径规划方法得到相应的不同的路径节点代价 函数，所述路径节点代价函数均能够加入定位能力作为规划时的指标;每个栅格的定位能 力由每个栅格的L值反映。
[0024] 优选地，所述步骤2中的路径规划算法包括:A*、Dijkstra、Fallback或者Floyd算 法。
[0025]优选地，当使用A*算法进行路径规划时，计算公式如下：
[0026] G(n) =G(n_l )+Kc · C(n_l，η)+Κι · L(n)
[0027] F(n) =G(n)+H(n)
[0028] 其中F(n)为路径节点n的综合代价;G(n)为从初始节点移动至节点n的实际代价;H U)为从节点η移动至目标点的启发式估计代价;L(n)为路径节点η所在位置栅格的定位能 力;C(n-l，n)为机器人由节点η-l移动至节点η的代价;K C和KL均为权重系数；当权重系数KL 为零时，代价函数退化为A*算法。
[0029] 与现有技术相比，本发明具有如下的有益效果：
[0030] 本发明提供的基于地图定位能力的路径规划方法，通过在路径节点代价函数中考 虑地图各处的定位能力，从而能够有效优化规划路径，使得机器人沿着定位能力相对较强 的区域进行规划，并避免因为路径节点的定位能力过低而引起机器人定位失败甚至丢失的 情况。本法的定位算法简单，能够离线计算环境中的不确定信息，定位效率高。应用本发明 所提出的路径规划方法具有提尚移动单位在环境中的定位能力、避免移动单位定位失败等 特点，适用于民用、工业、军事等各个领域。
【附图说明】
[0031] 通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、 目的和优点将会变得更明显：
[0032] 图1为扫描方程示意图；
[0033]图2为传感器观测模型示意图；
[0034]图3为仿真环境概率栅格地图；
[0035]图4为仿真环境定位能力强度图；
[0036]图5为路径规划结果示例图。
[0037]图6为本发明的方法流程图。
【具体实施方式】
[0038]下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术 人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术 人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明 的保护范围。
[0039]本发明的规划方法包括（1)计算概率栅格地图各点的定位能力；（2)使用路径规划 算法进行搜索两个部分，由以下步骤组成：
[0040] 步骤A1，获取待进行路径规划的环境的概率栅格地图：
[0041] 待进行路径规划的环境的建图工作可通过SLAM等算法完成。建图结束后可以得到 环境的概率栅格地图。
[0042]步骤A2,计算概率栅格地图各点的定位能力：
[0043] 对概率栅格地图中各栅格进行如下计算：
[0044] 步骤A2.1，设机器人位姿坐标为= [V 3W ΘΡ, %]，以该坐标为球心，根据 实际传感器感知半径r可得机器人最大扫描边界：
[0045] (x-xp)2+(y-yp) 2+(z-zp)2 = r2 (1)
[0046] 以△ τ为扫描间隔，在式（1)的约束范围内，按照传感器的扫描顺序可以得到η条扫 描射线。
[0047]步骤Α2.2,对每条扫描射线i计算^方向上机器人到障碍物栅格的期望距离rlE:
[0048]图示2为传感器观测模型示意图，其中概率栅格地图的灰度区域为已知障碍物栅 格，μ为栅格被占据的概率值。其值越大，表示栅格被障碍物占有的可能性越大。第i条射线 到障碍物的期望长度rlE可表示为：
[0050] 其中^」为机器人沿着第i条扫描射线方向至第j个概率栅格的距离，为对应的 栅格概率值。由于只需考虑该方向的最近已知障碍物，因此截止栅格序号m需满足条件:μ ιη〈 Τ为概率阈值。当栅格概率值小于Τ时，可近似认为该栅格没有被占据。
[0051] 步骤Α2.3，求取栅格的静态定位能力矩阵：
[0053] 其中Ρ - ％?..，％，.約3:，％，.少ρ]为机器人位姿·坐标；为传感器数据尚斯白卩栄声方 差;η为扫描射线的个数;rlE为第i条射线到障碍物的期望长度。本矩阵根据实际使用的传感 器的不同，可以转化为三维全景（只考虑Xp，yP，zP信息）、二维全景（只考虑xP，y P信息)或二维 非全景（只考虑xp，yP，M言息)等形式。
[0054]步骤A2.4,计??:?(：Ρ：)的行列式，并将结果作为反映栅格定位能力的代价L，即 [0055] i(P) = det/(P) (4)
[0056]步骤A3,将各栅格的定位能力加入路径节点代价函数：
[0057]赋予每个路径节点的定位能力即是其所处栅格的定位能力。路径节点代价函数的 形式可根据具体路径规划算法的不同而有不同的形式，以下以使用A*算法进行路径规划的 情况为例
[0058] G(n) =G(n_l)+Kc · C(n_l，η)+Κι · L(n) (5)
[0059] F(n)=G(n)+H(n) (6)
[0060] 其中F(n)为路径节点n的综合代价;G(n)为从初始节点移动至节点n的实际代价;H U)为从节点η移动至目标点的启发式估计代价;L(n)为路径节点η所在位置栅格的定位能 力;C(n-l，n)为机器人由节点n-Ι移动至节点η的代价;K C和KL均为权重系数。当权重系数KL 为零时，代价函数退化为A*算法。
[0061 ]步骤A4，使用路径规划算法进行路径搜索，并计算出目标移动路径：
[0062] 本步骤可使用的路径规划算法包括A*、Di」1^1:瓜、？31]^3〇1^、？1〇7(1算法。
[0063]下面结合附图和实施例对本发明作进一步的详细说明。此处以二维全景（只考虑 xP，yP信息)情况为例加以说明，在此情况公式⑶将退化为以下形式：
L〇〇65」本例使用R0S糸统仿真环境加以说明。该环境为一室内办公区域，包括两条走廊及 若干间办公室。其中一条走廊经过办公室门口，另外一条走廊两侧为墙面。步骤S1，在上述 仿真环境中使用激光测距仪作为传感器进行SLAM建图，可以得到该环境的概率栅格地图， 见图示3。其中栅格颜色深浅代表其被障碍物占用的概率，颜色越深表示其为障碍物的概率 越高。栅格每一格尺寸为0.1X0.1米，激光传感器的感知范围为10米。
[0066] 对离线建立的概率栅格地图上的各个栅格计算期望感知数据。机器人模型及扫描 示意图见图示1。设在位姿P=[xP，y P]下进行扫描，在式（1)的约束范围内，按照传感器的扫 描顺序，可以得到η条扫描射线。对于每条激光射线i，在给定概率阈值T并确定截止栅格序 号m后，可以根据式(2)计算期望的激光扫描距离r lE。设定扫描范围为360°，扫描距离r为10 米，概率阈值T为0.65。
[0067] 对概率栅格地图上的各个栅格按照公式(7)计算其静态定位能力矩阵。Δχ、Ay均 取栅格的分辨率，即0.1米。在计算该矩阵后，计算该矩阵的行列式，并作为该栅格最终的定 位能力。图示4为仿真环境的定位能力计算结果。
[0068] 在栅格地图上设定初始点和目标点后使用路径规划算法进行搜索，并计算出机器 人的目标移动路径。示例规划结果如图示5所示，其中绿色路径为仅使用A*算法规划的结 果，新规划路径为加入栅格定位能力后的规划结果。
[0069] 以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述 特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影 响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相 互组合。
【主权项】
1. 一种基于地图定位能力的路径规划方法，其特征在于，包括如下步骤： 步骤1:计算概率栅格地图各点的定位能力； 步骤2:将概率栅格地图各点的定位能力加入路径节点代价函数，使用路径规划算法进 行搜索，计算出目标移动路径。2. 根据权利要求1所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤1 包括： 步骤1.1:获取待进行路径规划的概率栅格地图；具体地，包括采用SLAM算法对待进行 路径规划的环境进行建图，得到环境的概率栅格地图； 步骤1.2:针对概率栅格地图上的多个点，分别计算每个点的定位能力。3. 根据权利要求1所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤2 中根据不同的路径规划方法得到相应的不同的路径节点代价函数，所述路径节点代价函数 均能够加入定位能力作为规划时的指标;每个栅格的定位能力由每个栅格的L值反映。4. 根据权利要求1或3所述的基于地图定位能力的路径规划方法，其特征在于，所述步 骤2中的路径规划算法包括:A*、Di jkstra、Fallback或者Floyd算法。5. 根据权利要求2所述的基于地图定位能力的路径规划方法，其特征在于，所述步骤 1.2包括： 步骤1.2.1:将机器人设置在概率栅格地图上，记录机器人的位姿坐标为,以坐标P为球心，且已知机器人的传感器感知半径Sr，从而 能够确定机器人的最大扫描边界，以Ar为间隔进行扫描，得到η条扫描射线;其中Xp，yp， Zp 为三维空间直角坐标系下的坐标，分别为xP，yP，zP对应坐标轴上的角度，A r为相 邻的两束传感器射线的间隔； 步骤1.2.2:对每条扫描射线i计算T1方向上机器人到障碍物栅格的期望距离rlE; 步骤1.2.3:求取栅格的静态定位能力矩阵，计算公式如下：式中：ΔΓιΕ表示期望距离的单位偏差，八&表示位姿P及沿X轴方向移动的单位距离，Δ 7[)表示位姿P及沿y轴方向移动的单位距离，△ 2[)表示位姿P及沿z轴方向移动的单位距离， 表示位姿P及沿货角方向旋转的单位角度，A θρ表示位姿P及沿Θ角方向旋转的单位角 度，△%表示位姿P及沿Φ角方向旋转的单位角度;^表示位姿P及沿X轴方向移动单位距离 处两者期望距离rlE的偏差，^表示位姿P及沿y轴方向移动单位距离处两者期望距离rlE的 偏差，^表示位姿P及沿z轴方向移动单位距离处两者期望距离rlE的偏差，^表示位姿P 及沿中角方向旋转单位角度后两者期望距离rlE的偏差，^表示位姿P及沿Θ角方向旋转单 位角度后两者期望距离rlE的偏差，^表示位姿P及沿Φ角方向旋转单位角度后两者期望距 ^riE的偏差； 其中：矩阵的形式能够根据实际使用的传感器的不同转化为三维全景、二维全景、 二维非全景中的任一种形式;三维全景是指机器人的位姿坐标P只考虑含&，7[)，2[)项，其余 项为零;二维全景是指机器人的位姿坐标P只考虑含&，yP项，其余项为零;二维非全景是指 机器人的位姿坐标P只考虑含&，7 [)，9[)项，其余项为零的形式;11为总射线数，〇 2为传感器噪 声方差;所述单位距离和单位角度可根据实际需要确定； 步骤1.2.4:计算iGP)的行列式，并将结果作为反映栅格定位能力的代价L;对每个栅格 计算定位能力矩阵并计算行列式，行列式的计算结果即为该栅格的L值。6.根据权利要求4所述的基于地图定位能力的路径规划方法，其特征在于，当使用A*算 法进行路径规划时，计算公式如下： G(n)=G(n_l)+Kc · C(n_l，n)+KL · L(n) F(n) =G(n)+H(n) 其中F (n)为路径节点n的综合代价;G (n)为从初始节点移动至节点n的实际代价;H (n) 为从节点η移动至目标点的启发式估计代价;L(n)为路径节点η所在位置栅格的定位能力;C (η-1，η)为机器人由节点n-Ι移动至节点η的代价;Kc和Kl均为权重系数；当权重系数Kl为零 时，代价函数退化为Α*算法。
【文档编号】G01C21/34GK106017497SQ201610527990
【公开日】2016年10月12日
【申请日】2016年7月6日
【发明人】王景川, 陈卫东, 刘成志
【申请人】上海交通大学