CN114167856B - 一种基于人工情感的服务机器人局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于人工情感的服务机器人局部路径规划方法。该方法包括：采用非全向移动模型，推算机器人在下一时段内的位移轨迹，然后计算采样速度区间，确定预测轨迹。将采集到的机器人周边环境因素量化建模，映射至人工情感的亲密度，恐惧度，愉悦度，喜悦度四个维度。设计合理的适应度函数fitness，评价PSO算法解的品质，在其适应度函数值最小的时候，得到优化后人工情感函数值的权重。再次对多条预测轨迹的期望值进行计算，期待值E(l_i)最高的预测轨迹会被决策层选择，将其作为下一帧的位姿。通讯机制将控制信息发送给控制器，控制器执行机器人在决策层所选择的动作，最终实现以人工情感为核心的自主导航过程。

Description

一种基于人工情感的服务机器人局部路径规划方法

技术领域

本发明属于机器人路径规划技术领域，具体涉及一种基于人工情感的服务机器人局部路径规划方法。

背景技术

在复杂不确定环境下，移动机器人路径规划的评价指标各异，通常采用以下指标中的一种或几种，例如：路径长度、运行时间、算法复杂度、路径安全程度、能耗等。情感在人类和其他哺乳动物的决策和路径规划过程中会起到极其关键的作用，且拥有情感的机器人可更好的与人交互。

在心理学情绪认知领域，有机体经常搜索环境中所需要的线索和需要逃避的危险，然后对每一个刺激物与自身的利害关系进行评估。评估过程一般包括：事件与自己的目标是否一致？是否与自身幸福有关？这种评估具有实时性、反复性、多回合性等特点，可分为初评价和再评价。评估过程中，机器人人工情感的情感强度和持续时间会不断变化，若向任务目标运动，则存在积极愉快的核心感情激励；反之，若存在负面的情感过程，则会导致逃避反应。

在面对多变的室内环境建模，人工情感具备很强的映射能力，能够将复杂的“刺激-行为”转化成易于管理“情感-行为”模式，能够提高服务机器人的智能性与自主性。截至目前，人工情感极少被考虑到机器人的决策和规划过程中。

发明内容

针对现有技术的不足，本发明提出了一种基于人工情感的服务机器人局部路径规划方法，对机器人感知层与规划层的传感器数据进行融合，使机器人感知其所处的环境，然后环境信息量化建模，映射至情感的四个维度，并实时对自身的“认知情感”进行初评价与再评价，建立机器人的情感决策模型，提高在移动过程中机器人的自适应能力和路径规划算法的鲁棒性。

一种基于人工情感的服务机器人局部路径规划方法，具体包括以下步骤：

步骤一、地图建立

使用激光雷达扫描周边环境，建立服务机器人的活动地图。在服务机器人与全局目标点之间等距离设置P个采样点，在服务机器人向全局目标点前进的过程中，将服务机器人前方的第p个采样点作为局部目标点。当服务机器人当前位置与全局目标点之间的采样点数量少于p个时，将全局目标点作为局部目标点。

作为优选，设置P＝50，p＝10。

步骤二、障碍物检测

将步骤一建立的活动地图栅格化为代价地图，代价地图中每个单元格的危险程度代价cost由活动地图中的周边环境确定。将危险程度根据危险程度代价cost的取值分为三类。

s2.1、cost∈[0,127]时，服务机器人移动过程中的垂直投影边界与障碍物膨胀边缘不相交，该移动区域属于服务机器人无危险的自由区间。

s2.2、cost∈[128,252)时，服务机器人垂直投影边界与障碍膨胀区域相交，但不与致命区域相交，在此区域中的服务机器人已经处于危险状态中，但尚未造成致命威胁。

s2.3、cost∈[253,255]时，服务机器人垂直投影边界与致命区域相交，在该区域中继续移动的服务机器人将与危险物发生物理碰撞，已处于致命危险状态。

步骤三、预测轨迹计算

服务机器人在二维空间中移动路线由其移动速度决定，移动速度包括线速度v和角速度w，受到两个方面的限制。

s3.1、服务机器人自身的最大速度[v_max,w_max]和最小速度[v_min,w_min]限制的移速区间[v_m,w_m]为：

