一种基于防碰撞和社交约束条件的路径成本分析方法和装置与流程

本发明涉及具身智能，特别涉及一种基于防碰撞和社交约束条件的路径成本分析方法和装置。

背景技术：

1、在酒店、候车室、商场等人流密集场所使用的送货机器人，其路径规划方案大多只关注机器人与动静态障碍物之间的防碰撞约束、并未考虑将符合人类生活习惯的社交约束融入其中。所以，时常会看到送货机器人以较近距离经过身侧，又或者从正在聊天的一群人中间经过。

2、有鉴于此，我们为机器人具身智能系统的轨迹规划模块提供一种能同时兼顾防碰撞和社交约束条件的轨迹评估手段：将防碰撞、社交间距、人群禁穿行三类约束量化为可计算的归一化成本项，并通过对三类成本进行加权求和的方式计算总成本。如此一来，既保障机器人与动静态目标的绝对安全距离，又能遵循公共场所的社交行为规范。而如何实现上述改进手段则是本发明需要解决的技术问题。

技术实现思路

1、本发明的目的，就是针对现有技术的缺陷，提供一种基于防碰撞和社交约束条件的路径成本分析方法、装置、电子设备及计算机可读存储介质。本发明在任意时刻收到一组规划路径p、非人类目标集d和人类目标集h之后，先按匀速运动模式根据非人类目标集d和人类目标集h进行运动路径预测得到对应的非人类路径集gd和人类路径集gh；再按机器人不应与任何动静态目标发生碰撞的防碰撞约束条件，根据非人类路径集gd和人类路径集gh对规划路径p进行成本分析；并按机器人应与人类保持合理间距并在无法保持合理间距时最大化社交间距成本的社交约束条件，根据人类路径集gh对规划路径p进行成本分析；并按机器人不应从人群中穿行的社交约束条件，根据人类路径集gh对规划路径p进行成本分析；然后，根据三类成本进行总成本分析；最后根据防碰撞约束成本和总成本判断本次轨迹评估是否通过。基于本发明，既保障机器人与动静态目标的绝对安全距离，又能遵循公共场所的社交行为规范。

2、为实现上述目的，本发明实施例第一方面提供了一种基于防碰撞和社交约束条件的路径成本分析方法，所述方法包括：

3、在任意时刻t0接收一组规划路径p、非人类目标集d和人类目标集h；所述规划路径p包括n+1个路径点pi，数量n为预设的正整数，0≤索引i≤n；所述路径点pi包括时刻ti、位置si和速度vi，ti+1=t0+i×t，t为预设的采样间隔时长；第1个路径点p0为机器人在时刻t0的实时状态，第2到第n+1个路径点p1→n为机器人在时刻t+t到t+nt的n个规划状态；所述非人类目标集d包括多个非人类目标的目标状态dj(0), 1≤索引j≤nd，nd为d的目标总数；所述目标状态dj(0)为第j个非人类目标在时刻t0的实时状态，包括位置sj(0)和速度vj(0)，若所述vj(0)的向量模长为0则当前非人类目标为静态障碍物，若所述vj(0)的向量模长不为0则当前非人类目标为动态障碍物；所述人类目标集h包括多个人类目标的目标状态hk(0), 1≤索引k≤nh，nh为h的目标总数；所述目标状态hk(0)为第k个人类目标在时刻t0的实时状态，包括位置sk(0)和速度vk(0)；所有位置和速度均为指定场景鸟瞰高精地图的地图坐标系的2d坐标和速度向量；所述指定场景至少包括酒店、候车室和商场；机器人的类型为所述指定场景中的送货机器人；

4、按匀速运动模式根据所述非人类目标集d和所述人类目标集h进行运动路径预测得到对应的非人类路径集gd和人类路径集gh；所述非人类路径集gd包括nd个非人类路径；所述非人类路径包括n+1个路径点，所述路径点包括所述时刻ti、位置和速度；所述人类路径集gh包括nh个人类路径；所述人类路径包括n+1个路径点，所述路径点包括所述时刻ti、位置和速度；

5、按机器人不应与任何动静态目标发生碰撞的防碰撞约束条件，根据所述非人类路径集gd和所述人类路径集gh对所述规划路径p进行成本分析得到成本c1； 0≤c1≤1；

6、按机器人应与人类保持合理间距并在无法保持合理间距时最大化社交间距成本的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c2；0≤c2≤1；

7、按机器人不应从人群中穿行的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c3；0≤c3≤1；

8、根据成本c1、c2和c3进行总成本分析得到总成本call；

9、对所述成本c1和所述总成本call是否都小于各自对应的防碰撞成本阈值和总成本阈值进行识别；若是，则生成轨迹评估通过信息并反馈；若否，则生成轨迹评估未通过信息并反馈。

