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

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

【技术实现步骤摘要】

本专利技术涉及路径规划
，具体地，涉及一种基于地图定位能力的路径规划方法。
技术介绍
在机器人领域中，移动机器人所处的环境情况是相对复杂的。移动机器人的基本需求之一为在定位导航过程中规划出一条避开所有障碍物的路径。在环境各点中由于可观测信息的不同，导致机器人在不同点有不同的定位能力(或称定位能力)。在进行路径规划时须考虑该问题，沿着定位能力相对较强的区域进行规划，并避免因为路径节点的定位能力过低而引起机器人定位失败甚至丢失的情况。Kaelbling[1]等人提出了一种通过使定位过程中的期望熵值最低来提高定位预测信息的准确性的方法，但这种方法在规划路径时存在局部最小点的问题。Fox[2]等人提出了一种主动选择高定位精度的路径的策略，他们在代价函数中引入了表示定位不确定性的熵值。应用熵的策略通常存在计算复杂度过大的问题，该问题在针对较大环境时尤为明显。同时，使用熵的方法还存在不能离线计算环境的不确定信息的问题，以及只能对不确定的观测估计值进行一个整体评估的问题。参考文献：[1]L.P.Kaelbling,M.L.Littman,and A.R.Cassandra.Planning and acting in partially observable 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条扫描射线；其中xp,yp,zp为三维空间直角坐标系下的坐标，分别为xp,yp,zp对应坐标轴上的角度，Δr为相邻的两束传感器射线的间隔；步骤1.2.2：对每条扫描射线i计算τi方向上机器人到障碍物栅格的期望距离riE；步骤1.2.3：求取栅格的静态定位能力矩阵，计算公式如下：式中：ΔriE表示期望距离的单位偏差，Δxp表示位姿P及沿x轴方向移动的单位距离，Δyp表示位姿P及沿y轴方向移动的单位距离，Δzp表示位姿P及沿z轴方向移动的单位距离，表示位姿P及沿角方向旋转的单位角度，Δθp表示位姿P及沿θ角方向旋转的单位角度，Δψp表示位姿P及沿ψ角方向旋转的单位角度；表示位姿P及沿
x轴方向移动单位距离处两者期望距离riE的偏差，表示位姿P及沿y轴方向移动单位距离处两者期望距离riE的偏差，表示位姿P及沿z轴方向移动单位距离处两者期望距离riE的偏差，表示位姿P及沿角方向旋转单位角度后两者期望距离riE的偏差，表示位姿P及沿θ角方向旋转单位角度后两者期望距离riE的偏差，表示位姿P及沿ψ角方向旋转单位角度后两者期望距离riE的偏差；其中：矩阵的形式能够根据实际使用的传感器的不同转化为三维全景、二维全景、二维非全景中的任一种形式；三维全景是指机器人的位姿坐标P只考虑含xp,yp,zp项，其余项为零；二维全景是指机器人的位姿坐标P只考虑含xp,yp项，其余项为零；二维非全景是指机器人的位姿坐标P只考虑含xp,yp,θp项，其余项为零的形式；n为总射线数，σ2为传感器噪声方差；所述单位距离和单位角度可根据实际需要确定；步骤1.2.4：计算的行列式，并将结果作为反映栅格定位能力的代价L；对每个栅格计算定位能力矩阵并计算行列式，行列式的计算结果即为该栅格的L值。优选地，所述步骤2中根据不同的路径规划方法得到相应的不同的路径节点代价函数，所述路径节点代价函数均能够加入定位能力作为规划时的指标；每个栅格的定位能力由每个栅格的L值反映。优选地，所述步骤2中的路径规划算法包括：A*、Dijkstra、Fallback或者Floyd算法。优选地，当使用A*算法进行路径规划时，计算公式如下：G(n)＝G(n-1)+KC·C(n-1,n)+KL·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的代价；KC和KL均为权重系数；当权重系数KL为零时，代价函数退化为A*算法。与现有技术相比，本专利技术具有如下的有益效果：本专利技术提供的基于地图定位能力的路径规划方法，通过在路径节点代价函数中考虑地图各处的定位能力，从而能够有效优化规划路径，使得机器人沿着定位能力相对较强的区域进行规划，并避免因为路径节点的定位能力过低而引起机器人定位失败甚至丢失的情况。本法的定位算法简单，能够离线计算环境中的不确定信息，定位效率高。应用
本专利技术所提出的路径规划方法具有提高移动单位在环境中的定位能力、避免移动单位定位失败等特点，适用于民用、工业、军事等各个领域。附图说明通过阅读参照以下附图对非限制性实施例所作的详细描述，本专利技术的其它特征、目的和优点将会变得更明显：图1为扫描方程示意图；图2为传感器观测模型示意图；图3为仿真环境概率栅格地图；图4为仿真环境定位能力强度图；图5为路径规划结果示例图。图6为本专利技术的方法流程图。具体实施方式下面结合具体实施例对本专利技术进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本专利技术，但不以任何形式限制本专利技术。应当指出的是，对本领域的普通技术人员来说，在不脱离本专利技术构思的前提下，还可以做出若干变化和改进。这些都属于本专利技术的保护范围。本专利技术的规划方法包括(1)计算概率栅格地图各点的定位能力；(2)使用路径规划算法进行搜索两个部分，由以下步骤组成：步骤A1，获取待进行路径规划的环境的概率栅格地图：待进行路径规划的环境的建图工作可通过SLAM等算法完成。建图结束后可以得到环境的概率栅格地图。步骤A2，计算概率栅格地图各点的定位能力：对概率栅格地图中各栅格进行如下计算：步骤A2.1，设机器人位姿坐标为以该坐标为球心，根据实际传感器感知半径r可得机器人最大扫描边界：(x-xp)2+(y-yp)2+(z-zp)2＝r2 (1)以Δτ为扫描间隔，在式(1)的约束范围内，按照传感器的扫描顺序可以得到n条扫描射线。步骤A2.2，对每条扫描射线i计算τi方向上机器人到障碍物栅格的期望距离riE：图示2为传感器观测本文档来自技高网...

【技术保护点】
一种基于地图定位能力的路径规划方法，其特征在于，包括如下步骤：步骤1：计算概率栅格地图各点的定位能力；步骤2：将概率栅格地图各点的定位能力加入路径节点代价函数，使用路径规划算法进行搜索，计算出目标移动路径。

【技术特征摘要】
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条扫描射线；其中xp,yp,zp为三维空间直角坐标系下的坐标，分别为xp,yp,zp对应坐标轴上的角度，Δr为相邻的两束传感器射线的间隔；步骤1.2.2：对每条扫描射线i计算τi方向上机器人到障碍物栅格的期望距离riE；步骤1.2.3：求取栅格的静态定位能力矩阵，计算公式如下：式中：ΔriE表示期望距离的单位偏差，Δxp表示位姿P及沿x轴方向移动的单位距离，Δyp表示位姿P及沿y轴方向移动的单位距离，Δz...

【专利技术属性】
技术研发人员：王景川，陈卫东，刘成志，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：上海;31