[v_m,w_m]＝{v∈[v_min,v_max],ω∈[w_min,ω_max]} (1)

s3.2、服务机器人自身的最大加速度和最小加速度/>限制的移速区间[v_d,w_d]为：

因此，服务机器人实际的移速区间[v,w]为：

[v,w]＝[v_m,w_m]∩[v_d,w_d] (3)

移速区间[v,w]中线速度v和角速度w的k种采样组合为服务机器人前进的k条预测轨迹。

步骤四、构建运动模型进行位姿计算

将相邻采样时刻的位置之间的运动轨迹以直线拟合，根据t时刻服务机器人的位置(x_t，y_t)和速度(v_t,w_t)，计算t+△t时刻服务机器人的位置(x_t+△t，y_t+△t)和导航角θ_t+△t：

步骤五、建立情绪认知模型进行初评价

情绪认知模型将k条预测轨迹上的周边环境信息分别映射到亲密度、恐惧度、愉悦度和喜悦度4个方面。

s5.1、亲密度情感函数relevance(v,w)表征服务机器人对局部目标的人工情感：

其中在全局坐标系下，θ_nav为服务机器人到达预测轨迹末端时的导航角，θ_local为局部目标点(x_i,y_i)的反正切角，θ_abs为导航角与目标点的差角。

在初评价阶段，亲密度情感函数表示服务机器人与局部目标的亲密程度。

s5.2、恐惧度情感函数fear(v,w)表征服务机器人对代价地图中单元格危险程度的人工情感：依次以服务机器人前进的预测轨迹上的采样点为圆心，根据步骤二的障碍物检测方法，判断在半径为4r_rob的圆形区域内是否存在危险，将危险与采样点的最小距离的值记为恐惧度情感函数fear(v,w)的值；若未检测到危险，则记恐惧度情感函数fear(v,w)的值为4r_rob，r_rob为服务机器人的半径。

s5.3、愉悦度情感函数Pleasure(v,w)表征服务机器人自身的人工情感：

Pleasure(v,w)＝v (6)

愉悦度情感函数Pleasure(v,w)的值与服务机器人当前线速度v的值相等，如果在周围半径为4r_rob的圆形区域内发现危险，恐惧度情感函数fear(v,w)的值增大，愉悦度情感函数Pleasure(v,w)的值降低，相应的服务机器人的线速度v会降低。

s5.4、喜悦度情感函数happiness(v,w)表征服务机器人对全局目标点的人工情感：喜悦度情感函数happiness(v,w)的值为预测轨迹末端与全局目标点的欧几里得距离，服务机器人与全局目标点的距离越近，喜悦度情感函数happiness(v,w)的值越大。

步骤六、建立再评价模型进行二次评价

对步骤五得到的k条预测轨迹的亲密度情感函数relevance(v,w)、恐惧度情感函数fear(v,w)、愉悦度情感函数Pleasure(v,w)和喜悦度情感函数happiness(v,w)的值分别进行归一化，得到再评价模型中亲密度情感函数n_relevance(v,w)、恐惧度情感函数n_fear(v,w)、愉悦度情感函数n_Pleasure(v,w)和喜悦度情感函数n_happiness(v,w)的值，然后利用PSO算法对再评价模型中4个维度的情感函数的权重进行优化，根据优化后的权重α、β、γ、计算第k条预测轨迹l_k的期望值E(l_k)：

计算k条预测轨迹的期望值，选取其中期望值最大的预测轨迹作为t+1时刻机器人移动的轨迹。重复步骤四、五、六，直至服务机器人到达全局目标点。

作为优选，在对预测轨迹的进行二次评价前，还包括碰撞检测，具体步骤为：

将恐惧度情感函数fear(v,w)的值小于2r_rob的预测轨迹丢弃，再对剩余的预测轨迹进行二次评价。若不存在剩余预测轨迹，则返回步骤一，重新进行采样点设置。

本发明具有以下有益效果：

1、本方法与现有技术中广泛应用的AGV导航、巡线导航相比，无需二维码、巡线等外部环境辅助装置。对室内环境友好度高，对于动态多变的家庭环境适应度高。