10、优选的，所述按匀速运动模式根据所述非人类目标集d和所述人类目标集h进行运动路径预测得到对应的非人类路径集gd和人类路径集gh，具体包括：

11、步骤21，将所述非人类目标集d的各个所述目标状态dj(0)对应的非人类目标作为当前目标；并按匀速运动模式，根据所述当前目标对应的所述位置sj(0)和所述速度vj(0)计算对应的n+1个所述位置和所述速度；并由各个所述时刻ti与其对应的所述和所述速度组成对应的所述路径点；并由所述当前目标对应的n+1个所述路径点组成对应的所述非人类路径；

12、其中，

13、，，

14、；

15、步骤22，将所述人类目标集h的各个所述目标状态hk(0)对应的人类目标作为当前目标；并按匀速运动模式，根据所述当前目标对应的所述位置sk(0)和所述速度vk(0)计算对应的n+1个所述位置和所述速度；并由各个所述时刻ti与其对应的所述和所述速度组成对应的所述路径点；并由所述当前目标对应的n+1个所述路径点组成对应的所述人类路径；

16、其中，

17、，，

18、。

19、优选的，所述按机器人不应与任何动静态目标发生碰撞的防碰撞约束条件，根据所述非人类路径集gd和所述人类路径集gh对所述规划路径p进行成本分析得到成本c1，具体包括：

20、将预设距离lrob以及所述规划路径p、所述非人类路径集gd和所述人类路径集gh中的所有位置向量代入预设的归一化碰撞成本函数进行计算得到对应的所述成本c1；

21、其中，所述归一化碰撞成本函数为：

22、；

23、为l2范数；

24、所述成本c1越低则发生碰撞事件的概率越低、反之若所述成本c1越高则发生碰撞事件的概率越高。

25、优选的，所述按机器人应与人类保持合理间距并在无法保持合理间距时最大化社交间距成本的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c2，具体包括：

26、步骤41，对所述人类路径集gh是否为空进行识别；若是，则设置目标总数nh为0；若否，则对所述人类路径集gh的所述目标总数nh进行统计；

27、步骤42，若所述目标总数nh为0，则将所述成本c2设为0并输出；

28、步骤43，若所述目标总数nh大于0，则将预设间距lsocial以及所述规划路径p和所述人类路径集gh中的所有位置向量代入预设的归一化社交间距成本函数进行计算得到对应的所述成本c2并输出；

29、其中，lsocial＞lrob；

30、所述归一化社交间距成本函数为：

31、，

32、；

33、csocial为社交间距成本函数，为预设的社交间距成本阈值，；

34、所述成本c2越低则发生无法保持合理间距事件的概率越低、反之若所述成本c2越高则发生无法保持合理间距事件的概率越高。

35、优选的，所述按机器人不应从人群中穿行的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c3，具体包括：

36、步骤51，对所述人类路径集gh是否为空进行识别；若是，则设置目标总数nh为0；若否，则对所述人类路径集gh的所述目标总数nh进行统计；

37、步骤52，若所述目标总数nh为0或1，则将所述成本c3设为0并转至步骤56；

38、步骤53，若所述目标总数nh大于1，则基于所述时刻ti对(n+1)×nh个所述路径点进行聚类，将所述时刻ti相同的nh个所述路径点聚为一类组成一个对应的聚类点集pi；

39、步骤54，将各个所述聚类点集pi作为当前点集；并将所述规划路径p中对应的所述位置si作为当前机器人位点；并以簇内任意两个所述路径点的相对位置间距不大于预设最大间距lgroup为条件对所述当前点集进行分簇，并对得到的点簇总数进行统计，所述点簇总数大于0时得到的各个点簇内的路径点总数大于或等于2；若所述点簇总数为0，则设置对应的穿行标记cri为0；若所述点簇总数大于0，则初始化所述穿行标记cri为0，并对所述当前点集的所有点簇进行一轮遍历；并在本轮遍历过程中，将当前遍历点簇作为当前点簇；并对所述当前点簇的边沿点进行识别，并对识别出的边沿点总数进行统计；若所述边沿点总数为2，则对所述当前点簇两个边沿点的中心点与所述当前机器人位点的点间距是否小于预设间距lsocial进行识别，若是则将所述穿行标记cri重置为1并停止本轮遍历；若所述边沿点总数大于2，则对所述当前点簇的所有边沿点进行顺次连接得到一个对应的多边形区域，并对所述当前机器人位点是否在当前多边形区域内进行识别，若是则将所述穿行标记cri重置为1并停止本轮遍历；

40、其中，点簇两个边沿点的中心点即两个边沿点位置坐标的平均坐标；

41、步骤55，将得到的(n+1)个所述穿行标记cri代入预设的归一化人群穿行成本函数进行计算得到对应的所述成本c3；

42、其中，所述归一化人群穿行成本函数为：

43、；

44、步骤56，将得到的所述成本c3作为本次成本分析结果输出。

