[发明专利]一种区分人员影响的服务机器人导航规划方法

权利要求书

1.一种区分人员影响的服务机器人导航规划方法，其特征在于，步骤如下：

第一部分，成本建模

步骤1，人员影响特性分类：根据对服务机器人需求的不同，将人员划分为两类：行人和用户，用户即为被服务对象；要求服务机器人最基本的行为特性表现为趋、避两种；

步骤2，对行人、用户影响的抽象描述进行量化；分别使用二维非标高斯函数进行几何建模，得到人员影响成本C₁；

(1)趋、避行为特性直观表现为服务机器人距离上的接近和远离，即用距离语言作为量化标准；

(2)行人对服务机器人来说具有陌生性，而侵入到陌生对象的私人空间又被认为是不合理的；因此，使用二维非标高斯函数进行距离及几何建模，通过高斯曲面值的大小和截断处理将行人影响转化为各个方向、各种等级的距离对应值，曲面值分布于[0,1]区间，曲面值越高代表越不可接近；用户是服务机器人的服务对象，具有亲密性，因此建立反向二维非标高斯函数进行几何建模，即整体取负，曲面值分布于[-1,0]区间，值越低代表越应该接近；

(2)几何建模及二维非标高斯函数为：

式(1)中，f(x,y)为量化成本值，(x₀,y₀)为人员位置，幅值A按人员角色取1或-1；由于人员视野范围和靠右行驶的原则，对正前方的接近比后方要敏感，对右边的接近比左边要敏感，因此高斯函数按人员朝向为正前方，在不同方向取不同的方差值σ+x、σ-x、σ+y、σ-y；

步骤3，得到静态环境地图，服务机器人利用传感器进行SLAM(SimultaneousLocalization and Mapping)过程对服务场所、静态障碍进行感知，输出静态环境地图成本C₂；地图以栅格形式表示，栅格占据状态值代表服务机器人通过成本；

步骤4，得到总的成本地图C_总＝C₁+C₂；将人员影响成本与静态环境地图成本两部分叠加；

(1)服务机器人SLAM过程利用Hokuyo激光雷达和Rao-Blackwellized粒子滤波算法，外加服务机器人本体提供的里程计信息，输出得到环境地图成本C₂；该环境地图以占据二值图的栅格形式表示，占据值为1表示该栅格有障碍物存在，占据值为0表示可自由通行；

(2)将人员影响成本C₁与静态环境地图成本C₂两部分叠加，如式(2)，因人员影响成本C₁原有占据值为1的部分不可能再有人员存在，则表征移动成本值的栅格单元值最终分布于[-1,1]区间；这样，将建立的人员影响成本考虑进来，以约束服务机器人的行为表现，使之具备趋、避倾向；

C_总＝C₁+C₂ (2)

第二部分，感知切换

步骤5，传感器对人员参与进行检测：先从环境中将动态人员检出，再进一步区分出用户和无关行人，具体：采用对人腿部点云聚类分割，以激光扫描作为输入，使用随机森林分类器检测数据簇，作为人腿候选处，从环境中将动态人员检出；进一步，深度相机采用模板匹配的方法对动态人员的轮廓外形进行识别，与用户数据库对比，区分出用户和行人；

步骤6，目标位置的确定和切换：步骤5未探测到用户时，暂定探索目标位置，以在前进中寻找用户；传感器探测到用户时，则切换线程，以用户为目标位置；

第三部分，导航规划

步骤7，全局路径规划器启动，获得至目标点的全局路径；获得目标位置的输入后，在总成本地图C_总上使用改进的全局规划算法，启发式函数加入人员成本，基于栅格地图的成本进行路径搜索；

地图服务器接收成本地图信息；一旦获得目标位置的输入，利用传感器观测，基于蒙特卡洛自主定位，获得服务机器人当前位置及当前位姿朝向；全局路径规划器按规划频率更新用户相对于服务机器人的位置，人员影响成本也在此阶段更新，寄存用户信息；

(1)全局路径规划器启动，在总成本地图C_总上进行路径搜索；以八连通栅格为搜索策略，传统A*算法在此处被优化改进，其启发式函数h部分最初是服务机器人当前位置到目标位置的距离估价；在此基础上，h修改为距离成本和人员影响成本之和，还对动态成本加权以为加快搜索过程，最终获得全局路线，如式(3)所示：

f＝g+h+δh₁ (3)

其中，f为服务机器人从初始位置到目标位置的移动成本函数，g为从初始点到当前点的实际消耗成本，h为从当前位置到目标位置的距离估价，h₁为栅格的人员影响成本，δ为加权系数，取经验值；

步骤8，局部规划器启动，避障行进；令局部算法跟随全局路径，优化轨迹预测评分机制，利用传感器实时观测避开局部障碍，并选择最优的规划速度发送给执行机构；

(1)全局路线发布参考路径至局部规划器；同时传感器进行实时观测，扫描障碍物、人员检测信息；

(2)在最大最小速度限制、电机转矩限制、停车距离限制所形成对速度空间内进行(线速度v,角速度w)组的采样过程；依照该(线速度v,角速度w)组和规划时间间隔ΔT前向模拟轨迹；对轨迹进行评分，评分项：目标朝向角大小、速度大小、与最近障碍物距离大小和与全局路径的最短距离大小；选择评分最高的模拟轨迹对应的规划速度(v₀，w₀)；

(3)最后，直接将(v₀，w₀)发送给移动底盘，由执行机构进行速度平滑输出。