2、本方法将机器人对外部环境的感知映射到人工情感，能够提高机器人本体在导航过程中的自主性与对动态障碍物的应对能力，大大增加了用户居家环境的安全性。

3、本方法在路径规划过程中计算量小，能够减少决策层的计算成本，能够提高机器人在导航过程中的反应速度。

附图说明

图1为本方法的路径规划流程图；

图2为实施例中得到的多条预测轨迹示意图；

图3为实施例中情绪认知示意图。

具体实施方式

以下结合附图对本发明作进一步的解释说明；

本实施例中采用移动机器人和雷达搭建实验平台，在Ubuntu系统下运行ros，设置局部目标点与全局目标点，按照图1所示的流程，开始执行导航任务。

步骤一、地图建立

使用激光雷达扫描周边环境，建立服务机器人的活动地图。在服务机器人与全局目标点之间等距离设置50个采样点，在服务机器人向全局目标点前进的过程中，将服务机器人前方的第10个采样点作为局部目标点。当服务机器人当前位置与全局目标点之间的采样点数量少于10个时，将全局目标点作为局部目标点。

步骤二、障碍物检测

将步骤一建立的活动地图栅格化为代价地图，代价地图中每个单元格的危险程度代价cost由活动地图中的周边环境确定。将危险程度根据危险程度代价cost的取值分为三类。

s2.1、cost∈[0,127]时，服务机器人移动过程中的垂直投影边界与障碍物膨胀边缘不相交，该移动区域属于服务机器人无危险的自由区间。

s2.2、cost∈[128,252)时，服务机器人垂直投影边界与障碍膨胀区域相交，但不与致命区域相交，在此区域中的服务机器人已经处于危险状态中，但尚未造成致命威胁。

s2.3、cost∈[253,255]时，服务机器人垂直投影边界与致命区域相交，在该区域中继续移动的服务机器人将与危险物发生物理碰撞，已处于致命危险状态。

步骤三、预测轨迹计算

服务机器人在二维空间中移动路线由其移动速度决定，移动速度包括线速度v和角速度w，受到两个方面的限制。

s3.1、服务机器人自身的最大速度[v_max,w_max]和最小速度[v_min,w_min]限制的物理移速区间[v_m,w_m]为：

[v_m,w_m]＝{v∈[v_min,v_max],ω∈[w_min,ω_max]} (1)

s3.2、因电机力矩有限，移动机器人存在最大加速度限制，因此移动机器人轨迹向前模拟的周期△t内，存在一个动态窗口，该窗口内的速度为机器人实际移动过程中可达到的速度移速区间[v_d,w_d]为：

其中和/>分别为服务机器人自身的最大加速度和最小加速度。

因此，在这两方面的限制下，服务机器人实际的移速区间[v,w]为：

[v,w]＝[v_m,w_m]∩[v_d,w_d] (4)

如图2所示，移速区间[v,w]中线速度v和角速度w的采样组合为服务机器人前进的预测轨迹。

步骤四、构建运动模型进行位姿计算

采用非全向移动模型，将相邻采样时刻的位置之间的运动轨迹以直线拟合，沿全局坐标系x轴移动v_t*△t，将此距离分别投影至坐标系的x轴和y轴，计算机器人在全局坐标系中沿x轴和沿y轴的位移增量△x和△y：

然后将一段时间的位移增量分别累计求和，推算一段时间内的轨迹，即可求得t+△t时刻服务机器人的位置(x_t+△t，y_t+△t)和导航角θ_t+△t：

步骤五、建立情绪认知模型进行初评价

情绪认知模型将k条预测轨迹上的周边环境信息分别映射到亲密度、恐惧度、愉悦度和喜悦度4个方面，如图3所示。

s5.1、亲密度情感函数relevance(v,w)表征服务机器人对局部目标的人工情感：

获取局部目标与服务机器人当前位置在全局坐标系下的坐标和计算反正切角θ_local：

亲密度情感函数relevance(v,w)为：

relevance(v,w)＝2π-θ_abs (8)