45、优选的，所述根据成本c1、c2和c3进行总成本分析得到总成本call，具体包括：

46、将成本c1、c2和c3代入预设的总成本函数进行计算得到对应的所述总成本call并输出；

47、其中，所述总成本函数为：

48、；

49、、、为预设的第一、第二、第三成本系数。

50、本发明实施例第二方面提供了一种用于实现上述第一方面所述的基于防碰撞和社交约束条件的路径成本分析方法的装置，所述装置包括：数据接收模块、环境目标轨迹推导模块、防碰撞约束成本评估模块、社交合理间距成本评估模块、人群穿行成本评估模块、总成本评估模块、评估反馈模块；

51、所述数据接收模块用于在任意时刻t0接收一组规划路径p、非人类目标集d和人类目标集h；所述规划路径p包括n+1个路径点pi，数量n为预设的正整数，0≤索引i≤n；所述路径点pi包括时刻ti、位置si和速度vi，ti+1=t0+i×t，t为预设的采样间隔时长；第1个路径点p0为机器人在时刻t0的实时状态，第2到第n+1个路径点p1→n为机器人在时刻t+t到t+nt的n个规划状态；所述非人类目标集d包括多个非人类目标的目标状态dj(0), 1≤索引j≤nd，nd为d的目标总数；所述目标状态dj(0)为第j个非人类目标在时刻t0的实时状态，包括位置sj(0)和速度vj(0)，若所述vj(0)的向量模长为0则当前非人类目标为静态障碍物，若所述vj(0)的向量模长不为0则当前非人类目标为动态障碍物；所述人类目标集h包括多个人类目标的目标状态hk(0), 1≤索引k≤nh，nh为h的目标总数；所述目标状态hk(0)为第k个人类目标在时刻t0的实时状态，包括位置sk(0)和速度vk(0)；所有位置和速度均为指定场景鸟瞰高精地图的地图坐标系的2d坐标和速度向量；所述指定场景至少包括酒店、候车室和商场；机器人的类型为所述指定场景中的送货机器人；

52、所述环境目标轨迹推导模块用于按匀速运动模式根据所述非人类目标集d和所述人类目标集h进行运动路径预测得到对应的非人类路径集gd和人类路径集gh；所述非人类路径集gd包括nd个非人类路径；所述非人类路径包括n+1个路径点，所述路径点包括所述时刻ti、位置和速度；所述人类路径集gh包括nh个人类路径；所述人类路径包括n+1个路径点，所述路径点包括所述时刻ti、位置和速度；

53、所述防碰撞约束成本评估模块用于按机器人不应与任何动静态目标发生碰撞的防碰撞约束条件，根据所述非人类路径集gd和所述人类路径集gh对所述规划路径p进行成本分析得到成本c1； 0≤c1≤1；

54、所述社交合理间距成本评估模块用于按机器人应与人类保持合理间距并在无法保持合理间距时最大化社交间距成本的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c2；0≤c2≤1；

55、所述人群穿行成本评估模块用于按机器人不应从人群中穿行的社交约束条件，根据所述人类路径集gh对所述规划路径p进行成本分析得到成本c3；0≤c3≤1；

56、所述总成本评估模块用于根据成本c1、c2和c3进行总成本分析得到总成本call；

57、所述评估反馈模块用于对所述成本c1和所述总成本call是否都小于各自对应的防碰撞成本阈值和总成本阈值进行识别；若是，则生成轨迹评估通过信息并反馈；若否，则生成轨迹评估未通过信息并反馈。

58、本发明实施例第三方面提供了一种电子设备，包括：存储器、处理器和收发器；

59、所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现上述第一方面所述的方法步骤；

60、所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。

61、本发明实施例第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令被计算机执行时，使得所述计算机执行上述第一方面所述的方法的指令。

62、本发明实施例提供了一种基于防碰撞和社交约束条件的路径成本分析方法、装置、电子设备及计算机可读存储介质。由上述内容可知，本发明实施例在任意时刻收到一组规划路径p、非人类目标集d和人类目标集h之后，先按匀速运动模式根据非人类目标集d和人类目标集h进行运动路径预测得到对应的非人类路径集gd和人类路径集gh；再按机器人不应与任何动静态目标发生碰撞的防碰撞约束条件，根据非人类路径集gd和人类路径集gh对规划路径p进行成本分析；并按机器人应与人类保持合理间距并在无法保持合理间距时最大化社交间距成本的社交约束条件，根据人类路径集gh对规划路径p进行成本分析；并按机器人不应从人群中穿行的社交约束条件，根据人类路径集gh对规划路径p进行成本分析；然后，根据三类成本进行总成本分析；最后根据防碰撞约束成本和总成本判断本次轨迹评估是否通过。基于本发明实施例，既保障机器人与动静态目标的绝对安全距离，又能遵循公共场所的社交行为规范。