其中θ_nav为服务机器人到达预测轨迹末端时的导航角。在初评价阶段，亲密度情感函数表示服务机器人与局部目标的亲密程度。θ_abs与亲密相关度负相关，θ_abs越小，则该维度评分越高。

s5.2、恐惧度情感函数fear(v,w)表征服务机器人对代价地图中单元格危险程度的人工情感：依次以服务机器人前进的预测轨迹上的位姿点为圆心，根据步骤二的障碍物检测方法，判断在半径为4r_rob的圆形区域内是否存在危险，将危险与采样点的最小距离的值记为恐惧度情感函数fear(v,w)的值；若未检测到危险，则记恐惧度情感函数fear(v,w)的值为4r_rob，r_rob为服务机器人的半径。

s5.3、愉悦度情感函数Pleasure(v,w)表征服务机器人自身的人工情感：

Pleasure(v,w)＝v (9)

愉悦度情感函数Pleasure(v,w)的值与服务机器人当前线速度v的值相等，如果在周围半径为4r_rob的圆形区域内发现危险，恐惧度情感函数fear(v,w)的值增大，愉悦度情感函数Pleasure(v,w)的值降低，相应的服务机器人的线速度v会降低。

s5.4、喜悦度情感函数happiness(v,w)表征服务机器人对全局目标点的人工情感：喜悦度情感函数happiness(v,w)的值为服务机器人预测轨迹末端与全局目标点/>的欧几里得距离：

服务机器人与全局目标点的距离越近，喜悦度情感函数happiness(v,w)的值越大。

步骤六、碰撞检测

将步骤五中所有恐惧度情感函数fear(v,w)的值小于2r_rob的预测轨迹丢弃，再对剩余的预测轨迹进行二次评价。若不存在剩余预测轨迹，则返回步骤一，重新进行采样点设置。

步骤七、建立再评价模型进行二次评价

为了评价函数的连续性与合理性以及轨迹平滑性，对步骤六碰撞检测后剩余的预测轨迹的亲密度情感函数relevance(v,w)、恐惧度情感函数fear(v,w)、愉悦度情感函数Pleasure(v,w)和喜悦度情感函数happiness(v,w)的值分别进行归一化，得到再评价模型中亲密度情感函数n_relevance(v,w)、恐惧度情感函数n_fear(v,w)、愉悦度情感函数n_Pleasure(v,w)和喜悦度情感函数n_happiness(v,w)的值，然后利用PSO算法对再评价模型中4个维度的情感函数的权重进行优化，在PSO算法中，需要选用综合性指标衡量适应度函数的设计效果，本实施例采用求和的方式。适应度函数fitness接收感知层传送的路程path、时间time、实时最短危险距离risk_distance等信息，fitness具体如下：

fitness＝path+time+risk_distance (11)

通过适应度函数fitness来评价解的品质，在适应度函数最小的时候，解的品质是最符合二次评价函数的要求。

根据优化后的权重α、β、γ、计算第k条预测轨迹l_k的期望值E(l_k)：

计算每条预测轨迹的期望值，选取其中期望值最大的预测轨迹作为t+1时刻机器人移动的轨迹。重复步骤四、五、六、七，直至服务机器人到达全局目标点。

Claims (5)

1.一种基于人工情感的服务机器人局部路径规划方法，其特征在于：该方法包括以下步骤：

步骤一、地图建立

扫描周边环境建立服务机器人的活动地图；在服务机器人与全局目标点之间等距离设置P个采样点，在局部路径规划过程中，将服务机器人前方的第p个采样点作为局部目标点；当服务机器人当前位置与全局目标点之间的采样点数量少于p个时，将全局目标点作为局部目标点；

步骤二、障碍物检测

将步骤一建立的活动地图栅格化为代价地图，代价地图中每个单元格的危险程度代价cost由活动地图中的周边环境确定；

步骤三、预测轨迹计算

由服务机器人自身的最大、最小速度[v_max,w_max]、[v_min,w_min]，以及最大、最小加速度 限制的移速区间[v_m,w_m]、[v_d,w_d]分别为：

[v_m,w_m]＝{v∈[v_min,v_max],ω∈[w_min,ω_max]} (1)

因此，服务机器人实际的移速区间[v,w]为：

[v,w]＝[v_m,w_m]∩[v_d,w_d] (3)

移速区间[v,w]中线速度v和角速度w的k种采样组合即为k条预测轨迹；

步骤四、构建运动模型进行位姿计算

根据t时刻服务机器人的位置(x_t，y_t)和速度(v_t,w_t)，计算t+△t时刻服务机器人的位置(x_t+△t，y_t+△t)和导航角θ_t+△t：

步骤五、建立情绪认知模型进行初评价

计算步骤三中k条预测轨迹的亲密度、恐惧度、愉悦度和喜悦度；

s5.1、亲密度情感函数relevance(v,w)：

relevance(v,w)＝2π-|θ_nav-θ_local| (5)

其中θ_nav为全局坐标系下服务机器人到达预测轨迹末端时的导航角，θ_local为局部目标点(x_i,y_i)在全局坐标系下的反正切角，θ_local＝arctan(x_i,y_i)；

s5.2、恐惧度情感函数fear(v,w)：

检测预测轨迹上，以各位姿点为圆心、半径为4r_rob的圆形区域内是否存在危险点，以位姿点与危险点的最小距离作为恐惧度情感函数fear(v,w)的值；若不存在危险点，则记恐惧度情感函数fear(v,w)的值为4r_rob，r_rob为服务机器人的半径；

s5.3、愉悦度情感函数Pleasure(v,w)：

Pleasure(v,w)＝v (6)

v表示服务机器人当前的线速度；

s5.4、喜悦度情感函数happiness(v,w)：

喜悦度情感函数happiness(v,w)的值为预测轨迹末端与全局目标点的欧几里得距离；

步骤六、建立再评价模型进行二次评价

对步骤五初评价后预测轨迹的情感函数值分别进行归一化，得到再评价模型中亲密度情感函数n_relevance(v,w)、恐惧度情感函数n_fear(v,w)、愉悦度情感函数n_Pleasure(v,w)和喜悦度情感函数n_happiness(v,w)的值，然后利用PSO算法对再评价模型中4个维度的情感函数的权重进行优化，根据优化后的权重α、β、γ、计算第k条预测轨迹l_k的期望值E(l_k)：

计算k条预测轨迹的期望值，选取其中期望值最大的预测轨迹作为t+1时刻机器人移动的轨迹；重复步骤四、五、六，直至服务机器人到达全局目标点。

2.如权利要求1所述一种基于人工情感的服务机器人局部路径规划方法，其特征在于：步骤一中，在服务机器人初始位置与全局目标点之间设置50个采样点，并且将服务机器人前方的第10个采样点设置为局部目标点。

3.如权利要求1所述一种基于人工情感的服务机器人局部路径规划方法，其特征在于：

栅格地图中，根据危险程度代价cost的取值将单元格的危险程度分为三类：

s2.1、cost∈[0,127]时，服务机器人移动过程中的垂直投影边界与障碍物膨胀边缘不相交，该移动区域属于服务机器人无危险的自由区间；

s2.2、cost∈[128,252)时，服务机器人垂直投影边界与障碍膨胀区域相交，但不与致命区域相交，在此区域中的服务机器人已经处于危险状态中，但尚未造成致命威胁；

s2.3、cost∈[253,255]时，服务机器人垂直投影边界与致命区域相交，在该区域中继续移动的服务机器人将与危险物发生物理碰撞，已处于致命危险状态。

4.如权利要求1所述一种基于人工情感的服务机器人局部路径规划方法，其特征在于：在对预测轨迹的进行二次评价前，还包括碰撞检测，具体步骤为：

将恐惧度情感函数fear(v,w)的值小于2r_rob的预测轨迹丢弃，再对进入步骤六，对剩余的预测轨迹进行二次评价；若不存在剩余预测轨迹，则返回步骤一，重新进行采样点设置。

5.如权利要求1所述一种基于人工情感的服务机器人局部路径规划方法，其特征在于：步骤六中，使用PSO算法进行权重优化时，采用求和的方式，设置适应度函数fitness为：

fitness＝path+time+risk_distance (8)

其中，path、time分别为感知层传送的路程、时间，risk_distance为实时最短危险距离。